技术标签: 自动驾驶 机器学习 人工智能 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;
至于后面利用得到的旋转增量和平移增量进行仿射变换,把当前的激光雷达数据帧变换到初始位置的数据帧则使用到前面的仿射变换的知识。
文章浏览阅读1.6k次。安装配置gi、安装数据库软件、dbca建库见下:http://blog.csdn.net/kadwf123/article/details/784299611、检查集群节点及状态:[root@rac2 ~]# olsnodes -srac1 Activerac2 Activerac3 Activerac4 Active[root@rac2 ~]_12c查看crs状态
文章浏览阅读1.3w次,点赞45次,收藏99次。我个人用的是anaconda3的一个python集成环境,自带jupyter notebook,但在我打开jupyter notebook界面后,却找不到对应的虚拟环境,原来是jupyter notebook只是通用于下载anaconda时自带的环境,其他环境要想使用必须手动下载一些库:1.首先进入到自己创建的虚拟环境(pytorch是虚拟环境的名字)activate pytorch2.在该环境下下载这个库conda install ipykernelconda install nb__jupyter没有pytorch环境
文章浏览阅读5.2k次,点赞19次,收藏28次。选择scoop纯属意外,也是无奈,因为电脑用户被锁了管理员权限,所有exe安装程序都无法安装,只可以用绿色软件,最后被我发现scoop,省去了到处下载XXX绿色版的烦恼,当然scoop里需要管理员权限的软件也跟我无缘了(譬如everything)。推荐添加dorado这个bucket镜像,里面很多中文软件,但是部分国外的软件下载地址在github,可能无法下载。以上两个是官方bucket的国内镜像,所有软件建议优先从这里下载。上面可以看到很多bucket以及软件数。如果官网登陆不了可以试一下以下方式。_scoop-cn
文章浏览阅读4.5k次,点赞2次,收藏3次。首先要有一个color-picker组件 <el-color-picker v-model="headcolor"></el-color-picker>在data里面data() { return {headcolor: ’ #278add ’ //这里可以选择一个默认的颜色} }然后在你想要改变颜色的地方用v-bind绑定就好了,例如:这里的:sty..._vue el-color-picker
文章浏览阅读640次。基于芯片日益增长的问题,所以内核开发者们引入了新的方法,就是在内核中只保留函数,而数据则不包含,由用户(应用程序员)自己把数据按照规定的格式编写,并放在约定的地方,为了不占用过多的内存,还要求数据以根精简的方式编写。boot启动时,传参给内核,告诉内核设备树文件和kernel的位置,内核启动时根据地址去找到设备树文件,再利用专用的编译器去反编译dtb文件,将dtb还原成数据结构,以供驱动的函数去调用。firmware是三星的一个固件的设备信息,因为找不到固件,所以内核启动不成功。_exynos 4412 刷机
文章浏览阅读2w次,点赞24次,收藏42次。Linux系统配置jdkLinux学习教程,Linux入门教程(超详细)_linux配置jdk
文章浏览阅读3.3k次,点赞5次,收藏19次。xlabel('\delta');ylabel('AUC');具体符号的对照表参照下图:_matlab微米怎么输入
文章浏览阅读119次。顺序读写指的是按照文件中数据的顺序进行读取或写入。对于文本文件,可以使用fgets、fputs、fscanf、fprintf等函数进行顺序读写。在C语言中,对文件的操作通常涉及文件的打开、读写以及关闭。文件的打开使用fopen函数,而关闭则使用fclose函数。在C语言中,可以使用fread和fwrite函数进行二进制读写。 Biaoge 于2024-03-09 23:51发布 阅读量:7 ️文章类型:【 C语言程序设计 】在C语言中,用于打开文件的函数是____,用于关闭文件的函数是____。
文章浏览阅读3.4k次,点赞2次,收藏13次。跟随鼠标移动的粒子以grid(SOP)为partical(SOP)的资源模板,调整后连接【Geo组合+point spirit(MAT)】,在连接【feedback组合】适当调整。影响粒子动态的节点【metaball(SOP)+force(SOP)】添加mouse in(CHOP)鼠标位置到metaball的坐标,实现鼠标影响。..._touchdesigner怎么让一个模型跟着鼠标移动
文章浏览阅读178次。项目运行环境配置:Jdk1.8 + Tomcat7.0 + Mysql + HBuilderX(Webstorm也行)+ Eclispe(IntelliJ IDEA,Eclispe,MyEclispe,Sts都支持)。项目技术:Springboot + mybatis + Maven +mysql5.7或8.0+html+css+js等等组成,B/S模式 + Maven管理等等。环境需要1.运行环境:最好是java jdk 1.8,我们在这个平台上运行的。其他版本理论上也可以。_基于java技术的停车场管理系统实现与设计
文章浏览阅读3.5k次。前言对于MediaPlayer播放器的源码分析内容相对来说比较多,会从Java-&amp;gt;Jni-&amp;gt;C/C++慢慢分析,后面会慢慢更新。另外,博客只作为自己学习记录的一种方式,对于其他的不过多的评论。MediaPlayerDemopublic class MainActivity extends AppCompatActivity implements SurfaceHolder.Cal..._android多媒体播放源码分析 时序图
文章浏览阅读2.4k次,点赞41次,收藏13次。java 数据结构与算法 ——快速排序法_快速排序法