PCL Matrix4f實現點雲座標變換

使用變換矩陣對點雲進行旋轉平移操作，輸入的是變換矩陣，輸出的是變換後的點雲。

代碼實現

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>  
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
using namespace std;
// 主函數
int
main(int argc, char** argv) {

    // 加載點雲數據文件
    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::io::loadPCDFile<pcl::PointXYZ>("數據\\bunny.pcd", *source);
    Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
    transform_1(0, 0) = 0.995870604090736;
    transform_1(0, 1) = 0.0461999293906251;
    transform_1(0, 2) = -0.0781492574005297;
    transform_1(0, 3) = 0.0;
 
    transform_1(1, 0) = -0.0170114840727817;
    transform_1(1, 1) = 0.940543863547451;
    transform_1(1, 2) = 0.339246002412522;
    transform_1(1, 3) = 0.0;

    transform_1(2, 0) = 0.0891759458463688;
    transform_1(2, 1) = -0.336515686510356;
    transform_1(2, 2) = 0.937445914927826;
    transform_1(2, 3) = 0.0;
 
    transform_1(3, 0) =0.0;
    transform_1(3, 1) = 0.0;
    transform_1(3, 2) = 0.0;
    transform_1(3, 3) = 1.0;

    cout << "變換矩陣\n"<<transform_1 << std::endl;

    // 執行變換，並將結果保存在新創建的 transformed_cloud 中
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    // 可以使用 transform_1 或 transform_2; t它們是一樣的
    pcl::transformPointCloud(*source, *t_cloud, transform_1);
    pcl::io::savePCDFileASCII("數據\\bunny1.pcd", *t_cloud);
    // 可視化
  
    pcl::visualization::PCLVisualizer viewer("Matrix transformation example");
    // 爲點雲定義 R,G,B 顏色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler(source, 255, 0, 0);
    // 輸出點雲到查看器，使用顏色管理
    viewer.addPointCloud(source, source_cloud_color_handler, "original_cloud");

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler(t_cloud, 0, 255, 0); // 紅
    viewer.addPointCloud(t_cloud, transformed_cloud_color_handler, "transformed_cloud");

   // viewer.addCoordinateSystem(1.0, "cloud", 0);
    viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // 設置背景爲深灰
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
    //viewer.setPosition(800, 400); // 設置窗口位置
while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(1000));
	}

	return (0);
}

結果展示