网站开发 项目接单装饰工程公司经营范围

当前位置： 首页 > news >正文

网站开发 项目接单,装饰工程公司经营范围,公司注册咨询电话,做外贸网站好的公司SAICP(模拟退火迭代最近点)的实现 注: 本系列所有文章在github开源, 也是我个人的学习笔记, 欢迎大家去star以及fork, 感谢! 仓库地址: pointcloud-processing-visualization 总结一下上周的学习情况 ICP会存在局部最小值的问题, 这个问题可能即使是没有实际遇到过, 也或多…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 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 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::vectorstd::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_castunsigned 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.block3,1(0, 3);Eigen::Vector3f translation2 matrix2.block3,1(0, 3);error.translationError (translation2 - translation1).norm();// 计算旋转误差Eigen::Quaternionf quaternion1(matrix1.block3,3(0,0));Eigen::Quaternionf quaternion2(matrix2.block3,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_castunsigned 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::PointCloudpcl::PointXYZ::Ptr cloud_in(new pcl::PointCloudpcl::PointXYZ);if (pcl::io::loadPCDFilepcl::PointXYZ(file_to_load, *cloud_in) -1) {PCL_ERROR(Couldnt read file\n);return -1;}// 移除NaN值std::vectorint indices;pcl::removeNaNFromPointCloud(*cloud_in, *cloud_in, indices);pcl::PointCloudpcl::PointXYZ::Ptr cloud_filtered(new pcl::PointCloudpcl::PointXYZ);// 进行体素滤波pcl::VoxelGridpcl::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_limitsdouble::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::PointCloudpcl::PointXYZ::Ptr cloud_transformed(new pcl::PointCloudpcl::PointXYZ);pcl::transformPointCloud(*cloud_filtered, *cloud_transformed, base_transformation);// 设置SAICP实例pcl::IterativeClosestPointpcl::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.addPointCloudpcl::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::PointCloudpcl::PointXYZ::Ptr cloud_saicp(new pcl::PointCloudpcl::PointXYZ(*cloud_transformed));pcl::PointCloudpcl::PointXYZ::Ptr cloud_icp_normal(new pcl::PointCloudpcl::PointXYZ(*cloud_transformed));viewer.addPointCloudpcl::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.addPointCloudpcl::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::PointCloudpcl::PointXYZ::Ptr saicp_cloud(new pcl::PointCloudpcl::PointXYZ);pcl::PointCloudpcl::PointXYZ::Ptr normal_icp_cloud(new pcl::PointCloudpcl::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_cntsaicp_cnt;else saicp_cnt 1;// std::cout [ICP] 分数为 fitness_score std::endl;// 获取最新一次的变换, 并将该变换应用到带先验的点云上, 更新该点云base_transformation saicp.getFinalTransformation().castfloat();pcl::transformPointCloud(*cloud_transformed, *saicp_cloud, base_transformation);viewer.updatePointCloudpcl::PointXYZ(saicp_cloud, cloud_saicp);//真正的停止条件(收敛条件)if(fitness_score0.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_normal0.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().castfloat();pcl::transformPointCloud(*cloud_transformed, *normal_icp_cloud, base_transformation_normal);viewer.updatePointCloudpcl::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 标准为 C17 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 库和可能需要的 stdcfs 库 target_link_libraries(saicp_example \){PCL_LIBRARIES})