日本ロボット学会誌 Vol. xx No. xx, pp.1〜4, 202x1

レター

特異値分解に基づくビジュアルオドメトリのための特徴点選択法
本

橋

優

俊∗1

久保田

孝∗2

Feature Points Selection Scheme for SVD-based Visual Odometry
Masatoshi Motohashi∗1 and Takashi Kubota∗2
The localization technique is essential for a mobile robot to perform autonomous navigation safely. Visual odometry (VO) is one of the ego-motion estimation techniques by sequentially tracking the feature points in the image. VO
is used for many robots because of its higher accuracy and usefulness in GPS-denied environments, such as indoor
robots or planetary exploration rovers. It is required to carefully select the feature points for VO in order to reduce
the processing time and improve the estimation accuracy. This paper proposes a new method, which selects featurepoints by taking advantage of separating rotation and translation estimation by Singular Value Decomposition. The
proposed method can select suitable feature points in each. The eﬀectiveness of the proposed method is validated
by experiment using a wheeled testbed rover.
Key Words: Localization, Visual Odometry, Feature Points Selection, Singular Value Decomposition

1. 緒

言

自律移動ロボットにおいてロボットが自身の位置を正確に把握することは走行時に自己の安全を保証するうえで非常に重要である． 自己位置推定は地図構築や経路計画の前提となる機能であり，推定精度が他の機能に大きな影響を及ぼすため，近年は様々なアプローチにより研究が進められている [1] [2] [3]．屋
外環境で活動するロボットにおいては GPS による絶対位置推定が用いられるが，屋内や都市環境，トンネル内，月・惑星環境
では GPS を使用することが困難であるため，カメラや LiDARなどのセンサを用いた相対位置推定が必要となる．
相対位置推定手法の一つである Visual Odometry (VO) は画
像内の特徴点を逐次的に追跡することにより位置を推定する [4]．
GPS を使用できない屋内環境において有効な位置推定手法であり，トンネル，都市部では一時的に位置を補間する手法として用いられる．またタイヤの回転角に基づく Wheel Odometry に比べスリップに対する頑健性が高いため，起伏のある地形や滑りやすい環境においても有効である．そのため惑星探査ローバ
ではステレオカメラを用いた VO（ステレオ VO）が長きにわたり活用され，その有用性が実証されている [5] [6] [7]．
一方で VO の課題は計算コストの高さであり，なかでもステ
レオ VO はステレオマッチングと相対移動量を推定するための最適化計算に多くの時間を要する．特に計算リソースが限られる惑星探査ローバにおいては VO 処理に 1 フレームあたり 4.9
秒の時間を要し，走行効率低下の原因となっている [8]．VO は

原稿受付
東京大学
宇宙航空研究開発機構∗1
The University of Tokyo∗2
Japan Aerospace Exploration Agency■ 要素分野∗1∗2

日本ロボット学会誌 xx 巻 xx 号

使用する特徴点が多いほど処理に時間を要するため，位置推定に適した特徴点のみを選択し，いかに少ない特徴点で精度の高い推定を行うかが計算コスト削減の際に重要となる．
VO における特徴点選択の代表的な手法として，BucketingTechnique [9] がある．Bucketing Technique は画像をいくつかのグリッドに分割し，それぞれの領域で用いる特徴点を一定数に制限する手法である．特徴点を減らすだけでなく特徴点が画像全体に均一に分布するため，効率性と精度を両立させることができる．シンプルでありながら有用性の高い手法であり，VO に限らず Visual SLAM など様々な手法で活用されている [10] [11]．
Nguyen らは Orthogonality Index を用いた特徴点選択法を提案した [12]．特徴点の座標ベクトルの直交度を表す Orthogonality Index を定義し，その指標が高くなる特徴点の組み合わせを選択する．大津らの 2 点アルゴリズム [13] はロボットからみて近傍の点と遠方の点を 1 点ずつ選択し，それぞれを並進推
定・回転推定に用いる．VO 問題を解く際に少なくとも 3 点必要だった特徴点数を 2 点に削減することを実現した．Nguyen
らと大津らの手法は最適化における繰り返し回数を削減することで計算量を抑えることができる．一方で上述のいずれの手法においても特徴点のステレオマッチングの回数は従来の VO 手法と変わらないため，検出された特徴点が多い環境では最適化
より前の段階で処理に時間を要する．
以上の議論のもと，本論文では車輪型移動ロボットにおける位置推定の処理時間の短縮を目指し，ステレオ VO においてス
テレオマッチング以前に特徴点数を削減する手法を検討する．
ステレオ VO の最適化の際に特異値分解を用いると回転成分と並進成分の推定を分離することができる．このことを利用し，提案手法では回転推定に適した特徴点と並進推定に適した特徴
点を分けて選択し，位置推定の精度を維持しつつ計算量の削減を実現する．

—1—

202x 年 xx 月

本

2

橋

優

俊

久保田

孝

2. 特異値分解を用いた相対移動量推定
本研究ではステレオカメラを搭載した移動ロボットの位置推定問題を扱う．ステレオ VO は以下に示す手順により位置推定を行う．時刻 k で得られた画像 I k 上で特徴点を検出し，隣接画像 I k−1 の特徴点とのマッチングを行う．得られた N 個の特徴点ペアに対しステレオマッチングを行い，基準画像に対応する点群 pk と隣接画像に対応する点群 pk−1 を得る．ただN ×3
し pk = {p1k , p2k · · · , pNである．最後に対応する点k } ∈ R群が重なるような回転行列 Rk ∈ SO(3) および並進ベクトル
tk ∈ R3 を求めることで画像間の相対姿勢を推定する．すなわ
ちステレオ VO は式（1）に示す最適化問題として定式化される．

{Rk , tk } = arg min
R,t∑||pik − R(pik−1 − t)||2（1）i

この最適化問題を毎フレームで解き，各時刻ステップにおける相対姿勢を計算する．VO では初期姿勢から相対姿勢を重ね合わせることにより，現ステップにおける位置および姿勢を計算する．
上式の最適化問題の解法として特異値分解 (Singular Value
Decomposition, SVD) を用いた手法 [14] がある．まず次式により回転行列 Rk を求める．

Rk = VUT
USVT = svd(∆pk ∆pT
k−1 )tk = p̄k − Rk p̄k−1

Fig. 2 Coordinate transform from image coordinate to robot coordinate（2）

ただし ∆pk は pk の各点における重心からの差分を表す．また
U，V はそれぞれ ∆pk ∆pT
k−1 の左特異ベクトル，右特異ベク
トルで構成される行列であり，S は特異値を対角成分に並べた行列である．得られた回転行列を用いて並進ベクトル tk を次式により求める．
（3）

ただし p̄k ∈ R3 は点群 pk の重心である．式（2），（3）を用いた手法は，pk ，pk−1 の誤差が等方的な正規分布に従うものであれば式（1）の最適解となることが代数的に証明されている [15]．
上述の通り SVD を用いることにより回転成分と並進成分を順に推定するため，最適化に用いる特徴点を回転推定用と並進
推定用とに分離することができる．以上の議論に基づき，本研
究ではそれぞれの推定において適した特徴点を選択することで少ない特徴点を用いた位置推定を実現する．

3. 提 案 手 法
提案する特徴点選択法を用いた VO アルゴリズムを Fig. 1 に示す．一般的な VO と同様に，まず特徴点検出およぶ追跡を実施する．次に提案手法により特徴点選択を行い，得られた点に対しステレオマッチングを行う．最後に式（2），（3）を用いて相対姿勢を計算する．以上のように提案手法を VO に組み込むことでステレオマッチング以前に特徴点数を削減し計算効率を向上させる．以下に特徴点選択の詳細を述べる．
3. 1 座標変換
特徴点選択に先立ち，検出された特徴点をロボット基準の座標系に変換する．厳密な 3 次元座標を求めるためにはステレオ
マッチングを行う必要があり，本研究の趣旨と反する．そこでFig. 2 に示すようにアフィン変換を施すことでロボットの真上から観た座標系へ変換する．この変換はカメラの取り付け高と俯角が既知であることを前提としている．不整地においては地形が平面でなくまた凹凸があるため厳密に正確な座標とはなら

JRSJ Vol. xx No. xx

Fig. 1 Algorithm of the proposed method

Fig. 3 Feature point candidates

ないが，以上の変換は特徴点選択のためだけに用いるため，位置推定への影響は軽微と考える．以降，特徴点の選択は変換後の座標系で行う．

3. 2 特徴点選択法
次に Fig. 3 に示す方法により回転推定用の特徴点を NR 個，
並進推定用の特徴点を Nt 個選択する．提案手法では回転推定と並進推定を分けて行うため，特徴点も同様にそれぞれ別に選択する．

3. 2. 1 回転推定のための特徴点選択
式（2）に記述されるように SVD を用いた解法では 2 つの点群の重心を基準に考え回転行列を推定する．各特徴点の空間座
標は特徴点マッチングやステレオマッチングに起因する誤差が含まれるため，回転推定の際には重心から遠方にある点ほど精度の高い推定が可能である．そのため検出された特徴点すべてを包含する凸包の頂点を回転推定のための特徴点として選択する．頂点の数が NR 個に満たない場合，外れ値に対する頑健性を保つため残りの特徴点は全領域からランダムに選択する．

3. 2. 2 並進推定のための特徴点選択
並進推定では 2 つの点群の重心が重なるように並進ベクトルを求めるため，用いる特徴点の 3 次元座標の推定精度が高いことが望ましい．ステレオマッチングは近傍にある点ほど高い精度で深度を計算することができるため，ロボットに近い領域，すなわち画像の下側から順に Nt 個を選択する．
最終的に選択された特徴点の一例を Fig. 4 に示す．以上の手順で選択した特徴点に対してステレオマッチングを実施し，回転行列推定のための特徴点 pk と並進ベクトル推定のための特徴点 q k を求める．隣接画像における対応特徴点の座標 pk−1 ，—2—

xx, 202x特異値分解に基づくビジュアルオドメトリのための特徴点選択法

Fig. 4 Selected feature points

3

Fig. 5 Testbed rover : AKI

q k−1 も同様に計算する．
3. 3 選択した特徴点を用いた位置推定
前項で得られた回転推定に適した特徴点群 pk と並進推定に適した特徴点群 q k を用いて次式の通りに相対姿勢を計算する．

Rk = VUT
USVT = svd(∆pk ∆pT
k−1 )

（4）

tk = q̄ k − Rk q̄ k−1
Fig. 6 Captured images

ステレオマッチングによる深度推定や隣接画像間のマッチングが誤った特徴点があると推定精度が大幅に悪化する恐れがある．
そのため外れ値に対するロバスト性を高めるため提案手法では上式を解く際に RANSAC (RANdom SAmple Concensus) [16]を採用する．RANSAC アルゴリズムの手順を以下に示す．
（1）回転用の特徴点 pk から 3 点，並進用の特徴点 q k から 1 点を無作為に抽出する．
（2）抽出された点を式（4）に適用し，回転行列 Rsample ，並進ベクトル tsample を求める．
（3）Rsample ，tsample を用いて pk ，q k との対応を計算し，その誤差が ϵsample 以下のもの pI k ，qI k を計上する．
（4）pI k ，qI k を式（4）に適用し，回転行列 RI ，並進ベクトルtI を求める．
（5）RI ，tI を用いて pI k ，qI k との対応を計算し，その誤差 ϵIを求める．
（6）上記のステップを繰り返し，ϵI が最も小さいときの解 RI ，tI を採用する．

4. 検

Table 1 Average values of processing time, ATE and successrate of each method

SVD-ALL
LM-ALLSVD-Random
ProposedTime [s]ATE [m]0.555 ± 0.300
0.615 ± 0.295
0.209 ± 0.138
0.247 ± 0.131

0.0600 ± 0.0143
0.0541 ± 0.00583
0.141 ± 0.108
0.0924 ± 0.0296

Success
Rate
10/10
10/10
6/10
10/10

証

4. 1 実験設定
提案手法の有効性を検証するため，実機ロボットを用いた実験を行った．機体は JAXA/ISAS が開発した月・惑星探査用の試験ローバ AKI (Advanced Kinematics Intelligence，Fig. 5)を用いた． 実験は月・火星表面を再現したフィールドを使用した．フィールド内にはレゴリスを模した砂を敷き詰め，岩石や岩盤を配置した．Fig. 6 に実験時に撮影した画像を示す．カメラは Stereolabs 社製の ZED2i を使用し，高さ 65cm に俯
角 45 deg で設置した．ローバは平均 0.05m/s の速度で走行し，640pixel × 360pixel のカラー画像を 3 秒間隔で撮影した．またローバの位置はモーションキャプチャカメラシステム OptiTrackを用いて計測した．
比較対象として以下の 3 つの手法を用いた．
• SVD-ALL：全特徴点を利用し，式（2），（3）で最適化• LM-ALL：全 特 徴 点 を 利 用 し ，式（1）を LevenbergMarquardt 法で最適化• SVD-Random：無作為に選択した特徴点を利用し，式（2），（3）で最適化
各手法に対し 10 シーケンスの検証を行い，1 フレームあたりの処理時間，Absolute Trajectory Error (ATE) および成功率を日本ロボット学会誌 xx 巻 xx 号

Fig. 7 Average processing time for each element

評価した．ここで位置推定失敗は，最適化で RANSAC を実行する際にすべての試行で ϵsample が 0.05m 以下となる特徴点が5 個未満であるとき，と定義した．また ATE は成功したシーケ
ンスに対してのみ評価した．検証に用いた CPU は Intel Corei5-8200U@1.60GHz である．
すべての手法において ZED2i の両眼画像を入力とし，特徴点検出は Harris のコーナー検出器 [17] を，特徴記述子は ORB [18]を用いた．提案手法においては NR = 20，Nt = 10 とし，また
SVD-Random で選択する特徴点数は 30 点とした．また最適化の際の RANSAC アルゴリズムもすべての手法で実施した．
4. 2 結果および考察
実験結果を Table 1，Fig. 7, 8 にまとめる．Table 1 に示す通り，提案手法は SVD-ALL に比べ処理速度を 2.2 倍向上することに成功した．Fig. 7 は提案手法および比較手法における特徴点検出・追跡，特徴点選択，ステレオマッチング，最適化に要した計算時間をまとめたものである．提案手法はステレオマッ
チングに要する時間を大幅に短縮した．SVD-ALL，LM-ALL—3—

202x 年 xx 月

本

4

橋

優

俊

久保田

孝

する可能性がある．本手法が有効な環境を検討することも今後の課題である．
謝 辞 本研究は，JST 次世代研究者挑戦的研究プログラム
JPMJSP2108 および JSPS 科研費 23H01386 の助成を受けた
ものです．

参

(a) SVD-ALL(b) LM-ALL(c) SVD-Random(d) Proposed

Fig. 8 Estimated trajectory for each method and each sequence

における検出特徴点数はいずれも 1 フレームあたり 1768 ± 865個である．一方で提案手法では特徴点数を 30 点まで削減しており，その削減分が計算時間の短縮につながったと推察される．
以上よりステレオマッチング以前に特徴点を絞り込むことの有効性を示すことができたと考える．SVD-Random においても処理時間に関しては提案手法と同様な結果となった．一方で他の手法に比べて位置推定に失敗するケースが多く，有効な特徴点を選択することの重要性を示す結果となった．
ATE を比較すると提案手法は SVD-ALL や LM-ALL に比
べ誤差とその分散が大きくなり，同様の傾向が SVD-Randomにもみられる．Fig. 8 は各手法の各シーケンスにおける推定経路である．SVD-ALL や LM-ALL ではシーケンスごとの推定
経路のばらつきがほとんどない一方で，選択ベースの手法は推定経路がシーケンスごとに大きく異なる．本研究では特徴点選
択を位置関係に基づいて行っており，深度推定や特徴点マッチ
ングの精度を考慮していない．そのため，選択の段階でそれらの精度が高い点を除外してしまうと位置推定の精度が悪化する．
精度を改善するためには，位置関係だけでなく高精度な推定に寄与する特徴点を選択することが必要であると考えられる．

5. 結

言

本稿では，VO における特徴点を位置関係に基づき選択する手法を提案した．最適化の際に特異値分解を用いることで回転成分と並進成分を分離できることを利用し，それぞれに適した特徴点を選択することにより，少ない特徴点での位置推定を可能とした．実機ローバを用いた検証により，提案手法が処理速度の面で有効性が高いことを示した．一方で位置推定精度は既存手法に比べ低下するケースがあり，位置推定において重要な特徴点を選択する手法を検討することが必要である．また 3. 1
節にて設定した仮定は不整地においては正確な座標変換とはならないため，起伏や凹凸のある環境では位置推定の精度に影響

JRSJ Vol. xx No. xx

—4—

考

文

献

[ 1 ] D. Wisth, M. Camurri, and M. Fallon, “Vilens: Visual, inertial, lidar, and leg odometry for all-terrain legged robots,”
IEEE Transactions on Robotics, vol. 39, no. 1, pp. 309–326,
2023.
[ 2 ] J. Mo, M. J. Islam, and J. Sattar, “Fast direct stereo visualslam,” IEEE Robotics and Automation Letters, vol. 7, no. 2,
pp. 778–785, 2022.
[ 3 ] J. Lin and F. Zhang, “R3live: A robust, real-time, rgb-colored,
lidar-inertial-visual tightly-coupled state estimation and mapping package,” in 2022 International Conference on Robotics
and Automation (ICRA), 2022, pp. 10 672–10 678.
[ 4 ] D. Scaramuzza and F. Fraundorfer, “Visual odometry [tutorial],” IEEE Robotics & Automation Magazine, vol. 18, no. 4,
pp. 80–92, 2011.
[ 5 ] M. Maimone, Y. Cheng, and L. Matthies, “Two years of visual
odometry on the mars exploration rovers,” Journal of Field
Robotics, vol. 24, pp. 169–186, Mar. 2007.
[ 6 ] A. Rankin, M. Maimone, J. Biesiadecki, N. Patel, D. Levine,
and O. Toupet, “Driving curiosity: Mars rover mobility trends
during the ﬁrst seven years,” in 2020 IEEE Aerospace Conference, 2020, pp. 1–19.
[ 7 ] S. Andolfo, F. Petricca, and A. Genova, “Precise pose
estimation of the nasa mars 2020 perseverance rover througha stereo-vision-based approach,” Journal of Field Robotics,
vol. 40, no. 3, pp. 684–700, 2023. [Online]. Available:
https://onlinelibrary.wiley.com/doi/abs/10.1002/rob.22138[ 8 ] R. Rieber, “Mars 2020 project autonavigation: Intuitive autonomy on mars and at sea the mobility system for mars-2020,”
2018.
[ 9 ] Z. Zhang, R. Deriche, O. Faugeras, and Q.-T. Luong, “A robust
technique for matching two uncalibrated images through therecovery of the unknown epipolar geometry,” Artiﬁcial Intelligence, vol. 78, pp. 87–119, Oct. 1995. [Online]. Available: https://linkinghub.elsevier.com/retrieve/pii/0004370295000224[10] B. Kitt, F. Moosmann, and C. Stiller, “Moving on to dynamic
environments: Visual odometry using feature classiﬁcation,”in 2010 IEEE/RSJ International Conference on Intelligent
Robots and Systems, 2010, pp. 5551–5556.
[11] A. Geiger, J. Ziegler, and C. Stiller, “Stereoscan: Dense 3dreconstruction in real-time,” 2011, pp. 963–968.
[12] H. H. Nguyen and S. Lee, “Orthogonality index based optimalfeature selection for visual odometry,” IEEE Access, vol. 7, pp.
62 284–62 299, 2019.
[13] K. Otsu and T. Kubota, “Visual odometry for planetary exploration rovers in untextured terrains,” Journal of the Robotics
Society of Japan, vol. 32, no. 9, pp. 825–831, 2014.
[14] K. S. Arun, T. S. Huang, and S. D. Blostein, “Least-squares
ﬁtting of two 3-d point sets,” IEEE Transactions on PatternAnalysis and Machine Intelligence, vol. PAMI-9, no. 5, pp.
698–700, 1987.
[15] 金谷健一 著，3 次元回転 = 3D Rotations : パラメータ計算とリー代数による最適化 共立出版，2019．
[16] M. A. Fischler and R. C. Bolles, “Random sample consensus: A paradigm for model ﬁtting with applications toimage analysis and automated cartography,” Commun. ACM,
vol. 24, no. 6, p. 381–395, June 1981. [Online]. Available:
https://doi.org/10.1145/358669.358692[17] C. Harris, M. Stephens et al., “A combined corner and edge detector,” in Alvey vision conference, vol. 15, no. 50. Citeseer,
1988, pp. 10–5244.
[18] E. Rublee, V. Rabaud, K. Konolige, and G. Bradski, “Orb:An eﬃcient alternative to sift or surf,” in 2011 International
Conference on Computer Vision, 2011, pp. 2564–2571.

xx, 202x