LIOとは
LiDAR-Inertial Odometry（LIO）が近年広く利用されています．LIOとは，LiDARとIMUの計測値を融合して，高精度に移動量を推定する技術です．自己位置推定やSLAMにも応用可能な技術です．
一般的にLiDARとIMUには以下の利点と欠点があります．
- LiDAR
- 利点：計測精度が高く高精度な位置決めに利用可能
- 欠点：計測周期が遅く（10~20Hz）素早い移動，特に回転を追従できない
- IMU
- 利点：高周期（50~1000Hz）で角速度と加速度を計測可能であり，これらを積分するだけで位置・姿勢が求まる
- 欠点: ドリフト誤差が大きく，求められる位置・姿勢の精度は極めて低い
LIOはLiDARとIMUの利点をうまく組み合わせて，素早い回転を含む様な動きに対しても高精度な位置決めを行えるようにした技術です．
LIOの実装は主にタイトカップリングに基づく手法とルーズカップリングに基づく手法の2つがあります．ここではこれらの種類に関する解説は省きますが，端的に言えばこれらの違いはLiDARとIMUの計測値の融合方法であり，タイトカップリングの方がより正確な融合を行っていると言えます．そのため，基本的にはタイトカップリングの方が性能が高いです．ただしパラメータ調整が大変なことが多いです．
一般的なLIOでは，推定する状態を以下とします．
$$
\left( R, \mathbf{p}, \mathbf{v}, \mathbf{b}^{g}, \mathbf{b}^{a} \right)
$$
ここで$R \in SO(3)$と$\mathbf{p} \in \mathbb{R}^{3}$は3次元の回展行列と並進ベクトルです（多くの論文では${}^{W}R_{I}$（IMU座標からWorld座標に点を変換する回転変換行列）のようにフレームが記載されていますが，今回の記事では煩雑さを回避するために省略しています）．また$\mathbf{v}, \mathbf{b}^{g}, \mathbf{b}^{a} \in \mathbb{R}^{3}$はそれぞれWorld座標での速度と，IMUが計測する角速度と加速度に対するバイアスになります．
時刻$t$におけるこの状態変数を例えば$\mathbf{x}_{t}$とすると，一般的なLIOでは，時刻$t+1$での状態$\mathbf{x}_{t+1}$，時刻$t+2$での状態$\mathbf{x}_{t+2}$のように，各時刻の状態を離散的に求めていきます．なお今回の記事ではこれらの実装については触れませんが，実装の例は筆者が以前にまとめたLiDAR-Inertial SLAM技術ノートにて紹介しています．
前置きが長くなりましたが，今回紹介するのはこのLIOを連続時間の軌跡表現を利用して実装したContinuous-Time LIO（CT-LIO）になります．
連続時間の軌跡表現を用いる利点
連続時間の軌跡表現を用いると，例えばある任意の時刻$t$における姿勢を$\left( R(t), {\bf p}(t) \right)$として得ることができます．これは離散的な時刻ではなく，例えば時刻$t$，$t+1$の間の時間でも求めることができるという意味になります．
LIO（それ以外のカメラなどの融合を含むものも）では，異なる種類のセンサーを扱うため，どうしても時間同期が問題になります．前述の通り離散時間で状態を表現すると，厳密にセンサー間のデータを同期することはできません．
例えばLIOでは，LiDARが計測した点に付与されているタイムスタンプを用いて，計測点群の歪み補正を行います．この歪み補正は基本的に，計測点のタイムスタンプとIMUのタイムスタンプを比較し，最も近いIMUの計測値（もしくは補間）を元に移動量を予測し，これに基づき計測点の位置を変化させます．そのため，厳密に正しい時間で計測点を変化させているとは言えません．
しかし連続時間表現を用いると，任意の時刻の姿勢を計算できるため，時間同期がかなり厳密に行えるようになります．
Cumulative B-Splineを用いた軌跡の表現
今回の記事では，3次のスプライン曲線を考えて，連続時間の軌跡を表現する方法で考えます（CT-LIOの実装方法は他にもあります）．
今，4つの制御点（Control Point）として，$\left( R_{0}, {\bf p}_{0} \right)$，$\left( R_{1}, {\bf p}_{1} \right)$，$\left( R_{2}, {\bf p}_{2} \right)$，$\left( R_{3}, {\bf p}_{3} \right)$が選ばれたとします．なお，各制御点の時刻を$t_{0}$，$t_{1}$，$t_{2}$，$t_{3}$，計算したい姿勢の時刻を$t$とすると，$t_{0} < t_{1} < t_{2} \leq t < t_{3}$という関係になります．また，各制御点間の時間間隔は均等であるとします．これらの制御点を用いて，時刻$t$における姿勢$\left( R(t), {\bf p}(t) \right)$を以下の様に計算されます．
$$
R(t) = R_{0} \prod_{j=1}^{3} {\rm Exp} \left( \lambda_{j}(t) \boldsymbol \phi_{j} \right)
$$
$$
\boldsymbol \phi_{j} = {\rm Log} \left( R_{j-1}^{-1} R_{j} \right)
$$
$$
{\bf p}(t) = {\bf p}_{0} + \sum_{j=1}^{3} \lambda_{j}(t) {\bf d}_{j}
$$
$$
{\bf d}_{j} = {\bf p}_{j} - {\bf p}_{j-1}
$$
ただし，${\rm Exp}(\cdot)$はある3次元ベクトルを対応する$\mathfrak{so}(3)$に変換した後に対応する$SO(3)$に写像する関数，${\rm Log}(\cdot)$は$SO(3)$を対応する$\mathfrak{so}(3)$に写像した後に3次元ベクトルを取り出す関数になります．
ここで，$\boldsymbol \lambda = \left( \lambda_{1}, \lambda_{2}, \lambda_{3} \right)^{\top} \in \mathbb{R}^{3}$とすると，$\boldsymbol \lambda$は以下のように計算されます．
$$
u = \frac{t - t_{2}}{t_{3} - t_{2}}
$$
$$
{\bf u} = (1, u, u^{2}, u^{3})^{\top} \in \mathbb{R}^{4}
$$
$$
\boldsymbol \beta = \frac{1}{6}
\left( \begin{matrix}
1 & -3 & 3 & -1 \newline
4 & 0 & -6 & 3 \newline
1 & 3 & 3 & -3 \newline
0 & 0 & 0 & 1
\end{matrix} \right) {\bf u}
$$
$$
\boldsymbol \lambda = \left( \beta_{1} + \beta_{2} + \beta_{3}, \beta_{2} + \beta_{3}, \beta_{3} \right)^{\top}
$$
残差の定義とヤコビアンの計算
Cumulative B-Splineを用いて連続時間状の姿勢$\left( R(t), {\bf p}(t) \right)$が計算された後に，最適化に使用する残差の計算を行います．今回はスキャンマッチングの残差について考えます．
スキャンマッチングの残差${\bf r}$は以下の様に記述されます．
$$
{\bf r} = {}^{M}{\bf p} - \left( R(t) {}^{I}{\bf p} + {\bf p}(t) \right) \in \mathbb{R}^{3}
$$
なお${}^{I}{\bf p}$はIMU座標での点，${}^{M}{\bf p}$は${}^{I}{\bf p}$を座標変換した後の点に対する地図点群内に含まれる最近傍点です（スキャンマッチングではLiDARが計測した点を使うため，計測点はLiDAR座標にありますが，LIOではよくこれをIMU座標に変換して利用します）．またこの残差を計算する際に利用される時間$t$はLiDARの計測点に付与されているタイムスタンプであるため，わざわざIMUとの時間同期を行わなくとも，厳密に時間を合わせることができ，明示的に歪み補正を行わなくても歪みを補正が行われるようになります．
通常のLIOでは，この残差${\bf r}$に関する$R(t)$と${\bf p}(t)$に関するヤコビアンを計算し，これに基づき姿勢を更新していきます．一方でCT-LIOでは，残差${\bf r}$に関する各制御点でのヤコビアン$\partial {\bf r} / \partial R_{j}$，$\partial {\bf r} / \partial {\bf p}_{j}$を求め，各制御点を更新していきます．そして更新した制御点を用いて最終的な姿勢を求めます．
なおこれらのヤコビアンは，連鎖則を用いてそれぞれ以下の様に計算されます．
$$
\frac{ \partial {\bf r} }{ \partial R_{j} } = \frac{ \partial {\bf r} }{ \partial R(t) } \frac{ \partial R(t) }{ \partial \boldsymbol \phi_{j} } \frac{ \partial \boldsymbol \phi_{j} }{ \partial R_{j} } + \frac{ \partial {\bf r} }{ \partial R(t) } \frac{ \partial R(t) }{ \partial \boldsymbol \phi_{j+1} } \frac{ \partial \boldsymbol \phi_{j+1} }{ \partial R_{j} }
$$
$$
\frac{ \partial {\bf r} }{ \partial {\bf p}_{j} } = \frac{ \partial {\bf r} }{ \partial {\bf p}(t) } \frac{ \partial {\bf p}(t) }{ \partial {\bf d}_{j} } \frac{ \partial {\bf d}_{j} }{ \partial {\bf p}_{j} } + \frac{ \partial {\bf r} }{ \partial {\bf p}(t) } \frac{ \partial {\bf p}(t) }{ \partial {\bf d}_{j+1} } \frac{ \partial {\bf d}_{j+1} }{ \partial {\bf p}_{j} }
$$
なお$j=0$と$j=3$の場合は，少し計算結果が変わりますが，そちらは割愛します．
一般的なLIOで使われるヤコビアンと比べると，かなり複雑になります．特に回転に関するヤコビアンは，${\rm Exp}(\cdot)$と${\rm Log}(\cdot)$を含むため，リー群やリー代数を使いながらヤコビアンを計算していくので非常に煩雑になります．
このヤコビアンの詳細な導出方法に関しては，Continuous-Time LIOにおけるヤコビアンの導出に別途まとめています．
IMUとの融合
一般的なLIOでは，IMU preintegrationというものを用いてLiDARとの融合を行います．IMU preintegrationを用いると，離散的に回転，速度，並進を以下の様に更新できます．
$$
R_{t} = R_{t-1} {\rm Exp} \left( \left( \boldsymbol \omega_{t} - {\bf b}^{g}_{t}\right) \Delta t \right)
$$
$$
{\bf v}_{t} = {\bf v}_{t-1} + R_{t-1} \left( {\bf a}_{t} - {\bf b}^{a}_{t} \right) \Delta t + {\bf g} \Delta t
$$
$$
{\bf p}_{t} = {\bf p}_{t-1} + {\bf v}_{t-1} \Delta t + \frac{1}{2} R_{t-1} \left( {\bf a}_{t} - {\bf b}^{a}_{t} \right) \Delta t^{2} + \frac{1}{2} {\bf g} \Delta t^{2}
$$
ここで$\boldsymbol \omega_{t}$と${\bf a}_{t}$はIMUが計測した角速度と加速度，${\bf g}$は重力加速度ベクトル，$\Delta t$は時間間隔です．
IMU preintegrationを用いると，ある区間でどれだけ回転，速度，並進成分が変化したかを観測することができます．一般的なタイトカップリングに基づくLIOでは，この変化の観測値と，実際に推定した値間での変化の差が最小になる様に，最適化を実施します．
一方CT-LIOでは異なる方法で融合を行います．
CT-LIOでは軌跡を連続時間を用いて表現してるため，軌跡に対して時間微分を定義することができます．すなわち，回転の一階微分から角速度$\boldsymbol \omega(t)$，並進のニ階微分から加速度${\bf a}(t)$を計算することができます．なおこれらは以下の様に計算できます．
$$
\boldsymbol \omega(t) = \left( R(t)^{\top} \dot{R}(t) \right)^{\vee} = \left( R(t)^{\top} \left( \dot{A}_{1} A_{2} A_{3} + A_{1} \dot{A}_{2} A_{3} + A_{1} A_{2} \dot{A}_{3} \right) \right)^{\vee}
$$
$$
A_{j} = {\rm Exp} \left( \lambda_{j} \boldsymbol \phi_{j} \right)
$$
$$
{\bf a}(t) = \sum_{j=1}^{3} \ddot{\lambda}_{j} {\bf d}_{j}
$$
そして，この計算により得られた角速度$\boldsymbol \omega(t)$と加速度${\bf a}(t)$と，IMUにより計測された角速度${}^{I}\boldsymbol \omega$と加速度${}^{I}{\bf a}$を比較することで残差を定義できます．
$$
{\bf r} =
\left( \begin{matrix}
\boldsymbol \omega(t) - {}^{I}\boldsymbol \omega + {\bf b}^{g}_{t} \newline
R(t)^{\top} \left( {\bf a}(t) + {\bf g} \right) -{}^{I}{\bf a} + {\bf b}^{a}_{t} \newline
\end{matrix} \right) \in \mathbb{R}^{6}
$$
CT-LIOでは，スキャンマッチングの誤差とこの残差を同時に最小化することで融合を行います．なお計算により得られた加速度${\bf a}(t)$はWorld座標になっているため，IMU座標に戻してから比較を行います．
なおこの残差に関しても，同様に制御点毎にヤコビアンを求めていきますが，ヤコビアンの導出はより煩雑になります．特に角速度に関しては，回転の一階微分も含まれているためより煩雑さが増します．
こちらのヤコビアンの導出方法についてもContinuous-Time LIOにおけるヤコビアンの導出にまとめています．
実装
CT-LIOの実装例としてctlio_ros2を公開しています．
こちらのパッケージと，Livox Mid-360を用いてマッピングを行った一例が以下となります．この画像ではグリッドの間隔が10mなので比較的大きな環境ですが，定性的には，大きく歪まずきれいにマッピングができています．
おわりに
Cumulative B-Splineにより軌跡を連続時間表現したCT-LIOについて解説しました．CT-LIOは一般的なLIOと比べてヤコビアンの導出が複雑になります．そのため実装はかなり大変になりますが，実装してみた感想として，かなり細かい部分まで色々把握できたなと感じています．実装にあたってどの様にヤコビアンを導出したかも別途まとめていますので，SLAMに関する勉強をされる方の参考になれば幸いです．
また，この様な実装をお願いしたいなどあればご連絡ください．
n.akai.goo[at]gmail.com [at] -> @