计算点云的最小BBOX

本文主要是介绍计算点云的最小BBOX，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

参考1
参考2

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/project_inliers.h>
#include <string>
#include <Eigen/Core>
#include <boost/thread/thread.hpp>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/common/transforms.h>using namespace std;
intmain (int argc, char** argv)
{pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);string fileName("test.pcd");pcl::io::loadPCDFile(fileName,*cloud);pcl::visualization::PCLVisualizer viewer;pcl::NormalEstimationOMP<pcl::PointXYZRGBNormal,pcl::PointXYZRGBNormal> nor;pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGBNormal>);nor.setSearchMethod(tree);nor.setInputCloud(cloud);nor.setNumberOfThreads(10);nor.setRadiusSearch(0.03);nor.compute(*cloud);size_t cloud_size = cloud->size();for (size_t i = 0;i<cloud_size;++i){uint8_t r = (cloud->at(i).normal_x + 1)/2 * 255;uint8_t g = (cloud->at(i).normal_y + 1)/2 * 255;uint8_t b = (cloud->at(i).normal_z + 1)/2 * 255;uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);cloud->at(i).rgb = *reinterpret_cast<float*>(&rgb);}Eigen::Vector4f pcaCentroid;pcl::compute3DCentroid(*cloud,pcaCentroid);Eigen::Matrix3f covariance;pcl::computeCovarianceMatrixNormalized(*cloud,pcaCentroid,covariance);Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance,Eigen::ComputeEigenvectors);Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));// Transform the original cloud to the origin where the principal components correspond to the axes.Eigen::Matrix4f transform(Eigen::Matrix4f::Identity());transform.block<3,3>(0,0) = eigenVectorsPCA.transpose();transform.block<3,1>(0,3) = -1.f * (transform.block<3,3>(0,0) * pcaCentroid.head<3>());pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);pcl::transformPointCloudWithNormals(*cloud,*transformedCloud,transform);pcl::PointXYZRGBNormal minPoint,maxPoint;pcl::getMinMax3D(*transformedCloud,minPoint,maxPoint);const Eigen::Vector3f meanDiagonal = 0.5f*(maxPoint.getVector3fMap() + minPoint.getVector3fMap());const Eigen::Quaternionf bboxQuaternion(eigenVectorsPCA); //Quaternions are a way to do rotations https://www.youtube.com/watch?v=mHVwd8gYLnIconst Eigen::Vector3f bboxTransform = eigenVectorsPCA * meanDiagonal + pcaCentroid.head<3>();viewer.addPointCloud<pcl::PointXYZRGBNormal>(cloud);viewer.addCube(bboxTransform, bboxQuaternion, maxPoint.x - minPoint.x, maxPoint.y - minPoint.y, maxPoint.z - minPoint.z, "bbox");viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME,"bbox");while(!viewer.wasStopped()){viewer.spinOnce(100);boost::this_thread::sleep (boost::posix_time::microseconds (100000));}return (0);
}

这篇关于计算点云的最小BBOX的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---