pcl自动计算旋转矩阵, 将点云归正

最新推荐文章于 2025-09-10 14:09:00 发布

3D_DLW

最新推荐文章于 2025-09-10 14:09:00 发布

阅读量1k

点赞数

CC 4.0 BY-SA版权

分类专栏： # PCL 点云文章标签：矩阵线性代数

本文链接：https://blog.csdn.net/DLW__/article/details/127728542

点云同时被 2 个专栏收录

18 篇文章

订阅专栏

PCL

10 篇文章

订阅专栏

本文介绍了一种使用PCL库对点云文件进行读取，并通过寻找点云中XY轴上的最小值点来确定旋转基准点的方法。之后计算旋转角度并应用到点云上，实现了点云数据的旋转和平移操作。

在这里插入图片描述

核心代码

#define PCL_NO_PRECOMPILE

#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>

using namespace std;

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

int main(int argc, char** argv)
{
	string in_path = argv[1];
	string out_path = in_path.substr(0, in_path.find_first_of("."));

	PointCloudT::Ptr cloud(new PointCloudT);
	pcl::io::loadPCDFile(in_path, *cloud);

	//迭代搜索xy最小点index
	int i_min_x = -1;
	int i_min_y = -1;
	float v_min_x = 1e+16;
	float v_min_y = 1e+16;
	for (size_t i = 0; i < cloud->points.size(); i++) {
		if (cloud->points[i].x < v_min_x) {
			v_min_x = cloud->points[i].x;
			i_min_x = i;
		}
		if (cloud->points[i].y < v_min_y) {
			v_min_y = cloud->points[i].y;
			i_min_y = i;
		}
	}
	assert(i_min_x != -1 and i_min_y != -1);
	//计算旋转角度
	PointT point_a = cloud->points[i_min_x];
	PointT point_b = cloud->points[i_min_y];
	float theta = -atan2(point_b.y - point_a.y, point_b.x - point_a.x);
	//构造旋转矩阵
	Eigen::Affine3f transform = Eigen::Affine3f::Identity();
	transform.translation() << -point_a.x, -point_a.y, 0.;	// 设置平移参数
	transform.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));// 定义旋转角度, 绕z轴旋转
	//执行旋转
	pcl::transformPointCloud(*cloud, *cloud, transform);
	pcl::io::savePCDFile(out_path + "_trans.pcd", *cloud, true);	//保存
	//还原旋转
	transform = transform.inverse();//反转矩阵
	pcl::transformPointCloud(*cloud, *cloud, transform);
	pcl::io::savePCDFile(out_path + "_trans_inv.pcd", *cloud, true);	//保存
}