《PCL点云库学习&VS2010(X64)》Part 28 BoundingBox&addArray

《PCL点云库学习&VS2010(X64)》Part 28 BoundingBox&addArray

参考资源1

参考资源2

参考资源3



代码：

#include <vtkAutoInit.h>
VTK_MODULE_INIT(vtkRenderingOpenGL);
VTK_MODULE_INIT(vtkInteractionStyle);
VTK_MODULE_INIT(vtkRenderingFreeType);

#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;
int
main(int argc, char** argv)
{
//pcl::PointXYZRGBNormal

pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);

string fileName("Bunny.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));

/*
// Note that getting the eigenvectors can also be obtained via the PCL PCA interface with something like:
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPCAprojection (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCA<pcl::PointXYZ> pca;
pca.setInputCloud(cloudSegmented);
pca.project(*cloudSegmented, *cloudPCAprojection);
std::cerr << std::endl << "EigenVectors: " << pca.getEigenVectors() << std::endl;//计算特征向量
std::cerr << std::endl << "EigenValues: " << pca.getEigenValues() << std::endl;//计算特征值
// In this case, pca.getEigenVectors() gives similar eigenVectors to eigenVectorsPCA.
*/

// 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 min_pt, max_pt;
pcl::getMinMax3D(*transformedCloud, min_pt, max_pt);
const Eigen::Vector3f meanDiagonal = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap());

const Eigen::Quaternionf bboxQuaternion(eigenVectorsPCA); //Quaternions are a way to do rotations https://www.youtube.com/watch?v=mHVwd8gYLnI const Eigen::Vector3f bboxTransform = eigenVectorsPCA * meanDiagonal + pcaCentroid.head<3>();

//get the array
//------------------------------------------------------------------------------------------------------------------
// getting the two points for the arrow
pcl::PointXYZ front_midpoint, back_midpoint;
front_midpoint.x = (max_pt.x + min_pt.x) / 2;
front_midpoint.y = (max_pt.y + min_pt.y) / 2;
front_midpoint.z = max_pt.z;
back_midpoint.x = (max_pt.x + min_pt.x) / 2;
back_midpoint.y = (max_pt.y + min_pt.y) / 2;
back_midpoint.z = min_pt.z;
//------------------------------------------------------------------------------------------------------------------

//visualization
viewer.addPointCloud<pcl::PointXYZRGBNormal>(cloud);
viewer.addCube(bboxTransform, bboxQuaternion, max_pt.x - min_pt.x, max_pt.y - min_pt.y, max_pt.z - min_pt.z, "bbox");
viewer.addArrow(front_midpoint, back_midpoint, 1.0, 0.7, 0.0, false, "arrow_box");
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "bbox");
viewer.addCoordinateSystem(1.0);
while (!viewer.wasStopped())
{
viewer.spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}

return 0;
}

注：

算法的基本流程步骤如下:

1) compute the centroid (c0, c1, c2) and the normalized covariance

2) compute the eigenvectors e0, e1, e2. The reference system will be (e0, e1, e0 X e1) --- note: e0 X e1 = +/- e2

3) move the points in that RF --- note: the transformation given by the rotation matrix (e0, e1, e0 X e1) & (c0, c1, c2) must be inverted

4) compute the max, the min and the center of the diagonal

5) given a box centered at the origin with size (max_pt.x - min_pt.x, max_pt.y - min_pt.y, max_pt.z - min_pt.z) the transformation you have to apply is Rotation = (e0, e1, e0 X e1) & Translation = Rotation * center_diag + (c0, c1, c2)