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

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 给方差

ouble 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(能看到红色帧,相比子图发生变化)

在这里插入图片描述
在这里插入图片描述