利用ICP进行点云配准(两个输入为txt)

//https://lorensen.github.io/VTKExamples/site/Cxx/Filtering/IterativeClosestPointsTransform/
//渲染出来红色为source这是待转换的点云；绿色为target，这是金标准；蓝色为solution后的结果由source向金标准转换
#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>

int main(int argc, char *argv[]) {
    vtkSmartPointer<vtkPolyData> source =
        vtkSmartPointer<vtkPolyData>::New();
    vtkSmartPointer<vtkPolyData> target =
        vtkSmartPointer<vtkPolyData>::New();
    //-----------------------------------第一个输入source-------------------------------------------------------------------
    std::string filename = "../data/f_9";//2的副本.txt
    std::ifstream filestream(filename.c_str()); //文件流
    std::string line;

    vtkSmartPointer<vtkPoints> m_Points = vtkSmartPointer<vtkPoints>::New();
    while(std::getline(filestream, line)) { //整行读取文件
        double x, y, z;
        int i = 0;
        std::stringstream linestream;
        linestream << line;
        linestream >> x >> y >> z;
        m_Points->InsertNextPoint(x, y, z);  //新读取的数据赋予点的几何结构
    }
    filestream.close();  //关闭文件流操作
    //
    vtkSmartPointer<vtkPolyData> PointsPolyData = vtkSmartPointer<vtkPolyData>::New();
    PointsPolyData->SetPoints(m_Points);
    //这里我想看看有多少个点构成source
    vtkIdType PointsNumber1 = PointsPolyData->GetNumberOfPoints();
    std::cout << "输入TXT构成source几何数据"
              "点数：" << PointsNumber1 - 1 << std::endl;
    vtkSmartPointer<vtkVertexGlyphFilter> vertexGlyphFilter = vtkSmartPointer<vtkVertexGlyphFilter>::New();
    vertexGlyphFilter->SetInputData(PointsPolyData);
    vertexGlyphFilter->Update();

    source->ShallowCopy(vertexGlyphFilter->GetOutput());
//-----------------------------------第二个输入target---------------------------
 
    std::string filename2 = "../data/f_10";//2.txt
    std::ifstream filestream2(filename2.c_str()); //文件流
    std::string line2;

    vtkSmartPointer<vtkPoints> m_Points2 = vtkSmartPointer<vtkPoints>::New();
    while(std::getline(filestream2, line2)) { //整行读取文件
        double x2, y2, z2;
        int i2 = 0;
        std::stringstream linestream2;
        linestream2 << line2;
        // cout << line << endl;
        linestream2 >> x2 >> y2 >> z2;
        m_Points2->InsertNextPoint(x2, y2, z2);  //新读取的数据赋予点的几何结构
    }
    filestream2.close();  //关闭文件流操作
    vtkSmartPointer<vtkPolyData> PointsPolyData2 = vtkSmartPointer<vtkPolyData>::New();
    PointsPolyData2->SetPoints(m_Points2);
    vtkIdType PointsNumber = PointsPolyData2->GetNumberOfPoints();
    std::cout << "输入TXT构成target几何数据点数：" << PointsNumber - 1 << std::endl;
    vtkSmartPointer<vtkVertexGlyphFilter> vertexGlyphFilter2 = vtkSmartPointer<vtkVertexGlyphFilter>::New();
    vertexGlyphFilter2->SetInputData(PointsPolyData2);//AddInputData(VesselPointsPolyData);
    vertexGlyphFilter2->Update();
    target->ShallowCopy(vertexGlyphFilter2->GetOutput());

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

    // 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();
    icpTransformFilter->SetInputData(source);
    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();
    sourceMapper->SetInputData(source);

    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();
    targetMapper->SetInputData(target);

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

    //
    vtkSmartPointer<vtkRenderer> renderer =
        vtkSmartPointer<vtkRenderer>::New();
    vtkSmartPointer<vtkRenderWindow> renderWindow =
        vtkSmartPointer<vtkRenderWindow>::New();
    renderWindow->AddRenderer(renderer);
    vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor =
        vtkSmartPointer<vtkRenderWindowInteractor>::New();
    renderWindowInteractor->SetRenderWindow(renderWindow);

    //
    renderer->AddActor(sourceActor);
    renderer->AddActor(targetActor);
    renderer->AddActor(solutionActor);
    renderer->SetBackground(.4, .5, .7); // 整个背景颜色-浅蓝色

    //
    renderWindow->Render();
    renderWindowInteractor->Start();
// ----------------------------可视化管道结束----------------------------------
    return EXIT_SUCCESS;
}