银川网站建设,网站怎么做图片轮播,wordpress文章如何搬家,企业网上的推广文章目录 引言外参标定原理ICP匹配示例参考文献 引言
多激光雷达系统通常用于自动驾驶或机器人#xff0c;每个雷达的位置和姿态不同#xff0c;需要将它们的数据统一到同一个坐标系下。多激光雷达外参标定的核心目标是通过计算不同雷达坐标系之间的刚性变换关系#xff08… 文章目录 引言外参标定原理ICP匹配示例参考文献 引言
多激光雷达系统通常用于自动驾驶或机器人每个雷达的位置和姿态不同需要将它们的数据统一到同一个坐标系下。多激光雷达外参标定的核心目标是通过计算不同雷达坐标系之间的刚性变换关系旋转矩阵 R R R 和平移向量 t t t将多个雷达的点云数据统一到同一坐标系下。具体需求包括
数据融合消除多雷达间的位姿差异生成全局一致的点云。减少累积误差避免多传感器数据因坐标系不统一导致的定位与建图误差。提升感知精度为自动驾驶或机器人提供更可靠的环境感知能力。
外参标定原理
外参标定本质是求解两个坐标系之间的最优变换参数设雷达A的坐标系为源坐标系雷达B为目标坐标系。对于同一物理点 P P P其在两个坐标系下的坐标分别为 P A P_A PA 和 P B P_B PB满足 P B R ⋅ P A t P_B R \cdot P_A t PBR⋅PAt 其中 R ∈ S O ( 3 ) R \in SO(3) R∈SO(3) 为旋转矩阵 t ∈ R 3 t \in \mathbb{R}^3 t∈R3 为平移向量。
一般来说多激光雷达的主流的外参标定方法有手动标定调参法、自动标定法、基于车辆运动轨迹的标定法。
手动标定法 使用已知几何形状的标定物如立方体、棋盘格通过人工测量或标定物特征点计算外参。优点精度高适合实验室环境。 缺点依赖标定物效率低。 基本步骤是 将标定物放置在雷达共同视场内提取标定物的角点或平面基于最小二乘法求解 R R R 和 t t t 自动标定法: 基本方式是利用环境中的稳定特征如地面、建筑物边缘自动对齐点云优点在于无需标定物、适应性较强但易受动态物体干扰且要求场景特征丰富。 多采用 ICPIterative Closest Point、NDTNormal Distributions Transform 求解 R R R 和 t t t。 ICP 通过迭代最近点匹配最小化点对距离 min R , t ∑ i 1 N ∥ ( R ⋅ P A , i t ) − P B , i ∥ 2 \min_{R,t} \sum_{i1}^N \| (R \cdot P_{A,i} t) - P_{B,i} \|^2 R,tmini1∑N∥(R⋅PA,it)−PB,i∥2 NDTNormal Distributions Transform 则将点云转换为概率密度函数通过优化概率分布相似性求解变换。 基于运动轨迹的标定: 利用雷达在运动过程中采集的数据通过里程计或SLAM生成轨迹约束。一般是联合优化多个雷达的外参和运动轨迹再使用因子图优化Factor Graph Optimization等方式进行优化求解雷达之间的运动轨迹所匹配的 R R R 和 t t t。 这种方式适合动态环境却可在线标定但计算复杂度高需高精度里程计支持。
注实际工程落地时可不用追求高难度的优化算法可根据具体场景选择合适的方式来进行标定
ICP匹配示例
在这篇文章中我们可结合手动标定法与自动标定法联调方式来确保标定精度简单操作是假设我们通过手动标定方式初步得到基准激光雷达与附属激光雷达的相对坐标变换参数 R 0 R_0 R0 和 t 0 t_0 t0 在此基础上采用 G I C P GICP GICP、 I C P ICP ICP等基础算法进行多次配准提高标定精度得到最终的 R R R 和 t t t
这里展示一个 I C P ICP ICP的使用示例: 按 空格 即可观察迭代匹配的效果。
#include pcl/console/time.h
#include pcl/io/ply_io.h
#include pcl/point_types.h
#include pcl/registration/icp.h
#include pcl/visualization/pcl_visualizer.h#include stringtypedef pcl::PointXYZ PointT;
typedef pcl::PointCloudPointT PointCloudT;bool next_iteration false;void print4x4Matrix(const Eigen::Matrix4d matrix) {printf(Rotation matrix :\n);printf( | %6.3f %6.3f %6.3f | \n, matrix(0, 0), matrix(0, 1),matrix(0, 2));printf(R | %6.3f %6.3f %6.3f | \n, matrix(1, 0), matrix(1, 1),matrix(1, 2));printf( | %6.3f %6.3f %6.3f | \n, matrix(2, 0), matrix(2, 1),matrix(2, 2));printf(Translation vector :\n);printf(t %6.3f, %6.3f, %6.3f \n\n, matrix(0, 3), matrix(1, 3),matrix(2, 3));
}
/*** 此函数是查看器的回调。 当查看器窗口位于顶部时只要按任意键就会调用此函数。* 如果碰到“空格” 将布尔值设置为true。* param event* param nothing*/
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent event,void* nothing) {if (event.getKeySym() space event.keyDown()) next_iteration true;
}int main(int argc, char* argv[]) {// The point clouds we will be usingPointCloudT::Ptr cloud_in(new PointCloudT); // Original point cloudPointCloudT::Ptr cloud_tr(new PointCloudT); // Transformed point cloudPointCloudT::Ptr cloud_icp(new PointCloudT); // ICP output point cloud// 我们检查程序的参数设置初始ICP迭代的次数然后尝试加载PLY文件。// Checking program argumentsif (argc 2) {printf(Usage :\n);printf(\t\t%s file.ply number_of_ICP_iterations\n, argv[0]);PCL_ERROR(Provide one ply file.\n);return (-1);}int iterations 1; // Default number of ICP iterationsif (argc 2) {// If the user passed the number of iteration as an argumentiterations atoi(argv[2]);if (iterations 1) {PCL_ERROR(Number of initial iterations must be 1\n);return (-1);}}pcl::console::TicToc time;time.tic();if (pcl::io::loadPLYFile(argv[1], *cloud_in) 0) {PCL_ERROR(Error loading cloud %s.\n, argv[1]);return (-1);}std::cout \nLoaded file argv[1] ( cloud_in-size() points) in time.toc() ms\n std::endl;// 我们使用刚性矩阵变换来变换原始点云。// cloud_in包含原始点云。// cloud_tr和cloud_icp包含平移/旋转的点云。// cloud_tr是我们将用于显示的备份绿点云。// Defining a rotation matrix and translation vectorEigen::Matrix4d transformation_matrix Eigen::Matrix4d::Identity();// A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)double theta M_PI / 8; // The angle of rotation in radianstransformation_matrix(0, 0) std::cos(theta);transformation_matrix(0, 1) -sin(theta);transformation_matrix(1, 0) sin(theta);transformation_matrix(1, 1) std::cos(theta);// A translation on Z axis (0.4 meters)transformation_matrix(2, 3) 0.4;// Display in terminal the transformation matrixstd::cout Applying this rigid transformation to: cloud_in - cloud_icp std::endl;print4x4Matrix(transformation_matrix);// Executing the transformationpcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);*cloud_tr *cloud_icp; // We backup cloud_icp into cloud_tr for later use// 这是ICP对象的创建。 我们设置ICP算法的参数。// setMaximumIterationsiterations设置要执行的初始迭代次数默认值为1。// 然后我们将点云转换为cloud_icp。// 第一次对齐后我们将在下一次使用该ICP对象时当用户按下“空格”时将ICP最大迭代次数设置为1。// The Iterative Closest Point algorithmtime.tic();pcl::IterativeClosestPointPointT, PointT icp;icp.setMaximumIterations(iterations);icp.setInputSource(cloud_icp);icp.setInputTarget(cloud_in);icp.align(*cloud_icp);icp.setMaximumIterations(1); // We set this variable to 1 for the next time// we will call .align () functionstd::cout Applied iterations ICP iteration(s) in time.toc() ms std::endl;// 检查ICP算法是否收敛 否则退出程序。// 如果返回true我们将转换矩阵存储在4x4矩阵中然后打印刚性矩阵转换。if (icp.hasConverged()) {// std::cout \nICP has converged, score is icp.getFitnessScore()// std::endl;// std::cout \nICP transformation iterations// : cloud_icp - cloud_in std::endl;transformation_matrix icp.getFinalTransformation().castdouble();print4x4Matrix(transformation_matrix);} else {PCL_ERROR(\nICP has not converged.\n);return (-1);}// Visualizationpcl::visualization::PCLVisualizer viewer(ICP demo);// Create two vertically separated viewportsint v1(0);int v2(1);viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);// The color we will be usingfloat bckgr_gray_level 0.0; // Blackfloat txt_gray_lvl 1.0 - bckgr_gray_level;// Original point cloud is whitepcl::visualization::PointCloudColorHandlerCustomPointT cloud_in_color_h(cloud_in, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl,(int)255 * txt_gray_lvl);viewer.addPointCloud(cloud_in, cloud_in_color_h, cloud_in_v1, v1);viewer.addPointCloud(cloud_in, cloud_in_color_h, cloud_in_v2, v2);// Transformed point cloud is greenpcl::visualization::PointCloudColorHandlerCustomPointT cloud_tr_color_h(cloud_tr, 20, 180, 20);viewer.addPointCloud(cloud_tr, cloud_tr_color_h, cloud_tr_v1, v1);// ICP aligned point cloud is redpcl::visualization::PointCloudColorHandlerCustomPointT cloud_icp_color_h(cloud_icp, 180, 20, 20);viewer.addPointCloud(cloud_icp, cloud_icp_color_h, cloud_icp_v2, v2);// Adding text descriptions in each viewportviewer.addText(White: Original point cloud\nGreen: Matrix transformed point cloud, 10,15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, icp_info_1, v1);viewer.addText(White: Original point cloud\nRed: ICP aligned point cloud,10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl,icp_info_2, v2);std::stringstream ss;ss iterations;std::string iterations_cnt ICP iterations ss.str();viewer.addText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl,txt_gray_lvl, iterations_cnt, v2);// Set background colorviewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level,bckgr_gray_level, v1);viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level,bckgr_gray_level, v2);// Set camera position and orientationviewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947,-0.256907, 0);viewer.setSize(1280, 1024); // Visualiser window size// Register keyboard callback :viewer.registerKeyboardCallback(keyboardEventOccurred, (void*)NULL);// Display the visualiserwhile (!viewer.wasStopped()) {viewer.spinOnce();// The user pressed space :if (next_iteration) {// The Iterative Closest Point algorithmtime.tic();// 如果用户按下键盘上的任意键则会调用keyboardEventOccurred函数。// 此功能检查键是否为“空格”。// 如果是则全局布尔值next_iteration设置为true从而允许查看器循环输入代码的下一部分调用ICP对象以进行对齐。// 记住我们已经配置了该对象输入/输出云并且之前通过setMaximumIterations将最大迭代次数设置为1。icp.align(*cloud_icp);std::cout Applied 1 ICP iteration in time.toc() ms std::endl;// 和以前一样我们检查ICP是否收敛如果不收敛则退出程序。if (icp.hasConverged()) {// printf“ 033 [11A”;// 在终端增加11行以覆盖显示的最后一个矩阵是一个小技巧。// 简而言之它允许替换文本而不是编写新行 使输出更具可读性。// 我们增加迭代次数以更新可视化器中的文本值。printf(\033[11A); // Go up 11 lines in terminal output.printf(\nICP has converged, score is %.0e\n, icp.getFitnessScore());// 这意味着如果您已经完成了10次迭代则此函数返回矩阵以将点云从迭代10转换为11。std::cout \nICP transformation iterations : cloud_icp - cloud_in std::endl;// 函数getFinalTransformation返回在迭代过程中完成的刚性矩阵转换此处为1次迭代。transformation_matrix *icp.getFinalTransformation().castdouble(); // WARNING /!\ This is not accurate! For// educational purpose only!print4x4Matrix(transformation_matrix); // Print the transformation between// original pose and current posess.str();ss iterations;std::string iterations_cnt ICP iterations ss.str();viewer.updateText(iterations_cnt, 10, 60, 16, txt_gray_lvl,txt_gray_lvl, txt_gray_lvl, iterations_cnt);viewer.updatePointCloud(cloud_icp, cloud_icp_color_h, cloud_icp_v2);} else {PCL_ERROR(\nICP has not converged.\n);return (-1);}//这不是我们想要的。//如果我们将最后一个矩阵与新矩阵相乘那么结果就是从开始到当前迭代的转换矩阵。}next_iteration false;}return (0);
}参考文献
[1] 姜聿于——自动驾驶感知【激光雷达】一、标定
[2] Segal A , Hhnel D , Thrun S .Generalized-ICP[J]. 2009.DOI:10.15607/RSS.2009.V.021.
[3] Kulmer D , Tahiraj I , Chumak A ,et al.Multi-LiCa: A Motion and Targetless Multi LiDAR-to-LiDAR Calibration Framework[J].IEEE, 2025.DOI:10.1109/MFI62651.2024.10705773.
…
