写在前面

1、本文内容
给定一个点云P和一个圆心为center，半径为r的球，搜索出P中属于球内的点

2、平台/环境
使用open3d, cmake，适用windows/linux
3、转载请注明出处：
https://blog.csdn.net/qq_41102371/article/details/131666033

原理

利用open3d的kdtree，以点云P建立kdtree，对给定点进行进行半径搜索，在搜索之前先用边长=2*radius的box裁剪以加速搜索

代码

目录结构如下，请将sphere_search.cpp和CMakeLists.txt放入src

sphere_search.cpp

#include <iostream>
#include <chrono>#include <open3d/Open3D.h>std::shared_ptr<open3d::geometry::PointCloud> PointInSphere(std::shared_ptr<open3d::geometry::PointCloud> pcd, Eigen::Vector3d center, double radius, bool vis)
{auto search_s = std::chrono::high_resolution_clock::now();std::shared_ptr<open3d::geometry::PointCloud> points_in_sphere(new open3d::geometry::PointCloud);if (nullptr == pcd || pcd->IsEmpty()){*points_in_sphere = *pcd;return points_in_sphere;}std::cout << "point size: " << pcd->points_.size() << " search center: " << center.transpose() << " search radius: " << radius << std::endl;if (radius <= 0.0){*points_in_sphere = *pcd;return points_in_sphere;}// 先用边长等于2*radius的正方体裁剪，得到更少的点云，然后再搜索更快open3d::geometry::OrientedBoundingBox obb;obb.center_ = center;obb.extent_ = Eigen::Vector3d(radius * 2, radius * 2, radius * 2);std::vector<std::size_t> idx_crop = obb.GetPointIndicesWithinBoundingBox(pcd->points_);auto pcd_crop = pcd->SelectByIndex(idx_crop);auto crop_e = std::chrono::high_resolution_clock::now();auto crop_cost = std::chrono::duration_cast<std::chrono::microseconds>(crop_e - search_s).count() / 1000.0;std::cout << "crop_cost: " << crop_cost << " ms" << std::endl;open3d::geometry::KDTreeFlann target_kdtree;target_kdtree.SetGeometry(*pcd_crop);std::vector<int> indices;std::vector<double> dis;target_kdtree.SearchRadius(center, radius, indices, dis);if (indices.empty()){std::cout << "no point in sphere, please check your center or radius" << std::endl;return points_in_sphere;}else{std::cout << indices.size() << " pints searched in pcd" << std::endl;}// std::vector<int>转std::vector<std::size_t>std::vector<std::size_t> idx_in_sphere;for (auto i : indices){idx_in_sphere.push_back(i);}points_in_sphere = pcd_crop->SelectByIndex(idx_in_sphere);auto search_e = std::chrono::high_resolution_clock::now();auto search_cost = std::chrono::duration_cast<std::chrono::microseconds>(search_e - search_s).count() / 1000.0;std::cout << "total cost: " << search_cost << " ms" << std::endl;if (vis){auto coordinate = open3d::geometry::TriangleMesh::CreateCoordinateFrame(1.0);auto pcd_remain = pcd->SelectByIndex(idx_crop, true);auto pcd_inbox_remain = pcd_crop->SelectByIndex(idx_in_sphere, true);// pcd_remain->PaintUniformColor({1, 0, 0});pcd_inbox_remain->PaintUniformColor({0, 1, 0});points_in_sphere->PaintUniformColor({1, 1, 0});obb.color_ = Eigen::Vector3d(0, 0, 1);open3d::visualization::DrawGeometries({coordinate, pcd_remain, pcd_inbox_remain, points_in_sphere,std::make_shared<open3d::geometry::OrientedBoundingBox>(obb)}, "sphere search result");}return points_in_sphere;
}
int main(int argc, char *argv[])
{int idx = open3d::utility::GetProgramOptionAsInt(argc, argv, "--idx", 0);bool vis = open3d::utility::ProgramOptionExists(argc, argv, "--vis");double radius = open3d::utility::GetProgramOptionAsDouble(argc, argv, "--radius", 0);std::string path_pcd = open3d::utility::GetProgramOptionAsString(argc, argv, "--path_pcd", "");if (path_pcd.empty()){std::cout << "please input pcd path with --path_pcd" << std::endl;return 0;}std::shared_ptr<open3d::geometry::PointCloud> pcd(new open3d::geometry::PointCloud);open3d::io::ReadPointCloud(path_pcd, *pcd);std::cout << "read " << path_pcd << " with " << pcd->points_.size() << " points" << std::endl;// 使用索引以点云内某一点为圆心if (open3d::utility::ProgramOptionExists(argc, argv, "--idx")){PointInSphere(pcd, pcd->points_[idx], radius, true);}else{// 另外指定圆心PointInSphere(pcd, {0, 0, 0}, radius, vis);}return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.18)
project(SphereSearch)option(STATIC_WINDOWS_RUNTIME "Use static (MT/MTd) Windows runtime" ON)if(STATIC_WINDOWS_RUNTIME)set(CMAKE_MSVC_RUNTIME_LIBRARY "MultiThreaded$<$<CONFIG:Debug>:Debug>")
else()set(CMAKE_MSVC_RUNTIME_LIBRARY "MultiThreaded$<$<CONFIG:Debug>:Debug>DLL")
endif()# set(Open3D_ROOT /root/open3D_install/) #linux
find_package(Open3D REQUIRED)
include_directories(${Open3D_INCLUDE_DIRS}
)add_executable(sphere_search ./sphere_search.cpp)
target_link_libraries(sphere_search ${Open3D_LIBRARIES})

compile.bat
请为替换自己的open3d目录，该目录包含open3d的cmake文件

cmake -DCMAKE_BUILD_TYPE=Release -DOpen3D_DIR="D:\carlos\install\open3d141\CMake" -S ./src -B ./build
cmake --build ./build --config Release --target ALL_BUILD

run.bat

.\build\Release\sphere_search.exe .\build\Release\sphere_search.exe --path_pcd D:\carlos\my_tools\data\bun000.pcd --radius 0.05 --vis --idx 0                                     

结果

黄色是搜索到的点云

参考

完

主要做激光/影像三维重建，配准、分割等常用点云算法，技术交流、咨询可私信