# PCL学习笔记--利用矩阵对点云进行刚体变换(ICP)（提醒了我很多思路）附rabbit的效果图

**程序**
/*
* 功能： 点云刚体变换
* 头文件： #include <pcl/common/transforms.h>
* 功能函数： pcl::transformPointCloud(*cloud, *cloudOut, transform_1);
*/
#include "stdafx.h"
#include <iostream>
#include <pcl\common\transforms.h>
#include <pcl\io\pcd_io.h>
#include <pcl\visualization\pcl_visualizer.h>
#include <pcl\registration\icp.h>

int _tmain(int argc, _TCHAR* argv[])
{
//读入点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

//可视化初始化
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<pcl::PointXYZ>::Ptr cloudOut(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud, *cloudOut, transform_1);

//点云可视化
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> incloudHandler(cloud, 255, 255, 255);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> outcloudHandler(cloudOut, 225, 30, 30);

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<pcl::PointXYZ, pcl::PointXYZ> icp;
// pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1 (new pcl::search::KdTree<pcl::PointXYZ>);
// tree1->setInputCloud(cloud_source);
// pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::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<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud);
icp.setInputTarget(cloudOut);
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;
//输出最终的变换矩阵（4x4）
std::cout << icp.getFinalTransformation() << std::endl;

//点云可视化
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> incloudHandler1(cloud, 255, 255, 255);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> outcloudHandler1(cloudOut, 225, 30, 30);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> FinalcloudHandler1(Final, 0, 255, 0);

while(!viewer.wasStopped())
{
viewer.spinOnce();
}

system("pause");
return 0;
}

• 广告
• 抄袭
• 版权
• 政治
• 色情
• 无意义
• 其他

120