g2o从入门到精通

前言

这里将我对g2o的一些常见用法,做个简单总结,便于后续温习回顾,如能对大家起到半点借鉴作用,实乃我之幸事。

一 先看两个案例

1.1 案例一:

// compute uwb pose by mutli distances using least square estimate
// fix_xy means only optimize z
EdgeUwbDistanceToPose::EdgeUwbDistanceToPose()
{

}

EdgeUwbDistanceToPose::EdgeUwbDistanceToPose(Eigen::Vector3d anchor_pose, double distance, bool fix_xy)
    : anchor_pose_ ( anchor_pose ), distance_(distance), fix_xy_(fix_xy)
{

}

void EdgeUwbDistanceToPose::computeError()
{
    const g2o::VertexSBAPointXYZ* v1  = static_cast<const g2o::VertexSBAPointXYZ*> ( _vertices[0] );

    double tag_anchor_dist =  (v1->estimate() - anchor_pose_).norm();

    _error(0) = tag_anchor_dist - distance_;
    return;
}

void EdgeUwbDistanceToPose::linearizeOplus()
{
    if ( level() == 1 )
    {
        _jacobianOplusXi = Eigen::Matrix<double, 1, 3>
3D视觉工坊 CSDN认证博客专家 算法 3D视觉
个人公众号:3D视觉工坊。公众号特邀嘉宾及合伙人,先后就职于国内知名研究机构、自动驾驶公司、海康研究院,主要研究方向为深度学习、目标检测、语义分割、图像处理、自动驾驶感知算法等,CSDN博客专家。博主先后任职于国内知名研究院、知名大厂,致力于3D视觉算法、VLAM算法开发,涉及相机标定、手眼标定、结构光、点云后处理、三维重建等相关领域的研究,同时也是CSDN博客专家。3D视觉工坊坚持原创,近一年来输出了非常多的高质量文章,获得了粉丝的一致好评,我们将始终坚持走原创路线,打造一个铁杆粉丝的聚集区。
已标记关键词 清除标记
相关推荐
©️2020 CSDN 皮肤主题: 书香水墨 设计师:CSDN官方博客 返回首页
实付 49.90元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值