How to use iterative closest point
http://pointclouds.org/documentation/tutorials/iterative_closest_point.php#iterative-closest-point
如何拼接点云
不知道icp要求具体有多高,这里是先构建点云A(就几个点),然后B在A的基础上加一点扰动,然后利用icp算法计算A到B的刚体变换矩阵。
#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);
//构建点云A
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;
//点云B在A的基础上稍作改动
*cloud_out = *cloud_in;
std::cout << "size:" << cloud_out->points.size() << std::endl;
//准确来说只是在x周上动一动
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;
//构建icp
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
//将A 和 B喂入 icp
icp.setInputCloud(cloud_in);
icp.setInputTarget(cloud_out);
//这个Final 有啥用?
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
//如果点云B 只是由 A进行一些简单的刚体变换得来的,icp.hasConverged()值1,如果存在形变则不为1
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
//输出刚体变换矩阵信息
std::cout << icp.getFinalTransformation() << std::endl;
return (0);
}