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

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. 微信扫码关注我们：跟着数理化走天下

    
    

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