LIO-SAM：在高斯牛顿法求解过程中用SO3代替欧拉角

LIO-SAM发表于IROS2020，是一个效果非常好的惯性-激光紧耦合里程计
我打算给我们的机器人搞一个激光里程计，于是打算把LIO-SAM改一改搞过来
修改过程中发现一个问题：在里程计求解(mapOptimization的LMOptimization函数)过程中，作者是用欧拉角来表示位姿的旋转部分的
- 这对于移动机器人来说还算可行，因为一般情况下地面移动机器人的pitch不会到达90°，也就不会引起死锁
- 但我们的机器人是攀爬机器人，经常出现翻转的动作，因此不能使用欧拉角来表示旋转；因此，需要对这部分做一定修改
- 修改思路：“用SO3加平移”的方式来表示位姿
先看看原代码整体思路：

void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn){// extract time stamptimeLaserInfoStamp = msgIn->header.stamp;timeLaserInfoCur = msgIn->header.stamp.toSec();// extract info and feature cloudcloudInfo = *msgIn;pcl::fromROSMsg(msgIn->cloud_corner,  *laserCloudCornerLast);pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);std::lock_guard<std::mutex> lock(mtx);static double timeLastProcessing = -1;if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval){timeLastProcessing = timeLaserInfoCur;updateInitialGuess();extractSurroundingKeyFrames();downsampleCurrentScan();scan2MapOptimization();saveKeyFramesAndFactor();correctPoses();publishOdometry();publishFrames();}}

翻译下，整个mapOptimization的大致流程就是：
- updateInitialGuess：将IMU的预测结果设为优化初值
- extractSurroundingKeyFrames：提取与当前帧相邻的关键帧点云，组成一个局部地图
- downsampleCurrentScan：降采样咯，分别对corner和surface特征集合降采样
- scan2MapOptimization：重点！！利用高斯牛顿法求解当前帧的位姿
- saveKeyFramesAndFactor：判断是否需要插入新的关键帧；给iSAM加入新的因子，进行进一步优化(这个优化的效果主要体现在长距离、长时间、有回环情况下的误差抑制)
- correctPoses：与IMU预测的pitch,roll进行融合
那么，我要改的主要是scan2MapOptimization这个部分，接下来看一看scan2MapOptimization里面的主要流程：

    void scan2MapOptimization(){if (cloudKeyPoses3D->points.empty())return;if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum){kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);for (int iterCount = 0; iterCount < 30; iterCount++){laserCloudOri->clear();coeffSel->clear();cornerOptimization();surfOptimization();combineOptimizationCoeffs();if (LMOptimization(iterCount) == true)break;              }transformUpdate(); } else {ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);}}

翻译下：
- cornerOptimization, surfOptimization里进行了特征匹配、残差计算以及对平移部分的雅克比计算(旋转部分的雅克比可由平移部分的雅克比进一步计算得到)
- combineOptimizationCoeffs是将上面两个函数计算的残差与雅克比的部分计算合并在一起，以便在LMOptimization里面用同一个for来计算迭代步长
- LMOptimization里面计算对旋转部分的雅克比，并用高斯牛顿法计算一次迭代步长、更新位姿
- 整个scan2MapOptimization的内容就是迭代优化30次，每次迭代都会重新匹配特征
好了，那么我具体要改哪些函数？
- 首要明确：我要改的是在优化过程中，状态变量的表示方式
- 优化迭代公式如下：J^T*J*δx = - J^T * f(x) ，状态变量的表示方式会影响雅克比的计算；本文修改的是旋转的表示方式，因此涉及旋转雅克比计算的函数都要进行修改，状态更新的地方也要修改
- 因此，主要修改这三个函数：cornerOptimization, surfOptimization，LMOptimization
先看surfOptimization:

void surfOptimization()
{// 更新当前帧位姿updatePointAssociateToMap();// 遍历当前帧平面点集合#pragma omp parallel for num_threads(numberOfCores)for (int i = 0; i < laserCloudSurfLastDSNum; i++){PointType pointOri, pointSel, coeff;std::vector<int> pointSearchInd;std::vector<float> pointSearchSqDis;// 平面点（坐标还是lidar系）pointOri = laserCloudSurfLastDS->points[i];// 根据当前帧位姿，变换到世界坐标系（map系）下pointAssociateToMap(&pointOri, &pointSel); // 在局部平面点map中查找当前平面点相邻的5个平面点kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);Eigen::Matrix<float, 5, 3> matA0;Eigen::Matrix<float, 5, 1> matB0;Eigen::Vector3f matX0;matA0.setZero();matB0.fill(-1);matX0.setZero();// 要求距离都小于1mif (pointSearchSqDis[4] < 1.0) {for (int j = 0; j < 5; j++) {matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;}// 假设平面方程为ax+by+cz+1=0，这里就是求方程的系数abc，d=1matX0 = matA0.colPivHouseholderQr().solve(matB0);// 平面方程的系数，也是法向量的分量float pa = matX0(0, 0);float pb = matX0(1, 0);float pc = matX0(2, 0);float pd = 1;// 单位法向量float ps = sqrt(pa * pa + pb * pb + pc * pc);pa /= ps; pb /= ps; pc /= ps; pd /= ps;// 检查平面是否合格，如果5个点中有点到平面的距离超过0.2m，那么认为这些点太分散了，不构成平面bool planeValid = true;for (int j = 0; j < 5; j++) {if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {planeValid = false;break;}}// 平面合格if (planeValid) {// 当前激光帧点到平面距离float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));// 点到平面垂线单位法向量（其实等价于平面法向量）coeff.x = s * pa;coeff.y = s * pb;coeff.z = s * pc;// 点到平面距离coeff.intensity = s * pd2;if (s > 0.1) {// 当前激光帧平面点，加入匹配集合中laserCloudOriSurfVec[i] = pointOri;// 参数coeffSelSurfVec[i] = coeff;laserCloudOriSurfFlag[i] = true;}}}}
}

总结下：

先把当前特征点转到map坐标系下，用kd树找map中与此点相邻的5个平面点
用这五个平面点拟合一个平面，并检查这个平面质量是否足够好
如果平面质量够好，就计算点到面的残差，然后根据这个点到面的距离，给这个残差项分配一个可信度因子(距离被匹配平面远的点可信度没那么高)；如果可信度因子足够大，则将这个点放到匹配集合里去
这里要注意一下coeff变量，它的xyz保存的是(乘了一个可信度因子的)这个面的法向量，intensity保存的是(乘了一个可信度因子的)点到这个面的距离
这里要注意一下：按照点面特征的雅克比计算公式("SO3加平移"版本)，点面残差对平移部分的雅克比就是这个面的单位法向量转置，而对旋转部分的雅克比也只是在这个基础上右乘一个-Rp(当前旋转估计乘以当前点在雷达坐标系下的表示)的反对称矩阵而已
对旋转部分的雅克比计算是在LMOptimization中完成的，在LMOptimization之前我们就要完成-Rp的计算了
修改后的代码：

    Eigen::Vector3d CulRp(PointType const * const pi) {return Eigen::Vector3d(transPointAssociateToMap(0,0) * pi->x + transPointAssociateToMap(0,1) * pi->y + transPointAssociateToMap(0,2) * pi->z,transPointAssociateToMap(1,0) * pi->x + transPointAssociateToMap(1,1) * pi->y + transPointAssociateToMap(1,2) * pi->z,transPointAssociateToMap(2,0) * pi->x + transPointAssociateToMap(2,1) * pi->y + transPointAssociateToMap(2,2) * pi->z);
}void surfOptimization()
{updatePointAssociateToMap();#pragma omp parallel for num_threads(numberOfCores)for (int i = 0; i < laserCloudSurfLastDSNum; i++){PointType pointOri, pointSel, coeff;std::vector<int> pointSearchInd;std::vector<float> pointSearchSqDis;pointOri = laserCloudSurfLastDS->points[i];pointAssociateToMap(&pointOri, &pointSel); kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);Eigen::Matrix<float, 5, 3> matA0;Eigen::Matrix<float, 5, 1> matB0;Eigen::Vector3f matX0;matA0.setZero();matB0.fill(-1);matX0.setZero();if (pointSearchSqDis[4] < 1.0) {for (int j = 0; j < 5; j++) {matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;}matX0 = matA0.colPivHouseholderQr().solve(matB0);float pa = matX0(0, 0);float pb = matX0(1, 0);float pc = matX0(2, 0);float pd = 1;float ps = sqrt(pa * pa + pb * pb + pc * pc);pa /= ps; pb /= ps; pc /= ps; pd /= ps;bool planeValid = true;for (int j = 0; j < 5; j++) {if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {planeValid = false;break;}}if (planeValid) {float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));coeff.x = s * pa;coeff.y = s * pb;coeff.z = s * pc;coeff.intensity = s * pd2;if (s > 0.1) {laserCloudOriSurfVec[i] = pointOri;coeffSelSurfVec[i] = coeff;laserCloudOriSurfFlag[i] = true;surfRp[i] = (CulRp(&pointOri)); //    新增的类成员变量， std::vector<Eigen::Vector3d> surfRp; // for jacobian orientation}}}}
}

其实就加了一行surfRp[i] = (CulRp(&pointOri)); // 新增的类成员变量， std::vector<Eigen::Vector3d> surfRp; // for jacobian orientation。。。
cornerOptimization的修改思路与上面差不多，但线特征的雅克比计算有所不同：

+ 放一放原码：

    void cornerOptimization(){updatePointAssociateToMap();#pragma omp parallel for num_threads(numberOfCores)for (int i = 0; i < laserCloudCornerLastDSNum; i++){PointType pointOri, pointSel, coeff;std::vector<int> pointSearchInd;std::vector<float> pointSearchSqDis;pointOri = laserCloudCornerLastDS->points[i];pointAssociateToMap(&pointOri, &pointSel);kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));if (pointSearchSqDis[4] < 1.0) {float cx = 0, cy = 0, cz = 0;for (int j = 0; j < 5; j++) {cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;}cx /= 5; cy /= 5;  cz /= 5;float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;for (int j = 0; j < 5; j++) {float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;a11 += ax * ax; a12 += ax * ay; a13 += ax * az;a22 += ay * ay; a23 += ay * az;a33 += az * az;}a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;cv::eigen(matA1, matD1, matV1);if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {float x0 = pointSel.x;float y0 = pointSel.y;float z0 = pointSel.z;float x1 = cx + 0.1 * matV1.at<float>(0, 0);float y1 = cy + 0.1 * matV1.at<float>(0, 1);float z1 = cz + 0.1 * matV1.at<float>(0, 2);float x2 = cx - 0.1 * matV1.at<float>(0, 0);float y2 = cy - 0.1 * matV1.at<float>(0, 1);float z2 = cz - 0.1 * matV1.at<float>(0, 2);// a012其实就是((p_i - p_a) x (p_i - p_b)).normfloat a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2)); // (pa - pb).normfloat la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;float ld2 = a012 / l12;float s = 1 - 0.9 * fabs(ld2);coeff.x = s * la;coeff.y = s * lb;coeff.z = s * lc;coeff.intensity = s * ld2;if (s > 0.1) {laserCloudOriCornerVec[i] = pointOri;coeffSelCornerVec[i] = coeff;laserCloudOriCornerFlag[i] = true;cornerRp[i] = CulRp(&pointOri);}}}}}

对于这个函数的coeff变量，它的xyz保存的直接就是是(乘了一个可信度因子的)对平移部分的雅克比，intensity保存的是(乘了一个可信度因子的)点到这个线特征的距离
注意：线特征对旋转部分的雅克比矩阵中，Rp是不带负号的
修改完surfOptimization和cornerOptimization后，由于多出了Rp的计算，因此要把Rp也合并到一起去，因此要改combineOptimizationCoeffs:

    void combineOptimizationCoeffs(){// combine corner coeffsfor (int i = 0; i < laserCloudCornerLastDSNum; ++i){if (laserCloudOriCornerFlag[i] == true){laserCloudOri->push_back(laserCloudOriCornerVec[i]);coeffSel->push_back(coeffSelCornerVec[i]);pcl::PointXYZ p;p.x = cornerRp[i].x(); p.y = cornerRp[i].y(); p.z = cornerRp[i].z();Rp->push_back(p); // pcl::PointCloud<pcl::PointXYZ>::Ptr Rp; // for jacobian orientation}}// combine surf coeffsfor (int i = 0; i < laserCloudSurfLastDSNum; ++i){if (laserCloudOriSurfFlag[i] == true){laserCloudOri->push_back(laserCloudOriSurfVec[i]);coeffSel->push_back(coeffSelSurfVec[i]);pcl::PointXYZ p;p.x = -1 * surfRp[i].x(); p.y = -1 * surfRp[i].y(); p.z = -1 * surfRp[i].z();Rp->push_back(p);}}// reset flag for next iterationstd::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);}

如果是线特征，则给Rp加负号
最后，要修改LMOptimization,先上一波源码：

bool LMOptimization(int iterCount)
{// This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation// lidar <- camera      ---     camera <- lidar// x = z                ---     x = y// y = x                ---     y = z// z = y                ---     z = x// roll = yaw           ---     roll = pitch// pitch = roll         ---     pitch = yaw// yaw = pitch          ---     yaw = roll// lidar -> camerafloat srx = sin(transformTobeMapped[1]);float crx = cos(transformTobeMapped[1]);float sry = sin(transformTobeMapped[2]);float cry = cos(transformTobeMapped[2]);float srz = sin(transformTobeMapped[0]);float crz = cos(transformTobeMapped[0]);// 当前帧匹配特征点数太少int laserCloudSelNum = laserCloudOri->size();if (laserCloudSelNum < 50) {return false;}cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));PointType pointOri, coeff;// 遍历匹配特征点，构建Jacobian矩阵for (int i = 0; i < laserCloudSelNum; i++) {// lidar -> camera todopointOri.x = laserCloudOri->points[i].y;pointOri.y = laserCloudOri->points[i].z;pointOri.z = laserCloudOri->points[i].x;// lidar -> cameracoeff.x = coeffSel->points[i].y;coeff.y = coeffSel->points[i].z;coeff.z = coeffSel->points[i].x;coeff.intensity = coeffSel->points[i].intensity;// in camerafloat arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x+ (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y+ (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;float ary = ((cry*srx*srz - crz*sry)*pointOri.x + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x+ ((-cry*crz - srx*sry*srz)*pointOri.x + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x+ (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y+ ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;// lidar -> cameramatA.at<float>(i, 0) = arz;matA.at<float>(i, 1) = arx;matA.at<float>(i, 2) = ary;matA.at<float>(i, 3) = coeff.z;matA.at<float>(i, 4) = coeff.x;matA.at<float>(i, 5) = coeff.y;// 点到直线距离、平面距离，作为观测值matB.at<float>(i, 0) = -coeff.intensity;}cv::transpose(matA, matAt);matAtA = matAt * matA;matAtB = matAt * matB;// J^T·J·delta_x = -J^T·f 高斯牛顿cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);// 首次迭代，检查近似Hessian矩阵（J^T·J）是否退化，或者称为奇异，行列式值=0 todoif (iterCount == 0) {cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));cv::eigen(matAtA, matE, matV);matV.copyTo(matV2);isDegenerate = false;float eignThre[6] = {100, 100, 100, 100, 100, 100};for (int i = 5; i >= 0; i--) {if (matE.at<float>(0, i) < eignThre[i]) {for (int j = 0; j < 6; j++) {matV2.at<float>(i, j) = 0;}isDegenerate = true;} else {break;}}matP = matV.inv() * matV2;}if (isDegenerate){cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX = matP * matX2;}// 更新当前位姿 x = x + delta_xtransformTobeMapped[0] += matX.at<float>(0, 0);transformTobeMapped[1] += matX.at<float>(1, 0);transformTobeMapped[2] += matX.at<float>(2, 0);transformTobeMapped[3] += matX.at<float>(3, 0);transformTobeMapped[4] += matX.at<float>(4, 0);transformTobeMapped[5] += matX.at<float>(5, 0);float deltaR = sqrt(pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));float deltaT = sqrt(pow(matX.at<float>(3, 0) * 100, 2) +pow(matX.at<float>(4, 0) * 100, 2) +pow(matX.at<float>(5, 0) * 100, 2));// delta_x很小，认为收敛if (deltaR < 0.05 && deltaT < 0.05) {return true; }return false;
}

要改的地方：
- 状态变量(状态变量定义为[roll, pitch, yaw, x, y, z],优化过程中会临时修改这个顺序)
- 雅克比矩阵( matA就是雅克比矩阵，matAt是其转置，matB是残差)，matX是迭代步长
状态变量：
- 从[roll, pitch, yaw, x, y, z]修改为[so3, x, y, z]
- 只有优化过程这样改，其他部分还是保持原来的表示；因此这里有一个欧拉角到SO3的转换过程
- 对旋转进行更新时，需要将迭代步长的旋转部分从李代数转成李群，然后再左乘到原状态上
- 计算完迭代之后，要将SO3转回到欧拉角
雅克比矩阵：
- 平移部分是不用改了，只要改旋转部分
懒得写了…看码吧

    bool LMOptimization(int iterCount){// This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation// lidar <- camera      ---     camera <- lidar// x = z                ---     x = y// y = x                ---     y = z// z = y                ---     z = x// roll = yaw           ---     roll = pitch// pitch = roll         ---     pitch = yaw// yaw = pitch          ---     yaw = roll// lidar -> camera
//        float srx = sin(transformTobeMapped[1]);
//        float crx = cos(transformTobeMapped[1]);
//        float sry = sin(transformTobeMapped[2]);
//        float cry = cos(transformTobeMapped[2]);
//        float srz = sin(transformTobeMapped[0]);
//        float crz = cos(transformTobeMapped[0]);int laserCloudSelNum = laserCloudOri->size();if (laserCloudSelNum < 50) {return false;}cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));PointType pointOri, coeff;for (int i = 0; i < laserCloudSelNum; i++) {
//            // lidar -> camera
//            pointOri.x = laserCloudOri->points[i].y;
//            pointOri.y = laserCloudOri->points[i].z;
//            pointOri.z = laserCloudOri->points[i].x;
//            // lidar -> camera
//            coeff.x = coeffSel->points[i].y;
//            coeff.y = coeffSel->points[i].z;
//            coeff.z = coeffSel->points[i].x;
//            coeff.intensity = coeffSel->points[i].intensity;// lidar -> camerapointOri.x = laserCloudOri->points[i].x;pointOri.y = laserCloudOri->points[i].y;pointOri.z = laserCloudOri->points[i].z;// lidar -> cameracoeff.x = coeffSel->points[i].x;coeff.y = coeffSel->points[i].y;coeff.z = coeffSel->points[i].z;coeff.intensity = coeffSel->points[i].intensity;// skew of Rpauto p_pcl = Rp->points[i];Eigen::Vector3f p(p_pcl.x, p_pcl.y, p_pcl.z);Eigen::Matrix3f RpSkew = mapOptimization::skewSymmetric(p);Eigen::Vector3f tmp(coeff.x, coeff.y, coeff.z);Eigen::Matrix<float, 1, 3> jacobian_theta = tmp.transpose() * RpSkew;//            // in camera
//            float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
//                      + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
//                      + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;
//
//            float ary = ((cry*srx*srz - crz*sry)*pointOri.x
//                      + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
//                      + ((-cry*crz - srx*sry*srz)*pointOri.x
//                      + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;
//
//            float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
//                      + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
//                      + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;
//            // lidar -> camera
//            matA.at<float>(i, 0) = arz;
//            matA.at<float>(i, 1) = arx;
//            matA.at<float>(i, 2) = ary;matA.at<float>(i, 0) = jacobian_theta(0, 0);matA.at<float>(i, 1) = jacobian_theta(0, 1);matA.at<float>(i, 2) = jacobian_theta(0, 2);
//            matA.at<float>(i, 3) = coeff.z;
//            matA.at<float>(i, 4) = coeff.x;
//            matA.at<float>(i, 5) = coeff.y;matA.at<float>(i, 3) = coeff.x;matA.at<float>(i, 4) = coeff.y;matA.at<float>(i, 5) = coeff.z;matB.at<float>(i, 0) = -coeff.intensity;}cv::transpose(matA, matAt);matAtA = matAt * matA;matAtB = matAt * matB;cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);if (iterCount == 0) {cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));cv::eigen(matAtA, matE, matV);matV.copyTo(matV2);isDegenerate = false;float eignThre[6] = {100, 100, 100, 100, 100, 100};for (int i = 5; i >= 0; i--) {if (matE.at<float>(0, i) < eignThre[i]) {for (int j = 0; j < 6; j++) {matV2.at<float>(i, j) = 0;}isDegenerate = true;} else {break;}}matP = matV.inv() * matV2;}if (isDegenerate){cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX = matP * matX2;}Eigen::Quaternionf quat(transPointAssociateToMap.matrix().block<3, 3>(0, 0));Sophus::SO3f so3_orientation;so3_orientation.setQuaternion(quat.normalized());
//        transformTobeMapped[0] += matX.at<float>(0, 0);
//        transformTobeMapped[1] += matX.at<float>(1, 0);
//        transformTobeMapped[2] += matX.at<float>(2, 0);
//        transformTobeMapped[3] += matX.at<float>(3, 0);
//        transformTobeMapped[4] += matX.at<float>(4, 0);
//        transformTobeMapped[5] += matX.at<float>(5, 0);Eigen::Vector3f update_so3(matX.at<float>(0, 0), matX.at<float>(1, 0), matX.at<float>(2, 0));so3_orientation = Sophus::SO3f::exp(update_so3) * so3_orientation;Eigen::Vector3f euler_angles =  so3_orientation.matrix().eulerAngles(2, 1, 0);transformTobeMapped[0] = euler_angles[2];transformTobeMapped[1] = euler_angles[1];transformTobeMapped[2] = euler_angles[0];transformTobeMapped[3] += matX.at<float>(3, 0);transformTobeMapped[4] += matX.at<float>(4, 0);transformTobeMapped[5] += matX.at<float>(5, 0);//        float deltaR = sqrt(
//                            pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +
//                            pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +
//                            pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));auto tmp = Sophus::SO3f::exp(update_so3);float deltaR = sqrt(pow(pcl::rad2deg(tmp.angleX()), 2) +pow(pcl::rad2deg(tmp.angleY()), 2) +pow(pcl::rad2deg(tmp.angleZ()), 2));float deltaT = sqrt(pow(matX.at<float>(3, 0) * 100, 2) +pow(matX.at<float>(4, 0) * 100, 2) +pow(matX.at<float>(5, 0) * 100, 2));if (deltaR < 0.05 && deltaT < 0.05) {return true; // converged}return false; // keep optimizing}

这里的SO3转回欧拉角后，再与IMU做融合会出问题(可能涉及到欧拉角的插值问题)，因此我把scan2MapOptimization中的transformUpdate换成incrementalOdometryAffineBack = trans2Affine3f(transformTobeMapped);,这样就不会与IMU的pitch,roll融合了

LIO-SAM：在高斯牛顿法求解过程中用SO3代替欧拉角相关推荐

迭代求解最优化问题TOA定位——最小二乘问题、高斯牛顿法
迭代求解最优化问题TOA定位--最小二乘问题.高斯牛顿法优化算法/TOA室内定位/ Target localization with range-only measurements 方法:1-非线性 ...
高斯牛顿法去畸变（C++实现）
1. 背景在实际三维重建时,通常会遇到如下问题: 已知有畸变的点坐标x′,y′x',y'x′,y′,已知畸变模型f(x,y),g(x,y)f(x,y),g(x,y)f(x,y),g(x,y), 求无 ...
曲线拟合（高斯牛顿法，Ceres,g2o)
实践:曲线拟合问题我们先通过高斯牛顿法来求解最小二乘问题,然后介绍使用优化库(Ceres/g2o)来求解此问题. 例题: 考虑一条满足如下方程的曲线: y = exp(ax2x^2x2+ bx + ...
割线法求解过程_求解稀疏优化问题2——临近点方法+半光滑牛顿法
这篇文章是我之前一篇文章的兄弟篇,没看过的可以看下面这个. 邓康康:求解稀疏优化问题--半光滑牛顿方法zhuanlan.zhihu.com 我们考虑的问题仍然是如下的一般问题: 其中 ,并且特别大 ...
SLAM从0到1——状态估计之最小二乘问题解法：最速下降法、牛顿法、高斯牛顿法、LM法...
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达学习3D视觉核心技术,扫描查看介绍,3天内无条件退款圈里有高质量教程资料.可答疑解惑.助你高效解决问 ...
牛顿法、梯度下降法、高斯牛顿法、Levenberg-Marquardt算法
何为梯度? 一般解释: f(x)在x0的梯度:就是f(x)变化最快的方向举个例子,f()是一座山,站在半山腰, 往x方向走1米,高度上升0.4米,也就是说x方向上的偏导是 0.4 往y方向走1米,高 ...
最优化八：高斯牛顿法、LM法
梯度法:,负梯度方向牛顿法:,A为Hession矩阵高斯牛顿法:,为的解 LM法:,为的解 1 高斯牛顿法(Gauss-Newton) 针对优化问题求解x使得f(x)取得最小值,采用高斯牛顿法,步 ...
相机校正、张氏标定法、极大似然估计/极大似然参数估计、牛顿法、高斯牛顿法、LM算法、sin/cos/tan/cot
日萌社人工智能AI:Keras PyTorch MXNet TensorFlow PaddlePaddle 深度学习实战(不定时更新) CNN:RCNN.SPPNet.Fast RCNN.Faste ...
【slam十四讲第二版】【课本例题代码向】【第七讲~视觉里程计Ⅱ】【使用LK光流（cv）】【高斯牛顿法实现单层光流和多层光流】【实现单层直接法和多层直接法】
[slam十四讲第二版][课本例题代码向][第七讲~视觉里程计Ⅱ][使用LK光流(cv)][高斯牛顿法实现单层光流和多层光流][实现单层直接法和多层直接法] 0 前言 1 使用LK光流(cv) 1.1 ...

LIO-SAM：在高斯牛顿法求解过程中用SO3代替欧拉角

LIO-SAM：在高斯牛顿法求解过程中用SO3代替欧拉角相关推荐

最新文章

热门文章