【SVO】位姿优化及其误差模型

位姿优化及其误差模型

      • 1. calculateFeatureResidualUnitPlane 函数功能
      • 2. calculateFeatureResidualUnitPlane 函数功能实现步骤:
      • 3. 位姿优化误差模型:

1. calculateFeatureResidualUnitPlane 函数功能

计算特征点在单位平面上的残差(residual),并根据给定的测量噪声进行加权处理和误差计算。

输入参数:
- `f`:特征点的方向向量。
- `xyz_in_world`:特征点在世界坐标系下的位置。
- `T_imu_world`:IMU(惯性测量单元)相对于世界坐标系的变换。
- `T_cam_imu`:相机相对于IMU的变换。
- `measurement_sigma`:测量噪声的标准差。
- `robust_weight`:鲁棒加权函数。
- `unwhitened_error`:非白化误差的指针。
- `chi2_error`:卡方误差的指针。
- `H`:Hessian矩阵的指针(可选)。
- `g`:梯度向量的指针(可选)。输出参数:
- `unwhitened_error`:非白化误差,即特征点投影与观测值之间的范数。
- `chi2_error`:卡方误差,即误差平方范数的加权一半。
- `H`:Hessian矩阵,用于优化过程(可选)。
- `g`:梯度向量,用于优化过程(可选)。
- 
void calculateFeatureResidualUnitPlane(const Eigen::Ref<const BearingVector> &f,const Position &xyz_in_world, const Transformation &T_imu_world,const Transformation &T_cam_imu, double measurement_sigma,const PoseOptimizer::RobustWeightFunction &robust_weight, double *unwhitened_error,double *chi2_error, PoseOptimizer::HessianMatrix *H, PoseOptimizer::GradientVector *g) {const Vector3d xyz_in_imu(T_imu_world * xyz_in_world);const Vector3d xyz_in_cam(T_cam_imu * xyz_in_imu);// Prediction error.Eigen::Vector2d e = vk::project2(f) - vk::project2(xyz_in_cam);if (unwhitened_error)*unwhitened_error = e.norm();// Whiten error: R*e, where R is the square root of information matrix// (1/sigma).double R = 1.0 / measurement_sigma;e *= R;// M-estimator weightingdouble weight = robust_weight.weight(e.norm());// Compute log-likelihood : 1/(2*sigma^2)*(z-h(x))^2 = 1/2*e'R'*R*e*chi2_error = 0.5 * e.squaredNorm() * weight;if (H && g) {// compute jacobianPoseOptimizer::Matrix26d J_proj;Frame::jacobian_xyz2uv_imu(T_cam_imu, xyz_in_imu, J_proj);J_proj *= R;H->noalias() += J_proj.transpose() * J_proj * weight;g->noalias() -= J_proj.transpose() * e * weight;}
}  

2. calculateFeatureResidualUnitPlane 函数功能实现步骤:

  1. 将特征点投影到相机坐标系下,得到 xyz_in_cam
  2. 计算预测误差 e,即将特征点在相机坐标系下的投影减去 f(特征点在像素坐标系下的观测值)。
  3. 如果传入了 unwhitened_error 指针,计算非白化误差并保存,即将预测误差的范数作为非白化误差。

unwhitened_error = ∥ e ∥ \text{{unwhitened\_error}} = \|e\| unwhitened_error=e

  1. 对预测误差进行白化处理,乘以信息矩阵的平方根(即测量噪声的倒数),得到白化误差 e

e whitened = R ⋅ e e_{\text{{whitened}}} = R \cdot e ewhitened=Re

其中 R R R 是信息矩阵的平方根, R = 1 measurement_sigma R = \frac{1}{\text{{measurement\_sigma}}} R=measurement_sigma1

  1. 计算加权系数 weight,使用鲁棒加权函数 robust_weight 对白化误差进行加权。

weight = robust_weight ( e whitened ) \text{{weight}} = \text{{robust\_weight}}(e_{\text{{whitened}}}) weight=robust_weight(ewhitened)

  1. 计算卡方误差 chi2_error,即误差平方范数乘以加权系数的一半。

chi2_error = 1 2 ∥ e whitened ∥ 2 ⋅ weight \text{{chi2\_error}} = \frac{1}{2} \|e_{\text{{whitened}}}\|^2 \cdot \text{{weight}} chi2_error=21ewhitened2weight

  1. 如果传入了 Hg 指针,计算雅可比矩阵 J_proj,并根据 J_proj、白化误差 e 和加权系数 weight 来更新 H 矩阵和 g 向量。

3. 位姿优化误差模型:

enum class ErrorType { kUnitPlane, kBearingVectorDiff, kImagePlane };
用于表示错误类型。它定义了三个枚举值:kUnitPlanekBearingVectorDiffkImagePlane

  1. kUnitPlane:表示单位平面误差。在 SVO 中,相机位姿估计通常使用单位平面误差来优化。该误差指的是特征点在三维空间中的投影点与相机成像平面上的对应点之间的差异。

  2. kBearingVectorDiff:表示方向向量差异误差。在 SVO 中,为了提高位姿估计的准确性,常常利用相机的方向向量进行优化。该误差指的是两个方向向量之间的差异。

  3. kImagePlane:表示图像平面误差。在 SVO 中,为了实现视觉里程计(Visual Odometry),需要匹配和跟踪图像中的特征点。该误差指的是特征点在当前帧和参考帧之间的图像平面投影差异。

通过定义这些错误类型的枚举值,可以在不同的阶段和过程中标识和处理不同类型的误差。这有助于优化相机位姿以及实现高精度和鲁棒性的视觉里程计算法。请注意,以上提到的内容仅针对常见情况,具体实现和使用方式可能因不同的 SVO 系统而有所差异。

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

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

iOS--frame和bounds

坐标系 首先&#xff0c;我们来看一下iOS特有的坐标系&#xff0c;在iOS坐标系中以左上角为坐标原点&#xff0c;往右为X正方向&#xff0c;往下是Y正方向如下图&#xff1a; bounds和frame都是属于CGRect类型的结构体&#xff0c;系统的定义如下&#xff0c;包含一个CGPoint…

递归竖栏菜单简单思路

自己的项目要写一个竖栏菜单&#xff0c;所以记录一下思路吧&#xff0c;先粗糙的实现一把&#xff0c;有机会再把细节修饰一下 功能上就是无论这个菜单有多少层级&#xff0c;都能显示出来&#xff0c;另外&#xff0c;需要带图标&#xff0c;基于element-plus写成&#xff0…

ConcurrentHashMap底层具体实现以及实现原理

问题描述 ConcurrentHashMap 底层具体实现以及实现原理 分析维度&#xff1a; 1. ConcurrentHashMap的整体架构 2. ConcurrentHashMap的基本功能 3. ConcurrentHashMap在性能方面的优化 解决方案&#xff1a; ConcurrentHashMap 的整体架构 如图所示&#xff0c;这个是 Concu…

清风数学建模——层次分析法

层次分析法 文章目录 层次分析法评价类问题可以用打分来解决1.通过查阅资料选定指标2.画出权重表格并填写2.1.判断矩阵一致矩阵2.3一致性检验的步骤先算一致性指标CI根据表格查找n对应的RI&#xff08;平均随机一致性指标&#xff09;&#xff0c;表格一般会在题目中给出计算一…

3 PostGIS基础查询

PostGIS 基础查询 数据库维护 ps aux | grep postgrespsql 使用命令登录数据库psql -U postgres -d testdb -h localhost -p 5432postgres用户名&#xff0c;testdb数据库名称&#xff0c;localhost ip地址&#xff0c;可以省略&#xff0c;5432端口&#xff0c;可以省略。 …

【iOS】—— UIKit相关问题

文章目录 UIKit常用的UIKit组件懒加载的优势 CALayer和UIView区别关系 UITableViewUITableView遵循的两个delegate以及必须实现的方法上述四个必须实现方法执行顺序其他方法的执行顺序&#xff1a; UICollectionView和UITableView的区别UICollectionViewFlowLayout和UICollecti…

uniapp scroll-view显示滚动条

在style中添加样式&#xff1a; ::v-deep ::-webkit-scrollbar {/* 滚动条整体样式 */display: block;width: 10rpx !important;height: 10rpx !important;-webkit-appearance: auto !important;background: transparent;overflow: auto !important;}::v-deep ::-webkit-scroll…

oracle rman不能自动删除归档日志备份解决

发现在日常备份中&#xff0c;rman无法将过期的归档日志备份删除&#xff0c;查相关资料&#xff0c; delete noprompt backup completed before sysdate-2; 可通过该语句将所有备份记录删除&#xff0c;包括归档日志备份。 整理的脚本如下&#xff1a; 10 20 * * * su - or…

【Java】详解volatile和synchronized关键字

volatile和synchronized都是Java中用于控制并发的关键字&#xff0c;但是它们的使用场景和原理是不同的。 volatile关键字&#xff1a; 特点&#xff1a;volatile关键字主要有两个特性&#xff1a;保证变量的可见性和防止指令重排。当一个共享变量被volatile修饰时&#xff0c…

大数据Flink(五十一):Flink的引入和Flink的简介

文章目录 Flink的引入和Flink的简介 一、Flink的引入 1、第1代——Hadoop MapReduce

CentOS 8 服务器安装 MySQL 报错:no match mysql-community-server

参考 MySQL 官方文档&#xff1a; Re: No match for argument: mysql-community-serverMySQL yum 安装文档 报错如下&#xff1a; 1.No match for argument: mysql-community-server 2.Error: Unable to find a match: mysql-community-server上面的错误都提示找不到 mysql-…

RK809 电源管理芯片配置3-RK3568

提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档 文章目录 前言RK809 芯片概述认识DCDC和LDORK809的引脚RK809 的功能电源管理的概念配置内核驱动电源域IO 电源域配置方法前言 前面提到编译的时候需要正确配置电源,RK3568的电源管理芯片是RK809,下面就来…

MyBatis 核心组件 —— Configuration

概述 Mybatis 的核心组件如下所示&#xff1a; Configuration&#xff1a;用于描述 MyBatis 的主配置信息&#xff0c;其他组件需要获取配置信息时&#xff0c;直接通过 Configuration 对象获取。除此之外&#xff0c;MyBatis 在应用启动时&#xff0c;将 Mapper 配置信息、类…

精选算法题(4)——字符串比较

作者&#xff1a;翟天保Steven 版权声明&#xff1a;著作权归作者所有&#xff0c;商业转载请联系作者获得授权&#xff0c;非商业转载请注明出处 题目描述&#xff1a; 好久没做题目了&#xff0c;近期刷抖音碰到一个题目&#xff0c;乍一看不是很难&#xff0c;但是手生了&a…

HTTP——HTTP报文内的HTTP信息

HTTP 通信过程包括从客户端发往服务器端的请求及从服务器端返回客户端的响应。本章就让我们来了解一下请求和响应是怎样运作的。 HTTP 一、HTTP报文二、请求报文及响应报文的结构三、编码提升传输速率1、报文主体和实体主题的差异2、压缩传输的内容编码3、分割发送的分块传输编…

HCIP——前期综合实验

前期综合实验 一、实验拓扑二、实验要求三、实验思路四、实验步骤1、配置接口IP地址2、交换机配置划分vlan10以及vlan203、总部分部&#xff0c;骨干网配置OSPF分部总部骨干网 4、配置BGP建立邻居关系总部骨干网分部 5、发布用户网段6、将下一跳改为本地7、允许AS重复8、重发布…

【Linux】 UDP网络套接字编程

&#x1f34e;作者&#xff1a;阿润菜菜 &#x1f4d6;专栏&#xff1a;Linux系统网络编程 文章目录 一、网络通信的本质&#xff08;port标识的进程间通信&#xff09;二、传输层协议UDP/TCP认识传输层协议UDP/TCP网络字节序问题&#xff08;规定大端&#xff09; 三、socket编…

前端面试题-react

1 React 中 keys 的作⽤是什么&#xff1f; Keys 是 React ⽤于追踪哪些列表中元素被修改、被添加或者被移除的辅助标识在开发过程中&#xff0c;我们需要保证某个元素的 key 在其同级元素中具有唯⼀性。在 React Diff 算法中 React 会借助元素的 Key 值来判断该元素是新近创建…

第十三章 利用PCA简化数据

文章目录 第十三章 利用PCA简化数据13.1降维技术13.2PCA13.2.1移动坐标轴 13.2.2在NumPy中实现PCA13.3利用PCA对半导体制造数据降维 第十三章 利用PCA简化数据 PCA&#xff08;Principal Component Analysis&#xff0c;主成分分析&#xff09;是一种常用的降维技术&#xff0…

剑指offer41.数据流中的中位数

我一开始的想法是既然要找中位数&#xff0c;那肯定要排序&#xff0c;而且这个数据结构肯定要能动态的添加数据的&#xff0c;肯定不能用数组&#xff0c;于是我想到了用优先队列&#xff0c;它自己会排序都不用我写&#xff0c;所以addNum方法直接调用就可以&#xff0c;但是…