ROS-3DSLAM(8):视觉部分visual_estimator第二节:initial 2

2024-02-24 05:10

本文主要是介绍ROS-3DSLAM(8):视觉部分visual_estimator第二节:initial 2,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

2021@SDUSC
2021年11月12日星期四——2021年11月13日星期六

一、背景简介:

结束了对于initial_alignment的初步阅读,下一步还要继续对于initial文件夹下的其他文件的分析,毕竟我们的最终目的还是对于函数esitimator中的的processImage()做分析。

这次的计划是针对Initial_ex_rotation和solve_5ph给出分析。

二、Initial_ex_rotation分析:

在这里插入图片描述

A、功能分析

函数的功能是求解从相机(Camera)坐标系到IMU坐标系的相对旋 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-mHOutEzL-1636789051650)(https://www.zhihu.com/equation?tex=R_%7Bbc%7D)],在该函数中用的ric表示的。

经过我的搜索,发现这一部分被称为SLAM的基础的算法,也算是著名的部分经典部分了。

这部分的原理介绍网上有很多,我在这里就不搬过来了,只是简单地给出自己的说明:

首先利用相机两帧的位姿计算出来一个旋转量R1,或利用imu预积分得到R2,经过计算得到R1‘,根据两者之间的差距计算得出最准确的RC。

B、代码分析

1、首先是头文件

开头的注释说明了代码的功能,在后面给出了五个函数的声明(不包含类的构造函数)

/* This class help you to calibrate extrinsic rotation between imu and camera when your totally don't konw the extrinsic parameter */
class InitialEXRotation
{
public:InitialEXRotation();bool CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result);
private:Matrix3d solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres);double testTriangulation(const vector<cv::Point2f> &l,const vector<cv::Point2f> &r,cv::Mat_<double> R, cv::Mat_<double> t);void decomposeE(cv::Mat E,cv::Mat_<double> &R1, cv::Mat_<double> &R2,cv::Mat_<double> &t1, cv::Mat_<double> &t2);int frame_count;vector< Matrix3d > Rc;vector< Matrix3d > Rimu;vector< Matrix3d > Rc_g;Matrix3d ric;
};
2、CalibrationExRotation,最重要的函数
#include "initial_ex_rotation.h"
//构造函数,初始化各个参数,MatrixXd::Identity(rows,cols)               // eye(rows,cols)      //单位矩阵  
InitialEXRotation::InitialEXRotation(){frame_count = 0;Rc.push_back(Matrix3d::Identity());Rc_g.push_back(Matrix3d::Identity());Rimu.push_back(Matrix3d::Identity());ric = Matrix3d::Identity();
}
//先要弄清楚这些传进来的参数是什么, 一共有三个传进来的参数
//corres 两帧之间匹配的特征点 归一化坐标,通过对极约束?,构建本质矩阵E,分解得到两帧之间的旋转矩阵, solveRelativeR(corres) 函数实现,也就是开头提到的通过相机获得的旋转量。
//delta_q_imu 为相邻两时刻 IMU预积分的旋转量,四元数表示传入进来的 匹配的归一化坐标,IMU预计分旋转量,会先存在vector中,因为需要多对信息,构建超定方程矩阵A
//calib_ric_result 为待求的外参数
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{frame_count++;//利用下面的函数计算得到旋转量1Rc.push_back(solveRelativeR(corres));//将原来的四元数转化成了旋转矩阵,方便后面的计算。Rimu.push_back(delta_q_imu.toRotationMatrix());//每帧IMU相对于起始帧IMU的R,初始化为IMU预积分Rc_g.push_back(ric.inverse() * delta_q_imu * ric);//构建公式当中出现的矩阵Eigen::MatrixXd A(frame_count * 4, 4);A.setZero();int sum_ok = 0;for (int i = 1; i <= frame_count; i++){Quaterniond r1(Rc[i]);Quaterniond r2(Rc_g[i]);/* angularDistance就是计算两个坐标系之间相对旋转矩阵在做轴角变换后(u * theta)的角度theta, theta越小说明两个坐标系的姿态越接近,这个角度距离用于后面计算权重,这里计算权重就是为了降低外点的干扰,意思就是为了防止出现误差非常大的R_bk+1^bk和 R_ck+1^ck约束导致估计的结果偏差太大*/double angular_distance = 180 / M_PI * r1.angularDistance(r2);ROS_DEBUG("%d %f", i, angular_distance);//核函数 加权 

这篇关于ROS-3DSLAM(8):视觉部分visual_estimator第二节:initial 2的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/741032

相关文章

如何在Visual Studio中调试.NET源码

今天偶然在看别人代码时,发现在他的代码里使用了Any判断List<T>是否为空。 我一般的做法是先判断是否为null,再判断Count。 看了一下Count的源码如下: 1 [__DynamicallyInvokable]2 public int Count3 {4 [__DynamicallyInvokable]5 get

poj 2976 分数规划二分贪心(部分对总体的贡献度) poj 3111

poj 2976: 题意: 在n场考试中,每场考试共有b题,答对的题目有a题。 允许去掉k场考试,求能达到的最高正确率是多少。 解析: 假设已知准确率为x,则每场考试对于准确率的贡献值为: a - b * x,将贡献值大的排序排在前面舍弃掉后k个。 然后二分x就行了。 代码: #include <iostream>#include <cstdio>#incl

计算机视觉工程师所需的基本技能

一、编程技能 熟练掌握编程语言 Python:在计算机视觉领域广泛应用,有丰富的库如 OpenCV、TensorFlow、PyTorch 等,方便进行算法实现和模型开发。 C++:运行效率高,适用于对性能要求严格的计算机视觉应用。 数据结构与算法 掌握常见的数据结构(如数组、链表、栈、队列、树、图等)和算法(如排序、搜索、动态规划等),能够优化代码性能,提高算法效率。 二、数学基础

笔记整理—内核!启动!—kernel部分(2)从汇编阶段到start_kernel

kernel起始与ENTRY(stext),和uboot一样,都是从汇编阶段开始的,因为对于kernel而言,还没进行栈的维护,所以无法使用c语言。_HEAD定义了后面代码属于段名为.head .text的段。         内核起始部分代码被解压代码调用,前面关于uboot的文章中有提到过(eg:zImage)。uboot启动是无条件的,只要代码的位置对,上电就工作,kern

项目实战系列三: 家居购项目 第四部分

购物车 🌳购物车🍆显示购物车🍆更改商品数量🍆清空购物车&&删除商品 🌳生成订单 🌳购物车 需求分析 1.会员登陆后, 可以添加家居到购物车 2.完成购物车的设计和实现 3.每添加一个家居,购物车的数量+1, 并显示 程序框架图 1.新建src/com/zzw/furns/entity/CartItem.java, CartItem-家居项模型 /***

ROS - C++实现RosBag包回放/提取

文章目录 1. 回放原理2. 回放/提取 多个话题3. 回放/提取数据包,并实时发布 1. 回放原理 #include <ros/ros.h>#include <rosbag/bag.h>#include <std_msgs/String.h>int main(int argc, char** argv){// 初始化ROS节点ros::init(argc, argv,

码蹄集部分题目(2024OJ赛9.4-9.8;线段树+树状数组)

1🐋🐋配对最小值(王者;树状数组) 时间限制:1秒 占用内存:64M 🐟题目思路 MT3065 配对最小值_哔哩哔哩_bilibili 🐟代码 #include<bits/stdc++.h> using namespace std;const int N=1e5+7;int a[N],b[N],c[N],n,q;struct QUERY{int l,r,id;}que

《计算机视觉工程师养成计划》 ·数字图像处理·数字图像处理特征·概述~

1 定义         从哲学角度看:特征是从事物当中抽象出来用于区别其他类别事物的属性集合,图像特征则是从图像中抽取出来用于区别其他类别图像的属性集合。         从获取方式看:图像特征是通过对图像进行测量或借助算法计算得到的一组表达特性集合的向量。 2 认识         有些特征是视觉直观感受到的自然特征,例如亮度、边缘轮廓、纹理、色彩等。         有些特征需要通

ROS - C++实现RosBag包录制

文章目录 1. 录制原理2. 录制多个话题3. 订阅ROS消息,实时录制 1. 录制原理 #include <ros/ros.h>#include <rosbag/bag.h>#include <std_msgs/String.h>int main(int argc, char** argv){// 初始化ROS节点ros::init(argc, argv, "reco

【python计算机视觉编程——7.图像搜索】

python计算机视觉编程——7.图像搜索 7.图像搜索7.1 基于内容的图像检索(CBIR)从文本挖掘中获取灵感——矢量空间模型(BOW表示模型)7.2 视觉单词**思想****特征提取**: 创建词汇7.3 图像索引7.3.1 建立数据库7.3.2 添加图像 7.4 在数据库中搜索图像7.4.1 利用索引获取获选图像7.4.2 用一幅图像进行查询7.4.3 确定对比基准并绘制结果 7.