loam中激光里程计部分对欧拉角以及平移分量的求导的数学推导

首先我们看一下论文关于这部分的描述:
在这里插入图片描述
通过上面这个公式把之前通过点到线的距离以及点到面的距离构建的误差函数转化成关于Tk+1的函数,即源码中的transform[i]数组,这部分和建图部分求导不同在于transform数组表示的是当前帧的起始点相对于终点的位姿,欧拉角表示的是Y,X,Z的旋转,旋转矩阵对应上式的R矩阵,通过链式法则求导,计算过程如下:
loam中激光里程计部分对欧拉角以及平移分量的求导的数学推导_第1张图片
la,lb,lc代表距离对起始点的偏导,在前面已经计算过了即源码对应的coeff.x,coeff.y,coeff.z
loam中激光里程计部分对欧拉角以及平移分量的求导的数学推导_第2张图片
计算出来的arx和源码一模一样,依次可算出ary,arz,源码如下:
loam中激光里程计部分对欧拉角以及平移分量的求导的数学推导_第3张图片
对平移分量tx,ty,tz的偏导计算如下:
loam中激光里程计部分对欧拉角以及平移分量的求导的数学推导_第4张图片
和源码中的atx一模一样,以此类推即可计算出aty,atz
loam中激光里程计部分对欧拉角以及平移分量的求导的数学推导_第5张图片

你可能感兴趣的:(loam中激光里程计部分对欧拉角以及平移分量的求导的数学推导)