open3d实现搜索在一个球内部的点云

写在前面

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                                     

结果

黄色是搜索到的点云

参考

完

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