#pragma once\n#include "pcl/point_types.h"\n#include "pcl/io/ply_io.h"\n#include "pcl/visualization/pcl_visualizer.h"\n#include \n#include "pcl/common/common.h"\n#include "pcl/visualization/pcl_plotter.h"\n#include "pcl/filters/voxel_grid.h"\n#include \n\nusing namespace pcl; \nusing namespace Eigen; \nusing namespace std; \ntypedef 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(PointCloud::Ptr point_); \n void VoxelGrid_(float size_, PointCloud::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 PointCloud::Ptr cloud; \n int point_num; \n PointT point_min; \n PointT 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(PointCloud::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_, PointCloud::Ptr &voxel_cloud) { \n PointCloud::Ptr tem_voxel_cloud(new PointCloud); \n VoxelGrid 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 visualization::PCLPlotter plot_(new 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 visualization::PCLPlotter *plot_line(new 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 PointCloud::Ptr cloud(new PointCloud); \n pcl::io::loadPLYFile("D:\DIANYUNWENJIANJIA\test5_ply.ply", *cloud); \n PointCloud::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