![在这里插入图片描述](https://i-blog.csdnimg.cn/blog_migrate/09780b20a7f4ecedd4513c70d03cbc26.png)
核心代码
#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);
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()));
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);
}