Raspberry Pi と MPU-6050を使ってデータ取得(C++)

CやC++でMPU-6050のデータを取得したい場合は

http://www.raspberrypi.org/forums/viewtopic.php?t=22266

のプログラムを使えばよい。

RaspberryPiのBを使っている場合はリンク先にも書いてあるようにI2Cdev.cppの中の/dev/i2c-0を/dev/i2c-1に書き換えなければならない。

3D fireframeを動かすでもはみていないけど、クォータニオンを使っているようだ。勉強してないので分からんな。

Raspberry Pi と MPU-6050で角度、加速度データを取得する。

ポータブルな角度データ取得装置を作りたかったので、RaspberryPiとMPU-6050で作ってみた。
MPU-6050はSparkfunとかでは$40とかいう値段やけど、アマゾンだと480円であった。

解説はここに乗っているのでAruduinoを使う場合は参考になりそうだ。
今回の用途はデータを記録したいので、記録容量の観点からRaspberryPiを利用してみる。

まずはこのサイトを見ながら接続と動作を確認する。
Pythonでいけるんですねー。
http://blog.bitify.co.uk/2013/11/interfacing-raspberry-pi-and-mpu-6050.html
http://blog.bitify.co.uk/2013/11/reading-data-from-mpu-6050-on-raspberry.html

今売っているのはRevision2なので注意。
この通りにやるとちゃんと動いていそうだ。


Androcitiには5V動作もOKとか書いてあったのではじめはPiの5Vに接続しとったけど、
データシートをみるとそんなことは書いていない…一応まだ動いている。

Pythonスケールするところは、Rawデータを変換する係数らしい。データシートから値が分かる。
リンクのプログラムでは加速度データを整数で割り算している。これは精度の問題で小数点以下が意味を成さないということなのだろう、多分。止めて測っても下一桁は変わっとるし。

エピポーラ幾何

OpenCVでステレオカメラのキャリブレーションを行うと、引数にEssential MatrixやらFundamental Matrixやら出てきてわからなかった。
英語の文献を読むとわかりやすかった。分かれば大したことないかもしれんけど、ぼくみたいな誰かの参考になるようここにメモっておこう。

参考文献：　http://www.robots.ox.ac.uk/~vgg/hzbook/hzbook1/HZepipolar.pdf



ではエピポーラ幾何について話す。
下の図のようにカメラが２つあって、どちらにも点Pが写っているとする。

左のカメラから見た点が、右のカメラのどの座標に写っているかが分かれば、
△OlOrPからPの深さが分かる。

しかし、実際はその対応関係がわからない（下図）。
対応付けをするためには、画素を調べて同じ点を探す必要がある。



しかし、2次元画像のすべての画素の対応を調べる必要はない。
２つのカメラの位置関係がわかっていれば、
その探索は1次元（エピポーラライン上）で済む。

ではその理由を数式を用いて探っていく。
△OlOrPの形は定かではないが、
,


は同じ平面上にあるという制約（Epipolar constraint)がある。
よって以下の式が成り立つ。



これを左のカメラの座標で書き直すと



ｔ、Rは右の座標から左の座標へのtranslation vectorとrotation matrix。
ここで外積はベクトル→ベクトルの変換なので等価な行列表記にすると以下のようになる。





このEがEssential Matrixで、Rとt（カメラの外部パラメータ）から成り立っている。

これはフィルム上の点でも成り立つ。つまり



などとフイルム上の座標を表記したとき、



が成り立つ。


Fundamental Matrixはにカメラの内部パラメータも押し込んだ形のものだ。
フィルム座標からピクセル座標への変換はAffine transform matrix以下のように表せる。



これを上の式に代入すると、





Fはカメラの外部パラメータ、内部パラメータをひっくるめたものであることがわかる。

で、ここから何がわかるかだ。

ピクセル座標での直線を考えるよう。直線の方程式は以下のようになる。




とおくと上のFundamental Matrixの式と同じになる。がライン上にあることを示している。はカメラの構成によってのみ決まるので、どちらかのピクセル座標を与えれば、どのライン上にもう片方のピクセルが存在するか決めることができる。


OpenCVに実装されているので、実際に計算することはないと思うけど、意味は知っておきたいですね。

Boost.pythonでOpenMP

Boost.pythonを使えばC++でコンパイルしたものをPythonから呼び出せる。
OpenMPでコンパイルしたものでも読み出せるのか？ということなのでちょっとやってみた。
実行するとちゃんと速く動いていた。

以下忘れないようにメモ。

#include <iostream>
#include <omp.h>
#include <time.h>
#include <boost/python.hpp>

using namespace std;

double hoge(int N)
{
    long x = 0;
    double time;
    cout << "function start" << endl;
#ifdef _OPENMP
    double start = omp_get_wtime();
    #pragma omp parallel for
    for(int i = 0; i < N; i++) {
        x += i;
    }   
    time = omp_get_wtime() - start; 
#else
    clock_t start, end;
    start = clock();
    for(int i = 0; i < N; i++) {
        x += i;
    }   
    end = clock();
    time = (double(end-start)/CLOCKS_PER_SEC)
#endif
    cout << "function done" << endl;    
    return time;
}


BOOST_PYTHON_MODULE(basic)
{
    using namespace boost::python;
    def("hoge", &hoge);
}

Makefileを書く

Makefileの書き方を少し覚えた。コンパイルオプションとかもまだ分かってないけど。
メモしておこう。

COMPILER = g++
CFLAG    = -g
OBJGROUP = main.o
LIBDIR   =
LDFLAGS  = -lopencv_core -lopencv_highgui
LIBS     =
INCLUDE  =/usr/local/include

program1: $(OBJGROUP)
    $(COMPILER) -o a.out $(OBJGROUP) -L$(LIBDIR) $(LIBS) $(LDFLAGS)

all: program1

clean:
    \rm *.o

まだファイル1つしかないけど、ここから拡張していこう。

Raspberry Pi camera boardをOpenCVから使う準備

Pi Cameraは少しインストール作業をしないとOpenCVからは直接使えないようだ。残念。USBカメラの場合はすぐに使えるようだが。

2つのライブラリを試したのでちょっとその過程をメモしておこう。

RaspiCamその1

https://github.com/robidouille/robidouille/tree/master/raspicam_cv

に書いてることにしたがって、ソースをコンパイルする。

READMEにものすごく詳しく書いてあって初心者でも簡単にインストール作業ができる。

これを導入するといつもOpenCVで使う関数名を少し変えるだけでPi Cameraが使える。

具体的には以下のような対応になっているようだ。
cvCreateCameraCapture → raspiCamCvCreateCameraCapture
cvQueryFrame → raspiCamCvQueryFrame
CvCapture → RaspiCamCvCapture

http://erget.wordpress.com/2013/12/17/using-opencv-with-the-raspberry-pi-camera/

に書いてあるけど、解像度の変更は今のところできないらしい。 サイトにあるサンプルプログラムを走らせて見たけど、色が何かおかしい… 明らかにraspistillコマンドで吐き出した写真とは色が違う。(緑っぽい) 色の設定とかしたら治るんかな? 今日はここまでで保留。

コンパイルオプションをメモっておく。(makeはこれから勉強します)

g++ test.cpp -I/home/pi/git/robidouille/raspicam_cv -L/home/pi/git/robidouille/raspicam_cv -lraspicamcv -L/home/pi/git/raspberrypi/userland/build/lib -lmmal_core -lmmal -l mmal_util -lvcos -lbcm_host `pkg-config --cflags opencv` `pkg-config --libs opencv`

RaspiCam　その2

http://www.uco.es/investiga/grupos/ava/node/40
おなじ名前のライブラリだ。同じものではないようだ。
こっちはOpenCVを使わなくても使えるもののようだ。
こちらも導入は手順通りでよかった。

ただ、自分の作ったサンプルをコンパイルするところで-lmmalがないとか言われた。
http://www.raspberrypi.org/forums/viewtopic.php?f=33&t=67381
ここに書いてあるけど、-L/opt/vc/libが足りないようだ。
とりあえずコンパイルコマンドをメモ。

g++ simpletest_raspicam_cv.cpp -o simpletest_raspicam_cv -I/usr/local/include/ -lraspicam -lraspicam_cv -lmmal -lmmal_core -lmmal_util -lopencv_core -lopencv_highgui -L/opt/vc/lib

無事に動いて写真をとることができた。
写真が1chのときは30FPSくらいでたけど、3chにすると6FPSくらいだった。



ところで、緑っぽい画像はどうやらどっちでとっても同じのようだ。
試しにもう一度raspisillをやってみたところ、raspistillは最後に画像処理でホワイトバランスを調整しているようだ。(コマンドを実行すると最後に調整が入っているのが分かる)
とりあえず白黒でことはたりるけど、この辺は簡単に補正する方法とかあるんかな?

OpenMPで時間を計測したメモ

OpenMPを使いはじめた。
時間計測プログラムをメモ。
https://stackoverflow.com/questions/10874214/measure-execution-time-in-c-openmp-code

time.hのclock()はCPUの費やした時間をかえすのでこの場合は適していないらしい。
そんなときはomp_get_wtime()を使う。戻り値は秒。

#include <iostream>
#include <omp.h>
#include <time.h>

using namespace std;

int hoge(int x, int y)
{
    return x + y;
}

int main()
{
    double start, end;
    clock_t t_start, t_end;
    long N = 100000000;    
    t_start = clock();
#ifdef _OPENMP
    start = omp_get_wtime();
    #pragma omp parallel for
    for(int i = 0; i < N; i++) {
        hoge(1,1);
    }   
    end = omp_get_wtime();
    cout << end - start << endl;
#else
    for(int i = 0; i < N; i++) {
        hoge(1,1);
    }   
 
#endif
    t_end = clock();
    cout << (double(t_end - t_start)/CLOCKS_PER_SEC) << endl;
    return 0;
}