8-1_利用ICP找出點群位姿

8-1_利用ICP找出點群位姿

程式說明:

範例程式:

#include <iostream#62
#include <pcl/io/pcd_io.h#62
#include <pcl/visualization/cloud_viewer.h#62
#include <pcl/common/transforms.h#62
#include <pcl/registration/icp.h#62

int main(int argc, char** argv)
{
    pcl::PointCloud::Ptr showPointsSrc(new pcl::PointCloud);
    pcl::io::loadPCDFile("points.pcd", *showPointsSrc);

    pcl::PointCloud::Ptr showPointsTar(new pcl::PointCloud);

    Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();

    float theta = M_PI / 18; // 10 degree, The angle of rotation in radians
    transform(0, 0) = cos(theta);
    transform(0, 1) = -sin(theta);
    transform(1, 0) = sin(theta);
    transform(1, 1) = cos(theta);

    transform(0, 3) = 0.5; //x
    transform(1, 3) = 0.5; //y
    transform(2, 3) = 0.5; //z

    std::cout << "transform: " << std::endl << transform << std::endl;

    pcl::transformPointCloud(*showPointsSrc, *showPointsTar, transform);

    pcl::IterativeClosestPoint icp;

    icp.setInputCloud(showPointsSrc);
    icp.setInputTarget(showPointsTar);
    pcl::PointCloud Final;
    icp.align(Final);
    std::cout << "has converged:" << icp.hasConverged() << std::endl << " score: " << icp.getFitnessScore() << std::endl;
    std::cout << "icp get transformation: " << std::endl << icp.getFinalTransformation() << std::endl;

    return 0;
}