利用pcl的icp方法实现对两个点云的配准及可视化显示

利用pcl的icp方法实现对两个点云的配准

1.读取点云数据

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
if(argc != 3)
{
    cerr<<"输入点云数量不对!"<<endl;
    exit(1);
}

string input_filename = argv[1];
string output_filename = argv[2];

std::string format = input_filename.substr(input_filename.length()-4, 4);
std::cout<<"pointcloud format:"<<format<<std::endl;
if(format == ".ply")
{
    pcl::io::loadPLYFile(input_filename, *cloud_in);
    pcl::io::loadPLYFile(output_filename, *cloud_out);
}
else if(format == ".pcd")
{
    pcl::io::loadPCDFile(input_filename, *cloud_in);
    pcl::io::loadPCDFile(output_filename, *cloud_out);
}

2.定义icp对象

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZ>::Ptr Final(new pcl::PointCloud<pcl::PointXYZ>);
icp.align(*Final);
std::cout << "has converged:" << icp.hasConverged() << " score: "
          << icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;

Eigen::Matrix4d T(4,4);
T= icp.getFinalTransformation().cast<double> ();//getFinalTransformation()输出的是float 转成double

3.可视化

pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);

pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color1 (cloud_out, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color2 (Final, 255, 0, 0);
viewer->addPointCloud<pcl::PointXYZ> (cloud_out, color1, "target");
viewer->addPointCloud<pcl::PointXYZ> (Final, color2, "final");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
viewer->addCoordinateSystem (1.0);
viewer->initCameraParameters ();

while (!viewer->wasStopped ())
{
    viewer->spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}

结果展示

输出结果:源点云到目标点云的变换矩阵

has converged:1 score: 0.0264682
 0.918355  0.298267  -0.26012   5.70218
-0.339217    0.9318 -0.129158   9.53576
 0.203856   0.20685    0.9569  -0.52615
        0         0         0         1

配准前:

配准后

附上完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/io/ply_io.h>
#include <string>
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
using namespace std;


int main (int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
    if(argc != 3)
    {
        cerr<<"输入点云数量不对!"<<endl;
        exit(1);
    }

    string input_filename = argv[1];
    string output_filename = argv[2];

    std::string format = input_filename.substr(input_filename.length()-4, 4);
    std::cout<<"pointcloud format:"<<format<<std::endl;
    if(format == ".ply")
    {
        pcl::io::loadPLYFile(input_filename, *cloud_in);
        pcl::io::loadPLYFile(output_filename, *cloud_out);
    }
    else if(format == ".pcd")
    {
        pcl::io::loadPCDFile(input_filename, *cloud_in);
        pcl::io::loadPCDFile(output_filename, *cloud_out);
    }


    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    //将输入点云对齐到输出点云上
    icp.setInputSource(cloud_in);
    icp.setInputTarget(cloud_out);
    pcl::PointCloud<pcl::PointXYZ>::Ptr Final(new pcl::PointCloud<pcl::PointXYZ>);
    icp.align(*Final);
    std::cout << "has converged:" << icp.hasConverged() << " score: "
              << icp.getFitnessScore() << std::endl;
    std::cout << icp.getFinalTransformation() << std::endl;

    Eigen::Matrix4d T(4,4);
    T= icp.getFinalTransformation().cast<double> ();//getFinalTransformation()输出的是float 转成double

    //cout << T.block<3,3>(0,0) << endl << endl;
    Eigen::Matrix3d R = T.block<3,3>(0,0);// = Eigen::Matrix3f::Identity();
    Eigen::Quaterniond q = Eigen::Quaterniond(R);
    cout<<"R = \n"<<R<<endl;
    cout<<"Quaternion = \n"<<q.coeffs()<<endl;

    Eigen::MatrixXd t_mat  = T.topRightCorner(3, 1).transpose();
    Eigen::Vector3d t;
    t<<t_mat(0), t_mat(1), t_mat(2);
    cout<<"t = "<<t<<endl;

    ofstream outfile("Transform.txt");
    if(!outfile)
    {
        cerr<<"open error!"<<endl;
        exit(1);
    }
    outfile<<icp.getFinalTransformation();
    outfile.close();

    //对点云进行旋转 结果与icp计算的相同
    //pcl::transformPointCloud(*cloud_in, *cloud_tran, T);

    //点云可视化
    pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->setBackgroundColor (0, 0, 0);

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color1 (cloud_out, 0, 255, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color2 (Final, 255, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ> (cloud_out, color1, "target");
    viewer->addPointCloud<pcl::PointXYZ> (Final, color2, "final");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
    viewer->addCoordinateSystem (1.0);
    viewer->initCameraParameters ();

    while (!viewer->wasStopped ())
    {
        viewer->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
    return (0);
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)

project(icp)
find_package(PCL 1.7 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
add_executable (icp main.cpp)
target_link_libraries (icp ${PCL_LIBRARIES})