LOAMについてまとめました。
LOAM解説【SLAM】
SLAM分野に、LOAMという有名な手法があります。ベンチマークで上位につけており、多くの論文で比較手法としても引用されています。
この記事では、なるべく全体像をつかめるように解説していきます。
論文情報
J.Zhang and S.Singh, LOAM: Lidar Odometry and Mapping in Real-time, in Robotics: Science and Systems Conference, pp.161--195, 2014.
手法の概要
- 平面・エッジを特徴量として点群マッチングを行う
→ ドリフト低減 - オドメトリ計算（高周期）とマッピング計算（低周期）を分ける
→ 高速処理（リアルタイム）
処理の流れ
- 2次元点群を入力
- 平面・エッジ特徴点を抽出
- 現時刻の特徴点と過去時刻（蓄積点群）の特徴点を対応付け
- 目的関数を算出
- 目的関数を最適化して相対移動量を推定
- 相対移動量に合わせて現時刻の点群を蓄積点群に追加
- LiDARオドメトリ：低周期
- マッピング：高周期
平面度cで特徴点抽出
スキャンラインに対して、以下の式で平面度cを算出します。
\[ \begin{equation} c=\frac{1}{|\mathcal{S}| \cdot \| \boldsymbol{X}_{(k, i)}^{L}||}\left\|\sum_{j \in \mathcal{S}, j \neq i}\left(\boldsymbol{X}_{(k, i)}^{L}-\boldsymbol{X}_{(k, j)}^{L}\right)\right\| \end{equation} \]平面度cが小さい点を平面特徴量として、平面度cが大きい点をエッジ特徴量として抽出します。
分かりやすいように、それぞれを以下で図として示します。
エッジ：c = 大
下図のように、エッジの点では、平面度cが大きくなることが見て分かります。
平面：c = 小
下図のように、平面上の点は直線状に並ぶので、平面度cが小さくなることが見て分かります。
評価関数
最小化すべき評価関数を設定します。
エッジ特徴点の評価関数
点\(i\)に対して対応付けられた蓄積点群の点\(j\)、\(l\)との距離を下記のように算出します。
\[ \begin{equation} d_{\mathcal{E}}=\frac{\left|\left(\tilde{\boldsymbol{X}}_{(k+1, i)}^{L}-\overline{\boldsymbol{X}}_{(k, j)}^{L}\right) \times\left(\tilde{\boldsymbol{X}}_{(k+1, i)}^{L}-\overline{\boldsymbol{X}}_{(k, l)}^{L}\right)\right|}{\left|\overline{\boldsymbol{X}}_{(k, j)}^{L}-\overline{\boldsymbol{X}}_{(k, l)}^{L}\right|} \end{equation} \]平面特徴点の評価関数
点\(i\)に対して対応付けられた蓄積点群の点\(j\)、\(l\)、\(m\)との距離を下記のように算出します。
\[ \begin{equation} d_{\mathcal{H}}=\frac{\left|\begin{array}{c} \left(\tilde{\boldsymbol{X}}_{(k+1, i)}^{L}-\overline{\boldsymbol{X}}_{(k, j)}^{L}\right) \\ \left(\left(\overline{\boldsymbol{X}}_{(k, j)}^{L}-\overline{\boldsymbol{X}}_{(k, l)}^{L}\right) \times\left(\overline{\boldsymbol{X}}_{(k, j)}^{L}-\overline{\boldsymbol{X}}_{(k, m)}^{L}\right)\right) \end{array}\right|}{\left|\left(\overline{\boldsymbol{X}}_{(k, j)}^{L}-\overline{\boldsymbol{X}}_{(k, l)}^{L}\right) \times\left(\overline{\boldsymbol{X}}_{(k, j)}^{L}-\overline{\boldsymbol{X}}_{(k, m)}^{L}\right)\right|} \end{equation} \]最適化
レーベンバーグ・マーカート法（非線形最小二乗法）という手法で、上記の評価関数を最適化します。これを適用することで、LiDARの相対移動量を算出します。
ここで、最適化とは、評価関数を最小化するという意味で使っています。
LiDARオドメトリとマッピングは、ほとんど同じ処理ですが、オドメトリは高周期、マッピングは低周期でマッチングを行います。低周期のマッピングでは、より大きな点群をマッチングすることで、大域的に点群の位置合わせを行います。
オープンソース
Velodyne LiDAR用のレポジトリが公開されています。
私も使ったことがあり、とても精度が高い印象です。また、依存関係などもほとんどないので、すぐに導入できる点も便利です。
▶GitHub: laboshinl/loam_velodyne
さいごに
LOAMについてザックリまとめてみました。参考になれば幸いです。
以上です。
コメント