提要

ICP算法简介

ICP算法最初由Besl和Mckey提出，是一种基于轮廓特征的点配准方法。基准点在CT图像坐标系及世界坐标系下的坐标点集P = {Pi, i = 0,1, 2,…,k}及U = {Ui,i=0,1,2,…,n}。其中，U与P元素间不必存在一一对应关系，元素数目亦不必相同，设k≥n。配准过程就是求取2个坐标系间的旋转和平移变换矩阵，使得来自U与P的同源点间距离最小。其过程如下：
(1)计算最近点，即对于集合U中的每一个点，在集合P中都找出距该点最近的对应点，设集合P中由这些对应点组成的新点集为Q = {qi,i = 0,1,2,…,n}。
(2)采用最小均方根法，计算点集U与Q之间的配准，使得到配准变换矩阵R，T，其中R是3×3的旋转矩阵，T是3×1的平移矩阵。
(3)计算坐标变换，即对于集合U，用配准变换矩阵R，T进行坐标变换，得到新的点集U1，即U1 = RU + T
(4)计算U1与Q之间的均方根误差，如小于预设的极限值ε，则结束，否则，以点集U1替换U，重复上述步骤。

数学描述（感觉更好理解一些）

三维空间中两个3D点， ，他们的欧式距离表示为：

三维点云匹配问题的目的是找到P和Q变化的矩阵R和T，对于 ，，利用最小二乘法求解最优解使：

VTK中有一个类vtkIterativeClosestPointTransform实现了ICP算法，并将ICP算法保存在一个4×4的齐次矩阵中。下面就跟着官方demo来实践一下。

安装库

升级cmake

编译VTK6.1需要cmake2.8.8以上。

下载cmake2.8.12.2

解压终端cd进目录

sudo ./bootstrap

make

sudo make install

编译VTK6.1

官网下载解压终端cd进目录

mkdir  build

cd build

cmake ..

make

sudo make install

实战

ICP的输入是两个点云，这两个点云必须是针对同一个场景，而且必须有重叠部分。

这里关乎格式转换、读取的问题的。，对新手来说，xyz是做好的读取文件了，只含有坐标信息，而且是文本信息。如果不是.xyz格式，用meshlab导出一个ply，把文件头部的说明去掉，扩展名改成xyz就可以了。

代码：

#include <vtkVersion.h>
#include <vtkSmartPointer.h>
#include <vtkTransform.h>
#include <vtkVertexGlyphFilter.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkIterativeClosestPointTransform.h>
#include <vtkTransformPolyDataFilter.h>
#include <vtkLandmarkTransform.h>
#include <vtkMath.h>
#include <vtkMatrix4x4.h>
#include <vtkXMLPolyDataWriter.h>
#include <vtkPolyDataMapper.h>
#include <vtkActor.h>
#include <vtkRenderWindow.h>
#include <vtkRenderer.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkXMLPolyDataReader.h>
#include <vtkProperty.h>
#include <vtkPLYReader.h>
#include <sstream>
#include <iostream>int main(int argc, char *argv[])
{vtkSmartPointer<vtkPolyData> sourceTmp =vtkSmartPointer<vtkPolyData>::New();vtkSmartPointer<vtkPolyData> targetTmp =vtkSmartPointer<vtkPolyData>::New();vtkSmartPointer<vtkPolyData> source =vtkSmartPointer<vtkPolyData>::New();vtkSmartPointer<vtkPolyData> target =vtkSmartPointer<vtkPolyData>::New();if(argc == 3){// Get all data from the filestd::string strSource = argv[1];std::string strTarget = argv[2];std::ifstream fSource(strSource.c_str());std::ifstream fTarget(strTarget.c_str());std::string line;vtkSmartPointer<vtkPoints> sourcePoints =vtkSmartPointer<vtkPoints>::New();vtkSmartPointer<vtkPoints> targetPoints =vtkSmartPointer<vtkPoints>::New();while(std::getline(fSource, line)){double x,y,z;std::stringstream linestream;linestream << line;linestream >> x >> y >> z;sourcePoints->InsertNextPoint(x, y, z);}sourceTmp->SetPoints(sourcePoints);vtkSmartPointer<vtkVertexGlyphFilter> vertexFilter1 =vtkSmartPointer<vtkVertexGlyphFilter>::New();
#if VTK_MAJOR_VERSION <= 5vertexFilter1->SetInputConnection(sourceTmp->GetProducerPort());
#elsevertexFilter1->SetInputData(sourceTmp);
#endifvertexFilter1->Update();source->ShallowCopy(vertexFilter1->GetOutput());while(std::getline(fTarget, line)){double x,y,z;std::stringstream linestream;linestream << line;linestream >> x >> y >> z;targetPoints->InsertNextPoint(x, y, z);}targetTmp->SetPoints(targetPoints);vtkSmartPointer<vtkVertexGlyphFilter> vertexFilter2 =vtkSmartPointer<vtkVertexGlyphFilter>::New();
#if VTK_MAJOR_VERSION <= 5vertexFilter2->SetInputConnection(targetTmp->GetProducerPort());
#elsevertexFilter2->SetInputData(targetTmp);
#endifvertexFilter2->Update();target->ShallowCopy(vertexFilter2->GetOutput());}else{std::cout << "Error data..." << std::endl;}// Setup ICP transformvtkSmartPointer<vtkIterativeClosestPointTransform> icp =vtkSmartPointer<vtkIterativeClosestPointTransform>::New();icp->SetSource(source);icp->SetTarget(target);icp->GetLandmarkTransform()->SetModeToRigidBody();icp->SetMaximumNumberOfIterations(20);//icp->StartByMatchingCentroidsOn();icp->Modified();icp->Update();cout<<"bitch"<<endl;// Get the resulting transformation matrix (this matrix takes the source points to the target points)vtkSmartPointer<vtkMatrix4x4> m = icp->GetMatrix();std::cout << "The resulting matrix is: " << *m << std::endl;// Transform the source points by the ICP solutionvtkSmartPointer<vtkTransformPolyDataFilter> icpTransformFilter =vtkSmartPointer<vtkTransformPolyDataFilter>::New();
#if VTK_MAJOR_VERSION <= 5icpTransformFilter->SetInput(source);
#elseicpTransformFilter->SetInputData(source);
#endificpTransformFilter->SetTransform(icp);icpTransformFilter->Update();/*// If you need to take the target points to the source points, the matrix is:icp->Inverse();vtkSmartPointer<vtkMatrix4x4> minv = icp->GetMatrix();std::cout << "The resulting inverse matrix is: " << *minv << std::cout;*/// VisualizevtkSmartPointer<vtkPolyDataMapper> sourceMapper =vtkSmartPointer<vtkPolyDataMapper>::New();
#if VTK_MAJOR_VERSION <= 5sourceMapper->SetInputConnection(source->GetProducerPort());
#elsesourceMapper->SetInputData(source);
#endifvtkSmartPointer<vtkActor> sourceActor =vtkSmartPointer<vtkActor>::New();sourceActor->SetMapper(sourceMapper);sourceActor->GetProperty()->SetColor(1,0,0);sourceActor->GetProperty()->SetPointSize(4);vtkSmartPointer<vtkPolyDataMapper> targetMapper =vtkSmartPointer<vtkPolyDataMapper>::New();
#if VTK_MAJOR_VERSION <= 5targetMapper->SetInputConnection(target->GetProducerPort());
#elsetargetMapper->SetInputData(target);
#endifvtkSmartPointer<vtkActor> targetActor =vtkSmartPointer<vtkActor>::New();targetActor->SetMapper(targetMapper);targetActor->GetProperty()->SetColor(0,1,0);targetActor->GetProperty()->SetPointSize(4);vtkSmartPointer<vtkPolyDataMapper> solutionMapper =vtkSmartPointer<vtkPolyDataMapper>::New();solutionMapper->SetInputConnection(icpTransformFilter->GetOutputPort());vtkSmartPointer<vtkActor> solutionActor =vtkSmartPointer<vtkActor>::New();solutionActor->SetMapper(solutionMapper);solutionActor->GetProperty()->SetColor(0,0,1);solutionActor->GetProperty()->SetPointSize(3);// Create a renderer, render window, and interactorvtkSmartPointer<vtkRenderer> renderer =vtkSmartPointer<vtkRenderer>::New();vtkSmartPointer<vtkRenderWindow> renderWindow =vtkSmartPointer<vtkRenderWindow>::New();renderWindow->AddRenderer(renderer);vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor =vtkSmartPointer<vtkRenderWindowInteractor>::New();renderWindowInteractor->SetRenderWindow(renderWindow);// Add the actor to the scenerenderer->AddActor(sourceActor);renderer->AddActor(targetActor);renderer->AddActor(solutionActor);renderer->SetBackground(.3, .6, .3); // Background color green// Render and interactrenderWindow->Render();renderWindowInteractor->Start();return EXIT_SUCCESS;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)PROJECT(IterativeClosestPointsTransform)find_package(VTK REQUIRED)
include(${VTK_USE_FILE})add_executable(IterativeClosestPointsTransform MACOSX_BUNDLE IterativeClosestPointsTransform)if(VTK_LIBRARIES)target_link_libraries(IterativeClosestPointsTransform ${VTK_LIBRARIES})
else()target_link_libraries(IterativeClosestPointsTransform vtkHybrid)
endif()

编译运行一下，用两片点云来测试，得到的结果：

微小的点云平移：

稍微大一些的平移

加入旋转量

绿色是target，红色是source，蓝色是solution。

结论和思考

和同学一起试用了几种ICP的方法，包括PCL的和VTK的，得到的结果都差不多。并不是很理想，感觉最好的Registration适用情况应该是从不同方位扫描一个物体，然后将点云进行配准，而且点云的算法的初始状态也有要求，一是要有点云的重合，二是不能分开得太远。

难道就这样结束了？

答案是No... 难道传说中的ICP这点配准都搞不定！？那也太弱了吧。

继续看论文和尝试.

这次改用PCL的库来实现。

用blender基于stanford bunny来做一组测试数据

按照PCL的pipeline，首先采用的是进行一个初始化操作，将点云进行一次预处理，得到一个稍微好一点的结果，这里用到的是SAC-IA的算法，流程如下：

SAC-IA: Sampled Consesus-Initial Alignment
1. Draw n points di from the source cloud
(with a minimum distance d in between).
2. For each drawn di :
2.1 get k closest matches, and
2.2 draw one of the k closest matches as mi
(instead of taking closest match)
3. Estimate transformation (R, t) for these samples
4. Determine inlier pairs with ((Rdi + t) − mi )2 <
5. Repeat N times, and use (R, t) having most inliers

想搞懂算法的自己扒论文，只想知道怎么用的和我来看代码：

template_alignment.cpp

#include <iostream>
#include <limits>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/PolygonMesh.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/PolygonMesh.h>
#include <pcl/visualization/histogram_visualizer.h>
#include <boost/thread/thread.hpp>class FeatureCloud
{public:// A bit of shorthandtypedef pcl::PointCloud<pcl::PointXYZ> PointCloud;typedef pcl::PointCloud<pcl::Normal> SurfaceNormals;typedef pcl::PointCloud<pcl::FPFHSignature33> LocalFeatures;typedef pcl::search::KdTree<pcl::PointXYZ> SearchMethod;FeatureCloud () :search_method_xyz_ (new SearchMethod),normal_radius_ (0.06f),feature_radius_ (0.06f){}~FeatureCloud () {}// Process the given cloudvoidsetInputCloud (PointCloud::Ptr xyz){xyz_ = xyz;processInput ();}// Load and process the cloud in the given PCD filevoidloadInputCloud (const std::string &pcd_file){xyz_ = PointCloud::Ptr (new PointCloud);pcl::io::loadPCDFile (pcd_file, *xyz_);processInput ();}// Get a pointer to the cloud 3D pointsPointCloud::PtrgetPointCloud () const{return (xyz_);}// Get a pointer to the cloud of 3D surface normalsSurfaceNormals::PtrgetSurfaceNormals () const{return (normals_);}// Get a pointer to the cloud of feature descriptorsLocalFeatures::PtrgetLocalFeatures () const{return (features_);}protected:// Compute the surface normals and local featuresvoidprocessInput (){computeSurfaceNormals ();computeLocalFeatures ();}// Compute the surface normalsvoidcomputeSurfaceNormals (){normals_ = SurfaceNormals::Ptr (new SurfaceNormals);pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> norm_est;norm_est.setInputCloud (xyz_);norm_est.setSearchMethod (search_method_xyz_);norm_est.setRadiusSearch (normal_radius_);norm_est.compute (*normals_);}// Compute the local feature descriptorsvoidcomputeLocalFeatures (){features_ = LocalFeatures::Ptr (new LocalFeatures);pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;fpfh_est.setInputCloud (xyz_);fpfh_est.setInputNormals (normals_);fpfh_est.setSearchMethod (search_method_xyz_);fpfh_est.setRadiusSearch (feature_radius_);fpfh_est.compute (*features_);}private:// Point cloud dataPointCloud::Ptr xyz_;SurfaceNormals::Ptr normals_;LocalFeatures::Ptr features_;SearchMethod::Ptr search_method_xyz_;// Parametersfloat normal_radius_;float feature_radius_;
};class TemplateAlignment
{public:// A struct for storing alignment resultsstruct Result{float fitness_score;Eigen::Matrix4f final_transformation;EIGEN_MAKE_ALIGNED_OPERATOR_NEW};TemplateAlignment () :min_sample_distance_ (0.02f),max_correspondence_distance_ (0.001f*0.001f),nr_iterations_ (1000){// Intialize the parameters in the Sample Consensus Intial Alignment (SAC-IA) algorithmsac_ia_.setMinSampleDistance (min_sample_distance_);sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);sac_ia_.setMaximumIterations (nr_iterations_);}~TemplateAlignment () {}// Set the given cloud as the target to which the templates will be alignedvoidsetTargetCloud (FeatureCloud &target_cloud){target_ = target_cloud;sac_ia_.setInputTarget (target_cloud.getPointCloud ());sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());}// Add the given cloud to the list of template cloudsvoidaddTemplateCloud (FeatureCloud &template_cloud){templates_.push_back (template_cloud);}// Align the given template cloud to the target specified by setTargetCloud ()voidalign (FeatureCloud &template_cloud, TemplateAlignment::Result &result){sac_ia_.setInputCloud (template_cloud.getPointCloud ());sac_ia_.setSourceFeatures (template_cloud.getLocalFeatures ());pcl::PointCloud<pcl::PointXYZ> registration_output;sac_ia_.align (registration_output);result.fitness_score = (float) sac_ia_.getFitnessScore (max_correspondence_distance_);result.final_transformation = sac_ia_.getFinalTransformation ();}// Align all of template clouds set by addTemplateCloud to the target specified by setTargetCloud ()voidalignAll (std::vector<TemplateAlignment::Result, Eigen::aligned_allocator<Result> > &results){results.resize (templates_.size ());for (size_t i = 0; i < templates_.size (); ++i){align (templates_[i], results[i]);}}// Align all of template clouds to the target cloud to find the one with best alignment scoreintfindBestAlignment (TemplateAlignment::Result &result){// Align all of the templates to the target cloudstd::vector<Result, Eigen::aligned_allocator<Result> > results;alignAll (results);// Find the template with the best (lowest) fitness scorefloat lowest_score = std::numeric_limits<float>::infinity ();int best_template = 0;for (size_t i = 0; i < results.size (); ++i){const Result &r = results[i];if (r.fitness_score < lowest_score){lowest_score = r.fitness_score;best_template = (int) i;}}// Output the best alignmentresult = results[best_template];return (best_template);}private:// A list of template clouds and the target to which they will be alignedstd::vector<FeatureCloud> templates_;FeatureCloud target_;// The Sample Consensus Initial Alignment (SAC-IA) registration routine and its parameterspcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia_;float min_sample_distance_;float max_correspondence_distance_;int nr_iterations_;
};int main()
{
//  pcl::PolygonMesh::Ptr obj_in (new pcl::PolygonMesh);//    //Read obj file.
//  if(pcl::io::loadPolygonFileOBJ("tree/tarotemplate.obj",*obj_in)==-1)
//  {
//      PCL_ERROR("Couldn't read file template.obj");
//      return -1;
//  }// std::cout<<"Loaded "
//           <<obj_in->cloud.width * obj_in->cloud.height
//           << " data points: "
//             << std::endl;//Transform obj to source PCD.pcl::PointCloud<pcl::PointXYZ>::Ptr tree_template(new pcl::PointCloud<pcl::PointXYZ>);//pcl::fromROSMsg(obj_in->cloud, *tree_template);pcl::io::loadPCDFile("source.pcd",*tree_template);FeatureCloud object_template;object_template.setInputCloud(tree_template);//Load taget point cloud.pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);pcl::io::loadPCDFile("target.pcd",*cloud);FeatureCloud target_cloud;target_cloud.setInputCloud(cloud);TemplateAlignment template_align;template_align.addTemplateCloud(object_template);template_align.setTargetCloud(target_cloud);TemplateAlignment::Result best_alignment;template_align.align(object_template, best_alignment);// Print the alignment fitness score (values less than 0.00002 are good)printf ("fitness score: %f\n", best_alignment.fitness_score);// Print the rotation matrix and translation vectorEigen::Matrix3f rotation = best_alignment.final_transformation.block<3,3>(0, 0);Eigen::Vector3f translation = best_alignment.final_transformation.block<3,1>(0, 3);printf ("\n");printf ("    | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));printf ("R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));printf ("    | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));printf ("\n");printf ("t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));// Save the aligned template for visualizationpcl::PointCloud<pcl::PointXYZ> transformed_cloud;pcl::transformPointCloud (*object_template.getPointCloud (), transformed_cloud, best_alignment.final_transformation);pcl::io::savePCDFileBinary ("output.pcd", transformed_cloud);pcl::visualization::PCLHistogramVisualizer hViewer;hViewer.addFeatureHistogram(*target_cloud.getLocalFeatures(),"fpfh",0);hViewer.addFeatureHistogram(*object_template.getLocalFeatures(),"fpfh",0,"cloud1");while(1){hViewer.spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}return 0;
}

CMakeList.txt

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)project(template_alignment)find_package(PCL 1.2 REQUIRED)include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})add_executable (template_alignment template_alignment.cpp)
target_link_libraries (template_alignment ${PCL_LIBRARIES})

编译运行，得到结果：

参考

【3D】迭代最近点算法 Iterative Closest Points

ICP算法(Iterative Closest Point)及VTK实现

ICCV2011-registration 下载

ICCV2011-initial_registration 下载

ICP in VTK相关推荐

1. 软件测试--MonkeyRunner(3)

   MonkeyRunner--小结 #monkeyrunner导入模块 from com.android.monkeyrunner import MonkeyRunner, MonkeyDevice, ...

2. 3.1-3.31推荐文章汇总

   3.1-3.31推荐文章汇总 [Eclipse AST]AST的创建   刘伟 Android WebKit HTML主资源加载过程   谭海燕 HTML5物理游戏开发 - 越野山地自行车(一)建立各 ...

3. VTK修炼之道58：图形基本操作进阶_点云配准技术（迭代最近点ICP算法）

   1.Iterative Closest Points算法 点云数据配准最经典的方法是迭代最近点算法(Iterative Closest Points,ICP).ICP算法是一个迭代的过程,每次迭代中对 ...

4. QT4.8界面设计(MSVC2010X)+位姿哈希+ICP结果

   位姿检索的哈希算法:https://blog.csdn.net/wishchin/article/details/19164875 1.C++ IDE设计 MFC这种半死不活的windows C++平 ...

5. PCL点云库：ICP算法

   ICP(Iterative Closest Point迭代最近点)算法是一种点集对点集配准方法.在VTK.PCL.MRPT.MeshLab等C++库或软件中都有实现,可以参见维基百科中的ICP Alg ...

6. vtk能干什么（适用范围）

   参考博客:VTK能做什么_51CTO博客_vtk是什么 1.求一点与一条线之间的距离 2.两点之间的距离 3.生成均匀分布的随机数 4.生成高斯分布的随机数 5.确定点是否在面上 6.读取xgml文件 ...

7. PCL ICP算法概述总结

   ICP (iterative closest points) 点云配准算法 经典的icp算法(可以参考Chen and Medioni,[2] and Besl and McKay.[1]),对比于 ...

8. vtk相机_C#开发PACS医学影像三维重建(一)使用VTK重建3D影像

   VTK简介: VTK是一个开源的免费软件系统,主要用于三维计算机图形学.图像处理和可视化.Vtk是在面向对象原理的基础上设计和实现的,它的内核是用C++构建的. 因为使用C#语言开发,而VTK是C++ ...

9. cmake开发环境 linux qt_一步步搭建CMake+QT+VTK+BOOST开发环境

   开发环境: 1.可以迅速处理大量并发网络数据包,ASIO库和winpcap 4.1.3库; 2.处理与显示点云模型数据,使用VTK 8.2.0库; 3.项目使用CMake+VC2017编译,GUI使用 ...

最新文章

1. 1、数据库设计的基本步骤
2. ThinkCMF 和 OneThink内容管理系统对比
3. JAVA去掉指定字符
4. vue父组件异步获取动态数据传递给子组件获取不到值
5. python学生管理系统类图_类图 python
6. cmake安装配置及入门指南
7. Android 使用JSON格式与服务器交互 中文乱码问题解决
8. java 基础 1 final关键字
9. 【爬虫】获取新郑机场出租车实时数据
10. 如何使用迅雷下载百度网盘资源
11. 像中文的罗马音字体复制_罗马音字体大全可复制中文
12. win7旗舰版升级成win7SP1
13. Kaggle文本可读性识别大赛银牌方案复盘
14. 24.4. Prompting
15. (补充)微信长按识别二维码 -- 页面多个二维码如何识别？（二）
16. ShareSDK iOS端微信如何获取authcode值
17. 深大uooc学术道德与学术规范教育第九章
18. 多级树形目录mysql的使用_实现树形的遍历（关于多级菜单栏以及多级上下部门的查询问题）...
19. 国外搜索引擎+视频网站
20. 关系型数据库到文档型数据库的跨越

热门文章

1. Oracle从零开始-关系型数据库介绍
2. 移动端 - Android客户端性能测试常见指标
3. 提示Microsoft office word 遇到问题需要关闭。还问是否发送错误报告。
4. Nginx网站服务(安装nginx、平滑升级nginx、nginx各种访问配置)
5. 人过青年，我们的黄金时代过去了吗？
6. VBA 计算两个时间相差多少分钟
7. C++之命令(Command)模式
8. 云之讯php短信接口,菏泽java云之讯短信接口价格如何计算？ 新锐信息科技服务至上...
9. python 斗图图片爬虫
10. android 文本转语音