自动驾驶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);//GNSSproblem.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);//ODOMVec3d vel_world = Vec3d::Zero();Vec3d vel_odom = Vec3d::Zero();if (last_odom_set_) {// velocity obsdouble 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;}// 重置integoptions_.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小一些
没有实现解析求导, 正在搞
相关文章:

自动驾驶SLAM技术第四章习题2
在g2o的基础上改成ceres优化,高博都写好了其他的部分, 后面改ceres就很简单了. 这块我用的是ceres的自动求导,很方便,就是转化为模板仿函数的时候有点麻烦, 代码部分如下 ceres_type.h : ceres优化核心库的头文件 这个文件写的内…...

vue拖拽div盒子实现上下拖动互换
vue拖拽div盒子实现上下拖动互换 <div v-for"(item, index) in formList" :key"index" draggable"true"dragstart"handleDragStart($event, item)"dragenter"handleDragEnter($event, item)"dragover.prevent"han…...

Visual Studio 2022 右键单击项目没有出现View | View Class Diagram(Visual Studio 无法使用类设计器)
文章目录 问题描述原因.NET Core项目.NET Framework项目 问题描述 当我们在Solution Explorer窗口右键单击项目时,快捷菜单中没有出现“查看”,或者出现了“查看”,但是“查看”里没有View Class Diagram。 原因 首先你要确保你安装了类设…...
EFCore常见用法
EFCore官方文档置顶,看这个就行。下面的内容只是总结,算是备忘录。 一、创建和删除 //1、创建数据库和表 db.Database.EnsureCreated();//将创建数据库(如果不存在)并初始化数据库架构。 如果存在任何表 (包括另一 DbContext 类)…...

概率论与数理统计:第六章:数理统计
文章目录 Ch6. 数理统计(一) 总体与样本(二) 统计量 (5个)2.5个常用统计量3.矩的概念 (三) 抽样分布 (3个)0.上α分位点1.χ分布2.t分布3.F分布 (四) 抽样分布定理1.单个正态总体2.两个正态总体 Ch6. 数理统计 (一) 总体与样本 1.概念: (1)总体 (2)样本 简单随机…...

拥塞控制(TCP限制窗口大小的机制)
拥塞控制机制可以使滑动窗口在保证可靠性的前提下,提高传输效率 关于滑动窗口的属性以及部分机制推荐看TCP中窗口和滑动窗口的含义以及流量控制 拥塞控制出现的原因 看了上面推荐的博客我们已经知道了,由于接收方接收数据的能力有限,所以要通…...

校园供水系统智能管理
import pandas as pd data1pd.read_excel("C://Users//JJH//Desktop//E//附件_一季度.xlsx") data2pd.read_excel("C://Users//JJH//Desktop//E//附件_二季度.xlsx") data3pd.read_excel("C://Users//JJH//Desktop//E//附件_三季度.xlsx") data4…...
Flask-SocketIO和Flask-Login联合开发socketio权限系统
设置 Flask, Flask-SocketIO, Flask-Login: 首先,确保安装了必要的库: pip install Flask Flask-SocketIO Flask-Login基础设置: from flask import Flask, render_template, redirect, url_for, request from flask_socketio import SocketIO, emit from flask_…...

航空电子设备中的TSN通讯架构—直升机
前言 以太网正在迅速取代传统网络,成为航空电子设备和任务系统的核心高速网络。本文提出了以太网时间敏感网络(TSN)在航空电子设备上应用的技术优势问题。在实际应用中,TSN已成为一个具有丰富的机制和协议的工具箱,可满足与时间和可靠性相关…...

elment-ui中使用el-steps案例
el-steps案例 样式 代码 <div class"active-box"><div class"active-title">请完善</div><el-steps :active"active" finish-status"success" align-center><el-step title"第一步" /><…...

FPGA解析串口指令控制spi flash完成连续写、读、擦除数据
前言 最近在收拾抽屉时找到一个某宝的spi flash模块,如下图所示,我就想用能不能串口来读写flash,大致过程就是,串口向fpga发送一条指令,fpga解析出指令控制flah,这个指令协议目前就是: 55 AA …...

msvcp120.dll丢失的解决方法,分享三种快速修复的方法
今天,我将和大家分享一个关于电脑问题的解决方法——msvcp120.dll丢失的解决方法。希望对大家有所帮助。 首先,让我们来了解一下msvcp120.dll文件。msvcp120.dll是Microsoft Visual C 2010 Redistributable Package的一个组件,它包含了一些运…...

mysql 8.0 窗口函数 之 序号函数 与 sql server 序号函数 一样
sql server 序号函数 序号函数 ROW_NUMBER() 顺序排序RANK() 并列排序,会跳过重复的序号,比如序号为1,1,3DENSE_RANK() 并列排序,不会跳过重复的序号,比如 序号为 1,1,2 语法结构…...

fastgpt构建镜像
1.把client目录复制到服务器 .next和node_modules文件夹不用上传到服务器 在服务器目录运行 docker build -t fastgpt:1.0.3 . 构建服务 再运行 docker ps 就可以看到容器了...
Git笔记--分支常用命令
目录 1--git branch -v 2--git branch 3--git checkout 4--git merge 1--git branch -v git branch -v git branch -v 用于查看分支版本; 2--git branch git branch xxxxx # xxxxx表示分支名 git branch 用于创建分支; 3--git checkout git check…...
常见设计模式学习+面试总结
一 设计模式简介 二 面试总结 1 什么是单例模式?都有哪些地方用到单例? 内存中只会创建且仅创建一次对象的设计模式,保证一个类只有一个实例,并且提供一个访问该全局访问点。 应用场景: 网站的计数器,一般…...
sql解决取多个截至每个月的数据
问题:需要查询1月、1-2月、1-3月… 1-12月,分区间的累计数据,在同一个sql语句里面实现。 多个分开查询效率不高,并且数据手动合并麻烦。 with t1 as ( SELECT *,CASE WHEN insutype 390 THEN 居民 ELSE 职工 END 人员类别,SUBST…...

数据采集:selenium 获取 CDN 厂家各省市节点 IP
写在前面 工作需要遇到,简单整理理解不足小伙伴帮忙指正 对每个人而言,真正的职责只有一个:找到自我。然后在心中坚守其一生,全心全意,永不停息。所有其它的路都是不完整的,是人的逃避方式,是对…...

【el-tree】树形组件图标的自定义
饿了么树形组件的图标自定义 默认样式: 可以看到el-tree组件左侧自带展开与收起图标,咱们可以把它隐藏:: .groupList {::v-deep .el-tree-node { .el-icon-caret-right {display: none;} } } 我的全部代码 <div class"groupList"><el…...

UltralSO软碟通制作Linux系统盘
第一步: 下载镜像 阿里云下载地址:https://mirrors.aliyun.com/centos-vault/ 按照需求选择系统版本,我这要求安装CentOS7.5的系统,我以CentOS7.5为例 第二步: 下载UltralSO软件 官网下载地址:https://cn.…...
鸿蒙中用HarmonyOS SDK应用服务 HarmonyOS5开发一个医院挂号小程序
一、开发准备 环境搭建: 安装DevEco Studio 3.0或更高版本配置HarmonyOS SDK申请开发者账号 项目创建: File > New > Create Project > Application (选择"Empty Ability") 二、核心功能实现 1. 医院科室展示 /…...

对WWDC 2025 Keynote 内容的预测
借助我们以往对苹果公司发展路径的深入研究经验,以及大语言模型的分析能力,我们系统梳理了多年来苹果 WWDC 主题演讲的规律。在 WWDC 2025 即将揭幕之际,我们让 ChatGPT 对今年的 Keynote 内容进行了一个初步预测,聊作存档。等到明…...

屋顶变身“发电站” ,中天合创屋面分布式光伏发电项目顺利并网!
5月28日,中天合创屋面分布式光伏发电项目顺利并网发电,该项目位于内蒙古自治区鄂尔多斯市乌审旗,项目利用中天合创聚乙烯、聚丙烯仓库屋面作为场地建设光伏电站,总装机容量为9.96MWp。 项目投运后,每年可节约标煤3670…...
论文解读:交大港大上海AI Lab开源论文 | 宇树机器人多姿态起立控制强化学习框架(一)
宇树机器人多姿态起立控制强化学习框架论文解析 论文解读:交大&港大&上海AI Lab开源论文 | 宇树机器人多姿态起立控制强化学习框架(一) 论文解读:交大&港大&上海AI Lab开源论文 | 宇树机器人多姿态起立控制强化…...

Android15默认授权浮窗权限
我们经常有那种需求,客户需要定制的apk集成在ROM中,并且默认授予其【显示在其他应用的上层】权限,也就是我们常说的浮窗权限,那么我们就可以通过以下方法在wms、ams等系统服务的systemReady()方法中调用即可实现预置应用默认授权浮…...

QT: `long long` 类型转换为 `QString` 2025.6.5
在 Qt 中,将 long long 类型转换为 QString 可以通过以下两种常用方法实现: 方法 1:使用 QString::number() 直接调用 QString 的静态方法 number(),将数值转换为字符串: long long value 1234567890123456789LL; …...

python执行测试用例,allure报乱码且未成功生成报告
allure执行测试用例时显示乱码:‘allure’ �����ڲ����ⲿ���Ҳ���ǿ�&am…...

USB Over IP专用硬件的5个特点
USB over IP技术通过将USB协议数据封装在标准TCP/IP网络数据包中,从根本上改变了USB连接。这允许客户端通过局域网或广域网远程访问和控制物理连接到服务器的USB设备(如专用硬件设备),从而消除了直接物理连接的需要。USB over IP的…...

uniapp手机号一键登录保姆级教程(包含前端和后端)
目录 前置条件创建uniapp项目并关联uniClound云空间开启一键登录模块并开通一键登录服务编写云函数并上传部署获取手机号流程(第一种) 前端直接调用云函数获取手机号(第三种)后台调用云函数获取手机号 错误码常见问题 前置条件 手机安装有sim卡手机开启…...

STM32HAL库USART源代码解析及应用
STM32HAL库USART源代码解析 前言STM32CubeIDE配置串口USART和UART的选择使用模式参数设置GPIO配置DMA配置中断配置硬件流控制使能生成代码解析和使用方法串口初始化__UART_HandleTypeDef结构体浅析HAL库代码实际使用方法使用轮询方式发送使用轮询方式接收使用中断方式发送使用中…...