这篇博客主要是一些调试中踩的坑及验证过的东西，以供大家参考。
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);
4.正反转的小问题
1.右边为顺时针旋转建图效果；左边为逆时针旋转建图效果；旋转速度30度/S；
Scan-Submap（能看到红色帧，相比子图发生变化）
欢迎各位爱好者留言和加微信讨论（15112119047），也欢迎加入高新兴机器人！