点云配准9：Colored-ICP的Open3D实现

本文主要是介绍点云配准9：Colored-ICP的Open3D实现，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

写在前面

本文内容
- 基于Open3D实现；
- Colored-ICP算法进行点云配准；
- 包含CMakeLists，cpp源码，代码解析，编译脚本，运行结果可视化；
- 提供免费的可执行文件以及使用说明：待上传
平台/环境
Windows10, Ubuntu1804, CMake, Open3D
转载请注明出处：
https://blog.csdn.net/qq_41102371/article/details/136856276

准备

编译安装、使用Open3D、PCL见：
各个版本的Open3D、PCL的编译、使用教程
本文基于Open3D 0.14.1进行编程和测试

原理

点云配准基础、ICP及其变体算法解析，见专栏文章点云配准(PointCloud Registration)

代码实现

(当前20240319，待继续更新)


#include <Eigen/Dense>
#include <iostream>
#include <memory>#include "open3d/Open3D.h"using namespace open3d;void VisualizeRegistration(const open3d::geometry::PointCloud &source,const open3d::geometry::PointCloud &target,const Eigen::Matrix4d &Transformation) {std::shared_ptr<geometry::PointCloud> source_transformed_ptr(new geometry::PointCloud);std::shared_ptr<geometry::PointCloud> target_ptr(new geometry::PointCloud);*source_transformed_ptr = source;*target_ptr = target;source_transformed_ptr->Transform(Transformation);visualization::DrawGeometries({source_transformed_ptr, target_ptr},"Registration result");
}void PrintHelp() {using namespace open3d;PrintOpen3DVersion();// clang-format offutility::LogInfo("Usage:");utility::LogInfo("    > RegistrationColoredICP source_pcd target_pcd [--visualize]");// clang-format onutility::LogInfo("");
}int main(int argc, char *argv[]) {using namespace open3d;utility::SetVerbosityLevel(utility::VerbosityLevel::Debug);if (argc < 3 ||utility::ProgramOptionExistsAny(argc, argv, {"-h", "--help"})) {PrintHelp();return 1;}bool visualize = false;if (utility::ProgramOptionExists(argc, argv, "--visualize")) {visualize = true;}// Prepare inputstd::shared_ptr<geometry::PointCloud> source =open3d::io::CreatePointCloudFromFile(argv[1]);std::shared_ptr<geometry::PointCloud> target =open3d::io::CreatePointCloudFromFile(argv[2]);if (source == nullptr || target == nullptr) {utility::LogWarning("Unable to load source or target file.");return -1;}std::vector<double> voxel_sizes = {0.05, 0.05 / 2, 0.05 / 4};std::vector<int> iterations = {50, 30, 14};Eigen::Matrix4d trans = Eigen::Matrix4d::Identity();for (int i = 0; i < 3; ++i) {float voxel_size = voxel_sizes[i];auto source_down = source->VoxelDownSample(voxel_size);source_down->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(voxel_size * 2.0, 30));auto target_down = target->VoxelDownSample(voxel_size);target_down->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(voxel_size * 2.0, 30));auto result = pipelines::registration::RegistrationColoredICP(*source_down, *target_down, 0.07, trans,pipelines::registration::TransformationEstimationForColoredICP(),pipelines::registration::ICPConvergenceCriteria(1e-6, 1e-6,iterations[i]));trans = result.transformation_;if (visualize) {VisualizeRegistration(*source, *target, trans);}}std::stringstream ss;ss << trans;utility::LogInfo("Final transformation = \n{}", ss.str());return 0;
}