lio-sam中雅克比推导

lio-sam做的是scan-map，点变到世界系下，优化本帧在是世界系下的位姿，和loam有所不同。
符号：
本帧特征点云（相对机体系）PscanlidarP_{scan}^{lidar}Pscanlidar ，本帧点云变换至世界系PscanP_{scan}Pscan，局部地图中匹配的点云 PmapP_{map}Pmap（世界系），位姿X={R,T}X=\{R,T\}X={R,T}，点到对应线、面的距离残差f(X)f(X)f(X)，围绕机体xxx轴旋转的角度为rxrxrx，sin(rx)、cos(rx)sin(rx)、cos(rx)sin(rx)、cos(rx)记作sx、cxs_x、c_xsx、cx，其他轴同理。

坐标变换关系：

待优化的误差函数：
其实没有PmapP_{map}Pmap这个点，误差函数是点到线、面的距离，这里认为PmapP_{map}Pmap是线、面上垂直PscanP_{scan}Pscan的点（后面和PmapP_{map}Pmap没有关系）。
（打错了，是f=dist(Pscan,Pmap)f=dist(P_{scan},P_{map})f=dist(Pscan,Pmap) ）

误差函数的雅可比JJJ

1、分步求导第一项：

即为特征点到对应线面的距离的单位向量[la,lb,lc][la,lb,lc][la,lb,lc]（行向量），在求误差fff时，便已经得到，存放在coeff.xyz中。
2、分步求导第二项：
（应该是[rx,ry,rz,x,y,z][rx,ry,rz,x,y,z][rx,ry,rz,x,y,z]，下面写法括号里的TTT不该带转置的）

RRR是机体在世界系下的位姿，先围绕zzz轴旋转的角度为rzrzrz，后围绕xxx轴旋转的角度为rxrxrx，最后围绕yyy轴旋转的角度为ryryry，R=RyRxRzR=RyRxRzR=RyRxRz，表示为（推导过程可以见附录中wiki截图）：

2.1 我们先只看对rxrxrx的求导部分：

其中，PscanP_{scan}Pscan即点的坐标，存储在pointOri.xyz，这一块的雅可比为：

对应代码中的：

float arx = (crx*sry*srz*pointOri.x + crx * crz*sry*pointOri.y - srx * sry*pointOri.z) * coeff.x+ (-srx * srz*pointOri.x - crz * srx*pointOri.y - crx * pointOri.z) * coeff.y+ (crx*cry*srz*pointOri.x + crx * cry*crz*pointOri.y - cry * srx*pointOri.z) * coeff.z;

2.2 ry,rzry,rzry,rz略过，对TTT的求导部分为：

这一块的雅可比为：

对应代码中的：

matA(i, 3) = coeff.x;
matA(i, 4) = coeff.y;
matA(i, 5) = coeff.z;

3、最后把3个旋转，和T（x，y，z）的雅可比拼凑起来，就得到该特征点对应到1X6的雅可比矩阵：

附录：
参考1 2 3；
和参考123有些差异的地方，其中的RRR如下，为啥差着负号，还没搞懂，没看过loam源码，估计是优化的R和计算误差时用的R是逆的关系，优化用Rt−1tR_{t-1}^tRt−1t，误差函数是当前帧点变到上一帧Rtt−1R_{t}^{t-1}Rtt−1，但最后优化结果直接加在Rt−1worldR_{t-1}^{world}Rt−1world上了（也就是Rtt−1R_{t}^{t-1}Rtt−1）？不知道不知道…

这里附上wiki中的公式:

标量对列向量求导（参考1中有误）：

代码：

// 求roll、pitch、yaw对应的sin和cos
float srx = _transformTobeMapped.rot_x.sin();
float crx = _transformTobeMapped.rot_x.cos();
float sry = _transformTobeMapped.rot_y.sin();
float cry = _transformTobeMapped.rot_y.cos();
float srz = _transformTobeMapped.rot_z.sin();
float crz = _transformTobeMapped.rot_z.cos();Eigen::Matrix<float, Eigen::Dynamic, 6> matA(laserCloudSelNum, 6);  // J
Eigen::Matrix<float, 6, Eigen::Dynamic> matAt(6, laserCloudSelNum);   // JT
Eigen::Matrix<float, 6, 6> matAtA;                                    // JT*J
Eigen::VectorXf matB(laserCloudSelNum);                             // f
Eigen::VectorXf matAtB;                                             // JT*f
Eigen::VectorXf matX;                                                   // delta// 对每个点依次构建雅克比
for (int i = 0; i < laserCloudSelNum; i++)
{// scan中每个特征点的坐标（当前雷达系下）pointOri = _laserCloudOri.points[i];// scan中特征点到map中对应点的距离的方向单位向量（即误差在xyz方向上的单位分量）coeff = _coeffSel.points[i];// 雅克比中对roll求导部分float arx = (crx*sry*srz*pointOri.x + crx * crz*sry*pointOri.y - srx * sry*pointOri.z) * coeff.x+ (-srx * srz*pointOri.x - crz * srx*pointOri.y - crx * pointOri.z) * coeff.y+ (crx*cry*srz*pointOri.x + crx * cry*crz*pointOri.y - cry * srx*pointOri.z) * coeff.z;// 雅克比中对pitch求导部分float ary = ((cry*srx*srz - crz * sry)*pointOri.x+ (sry*srz + cry * crz*srx)*pointOri.y + crx * cry*pointOri.z) * coeff.x+ ((-cry * crz - srx * sry*srz)*pointOri.x+ (cry*srz - crz * srx*sry)*pointOri.y - crx * sry*pointOri.z) * coeff.z;// 雅克比中对yaw求导部分float arz = ((crz*srx*sry - cry * srz)*pointOri.x + (-cry * crz - srx * sry*srz)*pointOri.y)*coeff.x+ (crx*crz*pointOri.x - crx * srz*pointOri.y) * coeff.y+ ((sry*srz + cry * crz*srx)*pointOri.x + (crz*sry - cry * srx*srz)*pointOri.y)*coeff.z;// 雅克比中对roll，pitch，yaw求导部分值matA(i, 0) = arx;matA(i, 1) = ary;matA(i, 2) = arz;//雅克比中对x，y，z求导部分值matA(i, 3) = coeff.x;matA(i, 4) = coeff.y;matA(i, 5) = coeff.z;// 残差fmatB(i, 0) = -coeff.intensity;
}matAt = matA.transpose();
matAtA = matAt * matA;
matAtB = matAt * matB;// LM求解：JT*J*x=-JT*f （solve： A * X = B， 求解X）
matX = matAtA.colPivHouseholderQr().solve(matAtB);// 更新状态量： X = X + δX
_transformTobeMapped.rot_x += matX(0, 0);
_transformTobeMapped.rot_y += matX(1, 0);
_transformTobeMapped.rot_z += matX(2, 0);
_transformTobeMapped.pos.x() += matX(3, 0);
_transformTobeMapped.pos.y() += matX(4, 0);
_transformTobeMapped.pos.z() += matX(5, 0);// 更新小于阈值，则判断收敛;或达到指定次数，则退出
float deltaR = sqrt(pow(rad2deg(matX(0, 0)), 2) +pow(rad2deg(matX(1, 0)), 2) +pow(rad2deg(matX(2, 0)), 2));
float deltaT = sqrt(pow(matX(3, 0) * 100, 2) +pow(matX(4, 0) * 100, 2) +pow(matX(5, 0) * 100, 2));if (deltaR < _deltaRAbort && deltaT < _deltaTAbort)break;

lio-sam中雅克比推导相关推荐

传统ORB-SLam中位姿优化中雅克比矩阵讲解
由于之前的鱼眼orbslam只有单目部分,所以在优化时也只是用了单目位姿优化和三维坐标点优化,并没有将双目的优化添加进去,不知道是否对结果有影响: 这里添加双目的优化部分,主要是将添加雅克比矩阵: o ...
Lego-LOAM雅可比矩阵的推导
求解的问题是当前帧投影到上一帧中点到线的最小距离即F(X),因为优化R,T所以X={R,T}. 首先声明几个符号,为当前帧的点(在世界坐标系下的),为当前帧投影到上一帧中的点(lidar坐标系),为当 ...
g2o中 EdgeSE3Expmap类型Jacobian的计算
位姿优化的时候,两个顶点的类型是SE3,涉及到的误差雅克比是pose error对pose的求导,里面有些知识值得注意,故记录下来. 前期准备重新翻看Ethan Eade的<Lie Group ...
国内外LiDAR SLAM实验室总结
知乎上有一个关于视觉的SLAM实验室以及20年前的视觉SLAM开源算法的汇总SLAM 领域国内外优秀实验室汇总,非常详细,但是偏视觉一点,本文主要关注LiDAR SLAM. 这篇文章一开始是非常简短的 ...
SVO 半直接视觉里程计
SVO 从名字来看,是半直接视觉里程计,所谓半直接是指通过对图像中的特征点图像块进行直接匹配来获取相机位姿,而不像直接匹配法那样对整个图像使用直接匹配.整幅图像的直接匹配法常见于RGBD传感器,因为R ...
双目视惯雷达SLAM
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达本文转自:3D视觉工坊主要内容本文将Foster提出的紧耦合V ...
SLAM 论文阅读和分类整理
前言:以前读论文,都是靠脑子硬记,哪个实验室,谁,哪一年在什么会议上发了一篇关于什么的论文.当需要回溯的时候,每篇论文能给出个大概,不具体,找起来也麻烦,以后就在这个 List 里分类整理已经读过的论 ...
svo： semi-direct visual odometry 论文解析
SVO 从名字来看,是半直接视觉里程计,所谓半直接是指通过对图像中的特征点图像块进行直接匹配来获取相机位姿,而不像直接匹配法那样对整个图像使用直接匹配.整幅图像的直接匹配法常见于RGBD传感器,因 ...
【转】SLAM 论文阅读和分类整理
需要时不时膜拜一下大神,激发一下原动力!!! 前言:以前读论文,都是靠脑子硬记,哪个实验室,谁,哪一年在什么会议上发了一篇关于什么的论文.当需要回溯的时候,每篇论文能给出个大概,不具体,找起来也麻烦, ...

lio-sam中雅克比推导

lio-sam中雅克比推导相关推荐

最新文章

热门文章