VTK_Learning_ICP_Registration

1.ICP配准效果图

       

2.ICP配准代码

main.cpp

#include <QCoreApplication>
#include <QDebug>
#include <iostream>
#include <sstream>

#include <QtCore>

#include "ndi_capi.h"
#include "pointclouddata.h"

#include <QGuiApplication>
#include <QQmlApplicationEngine>

#include "tempframe.h"
#include "tempdataprocessor.h"
#include "tempdatarepository.h"
#include "tempthread001.h"
#include "registrationpointcloud.h"

#include <QMainWindow>
#include <QWidget>

#include <QGuiApplication>
#include <QQuickView>
#include <QtQml>
#include <QQmlApplicationEngine>


//vtk
#include <vtkActor.h>
#include <vtkCamera.h>
#include <vtkCylinderSource.h>
#include <vtkNamedColors.h>
#include <vtkPolyDataMapper.h>
#include <vtkProperty.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkRenderer.h>


#include <array>

#include <vtkAutoInit.h>
VTK_MODULE_INIT(vtkRenderingOpenGL2);
VTK_MODULE_INIT(vtkRenderingFreeType);
VTK_MODULE_INIT(vtkInteractionStyle);

#include <vtkSmartPointer.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyData.h>
#include <vtkTransform.h>
#include <vtkTransformPolyDataFilter.h>
#include <vtkVertexGlyphFilter.h>
#include <vtkIterativeClosestPointTransform.h>
#include <vtkLandmarkTransform.h>
#include <vtkTransformPolyDataFilter.h>
#include <vtkPolyDataMapper.h>
#include <vtkActor.h>
#include <vtkProperty.h>
#include <vtkAxesActor.h>
#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkOrientationMarkerWidget.h> //坐标系交互


int main(int argc, char *argv[])
{
    QCoreApplication::setAttribute(Qt::AA_EnableHighDpiScaling);
    qmlRegisterType<TempFrame>("qian.qml.TempInterface", 1, 0, "TempFrame");
    qmlRegisterType<Tool>("qian.qml.TempInterface", 1, 0, "Tool");
    qmlRegisterType<MyStrayMarker>("qian.qml.TempInterface", 1, 0, "MyStrayMarker");


    QGuiApplication app(argc, argv);
//    QObject *w= new QObject();
//    TempThread001 *m_tempThread;
//    m_tempThread = new TempThread001(w);

//    TempDataProcessor *m_tempDataProcessor=new TempDataProcessor();
//    TempDataRepository *m_tempDataRepository=new TempDataRepository();
//    m_tempDataProcessor->setTempDataRepository(m_tempDataRepository);


//    m_tempThread->setTempDataProcessor(m_tempDataProcessor);
//    m_tempThread->start();


//    QQmlApplicationEngine engine;
//    engine.rootContext()->setContextProperty("NDI_DataRepository",m_tempDataRepository);
//    engine.rootContext()->setContextProperty("NDI_DataProcessor",m_tempDataProcessor);
//    engine.load(QUrl(QStringLiteral("qrc:/qml/main.qml")));
//    if (engine.rootObjects().isEmpty())
//     return -1;



    vtkSmartPointer<vtkNamedColors> colors = vtkSmartPointer<vtkNamedColors>::New();
    std::array<unsigned char, 4> skinColor{ {255, 125, 64} };
    colors->SetColor("SkinColor", skinColor.data());
    std::array<unsigned char, 4> bkg{ {51, 77, 102, 255} };
    colors->SetColor("BkgColor", bkg.data());

    RegistrationPointCloud registrationIreach;
    std::vector<std::vector<float>> tempMatrix;

    vtkSmartPointer<vtkPolyData> orig ;
    vtkSmartPointer<vtkPolyData> targ ;

    tempMatrix.clear();
    registrationIreach.getVectorFieldData("../../pointCloud/test_3D_20191205145441.xyz",tempMatrix);
    registrationIreach.writeDataToVtk(orig,tempMatrix);

     std::vector<std::vector<float>> tempMatrix_1;
    tempMatrix_1.clear();
    registrationIreach.getVectorFieldData("../../DCM_points/sun_test.xyz",tempMatrix_1);
    registrationIreach.writeDataToVtk(targ,tempMatrix_1);
    std::cout<<"tempMatrix.size()"<<tempMatrix.size()<<"\t"<<tempMatrix_1.size()<<"\n" <<std::endl  ; //源数据 与 目标数据
    vtkSmartPointer<vtkPolyData> source =
        vtkSmartPointer<vtkPolyData>::New();
    source->SetPoints(orig->GetPoints());

    vtkSmartPointer<vtkPolyData> target =
            vtkSmartPointer<vtkPolyData>::New();
    target->SetPoints(targ->GetPoints());


    vtkSmartPointer<vtkVertexGlyphFilter>  sourceGlyph =
            vtkSmartPointer<vtkVertexGlyphFilter>::New();
    sourceGlyph->SetInputData(source);
    sourceGlyph->Update();

    vtkSmartPointer<vtkVertexGlyphFilter>  targetGlyph =
            vtkSmartPointer<vtkVertexGlyphFilter>::New();
    targetGlyph->SetInputData(target);
    targetGlyph->Update();


    //进行ICP配准求变换矩阵
    vtkSmartPointer<vtkIterativeClosestPointTransform> icptrans =
        vtkSmartPointer<vtkIterativeClosestPointTransform>::New();
    icptrans->SetSource(sourceGlyph->GetOutput());
    icptrans->SetTarget(targetGlyph->GetOutput());
    icptrans->GetLandmarkTransform()->SetModeToRigidBody();
    icptrans->SetMaximumNumberOfIterations(1000);
    icptrans->StartByMatchingCentroidsOn();
    icptrans->Modified();
    icptrans->Update();

    //配准矩阵调整源数据
    vtkSmartPointer<vtkTransformPolyDataFilter> solution =
        vtkSmartPointer<vtkTransformPolyDataFilter>::New();
    solution->SetInputData(sourceGlyph->GetOutput());
    solution->SetTransform(icptrans);
    solution->Update();
    /
    vtkSmartPointer<vtkPolyDataMapper> sourceMapper =
        vtkSmartPointer<vtkPolyDataMapper>::New();
    sourceMapper->SetInputConnection(sourceGlyph->GetOutputPort());
    vtkSmartPointer<vtkActor> sourceActor =
        vtkSmartPointer<vtkActor>::New();
    sourceActor->SetMapper(sourceMapper);
    sourceActor->GetProperty()->SetColor(0, 1, 0);
    sourceActor->GetProperty()->SetPointSize(1);

    vtkSmartPointer<vtkPolyDataMapper> targetMapper =
        vtkSmartPointer<vtkPolyDataMapper>::New();
    targetMapper->SetInputConnection(targetGlyph->GetOutputPort());
    vtkSmartPointer<vtkActor> targetActor =
        vtkSmartPointer<vtkActor>::New();
    targetActor->SetMapper(targetMapper);
    targetActor->GetProperty()->SetColor(1, 1, 0);
    targetActor->GetProperty()->SetPointSize(1);

    vtkSmartPointer<vtkPolyDataMapper> soluMapper =
        vtkSmartPointer<vtkPolyDataMapper>::New();
    soluMapper->SetInputConnection(solution->GetOutputPort());
    vtkSmartPointer<vtkActor> soluActor =
        vtkSmartPointer<vtkActor>::New();
    soluActor->SetMapper(soluMapper);
    soluActor->GetProperty()->SetColor(1, 0, 0);
    soluActor->GetProperty()->SetPointSize(4);


    //设置坐标系
        vtkSmartPointer<vtkAxesActor> axes =
            vtkSmartPointer<vtkAxesActor>::New();
        axes->SetScale(1000);
    ///
    vtkSmartPointer<vtkRenderer> renderer =  vtkSmartPointer<vtkRenderer>::New();
    renderer->AddActor(sourceActor);
    renderer->AddActor(targetActor);
    renderer->AddActor(soluActor);
    //renderer->AddActor(axes);



    // Set the background color.

    renderer->SetBackground(colors->GetColor3d("BkgColor").GetData());
    renderer->ResetCamera();
    renderer->GetActiveCamera()->Zoom(1.5);

    vtkSmartPointer<vtkRenderWindow> renderWindow = vtkSmartPointer<vtkRenderWindow>::New();
    renderWindow->AddRenderer(renderer);
    renderWindow->SetSize(988, 666);
    renderWindow->SetWindowName("ICP Regisration Test");

    vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor = vtkSmartPointer<vtkRenderWindowInteractor>::New();
    renderWindowInteractor->SetRenderWindow(renderWindow);

    /****************************************************************/

    vtkSmartPointer<vtkOrientationMarkerWidget> widget =
        vtkSmartPointer<vtkOrientationMarkerWidget>::New();
    widget->SetOutlineColor(1, 1, 1);
    widget->SetOrientationMarker(axes);
    widget->SetInteractor(renderWindowInteractor); //加入鼠标交互
    widget->SetViewport(0.0, 0.0, 0.3, 0.3);  //设置显示位置
    widget->SetEnabled(1);
    widget->InteractiveOn();//开启鼠标交互
    // This starts the event loop and as a side effect causes an initial render.
    renderWindow->Render();
    renderWindowInteractor->Initialize();
    renderWindowInteractor->Start();
    return app.exec();
}

3.对数据进行Transform变化(NDI_CAP_Ireach_qml_ireach_vtk_icp.zip例子中用到，对同一份数据进行变化)

  //构造浮动数据点集
    vtkSmartPointer<vtkTransform> trans =
        vtkSmartPointer<vtkTransform>::New();
    trans->Translate(200,200, 0);
    trans->RotateX(10);

    vtkSmartPointer<vtkTransformPolyDataFilter> transformFilter1 =
        vtkSmartPointer<vtkTransformPolyDataFilter>::New();
    transformFilter1->SetInputData(orig);
    transformFilter1->SetTransform(trans);
    transformFilter1->Update();

    vtkSmartPointer<vtkPolyData> target =
            vtkSmartPointer<vtkPolyData>::New();
    target->SetPoints(transformFilter1->GetOutput()->GetPoints());