loam中激光里程计部分accumulateRotation()函数, PluginIMURotation函数以及误差函数对欧拉角偏导的数学推导

函数如下:
AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], -transform[0], -transform[1] * 1.05, -transform[2], rx, ry, rz);

//transformSum代表的是前一帧的终点到世界坐标系的位姿,
//transform表示当前帧起始点到终点的位姿,不是终点相对于起始点的位姿,这也是为什么参数里面transform要用负号的原因
//AccumulateRotation计算的是当前帧的终点相对于世界坐标系的欧拉角rx,ry,rz
//要计算当前点相对于世界坐标系的欧拉角,先得计算出当前帧终点相对于世界坐标系的旋转矩阵R=Rtransformsum*Rtransform;(1)
源码中欧拉角对应的旋转矩阵为绕Z,X,Y的顺序。
先贴以下源码:
loam中激光里程计部分accumulateRotation()函数, PluginIMURotation函数以及误差函数对欧拉角偏导的数学推导_第1张图片
欧拉角所对应的旋转矩阵为:loam中激光里程计部分accumulateRotation()函数, PluginIMURotation函数以及误差函数对欧拉角偏导的数学推导_第2张图片
根据(1)式可知,
loam中激光里程计部分accumulateRotation()函数, PluginIMURotation函数以及误差函数对欧拉角偏导的数学推导_第3张图片
R23就是源码中的srx,然后反解即可求出欧拉角,依次类推即可求出oy,oz;

后面部分是求平移向量tx,ty,tz,可以根据转换矩阵T=TtransformsumTtransform;来求出平移偏量,这里有个细节需要注意,实际上transform[i]的数组表示的当前帧的起点相对于终点的位姿,平移量也是一样,也是表示的当前帧的起点相对于终点的平移,实际上根据transform[i]里面的欧拉角计算出来的矩阵R应该是绕Y,X,Z的顺序计算出来的矩阵,前面计算出来的
Rtransform表示的是当前帧的终点相对于起点旋转矩阵,所以应该是R的转置(逆),同样Ttransform就是T的逆,这样问题的第一步就变成了已知T计算T的逆
公式如下:
loam中激光里程计部分accumulateRotation()函数, PluginIMURotation函数以及误差函数对欧拉角偏导的数学推导_第4张图片
计算过程如下:(R的转置代表着Rtransform)
loam中激光里程计部分accumulateRotation()函数, PluginIMURotation函数以及误差函数对欧拉角偏导的数学推导_第5张图片
由前面可知R=Rtransformsum
Rtransform;
故t=[tx,ty,tz]=tsum-Rt,和源码一样。
loam中激光里程计部分accumulateRotation()函数, PluginIMURotation函数以及误差函数对欧拉角偏导的数学推导_第6张图片

PluginIMURotation函数的数学推导
//PluginIMURotation函数表示将rx,ry,rzimu补偿之后赋值给rx,ry,rz,大意是全局欧拉角乘于imu起点的欧拉角的转置乘于imu终点的欧拉角,就是用全局的绝对位姿乘于imu计算出来的相对位姿态,并不是很懂为什么这样可以补偿,有懂的指点一下。。。
loam中激光里程计部分accumulateRotation()函数, PluginIMURotation函数以及误差函数对欧拉角偏导的数学推导_第7张图片
推导过程:
loam中激光里程计部分accumulateRotation()函数, PluginIMURotation函数以及误差函数对欧拉角偏导的数学推导_第8张图片
和源码计算出来的srx一样,剩下的类似。。
loam中激光里程计部分accumulateRotation()函数, PluginIMURotation函数以及误差函数对欧拉角偏导的数学推导_第9张图片
关于误差函数对欧拉角及其平移分量的求导参考我另一篇博文。。https://blog.csdn.net/l1323/article/details/106407809

你可能感兴趣的:(loam中激光里程计部分accumulateRotation()函数, PluginIMURotation函数以及误差函数对欧拉角偏导的数学推导)