激光SLAM5-NDT方法

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

标签：map,pose,Eigen,Vector3d,激光,param,SLAM5,now,NDT
来源： https://blog.csdn.net/lsxpu/article/details/122158189