PCL中构建abb包围盒

最新推荐文章于 2024-07-12 16:34:36 发布

邶风击鼓162

最新推荐文章于 2024-07-12 16:34:36 发布

阅读量19

点赞数 2

文章标签： c++

本文链接：https://blog.csdn.net/2302_79615510/article/details/138387112

版权

本文介绍了如何使用PointCloudLibrary(PCL)对点云数据进行滤波，去除噪声，然后计算并可视化三维物体的最小外包立方体（OBB），包括主轴、质心和包围盒尺寸。

摘要由CSDN通过智能技术生成

#include <vector>
#include <thread>
#include<iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/features/moment_of_inertia_estimation.h>

int main(int argc, char** argv)
{
	// --------------------------------读取点云--------------------------------
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile<pcl::PointXYZ>("../cylindCloud3.pcd", *cloud);
	pcl::PCDReader reader;
	
	// --------------------------滤波-------------------------
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;

       sor.setInputCloud (cloud);
       sor.setMeanK (50);//设置在进行统计时考虑查询点近邻点数
       sor.setStddevMulThresh (1.0);//判断是否为离群点的阀值
       sor.filter (*cloud_filtered);//执行滤波处理保存内点
       std::cerr << "Cloud after filtering: " << std::endl;

       std::cerr << *cloud_filtered << std::endl;


      
    // -------------------------- OBB盒子  -------------------------
    pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
    feature_extractor.setInputCloud (cloud_filtered);
    feature_extractor.compute ();

    std::vector <float> moment_of_inertia;
    std::vector <float> eccentricity;
  
    pcl::PointXYZ min_point_OBB;
    pcl::PointXYZ max_point_OBB;
    pcl::PointXYZ position_OBB;
    Eigen::Matrix3f rotational_matrix_OBB;
    float major_value, middle_value, minor_value;
    Eigen::Vector3f major_vector, middle_vector, minor_vector;
    Eigen::Vector3f mass_center;

    // 获取惯性矩
    feature_extractor.getMomentOfInertia (moment_of_inertia);
    // 获取离心率
    feature_extractor.getEccentricity (eccentricity);
    
   
    // 获取OBB盒子
    feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
    feature_extractor.getEigenValues (major_value, middle_value, minor_value);
    // 获取主轴major_vector，中轴middle_vector，辅助轴minor_vector
    feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
    // 获取质心
    feature_extractor.getMassCenter (mass_center);
    std::cout << "方法一 质心坐标 " << mass_center(0) << " " << mass_center(1) << " " << mass_center(2) << std::endl;

    pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->setBackgroundColor (0, 0, 0);
    viewer->addCoordinateSystem (1.0);
    viewer->initCameraParameters ();
    viewer->addPointCloud<pcl::PointXYZ> (cloud_filtered, "sample cloud");
   
    // 添加OBB包容盒
    Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
    Eigen::Quaternionf quat (rotational_matrix_OBB);
    // position：中心位置
    // quat：旋转矩阵
    // max_point_OBB.x - min_point_OBB.x  宽度
    // max_point_OBB.y - min_point_OBB.y  高度
    // max_point_OBB.z - min_point_OBB.z  深度
    float length,width, height,a,b,c;
      
	std::cout << "包围盒的长为:" <<max_point_OBB.x - min_point_OBB.x  << "米" << std::endl; 
	std::cout << "包围盒的宽为:" << max_point_OBB.y - min_point_OBB.y << "米" << std::endl; 
	std::cout << "包围盒的高为:" <<max_point_OBB.z - min_point_OBB.z  << "米" << std::endl; 
    viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "OBB");

    pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
    pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));
    pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));
    pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));
    viewer->addLine (center, x_axis, 100.0f, 0.0f, 0.0f, "major eigen vector");
    viewer->addLine (center, y_axis, 0.0f, 100.0f, 0.0f, "middle eigen vector");
    viewer->addLine (center, z_axis, 0.0f, 0.0f, 100.0f, "minor eigen vector");

    while(!viewer->wasStopped())
    {
        viewer->spinOnce (10000000);
       
    }

    return (0);
}