landmark

landmarkの位置をworld座標にします。

CameraやLiDARなどは買うとします。
距離と向きは取得できるとします。

LiDARの位置はrobotのどこかに固定とします。
LiDARの位置はrobot中心に座標系変換できているとします。

001

robot中心からlandmarkまでの距離を\(l\)とします。
robotの向きからlandmarkの見える角度が\(\psi\)あるとします。

直交座標系に分解するので下記となります。

\[
y^{\prime\prime}=l\sin(\theta+\psi)
\]

\[
x^{\prime\prime}=l\cos(\theta+\psi)
\]

これはrobot中心からの\(x\)と\(y\)なのでrobot中心のworld座標系の位置を下記として

\[
\begin{pmatrix}
x \\
y
\end{pmatrix}
\]

landmarkのworld座標系の位置です。

\[
y^{\prime}=l\sin(\theta+\psi)+y
\]

\[
x^{\prime}=l\cos(\theta+\psi)+x
\]

広告