pcl中set程序_PCL学习笔记--利用矩阵对点云进行刚体变换(ICP)

利用矩阵对点云进行刚体变换

在这篇教程中，我们会学习如何用一个4*4的矩阵对点云进行变换。我们会加载一个点云，然后对其进行刚体变换(旋转加平移)，并可视化结果，最后通过ICP算法对点云进行配准。

**程序**

/*

* 功能： 点云刚体变换

* 头文件： #include

* 功能函数： pcl::transformPointCloud(*cloud, *cloudOut, transform_1);

*/

#include "stdafx.h"

#include

#include

#include

#include

#include

int _tmain(int argc, _TCHAR* argv[])

{

//读入点云

pcl::PointCloud<:pointxyz>::Ptr cloud(new pcl::PointCloud<:pointxyz>);

pcl::io::loadPCDFile("rabbit.pcd", *cloud);

//可视化初始化

pcl::visualization::PCLVisualizer viewer;

viewer.setCameraFieldOfView(0.785398);//fov 45° 视场角

viewer.setBackgroundColor(0.2, 0.2, 0.2);

viewer.setCameraPosition(

0, 0, 0,

0, 0, -1,

0, 0, 0);

//点云旋转

Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();//单位矩阵

float theta = M_PI / 20;

//给矩阵赋值

transform_1(0, 0) = cos(theta);

transform_1(0, 1) = -sin(theta);

transform_1(1, 0) = sin(theta);

transform_1(1, 1) = cos(theta);

//沿x轴平移0.025

transform_1(0, 3) = 0.01;

printf("Method #1: using a Matrix4f\n");

std::cout << transform_1 << std::endl;

//方法2

Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();

transform_2.translation() << 0.025, 0.0, 0.0;

transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));

// 打印变换矩阵

printf("\nMethod #2: using an Affine3f\n");

std::cout << transform_2.matrix() << std::endl;

//执行变换

pcl::PointCloud<:pointxyz>::Ptr cloudOut(new pcl::PointCloud<:pointxyz>);

pcl::transformPointCloud(*cloud, *cloudOut, transform_1);

//点云可视化

pcl::visualization::PointCloudColorHandlerCustom<:pointxyz> incloudHandler(cloud, 255, 255, 255);

pcl::visualization::PointCloudColorHandlerCustom<:pointxyz> outcloudHandler(cloudOut, 225, 30, 30);

viewer.addPointCloud(cloud, incloudHandler, "In");

viewer.addPointCloud(cloudOut, outcloudHandler, "Out");

viewer.addCoordinateSystem(0.1, "cloud", 0);

int v2 = 1;

viewer.createViewPort(0.5, 0, 1, 1, v2);

viewer.createViewPortCamera(v2);

viewer.setCameraFieldOfView(0.785398,v2);//fov 45° 视场角

viewer.setBackgroundColor(0.0, 0.2, 1.0,v2);

viewer.setCameraPosition(

0, 0, 0,

0, 0, -1,

0, 0, 0,v2);

// ICP

// pcl::IterativeClosestPoint<:pointxyz pcl::pointxyz> icp;

// pcl::search::KdTree<:pointxyz>::Ptr tree1 (new pcl::search::KdTree<:pointxyz>);

// tree1->setInputCloud(cloud_source);

// pcl::search::KdTree<:pointxyz>::Ptr tree2 (new pcl::search::KdTree<:pointxyz>);

// tree2->setInputCloud(cloud_target);

// icp.setSearchMethodSource(tree1);

// icp.setSearchMethodTarget(tree2);

// icp.setInputSource(cloud_source);

// icp.setInputTarget(cloud_target);

// icp.setMaxCorrespondenceDistance(1500);

// icp.setTransformationEpsilon(1e-10);

// icp.setEuclideanFitnessEpsilon(0.1);

// icp.setMaximumIterations(300);

// icp.align(*cloud_source_registration);

// Eigen::Matrix4f transformation = icp.getFinalTransformation();

//ICP配准

pcl::IterativeClosestPoint<:pointxyz pcl::pointxyz> icp;

icp.setInputSource(cloud);

icp.setInputTarget(cloudOut);

pcl::PointCloud<:pointxyz>::Ptr Final(new pcl::PointCloud<:pointxyz>);

icp.align(*Final);

std::cout << "has converged:" << icp.hasConverged() << " score: " <<

icp.getFitnessScore() << std::endl;

//输出最终的变换矩阵(4x4)

std::cout << icp.getFinalTransformation() << std::endl;

//点云可视化

pcl::visualization::PointCloudColorHandlerCustom<:pointxyz> incloudHandler1(cloud, 255, 255, 255);

pcl::visualization::PointCloudColorHandlerCustom<:pointxyz> outcloudHandler1(cloudOut, 225, 30, 30);

pcl::visualization::PointCloudColorHandlerCustom<:pointxyz> FinalcloudHandler1(Final, 0, 255, 0);

//viewer.addPointCloud(cloud, incloudHandler1, "In22",v2);

viewer.addPointCloud(cloudOut, outcloudHandler1, "Out22",v2);

viewer.addPointCloud(Final, FinalcloudHandler1, "Final22",v2);

viewer.addCoordinateSystem(0.1, "cloud22", v2);

while(!viewer.wasStopped())

{

viewer.spinOnce();

}

system("pause");

return 0;

}