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
#include
#include
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转std::vector
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$<$:Debug>")
else()
set(CMAKE_MSVC_RUNTIME_LIBRARY "MultiThreaded$<$: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
主要做激光/影像三维重建,配准、分割等常用点云算法,技术交流、咨询可私信