open3d使用fpfh做点云配准

写在前面

1、环境：win10，cmake3.22.0-rc1，环境：win10，cmake3.22.0-rc1，已编译好的open3d 0.13
2、源码使用的是官方example
https://github.com/isl-org/Open3D/blob/master/examples/cpp/RegistrationRANSAC.cpp
3、建议先看一下open3d的demo:
open3d 0.13的c++版本使用demo https://blog.csdn.net/qq_41102371/article/details/12106527
4、所有资源已给出链接

准备

编译open3d

windows10编译open3d 0.13 https://blog.csdn.net/qq_41102371/article/details/121014372

配准example

将官方demo的CMakeLists.txt与配准源码RegistrationRANSAC.cpp
下载下来，放在文件夹open3d_fpfh_registration下
https://github.com/isl-org/open3d-cmake-find-package/blob/master/CMakeLists.txt
https://github.com/isl-org/Open3D/blob/master/examples/cpp/RegistrationRANSAC.cpp
并将CMakeLists.txt里面的Draw全部换成RegistrationRANSAC
CMakeLists.txt

# On Ubuntu 18.04, get the latest CMake from https://apt.kitware.com/.
cmake_minimum_required(VERSION 3.18)

project(Open3DCMakeFindPackage LANGUAGES C CXX)

# The options need to be the same as Open3D's default
# If Open3D is configured and built with custom options, you'll also need to
# specify the same custom options.
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()

# Find installed Open3D, which exports Open3D::Open3D
find_package(Open3D REQUIRED)

add_executable(RegistrationRANSAC)
target_sources(RegistrationRANSAC PRIVATE RegistrationRANSAC.cpp)
target_link_libraries(RegistrationRANSAC PRIVATE Open3D::Open3D)

# On Windows if BUILD_SHARED_LIBS is enabled, copy .dll files to the executable directory
if(WIN32)
    get_target_property(open3d_type Open3D::Open3D TYPE)
    if(open3d_type STREQUAL "SHARED_LIBRARY")
        message(STATUS "Copying Open3D.dll to ${CMAKE_CURRENT_BINARY_DIR}/$")
        add_custom_command(TARGET RegistrationRANSAC POST_BUILD
                           COMMAND ${CMAKE_COMMAND} -E copy
                                   ${CMAKE_INSTALL_PREFIX}/bin/Open3D.dll
                                   ${CMAKE_CURRENT_BINARY_DIR}/$)
    endif()
endif()

RegistrationRANSAC.cpp，我在官方代码上做了些修改

// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include 
#include 
#include 

#include "open3d/Open3D.h"

using namespace open3d;
using namespace std;
std::tuple<std::shared_ptr<geometry::PointCloud>,
           std::shared_ptr<pipelines::registration::Feature>>
PreprocessPointCloud(const char *file_name, float voxel_size = 2.0) {
    //从文件读取点云
    auto pcd = open3d::io::CreatePointCloudFromFile(file_name);
    //降采样
    auto pcd_down = pcd->VoxelDownSample(voxel_size);
    std::cout << "read " << pcd->points_.size() << " points from " << file_name
              << std::endl;
    std::cout << "voxel_size=" << voxel_size << ", after downsample "
              << pcd_down->points_.size() << "points left" << endl;
    //计算法向量
    pcd_down->EstimateNormals(
            open3d::geometry::KDTreeSearchParamHybrid(voxel_size * 2, 30));
    //计算fpfh特征
    auto pcd_fpfh = pipelines::registration::ComputeFPFHFeature(
            *pcd_down,
            open3d::geometry::KDTreeSearchParamHybrid(voxel_size * 5, 100));
    return std::make_tuple(pcd_down, pcd_fpfh);
}


void PrintHelp() {
    using namespace open3d;

    PrintOpen3DVersion();
    // clang-format off
    utility::LogInfo("Usage:");
    utility::LogInfo("    > RegistrationRANSAC source_pcd target_pcd  voxel_size [--method=feature_matching] [--mutual_filter] [--visualize]");
    // clang-format on
    utility::LogInfo("");
}

int main(int argc, char *argv[]) {
    using namespace open3d;

    utility::SetVerbosityLevel(utility::VerbosityLevel::Debug);

    if (argc < 4 ||
        utility::ProgramOptionExistsAny(argc, argv, {"-h", "--help"})) {
        PrintHelp();
        return 1;
    }

    bool visualize = false;
    if (utility::ProgramOptionExists(argc, argv, "--visualize")) {
        visualize = true;
    }
    //visualize = true;


    bool mutual_filter = false;
    //if (utility::ProgramOptionExists(argc, argv, "--mutual_filter")) {
    //    mutual_filter = true;
    //}
    mutual_filter = true;

    // Prepare input
    std::shared_ptr<geometry::PointCloud> source, target;
    std::shared_ptr<pipelines::registration::Feature> source_fpfh, target_fpfh;
    //set voxel_size
    float voxel_size = std::atof(argv[3]);
    std::cout << voxel_size << std::endl;
    float distance_threshold = voxel_size * 1.5;

    std::tie(source, source_fpfh) = PreprocessPointCloud(argv[1], voxel_size);
    std::tie(target, target_fpfh) = PreprocessPointCloud(argv[2], voxel_size);

    pipelines::registration::RegistrationResult registration_result;

    // Prepare checkers
    std::vector<std::reference_wrapper<
            const pipelines::registration::CorrespondenceChecker>>
            correspondence_checker;
    auto correspondence_checker_edge_length =
            pipelines::registration::CorrespondenceCheckerBasedOnEdgeLength(
                    0.9);
    auto correspondence_checker_distance =
            pipelines::registration::CorrespondenceCheckerBasedOnDistance(
                    distance_threshold);

    correspondence_checker.push_back(correspondence_checker_edge_length);
    correspondence_checker.push_back(correspondence_checker_distance);



    registration_result = pipelines::registration::
            RegistrationRANSACBasedOnFeatureMatching(
                    *source, *target, *source_fpfh, *target_fpfh,
                    mutual_filter, distance_threshold,
                    pipelines::registration::
                            TransformationEstimationPointToPoint(false),
                    3, correspondence_checker,
                    pipelines::registration::RANSACConvergenceCriteria(
                            1000000, 0.999));
    cout << endl
            << "fpfh matrix:" << endl
            << registration_result.transformation_ << endl;
    cout << "inlier(correspondence_set size):"
            << registration_result.correspondence_set_.size() << endl;


    std::vector<std::pair<int, int>> correspondences_ransac;
    for (int m = 0; m < registration_result.correspondence_set_.size();
            ++m) {
        std::pair<int, int> pair_(0, 0);
        pair_.first = registration_result.correspondence_set_[m][0];
        pair_.second = registration_result.correspondence_set_[m][1];
        correspondences_ransac.push_back(pair_);
    }
    // std::shared_ptr
    auto ransac_lineset =
            geometry::LineSet::CreateFromPointCloudCorrespondences(
                    *source, *target, correspondences_ransac);

    std::shared_ptr<geometry::PointCloud> source_transformed_ptr(
            new geometry::PointCloud);
    std::shared_ptr<geometry::PointCloud> source_ptr(
            new geometry::PointCloud);
    std::shared_ptr<geometry::PointCloud> target_ptr(
            new geometry::PointCloud);
    *source_transformed_ptr = *source;
    *target_ptr = *target;
    *source_ptr = *source;
    target->PaintUniformColor({1, 0, 0});                  //红
    source->PaintUniformColor({0, 1, 0});                  //绿
    source_transformed_ptr->PaintUniformColor({0, 0, 1});  //蓝
    source_transformed_ptr->Transform(registration_result.transformation_);
    
    if (visualize) {
        visualization::DrawGeometries(
                {target, source, source_transformed_ptr, ransac_lineset},
                "Registration result", 960, 900, 960, 100);
    }

    return 0;
}

编译demo

cd open3d_fpfh_registration
mkdir build
cd build
cmake -DOpen3D_ROOT="D:/Program Files/open3d" ..
cmake --build . --config Debug

成功生成RegistrationRANSAC.exe

配准测试

数据就用斯坦福的兔兔，http://graphics.stanford.edu/pub/3Dscanrep/bunny.tar.gz
解压后把bun000.ply和bun045.ply放在open3d_fpfh_registration目录下
回到命令行窗口：

start ./Debug/RegistrationRANSAC.exe ../bun045.ply ../bun000.ply 0.005 --visualize

若是其他数据，改一下…/bun045.ply …/bun000.ply 0.005 --visualize这几个参数就好，分别是source点云路径，target点云路径，降采样网格大小(根据点云大小和数据量来自行调节)，–visualize加可视化

配准结果，红色是bun000，绿色是bun045，蓝色是bun045配准至bun000的结果，连线是连接的匹配上的fpfh特征

参考

文中已列出

完

--------------------------------------------------------------------------------------------诺有缸的高飞鸟202110