提要

? ? ? ? ?今天要研究的是關于圖像配準的問題，圖像配準是圖像處理研究領域中的一個典型問題和技術難點，其目的在于比較或融合針對同一對象在不同條件下獲取的圖像，例如圖像會來自不同的采集設備，取自不同的時間，不同的拍攝視角等等，有時也需要用到針對不同對象的圖像配準問題。具體地說，對于一組圖像數據集中的兩幅圖像，通過尋找一種空間變換把一幅圖像映射到另一幅圖像，使得兩圖中對應于空間同一位置的點一一對應起來，從而達到信息融合的目的。?
? ? ? ?一個經典的應用是場景的重建，比如說一張茶幾上擺了很多杯具，用深度攝像機進行場景的掃描，通常不可能通過一次采集就將場景中的物體全部掃描完成，只能是獲取場景不同角度的點云，然后將這些點云融合在一起，獲得一個完整的場景。
? ? ? ? 對于點云的配準，給定一個源點云和一個目標點云，配準可以簡單地分為三個步驟： ● 找配準對（correspondence pairs）；
● 計算配準對之間的變換矩陣；
● 將對應的變換施加到源點云上；
* crrespondences 可以是點，特征等等。
? ? ? ? ICP算法是圖像配準極其重要的算法之一。

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，對于?，，利用最小二乘法求解最優解使：

最小時的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的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。