无人驾驶算法学习（九）：LeGo-LOAM激光雷达定位算法
LeGo-LOAM激光雷达定位算法源码阅读(一)
1.featureAssociation框架
1.1节点代码主体
int main(int argc, char** argv)
{
ros::init(argc, argv, "lego_loam");
ROS_INFO("\033[1;32m---->\033[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<ori<end
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;
}
1352
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
