C++ 点云ICP校准
我有两个点云，我想匹配。但是在对齐之后，我只得到输入云，而不是合并的两点云。有什么问题吗 我已经更新了我的代码，但它仍然没有对齐C++ 点云ICP校准,c++,point-cloud-library,point-clouds,C++,Point Cloud Library,Point Clouds,我有两个点云，我想匹配。但是在对齐之后，我只得到输入云，而不是合并的两点云。有什么问题吗 我已经更新了我的代码，但它仍然没有对齐 pcl::PCDReader reader; pcl::PassThrough<pcl::PointXYZ> pass; pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne; pcl::SACSegmentationFromNormals<pcl::Poin
pcl::PCDReader reader;
pcl::PassThrough<pcl::PointXYZ> pass;
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
pcl::PCDWriter writer;
pcl::ExtractIndices<pcl::PointXYZ> extract;
pcl::ExtractIndices<pcl::Normal> extract_normals;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
// Datasets
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>),
cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
reader.read ("match.pcd", *target_cloud);
//Read in the cloud data
reader.read ("pointClouds_000026.pcd", *input_cloud);
std::cerr << "PointCloud has: " << input_cloud->points.size () << " data points." << std::endl;
// pass.setInputCloud (input_cloud);
// pass.setFilterFieldName ("z");
// pass.setFilterLimits (-600,1400);
// pass.filter (*cloud_filtered);
pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
// build the filter
outrem.setInputCloud(input_cloud);
outrem.setRadiusSearch(50);
outrem.setMinNeighborsInRadius (5);
// apply filter
outrem.filter (*cloud_filtered);
std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
writer.write ("table_scene_mug_stereo_textured_filtered.pcd", *cloud_filtered, false);
// Estimate point normals
ne.setSearchMethod (tree);
ne.setInputCloud (cloud_filtered);
ne.setKSearch (50);
ne.compute (*cloud_normals);
// Create the segmentation object for the planar model and set all the parameters
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg.setNormalDistanceWeight (0.1);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.09);
seg.setInputCloud (cloud_filtered);
seg.setInputNormals (cloud_normals);
seg.setEpsAngle(0.05320);
Eigen::Vector3f YAxis(0,1,0);
seg.setAxis(YAxis);
// Obtain the plane inliers and coefficients
seg.segment (*inliers_plane, *coefficients_plane);
std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
// Extract the planar inliers from the input cloud
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers_plane);
extract.setNegative (false);
// Write the planar inliers to disk
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
extract.filter (*cloud_plane);
std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
writer.write ("cloudplane.pcd", *cloud_plane, false);
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setIndices (inliers_plane);
proj.setInputCloud(cloud_filtered);
proj.setModelCoefficients (coefficients_plane);
proj.filter (*cloud_projected);
std::cerr << "PointCloud after projection has: "
<< cloud_projected->points.size () << " data points." << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConvexHull<pcl::PointXYZ> chull;
chull.setInputCloud (cloud_projected);
chull.setComputeAreaVolume(true);
chull.reconstruct (*cloud_hull);
std::cerr <<"area of convex hull: "<< chull.getTotalArea()<<std::endl;
std::cerr << "Convex hull has: " << cloud_hull->points.size () << " data points." << std::endl;
writer.write ("table_scene_mug_stereo_textured_hull.pcd", *cloud_hull, false);
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource (cloud_plane);
icp.setInputTarget (target_cloud);
icp.setEuclideanFitnessEpsilon(0.000000000001);
icp.setTransformationEpsilon(0.0000001);
icp.setMaximumIterations(10000);
icp.setMaxCorrespondenceDistance(300);
writer.write("nonal_igned.pcd", *cloud_plane + *target_cloud, false);
pcl::PointCloud<pcl::PointXYZ> final;
icp.align (final);
pcl::transformPointCloud(*cloud_plane, *cloud_plane, icp.getFinalTransformation());
final = final + *target_cloud;
writer.write("aligned.pcd", final, false);
std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
pcl:：PCD读卡器；
pcl：：直通；
pcl：：正态估计；
pcl：：SAC从法线分段；
pcl：：PCDWriter编写器；
pcl:：提取物；
pcl:：ExtractIndex提取_法线；
pcl:：search:：KdTree:：Ptr树（新的pcl:：search:：KdTree（））；
//数据集
pcl:：PointCloud:：Ptr输入_cloud（新pcl:：PointCloud）；
pcl:：PointCloud:：Ptr cloud_过滤（新pcl:：PointCloud），
投影云（新pcl:：点云）；
pcl:：PointCloud:：Ptr cloud_法线（新pcl:：PointCloud）；
pcl:：ModelCourters:：Ptr系数_平面（新pcl:：ModelCourters）、系数_圆柱体（新pcl:：ModelCourters）；
pcl:：PointIndexes:：Ptr内插层\平面（新pcl:：PointIndexes）、内插层\圆柱（新pcl:：PointIndexes）；
pcl:：PointCloud:：Ptr目标云（新pcl:：PointCloud）；
reader.read（“match.pcd”，*target_cloud）；
//读取云数据
reader.read（“pointClouds_000026.pcd”，*输入_cloud）；
标准：cerrICP使用输入云及其视点作为参考。它尝试对齐目标_云，以使对应点之间的距离迭代最小化。此的最终转换将由
icp.align(Final);
对目标云进行algin并将其存储在pcl:：PointCloud Final
中
要在ICP后合并两个点云，必须将它们转换为相同的视点，如下所示
pcl::PointCloud<pcl::PointXYZ> transformed_target_cloud;
Eigen::Matrix4f transformation_matrix = icp.getFinalTransformation();
pcl::transformPointCloud(Final, transformed_target_cloud, transformation_matrix);
pcl::PointCloud<pcl::PointXYZ> transformed_input_cloud;
Eigen::Isometry3f pose(Eigen::Isometry3f(Eigen::Translation3f(
input_cloud.sensor_origin_[0],
input_cloud.sensor_origin_[1],
input_cloud.sensor_origin_[2])) *
Eigen::Isometry3f(input_cloud.sensor_orientation_));
transformation_matrix = pose.matrix();
pcl::transformPointCloud(input_cloud, transformed_input_cloud, transformation_matrix);
// now both point clouds are transformed to the origin and can be added up
pcl::PointCloud<pcl::PointXYZ> output_cloud;
output_cloud += transformed_input_cloud;
output_cloud += transformed_target_cloud;
pcl：：点云转化为目标云；
特征：：矩阵4f变换_矩阵=icp.getFinalTransformation（）；
pcl:：transformPointCloud（最终，转换的目标云，转换矩阵）；
pcl:：点云转换的输入云；
Eigen:：Isometry3f姿势（Eigen:：Isometry3f）（Eigen:：Translation3f(
输入云传感器原点[0]，
输入云传感器原点[1]，
输入\云传感器\原点\[2]）*
Eigen:：Isometry3f（输入云传感器方向）；
变换矩阵=姿势矩阵（）；
pcl:：transformPointCloud（输入云、转换云、转换矩阵）；
//现在，两个点云都转换为原点，可以相加
pcl:：点云输出\点云；
输出_cloud+=转换后的_输入_cloud；
输出_cloud+=转换的_目标_cloud；
ICP使用输入云及其视点作为参考。它尝试对齐目标_云，以使对应点之间的距离迭代最小化。此的最终转换将由
icp.align(Final);
对目标云进行algin并将其存储在pcl:：PointCloud Final
中
要在ICP后合并两个点云，必须将它们转换为相同的视点，如下所示
pcl::PointCloud<pcl::PointXYZ> transformed_target_cloud;
Eigen::Matrix4f transformation_matrix = icp.getFinalTransformation();
pcl::transformPointCloud(Final, transformed_target_cloud, transformation_matrix);
pcl::PointCloud<pcl::PointXYZ> transformed_input_cloud;
Eigen::Isometry3f pose(Eigen::Isometry3f(Eigen::Translation3f(
input_cloud.sensor_origin_[0],
input_cloud.sensor_origin_[1],
input_cloud.sensor_origin_[2])) *
Eigen::Isometry3f(input_cloud.sensor_orientation_));
transformation_matrix = pose.matrix();
pcl::transformPointCloud(input_cloud, transformed_input_cloud, transformation_matrix);
// now both point clouds are transformed to the origin and can be added up
pcl::PointCloud<pcl::PointXYZ> output_cloud;
output_cloud += transformed_input_cloud;
output_cloud += transformed_target_cloud;
pcl：：点云转化为目标云；
特征：：矩阵4f变换_矩阵=icp.getFinalTransformation（）；
pcl:：transformPointCloud（最终，转换的目标云，转换矩阵）；
pcl:：点云转换的输入云；
Eigen:：Isometry3f姿势（Eigen:：Isometry3f）（Eigen:：Translation3f(
输入云传感器原点[0]，
输入云传感器原点[1]，
输入\云传感器\原点\[2]）*
Eigen:：Isometry3f（输入云传感器方向）；
变换矩阵=姿势矩阵（）；
pcl:：transformPointCloud（输入云、转换云、转换矩阵）；
//现在，两个点云都转换为原点，可以相加
pcl:：点云输出\点云；
输出_cloud+=转换后的_输入_cloud；
输出_cloud+=转换的_目标_cloud；
它仍然没有对齐，我不知道我做错了什么。我已经更新了帖子中的代码，请看一看，你能以*.PCD文件的形式共享两个初始点云吗？所以我可以试试。也许ICP不会收敛到最优变换。最大迭代次数为1000，最大对应距离为200的设置似乎太大。这是第一个点云，这是第二个点云。请查看更新的代码，我正在尝试提取一个平面，然后使用icpdid与一个平面进行匹配。您找到解决方案了吗？您研究过这个示例吗。但我认为这与最初的问题无关！我不知道我做错了什么。我已经更新了帖子中的代码，请看一看，你能以*.PCD文件的形式共享两个初始点云吗？所以我可以试试。也许ICP不会收敛到最优变换。最大迭代次数为1000，最大对应距离为200的设置似乎太大。这是第一个点云，这是第二个点云。请查看更新的代码，我正在尝试提取一个平面，然后使用icpdid与一个平面进行匹配。您找到解决方案了吗？您研究过这个示例吗。但我认为这与最初的问题无关！