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

最新推荐文章于 2023-04-11 15:15:53 发布

尘埃1206

最新推荐文章于 2023-04-11 15:15:53 发布

阅读量3.9k

点赞数 7

本文链接：https://blog.csdn.net/u013094443/article/details/49465793

版权

#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;
}