点云平面拟合及可视化

不使用法线

#include <pcl/io/pcd_io.h> #include <pcl/common/centroid.h> #include <pcl/segmentation/sac_segmentation.h> #include <pcl/visualization/pcl_visualizer.h> void visualization(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, const pcl::ModelCoefficients::Ptr coefficients, const pcl::PointIndices::Ptr inliers) { boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Plane")); viewer->setBackgroundColor(0, 0, 0); Eigen::Vector4f centroid; pcl::compute3DCentroid(*cloud, centroid); pcl::PointXYZ p1(centroid[0], centroid[1], centroid[2]); pcl::PointXYZ p2(centroid[0] + coefficients->values[0], centroid[1] + coefficients->values[1], centroid[2] + coefficients->values[2]); viewer->addArrow(p2, p1, 1, 0, 0, false); viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "cloud"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "cloud"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud"); pcl::PointCloud<pcl::PointXYZRGB>::Ptr inlier_cloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::copyPointCloud(*cloud, *inliers, *inlier_cloud); viewer->addPointCloud<pcl::PointXYZRGB>(inlier_cloud, "inlier_cloud"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "inlier_cloud"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "inlier_cloud"); while (!viewer->wasStopped()) { viewer->spinOnce(100); boost::this_thread::sleep(boost::posix_time::microseconds(100000)); } } void FittingPlane(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, pcl::ModelCoefficients::Ptr& coefficients, pcl::PointIndices::Ptr& inliers) { // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZRGB> seg; // Optional seg.setOptimizeCoefficients(true); // Mandatory seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setDistanceThreshold(0.03); seg.setMaxIterations(1000); seg.setInputCloud(cloud); seg.segment(*inliers, *coefficients); } void FittingPlane(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, const Eigen::Vector3d& init_normal, const double& eps_angle_in_rad, pcl::ModelCoefficients::Ptr& coefficients, pcl::PointIndices::Ptr& inliers) { pcl::SACSegmentation<pcl::PointXYZRGB> seg; seg.setOptimizeCoefficients(true); // seg.setModelType(pcl::SACMODEL_PLANE); seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setDistanceThreshold(0.03); seg.setMaxIterations(1000); seg.setAxis(init_normal.cast<float>()); seg.setEpsAngle(eps_angle_in_rad); seg.setInputCloud(cloud); seg.segment(*inliers, *coefficients); } int main(int argc, char**argv) { // 加载点云模型 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>); if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("/tmp/color.pcd", *cloud) == -1) { std::cerr << "can't load point cloud file" << std::endl; return -1; } std::cout << "loaded " << cloud->width * cloud->height << " points" << std::endl; std::cout << "field: " << pcl::getFieldsList(*cloud) << std::endl; // 拟合平面 pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // FittingPlane(cloud, coefficients, inliers); Eigen::Vector3d init_normal; init_normal << 0, 0, 1; double eps_angle_in_rad = 10 * M_PI / 180; // 估计的法线和初始法线相差10°以内 FittingPlane(cloud, init_normal, eps_angle_in_rad, coefficients, inliers); std::cout << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3]; // 点云渲染和可视化 visualization(cloud, coefficients, inliers); return 0; } #include <pcl/io/pcd_io.h> #include <pcl/com