DSOからD3VO
[Engel+ 2017] Direct Sparse Odometry
[Engel+ 2017] ABSTRUCT
Photometric errorの最小化を行うDirect probablistic modelとジオメトリ(逆深度)と姿勢を含むすべてのモデルパラメータの一貫した共同最適化を行う。従来手法のsmoothness priorを省略し、代わりに画像全体でピクセルを均等にサンプリングすることでリアルタイムに実行が可能になった。露光時間、レンズのケラレ、非線形応答関数を考慮した完全なPhotometric calibrationを統合している。
[Engel+ 2017] MOTIVATION
- Indirect法
- マップ(未知数3)と画像表面上の特徴点の位置(未知数2)
- Direct法
- 画像表面上の逆深度(未知数1)
[Engel+ 2017] METHOD
- $I_i(x) = G(t_i V(x) B_i(x))$
- $I'_i(x) = t_i B_i(x) = \frac{{G}^{-1}(I_i(x))}{V(x)}$
- $t_i$ ... exposure time
- $V$ ... レンズのケラレ $[0, 1]$
- $G$ ... nonlinear response function $[0, 255]$
- $B$ ... irradiance
- ${E}_{{p}_{curr}} = \sum_{p \in \mathcal{N}_p} w_p || (I_{curr} [p']- b_{curr}) - \frac{t_{curr} {e}^{a_{curr}}}{t_{ref} {e}^{a_{ref}}} (I_{ref}[p] - b_{ref}) ||_{\gamma}$
- $p' = \Pi_c (R {\Pi}^{-1}_c (p, d_p) + t)$
- ${e}^{-a_i}(I_i - b_i)$ ... Affine brightness transter function
- $w_p = \frac{{c}^2}{{c}^2 + {||\nabla I_i (p)||_2}^2}$
- $E_{photo} = \sum_{i \in \mathcal{F}} \sum_{p \in \mathcal{P}_i} \sum_{j \in obs(p)} E_{p_j}$
- If exposure times are known,
- $E_{prior} = \sum_{i \in \mathcal{F}} (\lambda_a {a_i}^2 + \lambda_b {b}^2_i)$
- Affine brightness transferを0にする
- $E_{prior} = \sum_{i \in \mathcal{F}} (\lambda_a {a_i}^2 + \lambda_b {b}^2_i)$
- If no photometric calibration is available,
- $t_i = 0$
- $\lambda_a = \lambda_b = 0$
- Consistency
- Windowed Optimization
- Gauss-Newton
- $H = {J}^T W J, b = - {J}^T W r$
- Each point contributes $|\mathcal{N}_p|=8$ residuals.
- $r_k (x \boxplus \zeta_0) = (I_j [p'(T_i, T_j, d, c)] - b_j) - \frac{t_j {e}^{a^{j}}}{t_i {e}^{a_i}} (I_i[p] - b_i)$
- $J_k = \frac{\partial r_k((\delta + x) \boxplus \zeta_0)}{\partial \delta} = [J_I J_{geo}, J_{photo}] = [\frac{\partial I_j}{\partial p'} \frac{\partial p' ((\delta+x)\boxplus \zeta_0)}{\delta_{geo}}, \frac{\partial r_k((\delta + x) \boxplus \zeta_0)}{\partial \delta_{photo}}]$
- $\delta_{geo} = (T_i, T_j, d, c), \delta_{photo} = (a_i, a_j, b_i, b_j)$
- まず$J_{photo}, J_{geo}$を$x=0$で評価する(?)
- First Estimate Jacobians
- システムの整合性を保つ
- ${J}_{photo}$, ${J}_{geo}$は増分$x$に比べて滑らかなのでこの近似は良い
- $J_I$は滑らかでないがnull space(零空間)には影響しない(?)ので$x$の現在の値(残差$r_k$と同じ点)で評価される
- null space ... $Hx = 0$を満たすベクトル$x$の集合
- scaleなど最適化には影響しないパラメータが存在し、最適解が無限に存在するため解けない
- これらはSLAM/VOの最適化のnull spaceと考えることが出来る
- ORB-SLAM2では最適化の際にいくつかのポーズを固定して解く
- DSOではポーズの初期推定値を頼りに小さな領域のposeとpointsのみを最適化することでヌル空間の影響を排除する
- $x$のうちヌル空間に直交する部分だけ残す
- $x$のヌル空間への投影を削除(参考)
- $J_{geo}$は同じ点に属する全ての残差に対して同じと仮定し中央のピクセルについてのみ評価する
- $\mathcal{N}_p$で等しくなると仮定
- ${x}^{new} \leftarrow \delta + x = {H}^{-1} b + x$
- Marginalization (周辺化、無視)
- 変数のセットが大きくなりすぎると、古い変数はSchurの補集合を用いた周辺化によって削除される
- フレームiを周辺化する際には、まずPi内のすべての点と最後の2つのキーフレームで観測されていない点を周辺化する
- フレームiにおけるアクティブな点の残りの観測はシステムから削除されます
- $E'$ ... 周辺化されるべき状態変数に依存するすべての残差を含むエネルギーの部分
- $E'(x \boxplus \zeta_0) \approx {2x}^{T} (b - Hx_0) + {x}^{T} H x + (c + {x_0}^{T} H x_0 - {x_0}^{T} b) = {2x}^{T} b' + {x}^{T} H x + c'$
- $\begin{bmatrix} \begin{array}{ll} H_{\alpha \alpha} & H_{\alpha \beta} \\ H_{\beta \alpha} & H_{\beta \beta} \end{array} \end{bmatrix} \begin{bmatrix} \begin{array}{ll} x_{\alpha} \\ x_{\beta} \end{array} \end{bmatrix} = \begin{bmatrix} \begin{array}{ll} b'_{\alpha} \\ b'_{\beta} \end{array} \end{bmatrix}$
- $\alpha$ : 保持する変数
- $\beta$ : Marginalizeする変数
- Schur complement : $\hat{H_{\alpha \alpha}} x_{\alpha} = \hat{b'_{\alpha}}$
- $\hat{H_{\alpha \alpha}} = H_{\alpha \alpha} - H_{\alpha \beta} {H}^{-1}_{\beta \beta} H_{\beta \alpha}$
- $\hat{\beta'_{\alpha}} = \beta'_{\alpha} - H_{\alpha \beta} {H}^{-1}_{\beta \beta} b'_{\beta}$
- $E'(x \boxplus (\zeta_0)_{\alpha}) = {2x}^{T}_{\alpha} \hat{b'_{\alpha}} + {x}^{T}_{\alpha} \hat{H_{\alpha \alpha}} x_{\alpha}$
[Engel+ 2017] IMPRESSION
- Windowed Optimizationの近似がわからん
[Wang+ 2017] Stereo DSO: Large-Scale Direct Sparse Visual Odometry with Stereo Cameras
[Wang+ 2017] ABSTRUCT
- 正確なメトリック
- 大きなオプティカルフローやローリングシャッターの効果を受けにくい
[Wang+ 2020] METHOD
- Tracking
[Wang+ 2017] RESULTS
[Yang+ 2018] Deep Virtual Stereo Odometry: Leveraging Deep Depth Prediction for Monocular Direct Sparse Odometry
[Yang+ 2018] ABSTRUCT
- 深度推定してStereo DSO
[Yang+ 2018] MOTIVATION
- 低コストにしたい
- 単眼
- LiDARは使わずにStereo DSOの深度を利用
[Yang+ 2018] METHOD
- Architecture
- SimpleNet
- 入力
- ${I}^{left}$
- 出力
- ${disp}^{left}_{simple, s}, \space {disp}^{right}_{simple, s}, \space s \in [0, 3]$
- 0がoriginal size
- ${disp}^{left}_{simple, s}, \space {disp}^{right}_{simple, s}, \space s \in [0, 3]$
- ResNet-50ベースのエンコーダ+デコーダ(4層)
- Upprojectionはresize-convolution(nearest neighbor + convolution)により実装
- 入力
- ResidualNet
- 入力
- ${I}^{left}$
- ${disp}^{left}_{simple, 0}$
- ${I}^{right}_{recons}$ : the reconstructed right image by warping ${I}^{left}$ using ${disp}^{right}_{simple, 0}$
- ${I}^{left}_{recons}$ : the reconstructed left image by back-warping ${I}^{right}_{recons}$ using ${disp}^{left}_{simple, 0}$
- $e_l$ : $l_1$ reconstruction error between ${I}^{left}$ and ${I}^{left}_{recons}$
- 出力
- ${disp}^{left}_{res, s}, \space {disp}^{right}_{res, s}, \space s \in [0, 3]$
- 入力
- 最終出力
- $disp_s = disp_{simple, s} \oplus disp_{res, s} \space s \in [0, 3]$
- モデルは視差を出力するのでステレオの内部、外部パラメータは必要ない
- ワーピングには平行化された画像が必要
- SimpleNet
- 視差から新しいキーフレームの深度マップを初期化
[Yang+ 2018] IMPRESSION
- メトリックスケールは?
- 推定した視差からベースラインを用いて回復できるのでメトリックがシーケンス全体で一貫する
[Yang+ 2020] D3VO: Deep Depth, Deep Pose and Deep Uncertainty for Monocular Visual Odometry
Yang, Nan, et al. "D3VO: Deep Depth, Deep Pose and Deep Uncertainty for Monocular Visual Odometry." 2020 IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR). IEEE Computer Society, 2020.
[Yang+ 2020] ABSTRUCT
StereoからSelf-superviseで学習を行う、単眼VOのための深度、姿勢、不確実性を推定するネットワークを提案した。トレーニングペアの照明条件を予測可能な明るさ変換パラメータで整列させた。さらに、入力画像上のピクセルの測光不確かさをモデル化することで深度推定精度を向上させ、Direct VOにおける測光残差の重みづけを提供する。単眼VOの精度を凌駕し、Stereo VOやVIOと同等の性能を達成した。
[Yang+ 2020] MOTIVATION
- 単眼
- 従来手法
- 本手法
[Yang+ 2020] METHOD
[Yang+ 2020] IMPRESSION
- Why deep depth?
- メトリックスケールを回復してデプスマップを初期化するため
- Why deep pose?
- ロバストネス向上
- Why deep uncertainty?
- 輝度一定の仮定は現実的でなく、その失敗ケースを対処するため
Eigen、 Sophusまとめ
3年目くらいに書いたものだからアップデートが必要かも
Eigen
- Rotation matrix: Eigen::Matrix3d
- Rotation vector: Eigen::AngleAxisd
- Euler angle: Eigen::Vector3d
- Quaternion: Eigen::Quaterniond
- Euclidean transform: Eigen::Isometry3d
- Affine transform: Eigen::Affine3d
- Perspective transformation: Eigen::Projective3d
参考 https://github.com/gaoxiang12/slambook-en
- メンバ変数に用いる場合はpublicでマクロを定義する
class Foo
{
...
Eigen::Vector2d v;
...
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // このマクロを追加．
...
};
- STLコンテナを用いる場合にはallocatorを利用
- typedefしとくと楽
#include<Eigen/StdVector> typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
参考 https://qiita.com/vs4sh/items/2032fd1a5dab780d432c
Gauss-Newton法
例えばDirect法でGauss-Newtonを用いるとして
$$x_{t+1} = x_t + \Delta$$
$$\Delta = - {({J}^T J)}^{-1} {J}^T r $$
で$J$、$r$が求められていたら、
Eigen::VectorXf xi = Eigen::VectorXf::Zero(6); // 姿勢のベクトル Eigen::MatrixXf J = Eigen::MatrixXf::Zero(width*height, 6); Eigen::VectorXf residuals = Eigen::VectorXf::Zero(width*height); // ここでJとresidualsを求める Eigen::MatrixXf Jt = J.transpose(); Eigen::VectorXf b = Jt * residuals; Eigen::Matrix<float, 6, 6> H = Jt * J; Eigen::Matrix<float, 6, 1> inc = -(H.ldlt().solve(b)); // 逆行列は使わない xi = (Sophus::SE3f::exp(xi) * Sophus::SE3f::exp(inc)).log();
で$\Delta$が求まり$x_{t+1}$が計算できる
行列計算で逆行列は使うべきではない
行列の分解
- LU分解
- 一般の行列$A$を左下三角行列$L$と右上三角行列$U$の積$A=LU$に分解
- PartialPivLU
- FullPivLU 精度がPartialPivLUよりいい
Eigen::PartialPivLU<Eigen::MatrixXd> lu(A);
x = lu.solve(b);
// もしくは
Eigen::PartialPivLU<Eigen::MatrixXd> lu;
lu.compute(A);
x = lu.solve(b);
コレスキー分解
- $A$が対称正定値行列の場合のLU分解$A=L{L}^T$
- LU分解より2倍計算量が小さい
- $y={L}^T\delta x$として$A\delta x = L{L}^T \delta x=Ly=a$から$A\delta L=a$を解く
- LLT
- LDLT 精度がLLTよりいい
QR分解
- 最小二乗法により長方形の係数行列をもつ方程式$Ax=b$を解く場合はQR分解を用いる
- HouseholderQR
- ColPivHouseholderQR
- FullPivHouseholderQR
固有値分解
- EigenSolver 実数行列
- ComplexEigenSolver 複素数行列
- SelfAdjointEigenSolver 実対称、自己随伴行列
- GeneralizedEigenSolver
- GeneralizedSelfAdjointEigenSolver
SVD
- JacobiSVD 精度がいい サイズが増えると遅い
- BDCSVD 速い
- singularValues() 特異値
- matrixU() 左特異ベクトルを並べた行列
- matrixV() 右特異ベクトルを並べた行列
- solve() 方程式を解く
- compute() 分解
参考 https://qiita.com/MusicScience37/items/13fefa6bed25ab8fb6e9
Sophus
Lie群、Lie代数のライブラリ Eigenとの相性がいい
Use logarithmic map to get the Lie algebra
// Rotation matrix with 90 degrees along Z axis Matrix3d R = AngleAxisd(M_PI/2, Vector3d(0, 0, 1)).toRotationMatrix(); Sophus::SO3d SO3_R(R); Vector3d so3 = SO3_R.log(); cout << "so3 hat=\n" << Sophus::SO3d::hat(so3) << endl; cout << "so3 hat vee= " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)). transpose() << endl; // SE3 Vector3d t(1, 0, 0); Sophus::SE3d SE3_Rt(R, t); // Lie Algebra is 6d vector typedef Eigen::Matrix<double, 6, 1> Vector6d; Vector6d se3 = SE3_Rt.log();
se3からSE3の変換は$\exp(\hat{\xi})$だけどSophusはexp()に$\xi$をそのまま渡せば計算してくれる
log()も同様
クオータニオンと並進ベクトルからSE3への変換
Eigen::Quaternionf q(w, x, y, z); Eigen::Vector3f t(1.f, 0.f, 0.f); Sophus::SE3f pose(q, t);
