open3d可视化对应点连线

写在前面

1、本文内容
open3d点云配准时，添加点对连线，用于可视化当前的correspondences
2、平台
windows10, visual studio 2019
3、转载请注明出处：
https://blog.csdn.net/qq_41102371/article/details/123351399

准备

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

修改源码

Registration.h

本地找到pipelines/registration/Registration.h，在里面RegistrationResult添加成员CorrespondenceSet correspondence_set_mutulfilter
原始代码
https://github.com/isl-org/Open3D/blob/v0.13.0/cpp/open3d/pipelines/registration/Registration.h#L129

public:
    /// The estimated transformation matrix.
    Eigen::Matrix4d_u transformation_;
    /// Correspondence set between source and target point cloud.
    CorrespondenceSet correspondence_set_;
    /// RMSE of all inlier correspondences. Lower is better.
    double inlier_rmse_;
    /// For ICP: the overlapping area (# of inlier correspondences / # of points
    /// in target). Higher is better.
    /// For RANSAC: inlier ratio (# of inlier correspondences / # of
    /// all correspondences)
    double fitness_;

添加后

public:
    /// The estimated transformation matrix.
    Eigen::Matrix4d_u transformation_;
    /// Correspondence set between source and target point cloud after mutul filter
    CorrespondenceSet correspondence_set_mutulfilter;
    /// Correspondence set between source and target point cloud.(final)
    CorrespondenceSet correspondence_set_;

    /// RMSE of all inlier correspondences. Lower is better.
    double inlier_rmse_;
    /// For ICP: the overlapping area (# of inlier correspondences / # of points
    /// in target). Higher is better.
    /// For RANSAC: inlier ratio (# of inlier correspondences / # of
    /// all correspondences)
    double fitness_;

Registration.cpp

pipelines/registration/Registration.cpp，原始代码
https://github.com/isl-org/Open3D/blob/v0.13.0/cpp/open3d/pipelines/registration/Registration.cpp#L338

        // Empirically mutual correspondence set should not be too small
        if (int(corres_mutual.size()) >= ransac_n * 3) {
            utility::LogDebug("{:d} correspondences remain after mutual filter",
                              corres_mutual.size());
            return RegistrationRANSACBasedOnCorrespondence(
                    source, target, corres_mutual, max_correspondence_distance,
                    estimation, ransac_n, checkers, criteria);
        }

改成

        // Empirically mutual correspondence set should not be too small
        if (int(corres_mutual.size()) >= ransac_n * 3) {
            utility::LogDebug("{:d} correspondences remain after mutual filter",
                              corres_mutual.size());
            RegistrationResult result_output =
                    RegistrationRANSACBasedOnCorrespondence(
                            source, target, corres_mutual,
                            max_correspondence_distance, estimation, ransac_n,
                            checkers, criteria);
            result_output.correspondence_set_mutulfilter = corres_mutual;
            return result_output;

使用

在此基础上，(cmakelists文件不变)
open3d使用fpfh做点云配准 https://blog.csdn.net/qq_41102371/article/details/121066031，添加可视化ransac之前的特征匹配的代码：

// ----------------------------------------------------------------------------
// -                        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;
//mutual filter之后的特征匹配连线，202203
        std::vector<std::pair<int, int>> correspondences_mutual_filter;
        for (int m = 0; m < registration_result.correspondence_set_mutulfilter.size();
            ++m) {
            std::pair<int, int> pair_(0, 0);
            pair_.first = registration_result.correspondence_set_mutulfilter[m][0];
            pair_.second = registration_result.correspondence_set_mutulfilter[m][1];
            correspondences_mutual_filter.push_back(pair_);
        }
        // std::shared_ptr
        auto mutual_filter_lineset =
            geometry::LineSet::CreateFromPointCloudCorrespondences(
                *source, *target, correspondences_mutual_filter);

    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) {
    //add fpfh correspondences after mutual filter, 202203
        visualization::DrawGeometries(
            { target, source, source_transformed_ptr, mutual_filter_lineset },
            "fpfh correspondences after mutual filter", 960, 900, 960, 100);
 
        visualization::DrawGeometries(
                {target, source, source_transformed_ptr, ransac_lineset},
                "fpfh correspondences after ransace", 960, 900, 960, 100);
    }

    return 0;
}

编译与使用

cd open3d_fpfh_registration
cmake -DOpen3D_ROOT="D:/carlos/code/open3d13/test/install/" -S . -B ./build_r
cmake --build ./build_r --config Release --target ALL_BUILD
.\build_r\Release\RegistrationRANSAC.exe .\bun045.ply .\bun000.ply 0.003 --visualize

其中，D:/carlos/code/open3d13/test/install/是你当前编译open3d的安装路径，根据自己的来

结果
mutual filter之后，ransac之前

ransac之后

参考

文中已列出

完

如有错漏，敬请指正
--------------------------------------------------------------------------------------------诺有缸的高飞鸟202203