自动驾驶SLAM技术第四章习题2

在g2o的基础上改成ceres优化,高博都写好了其他的部分, 后面改ceres就很简单了. 这块我用的是ceres的自动求导,很方便,就是转化为模板仿函数的时候有点麻烦, 代码部分如下

ceres_type.h : ceres优化核心库的头文件

这个文件写的内容ceres优化的残差块. 把i, j时刻的状态都写成15维的数组, 顺序是r,p,v,bg,ba. 每个元素都是3维的, 所以r 部分涉及到R->r转换, sophus都实现好了.

/**
 * @file ceres_types.cc
 * @author Frank Zhang (tanhaozhang@connect.polyu.hk)
 * @brief 
 * @version 0.1
 * @date 2023-08-15
 * 
 * @copyright Copyright (c) 2023
 * 
 */

#ifndef SLAM_IN_AUTO_DRIVING_CH4_CERES_TYPE_H_
#define SLAM_IN_AUTO_DRIVING_CH4_CERES_TYPE_H_

#include <glog/logging.h>

#include "common/eigen_types.h"
#include "ceres/ceres.h"
#include "thirdparty/sophus/sophus/so3.hpp"
#include "ch4/imu_preintegration.h"

namespace sad
{

namespace ceres_optimization
{
  class PreintegrationCostFunctionCore {
    public:
     PreintegrationCostFunctionCore(std::shared_ptr<sad::IMUPreintegration> imu_preinit, const Eigen::Vector3d gravaty)
         : preinit_(imu_preinit), dt_(imu_preinit->dt_), grav_(gravaty) {}
     template <typename T>
     bool operator()(const T* const i, const T* const j, T* residual) const {
        Eigen::Matrix<T, 3, 1> r_i(i[0], i[1], i[2]);
        Eigen::Matrix<T, 3, 1> r_j(j[0], j[1], j[2]);
        Eigen::Matrix<T, 3, 1> p_i(i[3], i[4], i[5]);
        Eigen::Matrix<T, 3, 1> p_j(j[3], j[4], j[5]);
        Eigen::Matrix<T, 3, 1> v_i(i[6], i[7], i[8]);
        Eigen::Matrix<T, 3, 1> v_j(j[6], j[7], j[8]);
        Eigen::Matrix<T, 3, 1> bg(i[9], i[10], i[11]);
        Eigen::Matrix<T, 3, 1> ba(i[12], i[13], i[14]);

        Sophus::SO3<double> dR = preinit_->GetDeltaRotation(preinit_->bg_);

        Eigen::Matrix<double, 3, 1> dvd = preinit_->GetDeltaVelocity(preinit_->bg_, preinit_->ba_);
        Eigen::Matrix<T, 3, 1> dv(T(dvd.x()), T(dvd.y()), T(dvd.z()));
        Eigen::Matrix<double, 3, 1> dpd = preinit_->GetDeltaPosition(preinit_->bg_, preinit_->ba_);
        Eigen::Matrix<T, 3, 1> dp(T(dpd.x()), T(dpd.y()), T(dpd.z()));
      
        Sophus::SO3<T, 0> R_i = Sophus::SO3<T, 0>::exp(r_i);
        Sophus::SO3<T, 0> R_j = Sophus::SO3<T, 0>::exp(r_j);

        Eigen::Matrix<T, 3, 1> grav(T(grav_.x()), T(grav_.y()), T(grav_.z()));

        Eigen::Matrix<T, 3, 1> er = (dR.inverse() * R_i.inverse() * R_j).log();
        Eigen::Matrix<T, 3, 3> RiT = R_i.matrix();
        Eigen::Matrix<T, 3, 1> ev = RiT * (v_j - v_i - grav * T(dt_)) - dv;
        Eigen::Matrix<T, 3, 1> ep = RiT * (p_j - p_i - v_i * T(dt_) - grav * T(dt_) * T(dt_) * T(0.5)) - dp;
        residual[0] = T(er[0]);
        residual[1] = T(er[1]);
        residual[2] = T(er[2]);
        residual[3] = T(ev[0]);
        residual[4] = T(ev[1]);
        residual[5] = T(ev[2]);
        residual[6] = T(ep[0]);
        residual[7] = T(ep[1]);
        residual[8] = T(ep[2]);
        return true;
    }

    private:
    const double dt_;
    std::shared_ptr<sad::IMUPreintegration> preinit_ = nullptr;
    const Eigen::Vector3d grav_;
  };

  ceres::CostFunction* CreatePreintegrationCostFunction(std::shared_ptr<sad::IMUPreintegration> imu_preinit, const Eigen::Vector3d gravaty) {
    return new ceres::AutoDiffCostFunction<PreintegrationCostFunctionCore, 9, 15, 15>(new PreintegrationCostFunctionCore(imu_preinit, gravaty));
  }

  class BiasCostFunctionCore {
   public:
    BiasCostFunctionCore(){}
    template <typename T>
    bool operator() (const T* const i, const T* const j, T* residual) const
    {
      Eigen::Matrix<T, 3, 1> bg_i(i[9], i[10], i[11]);
      Eigen::Matrix<T, 3, 1> bg_j(j[9], j[10], j[11]);
      Eigen::Matrix<T, 3, 1> ba_i(i[12], i[13], i[14]);
      Eigen::Matrix<T, 3, 1> ba_j(j[12], j[13], j[14]);
      Eigen::Matrix<T, 3, 1> d_ba = ba_j - ba_i;
      Eigen::Matrix<T, 3, 1> d_bg = bg_j - bg_i;
      residual[0] = T(d_ba[0]);
      residual[1] = T(d_ba[1]);
      residual[2] = T(d_ba[2]);
      residual[3] = T(d_bg[0]);
      residual[4] = T(d_bg[1]);
      residual[5] = T(d_bg[2]);

      return true;
    }
  };
  ceres::CostFunction* CreateBiasCostFunction() {
    return new ceres::AutoDiffCostFunction<BiasCostFunctionCore, 6, 15, 15>(
      new BiasCostFunctionCore()
    );
  }

  class PriorCostFunctionCore {
    public:
     PriorCostFunctionCore(const std::shared_ptr<sad::NavStated> prior) : prior_(prior) {}
     template <typename T>
     bool operator()(const T* const i, T* residual) const {
      Eigen::Vector3d prior_r_d = prior_->R_.log();
      Eigen::Vector3d prior_p_d = prior_->p_;
      Eigen::Vector3d prior_v_d = prior_->v_;
      Eigen::Vector3d prior_bg_d = prior_->bg_;
      Eigen::Vector3d prior_ba_d = prior_->ba_;
      Eigen::Matrix<double, 15, 1> prior_M;
      prior_M << prior_r_d, prior_p_d, prior_v_d, prior_bg_d, prior_ba_d;
      for (int temp = 0; temp < prior_M.size(); temp++)
      {
        residual[temp] = T(prior_M[temp]) - i[temp];
      }
      return true;
    }
    private:
     const std::shared_ptr<sad::NavStated> prior_;
  };
  ceres::CostFunction* CreatePriorCostFunction(const std::shared_ptr<sad::NavStated> prior) {
     return new ceres::AutoDiffCostFunction<PriorCostFunctionCore, 15, 15>(new PriorCostFunctionCore(prior));
  }

  class GnssCostFunctionCore {
    public:
    GnssCostFunctionCore(const Sophus::SE3d gnss_states) : gnss_states_(gnss_states){}
    template <typename T>
    bool operator() (const T* const i, T* residual) const
    {
      Eigen::Matrix<T, 3, 1> r_i(i[0], i[1], i[2]);
      Sophus::SO3<T, 0> R_i = Sophus::SO3<T, 0>::exp(r_i);
      Eigen::Matrix<T, 3, 1> t_i(i[3], i[4], i[5]);
      Eigen::Matrix<T, 3, 1> e_r = (gnss_states_.so3().inverse() * R_i).log();
      Eigen::Matrix<T, 3, 1> e_t = t_i - gnss_states_.translation();
      residual[0] = T(e_r[0]);
      residual[1] = T(e_r[1]);
      residual[2] = T(e_r[2]);
      residual[3] = T(e_t[0]);
      residual[4] = T(e_t[1]);
      residual[5] = T(e_t[2]);
      return true;
    }
    private:
    const Sophus::SE3d gnss_states_;
  };
  static ceres::CostFunction* CreateGnssCostFunction(const Sophus::SE3d gnss_states){
    return new ceres::AutoDiffCostFunction<GnssCostFunctionCore, 6, 15> (
      new GnssCostFunctionCore(gnss_states)
    );
  }

  class OdomCostFunctionCore {
    public:
    OdomCostFunctionCore(const Eigen::Vector3d odom_speed_states) : odom_speed_states_(odom_speed_states) {}
    template <typename T>
    bool operator() (const T* const j, T* residual ) const {
      Eigen::Matrix<T, 3, 1> vj(j[6], j[7], j[8]);
      residual[0] = T(vj[0] - odom_speed_states_[0]);
      residual[1] = T(vj[1] - odom_speed_states_[1]);
      residual[2] = T(vj[2] - odom_speed_states_[2]);
      return true;
    }
    private:
    const Eigen::Vector3d odom_speed_states_;
  };
  static ceres::CostFunction* CreatOdomCostFunction(const Eigen::Vector3d odom_speed_states) {
    return new ceres::AutoDiffCostFunction<OdomCostFunctionCore, 3, 15> (
      new OdomCostFunctionCore(odom_speed_states)
    );
  }

} // namespace ceres_optimization


} //namespace sad

#endif

上面代码分别实现了预积分, bias, 先验, GNSS, odom的残差以及其工厂函数. 不得不说啊, ceres自动求导用起来真简单.

gins_pre_integ.cc: 实现ceres预积分优化

这一部分调用上面头文件构造的工厂函数实现残差计算, ceres优化与更新. 这里只粘贴一下不同的地方

    else {
        LOG_FIRST_N(INFO, 1) << "Using Ceres to Solve!";
        ceres::Problem problem;
        Eigen::Vector3d last_r_vec = last_frame_->R_.log();
        Eigen::Vector3d current_r_vec = this_frame_->R_.log();
        double last_state[15] = {last_r_vec.x(), last_r_vec.y(), last_r_vec.z(),       
                                last_frame_->p_.x(),last_frame_->p_.y(),  last_frame_->p_.z(), 
                                last_frame_->v_.x(),  last_frame_->v_.y(), last_frame_->v_.z(),  
                                last_frame_->bg_.x(), last_frame_->bg_.y(), last_frame_->bg_.z(), 
                                last_frame_->ba_.x(), last_frame_->ba_.y(), last_frame_->ba_.z()};
        double current_state[15] = {current_r_vec.x(),    current_r_vec.y(),    current_r_vec.z(),
                                    this_frame_->p_.x(),  this_frame_->p_.y(),  this_frame_->p_.z(),
                                    this_frame_->v_.x(),  this_frame_->v_.y(),  this_frame_->v_.z(),
                                    this_frame_->bg_.x(), this_frame_->bg_.y(), this_frame_->bg_.z(),
                                    this_frame_->ba_.x(), this_frame_->ba_.y(), this_frame_->ba_.z()};
        //预积分
        problem
            .AddResidualBlock(ceres_optimization::CreatePreintegrationCostFunction(pre_integ_, options_.gravity_), nullptr, last_state, current_state);
        // 两个零偏
        problem.AddResidualBlock(ceres_optimization::CreateBiasCostFunction(), nullptr, last_state, current_state);

        //GNSS
        problem.AddResidualBlock(ceres_optimization::CreateGnssCostFunction(last_gnss_.utm_pose_), nullptr, last_state);
        problem.AddResidualBlock(ceres_optimization::CreateGnssCostFunction(this_gnss_.utm_pose_), nullptr,
                                 current_state);
        //先验
        problem.AddResidualBlock(ceres_optimization::CreatePriorCostFunction(last_frame_), nullptr, last_state);
        //ODOM
        Vec3d vel_world = Vec3d::Zero();
        Vec3d vel_odom = Vec3d::Zero();
        if (last_odom_set_) {
            // velocity obs
            double velo_l = options_.wheel_radius_ * last_odom_.left_pulse_ /
                            options_.circle_pulse_ * 2 * M_PI /
                            options_.odom_span_;
            double velo_r = options_.wheel_radius_ * last_odom_.right_pulse_ /
                            options_.circle_pulse_ * 2 * M_PI /
                            options_.odom_span_;
            double average_vel = 0.5 * (velo_l + velo_r);
            vel_odom = Vec3d(average_vel, 0.0, 0.0);
            vel_world = this_frame_->R_ * vel_odom;

            problem.AddResidualBlock(ceres_optimization::CreatOdomCostFunction(vel_world), nullptr, current_state);

            // 重置odom数据到达标志位,等待最新的odom数据
            last_odom_set_ = false;
        }


        ceres::Solver::Options options;
        options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
        options.max_num_iterations = 20;
        options.num_threads = 4;
        options.minimizer_progress_to_stdout = false;

        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem, &summary);

        Eigen::Vector3d last_r(last_state[0], last_state[1], last_state[2]);
        last_frame_->R_ = Sophus::SO3d::exp(last_r);
        Eigen::Vector3d last_t(last_state[3], last_state[4], last_state[5]);
        last_frame_->p_ = last_t;
        Eigen::Vector3d last_v(last_state[6], last_state[7], last_state[8]);
        last_frame_->v_ = last_v;
        Eigen::Vector3d last_bg(last_state[9], last_state[10], last_state[11]);
        last_frame_->bg_ = last_bg;
        Eigen::Vector3d last_ba(last_state[12], last_state[13], last_state[14]);
        last_frame_->ba_ = last_ba;

        Eigen::Vector3d current_r(current_state[0], current_state[1], current_state[2]);
        this_frame_->R_ = Sophus::SO3d::exp(current_r);
        Eigen::Vector3d current_t(current_state[3], current_state[4], current_state[5]);
        this_frame_->p_ = current_t;
        Eigen::Vector3d current_v(current_state[6], current_state[7], current_state[8]);
        this_frame_->v_ = current_v;
        Eigen::Vector3d current_bg(current_state[9], current_state[10], current_state[11]);
        this_frame_->bg_ = current_bg;
        Eigen::Vector3d current_ba(current_state[12], current_state[13], current_state[14]);
        this_frame_->ba_ = current_ba;
    }
    // 重置integ
    options_.preinteg_options_.init_bg_ = this_frame_->bg_;
    options_.preinteg_options_.init_ba_ = this_frame_->ba_;
    pre_integ_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);
}

上面部分代码为了使用autodiff做了好多不必要的数据处理, 如果有更好的解题思路欢迎留言.
上面代码分为以下几步: 1. 初始处理 2. 添加残差 3. ceres优化 4. 更新
效果如下

感觉还行, 没有评价精度

问题

没有评价精度是不是比g2o好一些
没有评价算力是不是比g2o小一些
没有实现解析求导, 正在搞

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:/a/88636.html

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

openGauss学习笔记-48 openGauss 高级数据管理-函数

文章目录 openGauss学习笔记-48 openGauss 高级数据管理-函数48.1 数学函数48.2 三角函数列表48.3 字符串函数和操作符48.4 类型转换相关函数 openGauss学习笔记-48 openGauss 高级数据管理-函数 openGauss常用的函数如下&#xff1a; 48.1 数学函数 abs(x) 描述&#xff1a;…

IDEA中使用Docker插件构建镜像并推送至私服Harbor

一、开启Docker服务器的远程访问 1.1 开启2375远程访问 默认的dokcer是不支持远程访问的&#xff0c;需要加点配置&#xff0c;开启Docker的远程访问 # 首先查看docker配置文件所在位置 systemctl status docker# 会输出如下内容&#xff1a; ● docker.service - Docker Ap…

测试框架pytest教程(10)自定义命令行-pytest_addoption

pytest_addoption pytest_addoption是pytest插件系统中的一个钩子函数&#xff0c;用于向pytest添加自定义命令行选项。 在pytest中&#xff0c;可以使用命令行选项来控制测试的行为和配置。pytest_addoption钩子函数允许您在运行pytest时添加自定义的命令行选项&#xff0c;…

【VS Code插件开发】消息通信(四)

&#x1f431; 个人主页&#xff1a;不叫猫先生&#xff0c;公众号&#xff1a;前端舵手 &#x1f64b;‍♂️ 作者简介&#xff1a;前端领域优质作者、阿里云专家博主&#xff0c;共同学习共同进步&#xff0c;一起加油呀&#xff01; &#x1f4e2; 资料领取&#xff1a;前端…

adb 命令

1.adb shell dumpsys activity top | find "ACTIVITY" 查看当前运行的activity包名 2.adb shell am start -n 包名/页面名 打开应用的页面 3.查看将要启动或退出app的包名 adb shell am monitor 只有在启动或退出的时候才会打印 4.查看当前启动应用的包名 ad…

AR室内导航技术之技术说明与效果展示

随着科技的飞速发展&#xff0c;我们周围的环境正在经历着一场数字化的革命。其中&#xff0c;AR室内导航技术以其独特的魅力&#xff0c;为我们打开了一扇通往全新数字化世界的大门。本文将为您详细介绍这一技术的实现原理、工具应用以及成品展示&#xff0c;带您领略AR室内导…

mybatis-plus--配置-(sql)日志输出-自动填充-分页-多数据源-逻辑删除

写在前面&#xff1a; 本文主要介绍mybatis-plus的配置&#xff0c;以后在有的时候在补充。欢迎交流。 文章目录 日志输出自动填充分页全局字段配置多数据源 日志输出 调试的时候需要看执行的sql&#xff0c;这时候就很需要日志来记录查看了。 mybatis-plus的日志配置在yml…

自动化部署及监测平台基本架构

声明 本文是学习 政务计算机终端核心配置规范. 而整理的学习笔记,分享出来希望更多人受益,如果存在侵权请及时联系我们 核心配置自动化部署及监测技术要求 自动化部署及监测平台基本架构 对于有一定规模的政务终端核心配置应用&#xff0c;需要配备自动化部署及监测平台&am…

01-jupyter notebook的使用方法

一、Tab补全 在shell中输入表达式&#xff0c;按下Tab&#xff0c;会搜索已输入变量&#xff08;对象、函数等等&#xff09;的命名空间&#xff1a; 除了补全命名、对象和模块属性&#xff0c;Tab还可以补全其它的。当输入看似文件路径时 &#xff08;即使是Python字符串&…

浅拷贝与深拷贝

作者简介&#xff1a; zoro-1&#xff0c;目前大一&#xff0c;正在学习Java&#xff0c;数据结构等 作者主页&#xff1a; zoro-1的主页 欢迎大家点赞 &#x1f44d; 收藏 ⭐ 加关注哦&#xff01;&#x1f496;&#x1f496; 浅拷贝与深拷贝 浅拷贝浅拷贝定义浅拷贝代码演示浅…

优秀产品奖!移远5G RedCap模组,让5G真正“轻”下来

8月24日&#xff0c;在通信世界全媒体主办的“5G RedCap技术与物联网应用创新研讨会”上&#xff0c;“5G RedCap优秀产品和解决方案”获奖名单发布&#xff0c;移远通信5G RedCap模组Rx255C系列以其在创新性、实用性、经济性、成熟性等方面的综合领先优势&#xff0c;获此殊荣…

更高效稳定 | 基于ACM32 MCU的编程直流电源应用方案

随着电子设备的多样化发展&#xff0c;面对不同的应用场景&#xff0c;需要采用特定的供电电源。因此&#xff0c;在电子产品的开发测试过程中&#xff0c;必不可少使用编程直流电源来提供测试电压&#xff0c;协助完成初步的开发测试过程。 编程直流电源概述 编程直流电源结构…

李沐pytorch学习-经典CNN的原理及代码实现

一、LeNet 1.1 模型结构 LeNet结构如图1所示&#xff0c;汇聚层即池化层&#xff0c;这里池化Stride&#xff08;步幅&#xff09;与池化层长宽一致&#xff0c;因此使得池化后大小减半。 图1. LeNet结构 1.2 代码实现 代码实现如下&#xff1a; import torch from torch imp…

Arnold置乱

一、Arnold置乱概述 Arnold变换是俄国数学家弗拉基米尔阿诺德&#xff08;Vladimir Igorevich Arnold&#xff09;提出&#xff0c;Arnold将其应用在遍历理论研究中。由于Arnold本人最初对一张猫的图片进行了此种变换&#xff0c;因此它又被称为猫脸变换&#xff08;cat映射&am…

基于Citespace、vosviewer、R语言的文献计量学可视化分析技术及全流程文献可视化SCI论文高效写作方法

文献计量学是指用数学和统计学的方法&#xff0c;定量地分析一切知识载体的交叉科学。它是集数学、统计学、文献学为一体&#xff0c;注重量化的综合性知识体系。特别是&#xff0c;信息可视化技术手段和方法的运用&#xff0c;可直观的展示主题的研究发展历程、研究现状、研究…

HCIP-OpenStack组件之neutron

neutron&#xff08;ovs、ovn&#xff09; OVS OVS(Open vSwitch)是虚拟交换机&#xff0c;遵循SDN(Software Defined Network&#xff0c;软件定义网络)架构来管理的。 OVS介绍参考&#xff1a;https://mp.weixin.qq.com/s?__bizMzAwMDQyOTcwOA&mid2247485088&idx1…

基于AWS的3D模型搜索服务实现

3D模型广泛应用于计算机游戏、电影、工程、零售业、广告等许多领域。市场上有很多制作3D模型的工具&#xff0c;但几乎没有工具可以直观地搜索3D模型数据库以找到类似的3D模型 因为开发好的 3D 模型搜索工具非常具有挑战性。 它需要复杂的计算和 AI/ML 框架来创建模型描述符并提…

jmeter HTTP信息头管理器

首先&#xff0c;打开JMeter并创建一个新的测试计划。右键单击测试计划&#xff0c;选择"添加" > “线程组”&#xff0c;然后在线程组上右键单击&#xff0c;选择"添加" > “Sampler” > “HTTP请求”。 在HTTP请求中填写服务器的URL和其他必要…

Java基础之IO流File类创建及删除

1.File类概述及构造方法 2.File类创建功能 文件创建成功&#xff01; 如果文件不存在&#xff0c;就创建文件&#xff0c;并返回true 如果文件存在&#xff0c;就不创建文件&#xff0c;并返回false 如果文件夹不存在&#xff0c;就创建文件夹&#xff0c;并返回true 如果文件…