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

**程序**
/*
* 功能： 点云刚体变换
* 头文件： #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::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<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);
    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<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);
    //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;
}

转：https://blog.csdn.net/u014801811/article/details/79423942