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

核心代码

#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);	//保存
}