1. 第四讲(下)_基于滑动窗口算法的VIO系统
1.1. 滑动窗口算法
1.1.1. 图的表示
解释: (假设矩阵左上角元素索引(1,1))
- Λ 1 \Lambda_1 Λ1: (1,2)元素不为空, 表示第1个顶点与第2个顶点之间的残差 r 12 r_{12} r12与顶点1\2有关系
- Λ 2 \Lambda_2 Λ2: (1,3)元素不为空, 表示第1个顶点与第3个顶点之间的残差 r 13 r_{13} r13与顶点1\3有关系
- 这部分可以参考<<视觉SLAM14讲>>
1.1.2. 基于边际概率的滑动窗口算法
根据第四讲(上)部分的叙述, 由于我们只有信息矩阵的数值形式, 但是没有各个状态量 x 1 , x 2 , x 3 x_1,x_2,x_3 x1,x2,x3各自相关的项的区分, 因此老的变量移除就涉及到边际概率信息矩阵的计算.
解释:
- 在丢弃之前, 顶点2,3,4,5都只与顶点1相连
- 丢弃顶点1之后, 利用边际概率方法更新信息矩阵, 更新之后的信息矩阵所对应的图表示: 丢弃顶点1之后, 顶点2,3,4,5之间相互连接起来了.
- 总结: 当给定顶点1的情况下时, 顶点2,3,4,5之间是条件独立的, 当顶点1这个条件去掉之后, 顶点2,3,4,5又相互关联了.
1.1.3. 滑动窗口中的FEJ算法
1.1.3.1. 加入一个新的变量
- 造 成 的 问 题 就 是 : \color{red}{造成的问题就是:} 造成的问题就是: 对于 SLAM 系统而言(如单目 VO), 当我们改变状态量时, 测量不变意味着损失函数不会改变, 更意味着求解最小二乘时对应的信息矩阵 Λ \Lambda Λ存在着零空间。
1.1.3.2. 可观性
1.1.3.3. 举例
解释:
- 单目SLAM: 就是说测量到的姿态和位置都是相对于某个坐标系的, 如果这个坐标系没有固定下来, 那么可以对(姿态,位置以及路标点landmark坐标)都乘以某个变换矩阵 T T T, 而残差函数 e e e的值并不改变.
- 单目+IMU: IMU根据重力可以获取两个绝对的值:roll和pitch, 这两个值是相对于固定的坐标系的, 如东北天导航系. 另外, 由于IMU的测量信息, 把尺度的不确定性也消除了, 使得尺度因子变成可观. 最后: yaw角与重力没有关系, 没有了绝对的观测, 3维的位置也没有像GPS那样的绝对观测, 这4个自由度是不可观的, 即存在不确定性(最终的结果是一个相对的值而不是绝对的值).
1.1.3.4. 滑动窗口中出现的零空间问题
作业
//
// Created by hyj on 18-11-11.
//
#include <iostream>
#include <vector>
#include <random>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
struct Pose //姿态结构体
{
Pose(Eigen::Matrix3d R, Eigen::Vector3d t):Rwc(R),qwc(R),twc(t) {};
Eigen::Matrix3d Rwc;
Eigen::Quaterniond qwc;
Eigen::Vector3d twc;
};
int main()
{
int featureNums = 20; //特征点数
int poseNums = 10; //姿态数
int diem = poseNums * 6 + featureNums * 3; //待优化变量总维度
double fx = 1.;
double fy = 1.;
Eigen::MatrixXd H(diem,diem);
H.setZero();
std::vector<Pose> camera_pose;
double radius = 8;
for(int n = 0; n < poseNums; ++n ) {
double theta = n * 2 * M_PI / ( poseNums * 4); // 总共旋转:1/4 圆弧
// 绕 z轴 旋转
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius, radius * sin(theta), 1 * sin(2 * theta));
camera_pose.push_back(Pose(R,t));
}
// 随机数生成三维特征点
std::default_random_engine generator;
std::vector<Eigen::Vector3d> points;
for(int j = 0; j < featureNums; ++j)
{
std::uniform_real_distribution<double> xy_rand(-4, 4.0);
std::uniform_real_distribution<double> z_rand(8., 10.);
double tx = xy_rand(generator);
double ty = xy_rand(generator);
double tz = z_rand(generator);
//生成世界坐标系下的点
Eigen::Vector3d Pw(tx, ty, tz);
points.push_back(Pw);
for (int i = 0; i < poseNums; ++i) {
// 世界坐标系的点转换到相机坐标系下
Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
Eigen::Vector3d Pc = Rcw * (Pw - camera_pose[i].twc); //Pw= Rwc*pc + twc ==> pc= Rcw(Pw-twc)
double x = Pc.x();
double y = Pc.y();
double z = Pc.z();
double z_2 = z * z;
/// 两个优化变量分别是位姿和世界坐标系3D点(路标)
/// 因此,需要求误差e分别对 位姿6维 和 世界坐标系3D点求偏导
Eigen::Matrix<double,2,3> jacobian_uv_Pc; //这里只是图像点(u,v)对相机坐标系下3D点(x,y,z)求导
jacobian_uv_Pc<< fx/z, 0 , -x * fx/z_2,
0, fy/z, -y * fy/z_2;
// 这里用了链式求导法则来求对世界坐标系3D点求偏导
Eigen::Matrix<double,2,3> jacobian_Pj = jacobian_uv_Pc * Rcw; //实际需要的是 图像点(u,v)对世界坐标系下3D点求导
Eigen::Matrix<double,2,6> jacobian_Ti; //图像点(u,v)对姿态6维求偏导,旋转在前,平移在后,推导见<视觉SLAM第二版>P187
jacobian_Ti << -x* y * fx/z_2, (1+ x*x/z_2)*fx, -y/z*fx, fx/z, 0 , -x * fx/z_2,
-(1+y*y/z_2)*fy, x*y/z_2 * fy, x/z * fy, 0,fy/z, -y * fy/z_2;
/// 请补充完整作业信息矩阵块的计算
// H.block(j*3 + 6*poseNums,j*3 + 6*poseNums,3,3) +=?????
// H.block(i*6,j*3 + 6*poseNums, 6,3) += ???;
H.block(i*6,i*6,6,6) += jacobian_Ti.transpose() * jacobian_Ti; //左上角矩阵块
H.block(j*3 + 6*poseNums,j*3 + 6*poseNums,3,3) +=jacobian_Pj.transpose()*jacobian_Pj; //右下角矩阵块
H.block(i*6,j*3 + 6*poseNums, 6,3) += jacobian_Ti.transpose()*jacobian_Pj; //右上角矩阵块
H.block(j*3 + 6*poseNums,i*6 , 3,6) += jacobian_Pj.transpose() * jacobian_Ti; //左下角矩阵块
}
}
// std::cout << H << std::endl;
// Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(H);
// std::cout << saes.eigenvalues() <<std::endl;
Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
std::cout << svd.singularValues() <<std::endl;
return 0;
}