0
点赞
收藏
分享

微信扫一扫

寻找两个点云重叠部分

笙烛 2022-01-16 阅读 183

目录

网上大部分寻找重叠区域都是对一个点云建立kdtree,然后在r半径内搜索另外一个点云的点。这种方法适合两个点云完全一样。一般的点云数据并不完全一样,例如两条航带的点云,并不完全相同,如果应用这方法会损失很多点,造成特征计算的不准确性。下面介绍两种方法:

写在前面的结论:第二种方法速度和精度上均优于第一种方法(octree yyds!!!)

方法1:

(1)假设有两组点云A,B,计算A的OBB包围盒
(2)寻找点云A的OBB包围盒内的点云B,这样就得到点云B相对A的重叠区域
(3)反之计算,得到点云A相对于B的重叠区域
(4)综上可以得到一组重叠区域的点云,可用于后续分析。
show the codes

// 提取点云重叠区域.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include <pcl/features/moment_of_inertia_estimation.h>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include<pcl/filters/crop_box.h>
#include <pcl/common/transforms.h>
#include<pcl/common/common.h>
//获取第二个点云相对第一个点云的重叠区域
void getOverlappedCloud(const pcl::PointCloud<pcl::PointXYZ> &cloud, const pcl::PointCloud<pcl::PointXYZ>&cloud2, pcl::PointCloud<pcl::PointXYZ>&overlapped_cloud2)
{
    pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
    feature_extractor.setInputCloud(cloud.makeShared());
    feature_extractor.compute();
    pcl::PointXYZ min_point_AABB;
    pcl::PointXYZ max_point_AABB;
    pcl::PointXYZ min_point_OBB;
    pcl::PointXYZ max_point_OBB;
    pcl::PointXYZ position_OBB;
    Eigen::Matrix3f rotational_matrix_OBB;
    feature_extractor.getAABB(min_point_AABB, max_point_AABB);
    feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
   //将点云旋转,使得第一个点云的OBB=AABB,以此获得第一个点云OBB区域内的第二个点云
    Eigen::Matrix4f tf = Eigen::Matrix4f::Identity();
    tf << (rotational_matrix_OBB.inverse());
    pcl::PointCloud<pcl::PointXYZ>cloud_tf, cloud2_tf;
    pcl::transformPointCloud(cloud, cloud_tf, tf);
    pcl::transformPointCloud(cloud2, cloud2_tf, tf);
    Eigen::Vector4f pmin, pmax;

    pcl::getMinMax3D(cloud_tf, pmin, pmax);
    pcl::PointCloud<pcl::PointXYZ> tf_overlapped_cloud2;
    std::vector<int> indices;
    //第二个点云旋转之后的点云重叠区域
    pcl::getPointsInBox(cloud2_tf, pmin, pmax, indices);
    pcl::copyPointCloud(cloud2_tf, indices, tf_overlapped_cloud2);
    //再变换到原始坐标系中
    pcl::transformPointCloud(tf_overlapped_cloud2, overlapped_cloud2, tf.inverse());

    std::cout << tf << std::endl;
    std::cout << rotational_matrix_OBB << std::endl;
}
int main(int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZ>cloud, overlapped_cloud;
    pcl::io::loadPCDFile("SHCSCloud.pcd", cloud);

    pcl::PointCloud<pcl::PointXYZ>cloud2,overlapped_cloud2;
    pcl::io::loadPCDFile("SHCSCloud副本.pcd", cloud2);

    getOverlappedCloud(cloud, cloud2, overlapped_cloud2);
    getOverlappedCloud(cloud2, cloud, overlapped_cloud);

    pcl::io::savePCDFile<pcl::PointXYZ>("重叠点云.pcd", overlapped_cloud);
    pcl::io::savePCDFile<pcl::PointXYZ>("重叠点云2.pcd", overlapped_cloud2);
}


方法1实验效果:

原始点云:
在这里插入图片描述
图中白色点云为原始红色点云的重叠区域:
在这里插入图片描述
绿色点云为蓝色原始点云的重叠区域:
在这里插入图片描述

耗时:
在这里插入图片描述

结论:从图上看,这种方法只能粗略点云重叠区域,提取的重叠区域与实际重叠区域存在一定偏差。

方法2:

(1)假设有两组点云A,B,对A建立八叉树
(2)遍历B中所有点,查询其对应的A的体素是否存在点云(即同一体素中是否同时存在AB点云),若存在,则该点为B点云的重叠点云
(3)同理,得到A中重叠点云
show the codes

// 提取点云重叠区域2.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include <iostream>
#include<pcl/io/pcd_io.h>
#include<pcl/point_cloud.h>
#include<pcl/point_types.h>
#include<pcl/octree/octree.h>
#include<ctime>
using point = pcl::PointXYZ;
using cloud = pcl::PointCloud<point>;
void getOverlappedCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud1, const pcl::PointCloud<pcl::PointXYZ>& cloud2, pcl::PointCloud<pcl::PointXYZ>& overlapped_cloud2)
{
    double radius = 3.0;
    pcl::octree::OctreePointCloudSearch<point> octree(radius);
    octree.setInputCloud(cloud1.makeShared());
    octree.addPointsFromInputCloud();

    //pcl::PointCloud<point> overlapped_2;
    for (size_t i = 0; i < cloud2.size(); ++i)
    {
        std::vector<int> indices;
        octree.voxelSearch(cloud2.points[i], indices);
        pcl::PointCloud<point> cloud_out;
        if (indices.size())
        {
            overlapped_cloud2.push_back(cloud2.points[i]);
        }

    }
}
int main()
{
    auto start = std::clock();
    pcl::PointCloud<point> cloud1, cloud2;
    pcl::io::loadPCDFile<point>("SHCSCloud.pcd", cloud1);
    pcl::io::loadPCDFile<point>("SHCSCloud副本.pcd", cloud2);
    cloud overlapped1, overlapped2;
    getOverlappedCloud(cloud1, cloud2, overlapped2);
    getOverlappedCloud(cloud2, cloud1, overlapped1);

    pcl::io::savePCDFile("重叠1.pcd", overlapped1);
    pcl::io::savePCDFile("重叠2.pcd", overlapped2);
    auto end = std::clock();
    std::cerr << "耗时" << std::difftime(end, start) << "ms" << std::endl;
    std::cout << "Hello World!\n";
}


方法2实验效果:

黄色为重叠区域
在这里插入图片描述
棕色为重叠区域
在这里插入图片描述
耗时:
在这里插入图片描述

结论:

第二种方法速度和精度上均优于第一种方法
写在后面的参考资料:
寻找点云OBB
旋转点云

举报

相关推荐

0 条评论