【磕盐随记】关于位姿估计中的左导和右导，局部扰与全局扰动

最新推荐文章于 2024-09-20 15:09:03 发布

全日制学生混

最新推荐文章于 2024-09-20 15:09:03 发布

阅读量337

点赞数

文章标签：算法

本文链接：https://blog.csdn.net/LNSTOP/article/details/132205971

版权

文章讲述了作者在使用点面ICP算法处理点云数据时遇到的问题，包括扰动选择的坐标系问题、左导数与右导数的区别以及正确使用Eigen::Affine3d和Eigen::Isometry3d。作者强调了在优化过程中的坐标转换和求导策略对于算法性能的影响。

摘要由CSDN通过智能技术生成

昨天写了一个点面ICP的小程序，用于评估退化性。我自己加一个扰动当作先验状态x0,R0，然后试图让扰动后的点云帧与地图匹配，以恢复扰动前的位置。原理很简单，只需要用高斯牛顿法求解，然后每次迭代重新搜索关联匹配就行了。但是，在这一过程中，我发现当我只扰动x0时，程序能收敛的很好，只要扰动R0不是单位阵，就会匹配不上。和泽哥debug到半夜，在程序中发现如下误区：

误区1：获得初值时，对scan的扰动最好是基于body系，不然扰动的R0是在世界系下的，很小的R0可能会对scan的位置和姿态都产生很大的扰动。所以，我们采取的方法是，先把scan从World->Lidar，然后再将Lidar->disterb，然后再将点云直接转移到Lidar->world（这里还挺绕的）

sensor_msgs::PointCloud2 scan_pc_msg;
pcl::toROSMsg(*laserScan, scan_pc_msg);
scan_pc_msg.header.stamp = ros::Time().now();
scan_pc_msg.header.frame_id = "world";
pub_surround_v1.publish(scan_pc_msg);
std::this_thread::sleep_for(std::chrono::milliseconds(100));   

// perturbation under Lidar frame
Eigen::Affine3d transform_L2W = Eigen::Affine3d::Identity();
transform_L2W.rotate(_q.matrix());
transform_L2W.translation() = transition;
// 1) trans from W -> L
pcl::PointCloud<PointType> laserScan_purtubation;
pcl::transformPointCloud(*laserScan, laserScan_purtubation, transform_L2W.inverse());   // W->L


pcl::PointCloud<PointType> laserScan_Lidar;
pcl::transformPointCloud(laserScan_purtubation, laserScan_Lidar, Eigen::Affine3d::Identity()); // keep perturbated pc, for vis

pcl::toROSMsg(laserScan_purtubation, scan_pc_msg);
scan_pc_msg.header.stamp = ros::Time().now();
scan_pc_msg.header.frame_id = "world";
pub_surround_v1.publish(scan_pc_msg);
std::this_thread::sleep_for(std::chrono::milliseconds(100));   

// 2) perturbation 
Eigen::Vector3d pertubation_t_inL(1, 0, 0.0); // t扰动量
Eigen::Matrix3d pertubation_R_inL = rpy_to_rot_mat(0,5,5); // R扰动量
Eigen::Affine3d transform_L2d = Eigen::Affine3d::Identity();
transform_L2d.translation() = pertubation_t_inL;
transform_L2d.rotate(pertubation_R_inL);
pcl::transformPointCloud(laserScan_purtubation, laserScan_purtubation, transform_L2d); // L->d

pcl::toROSMsg(laserScan_purtubation, scan_pc_msg);
scan_pc_msg.header.stamp = ros::Time().now();
scan_pc_msg.header.frame_id = "world";
pub_surround_v1.publish(scan_pc_msg);
std::this_thread::sleep_for(std::chrono::milliseconds(100));    

// 3) trans from L -> W
pcl::transformPointCloud(laserScan_purtubation, laserScan_purtubation, transform_L2W); // L->W

// 4）trans & _q under world frame, after perturbation under Lidar frame
Eigen::Affine3d transform_d2W =transform_L2W*transform_L2d.inverse();
transition = transform_d2W.translation();
_q = Eigen::Quaterniond(transform_d2W.rotation());

pcl::toROSMsg(laserScan_purtubation, scan_pc_msg);
scan_pc_msg.header.stamp = ros::Time().now();
scan_pc_msg.header.frame_id = "world";
pub_surround_v1.publish(scan_pc_msg);
std::this_thread::sleep_for(std::chrono::milliseconds(100));

误区2：对R求导更新时，选择左导数还是右导数

首先应当明确的是，扰动求导法的左导数和右导数的形式一般时不一样的（显然）。这是因为虽然都是求delta_phi，但是意义不同，左扰动是对R左乘小量，代表了世界坐标系下的扰动，小扰动也可能对应大的变化，也就是说，左绕动对观测误差的影响还会受到当前观测在世界坐标系下的位置的影响，这显然是我们不想要的。我们其实想要的是，在当前的姿态下，旋转一个小扰动，这是与世界系下的位置无关的，我们想要的导数也是这个（这个比较平缓线性，更适合优化位姿），因此是右乘。说白了就一句话，左绕是世界扰，右绕是自身扰，左绕求导左更新，右绕求导右更新。另外，需要说一下的是，优化求出的delta_x是一个六维的量，前三位是delta_xyz，后三维是delta_phi（李代数/角轴），需要注意的是，在更新时，应该直接pose=pose+delta_xyz, R=R*deltaR或R=deltaR*R。而不是组成一个SE(3)，原因是我们求导的时候就是位置和姿态就是分开求的，所以更新的时候，也应该是各自更新各自的，这里千万不能理解成一个系转到另一个系的欧氏变换。

误区3：对Eigen::Affine3d的误用

Affine不是SE3，还是用Eigen::Isometry3d好。在pcl::transformPointCloud时，可以将Isometry3d通过affine()方法转换为Affine3d

// Eigen::Affine3d transform_L2W = Eigen::Affine3d::Identity();
Eigen::Isometry3d transform_L2W;
transform_L2W.rotate(_q.matrix());
transform_L2W.translation() = transition;

pcl::PointCloud<PointType> laserScan_purtubation;
pcl::transformPointCloud(*laserScan, laserScan_purtubation, transform_L2W.affine().inverse());   // W->L

// 虽然称为3d，实质上是4＊4的矩阵，齐次坐标
Eigen::Isometry3d Tc1w = Eigen::Isometry3d::Identity();
 
// 按照rotation_matrix进行旋转
Tc1w.rotate(rotation_matrix);
 
// 把平移向量设成t
Tc1w.pretranslate(t);                                                  
 
// 获取旋转矩阵
Eigen::Matrix3d rotation = Tc1w.rotation();
 
// 获取平移向量
Eigen::Vector3d position = Tc1w.translation();

注意：

使用前一定要初始化，否则后面的设置无法生效；

函数pretranslate(p)就是把平移t设置到位姿矩阵之中，即实现的是t = p

函数translate(t)先用把位姿用到到t上，再把得到的结果设置到位姿矩阵之中，即实现的是t = Rp + t

此外，这里还有很多需要注意的地方，参考链接：Eigen::Isometry3d的用法（pretranslate、translate、prerotate、rotate的区别）_star-keke的博客-CSDN博客