当前位置: 首页 > >

LeGo-LOAM激光雷达定位算法源码阅读(二)

发布时间:



文章目录
1.featureAssociation框架1.1节点代码主体1.2 FeatureAssociation构造函数1.3 runFeatureAssociation()主体函数
2.重要函数2.1 laserCloudHandler2.2 laserCloudInfoHandler2.3 outlierCloudHandler2.4 imuHandler2.5 adjustDistortion2.6 calculateSmoothness2.7 markOccludedPoints2.8 extractFeatures2.9 updateInitialGuess2.10 updateTransformation2.11 integrateTransformation



无人驾驶算法学*(九):LeGo-LOAM激光雷达定位算法
LeGo-LOAM激光雷达定位算法源码阅读(一)


1.featureAssociation框架
1.1节点代码主体

int main(int argc, char** argv)
{
ros::init(argc, argv, "lego_loam");

ROS_INFO("33[1;32m---->33[0m Feature Association Started.");

FeatureAssociation FA;//调用构造函数

ros::Rate rate(200);
while (ros::ok())
{
ros::spinOnce();

FA.runFeatureAssociation();//200hz调用主体函数

rate.sleep();
}

ros::spin();
return 0;
}

1.2 FeatureAssociation构造函数

作用:订阅分割点云和imu数据,存成类成员变量,为runFeatureAssociation()处理新数据做准备


//订阅话题:

"/segmented_cloud"(sensor_msgs::PointCloud2),数据处理函数laserCloudHandler
"/segmented_cloud_info"(cloud_msgs::cloud_info),数据处理函数laserCloudInfoHandler
"/outlier_cloud"(sensor_msgs::PointCloud2),数据处理函数outlierCloudHandler
imuTopic = "/imu/data"(sensor_msgs::Imu),数据处理函数imuHandler

//发布话题,这些topic有:

"/laser_cloud_sharp"(sensor_msgs::PointCloud2)
"/laser_cloud_less_sharp"(sensor_msgs::PointCloud2)
"/laser_cloud_flat"(sensor_msgs::PointCloud2)
"/laser_cloud_less_flat"(sensor_msgs::PointCloud2)
"/laser_cloud_corner_last"(sensor_msgs::PointCloud2)
"/laser_cloud_surf_last"(cloud_msgs::cloud_info)
"/outlier_cloud_last"(sensor_msgs::PointCloud2)
"/laser_odom_to_init"(nav_msgs::Odometry)


1.3 runFeatureAssociation()主体函数

作用:特征提取和配准的主体



void runFeatureAssociation()
{
// 如果有新数据进来则执行,否则不执行任何操作
if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05){

newSegmentedCloud = false;
newSegmentedCloudInfo = false;
newOutlierCloud = false;
}else{
return;
}

// 点云形状调整
//主要进行的处理是将点云数据进行坐标变换,进行插补等工作
adjustDistortion();

// 计算曲率,并保存结果
calculateSmoothness();

// 标记瑕点,loam中:1.*行scan的点(可能看不见);2.会被遮挡的点
markOccludedPoints();

// 特征提取
// 点云分类,然后分别保存到cornerPointsSharp等等队列
// 这一步中减少了点云数量,使计算量减少
extractFeatures();

// 发布cornerPointsSharp等4种类型的点云数据
publishCloud();

if (!systemInitedLM) {
checkSystemInitialization();
return;
}

// 预测位姿
updateInitialGuess();

// 更新变换
updateTransformation();

// 积分总变换
integrateTransformation();

publishOdometry();

publishCloudsLast();
}

2.重要函数
2.1 laserCloudHandler

订阅 "/segmented_cloud"的回调函数
作用:laserCloudHandler修改点云数据的时间戳,将点云数据从ROS定义的格式转化到pcl的格式。
pcl::fromROSMsg(*laserCloudMsg, *segmentedCloud);


2.2 laserCloudInfoHandler

订阅 "/segmented_cloud_info"的触发回调函数laserCloudInfoHandler
newSegmentedCloudInfo = true;
作用:为runFeatureAssociation()做准备


2.3 outlierCloudHandler

订阅 "/outlier_cloud"的回调函数
pcl::fromROSMsg(*msgIn, *outlierCloud); newOutlierCloud = true;
作用:为runFeatureAssociation()做准备


2.4 imuHandler

LOAM代码里的那个重要函数


void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
//通过接收到的imuIn里面的四元素得到roll,pitch,yaw三个角
double roll, pitch, yaw;
tf::Quaternion orientation;
tf::quaternionMsgToTF(imuIn->orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

// 加速度去除重力影响,同时坐标轴进行变换
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;

imuAngularVeloX[imuPointerLast] = imuIn->angular_velocity.x;
imuAngularVeloY[imuPointerLast] = imuIn->angular_velocity.y;
imuAngularVeloZ[imuPointerLast] = imuIn->angular_velocity.z;

//对速度,角速度,加速度进行积分,得到位移,角度和速度
AccumulateIMUShiftAndRotation();
}

2.5 adjustDistortion

void adjustDistortion()
{
bool halfPassed = false;
int cloudSize = segmentedCloud->points.size();

PointType point;

for (int i = 0; i < cloudSize; i++) {
// 这里xyz与laboshin_loam代码中的一样经过坐标轴变换
// imuhandler() 中相同的坐标变换
point.x = segmentedCloud->points[i].y;
point.y = segmentedCloud->points[i].z;
point.z = segmentedCloud->points[i].x;

// -atan2(p.x,p.z)==>-atan2(y,x)
// ori表示的是偏航角yaw,因为前面有负号,ori=[-M_PI,M_PI)
// 因为segInfo.orientationDiff表示的范围是(PI,3PI),在2PI附*
// 下面过程的主要作用是调整ori大小,满足start float ori = -atan2(point.x, point.z);
if (!halfPassed) {
if (ori < segInfo.startOrientation - M_PI / 2)
// start-ori>M_PI/2,说明ori小于start,不合理,
// 正常情况在前半圈的话,ori-stat范围[0,M_PI]
ori += 2 * M_PI;
else if (ori > segInfo.startOrientation + M_PI * 3 / 2)
// ori-start>3/2*M_PI,说明ori太大,不合理
ori -= 2 * M_PI;

if (ori - segInfo.startOrientation > M_PI)
halfPassed = true;
} else {
ori += 2 * M_PI;

if (ori < segInfo.endOrientation - M_PI * 3 / 2)
// end-ori>3/2*PI,ori太小
ori += 2 * M_PI;
else if (ori > segInfo.endOrientation + M_PI / 2)
// ori-end>M_PI/2,太大
ori -= 2 * M_PI;
}

// 用 point.intensity 来保存时间
float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;
point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;

if (imuPointerLast >= 0) {
float pointTime = relTime * scanPeriod;
imuPointerFront = imuPointerLastIteration;
// while循环内进行时间轴对齐
while (imuPointerFront != imuPointerLast) {
if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
break;
}
imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}

if (timeScanCur + pointTime > imuTime[imuPointerFront]) {
// 该条件内imu数据比激光数据早,但是没有更后面的数据
// (打个比方,激光在9点时出现,imu现在只有8点的)
// 这种情况上面while循环是以imuPointerFront == imuPointerLast结束的
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];
} else {
// 在imu数据充足的情况下可以进行插补
// 当前timeScanCur + pointTime < imuTime[imuPointerFront],
// 而且imuPointerFront是最早一个时间大于timeScanCur + pointTime的imu数据指针
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]);

// 通过上面计算的ratioFront以及ratioBack进行插补
// 因为imuRollCur和imuPitchCur通常都在0度左右,变化不会很大,因此不需要考虑超过2M_PI的情况
// imuYaw转的角度比较大,需要考虑超过2*M_PI的情况
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;
}

// imu速度插补
imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;

// imu位置插补
imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
}

if (i == 0) {
// 此处更新过的角度值主要用在updateImuRollPitchYawStartSinCos()中,
// 更新每个角的正余弦值
imuRollStart = imuRollCur;
imuPitchStart = imuPitchCur;
imuYawStart = imuYawCur;

imuVeloXStart = imuVeloXCur;
imuVeloYStart = imuVeloYCur;
imuVeloZStart = imuVeloZCur;

imuShiftXStart = imuShiftXCur;
imuShiftYStart = imuShiftYCur;
imuShiftZStart = imuShiftZCur;

if (timeScanCur + pointTime > imuTime[imuPointerFront]) {
// 该条件内imu数据比激光数据早,但是没有更后面的数据
imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];
imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];
imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];
}else{
// 在imu数据充足的情况下可以进行插补
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]);
imuAngularRotationXCur = imuAngularRotationX[imuPointerFront] * ratioFront + imuAngularRotationX[imuPointerBack] * ratioBack;
imuAngularRotationYCur = imuAngularRotationY[imuPointerFront] * ratioFront + imuAngularRotationY[imuPointerBack] * ratioBack;
imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront] * ratioFront + imuAngularRotationZ[imuPointerBack] * ratioBack;
}

// 距离上一次插补,旋转过的角度变化值
imuAngularFromStartX = imuAngularRotationXCur - imuAngularRotationXLast;
imuAngularFromStartY = imuAngularRotationYCur - imuAngularRotationYLast;
imuAngularFromStartZ = imuAngularRotationZCur - imuAngularRotationZLast;

imuAngularRotationXLast = imuAngularRotationXCur;
imuAngularRotationYLast = imuAngularRotationYCur;
imuAngularRotationZLast = imuAngularRotationZCur;

// 这里更新的是i=0时刻的rpy角,后面将速度坐标投影过来会用到i=0时刻的值
updateImuRollPitchYawStartSinCos();
} else {
// 速度投影到初始i=0时刻
VeloToStartIMU();
// 将点的坐标变换到初始i=0时刻
TransformToStartIMU(&point);
}
}
segmentedCloud->points[i] = point;
}

imuPointerLastIteration = imuPointerLast;
}

2.6 calculateSmoothness

// 计算光滑性,这里的计算没有完全按照公式进行,
// 缺少除以总点数i和r[i]
void calculateSmoothness()
{
int cloudSize = segmentedCloud->points.size();
for (int i = 5; i < cloudSize - 5; i++) {

float diffRange = segInfo.segmentedCloudRange[i-5] + segInfo.segmentedCloudRange[i-4]
+ segInfo.segmentedCloudRange[i-3] + segInfo.segmentedCloudRange[i-2]
+ segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i] * 10
+ segInfo.segmentedCloudRange[i+1] + segInfo.segmentedCloudRange[i+2]
+ segInfo.segmentedCloudRange[i+3] + segInfo.segmentedCloudRange[i+4]
+ segInfo.segmentedCloudRange[i+5];

cloudCurvature[i] = diffRange*diffRange;

// 在markOccludedPoints()函数中对该参数进行重新修改
cloudNeighborPicked[i] = 0;
// 在extractFeatures()函数中会对标签进行修改,
// 初始化为0,surfPointsFlat标记为-1,surfPointsLessFlatScan为不大于0的标签
// cornerPointsSharp标记为2,cornerPointsLessSharp标记为1
cloudLabel[i] = 0;

cloudSmoothness[i].value = cloudCurvature[i];
cloudSmoothness[i].ind = i;
}
}

2.7 markOccludedPoints

// 阻塞点指点云之间相互遮挡,而且又靠得很*的点
void markOccludedPoints()
{
int cloudSize = segmentedCloud->points.size();

for (int i = 5; i < cloudSize - 6; ++i){

float depth1 = segInfo.segmentedCloudRange[i];
float depth2 = segInfo.segmentedCloudRange[i+1];
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[i+1] - segInfo.segmentedCloudColInd[i]));

if (columnDiff < 10){

// 选择距离较远的那些点,并将他们标记为1
if (depth1 - depth2 > 0.3){
cloudNeighborPicked[i - 5] = 1;
cloudNeighborPicked[i - 4] = 1;
cloudNeighborPicked[i - 3] = 1;
cloudNeighborPicked[i - 2] = 1;
cloudNeighborPicked[i - 1] = 1;
cloudNeighborPicked[i] = 1;
}else if (depth2 - depth1 > 0.3){
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 diff1 = std::abs(segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i]);
float diff2 = std::abs(segInfo.segmentedCloudRange[i+1] - segInfo.segmentedCloudRange[i]);

// 选择距离变化较大的点,并将他们标记为1
if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] && diff2 > 0.02 * segInfo.segmentedCloudRange[i])
cloudNeighborPicked[i] = 1;
}
}

2.8 extractFeatures

void extractFeatures()
{
cornerPointsSharp->clear();
cornerPointsLessSharp->clear();
surfPointsFlat->clear();
surfPointsLessFlat->clear();

for (int i = 0; i < N_SCAN; i++) {

surfPointsLessFlatScan->clear();

for (int j = 0; j < 6; j++) {

// sp和ep的含义是什么???startPointer,endPointer?
int sp = (segInfo.startRingIndex[i] * (6 - j) + segInfo.endRingIndex[i] * j) / 6;
int ep = (segInfo.startRingIndex[i] * (5 - j) + segInfo.endRingIndex[i] * (j + 1)) / 6 - 1;

if (sp >= ep)
continue;

// 按照cloudSmoothness.value从小到大排序
std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());

int largestPickedNum = 0;
for (int k = ep; k >= sp; k--) {
// 每次ind的值就是等于k??? 有什么意义?
// 因为上面对cloudSmoothness进行了一次从小到大排序,所以ind不一定等于k了
int ind = cloudSmoothness[k].ind;
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > edgeThreshold &&
segInfo.segmentedCloudGroundFlag[ind] == false) {

largestPickedNum++;
if (largestPickedNum <= 2) {
// 论文中nFe=2,cloudSmoothness已经按照从小到大的顺序排列,
// 所以这边只要选择最后两个放进队列即可
// cornerPointsSharp标记为2
cloudLabel[ind] = 2;
cornerPointsSharp->push_back(segmentedCloud->points[ind]);
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
} else if (largestPickedNum <= 20) {
// 塞20个点到cornerPointsLessSharp中去
// cornerPointsLessSharp标记为1
cloudLabel[ind] = 1;
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
} else {
break;
}

cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
// 从ind+l开始后面5个点,每个点index之间的差值,
// 确保columnDiff<=10,然后标记为我们需要的点
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
// 从ind+l开始前面五个点,计算差值然后标记
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
}
}

int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++) {
int ind = cloudSmoothness[k].ind;
// *面点只从地面点中进行选择? 为什么要这样做?
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < surfThreshold &&
segInfo.segmentedCloudGroundFlag[ind] == true) {

cloudLabel[ind] = -1;
surfPointsFlat->push_back(segmentedCloud->points[ind]);

// 论文中nFp=4,将4个最*的*面点放入队列中
smallestPickedNum++;
if (smallestPickedNum >= 4) {
break;
}

cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
// 从前面往后判断是否是需要的邻接点,是的话就进行标记
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));
if (columnDiff > 10)
break;

cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
// 从后往前开始标记
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
if (columnDiff > 10)
break;

cloudNeighborPicked[ind + l] = 1;
}
}
}

for (int k = sp; k <= ep; k++) {
if (cloudLabel[k] <= 0) {
surfPointsLessFlatScan->push_back(segmentedCloud->points[k]);
}
}
}

// surfPointsLessFlatScan中有过多的点云,如果点云太多,计算量太大
// 进行下采样,可以大大减少计算量
surfPointsLessFlatScanDS->clear();
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.filter(*surfPointsLessFlatScanDS);

*surfPointsLessFlat += *surfPointsLessFlatScanDS;
}
}


2.9 updateInitialGuess

void updateInitialGuess(){

imuPitchLast = imuPitchCur;
imuYawLast = imuYawCur;
imuRollLast = imuRollCur;

imuShiftFromStartX = imuShiftFromStartXCur;
imuShiftFromStartY = imuShiftFromStartYCur;
imuShiftFromStartZ = imuShiftFromStartZCur;

imuVeloFromStartX = imuVeloFromStartXCur;
imuVeloFromStartY = imuVeloFromStartYCur;
imuVeloFromStartZ = imuVeloFromStartZCur;

// 关于下面负号的说明:
// transformCur是在Cur坐标系下的 p_start=R*p_cur+t
// R和t是在Cur坐标系下的
// 而imuAngularFromStart是在start坐标系下的,所以需要加负号
if (imuAngularFromStartX != 0 || imuAngularFromStartY != 0 || imuAngularFromStartZ != 0){
transformCur[0] = - imuAngularFromStartY;
transformCur[1] = - imuAngularFromStartZ;
transformCur[2] = - imuAngularFromStartX;
}

// 速度乘以时间,当前变换中的位移
if (imuVeloFromStartX != 0 || imuVeloFromStartY != 0 || imuVeloFromStartZ != 0){
transformCur[3] -= imuVeloFromStartX * scanPeriod;
transformCur[4] -= imuVeloFromStartY * scanPeriod;
transformCur[5] -= imuVeloFromStartZ * scanPeriod;
}
}

2.10 updateTransformation

void updateTransformation(){

if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)
return;

for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {
laserCloudOri->clear();
coeffSel->clear();

// 找到对应的特征*面
// 然后计算协方差矩阵,保存在coeffSel队列中
// laserCloudOri中保存的是对应于coeffSel的未转换到开始时刻的原始点云数据
findCorrespondingSurfFeatures(iterCount1);

if (laserCloudOri->points.size() < 10)
continue;
// 通过面特征的匹配,计算变换矩阵
if (calculateTransformationSurf(iterCount1) == false)
break;
}

for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {

laserCloudOri->clear();
coeffSel->clear();

// 找到对应的特征边/角点
// 寻找边特征的方法和寻找*面特征的很类似,过程可以参照寻找*面特征的注释
findCorrespondingCornerFeatures(iterCount2);

if (laserCloudOri->points.size() < 10)
continue;
// 通过角/边特征的匹配,计算变换矩阵
if (calculateTransformationCorner(iterCount2) == false)
break;
}
}

2.11 integrateTransformation

// 旋转角的累计变化量
void integrateTransformation(){
float rx, ry, rz, tx, ty, tz;

// AccumulateRotation作用
// 将计算的两帧之间的位姿“累加”起来,获得相对于第一帧的旋转矩阵
// transformSum + (-transformCur) =(rx,ry,rz)
AccumulateRotation(transformSum[0], transformSum[1], transformSum[2],
-transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);

// 进行*移分量的更新
float x1 = cos(rz) * (transformCur[3] - imuShiftFromStartX)
- sin(rz) * (transformCur[4] - imuShiftFromStartY);
float y1 = sin(rz) * (transformCur[3] - imuShiftFromStartX)
+ cos(rz) * (transformCur[4] - imuShiftFromStartY);
float z1 = transformCur[5] - imuShiftFromStartZ;

float x2 = x1;
float y2 = cos(rx) * y1 - sin(rx) * z1;
float z2 = sin(rx) * y1 + cos(rx) * z1;

tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
ty = transformSum[4] - y2;
tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);

// 与accumulateRotatio联合起来更新transformSum的rotation部分的工作
// 可视为transformToEnd的下部分的逆过程
PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart,
imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);

transformSum[0] = rx;
transformSum[1] = ry;
transformSum[2] = rz;
transformSum[3] = tx;
transformSum[4] = ty;
transformSum[5] = tz;
}



友情链接: