SAICP(模拟退火迭代最近点)的实现

SAICP(模拟退火迭代最近点)的实现

注: 本系列所有文章在github开源, 也是我个人的学习笔记, 欢迎大家去star以及fork, 感谢!

仓库地址: pointcloud-processing-visualization

总结一下上周的学习情况

ICP会存在局部最小值的问题, 这个问题可能即使是没有实际遇到过, 也或多或少会在各种点云匹配算法相关博客中看到,

于是我去查了一些资料, 发现可以通过模拟退火算法解决, 或者说有概率可以跳出局部最小值

最后将两个算法结合了起来, 写了这个demo, 同时因为我也是初学者, 因此将执行改成了单步运行,

同时将ICP以及模拟退火ICP(SAICP)的执行效果通过PCL Viewer逐步展示出来.

参考资料

@智能算法 模拟退火算法详解

@维基百科 模拟退火

@chatGPT 4.0 这个有点可惜, 已经找不到当时跟GPT的聊天记录了

模拟退火的实现

扰动的生成

在进行点云匹配时, 如果要实现模拟退火中的随机干扰, 我使用的思路是生成一组随机变换, 即一个Eigen::Matrix4f randTrans

这个变换随机生成, 包含x, y, z三轴横移, 以及roll, pitch, yaw三轴转动

通过温度值(temperature)来控制变换的幅度大小

对应代码块

// 根据温度生成随机的变换矩阵
Eigen::Matrix4f generateAnnealingTransformation(double temperature) {unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();boost::random::mt19937 gen(seed);double tras_scale = 0.15;double rot_scale = 0.17 * M_PI;boost::random::uniform_real_distribution<> rand_dis(0, 1);boost::random::uniform_real_distribution<> dis(-tras_scale * temperature, tras_scale * temperature);boost::random::uniform_real_distribution<> angle_dis(-rot_scale * temperature, rot_scale * temperature);Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();transform(0, 3) = dis(gen); // X-axis translation disturbancetransform(1, 3) = dis(gen); // Y-axis translation disturbancetransform(2, 3) = dis(gen); // Z-axis translation disturbance// Rotation disturbance around Z-axisfloat angleZ = angle_dis(gen);Eigen::Matrix4f rotZ = Eigen::Matrix4f::Identity();rotZ(0, 0) = cos(angleZ);rotZ(0, 1) = -sin(angleZ);rotZ(1, 0) = sin(angleZ);rotZ(1, 1) = cos(angleZ);// Rotation disturbance around Y-axisfloat angleY = angle_dis(gen);Eigen::Matrix4f rotY = Eigen::Matrix4f::Identity();rotY(0, 0) = cos(angleY);rotY(0, 2) = sin(angleY);rotY(2, 0) = -sin(angleY);rotY(2, 2) = cos(angleY);// Rotation disturbance around X-axisfloat angleX = angle_dis(gen);Eigen::Matrix4f rotX = Eigen::Matrix4f::Identity();rotX(1, 1) = cos(angleX);rotX(1, 2) = -sin(angleX);rotX(2, 1) = sin(angleX);rotX(2, 2) = cos(angleX);// Combine the transformationstransform = transform * rotZ * rotY * rotX;return transform;
}// 在原有变换的基础上添加模拟退火的随机扰动
Eigen::Matrix4f annealing_transform = generateAnnealingTransformation(temperature);
Eigen::Matrix4f perturbed_transformation = saicp_result * annealing_transform;// 应用带有扰动的变换进行ICP迭代
saicp.align(*saicp_cloud, perturbed_transformation);

公式的实现

模拟退火时, 如果当前结果优于上次的结果, 就采纳当前结果,

如果当前结果比上次的结果差, 按以下公式来选择是否会选择差的结果(跳出局部最优的手段)
$$e^{\frac{R_1-R_0}{T}}>rand()$$
其中R1为这次的结果, R0为上次的结果 T为当前温度, rand()是随机生成的一个数字

如果当前的结果比上次的差, 则R1-R0为负数, 因此概率会随着温度的降低越来越小, 随着误差的增大也越来越小

在ICP中, 可以用icp.getFitnessScore()作为误差的判断条件, 当前的fitnessscore比上次小, 则采纳当前结果, 反之根据两次fitnessscore的差值计算概率

对应代码块为

// 退火
double new_fitness_score = saicp.getFitnessScore();if (new_fitness_score < last_fitness_score) 
{saicp_result = new_icp_result; // 接受更好的变换last_fitness_score = new_fitness_score; // 更新最新的fitness score
} 
// 新值大于旧值说明结果差, 取反, 满足误差越大概率越小的条件
else if (exp((-(new_fitness_score - last_fitness_score)) / temperature) > ((double)rand() / RAND_MAX))
{saicp_result = perturbed_transformation; // 以一定概率接受较差的变换last_fitness_score = new_fitness_score; // 更新fitness score，即使它变差了
}
// 更新温度
temperature *= coolingRate;

完整代码

TODO: 计算两个变换之间的误差公式应该是有问题的, 即使匹配上输出的结果还是很差

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 <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <boost/random.hpp>
#include <chrono>
#include <thread>
#include <filesystem>// 生成随机扰动(方便跳出循环)
Eigen::Matrix4f generateAnnealingTransformation(double temperature) {unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();boost::random::mt19937 gen(seed);double tras_scale = 0.15;double rot_scale = 0.17 * M_PI;boost::random::uniform_real_distribution<> rand_dis(0, 1);boost::random::uniform_real_distribution<> dis(-tras_scale * temperature, tras_scale * temperature);boost::random::uniform_real_distribution<> angle_dis(-rot_scale * temperature, rot_scale * temperature);Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();transform(0, 3) = dis(gen); // X-axis translation disturbancetransform(1, 3) = dis(gen); // Y-axis translation disturbancetransform(2, 3) = dis(gen); // Z-axis translation disturbance// Rotation disturbance around Z-axisfloat angleZ = angle_dis(gen);Eigen::Matrix4f rotZ = Eigen::Matrix4f::Identity();rotZ(0, 0) = cos(angleZ);rotZ(0, 1) = -sin(angleZ);rotZ(1, 0) = sin(angleZ);rotZ(1, 1) = cos(angleZ);// Rotation disturbance around Y-axisfloat angleY = angle_dis(gen);Eigen::Matrix4f rotY = Eigen::Matrix4f::Identity();rotY(0, 0) = cos(angleY);rotY(0, 2) = sin(angleY);rotY(2, 0) = -sin(angleY);rotY(2, 2) = cos(angleY);// Rotation disturbance around X-axisfloat angleX = angle_dis(gen);Eigen::Matrix4f rotX = Eigen::Matrix4f::Identity();rotX(1, 1) = cos(angleX);rotX(1, 2) = -sin(angleX);rotX(2, 1) = sin(angleX);rotX(2, 2) = cos(angleX);// Combine the transformationstransform = transform * rotZ * rotY * rotX;return transform;
}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(-200, 200);  // 位移范围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轴平移// Rotation disturbance around Z-axisfloat angleZ = angle_dis(gen);Eigen::Matrix4f rotZ = Eigen::Matrix4f::Identity();rotZ(0, 0) = cos(angleZ);rotZ(0, 1) = -sin(angleZ);rotZ(1, 0) = sin(angleZ);rotZ(1, 1) = cos(angleZ);// Rotation disturbance around Y-axisfloat angleY = angle_dis(gen);Eigen::Matrix4f rotY = Eigen::Matrix4f::Identity();rotY(0, 0) = cos(angleY);rotY(0, 2) = sin(angleY);rotY(2, 0) = -sin(angleY);rotY(2, 2) = cos(angleY);// Rotation disturbance around X-axisfloat angleX = angle_dis(gen);Eigen::Matrix4f rotX = Eigen::Matrix4f::Identity();rotX(1, 1) = cos(angleX);rotX(1, 2) = -sin(angleX);rotX(2, 1) = sin(angleX);rotX(2, 2) = cos(angleX);// Combine the transformationstransform = transform * rotZ * rotY * rotX;return transform;
}// 函数：随机选择一个.pcd文件
std::string selectRandomPCDFile(const std::string& directory) {std::vector<std::string> file_names;for (const auto& entry : std::filesystem::directory_iterator(directory)) {if (entry.path().extension() == ".pcd") {file_names.push_back(entry.path().filename().string());}}// 如果没有找到任何文件，则返回空字符串if (file_names.empty()) {return "";}// 随机选择一个文件srand(static_cast<unsigned int>(time(NULL)));  // 初始化随机数生成器std::string selected_file = file_names[rand() % file_names.size()];// 返回完整的文件路径return directory + selected_file;
}void saveTransformation(const Eigen::Matrix4f &transform, const std::string &filename) {std::ofstream file(filename);if (file.is_open()) {file << transform;file.close();}
}struct TransformationError {float translationError;Eigen::Vector3f rotationError; // 存储绕X轴、Y轴和Z轴的旋转误差
};// 重载 << 运算符以打印 TransformationError
std::ostream& operator<<(std::ostream& os, const TransformationError& error) {os << "Translation Error: " << error.translationError << ", "<< "Rotation Error: [" << error.rotationError.transpose() << "]";return os;
}// 示例：计算两个变换矩阵之间的误差
TransformationError CalculateTransformationError(const Eigen::Matrix4f &matrix1, const Eigen::Matrix4f &matrix2) {TransformationError error;// 计算平移误差Eigen::Vector3f translation1 = matrix1.block<3,1>(0, 3);Eigen::Vector3f translation2 = matrix2.block<3,1>(0, 3);error.translationError = (translation2 - translation1).norm();// 计算旋转误差Eigen::Quaternionf quaternion1(matrix1.block<3,3>(0,0));Eigen::Quaternionf quaternion2(matrix2.block<3,3>(0,0));Eigen::Quaternionf deltaQuaternion = quaternion1.inverse() * quaternion2;Eigen::Vector3f deltaEulerAngles = deltaQuaternion.toRotationMatrix().eulerAngles(0, 1, 2); // X, Y, Zerror.rotationError = deltaEulerAngles.cwiseAbs(); // 绝对值误差return error;
}
Eigen::Matrix4f readMatrixFromFile(const std::string& filepath) {std::ifstream file(filepath);Eigen::Matrix4f matrix;if (file.is_open()) {for (int i = 0; i < 4; ++i) {for (int j = 0; j < 4; ++j) {if (!(file >> matrix(i, j))) {throw std::runtime_error("文件格式错误或数据不足以填充矩阵");}}}file.close();} else {throw std::runtime_error("无法打开文件: " + filepath);}return matrix;
}int main() {// 配置退火时的随机数种子srand(static_cast<unsigned int>(std::chrono::system_clock::now().time_since_epoch().count()));std::string directory = "/home/smile/Desktop/github/src/pointcloud-processing-visualization/pcd/";// 使用函数选择一个随机文件std::string file_to_load = selectRandomPCDFile(directory);// 加载点云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ>(file_to_load, *cloud_in) == -1) {PCL_ERROR("Couldn't read file\n");return -1;}// 移除NaN值std::vector<int> indices;pcl::removeNaNFromPointCloud(*cloud_in, *cloud_in, indices);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);// 进行体素滤波pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;voxel_grid.setInputCloud(cloud_in);voxel_grid.setLeafSize(0.07f, 0.07f, 0.07f);  // 设置体素大小voxel_grid.filter(*cloud_filtered);// 模拟退火参数double temperature = 5.2; // 初始温度double coolingRate = 0.985; // 冷却率// 全局变量double last_fitness_score = std::numeric_limits<double>::max(); // 初始设置为最大值// 生成变换并保存到文件Eigen::Matrix4f base_transformation = generateRandomTransformation();Eigen::Matrix4f base_transformation_normal = base_transformation;std::cout << "Base Transformation Matrix:\n" << base_transformation << std::endl;saveTransformation(base_transformation, "/home/smile/Desktop/github/src/pointcloud-processing-visualization/saicp/result.txt");// 应用初始变换pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);pcl::transformPointCloud(*cloud_filtered, *cloud_transformed, base_transformation);// 设置SAICP实例pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> saicp, normal_icp;saicp.setInputSource(cloud_transformed);saicp.setInputTarget(cloud_filtered);saicp.setMaximumIterations(1); // 每次调用align时执行一次迭代normal_icp.setInputSource(cloud_transformed);normal_icp.setInputTarget(cloud_filtered);normal_icp.setMaximumIterations(1);// 初始化可视化pcl::visualization::PCLVisualizer viewer("SAICP 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_saicp(new pcl::PointCloud<pcl::PointXYZ>(*cloud_transformed));pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp_normal(new pcl::PointCloud<pcl::PointXYZ>(*cloud_transformed));viewer.addPointCloud<pcl::PointXYZ>(cloud_saicp, "cloud_saicp");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_saicp");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "cloud_saicp");viewer.addPointCloud<pcl::PointXYZ>(cloud_icp_normal, "cloud_icp_normal");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_icp_normal");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 1.0, "cloud_icp_normal");viewer.addCoordinateSystem(1.0);viewer.initCameraParameters();// 创建初始变换的矩阵, 先验位姿的是一个单位阵, 即无先验位姿Eigen::Matrix4f saicp_result = Eigen::Matrix4f::Identity();Eigen::Matrix4f normal_icp_result = Eigen::Matrix4f::Identity();// 计数器int saicp_cnt = 1; // icp迭代次数int normal_icp_cnt = 1; // 先验icp迭代次数bool saicp_fitness_reached = false; bool normal_icp_fitness_reached = false;int iteration_counter = 0;  // 迭代频率计数器, 迭代的频率按照 10ms x iteration_counter 可以在下面的循环中修改int stop_iteration_cnt = 0;bool has_published = false;int bad_value_accetp_cnt = 0;while (!viewer.wasStopped()) {viewer.spinOnce();  // 图像化界面刷新频率10ms, 方便使用鼠标进行控制视角 std::this_thread::sleep_for(std::chrono::milliseconds(10));// 如果都完成了收敛, 则不再更新if((saicp_fitness_reached && normal_icp_fitness_reached) || (++stop_iteration_cnt >= 2000)) {if(!has_published){double icp_score = saicp.getFitnessScore();double icp_normal_score = normal_icp.getFitnessScore();std::cout << "SAICP迭代次数: " << saicp_cnt << " SAICP分数: " << icp_score <<std::endl;std::cout << "普通ICP迭代次数: " << normal_icp_cnt << " 普通ICP分数: " << icp_normal_score <<std::endl;std::cout << "迭代次数比率: " << (saicp_cnt-normal_icp_cnt)/normal_icp_cnt <<std::endl;std::cout << "分数差比率: " << std::abs((icp_score-icp_normal_score))/icp_normal_score <<std::endl;std::cout << "差值接收率: " << double(bad_value_accetp_cnt)/double(saicp_cnt) <<std::endl;std::cout << "[SAICP]变换矩阵 " << std::endl;std::cout << saicp.getFinalTransformation() << std::endl;std::cout << "[SAICP]误差 " << std::endl;Eigen::Matrix4f result = readMatrixFromFile("/home/smile/Desktop/github/src/pointcloud-processing-visualization/saicp/result.txt");std::cout << CalculateTransformationError(saicp.getFinalTransformation(),result) <<std::endl;std::cout << "-----------------------------------------------------------" << std::endl;std::cout << "[SAICP]变换矩阵 " <<std::endl;std::cout << normal_icp.getFinalTransformation() << std::endl;std::cout << "[SAICP]误差 " << std::endl;std::cout << CalculateTransformationError(normal_icp.getFinalTransformation(),result) <<std::endl;std::cout << "-----------------------------------------------------------" << std::endl;has_published = true;}continue;}// 创建icp之后的新点云pcl::PointCloud<pcl::PointXYZ>::Ptr saicp_cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr normal_icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);// 每10ms x 100 = 1000ms = 1s 即每1秒做一次icp并更新点云if (++iteration_counter >= 2) {// 如果没有达到0.0001的分值, 则icp继续迭代if(!saicp_fitness_reached){// 在原有变换的基础上添加模拟退火的随机扰动Eigen::Matrix4f annealing_transform = generateAnnealingTransformation(temperature);Eigen::Matrix4f perturbed_transformation = saicp_result * annealing_transform;// 应用带有扰动的变换进行ICP迭代saicp.align(*saicp_cloud, perturbed_transformation);Eigen::Matrix4f new_icp_result = saicp.getFinalTransformation();// 检查是否收敛(肯定收敛, 因为最多迭代1次,所以每一次都会收敛)if (saicp.hasConverged()) {// 退火double new_fitness_score = saicp.getFitnessScore();if (new_fitness_score < last_fitness_score) {saicp_result = new_icp_result; // 接受更好的变换last_fitness_score = new_fitness_score; // 更新最新的fitness score} else if (exp((-(new_fitness_score - last_fitness_score)) / temperature) > ((double)rand() / RAND_MAX)){bad_value_accetp_cnt++;	saicp_result = perturbed_transformation; // 以一定概率接受较差的变换last_fitness_score = new_fitness_score; // 更新fitness score，即使它变差了}// 更新温度temperature *= coolingRate;// std::cout << "======================================================="<<std::endl;// std::cout << "当前温度: " << temperature <<std::endl;// std::cout << "======================================================="<<std::endl;double fitness_score = saicp.getFitnessScore();if(saicp_fitness_reached) saicp_cnt=saicp_cnt;else saicp_cnt += 1;// std::cout << "[ICP] 分数为 " << fitness_score <<std::endl;// 获取最新一次的变换, 并将该变换应用到带先验的点云上, 更新该点云base_transformation = saicp.getFinalTransformation().cast<float>();pcl::transformPointCloud(*cloud_transformed, *saicp_cloud, base_transformation);viewer.updatePointCloud<pcl::PointXYZ>(saicp_cloud, "cloud_saicp");//真正的停止条件(收敛条件)if(fitness_score<=0.001){saicp_fitness_reached = true;std::cout << "======================================================="<<std::endl;std::cout << "[SAICP]完成收敛 " <<std::endl;std::cout << "[SAICP]迭代次数为 " << saicp_cnt <<std::endl;std::cout << "[SAICP]变换矩阵 " << std::endl;std::cout << saicp.getFinalTransformation() << std::endl;std::cout << "======================================================="<<std::endl;}     }}   // 普通icp    if(!normal_icp_fitness_reached){normal_icp.align(*normal_icp_cloud, normal_icp_result);normal_icp_result = normal_icp.getFinalTransformation();// 同理, 这里并不是真正的停止条件if (normal_icp.hasConverged()) {double fitness_score_normal = normal_icp.getFitnessScore();if(normal_icp_fitness_reached) normal_icp_cnt = normal_icp_cnt;else normal_icp_cnt += 1;// std::cout << "[普通ICP] 分数为 " << fitness_score_normal <<std::endl;// 带先验的停止条件也是0.0001分以下终止if(fitness_score_normal<=0.001){normal_icp_fitness_reached = true;std::cout << "======================================================="<<std::endl;std::cout << "[普通ICP]完成收敛 " <<std::endl;std::cout << "[普通ICP]迭代次数为 " << normal_icp_cnt <<std::endl;std::cout << "[普通ICP]变换矩阵 " <<std::endl;std::cout << normal_icp.getFinalTransformation() << std::endl;std::cout << "======================================================="<<std::endl;}// 获取最新一次的变换, 并将该变换应用到带先验的点云上, 更新该点云base_transformation_normal = normal_icp.getFinalTransformation().cast<float>();pcl::transformPointCloud(*cloud_transformed, *normal_icp_cloud, base_transformation_normal);viewer.updatePointCloud<pcl::PointXYZ>(normal_icp_cloud, "cloud_icp_normal"); }}// 重置迭代计数器iteration_counter = 0;}}
return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.0 FATAL_ERROR)
project(saicp_example)# 设置 C++ 标准为 C++17
set(CMAKE_CXX_STANDARD 17)
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(saicp_example main.cpp)# 链接 PCL 库和可能需要的 stdc++fs 库
target_link_libraries(saicp_example ${PCL_LIBRARIES})