ICP(迭代最近点)迭代过程的单步可视化程序-编程知识

ICP(迭代最近点)迭代过程的单步可视化程序

一个逐次迭代点云的程序, 可以逐步显示icp迭代的过程, 观察到点云逐步靠近的过程.

其中红色点云为target点云, 蓝色点云为带先验位姿的source点云, 绿色为无先验位姿的source点云

在程序中, 先验位姿存与实际变换之间的变化为, x, y, z轴误差5厘米, 三轴旋转均为0.05弧度

在程序运行后, 会在当前目录创建一个result.txt的文件, 保存的是初始变换, 即最终匹配应该输出的矩阵,

可以将控制台输出的矩阵与该文件中的矩阵进行比较, 判断对齐效果,

注: 有的值会是相反数, 因为旋转次序或方向可能不一样
测试数据集没记错的话应该是 frame005.pcd

运行结果

在这里插入图片描述

运行: 以ubuntu为例

mkdir ~/icp_test -p
cd ~/icp_test
vim main.cpp # 将下面cpp代码拷贝进来, 注意修改pcd文件的路径
vim CMakeLists.txt #将下面的CMakelist拷贝进来
cmake .
make
./icp_example

main.cpp

#include <iostream>
#include <fstream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <boost/random.hpp>
#include <chrono>
#include <thread>Eigen::Matrix4f generateRandomTransformation() {unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();boost::random::mt19937 gen(seed);  // 随机数生成器boost::random::uniform_real_distribution<> dis(-0.5, 0.5);  // 位移范围boost::random::uniform_real_distribution<> angle_dis(-M_PI, M_PI);  // 旋转范围Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();transform(0, 3) = dis(gen); // X轴平移transform(1, 3) = dis(gen); // Y轴平移transform(2, 3) = dis(gen); // Z轴平移// 绕Z轴旋转float angle = angle_dis(gen);transform(0, 0) = cos(angle);transform(0, 1) = -sin(angle);transform(1, 0) = sin(angle);transform(1, 1) = cos(angle);return transform;
}void saveTransformation(const Eigen::Matrix4f &transform, const std::string &filename) {std::ofstream file(filename);if (file.is_open()) {file << transform;file.close();}
}Eigen::Matrix4f generateCloseTransformation(const Eigen::Matrix4f &original) {// 单位 [m]Eigen::Matrix4f closeTransform = original;closeTransform(0, 3) += 0.05; // X轴微调closeTransform(1, 3) += 0.05; // Y轴微调closeTransform(2, 3) += 0.05; // Z轴微调// 单位 [rad]Eigen::Matrix3f rotation = closeTransform.block<3, 3>(0, 0);Eigen::AngleAxisf rotationX(0.05, Eigen::Vector3f::UnitX());Eigen::AngleAxisf rotationY(0.05, Eigen::Vector3f::UnitY());Eigen::AngleAxisf rotationZ(0.05, Eigen::Vector3f::UnitZ());rotation *= (rotationX * rotationY * rotationZ).matrix();closeTransform.block<3, 3>(0, 0) = rotation;return closeTransform;
}int main() {// 加载点云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ>("/home/smile/ros/icp/test.pcd", *cloud_in) == -1) {PCL_ERROR("Couldn't read file test.pcd \n");return -1;}// 移除NaN值std::vector<int> indices;pcl::removeNaNFromPointCloud(*cloud_in, *cloud_in, indices);// 进行体素滤波pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;voxel_grid.setInputCloud(cloud_in);voxel_grid.setLeafSize(0.08f, 0.08f, 0.08f);  // 设置体素大小pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);voxel_grid.filter(*cloud_filtered);// 生成变换并保存到文件Eigen::Matrix4f base_transformation = generateRandomTransformation();Eigen::Matrix4f base_transformation_prior = base_transformation;std::cout << "Base Transformation Matrix:\n" << base_transformation << std::endl;saveTransformation(base_transformation, "/home/smile/ros/icp/result.txt");// 生成接近的变换作为先验位姿Eigen::Matrix4f prior_pose = generateCloseTransformation(base_transformation);// 应用初始变换pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);pcl::transformPointCloud(*cloud_filtered, *cloud_transformed, base_transformation);// 设置ICP实例pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp, icp_with_prior;icp.setInputSource(cloud_transformed);icp.setInputTarget(cloud_filtered);icp.setMaximumIterations(1); // 每次调用align时执行一次迭代icp_with_prior.setInputSource(cloud_transformed);icp_with_prior.setInputTarget(cloud_filtered);icp_with_prior.setMaximumIterations(1);// 初始化可视化pcl::visualization::PCLVisualizer viewer("ICP demo");viewer.setBackgroundColor(0, 0, 0);viewer.addPointCloud<pcl::PointXYZ>(cloud_filtered, "cloud_filtered");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_filtered");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "cloud_filtered");pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp(new pcl::PointCloud<pcl::PointXYZ>(*cloud_transformed));pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp_prior(new pcl::PointCloud<pcl::PointXYZ>(*cloud_transformed));viewer.addPointCloud<pcl::PointXYZ>(cloud_icp, "cloud_icp");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_icp");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "cloud_icp");viewer.addPointCloud<pcl::PointXYZ>(cloud_icp_prior, "cloud_icp_prior");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_icp_prior");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 1.0, "cloud_icp_prior");viewer.addCoordinateSystem(1.0);viewer.initCameraParameters();// 创建初始变换的矩阵, 其中无先验位姿的是一个单位阵Eigen::Matrix4f icp_result = Eigen::Matrix4f::Identity();// 先验的矩阵为之前生成的, 在实际变换的基础上添加了扰动的变换矩阵Eigen::Matrix4f icp_result_prior = prior_pose;// 计数器int icp_cnt = 0; // icp迭代次数int icp_prior_cnt = 0; // 先验icp迭代次数bool icp_fitness_reached = false; bool icp_prior_fitness_reached = false;int iteration_counter = 0;  // 迭代频率计数器, 迭代的频率按照 10ms x iteration_counter 可以在下面的循环中修改while (!viewer.wasStopped()) {// 如果都完成了收敛, 则不再更新if(icp_fitness_reached && icp_prior_fitness_reached) continue;viewer.spinOnce();  // 图像化界面刷新频率10ms, 方便使用鼠标进行控制视角 std::this_thread::sleep_for(std::chrono::milliseconds(10));// 创建icp之后的新点云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp_it(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp_prior_it(new pcl::PointCloud<pcl::PointXYZ>);// 每10ms x 100 = 1000ms = 1s 即每1秒做一次icp并更新点云if (++iteration_counter >= 100) {// 如果没有达到0.0001的分值, 则icp继续迭代if(!icp_fitness_reached){// 对普通ICP和带有先验位姿的ICP进行迭代icp.align(*cloud_icp_it,icp_result);icp_result = icp.getFinalTransformation();// 检查是否收敛(肯定收敛, 因为最多迭代1次,所以每一次都会收敛)if (icp.hasConverged()) {double fitness_score = icp.getFitnessScore();if(icp_fitness_reached) icp_cnt=icp_cnt;else icp_cnt += 1;std::cout << "[ICP] 分数为 " << fitness_score <<std::endl;// 获取最新一次的变换, 并将该变换应用到带先验的点云上, 更新该点云base_transformation = icp.getFinalTransformation().cast<float>();pcl::transformPointCloud(*cloud_transformed, *cloud_icp_it, base_transformation);viewer.updatePointCloud<pcl::PointXYZ>(cloud_icp_it, "cloud_icp");//真正的停止条件(收敛条件)if(fitness_score<=0.001){icp_fitness_reached = true;std::cout << "======================================================="<<std::endl;std::cout << "[ICP]完成收敛 " <<std::endl;std::cout << "[ICP]迭代次数为 " << icp_cnt <<std::endl;std::cout << "[ICP]变换矩阵 " << std::endl;std::cout << icp.getFinalTransformation() << std::endl;std::cout << "======================================================="<<std::endl;}     }}       if(!icp_prior_fitness_reached){icp_with_prior.align(*cloud_icp_prior_it, icp_result_prior);icp_result_prior = icp_with_prior.getFinalTransformation();// 同理, 这里并不是真正的停止条件if (icp_with_prior.hasConverged()) {double fitness_score_prior = icp_with_prior.getFitnessScore();if(icp_prior_fitness_reached) icp_prior_cnt = icp_prior_cnt;else icp_prior_cnt += 1;std::cout << "[ICP+先验] 分数为 " << fitness_score_prior <<std::endl;// 带先验的停止条件也是0.0001分以下终止if(fitness_score_prior<=0.001){icp_prior_fitness_reached = true;std::cout << "======================================================="<<std::endl;std::cout << "[ICP+先验]完成收敛 " <<std::endl;std::cout << "[ICP+先验]迭代次数为 " << icp_prior_cnt <<std::endl;std::cout << "[ICP+先验]变换矩阵 " <<std::endl;std::cout << icp_with_prior.getFinalTransformation() << std::endl;std::cout << "======================================================="<<std::endl;}// 获取最新一次的变换, 并将该变换应用到带先验的点云上, 更新该点云base_transformation_prior = icp_with_prior.getFinalTransformation().cast<float>();pcl::transformPointCloud(*cloud_transformed, *cloud_icp_prior_it, base_transformation_prior);viewer.updatePointCloud<pcl::PointXYZ>(cloud_icp_prior_it, "cloud_icp_prior"); }}// 重置迭代计数器iteration_counter = 0;}}
return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.0 FATAL_ERROR)
project(icp_example)
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED True)find_package(PCL 1.8 REQUIRED)include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})add_executable(icp_example main.cpp)
target_link_libraries(icp_example ${PCL_LIBRARIES})