- #include <iostream>
- #include <pcl/common/common.h>
- #include <pcl/common/angles.h>
- #include <pcl/common/transforms.h>
- #include <pcl/point_cloud.h>
- #include <pcl/point_types.h>
- #include <pcl/io/pcd_io.h>
- #include <pcl/registration/transformation_estimation_svd.h>
- using namespace std;
- int main(int argc, char **argv) {
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ> ());
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ> ());
- cloud_in->width = 4;
- cloud_in->height = 1;
- cloud_in->is_dense = false;
- cloud_in->resize(cloud_in->width * cloud_in->height);
- cloud_out->width = 4;
- cloud_out->height = 1;
- cloud_out->is_dense = false;
- cloud_out->resize(cloud_out->width * cloud_out->height);
- //从文件中读取4对同名点
- FILE * fRead;
- fRead = fopen("C:\\Users\\Administrator\\Desktop\\pt.txt","r");
- cout<<"cloud_in : "<<endl;
- for (int i=0;i<cloud_in->points.size();i++)
- {
- double pt[3];
- fscanf(fRead,"%lf %lf %lf",pt,pt+1,pt+2);
- cloud_in->points[i].x = pt[0];
- cloud_in->points[i].y = pt[1];
- cloud_in->points[i].z = pt[2];
- cout<<cloud_in->points[i].x<<" \t"<<cloud_in->points[i].y<<" \t"<<cloud_in->points[i].z<<endl;
- }
- cout<<"cloud_out : "<<endl;
- for (int i=0;i<cloud_out->points.size();i++)
- {
- double pt[3];
- fscanf(fRead,"%lf %lf %lf",pt,pt+1,pt+2);
- cloud_out->points[i].x = pt[0];
- cloud_out->points[i].y = pt[1];
- cloud_out->points[i].z = pt[2];
- cout<<cloud_out->points[i].x<<" \t"<<cloud_out->points[i].y<<" \t"<<cloud_out->points[i].z<<endl;
- }
- //利用SVD方法求解变换矩阵
- pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ> TESVD;
- pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ>::Matrix4 transformation2;
- TESVD.estimateRigidTransformation (*cloud_in,*cloud_out,transformation2);
- //输出变换矩阵信息
- std::cout << "The Estimated Rotation and translation matrices (using getTransformation function) are : \n" << std::endl;
- printf ("\n");
- printf (" | %6.3f %6.3f %6.3f | \n", transformation2 (0,0), transformation2 (0,1), transformation2 (0,2));
- printf ("R = | %6.3f %6.3f %6.3f | \n", transformation2 (1,0), transformation2 (1,1), transformation2 (1,2));
- printf (" | %6.3f %6.3f %6.3f | \n", transformation2 (2,0), transformation2 (2,1), transformation2 (2,2));
- printf ("\n");
- printf ("t = < %0.3f, %0.3f, %0.3f >\n", transformation2 (0,3), transformation2 (1,3), transformation2 (2,3));
- return 0;
- }
- 原文地址
- https://blog.csdn.net/u013094443/article/details/49465793
微信扫码关注我们:跟着数理化走天下
获得更多的信息哦,一起交流,一起成长哦:微信号:跟着数理化走天下,纯属个人的交流,无盈利目的
pcl利用空间同名点求取旋转和平移矩阵
最新推荐文章于 2024-07-19 15:19:14 发布