计算点云的包围盒OBB

机器人 同时被 2 个专栏收录
27 篇文章 31 订阅
5 篇文章 1 订阅

  文中的点云文件和代码在这里:https://github.com/robinvista/pointcloud-OBB
  为什么要计算点云的包围盒,它有什么用?对于无人驾驶和机器人来说,包围盒主要用来做碰撞检测和显示。
  最简单的包围盒叫Axis-Aligned Bounding Box (AABB),它的边与坐标轴平行,所以与实际物体相差较大,不是太好用。稍微复杂点的是Orientation Bounding Box(OBB),它与AABB的区别就是考虑了包围盒的方向,不一定与坐标轴平行,可以旋转。本文就是计算OBB的。计算OBB最难的是怎么确定box的方向。计算box的中心很简单,求所有点的平均值或者最大最小点的中间值即可。但是方向却需要计算特征向量。不用担心,现有的库或者软件都提供这样的函数,直接调用即可。以Eigen库为例,它在ROS、百度无人驾驶中都被使用,它里面的函数特点是:
  Eigen中的eigenvalues函数返回的特征值是从小到大排列的,如下:

The eigenvalues are sorted in increasing order.

  Mathematica中的Eigenvalues函数返回的特征值是从大到小排列的,如下。对于数值矩阵,Eigenvectors函数返回的特征向量是单位向量。

特征值依绝对值递减排列

  MATLAB中的eig函数返回的特征值顺序是未排序的,需要用户自己排序,如下:

默认情况下,eig 并不总是返回已排序的特征值和特征向量。可以使用 sort 函数将特征值按升序排序,并重新排序相应的特征向量。

  所需的代码如下:

#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/common/common.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>

typedef pcl::PointXYZI PointType;

int main(int argc, char* argv[])
{
	// 1. 读入点云文件
	pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>());
    pcl::PLYReader reader;
    reader.read("pointcloud.ply", *cloud);

	//2. 开始欧式聚类
	pcl::search::KdTree<PointType>::Ptr tree(new pcl::search::KdTree<PointType>);
	tree->setInputCloud(cloud);
	std::vector<pcl::PointIndices> local_indices;
	pcl::EuclideanClusterExtraction<PointType> euclid;
	euclid.setInputCloud(cloud);
	float in_max_cluster_distance = 0.5;
	float MIN_CLUSTER_SIZE = 5;
	float MAX_CLUSTER_SIZE = 500000;
	euclid.setClusterTolerance(in_max_cluster_distance);
	euclid.setMinClusterSize(MIN_CLUSTER_SIZE);
	euclid.setMaxClusterSize(MAX_CLUSTER_SIZE);
	euclid.setSearchMethod(tree);
	euclid.extract(local_indices);

	//3. 取出第i个聚类,用户输入i的值
	std::cout << "Number of clusters = " << local_indices.size() << endl;
	size_t i = 0;
	pcl::PointCloud<PointType>::Ptr cloud_cluster(new pcl::PointCloud<PointType>());
	PointType point;
	for (auto pit = local_indices[i].indices.begin(); pit != local_indices[i].indices.end(); ++pit)
	{
		point.x = cloud->points[*pit].x;
		point.y = cloud->points[*pit].y;
		point.z = cloud->points[*pit].z;
		cloud_cluster->points.push_back(point);
	}
	
	//5. 计算点云的几何中心和重心,注意这两个概念不一样,重心是所有点的平均值,中心是最大最小点的平均值
	Eigen::Vector4f centroid;     // 重心
	pcl::compute3DCentroid(*cloud_cluster, centroid);
	PointType min_pt, max_pt;
	Eigen::Vector3f center;       // 中心
	pcl::getMinMax3D(*cloud_cluster, min_pt, max_pt); 
	center = (max_pt.getVector3fMap() + min_pt.getVector3fMap())/2;
	
	// 6. 计算协方差矩阵的特征向量
	Eigen::Matrix3f covariance;
	pcl::computeCovarianceMatrixNormalized(*cloud_cluster, centroid, covariance); // 这里必须用重心
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
	Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
	Eigen::Vector3f eigenValuesPCA  = eigen_solver.eigenvalues();
	eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //正交化
	eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
	eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));

	// 7. 计算变换矩阵,只考虑绕全局坐标系Z轴的转动
	Eigen::Vector3f ea = (eigenVectorsPCA).eulerAngles(2, 1, 0);
	Eigen::AngleAxisf keep_Z_Rot(ea[0], Eigen::Vector3f::UnitZ());
	Eigen::Affine3f transform = Eigen::Affine3f::Identity();
	transform.translate(center);  // translate与rotate书写的顺序不可搞反
	transform.rotate(keep_Z_Rot); // radians
	
	// 8. 计算包围盒的尺寸
	pcl::PointCloud<PointType>::Ptr transformedCloud(new pcl::PointCloud<PointType>);
	pcl::transformPointCloud(*cloud_cluster, *transformedCloud, transform.inverse());
	// 重新计算变换后的点云的中心,因为变换前的点云的中心不是其包围盒的几何中心
	PointType min_pt_T, max_pt_T;
	pcl::getMinMax3D(*transformedCloud, min_pt_T, max_pt_T);
	Eigen::Vector3f center_new = (max_pt_T.getVector3fMap() + min_pt_T.getVector3fMap()) / 2;
	Eigen::Vector3f box_dim;
	box_dim = max_pt_T.getVector3fMap() - min_pt_T.getVector3fMap();
	Eigen::Affine3f transform2 = Eigen::Affine3f::Identity();
	transform2.translate(center_new);
	Eigen::Affine3f transform3 = transform * transform2;

	//9. 显示
	pcl::visualization::PCLVisualizer viewer;
	pcl::visualization::PointCloudColorHandlerCustom<PointType> handler_1(cloud_cluster, 0, 255, 0);	//转换到原点的点云
	pcl::visualization::PointCloudColorHandlerCustom<PointType> handler_2(transformedCloud, 255, 0, 0);	//输入的初始点云
	viewer.addPointCloud(cloud_cluster, handler_1, "cloud1");
	viewer.addPointCloud(transformedCloud, handler_2, "cloud2");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud1");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud2");
	const Eigen::Quaternionf bboxQ(keep_Z_Rot);
	const Eigen::Vector3f    bboxT(transform3.translation()); // 这里用新的"中心",因为要显示的包围盒采用几何中心作为参考点
	viewer.addCube(bboxT, bboxQ, box_dim(0), box_dim(1), box_dim(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");
	viewer.addCoordinateSystem(2.0);
	viewer.setBackgroundColor(1, 1, 1);
    viewer.spin();
    return 0;
}

  PCLVisualizer中的addCube函数的输入参数如下图,其中位姿都是用的Eigen的类型,尺寸的顺序就是分别沿XYZ轴(从上往下看,X轴是宽度,Y轴是高度,Z轴是深度)。

  其它类似的计算OBB的项目有:
  https://github.com/LidarPerception/object_builders_lib

  但是,OBB计算出来的包围盒并不完美,我们知道包围盒并没有贴合物体的边缘。要对角度矫正需要知道边缘的方向,这个可以用RANSAC估计出来,但是有些慢。

  • 1
    点赞
  • 1
    评论
  • 1
    收藏
  • 一键三连
    一键三连
  • 扫一扫,分享海报

©️2021 CSDN 皮肤主题: 技术黑板 设计师:CSDN官方博客 返回首页
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值