寻找两个点云重叠部分

目录

• • 方法1：
  • 方法1实验效果：
  • 方法2(c++)：
  • 方法2(python)
  • 方法2实验效果：
  • 结论：

网上大部分寻找重叠区域都是对一个点云建立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 
#include 
#include 
#include #include
#include 
#include
//获取第二个点云相对第一个点云的重叠区域
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(c++)：

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

// 提取点云重叠区域2.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//#include 
#include
#include
#include
#include
#include
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();point min_pt, max_pt;pcl::getMinMax3D(cloud1, min_pt, max_pt);//pcl::PointCloud overlapped_2;for (size_t i = 0; i < cloud2.size(); ++i){//新增判断，防止越界if (cloud2.points[i].x>max_pt.x|| cloud2.points[i].y > max_pt.y || cloud2.points[i].z > max_pt.z || cloud2.points[i].x < min_pt.x || cloud2.points[i].y < min_pt.y || cloud2.points[i].z < min_pt.z){continue;}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>("D:/1重叠.pcd", cloud1);pcl::io::loadPCDFile<point>("D:/2重叠.pcd", cloud2);cloud overlapped1, overlapped2;getOverlappedCloud(cloud1, cloud2, overlapped2);getOverlappedCloud(cloud2, cloud1, overlapped1);pcl::io::savePCDFile("D:/11重叠.pcd", overlapped1);pcl::io::savePCDFile("D:/22重叠.pcd", overlapped2);auto end = std::clock();std::cerr << "耗时" << std::difftime(end, start) << "ms" << std::endl;std::cout << "Hello World!\n";
}

方法2(python)

import open3d as o3ddef find_overlapped_cloud(cloud1, cloud2):overlapped_cloud_indices = []octree = o3d.geometry.Octree(max_depth=4)octree.convert_from_point_cloud(cloud1, size_expand=0.01)min_pt = octree.get_min_bound()max_pt = octree.get_max_bound()for point in cloud2.points:if point[0] < min_pt[0] or point[1] < min_pt[1] or point[2] < min_pt[2] or \point[0] > max_pt[0] or point[1] > max_pt[1] or point[2] > max_pt[2]:continueelse:leaf_node, leaf_info = octree.locate_leaf_node(point)if leaf_info is not None:indices = leaf_node.indicesfor indice in indices:overlapped_cloud_indices.append(indice)return cloud1.select_by_index(overlapped_cloud_indices)if __name__ == '__main__':"read cloud"cloud1 = o3d.io.read_point_cloud("D:/1重叠.pcd")cloud2 = o3d.io.read_point_cloud("D:/2重叠.pcd")"find overlapped cloud"overlapped_cloud1 = find_overlapped_cloud(cloud2, cloud1)overlapped_cloud2 = find_overlapped_cloud(cloud1, cloud2)"save cloud"o3d.io.write_point_cloud("D:/o3d_overlapped1.pcd", overlapped_cloud1)o3d.io.write_point_cloud("D:/o3d_overlapped2.pcd", overlapped_cloud2)

方法2实验效果：

黄色为重叠区域

棕色为重叠区域

耗时：

结论：

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

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！