激光SLAM5-高斯牛顿优化方法

1.1 基于优化的方法(Optimization-based Method)

理论部分：




参考代码：

/**
 * @brief ComputeCompleteHessianAndb
 * 计算H*dx = b中的H和b
 * @param map
 * @param now_pose
 * @param laser_pts
 * @param H
 * @param b
 */
void ComputeHessianAndb(map_t* map, Eigen::Vector3d now_pose,
                        std::vector<Eigen::Vector2d>& laser_pts,
                        Eigen::Matrix3d& H, Eigen::Vector3d& b)
{
    H = Eigen::Matrix3d::Zero();
    b = Eigen::Vector3d::Zero();

    Eigen::Matrix3d trans = GN_V2T(now_pose);
    Eigen::Matrix<double, 2, 3> d_st;
    double sin_theta = std::sin(now_pose(2));
    double cos_theta = std::cos(now_pose(2));
    for (const auto& pt : laser_pts) {
        Eigen::Vector2d st = GN_TransPoint(pt, trans);
        Eigen::Vector3d map_value = InterpMapValueWithDerivatives(map, st);
        d_st << 1.0, 0.0, -sin_theta * pt(0) - cos_theta * pt(1),
                0.0, 1.0, cos_theta * pt(0) - sin_theta * pt(1);
        Eigen::Vector3d d_ms = (map_value.tail(2).transpose() * d_st).transpose();
        H += d_ms * d_ms.transpose();
        b += d_ms * (1.0 - map_value(0));
    }
}

1.2 地图双线性插值

理论部分:





参考代码:

/**
 * @brief InterpMapValueWithDerivatives
 * 在地图上的进行插值，得到coords处的势场值和对应的关于位置的梯度．
 * 返回值为Eigen::Vector3d ans
 * ans(0)表示市场值
 * ans(1:2)表示梯度
 * @param map
 * @param coords
 * @return
 */
Eigen::Vector3d InterpMapValueWithDerivatives(map_t* map,Eigen::Vector2d& coords)
{
    Eigen::Vector3d ans;
    double x = coords[0];
    double y = coords[1];
    int li = std::floor(MAP_GXWX_DOUBLE(map, x));
    int lj = std::floor(MAP_GYWY_DOUBLE(map, y));
    int ri = std::ceil(MAP_GXWX_DOUBLE(map, x));
    int rj = std::ceil(MAP_GYWY_DOUBLE(map, y));

    double z1 = map->cells[MAP_INDEX(map, li, lj)].score;
    double z2 = map->cells[MAP_INDEX(map, ri, lj)].score;
    double z3 = map->cells[MAP_INDEX(map, ri, rj)].score;
    double z4 = map->cells[MAP_INDEX(map, li, rj)].score;

    double x0 = MAP_WXGX(map, li);
    double y0 = MAP_WYGY(map, lj);
    double x1 = MAP_WXGX(map, ri);
    double y1 = MAP_WYGY(map, rj);

    double dx0 = (x - x0) / (x1 - x0);
    double dy0 = (y - y0) / (y1 - y0);
    double dx1 = (x1 - x) / (x1 - x0);
    double dy1 = (y1 - y) / (y1 - y0);
    
    ans(0) = dy0 * (dx0 * z3 + dx1 * z4) + dy1 * (dx0 * z2 + dx1 * z1);
    ans(1) = dy0 * ((z3 - z4) / (x1 - x0)) + dy1 * ((z2 - z1) / (x1 - x0));
    ans(2) = (dx0 * z3 + dx1 * z4) / (y1 - y0) - (dx0 * z2 - dx1 * z1) / (y1 - y0);

    return ans;
}

完整的代码参考Code