ICP配准

欧神

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/range_image_visualizer.h>
void visualize_pcd(pcl::PointCloud<pcl::PointXYZ>::Ptr pcd)
{
// Visualization of keypoints along with the original cloud
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color_handler (pcd, 255, 0, 0);
viewer.setBackgroundColor( 0.0, 0.0, 0.0 );
viewer.addPointCloud(pcd, cloud_color_handler, "cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
while(!viewer.wasStopped ())
{
  viewer.spinOnce ();
}
}
void visualize_pcd(pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_src1,
       pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_src2,
       pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_final)
{
int vp_1, vp_2;
// Create a PCLVisualizer object
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
viewer.createViewPort (0.0, 0, 0.5, 1.0, vp_1);
viewer.createViewPort (0.5, 0, 1.0, 1.0, vp_2);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src1_h (pcd_src1, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src2_h (pcd_src2, 255, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h (pcd_final, 0, 255, 0);
viewer.addPointCloud (pcd_src1, src1_h, "vp1_src1", vp_1);
viewer.addPointCloud (pcd_src2, src2_h, "vp1_src2", vp_1);
viewer.addPointCloud (pcd_final, final_h, "vp1_final", vp_2);
viewer.spin();
}
int main ()
{
std::string fname_src = "D:/pcl_test/robot1.pcd";
std::string fname_tgt = "D:/pcl_test/robot2.pcd";
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt (new pcl::PointCloud<pcl::PointXYZ>);
if(pcl::io::loadPCDFile<pcl::PointXYZ> (fname_src, *cloud_src) == -1)
{
  PCL_ERROR("Couldn't read file");
  return -1;
}
if(pcl::io::loadPCDFile<pcl::PointXYZ> (fname_tgt, *cloud_tgt) == -1)
{
  PCL_ERROR("Couldn't read file");
  return -1;
}
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputCloud(cloud_src);
icp.setInputTarget(cloud_tgt);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_final (new pcl::PointCloud<pcl::PointXYZ>);;
icp.align(*cloud_final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
pcl::io::savePCDFileASCII("D://PCL//icp_result.pcd", *cloud_final);
//visualize_pcd(Final);
visualize_pcd(cloud_src, cloud_tgt, cloud_final);
return (0);
}

等你的白兔

可以把配准的结果数据贴出来吗？
R，t， score

欧神

等你的白兔 发表于 2013-8-15 22:25
可以把配准的结果数据贴出来吗？
R，t， score

has converged:1 score: 0.000226586
  0.806977    0.10678  -0.580851   0.747677
  -0.11097   0.993418  0.0284526 -0.0379766
  0.580064  0.0414963   0.813515   0.239948
         0          0          0          1

等你的白兔

欧神 发表于 2013-8-15 23:21
has converged:1 score: 0.000226586
  0.806977    0.10678  -0.580851   0.747677
  -0.11097   0.9934 ...

看你的结果数据也不是很好啊，score小数点后才三位，精度一般啊，怎么看你贴的图结果只有一个颜色啊？感觉好像只有一幅图，还是重叠了？

欧神

等你的白兔 发表于 2013-8-16 21:58
看你的结果数据也不是很好啊，score小数点后才三位，精度一般啊，怎么看你贴的图结果只有一个颜色啊？感 ...

我把使用的数据贴上来，你可以运行下试试，具体细节我还没有分析，期待你的分析结果哈！

等你的白兔

欧神 发表于 2013-8-16 22:15
我把使用的数据贴上来，你可以运行下试试，具体细节我还没有分析，期待你的分析结果哈！ ...

我之前试了试FPFH特征初始配准，score达到小数点后5个零效果看起来才可以，然后我试了试ICP优化看能不能提高，结果还不如初始配准呢。

等你的白兔

欧神 发表于 2013-8-16 22:15
我把使用的数据贴上来，你可以运行下试试，具体细节我还没有分析，期待你的分析结果哈！ ...

这个是我用bunny的数据做的实验中最好的一个，不是ICP，但精度还行，不过速度很不好哦。
我QQ：460339076 宋景豪

欧神

等你的白兔 发表于 2013-8-18 11:04
这个是我用bunny的数据做的实验中最好的一个，不是ICP，但精度还行，不过速度很不好哦。
我QQ：46033907 ...

qq以加上了

等你的白兔

欧神 发表于 2013-8-18 17:03
qq以加上了

好的，我一般隐身的，直接给我发信息就行。

MarkingNH

等你的白兔 发表于 2013-8-18 10:59
我之前试了试FPFH特征初始配准，score达到小数点后5个零效果看起来才可以，然后我试了试ICP优化看能不能 ...

你好，我也在尝试例程中的ICP代码，但是理论上我不是很清楚，理论上应该是在原点云中提取特征点，然后基于特征点提取变换矩阵，但是PCL提供的源代码中我不清楚那几句是提取特征点信息，以及采用哪种特征点进行计算？