您的位置: 首页  >  文章  >  pcl利用空间同名点求取旋转和平移矩阵

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

分类: 文章 • 2024-11-25 19:49:40
  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.****.net/u013094443/article/details/49465793

  72. 微信扫码关注我们：跟着数理化走天下


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

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


相关推荐