从零开始构建2d激光slam-4.图优化

置顶我是飞行员

已于 2022-11-29 13:36:16 修改

阅读量233

点赞数

分类专栏： lite_slam 文章标签：算法

于 2022-09-07 19:32:14 首次发布

本文链接：https://blog.csdn.net/qq_30554779/article/details/126752287

版权

lite_slam 专栏收录该内容

6 篇文章 7 订阅

订阅专栏

参考网址:

peitianyu/lite_slam (github.com)

versatran01/graphslam: graphslam (github.com)

参考文献

A Tutorial on Graph-Based SLAM

GraphOptimize

- 优化方向: 
	- 由于使用的是scan_points具有角度不变性,因此可以将3 * num的H,降低为2 * num,降低计算量
	- 加入残差判断
- 这部分因为运行速度太低,导致根本无法实际使用,建议使用我的另一篇因子图优化

流程

void AddVertex(const uint &id, const Eigen::Vector3f &pose); // 添加顶点 std::map<uint, Eigen::Vector3f> vertex

void AddEdge(const uint &from_id, const uint &to_id, const Eigen::Vector3f &measurement, const Eigen::Matrix3f &info_matrix);// 添加边约束 

void Optimize(); // 通过高斯牛顿优化

const std::vector<Eigen::Vector3f> &GetOptimizedPoses() const; // 获得优化后坐标

const std::map<uint, Eigen::Vector3f> &GetOptimizedVertexes() const; // 获得优化后顶点

优化

void GraphOptimize::EstimateOnce()
{
    Eigen::MatrixXf H = Eigen::MatrixXf::Zero(m_vertex.size() * 3, m_vertex.size() * 3); // 之后优化这部分
    Eigen::VectorXf b = Eigen::VectorXf::Zero(m_vertex.size() * 3);
    GetHessianDerived(H, b); 

    Eigen::SimplicialLDLT<Eigen::SparseMatrix<float>> solver;
    solver.compute(H.sparseView());
    if(solver.info() != Eigen::Success)
        return;

    Eigen::VectorXf d_x = Eigen::VectorXf::Zero(m_vertex.size() * 3);
    d_x = solver.solve(b);
    if(solver.info() != Eigen::Success)
        return;

    m_ret_poses.clear();
    for(auto &vertice : m_vertex)
    {
        vertice.second += d_x.segment<3>(vertice.first * 3);
        m_ret_poses.push_back(vertice.second);
    }

    NormalizePose();
}