室外3D建图定位（三）Lego-Loam的深入调试篇_人工智能

这篇博客主要是一些调试中踩的坑及验证过的东西，以供大家参考。

1.GTSAM使用篇

调试在原有Lego-loam中Gtsam框架上添加三维里程计的BetweenFactor约束
1.1 给初值

 gtSAMgraph.add(PriorFactor<Pose3>(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
                                                       		 Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])), priorNoise));
 initialEstimate.insert(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
                                                  Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])));
 这里需要注意的就是插入初值不能在添加约束前面，不然会报错，挺郁闷的。

1.2 给方差

double lidarVar[6] = {1e-4, 1e-4, 1e-4, 1e-2, 1e-2, 1e-2};
Vector6 << lidarVar[0], lidarVar[1], lidarVar[2], lidarVar[3], lidarVar[4], lidarVar[5];
其中lidarVar[0-5]分别对应Roll，Pitch，Yaw，X，Y，Z方差，上面方差代表角度精度大概0.57度，位移精度在10cm左右；

1.3 huber核函数

*gtsam LossFunctions.cpp*
double Huber::weight(double distance) const {
  const double absError = std::abs(distance);
  if(absError > k_)
  {
   std::cout<<"absError:"<<absError<<","<<"weight:"<<k_ / absError<<std::endl;
  } 
  return (absError <= k_) ? (1.0) : (k_ / absError);
}

*lego-loam*
  lidarOdomeNoise = gtsam::noiseModel::Robust::Create(
                    gtsam::noiseModel::mEstimator::Huber::Create(170), //Huber 马式距离 超过 1m或者10度 为异常 
                    gtsam::noiseModel::Diagonal::Variances(Vector6)); 
  这里需要注意的是k_值给多少合适。举个例子，比如以轮式里程计估计作为初值，那么当激光匹配计算出来的值与初值相比位移差1m以内，角度差10度以内，则k_= norm(10/57.3*100,10/57.3*100,10/57.3*100,1*10,1*10,1*10) =34，其中100,10分别对应角度和位移的标准差，因为是马氏距离。

1.4 between

poseFrom.between(poseTo);//简单理解为poseTo - poseFrom
R12.between(R13)=R23 需要注意坐标系，其中R12代表P1=R12*P2;

2.高度的发散

1.高度的发散主要由Pitch配不准导致（类似在上下坡）不是位移Z没配准；
2.Pitch配不准主要是大部分机器前进方向特征空旷（缺少一堵墙）；所以室内高度基本不会发散（如果有发散，建议走来回，因为有可能是雷达水平安装有倾斜）；
3.解决Pitch角配不准，最好的办法是引入IMU的Pitch来约束，因为IMU的Pitch大部分可以由加速度计计算得到，是绝对参考值，不存在累积误差；

在这里插入图片描述

3.融合问题

（IMU+轮式）三维里程计与激光里程计的融合，主要是想实现在激光配准失败，能利用更多（IMU+轮式）三维里程计的权重，以不至于整个系统因为几帧点云配准失败而崩溃。（融合前注意验证三维里程计轨迹与激光雷达轨迹一致，杆臂效应）

1.构建三维里程计的约束

if(useOdom == true)//odom
{
   poseFrom = Pose3(Rot3::RzRyRx(lastTransformSum[2], lastTransformSum[0], lastTransformSum[1]),
                    Point3(lastTransformSum[5], lastTransformSum[3], lastTransformSum[4]));
   poseTo   = Pose3(Rot3::RzRyRx(transformSum[2], transformSum[0], transformSum[1]),
                     Point3(transformSum[5], transformSum[3], transformSum[4]));
   gtSAMgraph.add(BetweenFactor<Pose3>(size-1, size, poseFrom.between(poseTo), wheelOdomeNoise));

   gtsam::Pose3 poseInit  = pclPointTogtsamPose3(cloudKeyPoses6D->points[size-1]);
   poseTo = poseInit*poseFrom.between(poseTo);//poseTo - poseFrom
 }  
 initialEstimate.insert(cloudKeyPoses3D->points.size(), poseTo);

2.如何确定三维里程计的权重值

 /*利用Huber核函数，注意初值的插入需要使用三维里程计的值；缺点是轮子的打滑有点麻烦*/
 lidarOdomeNoise = gtsam::noiseModel::Robust::Create(
                   gtsam::noiseModel::mEstimator::Huber::Create(170), //Huber 马式距离 超过 1m或者10度 为异常 
                   gtsam::noiseModel::Diagonal::Variances(Vector6));

/*利用点云配准收敛否，deltaMax越大说明配准结果越发散；缺点是阈值参数不是太好给*/
  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));
  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));
  deltaMax = max(deltaR,deltaT);