概要
LiDARは自動運転アルゴリズムを組む上で非常に強力なセンサであり、Lv4向け、Lv2+（Lv2++）向け問わず多くの企業が使用しています。しかし、LiDARは様々な面で使いづらい部分もあり、特に移動ロボットでLiDARを用いる場合に発生する「歪み」は最も厄介な現象の一つです。今回はこれについて取り上げます。
私は現在自動運転開発企業でセンサデータを用いた認識周りの開発を行っているのですが、過去に度々議論にも上がる事項であるにも関わらず、あまりまとまった文章がなかったように感じており、備忘録も兼ねて書いておこうと思いたち、この記事を書きました。また、この記事は、特にLiDAR初心者の方（例えば弊社の新入社員の方々等）向けに書いているため、どちらかといえば一部詳細な議論を省いてでも簡潔な説明をする方向性で記述していきます。
仮定
本記事では、LiDARは360度回転式のものを使うことを仮定します。例えばVelodyne VLS1281等が広く知られているかと思います。特に近年のLiDARは様々なスキャンパターンのものがあり、ものによっては今回の議論が一切成り立たない可能性もあるのでご注意ください。
LiDARの歪み補正とは
LiDARのセンサデータは点群であり、より詳細には下記のようなデータフォーマットで取得されます。
$$
P = \lbrace{\bf p}_i, t_i \mid i = 1, 2, \ldots, N \rbrace
$$
ここで、 ${\bf p} = (p_x, p_y, p_z)$は座標で、$t$はその点が取得された時刻です2。
LiDARのデータは360度分（=1スキャン）のデータが集まってからそれに対して処理をかけていく方法が一般的ですが、そもそも360度分のデータを取得するのはそれなりに時間がかかり、多くの場合は0.1[sec]周期で取得されます。もし $t_1 < t_2 < .... < t_N $であると仮定するなら、
$$
t_N - t_1 = 0.1
$$
ということになります。
この1スキャン分のデータを処理にかける場合、理想的には全ての点の時刻情報も全て考慮しながら進めていけるとよいのですが、アルゴリズムの複雑さが上がってしまう、そもそも計算コストも上がってしまう等のデメリットから、前処理の最終段の段階ではこの時刻情報を落とす場合が多いです。
ここで、何も補正せずにこの時刻情報だけを落とすことを考えます。つまり、時刻 $t=t_1$に
$$
P_{final}(t_1) = \lbrace{\bf p}_i \mid i = 1, 2, \ldots, N \rbrace
$$
というようなスキャンが得られた、という情報をアルゴリズムに渡す場合です。
このとき、ある問題が発生します。実際の幾何的な形状と、上記のスキャンデータが一致しなくなるのです。これは「本当は $t=t_i$に取得したスキャン点なのに、 $t=t_1$に取得したと嘘をついている」という部分に由来しています。
そのため、使用用途にもよりますが、この座標値を補正したいニーズが生まれます。これが「歪み補正3」です。補正方法自体は難しくはなくて、ざっくり述べると「自車の軌跡情報を用いて、全ての点を$t=t_1$に取得されたと仮定して ${\bf p} = (p_x, p_y, p_z)$を修正する」という処理をします。より具体的には、時刻$t=t$における車両の位置姿勢を${\bf \zeta}(t) \in SE(3)$とすると、
\begin{align}
\begin{pmatrix}
{\bf \tilde{p}}_i \\
1 \\
\end{pmatrix} =
\zeta(t_i) \zeta(t_1)^{-1}
\begin{pmatrix}
{\bf p}_i \\
1 \\
\end{pmatrix}
\end{align}
という処理を行います。その結果、歪み補正をした場合、最終的にアルゴリズムに渡す点群は下記のようになります。
$$
\tilde{P}_{final}(t_1) = \lbrace {\bf \tilde{p}}_i \mid i = 1, 2, \ldots, N \rbrace
$$
LiDARのセンサデータは点群数も多い一方で低レイテンシが求められるため、実際には6-DoFではなく3-DoFで近似して処理する、${\bf \zeta}(t)$の差分自体も車速やIMU等の低レイテンシ・高周期のデータを用いて近似的に求める等の工夫もありえますが、数式上は上記のようになります。
誤解を恐れずにシンプルに図解すると、下図のような生のスキャンデータ（平坦な環境において、地面に当たったとあるringのデータのみを抽出）を
下図のように補正するイメージです。断裂が生じてしまいますが、物理的にはこちらが正しいです。
歪み補正を使いたいとき、使いたくないとき
歪み補正は、特に${\bf p}_i$があたっている点が静止物（例えば建物とか、停車車両とか）である場合には有効で、観測誤差などを考えなければ基本的に上記の歪み補正によって$t=t_1$における正しい点の位置が取得されます。よって、静止物の幾何的な形状が重要になるアルゴリズムにとっては非常に強力な武器となります。例えば、点群地図とのマッチングによる位置姿勢推定（ICPやNDT）、周囲の道路環境の意味的な認識（Driveable areaの算出等）を行う場合がそれに該当します。
一方で、歪み補正を行うと、動的な物体はより歪んでしまうので、例えば車両の物体検知を行う場合などは不利にはたらく可能性があります。極論、LiDARの中で回転しているエンコーダの先を常に飛び回る虫がいるケースを考えるのが一番わかりやすくて、実態の大きさは数センチ程度なのに、スキャンデータとしては常に360度に広がる長い物体がいるように見えてしまいます。
おまけ１：数式による議論
おまけと言いつつ本題かもしれません。上記の議論を具体的に数式で確かめしつつ、歪み具合の定量値を見てみたいと思います。
まず、自車両は現時点（$t=0$）で世界座標系原点におり、速さ$v_E$で$x$軸正方向に等速直線運動をしているものと仮定します。
$$
{\bf x}_E(t) = {\bf v_E} t = (v_Et, 0)^T
$$
その上には回転式LiDARが搭載されており、その角速度は$\omega$です。また同じ空間上にはターゲット物体が${\bf v}_T$の速度で同じく等速直線運動をしているものとします4。
ここで、LiDARが、時刻$t=0$でターゲット物体上の点$A$, $t=\Delta t$で点$B$をそれぞれスキャンしたと仮定します。またターゲット物体上の点$A, B$の座標をそれぞれ${\bf x}_A(t), {\bf x}_B(t)$であるとしましょう。このとき、LiDARの点$A, B$それぞれのスキャン点情報は（自車両のyaw角は常に0で一定であることを利用して）、
$$
P_A = \left ({\bf x}_A(0) - {\bf x}_E(0), 0 \right) = \left ({\bf x}_A(0), 0 \right)
$$
$$
P_B = \left( {\bf x}_B(\Delta t) - {\bf x}_E(\Delta t), \Delta t \right)
$$
それぞれを、点Aの取得時刻$t=0$に合わせて歪み補正して得られるスキャン点は
$$
\tilde{P}_A = \left( {\bf x}_A(0), 0 \right)
$$
$$
\tilde{P}_B = \left( {\bf x}_B(\Delta t) - {\bf x}_E(0), 0 \right) = \left( {\bf x}_B(\Delta t), 0 \right)
$$
それはそう、みたいな数式が出てきました。なお、$t=0$に合わせて歪み補正をしているため、点Aの座標は変わっていないことに注意してください。
ここから歪みの程度を計算していきます。仮に、AとBの距離（同じ物体上の点なので一定）を$d$として、
$$
s_{processed} = \frac{|{\bf \tilde{p}}_B - {\bf \tilde{p}}_A|}{d}
$$
を「（歪み補正処理を実施した後の）歪み率」と呼ぶことにします。これを書き下すと
\begin{align}
s_{processed} &= \frac{|{\bf \tilde{p}}_B - {\bf \tilde{p}}_A|}{d} \\
&= \frac{|{\bf x}_B(\Delta t) - {\bf x}_A(0)|}{|{\bf x}_B(0) - {\bf x}_A(0)|} \\
&= \frac{|{\bf x}_B(0) - {\bf x}_A(0) + {\bf v}_T\Delta t|}{|{\bf x}_B(0) - {\bf x}_A(0)|}
\end{align}
一方で、歪み補正処理をする前の歪み率は
\begin{align}
s_{not processed} &= \frac{|{\bf p}_B - {\bf p}_A|}{d} \\
&= \frac{|{\bf x}_B(\Delta t) - {\bf x}_E(\Delta t) - {\bf x}_A(0)|}{|{\bf x}_B(0) - {\bf x}_A(0)|} \\
&= \frac{|{\bf x}_B(0) - {\bf x}_A(0) + ({\bf v}_T - {\bf v}_E)\Delta t|}{|{\bf x}_B(0) - {\bf x}_A(0)|}
\end{align}
と記述できます。
ここから解釈できることは色々ありますが、まず第一に、$s=1$となる（つまりターゲット物体が歪まない）条件を考えてみましょう。下記のようになります。
- 歪み補正ありの場合：${\bf v}_T = 0$の場合、つまり、ターゲット物体が静止しているとき
- 歪み補正なしの場合：${\bf v}_T - {\bf v}_E = 0$の場合、つまり、相対速度がゼロであるとき
次に、より定量的に歪み率を計算するために、いくつか仮定をおいていきましょう。ここでは、自車両の真横$y$[m]を走行する他車両であると仮定します。また点$B$についても、点$A$からみて$x$軸負方向に位置しているなど、一定の仮定をおきます。
\begin{align}
{\bf x}_A(0) =& (0, y)^T \\
{\bf x}_B(0) =& (-\Delta d, y)^T \\
{\bf v}_T =& (v_T, 0) ^T
\end{align}
このときのスキャン情報は
\begin{align}
{\bf p}_A &= \left (0, y\right)\\
{\bf p}_B &= \left(-\Delta d + (v_T - v_E) \Delta t,y\right)
\end{align}
$\Delta t, \Delta d$が微小であると仮定すると、${\bf p}_A$と${\bf p}_B$の位相差$\Delta \theta$は
\begin{align}
\Delta \theta &= \arctan ({\bf p}_B) - \arctan({\bf p}_A) \\
& \simeq \frac{-\Delta d + (v_T - v_E)\Delta t}{y}
\end{align}
一方で、$\Delta \theta = \omega \Delta t$であるから、
\begin{align}
\omega \Delta t &= \frac{-\Delta d + (v_T - v_E)\Delta t}{y}\\
\frac{\Delta d}{\Delta t} &= v_T - v_E - \omega y
\end{align}
これらを歪み率の式にそれぞれ代入すると、
\begin{align}
s_{processed} &= \frac{|{\bf x}_B(0) - {\bf x}_A(0) + {\bf v}_T\Delta t|}{|{\bf x}_B(0) - {\bf x}_A(0)|} \\
&= \frac{|-\Delta d + v_T \Delta t|}{|\Delta d|} \\
&= \frac{|v_E + \omega y|}{|v_T - v_E - \omega y|}
\end{align}
\begin{align}
s_{not processed} &= \frac{|{\bf x}_B(0) - {\bf x}_A(0) + ({\bf v}_T - {\bf v}_E)\Delta t|}{|{\bf x}_B(0) - {\bf x}_A(0)|} \\
&= \frac{-\Delta d + (v_T - v_E)\Delta t}{\Delta d} \\
&= \frac{|\omega y|}{|v_T - v_E - \omega y|}
\end{align}
それぞれの歪み率をいい感じに表すことが出来ました。この数式で少し遊んでみます。
一般道で自社を54km/hで追い越す車両の歪み率
$v_T = 54 [{\rm km/h}] = 15 [{\rm m/s}]$, $v_E = 36 [{\rm km/h}] = 10 [{\rm m/s}]$, $y=1 [\rm {m}]$, $\omega = 2\pi / 0.1 = 20\pi [{\rm rad/s}]$とすると、
\begin{align}
s_{processed} &= 1.26 \\
s_{not processed} &= 1.09
\end{align}
特に歪み補正をしてしまうと26%も歪んでしまうんですね。この数式あっているんでしょうか、、、と思わせられるくらい、結構大きそうですね。
高速道路で自車を108km/hで追い越す車両の歪み率
$v_T = 108 [{\rm km/h}] = 30 [{\rm m/s}]$, $v_E = 90 [{\rm km/h}] = 25 [{\rm m/s}]$, $y=1 [\rm {m}]$, $\omega = 2\pi / 0.1 = 20\pi [{\rm rad/s}]$とすると、
\begin{align}
s_{processed} &= 1.59 \\
s_{not processed} &= 1.09
\end{align}
歪み補正後のターゲット車両の歪み率は、先程よりも大きくなってしまいました。歪み補正後の歪み率は対象物の絶対速度に依存しますから納得です。一方で歪み補正前の歪み率は相対的に小さいことが確認できます。
真横を自車と同じ速度で走る車両の歪み率
$v_T = v_E = 108 [{\rm km/h}] = 30 [{\rm m/s}]$, $y=1 [\rm {m}]$, $\omega = 2\pi / 0.1 = 20\pi [{\rm rad/s}]$とすると、
\begin{align}
s_{processed} &= 1.47 \\
s_{not processed} &= 1.00
\end{align}
歪み補正後のターゲット車両の歪み率は依然として大きい一方で、歪み補正前のターゲット車両の歪み率は1になります。
下道で自車横にある路駐車の歪み率
$v_T = 0 [{\rm m/s}]$, $v_E = 36 [{\rm km/h}] = 10 [{\rm m/s}]$, $y=1 [\rm {m}]$, $\omega = 2\pi / 0.1 = 20\pi [{\rm rad/s}]$とすると、
\begin{align}
s_{processed} &= 1.00 \\
s_{not processed} &= 0.86
\end{align}
記事の前半でも見たとおり、静止物に対しては歪み補正をすることにより歪み率が1になることが確認できます。
横をとんでもない速度で走行する車両の歪み率
$v_T - v_E = 20\pi [{\rm m/s}]$とすると、
\begin{align}
s_{processed} = s_{not processed} = \infty
\end{align}
LiDARのエンコーダの先に常に車両がいるわけですから無限に長くなります。当然と言えば当然です。流体力学でいう音速のような感じです。ちょうど相対速度も音速に近いし。
おまけ２：LiDAR-camera fusionと歪み補正
近年だと、LiDAR-cameraのfusionによる認識精度向上が主流なアプローチで、すでに多くの企業で用いられていますし、アカデミア界隈での研究も盛んです（例えば物体検知系だと、BEVFusion等のBEV系手法が最近は大人気です）。
こうした手法の場合、カメラとLiDARの取得時刻がピッタリあっているか（≒カメラ画像にLiDARを重畳した時に一致しているか）は非常に重要で、PTPやハードウェアトリガを使うなどしてsub-msのオーダーで時刻同期をするのが一般的です。
ここで一つ問題になるのが、カメラのシャッタータイミングです。というのも、歪み補正をするかどうかによって最適なシャッタータイミングが変わる可能性があります。歪み補正をする場合は、 $t=t_1$でシャッターを切ると、カメラとLiDARがうまいこと重畳してくれます。一方で歪み補正をしない場合は、カメラの設置角度によってシャッタータイミングを変えるほうが重畳したときの誤差が少なく、例えば ${\bf p}_i$が取得される角度と同じ角度にカメラが設置されている場合は$t=t_i$でシャッターを切るように調整することで、重畳が一致しやすくなります。例えば、Autowareでもサポートしているpointpainting等の手法は重畳の一致度合いが非常に重要であり、アプリケーションによってはここをかなり注意深くチューニングする必要があります。
またカメラ自体も物理的な素子ですので、ピクセルごとに撮像されるタイミングが微妙に異なったり、そもそもデータ転送等にかかる時間もあること、また一度に送る場合は通信帯域や計算負荷的にもスパイクが大きくなるため、それらも考慮して最適なシャッタータイミングを決定していく必要があります。
まとめ
最後の方は自分がいつかまとめてみたかった数式周りを趣味で書き起こしたに過ぎず、特にreferenceがあるわけでもないので、普通に間違っているかもしれません。もし間違いを見つけた際はご指摘くださると大変嬉しいです。