HOUGH_LINE: PCL Point Cloud Line Detection in C++

#pragma once\n#include "pcl\point_types.h"\n#include "pcl\io\ply_io.h"\n#include "pcl\visualization\pcl_visualizer.h"\n#include "iostream"\n#include "pcl\common\common.h"\n#include "pcl\visualization\pcl_plotter.h"\n#include "pcl\filters\voxel_grid.h"\n#include "queue"\n\nusing namespace pcl; \nusing namespace Eigen; \nusing namespace std; \ntypedef pcl::PointXYZ PointT; \nclass HOUGH_LINE\n{\npublic:\n HOUGH_LINE();\n ~HOUGH_LINE();\n template void vector_sort(std::vector vector_input, std::vector<size_t> &idx);\n inline void setinputpoint(pcl::PointCloudpcl::PointXYZ::Ptr point_);\n void VoxelGrid_(float size_, pcl::PointCloudpcl::PointXYZ::Ptr &voxel_cloud);\n void draw_hough_spacing();\n void HOUGH_line(int x_setp_num, double y_resolution, int grid_point_number_threshold, vector&K_, vector&B_, int line_num = -1);\n void draw_hough_line();\nprivate:\n pcl::PointCloudpcl::PointXYZ::Ptr cloud;\n int point_num;\n pcl::PointXYZ point_min;\n pcl::PointXYZ point_max;\n vector<pair<double, double>>result_;\n};\n\ntemplatevoid\nHOUGH_LINE::vector_sort(std::vector vector_input, std::vector<size_t> &idx) {\n idx.resize(vector_input.size());\n iota(idx.begin(), idx.end(), 0);\n sort(idx.begin(), idx.end(), [&vector_input](size_t i1, size_t i2) { return vector_input[i1] > vector_input[i2]; });\n}\ninline void HOUGH_LINE::setinputpoint(pcl::PointCloudpcl::PointXYZ::Ptr point_) {\n cloud = point_;\n point_num = cloud->size();\n}\n//HOUGH_LINE.cpp文件\n#include "HOUGH_LINE.h"\nHOUGH_LINE::HOUGH_LINE()\n{\n}\n\nHOUGH_LINE::~HOUGH_LINE()\n{\n cloud->clear();\n result_.clear();\n}\nvoid HOUGH_LINE::VoxelGrid_(float size_, pcl::PointCloudpcl::PointXYZ::Ptr &voxel_cloud) {\n pcl::PointCloudpcl::PointXYZ::Ptr tem_voxel_cloud(new pcl::PointCloudpcl::PointXYZ);\n pcl::VoxelGridpcl::PointXYZ vox;\n vox.setInputCloud(cloud);\n vox.setLeafSize(size_, size_, size_);\n vox.filter(tem_voxel_cloud);\n cloud = tem_voxel_cloud;\n voxel_cloud = tem_voxel_cloud;\n point_num = cloud->size();\n}\nvoid HOUGH_LINE::draw_hough_spacing() {\n pcl::visualization::PCLPlotter plot_(new pcl::visualization::PCLPlotter("Elevation and Point Number Breakdown Map"));\n plot_->setBackgroundColor(1, 1, 1);\n plot_->setTitle("hough space");\n plot_->setXTitle("angle");\n plot_->setYTitle("rho");\n vector<pair<double, double>>data_;\n double x_resolution1 = M_PI / 181;\n double a_1, b_1;\n for (int i_point = 0; i_point < point_num; i_point++)\n {\n a_1 = cloud->points[i_point].x;\n b_1 = cloud->points[i_point].y;\n for (int i = 0; i < 181; i++)\n {\n data_.push_back(make_pair(ix_resolution1, a_1 * cos(ix_resolution1) + b_1 * sin(i*x_resolution1)));\n }\n plot_->addPlotData(data_, "line", vtkChart::LINE);//X,Y均为double型的向量\n data_.clear();\n }\n plot_->setShowLegend(false);\n plot_->plot();//绘制曲线\n}\nvoid HOUGH_LINE::HOUGH_line(int x_setp_num, double y_resolution, int grid_point_number_threshold, vector&K_, vector&B_, int line_num) {\n vector<vector>all_point_row_col;\n vector Grid_Index;\n getMinMax3D(*cloud, point_min, point_max);\n double x_resolution = M_PI / x_setp_num;\n int raster_rows, raster_cols;\n raster_rows = ceil((M_PI - 0) / x_resolution);\n raster_cols = ceil((point_max.y + point_max.x) / y_resolution) * 2;\n MatrixXi all_row_col(raster_cols, raster_rows);//存储每个格网内的个数\n all_row_col.setZero();\n MatrixXd all_row_col_mean(raster_cols, raster_rows);//存储每个格网内的均值\n //统计每个格网内的数量，和rho均值\n all_row_col_mean.setZero();\n double a_, b_;\n for (int i_point = 0; i_point < point_num; i_point++) {\n a_ = cloud->points[i_point].x;\n b_ = cloud->points[i_point].y;\n for (int i_ = 0; i_ < x_setp_num; i_++)\n {\n double theta = 0 + i_ * x_resolution + x_resolution / 2;\n double rho = a_ * cos(theta) + b_ * sin(theta);\n //double rho = line_(theta,a_,b_);\n double idx = ceil(abs(rho / y_resolution));\n if (rho >= 0)\n {\n all_row_col(raster_cols / 2 - idx, i_) += 1;\n all_row_col_mean(raster_cols / 2 - idx, i_) += rho;\n }\n else {\n all_row_col(raster_cols / 2 + idx, i_) += 1;\n all_row_col_mean(raster_cols / 2 + idx, i_) += rho;\n }\n }\n }\n //求解邻域\n int min_num_threshold = grid_point_number_threshold;\n vector<pair<double, double>>result_jz;\n //vector<pair<double, double>>result_;\n vectortem_nebor;\n vectornum_grid;\n for (int i_row = 0; i_row < all_row_col.rows(); i_row++)\n {\n for (int i_col = 0; i_col < all_row_col.cols(); i_col++)\n {\n if (i_row == 0)\n {\n if (i_col == 0)\n {\n tem_nebor.push_back(all_row_col(i_row, i_col + 1));\n tem_nebor.push_back(all_row_col(i_row + 1, i_col + 1));\n tem_nebor.push_back(all_row_col(i_row + 1, i_col));\n }\n if (i_col == all_row_col.cols() - 1)\n {\n tem_nebor.push_back(all_row_col(i_row, i_col - 1));\n tem_nebor.push_back(all_row_col(i_row + 1, i_col));\n tem_nebor.push_back(all_row_col(i_row + 1, i_col - 1));\n }\n if (i_col != all_row_col.cols() - 1 && i_col != 0)\n {\n tem_nebor.push_back(all_row_col(i_row, i_col - 1));\n tem_nebor.push_back(all_row_col(i_row, i_col + 1));\n tem_nebor.push_back(all_row_col(i_row + 1, i_col));\n tem_nebor.push_back(all_row_col(i_row + 1, i_col - 1));\n tem_nebor.push_back(all_row_col(i_row + 1, i_col + 1));\n }\n }\n if (i_row == all_row_col.rows() - 1)\n {\n if (i_col == 0)\n {\n tem_nebor.push_back(all_row_col(i_row - 1, i_col));\n tem_nebor.push_back(all_row_col(i_row - 1, i_col + 1));\n tem_nebor.push_back(all_row_col(i_row, i_col + 1));\n }\n if (i_col == all_row_col.cols() - 1)\n {\n tem_nebor.push_back(all_row_col(i_row - 1, i_col));\n tem_nebor.push_back(all_row_col(i_row - 1, i_col - 1));\n tem_nebor.push_back(all_row_col(i_row, i_col - 1));\n }\n if (i_col != all_row_col.cols() - 1 && i_col != 0)\n {\n tem_nebor.push_back(all_row_col(i_row, i_col - 1));\n tem_nebor.push_back(all_row_col(i_row, i_col + 1));\n tem_nebor.push_back(all_row_col(i_row - 1, i_col));\n tem_nebor.push_back(all_row_col(i_row - 1, i_col - 1));\n tem_nebor.push_back(all_row_col(i_row - 1, i_col + 1));\n }\n }\n if (i_row != all_row_col.rows() - 1 && i_row != 0)\n {\n if (i_col == 0)\n {\n tem_nebor.push_back(all_row_col(i_row, i_col + 1));\n tem_nebor.push_back(all_row_col(i_row + 1, i_col));\n tem_nebor.push_back(all_row_col(i_row + 1, i_col + 1));\n tem_nebor.push_back(all_row_col(i_row - 1, i_col));\n tem_nebor.push_back(all_row_col(i_row - 1, i_col + 1));\n }\n if (i_col == all_row_col.cols() - 1)\n {\n tem_nebor.push_back(all_row_col(i_row - 1, i_col));\n tem_nebor.push_back(all_row_col(i_row - 1, i_col - 1));\n tem_nebor.push_back(all_row_col(i_row, i_col - 1));\n tem_nebor.push_back(all_row_col(i_row + 1, i_col - 1));\n tem_nebor.push_back(all_row_col(i_row + 1, i_col));\n }\n if (i_col != all_row_col.cols() - 1 && i_col != 0)\n {\n tem_nebor.push_back(all_row_col(i_row, i_col - 1));\n tem_nebor.push_back(all_row_col(i_row, i_col + 1));\n tem_nebor.push_back(all_row_col(i_row - 1, i_col));\n tem_nebor.push_back(all_row_col(i_row - 1, i_col - 1));\n tem_nebor.push_back(all_row_col(i_row - 1, i_col + 1));\n tem_nebor.push_back(all_row_col(i_row + 1, i_col));\n tem_nebor.push_back(all_row_col(i_row + 1, i_col - 1));\n tem_nebor.push_back(all_row_col(i_row + 1, i_col + 1));\n }\n }\n int max_tem = *max_element(tem_nebor.begin(), tem_nebor.end());\n tem_nebor.clear();\n if (all_row_col(i_row, i_col) > max_tem)\n {\n num_grid.push_back(all_row_col(i_row, i_col));\n double tem_ = all_row_col_mean(i_row, i_col) / all_row_col(i_row, i_col);\n result_jz.push_back(make_pair(0 + i_col * x_resolution + x_resolution / 2, tem_));\n if (all_row_col(i_row, i_col) > min_num_threshold)\n {\n result_.push_back(make_pair(0 + i_col * x_resolution + x_resolution / 2, tem_));\n }\n }\n }\n }\n if (line_num != -1)\n {\n result_.clear();\n vector<size_t>idx_;\n vector_sort(num_grid, idx_);\n if (result_jz.size() < line_num)\n {\n line_num = result_jz.size();\n }\n for (int i_ = 0; i_ < line_num; i_++)\n {\n result_.push_back(result_jz[idx_[i_]]);\n }\n }\n for (int i_hough = 0; i_hough < result_.size(); i_hough++) {\n B_.push_back(result_[i_hough].second / sin(result_[i_hough].first));\n K_.push_back(-cos(result_[i_hough].first) / sin(result_[i_hough].first));\n }\n}\nvoid HOUGH_LINE::draw_hough_line() {\n pcl::visualization::PCLPlotter *plot_line(new pcl::visualization::PCLPlotter);\n plot_line->setBackgroundColor(1, 1, 1);\n plot_line->setTitle("line display");\n plot_line->setXTitle("x");\n plot_line->setYTitle("y");\n vectorx_, y_;\n for (int i_point = 0; i_point < point_num; i_point++)\n {\n x_.push_back(cloud->points[i_point].x);\n y_.push_back(cloud->points[i_point].y);\n }\n for (int i_hough = 0; i_hough < result_.size(); i_hough++) {\n std::vector func1(2, 0);\n func1[0] = result_[i_hough].second / sin(result_[i_hough].first);\n func1[1] = -cos(result_[i_hough].first) / sin(result_[i_hough].first);\n plot_line->addPlotData(func1, point_min.x, point_max.x);\n }\n plot_line->addPlotData(x_, y_, "display", vtkChart::POINTS);//X,Y均为double型的向量\n plot_line->setShowLegend(false);\n plot_line->plot();//绘制曲线\n //plot_line->spin();\n}\n//主函数，调用\n#include "HOUGH_LINE.h"\nint main()\n{\n pcl::PointCloudpcl::PointXYZ::Ptr cloud(new pcl::PointCloudpcl::PointXYZ);\n pcl::io::loadPLYFilepcl::PointXYZ("D:\DIANYUNWENJIANJIA\test5_ply.ply", *cloud);\n pcl::PointCloudpcl::PointXYZ::Ptr VOXEL;\n HOUGH_LINE hough;\n hough.setinputpoint(cloud);\n hough.VoxelGrid_(1.0, VOXEL);\n hough.draw_hough_spacing();\n vectorK_, B_;\n hough.HOUGH_line(181, 0.5, 400, K_, B_);//按阈值自动检测\n hough.HOUGH_line(181, 0.5, 400, K_, B_, 3);//指定只选择极大值最大的3条直线\n hough.draw_hough_line();\n return 0;\n