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 disturbance
transform(1, 3) = dis(gen); // Y-axis translation disturbance
transform(2, 3) = dis(gen); // Z-axis translation disturbance
// Rotation disturbance around Z-axis
float 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-axis
float 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-axis
float 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 transformations
transform = 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 R 1 − R 0 T > r a n d ( ) e^\frac{R_1-R_0}{T} > rand() eTR1−R0>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
// std::cout << "当前温度: " << temperature <
// std::cout << "======================================================="<
double fitness_score = saicp.getFitnessScore();
if(saicp_fitness_reached) saicp_cnt=saicp_cnt;
else saicp_cnt += 1;
// std::cout << "[ICP] 分数为 " << fitness_score <
// 获取最新一次的变换, 并将该变换应用到带先验的点云上, 更新该点云
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 <
// 带先验的停止条件也是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})