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

86 阅读 0 评论 57 点赞

我是靠谱客的博主野性秋天，最近开发中收集的这篇文章主要介绍《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)