点云学习笔记17——求点云的最小包围盒
源码
#include <string>
#include <iostream>
#include <Eigen/Core>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <pcl/visualization/pcl_visualizer.h>
using PointT = pcl::PointXYZ;
int main(int argc, char **argv)
{
pcl::PointCloud<PointT>::Ptr original_cloud(new pcl::PointCloud<PointT>());
std::string fileName("0010.pcd");
pcl::io::loadPCDFile(fileName, *original_cloud);
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*original_cloud, centroid);
Eigen::Matrix3f covariance;
computeCovarianceMatrixNormalized(*original_cloud, centroid, covariance);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
Eigen::Matrix3f eigen_vectors = eigen_solver.eigenvectors();
eigen_vectors.col(2) = eigen_vectors.col(0).cross(eigen_vectors.col(1));
Eigen::Matrix4f transformation(Eigen::Matrix4f::Identity());
transformation.block<3, 3>(0, 0) = eigen_vectors.transpose();
transformation.block<3, 1>(0, 3) = -1.f * (transformation.block<3, 3>(0, 0) * centroid.head<3>());
pcl::PointCloud<PointT> transformed_cloud;
pcl::transformPointCloud(*original_cloud, transformed_cloud, transformation);
PointT min_pt, max_pt;
pcl::getMinMax3D(transformed_cloud, min_pt, max_pt);
const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap());
const Eigen::Quaternionf qfinal(eigen_vectors);
const Eigen::Vector3f tfinal = eigen_vectors * mean_diag + centroid.head<3>();
pcl::visualization::PCLVisualizer viewer;
viewer.addPointCloud(original_cloud);
viewer.addCoordinateSystem();
Eigen::Vector3f whd;
whd = max_pt.getVector3fMap() - min_pt.getVector3fMap();
float scale = (whd(0) + whd(1) + whd(2)) / 3;
PointT cp;
cp.x = centroid(0);
cp.y = centroid(1);
cp.z = centroid(2);
PointT principal_dir_X;
principal_dir_X.x = scale * eigen_vectors(0, 0) + cp.x;
principal_dir_X.y = scale * eigen_vectors(1, 0) + cp.y;
principal_dir_X.z = scale * eigen_vectors(2, 0) + cp.z;
PointT principal_dir_Y;
principal_dir_Y.x = scale * eigen_vectors(0, 1) + cp.x;
principal_dir_Y.y = scale * eigen_vectors(1, 1) + cp.y;
principal_dir_Y.z = scale * eigen_vectors(2, 1) + cp.z;
PointT principal_dir_Z;
principal_dir_Z.x = scale * eigen_vectors(0, 2) + cp.x;
principal_dir_Z.y = scale * eigen_vectors(1, 2) + cp.y;
principal_dir_Z.z = scale * eigen_vectors(2, 2) + cp.z;
viewer.addArrow(principal_dir_X, cp, 1.0, 0.0, 0.0, false, "arrow_x");
viewer.addArrow(principal_dir_Y, cp, 0.0, 1.0, 0.0, false, "arrow_y");
viewer.addArrow(principal_dir_Z, cp, 0.0, 0.0, 1.0, false, "arrow_z");
viewer.addCube(tfinal, qfinal, whd(0), whd(1), whd(2), "bbox");
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "bbox");
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "bbox");
while (!viewer.wasStopped())
{
viewer.spinOnce(100);
}
return 0;
}
效果图
参考
https://blog.csdn.net/u014072827/article/details/110921263