Globally Optimalな点群位置合わせ手法の調査

2010年台ごろに入ってから、３次元点群位置合わせの手法として"Globally Optimal*1"な物の提案が増えてきました。理論はおいて置いて、どのくらい実用になるかをテストするために、実装が公開されているものを中心にリストにまとめておきたいと思います。

1. Go-ICP: A Globally Optimal Solution to 3D ICP Point-Set Registration

Jiaolong Yang, et. al. ICCV2013, PAMI
iitlab.bit.edu.cn
解説

Go-ICP: グローバル最適(Globally optimal) なICPの解説 from Yusuke Sekikawa

www.slideshare.net

2. Super4PCS: Fast Global Pointcloud Registration via Smart Indexing

Nicolas Mellado, Dror Aiger, Niloy J. Mitra CGF2014
SGP | UCL
github.com
つかってみた、試してみた解説
「 Super4PCS 」という点群結合のライブラリを試してみました - Natural Software
JVR ― 自腹でバーチャルリアリティ ― Super4PCS　速度面で実用的ではなさそう。

3. Fast Global Registration

Qian-Yi Zhou, Jaesik Park, Vladlen Koltun, ECCV2016
Fast Global Registration - Vladlen Koltun
github.com
入力が３次元点群＋FPFHなどの３次元特徴量ベクトルなので、３次元特徴量の計算にも時間コストがかかることに留意。
速度的には、まだAlignment.exe(Indexed Imageを使った方法)で位置合わせした方が速そう。
読むべき論文多すぎて、サーベイがおっつかないよ。。。

*1:Globally Optimalの正しい和訳は知りませんが、端的な意訳は"初期位置あわせの要らない"だと思います。

初任給で買うと良いものの研究２０１７

初任給を貰ったら、まず親に御礼代わりのプレゼントを買うべきです。

が、自分で稼いだお金で自分の欲しいものを買うのも良いことです。
とくに、新生活が始まったばかりの人にとっては、今後の生活の潤いのためになるものを揃えておくのが良いでしょう。
というわけで、今自分が新社会人だったら何が欲しかったかなぁ、と考えてリストにまとめます。（仕事が手につかずに遊んでいる、ともいうが。。。）
物入りな時期でしょうから、予算は最大5万円をめどとします。

1. 大事なデータを保存しておくNAS

デジカメで撮りためた写真や書類など、日々溜まっていく重要なデータはほっておくと、すぐにどこかに行ってしまいなくして後悔します。
早めにデータの置き場所を決めて固めておいておく癖をつけておくとよいです。とくに、転勤などで引っ越しがあったときにも慌てずに済むので、オススメです。
HDDは時間が経つと壊れてしまうので、おすすめはミラーリングで良いのでRAID構成にしておくことです。

いろいろ調べた結果、性能値段比が一番いいのはQNAP TS-228のようです。

QNAP TS-228 専用OS QTS搭載 ARM デュアルコア1.1GHz CPU 1GBメモリ 2ベイ ホーム/SOHO向け プライベートクラウド機能対応 NAS 2年保証

本体にはHDDが付属していませんので、別途2台用意します。

東芝 MD04ACA400 4TB アマゾン限定モデル 2年保証 SATA 6Gbps対応3.5型内蔵ハードディスク

TOSHIBAの4TBモデルのAMAZON限定モデルは、2年保証が付いているのでお得感があります。（会社が心配ですが・・・）

2. 寝っ転がって本を読むタブレット端末

NASにデータを集める環境が作れたら、そのデータを簡単に閲覧する方法が欲しくなります。
PCがあればそれでも良いのですが、仕事で疲れているときには寝っ転がって写真を見たり本を読んだりしたくなります。
とくに仕事を始めると、眼の疲れを最小にすることの重要さにそのうち気が付きます。
タブレット端末は進化が早いので安いやつで良いのですが、ディスプレイの質の良いものがオススメです。
今選ぶなら、HuaweiのMedia T2 8.0 Proが2万未満で買えて、画面が奇麗でよいです。

Huawei 8インチ タブレット MediaPad T2 8.0 PRO ホワイト ※LTE,Wi-Fiモデル RAM 2GB/ROM 16GB【日本正規代理店品】

3. 腰や肩を大事にするための敷布団マットレス

仕事中は自由に体を動かせず、毎日決まった姿勢をし続けなければならないような状況が起きてしまいます。
とくにPCを相手にする仕事の場合、腰に負担がかかります。
普通の布団では寝ていても腰に力が掛かってしまい、あまり疲れが取れないという状況が起きます。
良さげなマットレスが世の中にいっぱいありますが、お年寄向けのものも多く、ちょっと気が引けます。
その点、西川Airはオシャレです。価格の高いモデルもあり、更にかっこよいですが、エアー01で十分効果があります。
長年使っているうちに多少ヘタって来てしまいますので、標準体重ぐらいの男性はハード、女性はソフトが良いように思います。

東京西川 [エアー01] マットレス シングル ハード ネイビー

シーツは洗濯の時に取り外ししやすい、純正のラップシーツが良いですよ。

東京西川 ラップシーツ シングル エアー マットレス専用 アウトラスト使用 ネイビー

4. 首を大事にするための良い枕

首は、腰と同じように負担がかかります。
自分の体に合った枕を選ぶのが一番良いですが、西川Airのマットレスを買ったら、同じシリーズの枕で揃えるのも良いですね。
中のマットを抜くと高さを調整することができます。

東京西川 枕 [エアー3D] コンディショニングピロー 高め タフ ネイビー

カバーは、マットレスの時と同じように、洗濯のために取り外ししやすいものが良いですよ。

東京西川 枕カバー wrap ストレッチ素材 抗菌防臭 ダークブルー

5. 快適な睡眠のためのゴアテックス羽毛掛け布団

敷布団は、西川Airがとても良いのですが、合わせる掛け布団に悩みます。
温かくて、軽くて、水分調整ができて、虫がわきにくい、そんな都合が良い羽毛掛け布団が西川にあります。

西川リビング　シングル　ゴアテックス　羽毛掛け布団（日本製）　 (ブルー色)

ちょっと5万円をはみ出てしまいますが、普通の羽毛布団に比べてメンテナンスが楽な分、長い間使えるメリットがあります。
掛け布団カバーは、デザイン重視でお好きなものを。

西川 リビング 掛けふとん カバー 150×210cm ON15 シングル ベージュ 2138-15137

毛布は肌触りの良いものを。

東京西川 GIVENCHY ジバンシィ アクリルニューマイヤー毛布 シングルサイズ 140x200cm カラー：ベージュ

結局、ほとんど睡眠用品になってしまいましたが、社会人にはそれだけ睡眠の質が重要だということ。

Emlid Navio2で動かすardupilotにxwindowを入れる

要らないものもあるかもしれないけれど、とりあえず起動するのでＯＫ。

sudo apt-get update
sudo apt-get install lxde
sudo apt-get install lightdm
sudo apt-get install xinit
sudo apt-get install xutils
sudo apt-get install xserver-xorg

startxでLXDEというデスクトップが立ち上がります。

lxdeは古いので、新しいPIXELを使いたい、という場合はこちら。

sudo apt-get install raspberrypi-ui-mods

sudo apt-get install firefox-esr

単にxwindowを入れた状態ではlibEGLの設定がおかしな状態になっていて、openFrameworksやQtが立ち上がらないという問題があります。
[1]のサイトを参考に、libEGLのライブラリへのリンクを張りなおすと立ち上がるようになります。
raspi-configでGL DriverをLegacyに設定するのを忘れずに。

# sudo ln -fs /opt/vc/lib/libEGL.so /usr/lib/arm-linux-gnueabihf/libEGL.so
# sudo ln -fs /opt/vc/lib/libGLESv2.so /usr/lib/arm-linux-gnueabihf/libGLESv2.so
# sudo ldconfig

[1] raspbian - Qt applications don't work due to libEGL - Raspberry Pi Stack Exchange

掃除機の買い方の研究

掃除機は、多くの人には数年に一度しか買い替える機会がないものかと思います。
急に掃除機が欲しくなるというよりも、そろそろ買い替えたいなー、という気分になったときしばらく時間をかけて検討して買い替えするかと思います。
ということは、時々安くなるセールを狙って買えるということです！定期的に安くなるモデルを見つけたので、紹介します。

何を買うか

ダイソンのハンディのバッテリー駆動モデルと、ロボット掃除機のルンバを狙います。

ビックカメラの量販店モデル
ダイソンDC74 MH EX：　

ダイソン 【国内正規品】 コードレスクリーナー 「Dyson V6 cord-free」　DC74 MH EX 【ビックカメラグループオリジナル】

ルンバ631：　

iRobot ルンバ ロボット掃除機 631 【国内正規品】

ジャパネットたかたの量販店モデル
ルンバ626：　

iRobot Roomba 自動掃除機 ルンバ626 ホワイト R626060 [ジャパネットオリジナルモデル]

いずれもお買い得感のあるエントリーモデルです。

どこで買うか

ビックカメラ・コジマか、ジャパネットたかたをチェックします。
店頭でもネットでも価格は同じでしたが、ポイントの付き方が違います。
アマゾンポイントが安く買えて使えるときは、アマゾンがお得かと思います。

いつ買うか

ジャパネットたかたのセールの日に、ビックカメラがえげつなくセールをぶつけてくることが多いので
半期に一度ぐらいのジャパネットたかたのセールをテレビで見かけたら、ビックカメラのセール品をチェックします。

いくらで買うか

半年に一度ぐらい、どちらも\29800で販売されることがあります。
去年の実績は10月ごろと3月ごろに、それぞれ一日だけその価格で販売されていました。

LAUNCHXL-F28069MとBOOSTXL-DRV8305EVMでSCIを使うとき、問題を避けるためにADCの割り込み優先順位を上げる方法

TiのC2000Piccoloの割り込みは、独自のPIEテーブルとかいうものに基づいたもので、とても分かりにくく使いにくいです。
C2000シリーズではPIEテーブルの順序の変更も容易ではなく（掲示板にはデキナイ、との書き込みも）、さらにその定義も小首を傾げたくなるような順序に並んでいます。
InstaSPINモータドライバを使うとき、ADCがフル稼働しているのですが、標準だとこの優先順位がSCI(UART)よりも低く設定されているため、SCI割り込みが入るとモータが動かせないという謎仕様です。
ただし、これは問題を知っていれば簡単に回避できます。

参考資料

このスレッドなどを参考に。
level interrupts in motorware_12 - InstaSPIN Motor Solutions Forum - C2000™ Microcontrollers - TI E2E Community

割り込みトリガーをADC_IntNumber_1からADC_IntNumber_1HP（High Priority)に変更

なぜか、同じADCに優先順位の設定が2つ割り当てられていて、10番（標準）と1番（優先）です。
デフォルトで10番に割り当てられているものを1番のPIEで駆動されるように変更します。

hal.c

void HAL_enableAdcInts(HAL_Handle handle)
{
  HAL_Obj *obj = (HAL_Obj *)handle;

  // enable the PIE interrupts associated with the ADC interrupts
  PIE_enableAdcInt(obj->pieHandle,ADC_IntNumber_1HP); // ADC_IntNumber_1 to ADC_IntNumber_1HP

  // enable the ADC interrupts
  ADC_enableInt(obj->adcHandle,ADC_IntNumber_1);

  // enable the cpu interrupt for ADC interrupts
  CPU_enableInt(obj->cpuHandle,CPU_IntNumber_1); // CPU_IntNumber_10 to CPU_IntNumber_1

  return;
} // end of HAL_enableAdcInts() function

hal.h

static inline void HAL_initIntVectorTable(HAL_Handle handle)
{
  HAL_Obj *obj = (HAL_Obj *)handle;
  PIE_Obj *pie = (PIE_Obj *)obj->pieHandle;

  ENABLE_PROTECTED_REGISTER_WRITE_MODE;

  pie->ADCINT1_HP = &mainISR; // pie->ADCINT1 to pie->ADCINT1_HP
  //pie->SCIRXINTA = &sciarxISR;

  DISABLE_PROTECTED_REGISTER_WRITE_MODE;

  return;
} // end of HAL_initIntVectorTable() function

hal.h

static inline void HAL_acqAdcInt(HAL_Handle handle,const ADC_IntNumber_e intNumber)
{
  HAL_Obj *obj = (HAL_Obj *)handle;

  // clear the ADC interrupt flag
  ADC_clearIntFlag(obj->adcHandle,intNumber);

  // Acknowledge interrupt from PIE group 1
  PIE_clearInt(obj->pieHandle,PIE_GroupNumber_1); // PIE_GroupNumber_10 to PIE_GroupNumber_1

  return;
} // end of HAL_acqAdcInt() function

これでも、SCITX割り込みがうまく動かない。なんでた”～。

SLAM実験に便利そうな移動ロボット用プラットフォーム

筑波大の油田研の移動ロボットの研究成果を元に、ハードウェアやソフトウェアが整備されているそうです。
現在は、東北大の渡辺先生を中心にメンテナンスされているとのことです。

ソフトウェア

openspurはskid型の移動ロボットを座標指令で動かしたいときに便利そうなライブラリです。
Spurはシュプールと発音するそうです。
OpenSpur.org

Google グループ

YP-Spur [Robot Platform Project Wiki]

ハードウェア

エンコーダ付きACモータや、モータドライバなどのハードウェアを購入できるようになっています。
t-frog.com

ROS連携なども整備されているようですから、
つくばチャレンジ的な本格的な実験や、ちょっとしたSLAMの実験にも便利に使えそうです。
論文や資料も充実していますので、勉強のための教科書的に一度目を通しておくと良さそうです。

ただ願わくば、
もう少し明確にハードウェアとソフトウェアの切り分けをして欲しかった。

Emlid Navio2で動かすardupilotにもzeroMQを導入する

navio2はEmlid社の開発するRaspberry Piベースのオートパイロット用ボードで、オープンソースのドローンシステムardupilotに対応しています。
emlid.com
詳細はこちら。
Navio2 docs

まず、マニュアルに従ってAPMを普通にインストールしてください。
ここでは、planeでもcopterでもなくroverをターゲットとすることを前提にします。

基本的にRCサーボモータを出力対象として設計されていますので、ロボット用コマンドサーボやもっと大型の汎用モータを動かすためにはインターフェースを自分で用意してやる必要があります。
navio2ボード上にI2CやUARTのコネクタがありますので、それを使って出力を取っても良いですし、RaspberryPiのUSBやLANも使えますのでそれらを使って外部のデバイスとつなげても良いです。

ただ、あらゆるデバイスはzeroMQで繋げてしまえばいいじゃん、という思想があるので、このページではardupilotでzeroMQを使う方法を調査します。

準備

1. wafを見てみる

新しいビルドシステムwafで、ライブラリを追加する方法がわからない。
[1]を勉強して、

def build(bld):
bld(...
lib = ['m'],
libpath = ['/usr/lib'],
...)

のように書けばいいのだろうな、などと推測はつくものの、
ardupilotのwscriptが大きくて、あちらこちらに散在するので、何がどういう順番で呼ばれるのかよくわからない。
*1

2. Configure/Makeを見てみる

> cd ardupilot
> make -C APMrover2 navio2

で試してみるとまだ使える。廃止されると書かれているが。こちらなら使い慣れていてどこに何を書けばいいのかもわかるので、当面こちらで乗り切ることにする。

3. zeroMQのインストール

zmqは普通にライブラリをインストールしてください。ソースからが無難だと思います。
c++用のbindingを/usr/local/includeに置いてください。

Makefileの編集

ardupilotのmakefileが存在する場所は、~/ardupilot/mkの下です。
zeroMQを追加するときに編集するべきファイルは、board_native.mkです。

CXXOPTS = -ffunction-sections -fdata-sections -fno-exceptions -fsigned-char

と書かれた一文を探して、zmqを使うのに都合がいいように次のように変更します。

CXXOPTS = -ffunction-sections -fdata-sections -fexceptions -fsigned-char
LIBS += -lzmq -lpthread

no-exceptionsを外してしまったので、予期せずエラー時の挙動が何か変わってしまうかもしれません。注意してください。
とりあえず、これでbuildは通るようになります。

モータの出力をpubで垂れ流す準備

APMrover2/Rover.hにコンテキストとソケットの定義を追加

#include <zmq.hpp> // 追加

class Rover
{
・・・

public:
    zmq::context_t *context;
    zmq::socket_t *publisher;
};

APMrover2/Rover.cppのRoverコンストラクタ内で初期化

Rover::Rover(void) :
・・・
{
    context = new zmq::context_t(1);
    publisher = new zmq::socket_t(*context, ZMQ_PUB);
    int v = 1;
    size_t size = sizeof(v);
    publisher->setsockopt(ZMQ_SNDHWM, &v, size);
    publisher->bind("tcp://*:6636"); // 送信先IPとポート番号 
}

RCサーボモータへの出力をzmqで流す

出力っぽい名前の関数呼び出しを探していくと、サーボモータへの出力はStering.cpp内で行われてることが見つかります。

void Rover::set_servos(void){
・・・
channel_steer->output();
channel_throttle->output();
・・・
}

ここのPWM出力していると思しき値の、元になった値をもらって来ればよさそうです。

channel_steerもchannel_throttleもRC_Channelというクラスのインスタンスのようなので、RC_Channelのクラスの定義を探しますが、APMrover2の中には見当たりません。
探すと、~/ardupilot/libraries/RC_Channelの下に関連ファイルがありそうです。
RC_Channel.hの中を見ていくと、

int_16t get_servo_out() const { return _servo_out; }
void set_servo_out(int_16t val) {_servo_out = val; }

というのがあり、RC_Channel.cppの中でoutput()から辿ってcalc_pwmの中を見ていくと、range_to_pwm()などの中で_servo_out変数が随所で使われていますので、とりあえずコレで良さそうです。
レンジをpwm用に整形された値を得たい場合は、get_pwm_out()関数を使います。

ということで、
Stering.cppのser_servos(void)関数の中に、zeroMQ出力を追加します。

channel_steer->output();
channel_throttle->output();

int16_t l = channel_steer->get_servo_out();
int16_t r = channel_throttle->get_servo_out();

int msg_size = 20;
zmq::message_t msg(msg_size);
memset (msg.data (), 0, msg_size);
snprintf((char*)msg.data(), msg_size,
"/servo_out %d %d", l, r);
publisher->send(msg);

これでひとまず行けそうです。

お片付け

行儀よくデストラクタで、contextとsocketを削除するようにしないといけません。
Rover::~Rover(void);の定義をRover.hの中に追加して、

Rover::~Rover(void)
{
    if(publisher){
         int v = 0;
         size_t size = sizeof(v);
         publisher->setsockopt(ZMQ_LINGER, &v, size);
         // publisher->close();
         delete publisher;
    }

    if(context){
         // context->close();
         delete context;
    }
}

とします。

実行は、別のコントローラ用PCを用意してAPMPlanner2などを立ち上げておいてから、
raspberrypi側で、

> cd ardupilot/APMrover2
> sudo ./APMrover2.elf -A udp:192.168.10.41:14550

のようにします。14550は規定のport番号、192.168.10.41はコントローラ用PCのIPアドレスの例です。

これとは別に、zeromqのsubを受け取るプログラムを書いて受け取ってみると、

/servo_out -15796 -4

といった感じでデータが送られてきていることが分かります。

zeroMQで繋がってさえしまえば、あとはもう自由に料理できますね！

追記

最近、ソースコードをダウンロードした場合、mavgenに必要なpythonライブラリが正しくインストールされなくてエラーが出ます。
解決方法は、こちらのページを参照してください。
ArduPilot 入門 (7)：ビルドが失敗する、シミュレーターが起動しない場合のトラブルシューティング – Drone Japan

> sudo pip install future
> sudo pip install lxml

[1] wafチュートリアル
waf チュートリアル - 純粋関数型雑記帳

*1:プログラムをビルドをするためだけに、また大きなプログラムを書かなければいけない本末転倒な仕組みな気がするので、wafは今のところ評価できない。 そのうち、wafを書くためだけのツールが出てきそうな気がする。