技术标签: 自动驾驶 机器学习 人工智能 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;
至于后面利用得到的旋转增量和平移增量进行仿射变换,把当前的激光雷达数据帧变换到初始位置的数据帧则使用到前面的仿射变换的知识。
文章浏览阅读2w次,点赞7次,收藏51次。四个步骤1.创建C++ Win32项目动态库dll 2.在Win32项目动态库中添加 外部依赖项 lib头文件和lib库3.导出C接口4.c#调用c++动态库开始你的表演...①创建一个空白的解决方案,在解决方案中添加 Visual C++ , Win32 项目空白解决方案的创建:添加Visual C++ , Win32 项目这......_c#调用lib
文章浏览阅读4.6k次。苹方字体是苹果系统上的黑体,挺好看的。注重颜值的网站都会使用,例如知乎:font-family: -apple-system, BlinkMacSystemFont, Helvetica Neue, PingFang SC, Microsoft YaHei, Source Han Sans SC, Noto Sans CJK SC, W..._ubuntu pingfang
文章浏览阅读159次。表单表单概述表单标签表单域按钮控件demo表单标签表单标签基本语法结构<form action="处理数据程序的url地址“ method=”get|post“ name="表单名称”></form><!--action,当提交表单时,向何处发送表单中的数据,地址可以是相对地址也可以是绝对地址--><!--method将表单中的数据传送给服务器处理,get方式直接显示在url地址中,数据可以被缓存,且长度有限制;而post方式数据隐藏传输,_html表单的处理程序有那些
文章浏览阅读1.2k次。使用说明:开启Google的登陆二步验证(即Google Authenticator服务)后用户登陆时需要输入额外由手机客户端生成的一次性密码。实现Google Authenticator功能需要服务器端和客户端的支持。服务器端负责密钥的生成、验证一次性密码是否正确。客户端记录密钥后生成一次性密码。下载谷歌验证类库文件放到项目合适位置(我这边放在项目Vender下面)https://github.com/PHPGangsta/GoogleAuthenticatorPHP代码示例://引入谷_php otp 验证器
文章浏览阅读4.3k次,点赞5次,收藏11次。matplotlib.plot画图横坐标混乱及间隔处理_matplotlib更改横轴间距
文章浏览阅读2.2k次。①Storage driver 处理各镜像层及容器层的处理细节,实现了多层数据的堆叠,为用户 提供了多层数据合并后的统一视图②所有 Storage driver 都使用可堆叠图像层和写时复制(CoW)策略③docker info 命令可查看当系统上的 storage driver主要用于测试目的,不建议用于生成环境。_docker 保存容器
文章浏览阅读834次,点赞27次,收藏13次。网络拓扑结构是指计算机网络中各组件(如计算机、服务器、打印机、路由器、交换机等设备)及其连接线路在物理布局或逻辑构型上的排列形式。这种布局不仅描述了设备间的实际物理连接方式,也决定了数据在网络中流动的路径和方式。不同的网络拓扑结构影响着网络的性能、可靠性、可扩展性及管理维护的难易程度。_网络拓扑csdn
文章浏览阅读1.8k次,点赞5次,收藏8次。IOS系统Date的坑要创建一个指定时间的new Date对象时,通常的做法是:new Date("2020-09-21 11:11:00")这行代码在 PC 端和安卓端都是正常的,而在 iOS 端则会提示 Invalid Date 无效日期。在IOS年月日中间的横岗许换成斜杠,也就是new Date("2020/09/21 11:11:00")通常为了兼容IOS的这个坑,需要做一些额外的特殊处理,笔者在开发的时候经常会忘了兼容IOS系统。所以就想试着重写Date函数,一劳永逸,避免每次ne_date.prototype 将所有 ios
文章浏览阅读5.3k次。方法一:用PLSQL Developer工具。 1 在PLSQL Developer的sql window里输入select * from test for update; 2 按F8执行 3 打开锁, 再按一下加号. 鼠标点到第一列的列头,使全列成选中状态,然后粘贴,最后commit提交即可。(前提..._excel导入pl/sql
文章浏览阅读83次。Git常用命令速查手册1、初始化仓库git init2、将文件添加到仓库git add 文件名 # 将工作区的某个文件添加到暂存区 git add -u # 添加所有被tracked文件中被修改或删除的文件信息到暂存区,不处理untracked的文件git add -A # 添加所有被tracked文件中被修改或删除的文件信息到暂存区,包括untracked的文件...
文章浏览阅读202次。分享119个ASP.NET源码总有一个是你想要的_千博二手车源码v2023 build 1120
文章浏览阅读1.8k次。版权声明:转载请注明出处 http://blog.csdn.net/irean_lau。目录(?)[+]1、缺省构造函数。2、缺省拷贝构造函数。3、 缺省析构函数。4、缺省赋值运算符。5、缺省取址运算符。6、 缺省取址运算符 const。[cpp] view plain copy_空类默认产生哪些类成员函数