首页 > 其他 > 详细

ICP in VTK

时间:2014-03-18 09:19:33      阅读:714      评论: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,布布扣 ,他们的欧式距离表示为:
bubuko.com,布布扣
三维点云匹配问题的目的是找到P和Q变化的矩阵R和T,对于 bubuko.com,布布扣bubuko.com,布布扣,利用最小二乘法求解最优解使:
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,布布扣


稍微大一些的平移

bubuko.com,布布扣


加入旋转量

bubuko.com,布布扣


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


结论和思考

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


参考

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

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

ICP in VTK,布布扣,bubuko.com

ICP in VTK

原文:http://blog.csdn.net/silangquan/article/details/21413747

(0)
(0)
   
举报
评论 一句话评论(0
关于我们 - 联系我们 - 留言反馈 - 联系我们:wmxa8@hotmail.com
© 2014 bubuko.com 版权所有
打开技术之扣,分享程序人生!