文章目录
1. IMU 数据处理
1.1 IMU handler函数
看下面的代码，流程是：
（1）直接获得imu信息里的roll pitch yaw
（2）通过姿态，消除重力对线加速度的影响
（3）将信息存到一个queue里面
（4）调用AccumulateIMUShift();函数，完成imu3维移动的积分。
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
double roll, pitch, yaw;
tf::Quaternion orientation;
//convert Quaternion msg to Quaternion
tf::quaternionMsgToTF(imuIn->orientation, orientation);
//This will get the roll pitch and yaw from the matrix about fixed axes X, Y, Z respectively. That's R = Rz(yaw)*Ry(pitch)*Rx(roll).
//Here roll pitch yaw is in the global frame
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
//减去重力的影响,求出xyz方向的加速度实际值，并进行坐标轴交换，统一到z轴向前,x轴向左的右手坐标系, 交换过后RPY对应fixed axes ZXY(RPY---ZXY)。Now R = Ry(yaw)*Rx(pitch)*Rz(roll).
float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;
//循环移位效果，形成环形数组
imuPointerLast = (imuPointerLast + 1) % imuQueLength;
imuTime[imuPointerLast] = imuIn->header.stamp.toSec();
imuRoll[imuPointerLast] = roll;
imuPitch[imuPointerLast] = pitch;
imuYaw[imuPointerLast] = yaw;
imuAccX[imuPointerLast] = accX;
imuAccY[imuPointerLast] = accY;
imuAccZ[imuPointerLast] = accZ;
AccumulateIMUShift();
}
1.2 AccumulateIMUShift()函数
先把上面存的信息读出来
float roll = imuRoll[imuPointerLast];
float pitch = imuPitch[imuPointerLast];
float yaw = imuYaw[imuPointerLast];
float accX = imuAccX[imuPointerLast];
float accY = imuAccY[imuPointerLast];
float accZ = imuAccZ[imuPointerLast];
再将线加速度值投影到世界坐标系下
//将当前时刻的加速度值绕交换过的ZXY固定轴（原XYZ）分别旋转(roll, pitch, yaw)角，转换得到世界坐标系下的加速度值(right hand rule)
//绕z轴旋转(roll)
float x1 = cos(roll) * accX - sin(roll) * accY;
float y1 = sin(roll) * accX + cos(roll) * accY;
float z1 = accZ;
//绕x轴旋转(pitch)
float x2 = x1;
float y2 = cos(pitch) * y1 - sin(pitch) * z1;
float z2 = sin(pitch) * y1 + cos(pitch) * z1;
//绕y轴旋转(yaw)
accX = cos(yaw) * x2 + sin(yaw) * z2;
accY = y2;
accZ = -sin(yaw) * x2 + cos(yaw) * z2;
（1）将当前点的上一个点指针找到
（2）计算两个连续的数据之间的时间差
（3）时间差要小于某个阈值
（4）根据上一时刻的位置和速度，计算出当前时刻imu在世界坐标系下的位置和速度
//上一个imu点
int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;
//上一个点到当前点所经历的时间，即计算imu测量周期
double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];
//要求imu的频率至少比lidar高，这样的imu信息才使用，后面校正也才有意义
if (timeDiff < scanPeriod) {//（隐含从静止开始运动）
//求每个imu时间点的位移与速度,两点之间视为匀加速直线运动
imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff
+ accX * timeDiff * timeDiff / 2;
imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff
+ accY * timeDiff * timeDiff / 2;
imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff
+ accZ * timeDiff * timeDiff / 2;
imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;
imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;
imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;
}
2. 处理激光点云laserCloudHandler函数
忽略掉开始的20帧，完成初始化
if (!systemInited) {//丢弃前20个点云数据
systemInitCount++;
if (systemInitCount >= systemDelay) {
systemInited = true;
}
return;
}
进行一些数据结构的初始化
//记录每个scan有曲率的点的开始和结束索引
std::vector<int> scanStartInd(N_SCANS, 0);
std::vector<int> scanEndInd(N_SCANS, 0);
//当前点云时间
double timeScanCur = laserCloudMsg->header.stamp.toSec();
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
//消息转换成pcl数据存放
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector<int> indices;
//移除空点
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
计算一个扫描周期的起始角度和终止角度
并将角度控制在
//点云点的数量
int cloudSize = laserCloudIn.points.size();
//lidar scan开始点的旋转角,atan2范围[-pi,+pi],计算旋转角时取负号是因为velodyne是顺时针旋转
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
//lidar scan结束点的旋转角，加2*pi使点云旋转周期为2*pi
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;
//结束方位角与开始方位角差值控制在(PI,3*PI)范围，允许lidar不是一个圆周扫描
//正常情况下在这个范围内：pi < endOri - startOri < 3*pi，异常则修正
if (endOri - startOri > 3 * M_PI) {
endOri -= 2 * M_PI;
} else if (endOri - startOri < M_PI) {
endOri += 2 * M_PI;
}
一些变量的初始化
//lidar扫描线是否旋转过半
bool halfPassed = false;
int count = cloudSize;
PointType point;
然后开始对每一个点进行循环，首先交换坐标轴，进行统一
for (int i = 0; i < cloudSize; i++) {
//坐标轴交换，velodyne lidar的坐标系也转换到z轴向前，x轴向左的右手坐标系
point.x = laserCloudIn.points[i].y;
point.y = laserCloudIn.points[i].z;
point.z = laserCloudIn.points[i].x;
计算当前点的仰角，来确定对应的线数，也就是在第几线上进行扫描得到的当前点
//计算点的仰角(根据lidar文档垂直角计算公式),根据仰角排列激光线号，velodyne每两个scan之间间隔2度
float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;
int scanID;
//仰角四舍五入(加减0.5截断效果等于四舍五入)
int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5));
if (roundedAngle > 0){
scanID = roundedAngle;
}
else {
scanID = roundedAngle + (N_SCANS - 1);
}
//过滤点，只挑选[-15度，+15度]范围内的点,scanID属于[0,15]
if (scanID > (N_SCANS - 1) || scanID < 0 ){
count--;
continue;
}
计算当前点，在水平方向上的旋转角
//该点的旋转角
float ori = -atan2(point.x, point.z);
if (!halfPassed) {//根据扫描线是否旋转过半选择与起始位置还是终止位置进行差值计算，从而进行补偿
//确保-pi/2 < ori - startOri < 3*pi/2
if (ori < startOri - M_PI / 2) {
ori += 2 * M_PI;
} else if (ori > startOri + M_PI * 3 / 2) {
ori -= 2 * M_PI;
}
if (ori - startOri > M_PI) {
halfPassed = true;
}
} else {
ori += 2 * M_PI;
//确保-3*pi/2 < ori - endOri < pi/2
if (ori < endOri - M_PI * 3 / 2) {
ori += 2 * M_PI;
} else if (ori > endOri + M_PI / 2) {
ori -= 2 * M_PI;
}
}
通过旋转角占整体的比例，再加上线数，确定一个intensity的值，后面会用
//-0.5 < relTime < 1.5（点旋转的角度与整个周期旋转角度的比率, 即点云中点的相对时间）
float relTime = (ori - startOri) / (endOri - startOri);
//点强度=线号+点相对时间（即一个整数+一个小数，整数部分是线号，小数部分是该点的相对时间）,匀速扫描：根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间
point.intensity = scanID + scanPeriod * relTime;
如果收到了imu数据，就进行去畸变操作
先计算点相对于本扫描周期第一个点过去的时间
if (imuPointerLast >= 0) {//如果收到IMU数据,使用IMU矫正点云畸变
{
float pointTime = relTime * scanPeriod;//计算点的周期时间
.....
}
看看imu信息 的 front指针和last指针是不是相等
如果不相等就在imu的queue里面，向前移动front指针，直到，移到的位置对应的imu时间戳，刚好超过当前点的point time
如果都走到了last指针，也没有找到这么一个时间戳，就退出while循环
//寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFront
while (imuPointerFront != imuPointerLast) {
if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
break;
}
imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}
如果真的没有找到满足条件的front，说明，imu信息可能被阻塞了，以至于有新的点列进来，没有更新的imu进来。
此时，就用front指向的imu信息，来更新当前点云对应的imuCur信息
if (timeScanCur + pointTime > imuTime[imuPointerFront]) {//没找到,此时imuPointerFront==imtPointerLast,只能以当前收到的最新的IMU的速度，位移，欧拉角作为当前点的速度，位移，欧拉角使用
imuRollCur = imuRoll[imuPointerFront];
imuPitchCur = imuPitch[imuPointerFront];
imuYawCur = imuYaw[imuPointerFront];
imuVeloXCur = imuVeloX[imuPointerFront];
imuVeloYCur = imuVeloY[imuPointerFront];
imuVeloZCur = imuVeloZ[imuPointerFront];
imuShiftXCur = imuShiftX[imuPointerFront];
imuShiftYCur = imuShiftY[imuPointerFront];
imuShiftZCur = imuShiftZ[imuPointerFront];
如果找到了刚好超过点云时间戳的front指针，那么
（1）向后退一格，定义为back指针，这两个指针把当前的点云时间戳夹在中间
（2）进行线性插值，获得当前点云时间戳对应的imuCur信息
} else {//找到了点云时间戳小于IMU时间戳的IMU位置,则该点必处于imuPointerBack和imuPointerFront之间，据此线性插值，计算点云点的速度，位移和欧拉角
int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
//按时间距离计算权重分配比率,也即线性插值
float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;
} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;
} else {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
}
//本质:imuVeloXCur = imuVeloX[imuPointerback] + (imuVelX[imuPointerFront]-imuVelX[imuPoniterBack])*ratioFront
imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;
imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
}
然后在看看是不是第一个点（注意，是这个扫描周期的第一个点，不是全局的第一帧），如果是,就把当前点定义成start点，记录start信息
如果不是，就要进行相应的去畸变处理，有是三个函数，下面分别看
if (i == 0) {//如果是第一个点,记住点云起始位置的速度，位移，欧拉角
imuRollStart = imuRollCur;
imuPitchStart = imuPitchCur;
imuYawStart = imuYawCur;
imuVeloXStart = imuVeloXCur;
imuVeloYStart = imuVeloYCur;
imuVeloZStart = imuVeloZCur;
imuShiftXStart = imuShiftXCur;
imuShiftYStart = imuShiftYCur;
imuShiftZStart = imuShiftZCur;
} else {//计算之后每个点相对于第一个点的由于加减速非匀速运动产生的位移速度畸变，并对点云中的每个点位置信息重新补偿矫正
ShiftToStartIMU(pointTime);
VeloToStartIMU();
TransformToStartIMU(&point);
}
然后将畸变补偿后的点，放到点列当中
laserCloudScans[scanID].push_back(point);//将每个补偿矫正的点放入对应线号的容器
按照scan线数的顺序把点列添加到一个共同的数据结构当中
//获得有效范围内的点的数量
cloudSize = count;
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++) {//将所有的点按照线号从小到大放入一个容器
*laserCloud += laserCloudScans[i];
}
对这个数据结构中的每一个点进行循环（这里是一个scan一个scan循环的）
计算一下每个点的曲率情况，并记录下来，并把每一个scan开始和结束的id记录下来
int scanCount = -1;
for (int i = 5; i < cloudSize - 5; i++) {//使用每个点的前后五个点计算曲率，因此前五个与最后五个点跳过
float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x
+ laserCloud->points[i - 3].x + laserCloud->points[i - 2].x
+ laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x
+ laserCloud->points[i + 1].x + laserCloud->points[i + 2].x
+ laserCloud->points[i + 3].x + laserCloud->points[i + 4].x
+ laserCloud->points[i + 5].x;
float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y
+ laserCloud->points[i - 3].y + laserCloud->points[i - 2].y
+ laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y
+ laserCloud->points[i + 1].y + laserCloud->points[i + 2].y
+ laserCloud->points[i + 3].y + laserCloud->points[i + 4].y
+ laserCloud->points[i + 5].y;
float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z
+ laserCloud->points[i - 3].z + laserCloud->points[i - 2].z
+ laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z
+ laserCloud->points[i + 1].z + laserCloud->points[i + 2].z
+ laserCloud->points[i + 3].z + laserCloud->points[i + 4].z
+ laserCloud->points[i + 5].z;
//曲率计算
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
//记录曲率点的索引
cloudSortInd[i] = i;
//初始时，点全未筛选过
cloudNeighborPicked[i] = 0;
//初始化为less flat点
cloudLabel[i] = 0;
//每个scan，只有第一个符合的点会进来，因为每个scan的点都在一起存放
if (int(laserCloud->points[i].intensity) != scanCount) {
scanCount = int(laserCloud->points[i].intensity);//控制每个scan只进入第一个点
//曲率只取同一个scan计算出来的，跨scan计算的曲率非法，排除，也即排除每个scan的前后五个点
if (scanCount > 0 && scanCount < N_SCANS) {
scanStartInd[scanCount] = i + 5;
scanEndInd[scanCount - 1] = i - 5;
}
}
}
然后再开始对每个点进行循环，这一次是标记出点云中不太好的点，共有以下几类：
（1）变化非常突兀的点，容易产生遮挡，把相邻的容易被遮挡的点去掉
（2）与前后点的平方和都大于深度平方和的万分之二，这些点视为离群点，包括陡斜面上的点，强烈凸凹点和空旷区域中的某些点，置为筛选过，弃用
//第一个scan曲率点有效点序从第5个开始，最后一个激光线结束点序size-5
scanStartInd[0] = 5;
scanEndInd.back() = cloudSize - 5;
//挑选点，排除容易被斜面挡住的点以及离群点，有些点容易被斜面挡住，而离群点可能出现带有偶然性，这些情况都可能导致前后两次扫描不能被同时看到
for (int i = 5; i < cloudSize - 6; i++) {//与后一个点差值，所以减6
float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;
float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;
float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;
//计算有效曲率点与后一个点之间的距离平方和
float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;
if (diff > 0.1) {//前提:两个点之间距离要大于0.1
//点的深度
float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x +
laserCloud->points[i].y * laserCloud->points[i].y +
laserCloud->points[i].z * laserCloud->points[i].z);
//后一个点的深度
float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x +
laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +
laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);
//按照两点的深度的比例，将深度较大的点拉回后计算距离
if (depth1 > depth2) {
diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1;
diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1;
diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1;
//边长比也即是弧度值，若小于0.1，说明夹角比较小，斜面比较陡峭,点深度变化比较剧烈,点处在近似与激光束平行的斜面上
if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {//排除容易被斜面挡住的点
//该点及前面五个点（大致都在斜面上）全部置为筛选过
cloudNeighborPicked[i - 5] = 1;
cloudNeighborPicked[i - 4] = 1;
cloudNeighborPicked[i - 3] = 1;
cloudNeighborPicked[i - 2] = 1;
cloudNeighborPicked[i - 1] = 1;
cloudNeighborPicked[i] = 1;
}
} else {
diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x;
diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y;
diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z;
if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) {
cloudNeighborPicked[i + 1] = 1;
cloudNeighborPicked[i + 2] = 1;
cloudNeighborPicked[i + 3] = 1;
cloudNeighborPicked[i + 4] = 1;
cloudNeighborPicked[i + 5] = 1;
cloudNeighborPicked[i + 6] = 1;
}
}
}
float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;
float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;
float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;
//与前一个点的距离平方和
float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;
//点深度的平方和
float dis = laserCloud->points[i].x * laserCloud->points[i].x
+ laserCloud->points[i].y * laserCloud->points[i].y
+ laserCloud->points[i].z * laserCloud->points[i].z;
//与前后点的平方和都大于深度平方和的万分之二，这些点视为离群点，包括陡斜面上的点，强烈凸凹点和空旷区域中的某些点，置为筛选过，弃用
if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) {
cloudNeighborPicked[i] = 1;
}
}
for 循环，对每一个 线进行循环
for 循环 每个线等分6份
将每份里面的点按曲率大小排序
挑出其中曲率大的点放到sharp里，相对大的放到less sharp里
删除掉挑出点相邻的距离过近的点
挑出其中曲率较小的点放到flat 和less flat里面
删除附近点
2.1 ShiftToStartIMU(pointTime)函数
先计算imu当前位置相对于本周期第一个点的坐标系发生了多少平移运动
== 这里有问题吧，是不是只要前两项就行，因为所有的运动都已经积分过了 ==
//计算相对于第一个点由于加减速产生的畸变位移(全局坐标系下畸变位移量delta_Tg)
//imuShiftFromStartCur = imuShiftCur - (imuShiftStart + imuVeloStart * pointTime)
imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime;
imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime;
imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime;
然后把这一段变化向量，放到第一个点的start坐标系下
/********************************************************************************
Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Tg
transfrom from the global frame to the local frame
*********************************************************************************/
//绕y轴旋转(-imuYawStart)，即Ry(yaw).inverse
float x1 = cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur;
float y1 = imuShiftFromStartYCur;
float z1 = sin(imuYawStart) * imuShiftFromStartXCur + cos(imuYawStart) * imuShiftFromStartZCur;
//绕x轴旋转(-imuPitchStart)，即Rx(pitch).inverse
float x2 = x1;
float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;
//绕z轴旋转(-imuRollStart)，即Rz(pitch).inverse
imuShiftFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;
imuShiftFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;
imuShiftFromStartZCur = z2;
2.2 VeloToStartIMU()函数
这个是把相对于第一个点的平移速度变化量，求出来，并放到start坐标系下
//计算相对于第一个点由于加减速产生的畸变速度(全局坐标系下畸变速度增量delta_Vg)
imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;
imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;
imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;
/********************************************************************************
Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Vg
transfrom from the global frame to the local frame
*********************************************************************************/
//绕y轴旋转(-imuYawStart)，即Ry(yaw).inverse
float x1 = cos(imuYawStart) * imuVeloFromStartXCur - sin(imuYawStart) * imuVeloFromStartZCur;
float y1 = imuVeloFromStartYCur;
float z1 = sin(imuYawStart) * imuVeloFromStartXCur + cos(imuYawStart) * imuVeloFromStartZCur;
//绕x轴旋转(-imuPitchStart)，即Rx(pitch).inverse
float x2 = x1;
float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;
//绕z轴旋转(-imuRollStart)，即Rz(pitch).inverse
imuVeloFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;
imuVeloFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;
imuVeloFromStartZCur = z2;
2.3 TransformToStartIMU(&point)函数
在这段代码中，先把在Cur坐标系下的点，变换到全局坐标系下（旋转），然后再变换到start坐标系下，然后再增加以下相对于start坐标系的偏移量就行
/********************************************************************************
Ry*Rx*Rz*Pl, transform point to the global frame
*********************************************************************************/
//绕z轴旋转(imuRollCur)
float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y;
float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y;
float z1 = p->z;
//绕x轴旋转(imuPitchCur)
float x2 = x1;
float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;
float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;
//绕y轴旋转(imuYawCur)
float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;
float y3 = y2;
float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;
/********************************************************************************
Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * Pg
transfrom global points to the local frame
*********************************************************************************/
//绕y轴旋转(-imuYawStart)
float x4 = cos(imuYawStart) * x3 - sin(imuYawStart) * z3;
float y4 = y3;
float z4 = sin(imuYawStart) * x3 + cos(imuYawStart) * z3;
//绕x轴旋转(-imuPitchStart)
float x5 = x4;
float y5 = cos(imuPitchStart) * y4 + sin(imuPitchStart) * z4;
float z5 = -sin(imuPitchStart) * y4 + cos(imuPitchStart) * z4;
//绕z轴旋转(-imuRollStart)，然后叠加平移量
p->x = cos(imuRollStart) * x5 + sin(imuRollStart) * y5 + imuShiftFromStartXCur;
p->y = -sin(imuRollStart) * x5 + cos(imuRollStart) * y5 + imuShiftFromStartYCur;
p->z = z5 + imuShiftFromStartZCur;
扫一扫
3万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
