pcl包围盒

AI脚下的巨人

于 2024-02-20 20:31:23 发布

阅读量915

点赞数 17

分类专栏： pcl 点云文章标签：算法

本文链接：https://blog.csdn.net/qq_20604231/article/details/136198114

版权

点云同时被 2 个专栏收录

3 篇文章 0 订阅

订阅专栏

pcl

2 篇文章 0 订阅

订阅专栏

1. 简介

计算一个点云团的包围盒是日常经常用到的功能,如被应用至:

体积测量
碰撞检测

包围盒有两种:

AABB:包围立方体的长宽高与点云坐标系的xyz轴相平行
OBB:包围立方体是可旋转的

2. 基本用法



pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;

feature_extractor.setInputCloud(cloud);
feature_extractor.compute();

// 惯性矩
std::vector <float> moment_of_inertia;
// 偏心率
std::vector <float> eccentricity;
Eigen::Vector3f mass_center;

feature_extractor.getMomentOfInertia(moment_of_inertia);
feature_extractor.getEccentricity(eccentricity);
feature_extractor.getMassCenter(mass_center);

bool is_AABB = true;

if(is_AABB) {

    // AABB包围盒
    pcl::PointXYZ min_point_AABB;
    pcl::PointXYZ max_point_AABB;

    feature_extractor.getAABB(min_point_AABB, max_point_AABB);
    
    Eigen::Vector3f center((min_point_AABB.x + max_point_AABB.x) / 2., 
                            (min_point_AABB.y + max_point_AABB.y) / 2.,
                            (min_point_AABB.z + max_point_AABB.z) / 2.);

} else {
    // OBB包围盒
    pcl::PointXYZ min_point_OBB;
    pcl::PointXYZ max_point_OBB;
    pcl::PointXYZ position_OBB;
    Eigen::Matrix3f rotation;

    feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotation);

    Eigen::Vector3f center(position_OBB.x, position_OBB.y, position_OBB.z);
}

注意点:

上面的MomentOfInertiaEstimation类方法只有compute是真正执行运算的,其他get前缀的类方法仅仅获取,不占计算量,所以getOBB不会比getAABB耗时;
注意区别质心和 几何中心.

3. 包围算法原理

3.1 PCA降维

参考链接:https://zhuanlan.zhihu.com/p/37777074

PCA常用于数据降维处理

3.1.1 什么是降维?

简单来说,假如在一个q维空间,有n个样本数据,举个例子:
在这里插入图片描述

上图红色的是2维空间的数据,直观上看这些样本能够被"拟合"成一条直线,这里所谓的"拟合"就是用线空间(1维空间)去表达这组样本的所在平面空间(2维空间),这样做以后就可以把原本2维的数据降维1维.

当然这个例子仅从几何角度解析了降维,但实际应用中,数据的维度不局限于空间维度,比如用户的身高/体重/饮食量/健康情况等组成一个抽象的高维空间,我们可以对这个高维空间的数据进行压缩,提取更加精简的指标数据以达到接近的描述!

3.1.2 协方差的引出

先讲个很直观的例子.
日常中如何对首富们的资产进行评估?国际上普遍用某某某有多少"美元"的资产作为衡量指标.但是现实世界中不仅仅只有"美元"一种货币,为什么我们能够仅靠这个指标就能和中国/日本的首富比较?正常说应该用一个向量(美元,日元,人民币)来描述一个首富的资产情况,但我们不需要这么做,因为我们有汇率这个东西,任何货币都可以转换为美元

饶了这么远,只是想说明一个问题,数据空间之所以能被压缩是因为数据本身存在冗余度,或者说是数据空间本身存在相关性.提到相关性,那么很自然想到概率论中的协方差

协方差越大,说明相关性越大,那么可以通过其中一个维度即可表达另一个维度.如上面的"直线拟合"例子,本质上就是因为样本数据在x与y轴上呈现线性相关,所以使用1条线,然后将样本投影在其上面则可以描述这组数据的特性.

3.2 MomentOfInertiaEstimation源码分析

通过阅读pcl的MomentOfInertiaEstimation源码可知,OBB的计算流程如下:

计算点云质心:点云3维的均值;
计算点云的3轴的协方差(3*3矩阵);
对协方差矩阵进行分解(由于是方阵,所以就是求特征向量和特征值);
特征值大小说明该特征向量(点云包围盒的正交坐标基)的权重(成分占比大小);
除此以外还做了惯性矩曲率等与包围盒无关的特征,实测发现这些特征的计算十分耗时!因此可以按照pcl的源码进行裁剪

4. 裁剪MomentOfInertiaEstimation实现包围盒

实验发现pcl的MomentOfInertiaEstimation类方法只有compute十分耗时,在边缘计算的板上运行不能够实时性较差,因此阅读pcl的源码发现其进行一些其它操作(惯性矩/曲率),因此如果只需要计算AABB/OBB的包围盒,可以进行裁剪.

CloudProcessor.hpp

class CloudProcessor {

    public:
        using PointT = pcl::PointXYZ;

        /**
         * 获取点云的AABB包围盒
         * @param min_point[out]: 包围盒最小角点
         * @param max_point[out]: 包围盒最大角点
        */
        void ComputeAABB(PointT& min_point, PointT& max_point);

        /**
         * 获取点云的OBB包围盒
         * @param min_point[out]: 包围盒最小角点
         * @param max_point[out]: 包围盒最大角点
         * @param position[out]:  包围盒几何中心
         * @param rotational_matrix[out]:  包围盒的旋转矩阵
        */
        void ComputeOBB(PointT& min_point, PointT& max_point, Eigen::Vector3f& position, Eigen::Matrix3f& rotational_matrix);



        /**
         * 设置待处理点云
         * @param cloud[in]: 默认使用整个点云
        */
        void SetInputCLoud(const pcl::PointCloud<PointT>::ConstPtr& cloud)
        {
            if(!cloud)
                return;
            
            mpCloud = cloud;
            
            mpIndices.reset(new pcl::Indices);
            mpIndices->resize(cloud->size());

            for (size_t i = 0; i < mpIndices->size (); ++i) {
                (*mpIndices)[i] = static_cast<int>(i); 
            }
        }

        /**
         * 设置待处理点云
         * @param cloud[in]: 点云指针
         * @param indices[in]: 待处理的子点云索引数组
        */
        void SetInputCLoud(const pcl::PointCloud<PointT>::ConstPtr& cloud, const pcl::IndicesPtr& indices)
        {
            if(!cloud || !indices)
                return;

            mpCloud = cloud;
            mpIndices = indices;
        }

    private:
        void ComputeMeanValue(Eigen::Vector3f& mean_value, PointT& aabb_min_point, PointT& aabb_max_point);

        void ComputeCovarianceMatrix(const Eigen::Vector3f& mean_value, Eigen::Matrix <float, 3, 3>& covariance_matrix);

        void ComputeEigenVectors(const Eigen::Matrix <float, 3, 3>& covariance_matrix,
            Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis, 
            float& major_value, float& middle_value, float& minor_value);

        pcl::PointCloud<PointT>::ConstPtr mpCloud;
        pcl::IndicesPtr mpIndices;
};

CloudProcessor.cpp

void CloudProcessor::ComputeAABB(PointT& min_point, PointT& max_point)
{
    Eigen::Vector3f mean_value;
    PointT aabb_min_point;
    PointT aabb_max_point;

    ComputeMeanValue(mean_value, aabb_min_point, aabb_max_point);
}

void CloudProcessor::ComputeOBB(PointT& min_point, PointT& max_point, Eigen::Vector3f& position, Eigen::Matrix3f& rotational_matrix)
{
    Eigen::Vector3f mean_value;
    PointT aabb_min_point, aabb_max_point;

    ComputeMeanValue(mean_value, aabb_min_point, aabb_max_point);

    Eigen::Matrix <float, 3, 3> covariance_matrix;
    ComputeCovarianceMatrix(mean_value, covariance_matrix);

    Eigen::Vector3f major_axis, middle_axis, minor_axis;
    float major_value, middle_value, minor_value;

    ComputeEigenVectors(covariance_matrix, major_axis, middle_axis, minor_axis, 
        major_value, middle_value, minor_value);
    
    PointT obb_min_point, obb_max_point;

    obb_min_point.x = std::numeric_limits <float>::max ();
    obb_min_point.y = std::numeric_limits <float>::max ();
    obb_min_point.z = std::numeric_limits <float>::max ();

    obb_max_point.x = std::numeric_limits <float>::min ();
    obb_max_point.y = std::numeric_limits <float>::min ();
    obb_max_point.z = std::numeric_limits <float>::min ();

    auto number_of_points = static_cast <unsigned int> (mpIndices->size ());
    for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
    {
        float x = ((*mpCloud)[(*mpIndices)[i_point]].x - mean_value (0)) * major_axis (0) +
                  ((*mpCloud)[(*mpIndices)[i_point]].y - mean_value (1)) * major_axis (1) +
                  ((*mpCloud)[(*mpIndices)[i_point]].z - mean_value (2)) * major_axis (2);
        float y = ((*mpCloud)[(*mpIndices)[i_point]].x - mean_value (0)) * middle_axis (0) +
                  ((*mpCloud)[(*mpIndices)[i_point]].y - mean_value (1)) * middle_axis (1) +
                  ((*mpCloud)[(*mpIndices)[i_point]].z - mean_value (2)) * middle_axis (2);
        float z = ((*mpCloud)[(*mpIndices)[i_point]].x - mean_value (0)) * minor_axis (0) +
                  ((*mpCloud)[(*mpIndices)[i_point]].y - mean_value (1)) * minor_axis (1) +
                  ((*mpCloud)[(*mpIndices)[i_point]].z - mean_value (2)) * minor_axis (2);

        if (x <= obb_min_point.x) obb_min_point.x = x;
        if (y <= obb_min_point.y) obb_min_point.y = y;
        if (z <= obb_min_point.z) obb_min_point.z = z;

        if (x >= obb_max_point.x) obb_max_point.x = x;
        if (y >= obb_max_point.y) obb_max_point.y = y;
        if (z >= obb_max_point.z) obb_max_point.z = z;
    }

    rotational_matrix << major_axis (0), middle_axis (0), minor_axis (0),
                         major_axis (1), middle_axis (1), minor_axis (1),
                         major_axis (2), middle_axis (2), minor_axis (2);

    Eigen::Vector3f shift (
        (obb_max_point.x + obb_min_point.x) / 2.0f,
        (obb_max_point.y + obb_min_point.y) / 2.0f,
        (obb_max_point.z + obb_min_point.z) / 2.0f);

    obb_min_point.x -= shift (0);
    obb_min_point.y -= shift (1);
    obb_min_point.z -= shift (2);

    obb_max_point.x -= shift (0);
    obb_max_point.y -= shift (1);
    obb_max_point.z -= shift (2);

    position = mean_value + rotational_matrix * shift;

    min_point = obb_min_point;
    max_point = obb_max_point;
}


void CloudProcessor::ComputeMeanValue(Eigen::Vector3f& mean_value, PointT& aabb_min_point, PointT& aabb_max_point)
{
    mean_value (0) = 0.0f;
    mean_value (1) = 0.0f;
    mean_value (2) = 0.0f;

    aabb_min_point.x = std::numeric_limits <float>::max ();
    aabb_min_point.y = std::numeric_limits <float>::max ();
    aabb_min_point.z = std::numeric_limits <float>::max ();

    aabb_max_point.x = -std::numeric_limits <float>::max ();
    aabb_max_point.y = -std::numeric_limits <float>::max ();
    aabb_max_point.z = -std::numeric_limits <float>::max ();

    auto number_of_points = static_cast <unsigned int> (mpIndices->size ());
    for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
    {
        mean_value (0) += (*mpCloud)[(*mpIndices)[i_point]].x;
        mean_value (1) += (*mpCloud)[(*mpIndices)[i_point]].y;
        mean_value (2) += (*mpCloud)[(*mpIndices)[i_point]].z;

        if ((*mpCloud)[(*mpIndices)[i_point]].x <= aabb_min_point.x) aabb_min_point.x = (*mpCloud)[(*mpIndices)[i_point]].x;
        if ((*mpCloud)[(*mpIndices)[i_point]].y <= aabb_min_point.y) aabb_min_point.y = (*mpCloud)[(*mpIndices)[i_point]].y;
        if ((*mpCloud)[(*mpIndices)[i_point]].z <= aabb_min_point.z) aabb_min_point.z = (*mpCloud)[(*mpIndices)[i_point]].z;

        if ((*mpCloud)[(*mpIndices)[i_point]].x >= aabb_max_point.x) aabb_max_point.x = (*mpCloud)[(*mpIndices)[i_point]].x;
        if ((*mpCloud)[(*mpIndices)[i_point]].y >= aabb_max_point.y) aabb_max_point.y = (*mpCloud)[(*mpIndices)[i_point]].y;
        if ((*mpCloud)[(*mpIndices)[i_point]].z >= aabb_max_point.z) aabb_max_point.z = (*mpCloud)[(*mpIndices)[i_point]].z;
    }

    if (number_of_points == 0)
        number_of_points = 1;

    mean_value (0) /= number_of_points;
    mean_value (1) /= number_of_points;
    mean_value (2) /= number_of_points;
}

void CloudProcessor::ComputeCovarianceMatrix(const Eigen::Vector3f& mean_value, Eigen::Matrix <float, 3, 3>& covariance_matrix)
{
    covariance_matrix.setZero ();

    auto number_of_points = static_cast <unsigned int> (mpIndices->size ());
    float factor = 1.0f / static_cast <float> ((number_of_points - 1 > 0)?(number_of_points - 1):1);
    for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
    {
        Eigen::Vector3f current_point (0.0f, 0.0f, 0.0f);
        current_point (0) = (*mpCloud)[(*mpIndices)[i_point]].x - mean_value (0);
        current_point (1) = (*mpCloud)[(*mpIndices)[i_point]].y - mean_value (1);
        current_point (2) = (*mpCloud)[(*mpIndices)[i_point]].z - mean_value (2);

        covariance_matrix += current_point * current_point.transpose ();
    }

    covariance_matrix *= factor;
}

void CloudProcessor::ComputeEigenVectors(const Eigen::Matrix <float, 3, 3>& covariance_matrix,
    Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis, 
    float& major_value, float& middle_value, float& minor_value)
{
    Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> > eigen_solver;
    eigen_solver.compute (covariance_matrix);

    Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> >::EigenvectorsType eigen_vectors;
    Eigen::EigenSolver <Eigen::Matrix <float, 3, 3> >::EigenvalueType eigen_values;
    eigen_vectors = eigen_solver.eigenvectors ();
    eigen_values = eigen_solver.eigenvalues ();

    unsigned int temp = 0;
    unsigned int major_index = 0;
    unsigned int middle_index = 1;
    unsigned int minor_index = 2;

    if (eigen_values.real () (major_index) < eigen_values.real () (middle_index))
    {
        temp = major_index;
        major_index = middle_index;
        middle_index = temp;
    }

    if (eigen_values.real () (major_index) < eigen_values.real () (minor_index))
    {
        temp = major_index;
        major_index = minor_index;
        minor_index = temp;
    }

    if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index))
    {
        temp = minor_index;
        minor_index = middle_index;
        middle_index = temp;
    }

    major_value = eigen_values.real () (major_index);
    middle_value = eigen_values.real () (middle_index);
    minor_value = eigen_values.real () (minor_index);

    major_axis = eigen_vectors.col (major_index).real ();
    middle_axis = eigen_vectors.col (middle_index).real ();
    minor_axis = eigen_vectors.col (minor_index).real ();

    major_axis.normalize ();
    middle_axis.normalize ();
    minor_axis.normalize ();

    float det = major_axis.dot (middle_axis.cross (minor_axis));
    if (det <= 0.0f)
    {
        major_axis (0) = -major_axis (0);
        major_axis (1) = -major_axis (1);
        major_axis (2) = -major_axis (2);
    }
}