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,重复上述步骤。
数学描述(感觉更好理解一些)
VTK中有一个类vtkIterativeClosestPointTransform实现了ICP算法,并将ICP算法保存在一个4×4的齐次矩阵中。下面就跟着官方demo来实践一下。
升级cmake
编译VTK6.1需要cmake2.8.8以上。
解压终端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 file std::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 <= 5 vertexFilter1->SetInputConnection(sourceTmp->GetProducerPort()); #else vertexFilter1->SetInputData(sourceTmp); #endif vertexFilter1->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 <= 5 vertexFilter2->SetInputConnection(targetTmp->GetProducerPort()); #else vertexFilter2->SetInputData(targetTmp); #endif vertexFilter2->Update(); target->ShallowCopy(vertexFilter2->GetOutput()); } else { std::cout << "Error data..." << std::endl; } // Setup ICP transform vtkSmartPointer<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 solution vtkSmartPointer<vtkTransformPolyDataFilter> icpTransformFilter = vtkSmartPointer<vtkTransformPolyDataFilter>::New(); #if VTK_MAJOR_VERSION <= 5 icpTransformFilter->SetInput(source); #else icpTransformFilter->SetInputData(source); #endif icpTransformFilter->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; */ // Visualize vtkSmartPointer<vtkPolyDataMapper> sourceMapper = vtkSmartPointer<vtkPolyDataMapper>::New(); #if VTK_MAJOR_VERSION <= 5 sourceMapper->SetInputConnection(source->GetProducerPort()); #else sourceMapper->SetInputData(source); #endif vtkSmartPointer<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 <= 5 targetMapper->SetInputConnection(target->GetProducerPort()); #else targetMapper->SetInputData(target); #endif vtkSmartPointer<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 interactor vtkSmartPointer<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 scene renderer->AddActor(sourceActor); renderer->AddActor(targetActor); renderer->AddActor(solutionActor); renderer->SetBackground(.3, .6, .3); // Background color green // Render and interact renderWindow->Render(); renderWindowInteractor->Start(); return EXIT_SUCCESS; }
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适用情况应该是从不同方位扫描一个物体,然后将点云进行配准,而且点云的算法的初始状态也有要求,一是要有点云的重合,二是不能分开得太远。
【3D】迭代最近点算法 Iterative Closest Points
ICP算法(Iterative Closest Point)及VTK实现
原文:http://blog.csdn.net/silangquan/article/details/21413747