- 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);
}