Other articles

1. Visual yaw estimation

   以前紹介した 室内測位システム ではフレームの姿勢をMAVLinkで貰ってるんですが室内なのとコアレスモーターがついてるせいでフレームに載せているコンパスがくるってしまい、それで得られるyawの値も酷いものなので代わりに天井のカムからみたマーカーで推定できるヘッドの方向を使っています.

   一般には斜め上からみることになるので頭と尾のマーカーがちょうど南北を向いているように見えても機体が水平でないと実際には南北を向いていません. 絵で書くとこんな感じになります

   

   wが機体の頭と尾のマーカーを結んだ直線の方向でカムからこれをみると方向がvであるかのように見えています. 実際は真上から見た方向v'をpitchの分だけ回転したのがwなわけです. 斜めになった緑の面Hはカムやvやwを含む平面です.

   なので問題は見かけの方向を機首の上げ下げの情報で修正する、つまり

   vとpitchからwを求める

   になります. 通常このような問題は回転行列や球面角で計算するのだと思うのですがgeometric algebraを使って求めることができます.

   

   図のようにvをvとz-軸方向でできる面内でpitchだけ回転した黄色のベクトルの終点をPとしてこの点を通って水平に球上を回る円周Kを描きます. wの終点はK上にあるはずです. vとwを含む面Hはvとカムから球の中心への方向uとを含む面でもあるのでKとHの2つの交点のどちらかの表す方向がwになります. これらの操作はgeometric algebraでの簡単な操作に対応していてlibvsrだと

   auto bi = v ^ Vec(0, 0, 1);
   auto rop = Gen::rot(bi*(-pitch_angle/2));
   auto H = (PT(0, 0, 0) ^ v ^ u ^ Inf(1)).dual();
   auto K = v.sp(rop …

   read more

2. 貧者的局所測位系 The versorer's apprentice

   小さい機体でもSLAMこそが未来だと思うもののさすがに私では手がでないので貧者的局所測位系と名付けて天井に貼り付けたOpenMV cam+魚眼レンズとフレームからの情報を合わせてフレームにのせたマーカーの水平位置を推計するというのを作っています.

   

   魚眼レンズで得られるイメージセンサー上のマーカーの位置からcamからの方向を計算してマーカーの高さの情報をヒントにマーカーのcamを基準にした水平位置をだしてます. このセンサ上のマーカーの位置から3Dの方向を計算するところはいわゆるステレオグラフィックプロジェクション(立体射影)になっていて、三角関数や線形代数を使って計算できます.

   前置きが長くなりましたがフレームの水平位置を求めるところまでをgeometric algebraを使ったらどうなるか試してみました.

   auto pointO = Construct::point(0,0,0);
   auto pointP = Construct::point(0,0,-h);
   auto v = - Vec(x,y,-1) * Vec(0,0,1) / Vec(x,y,-1);
   auto line = pointO ^ v ^ Inf(1);
   auto dlp = (pointP <= Drv(0,0 …

   read more