pcl使用迭代的最近的点

来源：互联网发布：淘宝新店防骗编辑：程序博客网时间：2024/06/04 19:46

这次我们将使用Iterative Closest Point（可迭代的最近点）算法，这个能决定是否一个点云是一个另一个点云的强制转换，通过最小化两个点云之间的点的距离。

#include <iostream>#include <pcl/io/pcd_io.h>#include <pcl/point_types.h>#include <pcl/registration/icp.h>int main (int argc, char** argv){  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);  // Fill in the CloudIn data  cloud_in->width    = 5;  cloud_in->height   = 1;  cloud_in->is_dense = false;  cloud_in->points.resize (cloud_in->width * cloud_in->height);  for (size_t i = 0; i < cloud_in->points.size (); ++i)  {    cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);    cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);    cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);  }  std::cout << "Saved " << cloud_in->points.size () << " data points to input:"      << std::endl;  for (size_t i = 0; i < cloud_in->points.size (); ++i) std::cout << "    " <<      cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<      cloud_in->points[i].z << std::endl;  *cloud_out = *cloud_in;  std::cout << "size:" << cloud_out->points.size() << std::endl;  for (size_t i = 0; i < cloud_in->points.size (); ++i)    cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;  std::cout << "Transformed " << cloud_in->points.size () << " data points:"      << std::endl;  for (size_t i = 0; i < cloud_out->points.size (); ++i)    std::cout << "    " << cloud_out->points[i].x << " " <<      cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;  icp.setInputCloud(cloud_in);  icp.setInputTarget(cloud_out);  pcl::PointCloud<pcl::PointXYZ> Final;  icp.align(Final);  std::cout << "has converged:" << icp.hasConverged() << " score: " <<  icp.getFitnessScore() << std::endl;  std::cout << icp.getFinalTransformation() << std::endl; return (0);}

解释

包含了我们将要使用类的所有的头文件

#include <iostream>#include <pcl/io/pcd_io.h>#include <pcl/point_types.h>#include <pcl/registration/icp.h>

创建了两个pcl::PointCloud<pcl::PointXYZ>的boost shared 指针，并把它们进行初始化。

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);

随机生成数据

  // Fill in the CloudIn data  cloud_in->width    = 5;  cloud_in->height   = 1;  cloud_in->is_dense = false;  cloud_in->points.resize (cloud_in->width * cloud_in->height);  for (size_t i = 0; i < cloud_in->points.size (); ++i)  {    cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);    cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);    cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);  }  std::cout << "Saved " << cloud_in->points.size () << " data points to input:"      << std::endl;  for (size_t i = 0; i < cloud_in->points.size (); ++i) std::cout << "    " <<      cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<      cloud_in->points[i].z << std::endl;  *cloud_out = *cloud_in;  std::cout << "size:" << cloud_out->points.size() << std::endl;

接着执行一个简单的对点云的强制转换，并再次输出数据值

  for (size_t i = 0; i < cloud_in->points.size (); ++i)    cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;  std::cout << "Transformed " << cloud_in->points.size () << " data points:"      << std::endl;  for (size_t i = 0; i < cloud_out->points.size (); ++i)    std::cout << "    " << cloud_out->points[i].x << " " <<      cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;

创建了一个IterativeClosestPoint这个实例，给它一些有效的信息。

  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;  icp.setInputCloud(cloud_in);  icp.setInputTarget(cloud_out);

接着创建了一个pcl::PointCloud<pcl::PointXYZ>这个类，可以让IterativeClosestPoint把点云的数据存储在里面。

  pcl::PointCloud<pcl::PointXYZ> Final;  icp.align(Final);  std::cout << "has converged:" << icp.hasConverged() << " score: " <<  icp.getFitnessScore() << std::endl;  std::cout << icp.getFinalTransformation() << std::endl;

运行程序

./iterative_closest_point

结果

Saved 5 data points to input: 0.352222 -0.151883 -0.106395-0.397406 -0.473106 0.292602-0.731898 0.667105 0.441304-0.734766 0.854581 -0.0361733-0.4607 -0.277468 -0.916762size:5Transformed 5 data points: 1.05222 -0.151883 -0.106395 0.302594 -0.473106 0.292602-0.0318983 0.667105 0.441304-0.0347655 0.854581 -0.0361733      0.2393 -0.277468 -0.916762[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sampleselection distance threshold of: 0.200928[pcl::IterativeClosestPoint::computeTransformation] Number ofcorrespondences 4 [80.000000%] out of 5 points [100.0%], RANSAC rejected:1 [20.000000%].[pcl::IterativeClosestPoint::computeTransformation] Convergence reached.Number of iterations: 1 out of 0. Transformation difference: 0.700001has converged:1 score: 1.95122e-14          1  4.47035e-08 -3.25963e-09          0.72.98023e-08            1 -1.08499e-07 -2.98023e-081.30385e-08 -1.67638e-08            1  1.86265e-08          0            0            0            1

0 1