hdl graph slam解析

• github/hdl_graph_slam传送门
• paper： A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement

这位老兄写的代码可读性非常好，大佬就是大佬。。。
总结了一个图如下，总的分为4大模块，分别是：点云预处理；地平面检测；scan matching的Odometry；图优化。

1 主要模块总结

感觉本文的主要亮点在与提供了一种通用的把gps，odometry，ground plane等都加入图优化的思路。
其中ground plane约束的加入，大大提高了slam的轨迹精度。

2 Ground Plane Constraint

其中ground plane约束的构建paper中的公式和g2o中自带的plane to plane代码有所不同，但是本质含义都是构建一种误差。误差的信息矩阵设置固定值。
SE3 node和ground plane node之间的误差计算核心代码如下

void computeError() override {
			const g2o::VertexSE3* v1 = static_cast<const g2o::VertexSE3*>(_vertices[0]);
			const g2o::VertexPlane* v2 = static_cast<const g2o::VertexPlane*>(_vertices[1]);

			Eigen::Isometry3d w2n = v1->estimate().inverse();
			Plane3D local_plane = w2n * v2->estimate();//地面参数变换到局部坐标系
			_error = local_plane.ominus(_measurement);//_measurement就是地平面检测模块估计出的地平面参数
		}

class Plane3D{
...
inline Eigen::Vector3d ominus(const Plane3D& plane){
      //construct the rotation that would bring the plane normal in (1 0 0)
      Eigen::Matrix3d R=rotation(normal()).transpose();
      Eigen::Vector3d n=R*plane.normal();
      double d=distance()-plane.distance();
      return Eigen::Vector3d(azimuth(n), elevation(n), d);
    }
...
}

3 GPS Constraint

找到当前关键帧时间戳附近最近的gps frame，记录第一帧gps，转化到utm坐标系下，后面的数据都是按照相对第一帧来处理。keyframe和gps位姿直接相减得到先验约束加入到graph中。
简单粗暴！！
误差信息矩阵设置固定值，其实更合理的方式，是利用gps消息自带的NavSatStatus status动态设置

// convert (latitude, longitude, altitude) -> (easting, northing, altitude) in UTM coordinate
geodesy::UTMPoint utm;
geodesy::fromMsg((*closest_gps)->position, utm);
Eigen::Vector3d xyz(utm.easting, utm.northing, utm.altitude);

// the first gps data position will be the origin of the map
if (!zero_utm) {
    zero_utm = xyz;
}
xyz -= (*zero_utm);

keyframe->utm_coord = xyz;

g2o::OptimizableGraph::Edge *edge;
if (std::isnan(xyz.z())) {//判断gps z轴是否有值，没有就只进行xy约束
    Eigen::Matrix2d information_matrix = Eigen::Matrix2d::Identity()/gps_edge_stddev_xy;
    edge = graph_slam->add_se3_prior_xy_edge(keyframe->node, xyz.head<2>(), information_matrix);
}
else {
    Eigen::Matrix3d information_matrix = Eigen::Matrix3d::Identity();
    information_matrix.block<2, 2>(0, 0) /= gps_edge_stddev_xy;
    information_matrix(2, 2) /= gps_edge_stddev_z;
    edge = graph_slam->add_se3_prior_xyz_edge(keyframe->node, xyz, information_matrix);
           }