pcl利用空间同名点求取旋转和平移矩阵

本文介绍了一个使用SVD方法估计两个点云之间的刚体变换的C++程序实例。程序通过读取文件中的点对,构建输入和输出点云,并应用pcl库中的TransformationEstimationSVD类来计算变换矩阵。
  1. #include <iostream>  
  2. #include <pcl/common/common.h>  
  3. #include <pcl/common/angles.h>  
  4. #include <pcl/common/transforms.h>  
  5. #include <pcl/point_cloud.h>  
  6. #include <pcl/point_types.h>  
  7. #include <pcl/io/pcd_io.h>  
  8. #include <pcl/registration/transformation_estimation_svd.h>  
  9.   
  10. using namespace std;  
  11.   
  12. int main(int argc, char **argv) {  
  13.   
  14.     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ> ());  
  15.     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ> ());  
  16.   
  17.     cloud_in->width = 4;  
  18.     cloud_in->height = 1;  
  19.     cloud_in->is_dense = false;  
  20.     cloud_in->resize(cloud_in->width * cloud_in->height);  
  21.   
  22.     cloud_out->width = 4;  
  23.     cloud_out->height = 1;  
  24.     cloud_out->is_dense = false;  
  25.     cloud_out->resize(cloud_out->width * cloud_out->height);  
  26.   
  27.     //从文件中读取4对同名点  
  28.     FILE * fRead;  
  29.     fRead = fopen("C:\\Users\\Administrator\\Desktop\\pt.txt","r");  
  30.   
  31.     cout<<"cloud_in : "<<endl;  
  32.     for (int i=0;i<cloud_in->points.size();i++)  
  33.     {  
  34.         double pt[3];  
  35.         fscanf(fRead,"%lf %lf %lf",pt,pt+1,pt+2);  
  36.         cloud_in->points[i].x = pt[0];  
  37.         cloud_in->points[i].y = pt[1];  
  38.         cloud_in->points[i].z = pt[2];  
  39.   
  40.         cout<<cloud_in->points[i].x<<"  \t"<<cloud_in->points[i].y<<"  \t"<<cloud_in->points[i].z<<endl;  
  41.     }  
  42.   
  43.     cout<<"cloud_out : "<<endl;  
  44.     for (int i=0;i<cloud_out->points.size();i++)  
  45.     {  
  46.         double pt[3];  
  47.         fscanf(fRead,"%lf %lf %lf",pt,pt+1,pt+2);  
  48.         cloud_out->points[i].x = pt[0];  
  49.         cloud_out->points[i].y = pt[1];  
  50.         cloud_out->points[i].z = pt[2];  
  51.   
  52.         cout<<cloud_out->points[i].x<<"  \t"<<cloud_out->points[i].y<<"  \t"<<cloud_out->points[i].z<<endl;  
  53.     }  
  54.   
  55.     //利用SVD方法求解变换矩阵  
  56.     pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ> TESVD;  
  57.     pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ>::Matrix4 transformation2;  
  58.     TESVD.estimateRigidTransformation (*cloud_in,*cloud_out,transformation2);  
  59.     //输出变换矩阵信息  
  60.     std::cout << "The Estimated Rotation and translation matrices (using getTransformation function) are : \n" << std::endl;  
  61.     printf ("\n");  
  62.     printf ("    | %6.3f %6.3f %6.3f | \n", transformation2 (0,0), transformation2 (0,1), transformation2 (0,2));  
  63.     printf ("R = | %6.3f %6.3f %6.3f | \n", transformation2 (1,0), transformation2 (1,1), transformation2 (1,2));  
  64.     printf ("    | %6.3f %6.3f %6.3f | \n", transformation2 (2,0), transformation2 (2,1), transformation2 (2,2));  
  65.     printf ("\n");  
  66.     printf ("t = < %0.3f, %0.3f, %0.3f >\n", transformation2 (0,3), transformation2 (1,3), transformation2 (2,3));  
  67.   
  68.     return 0;  
  69. }  
  70. 原文地址
  71. https://blog.youkuaiyun.com/u013094443/article/details/49465793
  72. 微信扫码关注我们:跟着数理化走天下


    获得更多的信息哦,一起交流,一起成长哦:微信号:跟着数理化走天下,纯属个人的交流,无盈利目的


您好!为了使用PCL显示旋转平移矩阵,您需要使用PCL的visualization模块。以下是一个示例代码,展示如何使用PCL实现您的需求: ```cpp #include <iostream> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/common/transforms.h> int main() { // 创建pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); cloud->width = 100; cloud->height = 1; cloud->points.resize(cloud->width * cloud->height); for (size_t i = 0; i < cloud->points.size(); ++i) { cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f); cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f); cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f); } // 构造旋转矩阵平移向量 Eigen::Matrix4f transform = Eigen::Matrix4f::Identity(); float theta = M_PI / 4; // 旋转角度 transform(0, 0) = cos(theta); transform(0, 1) = -sin(theta); transform(1, 0) = sin(theta); transform(1, 1) = cos(theta); transform(2, 3) = 0.4; // 平移向量 // 应用旋转平移变换 pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>()); pcl::transformPointCloud(*cloud, *transformed_cloud, transform); // 创建可视化对象 pcl::visualization::PCLVisualizer viewer("Matrix Visualization"); // 可视化原始云 viewer.addPointCloud(cloud, "cloud"); // 可视化旋转矩阵 Eigen::Matrix3f rotation = transform.block(0, 0, 3, 3); viewer.addCoordinateSystem(0.3, rotation, "rotation", 0); // 可视化平移向量 Eigen::Vector3f translation = transform.block(0, 3, 3, 1); viewer.addText3D("Translation", pcl::PointXYZ(translation[0], translation[1], translation[2]), 0.05, 1.0, 1.0, 1.0, "translation"); // 可视化变换后的云 viewer.addPointCloud(transformed_cloud, "transformed_cloud"); viewer.spin(); return 0; } ``` 在这个示例代码中,我们首先创建了一个随机云。然后,我们构造了一个旋转矩阵一个平移向量,并将它们应用到原始云上,得到了一个变换后的云。最后,我们使用PCL的可视化模块,将原始云、旋转矩阵平移向量变换后的云可视化出来。 您可以复制这段代码并进行修改,以满足您的具体需求。希望这能帮助到您!
评论 8
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值