室外3D建图定位（三）Lego-Loam的深入调试篇
这篇博客主要是一些调试中踩的坑及验证过的东西，以供大家参考。
1.GTSAM使用篇
调试在原有Lego-loam中Gtsam框架上添加三维里程计的BetweenFactor约束
1.1 给初值
gtSAMgraph.add(PriorFactor(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:"<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(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(0, 0)), 2) +
pow(pcl::rad2deg(matX.at(1, 0)), 2) +
pow(pcl::rad2deg(matX.at(2, 0)), 2));
deltaT = sqrt( pow(matX.at(3, 0) * 100, 2) +
pow(matX.at(4, 0) * 100, 2) +
pow(matX.at(5, 0) * 100, 2));
deltaMax = max(deltaR,deltaT);
4.正反转的小问题
1.右边为顺时针旋转建图效果；左边为逆时针旋转建图效果；旋转速度30度/S；
Scan-Submap（能看到红色帧，相比子图发生变化）
欢迎各位爱好者留言和加微信讨论（15112119047），也欢迎加入高新兴机器人！