ICP in VTK

时间：2014-03-18 09:19:33 阅读：716 评论：0 收藏：0 [点我收藏+]

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点， bubuko.com,布布扣

，他们的欧式距离表示为：

三维点云匹配问题的目的是找到P和Q变化的矩阵R和T，对于 bubuko.com,布布扣

，

，利用最小二乘法求解最优解使：

最小时的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 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;
}

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()

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

微小的点云平移：

bubuko.com,布布扣