VIO--后端优化实践（坑）
一. 非线性最小 二乘求解
二. 代码详细解读
顶点的通用形式：
class 顶点类名 : public Vertex {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;顶点类名() : Vertex(顶点中待优化变量的个数) {}};边的通用形式：
class 边的类名 : public Edge {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;边的类名(传入输入量): Edge构造函数 {输入量处理}/// 计算残差virtual void ComputeResidual() override;/// 计算雅可比virtual void ComputeJacobians() override;private:输入量
};因为
残差 = 观测值 - function（输入值）
输入值通过构造边的时候传入
观测值一般通过set方法得到
再理一下存储关系：
相机帧：存储特征点的归一化相机坐标
points：存储特征点的世界坐标
逆深度集合：各个特征点坐标在第0帧相机坐标系下的z轴值的倒数
2.1 生成虚拟化的相机位姿和特征点
并且假设特征点能被所有相机观测到
GetSimDataInWordFrame(cameras, points);相机观测到的特征点是归一化了的世界坐标
单独的points还是世界坐标
2.2. 创建一个SLAM求解器问题
Problem problem(Problem::ProblemType::SLAM_PROBLEM);
2.3. 创建位姿顶点
vector<shared_ptr<VertexPose> > vertexCams_vec;for (size_t i = 0; i < cameras.size(); ++i) {shared_ptr<VertexPose> vertexCam(new VertexPose());Eigen::VectorXd pose(7);pose << cameras[i].twc, cameras[i].qwc.x(), cameras[i].qwc.y(), cameras[i].qwc.z(), cameras[i].qwc.w();vertexCam->SetParameters(pose);// if(i < 2)
// vertexCam->SetFixed();problem.AddVertex(vertexCam);vertexCams_vec.push_back(vertexCam);}（1）首先创建一个存放位姿顶点指针的容器
（2）再把相机的位姿值传给位姿顶点
（3）往问题里面添加顶点
（4）再把顶点添加到容器里面
2.4. 针对每一个特征点，进行如下操作
Eigen::Vector3d Pc = cameras[0].Rwc.transpose() * (Pw - cameras[0].twc);已知第0帧相机坐标系到世界坐标系的变换矩阵，把世界坐标变换到第0帧的相机坐标系下。
double inverse_depth = 1. / (Pc.z() + noise);计算逆深度，并把逆深度放到容器noise_invd里面
shared_ptr<VertexInverseDepth> verterxPoint(new VertexInverseDepth());创建逆深度顶点指针
VecX inv_d(1);inv_d << inverse_depth;verterxPoint->SetParameters(inv_d);problem.AddVertex(verterxPoint);allPoints.push_back(verterxPoint);给逆深度顶点赋值，并且把这个顶点添加到问题里面。20个特征点，就有20个逆深度。
2.4.1. 针对第1帧以及第2帧的相机（除了第0帧）
首先我们要知道：
Frame类里面的featurePerId是无序map容器，map容器就是键值对
Eigen::Vector3d pt_i = cameras[0].featurePerId.find(i)->second;
Eigen::Vector3d pt_j = cameras[j].featurePerId.find(i)->second;由这两句代码就得去看一下键值对是如何存储的，因为这里两个取出来的特征点值不一样了
for (int i = 0; i < poseNums; ++i) {// pw = Rwc pc + twcEigen::Vector3d Pc = cameraPoses[i].Rwc.transpose() * (Pw - cameraPoses[i].twc);Pc = Pc / Pc.z(); // 归一化图像平面Pc[0] += noise_pdf(generator);Pc[1] += noise_pdf(generator);cameraPoses[i].featurePerId.insert(make_pair(j, Pc));}由上面的代码分析得，相机位姿内部存储的特征点，虽然索引相同意味着是同一个世界坐标点，但是变成各自的相机坐标系之后的值是不同的了。
因此回过头来看上面两行代码，实现的作用是取出同一个世界坐标点，在第0帧相机坐标系下的相机坐标，和第一帧或者第二帧坐标系下的相机坐标。
shared_ptr<EdgeReprojection> edge(new EdgeReprojection(pt_i, pt_j));edge->SetTranslationImuFromCamera(qic, tic);std::vector<std::shared_ptr<Vertex> > edge_vertex;edge_vertex.push_back(verterxPoint);edge_vertex.push_back(vertexCams_vec[0]);edge_vertex.push_back(vertexCams_vec[j]);edge->SetVertex(edge_vertex);problem.AddEdge(edge);再接着创建边-EdgeReprojection。
注意这个边的输入是同一个世界坐标点在不同相机帧下的相机坐标，并且都是第一帧跟其他帧两两配对形成的边。
又创建了一个容器edge_vertex，内部存放的元素是用来指向顶点的指针。
依次添加此特征点的逆深度顶点，第0帧相机位姿，以及构成残差的帧的相机位姿。
把这个容器添加给边。g2o是一个一个添加的，它这里是一次性添加。
最后再给问题添加边。
接下来就得关注这份代码边里面的残差构成形式，以及雅可比矩阵构成的形式了
2.5. 分析边里面计算残差是如何计算的（为了更加方便理解，这里固定一下各个参数）
由于我目前不清楚计算残差的数学公式是什么，因此只能从代码进行分析解读
首先由设置边代码，点进去看源码
edge->SetVertex(edge_vertex); /*** 设置一些顶点* @param vertices 顶点，按引用顺序排列* @return*/bool SetVertex(const std::vector<std::shared_ptr<Vertex>> &vertices) {verticies_ = vertices;return true;}以上就看到了跟g2o类似的量，verticies_，边与顶点就是通过这个桥梁进行链接的。
这里我们要注意
verticies_[0]是指向第0个特帧点在第0帧相机坐标系下的z轴逆深度的顶点 指针
（之后所有特征点的世界坐标都要变为第0帧坐标系下的相机坐标）
verticies_[1]是指向第0帧相机位姿的顶点指针
为了具体化，这里让verticies_[2]是指向第1帧相机位姿的顶点指针
接下来再进入到残差部分的代码，如下所示，反正第一眼挺懵的。但是看到了熟悉的verticies_变量。
void EdgeReprojection::ComputeResidual() {double inv_dep_i = verticies_[0]->Parameters()[0];VecX param_i = verticies_[1]->Parameters();Qd Qi(param_i[6], param_i[3], param_i[4], param_i[5]);Vec3 Pi = param_i.head<3>();VecX param_j = verticies_[2]->Parameters();Qd Qj(param_j[6], param_j[3], param_j[4], param_j[5]);Vec3 Pj = param_j.head<3>();Vec3 pts_camera_i = pts_i_ / inv_dep_i;Vec3 pts_imu_i = qic * pts_camera_i + tic;Vec3 pts_w = Qi * pts_imu_i + Pi;Vec3 pts_imu_j = Qj.inverse() * (pts_w - Pj);Vec3 pts_camera_j = qic.inverse() * (pts_imu_j - tic);double dep_j = pts_camera_j.z();residual_ = (pts_camera_j / dep_j).head<2>() - pts_j_.head<2>(); /// J^t * J * delta_x = - J^t * r
// residual_ = information_ * residual_; // remove information here, we multi information matrix in problem solver
}
首先分析这一句代码
double inv_dep_i = verticies_[0]->Parameters()[0];我们查看verticies_的类型，进而知道verticies_[0]是指向类Vertex的指针
std::vector<std::shared_ptr<Vertex>> verticies_又查看类Vertex的成员函数
VecX Parameters() const { return parameters_; }查看parameters_的类型
VecX parameters_;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VecX;由此可以知道verticies_[0]->Parameters()[0] 最后取出了逆深度值
接下来下面这一部分代码就返回取出了第0帧相机位姿四元数，Qi就是四元数。
param_i.head<3>()这一句代码就超出了我目前的认知了，param_i是VecX类型，也就是Matrix类型，这种类型的head是什么鬼？代表提取vector的前3个元素
这也代表了verticies_[0]->parameters()返回值类型长下面这个样子
x,y,z（相机原点平移量）, i,j,k,w（相机旋转四元数）
VecX param_i = verticies_[1]->Parameters();Qd Qi(param_i[6], param_i[3], param_i[4], param_i[5]);Vec3 Pi = param_i.head<3>();接下来同上，提取第1帧相机的位姿。
之后到这一句代码
Vec3 pts_camera_i = pts_i_ / inv_dep_i;看到pts_i_联想到输入值：
EdgeReprojection(const Vec3 &pts_i, const Vec3 &pts_j)
: Edge(2, 3, std::vector<std::string>{"VertexInverseDepth", "VertexPose", "VertexPose"}) {
pts_i_ = pts_i;
pts_j_ = pts_j;
}
然后再联想到创建边时的代码
shared_ptr<EdgeReprojection> edge(new EdgeReprojection(pt_i, pt_j));
因此，pts_i_是第0个特征点在第0帧相机帧下的特征点，数值是归一化相机坐标
pts_j_是第0个特征点在第1帧相机帧下的特征点，数值是归一化相机坐标
而inv_dep_i是第0个特征点在第0帧相机坐标系下的逆深度值，数值意义是相机坐标
所以pts_camera_i是第0个特征点在第0帧相机坐标下的相机坐标
再进入到下一行代码：
Vec3 pts_imu_i = qic * pts_camera_i + tic;这代表要把相机坐标转换成imu系坐标，原代码设置了相机坐标系和imu坐标系重合
edge->SetTranslationImuFromCamera(qic, tic);其中qic = （1，0，0，0），tic=（0，0，0）
又进行如下的代码
Vec3 pts_w = Qi * pts_imu_i + Pi;已知Qi是第0帧相机帧的位姿，把非惯性系坐标转为惯性系（即世界坐标系）
之后又把惯性系下的坐标转换为第1帧非惯性系下的坐标
Vec3 pts_imu_j = Qj.inverse() * (pts_w - Pj);之后又把imu系下坐标转换到相机坐标系下的坐标
Vec3 pts_camera_j = qic.inverse() * (pts_imu_j - tic);然后进行如下操作
double dep_j = pts_camera_j.z();residual_ = (pts_camera_j / dep_j).head<2>() - pts_j_.head<2>(); 具体就是，把经过投影得到的相机坐标归一化之后，与观测得到的归一化相机坐标作差，得到残差。
至此，残差的外貌也显现出来了，如下图所示：
紧接着，我们就能够写出对应的雅可比矩阵。如下所示：
2.6. 还是得分析雅可比矩阵是怎么构造的
这里就是第3章PPT有的，推导比较复杂，暂时跳过。
2.7. 接下来关注problem的Solve函数是怎么运行的
2.7.1 进入到构造hessian矩阵
hessian里面的逆深度雅可比矩阵为什么是1维的，不懂？
不是一维的，是你理解错了
进入到这个构造雅可比的函数
void EdgeReprojection::ComputeJacobians()
这里面的雅可比矩阵的维数是 2x13维，这还只是一小块
注意，不能看作2x13维，只能看作2x1，2x6，2x6维
根据你博客里面写到的，你需要确定待优化变量是如何排序的。
edge.second->ComputeResidual();edge.second->ComputeJacobians();auto jacobians = edge.second->Jacobians();auto verticies = edge.second->Verticies();当你取出这里的顶点时，你需要回看一下当初构建边时的顶点。
以下所示，3个顶点，一个边含有，三个顶点，顶点的顺序是：
第0个顶点-----特征点在第0帧相机坐标系下的逆深度。
第1个顶点-----第0帧相机坐标系的位姿。
第2个顶点-----第i帧相机坐标系的位姿。
edge_vertex.push_back(verterxPoint);edge_vertex.push_back(vertexCams_vec[0]);edge_vertex.push_back(vertexCams_vec[j]);然后还需要看一下有多少个残差块，总共有40个残差块。
一个特征点，分到两个残差块里面。
待优化向量有几个，决定了雅可比矩阵的维数。
待优化变量有3个位姿，18个变量，20个特征点逆深度，因此总共有38个待优化变量，雅可比矩阵就有38维。
for (size_t i = 0; i < verticies.size(); ++i) {auto v_i = verticies[i];if (v_i->IsFixed()) continue;上面的for循环，从残差边里面一个顶点一个顶点地拿，第一个顶点是逆深度，第二个顶点是第0帧的位姿，第3个顶点是其他帧的位姿顶点，
又根据 函数SetOrdering()里面的verticies_通过debug手段，可以发现，第0帧相机位姿排在第一个位置，其次是第1帧相机位姿，依次类推。
if (err_prior_.rows() > 0) {b_prior_ -= H_prior_ * delta_x_.head(ordering_poses_); // update the error_prior}Hessian_.topLeftCorner(ordering_poses_, ordering_poses_) += H_prior_;b_.head(ordering_poses_) += b_prior_;delta_x_ = VecX::Zero(size); // initial delta_x = 0_n;上面这一部分先验似乎一开始都为0
接下来进入到这一个函数
ComputeLambdaInitLM()VIO--后端优化实践（坑）相关推荐
- [从零手写VIO|第五节]——后端优化实践——单目BA求解代码解析
长篇警告⚠⚠⚠ 目录 solver 全流程回顾 Solver三要素 Solver求解中的疑问 核心问题 代码解析 1. TestMonoBA.cpp 2. 后端部分: 2.1 顶点 2.2 边(残差) ...
- 时间都去哪了-移动Web首屏优化实践
时间都去哪了-移动Web首屏优化实践 首屏时间 可用性的前提,后面用户是否使用你app很重要的因素之一: 我们PC上访问其实现在的带宽已经很好了,百兆光宽带,但是在移动端就不一样了,很多用户还是使用的 ...
- 兴趣部落的前端性能优化实践概览
本文对兴趣部落项目前端开发中使用到的性能优化方式进行总结.兴趣部落项目是手机QQ(以下简称手Q)中最大的纯网页应用,每日有大量的用户访问,对于腾讯这样一个对产品有着极致要求的公司,性能优化是一个绕不开 ...
- 《淘宝首页性能优化实践》文章阅读
想必很多人都已经看到了新版的淘宝首页,它与以往不太一样,这一版页面中四处弥散着个性化的味道,由于独特的个性化需求,前端也面临各方面的技术挑战: 数据来源多 串行请求渲染一个模块 运营数据和个性化数据匹 ...
- 基于ceres的后端优化的代码实现
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 来源丨从零开始搭SLAM 作者丨李太白lx 由于g2o天然是进行位姿图优化的, 所以十分契合karto ...
- 陆金所 CAT 优化实践
1 背景 CAT 介绍 CAT (Central Application Tracking)是一个实时监控系统,由美团点评开发并开源,定位于后端应用监控.应用集成客户端的方式上报中间件和业务数据,支持 ...
- 从工具到社区，美图秀秀大规模性能优化实践
导读:本文由演讲整理而成.美图秀秀社区自上线以来已经有近一年时间,不管是秀秀海量的用户还是图片社区特有的形态都给性能优化提出了巨大的挑战.本文将会结合这一年内我们遇到的具体案例和大家分享下美图秀秀社区 ...
- 高并发IM系统架构优化实践
互联网+时代,消息量级的大幅上升,消息形式的多元化,给即时通讯云服务平台带来了非常大的挑战.高并发的IM系统背后究竟有着什么样的架构和特性? 以上内容由网易云信首席架构师内部分享材料整理而成 相关阅读 ...
- 前端性能优化实践 | 百度APP个人主页优化
性能是每个前端工程师都应该关注的话题,通用的优化手段已有许多文章和实践,就不再赘述,本篇以百度 App 个人主页为例,聊聊针对业务特点进行的一些性能优化实践.适用于:传统意义的优化手段能用的都用了:打 ...
- 研效优化实践：Python单测——从入门到起飞
作者:uniquewang,腾讯安全平台后台开发工程师 福生于微,积微成著,一行代码的精心调试,一条指令的细心验证,一个字节的研磨优化,都是影响企业研发效能工程的细节因素.而单元测试,是指针对软件中的 ...
最新文章
- Java 之String、StringBuffer 和 StringBuilder 三者区别介绍
- 来领.NET Core学习资料，7天整理了30多个G（适合各阶段.Net开发者）
- linux 装nano命令,vim、nano在命令行上如何编辑文件
- 程序员面试金典——7.5平分的直线
- 从海康7816的ps流里获取数据h264数据
- 数据通信的基础知识（计算机网络 谢希仁）
- Android学习别“走弯路”，移动端混合开发框架
- Python实现王者农药自动刷金币
- Linux 之十五 Kernel 仓库、Kernel 协作方式、订阅邮件列表、提交 PATCH
- 正确区分标识（zhi）符、关键字与保留字
- 天牛须算法(BAS)python实现
- 更精确的新旧中国居民身份证号码验证算法
- python第三方库re库实例之爬取古诗词网上诗歌
- hdmi怎么支持2k分辨率_为什么显示器闪瞎眼 HDMI线版本有讲究
- python代码画猪头_如何用python画猪头
- 计算机编程平方怎么按,电脑键盘上怎么打平方,次方之类的
- Swiper + 图片懒加载
- 西门子plc s7-200写的先进先出范例 用fifo
- 海康iv4200支持多少_追赶极速：海康威视C2000 Pro 2T固态硬盘到手简评
- 输出亲朋字符串（C语言）