SVO 论文与代码分析总结

[TOC]

概述

SVO: Semi-direct Monocular Visual Odometry

  • 论文: SVO: Fast Semi-Direct Monocular Visual Odometry
  • 代码(注释版):cggos/svo_cg

SVO结合了直接法和特征点法,称为 半直接单目视觉里程计

初始化

获取第一关键帧和第二关键帧的相对位姿,并建立初始地图,代码主要在 initialization.cpp 中。

(1)处理第一帧 FrameHandlerMono::processFirstFrame

  • 第一帧位姿为单位阵 new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero())
  • addFirstFrame
    • detectFeatures: 获取 Shi-Tomas得分较高 且 均匀分布 的 FAST角点,并创建 Features
    • 初始化 px_vec(Coordinates in pixels on pyramid level 0) 和 f_vec(Unit-bearing vector of the feature)
  • 将当前帧设置为关键帧,并检测5个对应的关键点
  • 添加关键帧到地图 map_.addKeyframe(new_frame_);

(2)处理第二帧 FrameHandlerMono::processSecondFrame

  • addSecondFrame
    • 光流跟踪trackKlt
    • 计算 单应性矩阵(假设局部平面场景),估计相对位姿T_cur_from_ref_
    • 创建初始地图
  • BA优化 ba::twoViewBA
  • 添加关键帧到地图 map_.addKeyframe(new_frame_);
  • 添加关键帧到深度滤波器 depth_filter_->addKeyframe

位姿估计

代码主框架在 FrameHandlerMono::processFrame()

稀疏图像对齐(直接法)

代码主要在 SparseImgAlign

使用 稀疏直接法 计算两帧之间的初始相机位姿T_cur_from_ref:根据last_frame_的特征点 和 last_frame_new_frame_的相对位姿,建立损失函数(两帧之间稀疏的4x4的patch的光度误差)SparseImgAlign::computeResiduals,使用G-N优化算法获得两帧之间的位姿变换,没有特征匹配过程,效率较高。

\[T_{k,k-1} = \arg \min_{T_{k,k-1}} \frac{1}{2} \sum_{i} \| \delta I(T_{k,k-1}, u_i) \|^2\]

其中,

\[\delta I(T, u) = I_k(\pi(T \cdot {\pi}^{-1} (u,d_u))) - I_{k-1}(u)\]
  • patch选取了4x4的大小,忽略patch的变形,并且没有做仿射变换(affine warp),加快了计算速度
  • 在几个图像金字塔层之间迭代优化,从金字塔的最顶层(默认是第五层klt_max_level)逐层计算(默认计算到第3层klt_min_level
  • 广泛使用指针,比直接取值速度更快
  • 逆向组合(inverse compositional)算法,预先计算雅克比矩阵precomputeReferencePatches,节省计算量
  • 几处双线性插值方法对提高精度有帮助

最后,cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_;

特征对齐(光流法)

代码入口在 Reprojector::reprojectMap

通过上一步的帧间匹配能够得到当前帧相机的位姿,但是这种frame to frame估计位姿的方式不可避免的会带来累计误差从而导致漂移。

稀疏图像对齐之后使用 特征对齐,即通过地图向当前帧投影,并使用 逆向组合光流 以稀疏图像对齐的结果为初始值,完成基于patch的特征匹配,得到更精确的特征位置。

基于 光度不变性假设,特征块在以前参考帧中的亮度应该和new frame中的亮度差不多,所以重新构造一个残差,对特征预测位置进行优化(代码主要在 Matcher::findMatchDirect):

\[u_i' = \arg \min_{u_i'} \frac{1}{2} \| I_k(u_i') - A_i \cdot I_r(u_i) \|^2\]
  • 优化变量是 像素位置
  • 光度误差的前一部分是当前图像中的亮度值;后一部分不是 $I_{k-1}$ 而是 $I_r$,即它是根据投影的3D点追溯到其所在的KF中的像素值
  • 选取了8x8的patch,由于是特征块对比并且3D点所在的KF可能离当前帧new frame比较远,所以光度误差还加了一个仿射变换 $A_i$,可以得到亚像素级别的精度
  • 对于每个特征点单独考虑,找到和当前帧视角最接近的共视关键帧getCloseViewObs(这个关键帧和当前帧的视角差别越小,patch的形变越小,越可以更精准地匹配)

该过程通过 inverse compositional Lucas-Kanade algorithm 求解,得到优化后更加准确的特征点预测位置。

位姿和结构优化(特征点法)

代码主要在 pose_optimizer::optimizeGaussNewtonFrameHandlerBase::optimizeStructure

motion-only BA

\[T_{k,w} = \arg \min_{T_{k,w}} \frac{1}{2} \sum_{i} \| u_i - \pi(T_{k,w} \cdot p_i) \|^2\]
  • 优化变量是 相机位姿

structure-only BA

  • 优化变量是 三维点坐标

local BA

  • 附近关键帧和可见的地图点都被优化了,这一小步在fast模式下是不做的

重定位

代码主要在 FrameHandlerMono::relocalizeFrame

重定位效果一般,有待改进。

建图(深度滤波器)

SVO把像素的深度误差模型看做概率分布,使用 高斯——均匀混合分布的逆深度(深度值服从高斯分布,局外点的概率服从Beta分布),称为 深度滤波器Depth Filter,每个特征点作为 种子Seed(深度未收敛的像素点)有一个单独的深度滤波器。

  • 初始化种子:如果进来一个 关键帧,就提取关键帧上的新特征点,初始化深度滤波器,作为种子点放进一个种子队列中
  • 更新种子:如果进来一个 普通帧,就用普通帧的信息,更新所有种子点的概率分布;如果某个种子点的深度分布已经收敛,就把它放到地图中,供追踪线程使用

初始化种子

代码主要在 DepthFilter::initializeSeeds

  • 当前帧上已经有的特征点,占据住网格
  • 多层金字塔FAST特征检测并进行非极大值抑制,映射到第0层网格,每个网格保留Shi-Tomas分数最大的点
  • 对于所有的新的特征点,初始化成种子点Seed
    • 高斯分布均值:mu = 1.0/depth_mean
    • 高斯分布方差:sigma2 = z_range*z_range/36,其中 z_range = 1.0/depth_min

更新种子(深度滤波)

深度滤波 DepthFilter::updateSeeds 主要过程:

  • 极线搜索与三角测量 Matcher::findEpipolarMatchDirect
  • 计算深度不确定度 DepthFilter::computeTau
  • 深度融合,更新种子 DepthFilter::updateSeed
  • 初始化新的地图点,移除种子点

深度估计值(极线搜索)

  • 根据 深度均值mu和深度方差sigma2确定深度范围 $[d_{min},d_{max}]$,计算极线
  • 计算仿射矩阵 warp::getWarpMatrixAffine(帧间图像发生旋转)
  • 极线上搜索匹配:通过RANSAC计算ZMSSD获取最佳匹配点坐标
    • 当计算的极线长度小于两个像素时,直接采用下一步的图像对齐
    • 否则,继续沿极线搜索匹配
  • 光流法亚像素精度提取(图像对齐) feature_alignment::align2D
  • 三角法恢复深度Z depthFromTriangulationTriangulate in SVO

深度估计不确定度

REMODE 对由于特征定位不准导致的三角化深度误差进行了分析

假设 焦距为 $f$,像素扰动为 px_noise = 1.0,角度变化量 px_error_angle 为 $\delta \beta$ 则

\[\tan \frac{\delta \beta}{2} = \frac{1.0}{2f}\]

\[\delta \beta = 2 \arctan \frac{1.0}{2f}\]

\[\beta^{+} = \beta + \delta \beta\] \[\gamma^{+} = \pi - \alpha - \beta^{+}\]

已知 $t$ 为 T_ref_cur.translation(),根据正弦定理

\[Z^{+} = \|t\| \frac{\sin \beta^{+}}{\sin \gamma^{+}}\]

所以,深度不确定度 tau 为 $\tau = Z^{+} - Z$

逆深度不确定度 tau_inverse 为:

double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));

逆深度标准差

\[\tau_{inv} = \frac{1}{2} (\frac{1}{Z-\tau} - \frac{1}{Z+\tau})\]

深度融合

代码主要在 void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)

SVO的融合是不断利用最新时刻深度的观测值,来融合上一时刻深度最优值,直至深度收敛。

通过上面两步得到 逆深度测量值 $x$ 1./z逆深度不确定度 $\tau_{inv}$,则 逆深度 服从 高斯分布

\[N(x, \tau_{inv}^2)\]

SVO 采用 Vogiatzis的论文 Video-based, real-time multi-view stereo 提到的概率模型,使用 高斯–均匀混合分布的深度滤波器,根据 $x$ 和 $\tau_{inv}$ 更新以下四个参数:

  • 逆深度高斯分布的均值 $\mu$ seed->mu
  • 逆深度高斯分布的方差 $\sigma^2$ seed->sigma2
  • Beta分布的 $a$ seed->a
  • Beta分布的 $b$ seed->b

更新过程(Vogiatzis的Supplementary matterial)如下:

其中,论文公式19有误(代码正确),应为

\[\frac{1}{s^2} = \frac{1}{\sigma^2} + \frac{1}{\tau_{inv}^2}\]

最终,得到收敛的逆深度的最佳估计值 seed->mu

新的地图点

如果种子点的方差seed->sigma2,小于深度范围/200的时候,就认为收敛了,它就不再是种子点,而是TYPE_CANDIDATE点。

TYPE_CANDIDATE点被成功观察到1次,就变成TYPE_UNKNOWN点;TYPE_UNKNOWN被成功观察到10次,就变成TYPE_GOOD点;如果多次应该观察而没有被观察到,就变成TYPE_DELETED点。

总结与讨论

  • 特征点提取在地图线程,跟踪使用光流法
  • 后端的特征点只在关键帧上提取,用FAST加金字塔
  • 上一个关键帧的特征点在这一个关键帧上找匹配点的方法,是用极线搜索,寻找亮度差最小的点,最后再用深度滤波器把这个地图点准确地滤出来
  • 重定位比较简单,没有回环
  • 深度滤波器使用的是高斯–均匀混合分布
  • SVO对PTAM的改进主要在两个方面:高效的特征匹配、鲁棒的深度滤波器
  • 在Tracking线程,当前帧的特征点是从上一帧用光流法传递过来的,只有在Mapping线程DepthFilter插入新关键帧时才需要提取特征点,不需要每一帧都提取特征点
  • PTAM和ORB-SLAM只用两帧图像三角化出地图点,只要地图点没有被判定为外点,就固定不变了(除非BA阶段调整);而SVO的深度滤波器会根据多帧图片不断收敛地图点的不确定度,从而得到更可靠的地图点。因为地图点更可靠,所以SVO只需要维护更少的地图点(PTAM一般维护约160到220个特征点,SVO在fast模式下维护约120个地图点),从而加快了计算速度。

参考文献

Read More

视觉SLAM位姿估计(总结)

[TOC]

相关代码:

Features Based Method

2D-2D: Epipolar Geometry

2D-2D相机位姿估计 通常利用 对极几何 进行计算,是单目SLAM初始化时的关键技术。

2D-2D 对极几何 主要涉及到基础矩阵、本质矩阵和单应性矩阵的求解,并从中恢复出旋转矩阵 $R$ 和平移向量 $t$。

同时,还要根据匹配的特征点和计算出的相对位姿进行三角化,恢复出 3D空间点

在单目视觉SLAM中,以上过程主要用于SLAM的初始化:计算第二关键帧的相对位姿(假设第一帧位姿为 $[I \quad 0]$),并计算初始Map。

  • 计算 基础矩阵或本质矩阵 适用于特征点不共面的情况,计算 单应矩阵 适用于特征点共面的情况

  • 当 特征点共面 或者 相机发生纯旋转 时,基础矩阵 $F$ 的自由度下降,就会出现所谓的 退化(degenerate)。为了能够避免退化现象的影响,通常会 同时估计基础矩阵 $F$ 和 单应矩阵 $H$,选择重投影误差比较小的那个作为最终的运动估计矩阵

  • 平移向量t 的 尺度不确定性
  • 初始化的纯旋转问题:单目初始化不能只有旋转,必须要有一定程度的平移,否则由于t趋近于0,导致无从求解R或者误差非常大
  • 多于8对点:RANSAC

OpenCV 中相关函数:

  • findFundamentalMat
  • findEssentialMat
  • findHomography
  • recoverPose
  • decomposeEssentialMat
  • triangulatePoints

相关参考代码:

vector<Point2f> pts1;
vector<Point2f> pts2;
for ( int i = 0; i < ( int ) matches.size(); i++ ) {
    pts1.push_back(keypoints_1[matches[i].queryIdx].pt);
    pts2.push_back(keypoints_2[matches[i].trainIdx].pt);
}

Mat matF = findFundamentalMat(pts1, pts2, CV_FM_8POINT);

double cx = K.at<double>(0,2);
double cy = K.at<double>(1,2);
double fx = K.at<double>(0,0);
Mat matE = findEssentialMat(pts1, pts2, fx, Point2d(cx,cy));

Mat matH = findHomography(pts1, pts2, RANSAC, 3);

recoverPose(matE, pts1, pts2, R, t, fx, Point2d(cx,cy));

Ref: 2D-2D相机位姿估计

3D-2D: PnP

Perspective-n-Point is the problem of estimating the pose of a calibrated camera given a set of n 3D points in the world and their corresponding 2D projections in the image. [Wikipedia]

PnP(Perspective-n-Point) 是求解3D到2D点对运动的方法,求解PnP问题目前主要有直接线性变换(DLT)、P3P、EPnP、UPnP以及非线性优化方法。

在双目或RGB-D的视觉里程计中,可以直接使用PnP估计相机运动;而在单目视觉里程计中,必须先进行初始化,然后才能使用PnP。

在SLAM中,通常先使用 P3P或EPnP 等方法估计相机位姿,再构建最小二乘优化问题对估计值进行调整(BA)。

DLT

直接构建一个12个未知数的[R t]增广矩阵(先不考虑旋转矩阵的自由度只有3),取六个点对,去求解12个未知数(每一个3D点到归一化平面的映射给出两个约束),最后将[R t]左侧3x3矩阵块进行QR分解,用一个旋转矩阵去近似(将3x3矩阵空间投影到SE(3)流形上)。

P3P

通过3对3D/2D匹配点,得到A、B、C在相机坐标系下的3D坐标;然后,扥局3D-3D的点对,计算相机运动的R和t。

EPnP

需要4对不共面的(对于共面的情况只需要3对)3D-2D匹配点,是目前最有效的PnP求解方法。

Motion Only BA(非线性优化)

把 PnP问题 构建成一个定义于李代数上的非线性最小二乘问题,求解最好的相机位姿。

定义 残差(观测值-预测值)或 重投影误差

\[r(\xi) = u - K \exp({\xi}^{\wedge}) P\]

构建最小二乘问题 \({\xi}^* = \arg \min_\xi \frac{1}{2} \sum_{i=1}^{n} {\| r(\xi) \| }_2^2\)

细节可参考 应用: 基于李代数的视觉SLAM位姿优化

OpenCV 中相关函数:

  • solvePnP
  • Rodrigues

相关参考代码:

vector<KeyPoint> kpts_1, kpts_2;
vector<DMatch> matches;
find_feature_matches ( img_1, img_2, kpts_1, kpts_2, matches );

vector<Point3f> pts_3d;
vector<Point2f> pts_2d;
for ( DMatch m : matches ) {
    int x1 = int(kpts_1[m.queryIdx].pt.x);
    int y1 = int(kpts_1[m.queryIdx].pt.y);
    ushort d = img_d.ptr<unsigned short>(y1)[x1];
    if (d == 0)   // bad depth
        continue;
    float dd = d / depth_scale;

    Point2d p1 = pixel2cam(kpts_1[m.queryIdx].pt, K);

    pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
    pts_2d.push_back(kpts_2[m.trainIdx].pt);
}

Mat om, t;
solvePnP ( pts_3d, pts_2d, K, Mat(), om, t, false, SOLVEPNP_EPNP );
Mat R;
cv::Rodrigues ( om, R ); // rotation vector to rotation matrix

bundle_adjustment_3d2d ( pts_3d, pts_2d, K, R, t );

3D-3D: ICP

对于3D-3D的位姿估计问题可以用 ICP(Iterative Closest Point) 求解,其求解方式分为两种:

  • 线性代数方式(主要是 SVD)
  • 非线性优化方式(类似于BA)

线性代数方式

根据ICP问题,建立第 $i$ 对点的误差项

\[e_i = P_i - (R \cdot {P'}_i + t)\]

构建最小二乘问题,求使误差平方和达到极小的 $R, t$

\[\min_{R,t} J = \frac{1}{2} \sum_{i=1}^{n} {\| e_i \|}_2^2\]

对目标函数处理,最终为

\[\min_{R,t} J = \frac{1}{2} \sum_{i=1}^{n} ( {\| P_i - P_c - R({P'}_i-{P'}_c) \|}^2 + {\| P_c - R{P'}_c - t \|}^2 )\]

根据上式,可以先求解 $R$,再求解 $t$

  1. 计算两组点的质心 $P_c$ 和 ${P’}_c$
  2. 计算每个点的去质心坐标 $Q_i = P_i -P_c$ 和 ${Q’}_i = {P’}_i - {P’}_c$
  3. 定义矩阵 $W = \sum_{i=1}^{n} Q_i {Q’}_i^T$,再SVD分解 $W = U {\Sigma} V^T$
  4. 当 $W$ 满秩时, $R = UV^T \quad$
  5. 计算 $t = P_c - R \cdot {P’}_c$

相关参考代码:

cv::Point3f p1, p2;
int N = pts1.size();
for (int i = 0; i < N; i++) {
    p1 += pts1[i];
    p2 += pts2[i];
}
p1 /= N;
p2 /= N;

std::vector<cv::Point3f> q1(N), q2(N);
for (int i = 0; i < N; i++) {
    q1[i] = pts1[i] - p1;
    q2[i] = pts2[i] - p2;
}

Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
for (int i = 0; i < N; i++) {
    Eigen::Vector3d v3q1 = Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z);
    Eigen::Vector3d v3q2 = Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z);
    W += v3q1 * v3q2.transpose();
}

double determinantW =
 W(0,0)*W(1,1)*W(2,2) + W(0,1)*W(1,2)*W(2,0) + W(0,2)*W(1,0)*W(2,1) -
(W(0,0)*W(1,2)*W(2,1) + W(0,1)*W(1,0)*W(2,2) + W(0,2)*W(1,1)*W(2,0));

assert(determinantW>1e-8);

// SVD on W
Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();

R = U * (V.transpose());
t = Eigen::Vector3d(p1.x, p1.y, p1.z) - R * Eigen::Vector3d(p2.x, p2.y, p2.z);

Optical Flow

光流是一种描述像素随时间在图像之间运动的方法,计算部分像素的称为 稀疏光流,计算所有像素的称为 稠密光流。稀疏光流以 Lucas-Kanade光流 为代表,并可以在SLAM中用于跟踪特征点位置。

LK光流

灰度不变假设

\[I(x+dx, y+dy, t+dt) = I(x, y, t)\]

对左边一阶泰勒展开

\[I(x+dx, y+dy, t+dt) = I(x, y, t) + \frac{\partial I}{\partial x} dx + \frac{\partial I}{\partial y} dy + \frac{\partial I}{\partial t} dt\]

\[\frac{\partial I}{\partial x} dx + \frac{\partial I}{\partial y} dy + \frac{\partial I}{\partial t} dt = 0\]

整理得

\[\frac{\partial I}{\partial x} \frac{dx}{dt} + \frac{\partial I}{\partial y} \frac{dy}{dt} = - \frac{\partial I}{dt}\]

简写为

\[I_x u + I_y v = -I_t\]

\[\begin{bmatrix} I_x & I_y \end{bmatrix} \begin{bmatrix} u \\ v \end{bmatrix} = -I_t\]

在LK光流中,假设某个窗口(w x w)内的像素具有相同的运动

\[{\begin{bmatrix} I_x & I_y \end{bmatrix}}_k \begin{bmatrix} u \\ v \end{bmatrix} = -{I_t}_k, \quad k=1, \dots, w^2\]

简写为

\[J \begin{bmatrix} u \\ v \end{bmatrix} = -e\]

计算

\[J^T J \begin{bmatrix} u \\ v \end{bmatrix} = - J^T e\]

从而得到图像间的运动速度或者某块像素的位置。

分类

增量方式 Forward Inverse
Additive FAIA IAFA
Compositional FCIA ICIA

Forward-Additive 光流:

\[\min_{\Delta x_i, \Delta y_i} \sum_{W} {\| I_1(x_i,y_i) - I_2(x_i+\Delta x_i, y_i+\Delta y_i) \|}_2^2\]

在迭代开始时,Gauss-Newton 的计算依赖于 $I_2$ 在 $(x_i, y_i)$ 处的梯度信息。然而,角点提取算法仅保证了 $I_1(x_i,y_i)$ 处是角点(可以认为角度点存在明显梯度),但对于 $I_2$,我们并没有办法假设 $I_2$ 在 $(x_i,y_i)$ 处亦有梯度,从而 Gauss-Newton 并不一定成立。

反向的光流法(inverse) 则做了一个巧妙的技巧,即用 $I_1(x_i,y_i)$ 处的梯度,替换掉原本要计算的 $I_2(x_i+\Delta x_i, y_i+\Delta y_i)$ 的梯度。$I_1(x_i,y_i)$ 处的梯度不随迭代改变,所以只需计算一次,就可以在后续的迭代中一直使用,节省了大量计算时间。

讨论

  • 在光流中,关键点的坐标值通常是浮点数,但图像数据都是以整数作为下标的。在光流中,通常的优化值都在几个像素内变化,所以用浮点数的像素插值(双线性插值)。
  • 光流法通常只能估计几个像素内的误差。如果初始估计不够好,或者图像运动太大,光流法就无法得到有效的估计(不像特征点匹配那样)。但是,使用图像金字塔,可以让光流对图像运动不那么敏感
  • 多层金字塔光流

OpenCV 中相关函数:

  • calcOpticalFlowPyrLK

相关参考代码:

for (int index = 0; index < count; index++) {
    color = colorImgs[index];
    depth = depthImgs[index];

    if (index == 0) {
        vector<cv::KeyPoint> kps;
        cv::Ptr<cv::FastFeatureDetector> detector =
         cv::FastFeatureDetector::create();
        detector->detect(color, kps);
        for (auto kp:kps)
            keypoints.push_back(kp.pt);
        last_color = color;
        continue;
    }
    if (color.data == nullptr || depth.data == nullptr)
        continue;

    vector<cv::Point2f> next_keypoints;
    vector<cv::Point2f> prev_keypoints;
    for (auto kp:keypoints)
        prev_keypoints.push_back(kp);

    vector<unsigned char> status;
    vector<float> error;
    cv::calcOpticalFlowPyrLK(
      last_color, color, prev_keypoints, next_keypoints, status, error);

    int i = 0;
    for (auto iter = keypoints.begin(); iter != keypoints.end(); i++) {
        if (status[i] == 0) {
            iter = keypoints.erase(iter);
            continue;
        }
        * iter = next_keypoints[i];
        iter++;
    }

    last_color = color;
}

Direct Method

根据使用像素的数量,直接法分为以下三种

  • 稀疏直接法:使用稀疏关键点,不计算描述子
  • 半稠密直接法:只使用带有梯度的像素点,舍弃像素梯度不明显的地方
  • 稠密直接法:使用所有像素

利用直接法计算相机位姿,建立优化问题时,最小化的是 光度误差(Photometric Error)

\[r(\xi) = I_1(p) - I_2(p') = I_1(p) - I_2( K \exp({\xi}^{\wedge}) P )\]

构建最小二乘问题 \({\xi}^* = \arg \min_\xi \frac{1}{2} \sum_{i=1}^{n} {\| r(\xi) \| }_2^2\)

更具体地,稀疏直接法

\[{\xi}^* = \arg \min_\xi \frac{1}{N} \sum_{i=1}^N \sum_{W_i} {\| I_1(p_i) - I_2 (\pi(\exp({\xi}^{\wedge}) \pi_{-1}(p_i))) \|}_2^2\]

其雅克比矩阵的计算,可参见 视觉SLAM位姿优化时误差函数雅克比矩阵的计算

相关参考代码:

bool pose_estimation_direct(
        const vector<Measurement>& measurements,
        cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw) {

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 1>> DirectBlock;
    DirectBlock::LinearSolverType * linearSolver =
    new g2o::LinearSolverDense<DirectBlock::PoseMatrixType>();
    DirectBlock * solver_ptr = new DirectBlock(linearSolver);

    g2o::OptimizationAlgorithmLevenberg * solver =
    new g2o::OptimizationAlgorithmLevenberg(solver_ptr);

    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    g2o::VertexSE3Expmap * pose = new g2o::VertexSE3Expmap();
    pose->setEstimate(g2o::SE3Quat(Tcw.rotation(), Tcw.translation()));
    pose->setId(0);
    optimizer.addVertex(pose);

    int id = 1;
    for (Measurement m: measurements) {
        EdgeSE3ProjectDirect * edge =
          new EdgeSE3ProjectDirect(
            m.pos_world, K(0, 0), K(1, 1), K(0, 2), K(1, 2), gray);
        edge->setVertex(0, pose);
        edge->setMeasurement(m.grayscale);
        edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
        edge->setId(id++);
        optimizer.addEdge(edge);
    }

    optimizer.initializeOptimization();
    optimizer.optimize(30);
    Tcw = pose->estimate();
}

参考文献

Read More

Cameras

[TOC]

Overview

0. Basic Knowledge

Benchmark

1. Lenses

2. Camera Sensors

3. Digital Camera

  • gPhoto is a free, redistributable, ready to use set of digital camera software applications for Unix-like systems
  • digiCamControl: An innovative and easy to use solution for complex camera control!
  • DSLR Controller was the first and is still the best app to fully control your Canon EOS DSLR from your Android device, with nothing more than a USB cable.
  • Generic PTP control of digital cameras
    digital_cam_ctrl_p100.jpg

4. Camera Modules

MatrixVision mvBlueFox

iDS uEye

CMUcam

Open Source Programmable Embedded Color Vision Sensors, The first CMUcam made its splash in 1999 as a CREATE Lab project.

Pixy

pixy_cam.jpg

Pixy is the fifth version of the CMUcam, or CMUcam5, but “Pixy” is easier to say than CMUcam5, so the name more or less stuck. Pixy got its start in 2013 as part of a successful Kickstarter campaign, and as a partnership between Charmed Labs and CMU.

Pixy2 was announced recently as Pixy’s smaller, faster, and smarter younger sibling.
pixy2_cam.jpg

OpenMV

The OpenMV(Open-Source Machine Vision) project aims at making machine vision more accessible to beginners by developing a user-friendly, open-source, low-cost machine vision platform.

OpenMV cameras are programmable in Python3 and come with an extensive set of image processing functions such as face detection, keypoints descriptors, color tracking, QR and Bar codes decoding, AprilTags, GIF and MJPEG recording and more.

openmv_cam.jpg

NXTCam-v4

Vision Subsystem - Camera for NXT or EV3 (NXTCam-v4)

nxtcam_v4.jpg

JeVois Smart Machine Vision Camera

Open-source quad-core camera effortlessly adds powerful machine vision to all your PC, Arduino, and Raspberry Pi projects.

jevois.png

5. 3D (Depth) Cameras

Structure Light Camera

Kinect

Orbbec Astra Camera

ASUS Xtion 2

  • https://www.asus.com/3D-Sensor/

Stereo Camera

Realsense Camera

ZED Stereo Camera

FLIR Bumblebee

Tara

Event Camera

Read More

Camera Control

[TOC]

Interfaces

MIPI DSI CSI LVDS DVP SCCB SPI

Drivers

Viewer/Controller

with Arduino

Others

Read More

Ceres-Solver 从入门到上手视觉SLAM位姿优化问题

[TOC]

概述

Ceres Solver is an open source C++ library for modeling and solving large, complicated optimization problems.

使用 Ceres Solver 求解非线性优化问题,主要包括以下几部分:

  • 构建代价函数(cost function) 或 残差(residual)
  • 构建优化问题(ceres::Problem):通过 AddResidualBlock 添加代价函数(cost function)、损失函数(loss function) 和 待优化状态量
    • LossFunction: a scalar function that is used to reduce the influence of outliers on the solution of non-linear least squares problems.
  • 配置求解器(ceres::Solver::Options)
  • 运行求解器(ceres::Solve(options, &problem, &summary))

注意

Ceres Solver 只接受最小二乘优化,也就是 $\min r^T r$;若要对残差加权重,使用马氏距离,即 $\min r^T P^{-1} r$,则要对 信息矩阵$P^{-1}$ 做 Cholesky分解,即 $LL^T=P^{−1}$,则 $d = r^T (L L^T) r = (L^T r)^T (L^T r)$,令 $r’ = (L^T r)$,最终 $\min r’^T r’$。

代码cggos/state_estimation

入门

先以最小化下面的函数为例,介绍 Ceres Solver 的基本用法

\[\frac{1}{2} (10 - x)^2\]

Part 1: Cost Function

(1)AutoDiffCostFunction(自动求导)

  • 构造 代价函数结构体(例如:struct CostFunctor),在其结构体内对 模板括号() 重载,定义残差
  • 在重载的 () 函数 形参 中,最后一个为残差,前面几个为待优化状态量
struct CostFunctor {
    template<typename T>
    bool operator()(const T *const x, T *residual) const {
        residual[0] = 10.0 - x[0]; // r(x) = 10 - x
        return true;
    }
};
  • 创建代价函数的实例,对于模板参数的数字,第一个为残差的维度,后面几个为待优化状态量的维度
ceres::CostFunction *cost_function;
cost_function = new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);

(2) NumericDiffCostFunction

  • 数值求导法 也是构造 代价函数结构体,但在重载 括号() 时没有用模板
struct CostFunctorNum {
    bool operator()(const double *const x, double *residual) const {
        residual[0] = 10.0 - x[0]; // r(x) = 10 - x
        return true;
    }
};
  • 并且在实例化代价函数时也稍微有点区别,多了一个模板参数 ceres::CENTRAL
ceres::CostFunction *cost_function;
cost_function =
new ceres::NumericDiffCostFunction<CostFunctorNum, ceres::CENTRAL, 1, 1>(new CostFunctorNum);

(3) 自定义 CostFunction

  • 构建一个 继承自 ceres::SizedCostFunction<1,1> 的类,同样,对于模板参数的数字,第一个为残差的维度,后面几个为待优化状态量的维度
  • 重载 虚函数virtual bool Evaluate(double const* const* parameters, double *residuals, double **jacobians) const,根据 待优化变量,实现 残差和雅克比矩阵的计算
class SimpleCostFunctor : public ceres::SizedCostFunction<1,1> {
public:
    virtual ~SimpleCostFunctor() {};

    virtual bool Evaluate(
      double const* const* parameters, double *residuals, double** jacobians) const {
        const double x = parameters[0][0];

        residuals[0] = 10 - x; // r(x) = 10 - x

        if(jacobians != NULL && jacobians[0] != NULL) {
            jacobians[0][0] = -1; // r'(x) = -1
        }

        return true;
    }
};

Part 2: AddResidualBlock

  • 声明 ceres::Problem problem;
  • 通过 AddResidualBlock代价函数(cost function)、损失函数(loss function) 和 待优化状态量 添加到 problem
ceres::CostFunction *cost_function;
cost_function = new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);

ceres::Problem problem;
problem.AddResidualBlock(cost_function, NULL, &x);

Part 3: Config & Solve

配置求解器,并计算,输出结果

ceres::Solver::Options options;
options.max_num_iterations = 25;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;

ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";

Simple Example Code

#include "ceres/ceres.h"
#include "glog/logging.h"

struct CostFunctor {
    template<typename T>
    bool operator()(const T *const x, T *residual) const {
        residual[0] = 10.0 - x[0]; // f(x) = 10 - x
        return true;
    }
};

struct CostFunctorNum {
    bool operator()(const double *const x, double *residual) const {
        residual[0] = 10.0 - x[0]; // f(x) = 10 - x
        return true;
    }
};

class SimpleCostFunctor : public ceres::SizedCostFunction<1,1> {
public:
    virtual ~SimpleCostFunctor() {};

    virtual bool Evaluate(
      double const* const* parameters, double *residuals, double **jacobians) const {
        const double x = parameters[0][0];

        residuals[0] = 10 - x; // f(x) = 10 - x

        if(jacobians != NULL && jacobians[0] != NULL) {
            jacobians[0][0] = -1; // f'(x) = -1
        }

        return true;
    }
};

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);

    double x = 0.5;
    const double initial_x = x;

    ceres::Problem problem;

    // Set up the only cost function (also known as residual)
    ceres::CostFunction *cost_function;

    // auto-differentiation
//    cost_function = new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);

     //numeric differentiation
//    cost_function =
//    new ceres::NumericDiffCostFunction<CostFunctorNum, ceres::CENTRAL, 1, 1>(
//      new CostFunctorNum);

    cost_function = new SimpleCostFunctor;

    // 添加代价函数cost_function和损失函数NULL,其中x为状态量
    problem.AddResidualBlock(cost_function, NULL, &x);

    ceres::Solver::Options options;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    std::cout << summary.BriefReport() << "\n";
    std::cout << "x : " << initial_x << " -> " << x << "\n";

    return 0;
}

应用: 基于李代数的视觉SLAM位姿优化

下面以 基于李代数的视觉SLAM位姿优化问题 为例,介绍 Ceres Solver 的使用。

(1)残差(预测值 - 观测值)

\[r(\xi) = K \exp({\xi}^{\wedge}) P - u\]

(2)雅克比矩阵

\[\begin{aligned} J &= \frac{\partial r(\xi)}{\partial \xi} \\ &= \begin{bmatrix} \frac{f_x}{Z'} & 0 & -\frac{X'f_x}{Z'^2} & -\frac{X'Y'f_x}{Z'^2} & f_x+\frac{X'^2f_x}{Z'^2} & -\frac{Y'f_x}{Z'} \\ 0 & \frac{f_y}{Z'} & -\frac{Y'f_y}{Z'^2} & -f_y-\frac{Y'^2f_y}{Z'^2} & \frac{X'Y'f_y}{Z'^2} & \frac{X'f_y}{Z'} \end{bmatrix} \in \mathbb{R}^{2 \times 6} \end{aligned}\]

(3)核心代码

代价函数的构造:

class BAGNCostFunctor : public ceres::SizedCostFunction<2, 6> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    BAGNCostFunctor(Eigen::Vector2d observed_p, Eigen::Vector3d observed_P) :
            observed_p_(observed_p), observed_P_(observed_P) {}

    virtual ~BAGNCostFunctor() {}

    virtual bool Evaluate(
      double const* const* parameters, double *residuals, double **jacobians) const {

        Eigen::Map<const Eigen::Matrix<double,6,1>> T_se3(*parameters);

        Sophus::SE3 T_SE3 = Sophus::SE3::exp(T_se3);

        Eigen::Vector3d Pc = T_SE3 * observed_P_;

        Eigen::Matrix3d K;
        double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
        K << fx, 0, cx, 0, fy, cy, 0, 0, 1;

        Eigen::Vector2d residual =  (K * Pc).hnormalized() - observed_p_;

        residuals[0] = residual[0];
        residuals[1] = residual[1];

        if(jacobians != NULL) {

            if(jacobians[0] != NULL) {

                Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor>> J(jacobians[0]);

                double x = Pc[0];
                double y = Pc[1];
                double z = Pc[2];

                double x2 = x*x;
                double y2 = y*y;
                double z2 = z*z;

                J(0,0) =  fx/z;
                J(0,1) =  0;
                J(0,2) = -fx*x/z2;
                J(0,3) = -fx*x*y/z2;
                J(0,4) =  fx+fx*x2/z2;
                J(0,5) = -fx*y/z;
                J(1,0) =  0;
                J(1,1) =  fy/z;
                J(1,2) = -fy*y/z2;
                J(1,3) = -fy-fy*y2/z2;
                J(1,4) =  fy*x*y/z2;
                J(1,5) =  fy*x/z;
            }
        }

        return true;
    }

private:
    const Eigen::Vector2d observed_p_;
    const Eigen::Vector3d observed_P_;
};

构造优化问题,并求解相机位姿:

Sophus::Vector6d se3;

ceres::Problem problem;
for(int i=0; i<n_points; ++i) {
    ceres::CostFunction *cost_function;
    cost_function = new BAGNCostFunctor(p2d[i], p3d[i]);
    problem.AddResidualBlock(cost_function, NULL, se3.data());
}

ceres::Solver::Options options;
options.dynamic_sparsity = true;
options.max_num_iterations = 100;
options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
options.minimizer_type = ceres::TRUST_REGION;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.trust_region_strategy_type = ceres::DOGLEG;
options.minimizer_progress_to_stdout = true;
options.dogleg_type = ceres::SUBSPACE_DOGLEG;

ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";

std::cout << "estimated pose: \n" << Sophus::SE3::exp(se3).matrix() << std::endl;
Read More

^