技术标签: 自动驾驶 机器学习 人工智能 LVI-SAM分析
2021SC@SDUSC
物体姿态计算是由cloudHandle()
中的deskewInfo()
中的imuDeskewInfo()
完成的。首先,由上一篇blog,我们可以得知,imu和姿态有关的数据是方向(用四元数(Quaternion)表示),角速度来计算。下面开始用这2个数据,来计算当前物体的姿态。
lidar帧初始姿态角是由imu数据中的方向(用四元数(Quaternion)表示)计算得出。虽然,在代码中,用了以下几行代码就可以轻易计算得出,但其中的数学原理,我们也可以了解下。
template<typename T>
void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw)
{
double imuRoll, imuPitch, imuYaw;
tf::Quaternion orientation;
tf::quaternionMsgToTF(thisImuMsg->orientation, orientation);
tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
*rosRoll = imuRoll;
*rosPitch = imuPitch;
*rosYaw = imuYaw;
}
这里要接触2个概念,四元数和欧拉角。四元数在上一篇blog已经介绍了,下面介绍一下欧拉角。
莱昂哈德·欧拉用欧拉角来描述刚体在三维欧几里得空间的取向。对于任何参考系,一个刚体的取向,是依照顺序,从这参考系,做三个欧拉角的旋转而设定的。所以,刚体的取向可以用三个基本旋转矩阵来决定。换句话说,任何关于刚体旋转的旋转矩阵是由三个基本旋转矩阵复合而成的。
维基百科定义:
对于在三维空间里的一个参考系,任何坐标系的取向,都可以用三个欧拉角来表现。参考系(固定系)又称为实验室参考系,是静止不动的。而坐标系(固连系)则固定于刚体,随着刚体的旋转而旋转。
参阅下图。设定xyz-轴为参考系的参考轴。称xy-平面与XY-平面的相交为交点线,用英文字母(N)代表。zxz顺规的欧拉角可以静态地这样定义:
- α \alpha α(进动角)是x-轴与交点线的夹角,
- β \beta β(章动角)是z-轴与Z-轴的夹角,
- γ \gamma γ(自旋角)是交点线与X-轴的夹角。
对于夹角的顺序和标记,夹角的两个轴的指定,并没有任何常规。科学家对此从未达成共识。每当用到欧拉角时,我们必须明确的表示出夹角的顺序,指定其参考轴。
实际上,有许多方法可以设定两个坐标系的相对取向。欧拉角方法只是其中的一种。此外,不同的作者会用不同组合的欧拉角来描述,或用不同的名字表示同样的欧拉角。因此,使用欧拉角前,必须先做好明确的定义。
通俗解释一下:描述坐标系{B}相对于参考坐标系{A}的姿态有两种方式。第一种是绕固定(参考)坐标轴旋转:假设开始两个坐标系重合,先将{B}绕{A}的X轴旋转 γ \gamma γ,然后绕{A}的Y轴旋转 β \beta β,最后绕{A}的Z轴旋转 α \alpha α,就能旋转到当前姿态。可以称其为X-Y-Z fixed angles或RPY角(Roll, Pitch, Yaw)。如下图演示:
Roll:横滚
Pitch: 俯仰
Yaw: 偏航(航向)
前面提到,设定刚体取向的旋转矩阵 [ R ] [\mathbf {R}] [R]是由三个基本旋转矩阵合成的:
[ R ] = [ cos γ sin γ 0 − sin γ cos γ 0 0 0 1 ] [ 1 0 0 0 cos β sin β 0 − sin β cos β ] [ cos α sin α 0 − sin α cos α 0 0 0 1 ] [\mathbf{R}] = \begin{bmatrix} \cos \gamma & \sin \gamma & 0 \\ -\sin \gamma & \cos \gamma & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos \beta & \sin \beta \\ 0 & -\sin \beta & \cos \beta \end{bmatrix} \begin{bmatrix} \cos \alpha & \sin \alpha & 0 \\ -\sin \alpha & \cos \alpha & 0 \\ 0 & 0 & 1 \end{bmatrix} [R]=⎣⎡cosγ−sinγ0sinγcosγ0001⎦⎤⎣⎡1000cosβ−sinβ0sinβcosβ⎦⎤⎣⎡cosα−sinα0sinαcosα0001⎦⎤
从左到右依次代表绕着z轴的旋转、绕着交点线的旋转、绕着Z轴的旋转。
经过一番运算,
[ R ] = [ cos α cos γ − cos β sin α sin γ sin α cos γ + cos β cos α sin γ sin β sin γ − cos α sin γ − cos β sin α cos γ − sin α sin γ + cos β cos α cos γ sin β cos γ sin β sin α − sin β cos α cos β ] [\mathbf{R}] = \begin{bmatrix} \cos\alpha\cos\gamma-\cos\beta\sin\alpha\sin\gamma & \sin\alpha\cos\gamma+\cos\beta\cos\alpha\sin\gamma & \sin\beta\sin\gamma \\-\cos\alpha\sin\gamma-\cos\beta\sin\alpha\cos\gamma & -\sin\alpha\sin\gamma+\cos\beta\cos\alpha\cos\gamma & \sin\beta\cos\gamma \\ \sin\beta\sin\alpha & -\sin\beta\cos\alpha & \cos\beta \end{bmatrix} [R]=⎣⎡cosαcosγ−cosβsinαsinγ−cosαsinγ−cosβsinαcosγsinβsinαsinαcosγ+cosβcosαsinγ−sinαsinγ+cosβcosαcosγ−sinβcosαsinβsinγsinβcosγcosβ⎦⎤
[ R ] [\mathbf {R}] [R]的逆矩阵是:
[ R ] − 1 = [ cos α cos γ − cos β sin α sin γ − cos α sin γ − cos β sin α cos γ sin β sin α sin α cos γ + cos β cos α sin γ − sin α sin γ + cos β cos α cos γ − sin β cos α sin β sin γ sin β cos γ cos β ] [\mathbf{R}]^{-1}= \begin{bmatrix} \cos\alpha\cos\gamma-\cos\beta\sin\alpha\sin\gamma & -\cos\alpha\sin\gamma-\cos\beta\sin\alpha\cos\gamma & \sin\beta\sin\alpha \\ \sin\alpha\cos\gamma+\cos\beta\cos\alpha\sin\gamma & -\sin\alpha\sin\gamma+\cos\beta\cos\alpha\cos\gamma & -\sin\beta\cos\alpha \\ \sin\beta\sin\gamma & \sin\beta\cos\gamma & \cos\beta \end{bmatrix} [R]−1=⎣⎡cosαcosγ−cosβsinαsinγsinαcosγ+cosβcosαsinγsinβsinγ−cosαsinγ−cosβsinαcosγ−sinαsinγ+cosβcosαcosγsinβcosγsinβsinα−sinβcosαcosβ⎦⎤
绕坐标轴的多次旋转可以等效为绕某一转轴旋转一定的角度。假设等效旋转轴方向向量为 K ⃗ = [ k x , k y , k z ] T \vec{K} =[k_x,k_y,k_z]^T K=[kx,ky,kz]T,等效旋转角为 θ θ θ,则四元数 q = ( x , y , z , w ) q=(x,y,z,w) q=(x,y,z,w),其中:
x = k x ⋅ sin ( θ 2 ) y = k y ⋅ sin ( θ 2 ) z = k z ⋅ sin ( θ 2 ) w = cos ( θ 2 ) x=k_x·\sin(\frac{\theta}{2})\\ y=k_y·\sin(\frac{\theta}{2})\\ z=k_z·\sin(\frac{\theta}{2})\\ w=\cos(\frac{\theta}{2}) x=kx⋅sin(2θ)y=ky⋅sin(2θ)z=kz⋅sin(2θ)w=cos(2θ)
且有 x 2 + y 2 + z 2 + w 2 = 1 x^2+y^2+z^2+w^2=1 x2+y2+z2+w2=1
即四元数存储了旋转轴和旋转角的信息,它能方便的描述刚体绕任意轴的旋转。
四元数转换为旋转矩阵:
R = [ 1 − 2 y 2 − 2 z 2 2 ( x y − z w ) 2 ( x z + y w ) 2 ( x y + z w ) 1 − 2 x 2 − 2 z 2 2 ( y z − x w ) 2 ( x z − y w ) 2 ( y z + x w ) 1 − 2 x 2 − 2 y 2 ] \mathbf{R} = \begin{bmatrix} 1-2y^2-2z^2 & 2(xy-zw) & 2(xz+yw) \\2(xy+zw) & 1-2x^2-2z^2 & 2(yz-xw) \\ 2(xz-yw) & 2(yz+xw) & 1-2x^2-2y^2 \end{bmatrix} R=⎣⎡1−2y2−2z22(xy+zw)2(xz−yw)2(xy−zw)1−2x2−2z22(yz+xw)2(xz+yw)2(yz−xw)1−2x2−2y2⎦⎤
已知旋转矩阵为:
则对应的四元数为:
旋转角的的计算较为简单,因为imu的信息里面有关角速度的数据。因此,当前时刻的旋转角度可以通过角速度×时间计算得出。角速度的信息提取如下:
template<typename T>
void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z)
{
*angular_x = thisImuMsg->angular_velocity.x;
*angular_y = thisImuMsg->angular_velocity.y;
*angular_z = thisImuMsg->angular_velocity.z;
}
我们设一开始的旋转角度为0。然后,建立一个缓存的地方存放每个帧的旋转角。那么,当前时刻的旋转角就等于,前一时刻旋转角 + 角速度 * 时差。时间差也是有已经缓存的时间信息。然后再把计算得到的旋转角度放入缓存中,等待之后的计算。代码如下:
// imu帧间时差
double timeDiff = currentImuTime - imuTime[imuPointerCur-1];
// 当前时刻旋转角 = 前一时刻旋转角 + 角速度 * 时差
imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff;
imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff;
imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;
imuTime[imuPointerCur] = currentImuTime;
++imuPointerCur;
物体的里程计计算是由cloudHandle()
中的deskewInfo()
中的odomDeskewInfo()
完成的。这个函数的主要功能为遍历当前激光帧起止时刻之间的imu里程计数据,初始时刻对应imu里程计设为当前帧的初始位姿,并用起始、终止时刻对应imu里程计,计算相对位姿变换,保存平移增量。下面开始讲解技术要点。
位姿的计算和上面的四元数与欧拉角的相互转换相同。参考上面的理论介绍即可。
// 提取imu里程计姿态角
tf::Quaternion orientation;
tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);
double roll, pitch, yaw;
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
// 用当前激光帧起始时刻的odom,初始化lidar位姿,后面用于mapOptmization
cloudInfo.odomX = startOdomMsg.pose.pose.position.x;
cloudInfo.odomY = startOdomMsg.pose.pose.position.y;
cloudInfo.odomZ = startOdomMsg.pose.pose.position.z;
cloudInfo.odomRoll = roll;
cloudInfo.odomPitch = pitch;
cloudInfo.odomYaw = yaw;
cloudInfo.odomResetId = (int)round(startOdomMsg.pose.covariance[0]);
在用起始、终止时刻对应imu里程计,计算相对位姿变换,保存平移增量的时候,就涉及到了几何学的知识,需要在2个不同的坐标系之间进行变换。
Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);
tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw);
Eigen::Affine3f transBt = transBegin.inverse() * transEnd;
float rollIncre, pitchIncre, yawIncre;
pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);
这里其实是计算起止时刻imu里程计的相对变换,并且提取增量平移,旋转(欧拉角)。这里的相对变换其实是用了仿射变换。因此,我们需要了解仿射变换的相关知识。
仿射变换(Affine transformation),又称仿射映射,是指在几何中,对一个向量空间进行一次线性变换并接上一个平移,变换为另一个向量空间。一个对向量 x ⃗ \displaystyle {\vec {x}} x平移 b ⃗ \displaystyle {\vec {b}} b,与旋转放大缩小 A ⃗ \displaystyle{\vec A} A 的仿射映射为
y ⃗ = A x ⃗ + b ⃗ \vec y = A\vec x+\vec b y=Ax+b
上式在其次坐标上,等价于下面的式子
[ y ⃗ 1 ] = [ A b ⃗ 0 , . . . , 0 1 ] [ x ⃗ 1 ] \begin{bmatrix} \vec y \\ 1 \end{bmatrix} = \begin{bmatrix} A & \vec b \\ 0,...,0 & 1\end{bmatrix} \begin{bmatrix} \vec x \\ 1 \end{bmatrix} [y1]=[A0,...,0b1][x1]
在分形的研究里,收缩平移仿射映射可以制作具有自相似性的分形。
简单来讲,“仿射变换”就是:“线性变换”+“平移”。仿射变换从几何直观只有两个要点:
变换前是直线,变换后也应该是直线
直线比例保持不变
那么什么是线性变换呢?
在数学中,线性映射(有的书上将“线性变换”作为其同义词,有的则不然)是在两个向量空间(包括由函数构成的抽象的向量空间)之间的一种保持向量加法和标量乘法的特殊映射。线性映射从抽象代数角度看是向量空间的同态,从范畴论角度看是在给定的域上的向量空间所构成的范畴中的态射。
简单来讲,线性变换就是只含有向量的加法或标量的乘法的一种运算。线性变换从几何直观有三个要点:
比如说旋转:
比如说推移:
这两个叠加也是线性变换:
在上面通俗地理解了线性变换之后,可以看看它的数学定义:
设 V V V和 W W W是在相同域* K K K上的向量空间。法则 f : V → W f: V \rightarrow W f:V→W被称为是线性映射,如果对于 V V V中任何两个向量 x x x和 y y y与 K K K*中任何标量 a a a,满足下列两个条件:
可加性: f ( x + y ) = f ( x ) + f ( y ) f(x+y)=f(x)+f(y) f(x+y)=f(x)+f(y)
齐次性: f ( a x ) = a f ( x ) f(ax)=af(x) f(ax)=af(x)
这等价于要求对于任何向量 x 1 , … , x m \displaystyle x_{1},\ldots ,x_{m} x1,…,xm和标量 a 1 , … , a m \displaystyle a_{1},\ldots ,a_{m} a1,…,am,方程
f ( a 1 x 1 + ⋯ + a m x m ) = a 1 f ( x 1 ) + ⋯ + a m f ( x m ) f(a_1x_1+\dots+a_mx_m)=a_1f(x_1)+\dots+a_mf(x_m) f(a1x1+⋯+amxm)=a1f(x1)+⋯+amf(xm)
成立。
什么意思?继续举例子:
这样我就可以在三维空间下通过 [ A b ⃗ 0 1 ] \begin{bmatrix}A & \vec b \\ 0 & 1\end{bmatrix} [A0b1] 这个线性变换来操作 z = 1 z=1 z=1 平面上的二维正方形,完成仿射变换:
自己动手操作一下:
我们平移到需要的位置的时候:
如果还有没有清楚的地方,可以结合之前的描述,看一下维基百科“仿射变换”词条里的一个gif动图,非常生动的表明了这一过程:
在代码中,我找到不到pcl::getTransformation (float x, float y, float z, float roll, float pitch, float yaw)
的源代码,也找不到头文件中找不到实际的源码,只有对这个函数的描述:
Create a transformation from the given translation and Euler angles (XYZ-convention)
从给定的平移和欧拉角创建一个变换(XYZ-公约)。
但是按照我个人的理解,这个函数pcl::getTransformation (float x, float y, float z, float roll, float pitch, float yaw)
,最后得到的是描述这个平面的一个变换矩阵。然后用这个方法得到两个这样的矩阵 A A A和 B B B,矩阵之间能用这么一个式子来转换。
A ⋅ T = B A·T = B A⋅T=B
然后,只需要求出T就能求得它们的2个平面的点的转换关系。因此,可得下面的式子:
T = A T B T = A^TB T=ATB
在上面的代码中,变量transBt
就是这么一个T。
在代码中,检测激光雷达数据的时候有一个检测降采样的判断。而降采样对我来说,也是一个完全陌生的东西,因此,打算了解一下什么是降采样。下面是维基百科对降采样的描述。
在数位信号处理领域中,降采样,又作减采集,是一种多速率数字信号处理的技术或是降低信号采样率的过程,通常用于降低数据传输速率或者数据大小。
不过,这个降采样应该是其他部分处理得,等遇到了我们再深入研究。
激光运动矫正也是用上面提到的仿射变换来利用当前帧起止时刻之间的imu数据计算旋转增量,imu里程计数据计算平移增量,进而将每一时刻激光点位置变换到第一个激光点坐标系下,进行运动补偿。
首先计算当前帧相对于起始帧的旋转增量(因为初始帧的旋转设为0,所以增量就是当前的旋转的数值)。这个计算较为简单,要分2种情况讨论。
要计算的激光雷达帧的时间 >= 最新的IMU数据的时间。这时候激光雷达帧的旋转增量就是最近的IMU的旋转的增量。
要计算的激光雷达帧的时间 < 最新的IMU数据的时间。这时候假设2个IMU数据帧之间的旋转增量是线性变化的,激光雷达帧的旋转增量就是利用激光雷达帧时间的最近的前后2个IMU数据帧,通过计算时间加权得出。
int imuPointerBack = imuPointerFront - 1;
double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
*rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;
*rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;
*rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;
至于后面利用得到的旋转增量和平移增量进行仿射变换,把当前的激光雷达数据帧变换到初始位置的数据帧则使用到前面的仿射变换的知识。
文章浏览阅读174次。第一章1.1 数字图像一幅图像可以定义为一个而为函数, 其中x和y是空间坐标,而在任何一对空间坐标(x, y)处的幅值f称为图像在该点处的强度或灰度。当x, y和灰度值f是有限的离散数值时,该图像称为数字图像。1.2 数字图像处理的起源计算机断层(CT)是一种处理方法,图像处理在医学诊断领域最重要的应用之一。计算机方法用于增强对比度或将灰度编码为彩色,以解释工业、医学以及生物科..._是计算机生成图像的显著例子,它只不过是根据一些数学规则对一个基本模式的迭
文章浏览阅读252次。由于Master无法停机,并且master同时存在大量INNODB和MyISAM引擎的表,而且总体数据量很大,因此无法通过常规的手段来重做Slave。现在需要新增某个库的同步,刚好这个库在Slave上设置了replicate-ignore-db(历史原因),所幸这个库里的表全部是INNODB,可以通过如下方法来实现。首先在Master上将该库导出,使用mysqldump不加锁导出或者使用perco..._mysql 增加同步库
文章浏览阅读852次。run gradle with —stacktrace提示的错误消息如下,但是没有指明错误的位置。 ···· :dax_android_Lib:processDebugAndroidTestManifest /Users/dsh/Desktop/HaoCang/mango/New_MAX..._error: stash{0} is not a valid reference
文章浏览阅读3.4k次。集合类 单值类型、双值类型、实现。单 集合继承自collection双 继承自maparrayList 底层使用Object 数组。线程不安全。容量不够, 乘以1.5 + 1,getter、setter 效率高linkedList 底层使用双向循环链表数据结构,查询慢,增删快。线程不安全。hashSet 底层使用哈希表实现,内部是hashMaptreeSet 内部是treeMap 的sortedSet。底层使用二叉树实现,排序存储。linkedHashSet,底层是LinkedH_无法创建数据库快照以进行在线检查。可能前一个错误消息已给出原因,或者某个基础
文章浏览阅读2w次。三 Eclipse配置 1 安装android 开发插件 (1)打开Eclipse, 在菜单栏上选择 help->Install New SoftWare 出现如下界面: 点击 Add按钮,出现如下界面 输入网址: https://dl-ssl.google.com/android/eclipse_eclipse installsoftware的网址
文章浏览阅读1.6k次,点赞25次,收藏11次。CUDA以及CUDNN的下载、安装、配置与卸载详细介绍_cuda12.04对应的cudnn windows
文章浏览阅读121次。A:选出你想具有的人格,然后进行自我暗示。想象自己已经具备了这些优秀的品质。然后对自己讲:我已经是这样一个人了。那么不久你会发现自己身上出了你所羡慕的美德。B:用自制力控制住自己,每天多次提醒自己,并抓住每个表现的机会,把它展现出来。C:找到一些人,发现他们的长处并赞美他们,但切记不要虚伪、奉承,而是真诚热情的,这将证你养成发现别人美德的习惯,从而你将获得自尊及来自他人的感激。“吸引法则”..._培养和谐的个性和自尊的品格 博客
文章浏览阅读895次。1.关于conbobox取值以及赋值问题!使用的conbobox必须有class="easyui-combobox"的字样,否则为普通select!并非easyUi的控件。取值:var ex = $("#ex").combobox('getText');方法用于获取select中被选择的text字段。 var ex = $("#ex").combobox('getValue_easyui bobox
文章浏览阅读1.1k次。标签:理想的索引:相对于写操作来说,表查询很频繁的表建立索引字段区分度高长度小(合适的长度,不是越小越好)尽量能够覆盖常用字段这些条件综合起来才能够达到最优索引,本次我们着重聊一下建立合适长度的索引,索引的长度直接影响索引文件的大小,因此会影响增删改查的速度给字符类型的字段设置长度字段查询时区分度要高,如果字段只是设置了一个那么回查询很多相似的匹配度不高,长度要恰到好处,否则太长索引文件就会大,因..._mysql修改字段长度二级索引
文章浏览阅读1w次,点赞4次,收藏22次。WPF 控件专题 TextBlock 控件详解_wpf textblock
文章浏览阅读512次。在GItHub和Gitee上找了一些超好的Spring boot项目,如果大家觉得不错,可以顺手给这些项目点个小星星!一、云收藏云收藏是一个使用 Spring Boot 构建的开源网站,可以让用户在线随时随地收藏的一个网站,在网站上分类整理收藏的网站或者文章,可以作为稍后阅读的一个临时存放。作为一个开放开源的软件,可以让用户从浏览器将收藏夹内容导入到云收藏,也支持随时将云收藏收集的文章导出去做备份..._boot项目
文章浏览阅读1.7w次,点赞9次,收藏98次。用户调研后得到的需求我们可以大致分为业务需求、用户需求和功能需求这三个概念,对于产品新人来说,经常容易混淆,但了解这些是身为产品经理的一个基本功。首先我们先来了解一下这三种需求有什么异同,我们应该如何去区分这些不同的需求呢?一、需求的分类1、业务需求 :通常是基于市场营销部门、业务部门根据自己的业务需求和后续策划的活动方法所整理成的需求文档。2、用户需求:描述的是用户的目标,是用户能通过这个产品在什么场景(什么情况下)能完成什么动作(做什么)。3、功能需求:是指产品通过需求池筛选内容后,整理成此版_业务需求