2016-06-04

環境設定 - CppStyle

Eclipse CDT 用のフォーマッターをインストールしたので，そのメモ．

1. Install Clang-format

　まずは下記のコマンドで Clang-format をインストールします．

$ sudo apt-get install clang-foramt

2. Eclipse のプラグイン CPPStyle を Eclipse からインストール．

　Help -> Install NEW Software -> 下記の URL を新たなリポジトリとして加える．

http://wangzw.github.io/CppStyle/update

f:id:rkoichi2001:20160604175837p:plain

使用条件に同意して Finish.

3. CPPStyle にて Clang-format を選択する．

　Window -> Preferences -> C/C++ -> CppStyle

"Clang-format path" にインストールしたバイナリファイルを指定．
"Run clang-format on file save"にチェックを入れて OK を押して完了．

f:id:rkoichi2001:20160604183815p:plain

4. Formatter として CPPStyle を選択する．

　Window -> Preferences -> C/C++ -> Code Style -> Formatter

　"Code Formatter" というダイアログがあるので，そこで "CppStyle" を選択し，Apply, OK として設定完了．

f:id:rkoichi2001:20160604184031p:plain

2016-06-03

TopView Transformation (鳥瞰図変換) 3 ~ 画素間補間

　前回のエントリで鳥瞰図変換を実現しましたが，作成した鳥瞰図には色情報を保持していない画素が存在しました．（黒のライン）これは，画素変換をする下記の変換式によってうまくマッピングが取れていないことが原因です．

$x'\ =\ \displaystyle \frac{f'}{H_{vc}} \cdot \frac{H_{c}x}{fsin\theta-ycos\theta}$
$y'\ =\ \displaystyle \frac{f'}{H_{vc}} \cdot \{\frac{H_{c}(fcos\theta+ysin\theta)}{fsin\theta-ycos\theta}-D_{vc}\}$

　上記数式では，元画像座標 $\vec{x} = (x, y)^{T}$ を変換後座標 $\vec{x'} = (x', y')^{T}$ に変換する関数 $\vec{x'} = f(\vec{x})$ でした．ここで， $\vec{x}$ に関してループを回して計算していたので，変換後の画像で埋まらない座標がでてきてしまいました．

　今日は，上記の関数 $\vec{x'} = f(\vec{x})$ の逆関数 $\vec{x} = f^{-1}(\vec{x'})$ を求めることで，この問題を解決します．具体的には，変換後の画像に関してループを回し，元画像の色情報を該当ピクセルにコピーします．で，逆関数を求めるために上記の数式を変換後座標 $\vec{x'} = (x', y')^{T}$ に関してではなく，元画像座標 $\vec{x} = (x, y)^{T}$ に関して解きます．

　解いた結果は下記数式です．
$x'\ =\ \displaystyle \frac{H_{vc}}{H_{c}} \cdot \frac{f}{f'} \cdot cos\theta \cdot \left( \frac{sin\theta}{cos\theta} - \frac{y'H_{vc}sin\theta-f'H_{c}cos\theta+f'D_{vc}sin\theta}{f'H_{c}sin\theta + H_{vc}y'cos\theta+f'D_{vc}cos\theta} \right)$
$y\ =\ \displaystyle \frac{f(y'H_{vc}sin\theta-f'H_{c}cos\theta+f'D_{vc}sin\theta)}{f'H_{c}sin\theta+H_{vc}y'cos\theta+f'D_{vc}cos\theta}$

変換後写真

　前回のエントリで黒ピクセルだった箇所に色が付いていることがわかります．ただ，補間を全くしてないので，画像が荒くなってしまっています．
f:id:rkoichi2001:20160602072441p:plain

双一次補間法

　上記の変換写真は，最近傍法（もっとも近いピクセルの画素値を使う）を用いて計算しました．結果，カメラから離れた場所の画像が割と荒くなってしまいました．次に，双一次補間法という方法を用いて補間したいと思います．補間のイメージは下記のとおりです．

f:id:rkoichi2001:20160602080045p:plain

　ここでは，変換後の位置座標に近い 4 近傍の画素値を用いて按分します．使用する計算式は下記のとおりです．

$I(x, y) = (dy_{2}\ dy_{1})\begin{pmatrix} f_{11} &f_{12} \\ f_{21} &f_{22} \end{pmatrix}\begin{pmatrix} dx_{2} \\ dx_{1}\end{pmatrix}$

変換後の結果が以下です．画素のギザギザが少しなめらかになってます．
f:id:rkoichi2001:20160603063615p:plain

　下記，コード片です．ベタ書きしてしまったのでとっても汚いですがご容赦を！

/*
 * undistort.cpp
 *
 *  Created on: May 29, 2016
 *    Author: koichi
 */
#include <iostream>
#include <math.h>
#include "opencv2/opencv.hpp"
//#include "opencv2/calib3d.hpp"

//#define INTERPOLATED
#define BILINEAR

using namespace std;
using namespace cv;

int main(int argc, char** argv) {

  // 1. Load Raw Iamge
  string imagepath = "/home/koichi/my_photo-1.jpg";
  Mat rawImage = imread(imagepath, CV_LOAD_IMAGE_COLOR);
  namedWindow("Raw Image", WINDOW_AUTOSIZE);
  imshow("Raw Image", rawImage);

  // 2. Undistort Image Based on Calibration Matrix.
  Mat undistortImage(rawImage.rows, rawImage.cols, CV_8UC3);
  Matx33d K(  627.235434, 0,    654.957574,
    0,    630.482585,  494.346943,
    0,    0,    1    );
  cv::Vec4d D(-0.210146, 0.030563, 0.001172, -0.001306);
  cv::undistort(rawImage, undistortImage, K, D);
  namedWindow("Undistorted Image", WINDOW_AUTOSIZE);
  //line(undistortImage, Point(640, 0), Point(640, 960), Scalar(255, 255, 255), 10);
  //line(undistortImage, Point(0, 460), Point(1280, 460), Scalar(255, 255, 255), 10);
  imshow("Undistorted Image", undistortImage);

  // 3. Convert Image to Gray
  Mat grayImage(rawImage.rows, rawImage.cols, CV_8UC1);
  cvtColor(undistortImage, grayImage, COLOR_RGB2GRAY);
  namedWindow("Gray Image", WINDOW_AUTOSIZE);
  imshow("Gray Image", grayImage);

  // 4. Top View Conversion
  Mat topImage(rawImage.rows, rawImage.cols, CV_8UC3);
  topImage.setTo(Scalar(0));

  Mat topImageGray(rawImage.rows, rawImage.cols, CV_8UC1);
  topImageGray.setTo(Scalar(0));
  {
  double Hvc = 2;
  double Hc = 0.7;
  double Dvc = 1.7;
  double f = 630;
  double fp = f;
  //double theta = 15 / 180.0 * M_PI;
  double theta = 27 / 180.0 * M_PI;
  double s = sin(theta);
  double c = cos(theta);
  int cx = 640;
  int cy = 480;
  int cxp = 640;
  int cyp = 480;

  for (int y = 0; y < topImage.rows; y++) {
    for (int x = 0; x < topImage.cols; x++) {

#ifdef INTERPOLATED


    int xOrg = x - cx;
    int yOrg = - y + cy;

    int oldX = 0.5 + (Hvc / Hc) * (f / fp) * c * ( s/c - (yOrg*Hvc*s - fp*Hc*c + fp*Dvc*s) / (fp*Hc*s + Hvc*yOrg*c + fp*Dvc*c) ) * xOrg;
    int oldY = 0.5 + f * ((yOrg*Hvc*s - fp*Hc*c + fp*Dvc*s)/(fp*Hc*s + Hvc*yOrg*c + fp*Dvc*c));

    oldX = oldX + cxp;
    oldY = -oldY + cyp;


    if (oldX < 0 || topImage.cols - 1 < oldX || oldY < 0 || topImage.rows - 1 < oldY ) {
      continue;
    }

    topImageGray.data[y * topImageGray.cols + x] = grayImage.data[oldY * grayImage.cols + oldX];

    topImage.data[(y * topImage.cols + x) * topImage.channels()] = undistortImage.data[(oldY * topImage.cols + oldX) * topImage.channels()];
    topImage.data[(y * topImage.cols + x) * topImage.channels() + 1] = undistortImage.data[(oldY * topImage.cols + oldX) * topImage.channels() + 1];
    topImage.data[(y * topImage.cols + x) * topImage.channels() + 2] = undistortImage.data[(oldY * topImage.cols + oldX) * topImage.channels() + 2];

#else
#ifdef BILINEAR

    int xOrg = x - cx;
    int yOrg = - y + cy;

    double oldX = 0.5 + (Hvc / Hc) * (f / fp) * c * ( s/c - (yOrg*Hvc*s - fp*Hc*c + fp*Dvc*s) / (fp*Hc*s + Hvc*yOrg*c + fp*Dvc*c) ) * xOrg;
    double oldY = 0.5 + f * ((yOrg*Hvc*s - fp*Hc*c + fp*Dvc*s)/(fp*Hc*s + Hvc*yOrg*c + fp*Dvc*c));

    oldX = oldX + cxp;
    oldY = -oldY + cyp;

    if (oldX < 0 || topImage.cols - 1 < oldX || oldY < 0 || topImage.rows - 1 < oldY ) {
      continue;
    }

    if((int)oldX + 1 >= topImage.cols || (int)oldY + 1 >= topImage.rows) {
      topImage.data[(y * topImage.cols + x) * topImage.channels()] = undistortImage.data[((int)oldY * topImage.cols + (int)oldX) * topImage.channels()];
      topImage.data[(y * topImage.cols + x) * topImage.channels() + 1] = undistortImage.data[((int)oldY * topImage.cols + (int)oldX) * topImage.channels() + 1];
      topImage.data[(y * topImage.cols + x) * topImage.channels() + 2] = undistortImage.data[((int)oldY * topImage.cols + (int)oldX) * topImage.channels() + 2];
      continue;
    }


    for (int i = 0; i < topImage.channels(); i++) {

      uchar f11 = undistortImage.data[((int)oldY * topImage.cols + (int)oldX) * topImage.channels() + i];
      uchar f12 = undistortImage.data[(((int)oldY + 1) * topImage.cols + (int)oldX) * topImage.channels() + i];
      uchar f21 = undistortImage.data[((int)oldY * topImage.cols + (int)oldX + 1) * topImage.channels() + i];
      uchar f22 = undistortImage.data[(((int)oldY + 1) * topImage.cols + (int)oldX + 1) * topImage.channels() + i];

      double dx2 = (int)oldX + 1 - oldX;
      double dx1 = oldX - (int)oldX;

      double dy2 = (int)oldY + 1 - oldY;
      double dy1 = oldY - (int)oldY;

      topImage.data[(y * topImage.cols + x) * topImage.channels() + i] = dy2 * (f11 * dx2 + f21 * dx1) + dy1 * (f12 * dx2 + f22 * dx1);
    }
#else

    int xOrg = x - cx;
    int yOrg = - y + cy;

    int newX = fp / Hvc * Hc * xOrg / (f * s - yOrg * c);
    int newY = fp / Hvc * (Hc * (f * c + yOrg * s) / (f * s - yOrg * c) - Dvc);

    newX = newX + cxp;
    newY = -newY + cyp;

    if (newX < 0 || topImage.cols - 1 < newX || newY < 0 || topImage.rows - 1 < newY ) {
      continue;
    }

    topImageGray.data[newY * topImageGray.cols + newX] = grayImage.data[y * grayImage.cols + x];

    topImage.data[(newY * topImage.cols + newX) * topImage.channels()] = undistortImage.data[(y * topImage.cols + x) * topImage.channels()];
    topImage.data[(newY * topImage.cols + newX) * topImage.channels() + 1] = undistortImage.data[(y * topImage.cols + x) * topImage.channels() + 1];
    topImage.data[(newY * topImage.cols + newX) * topImage.channels() + 2] = undistortImage.data[(y * topImage.cols + x) * topImage.channels() + 2];


#endif
#endif


    }
  }
  }

  namedWindow("Top Image", WINDOW_AUTOSIZE);
  imshow("Top Image", topImage);
  //namedWindow("Top Image Gray", WINDOW_AUTOSIZE);
  //imshow("Top Image Gray", topImageGray);

  while(true) {
  waitKey(0);
  }
  return 0;
}

2016-05-31

TODO リスト to つくばチャレンジ 2016

つくばチャレンジ

　なんだかやることが多すぎでわけわからなくなりそうだったので，つくばチャレンジ 2016 本走行までの TODO を洗い出してみました．事前知識がたらなさすぎで，洗い出し＆時間の見積もりとかが全くできないので，思いついたベースで更新してきます．

つくばチャレンジ 2016 への TODO

自己位置推定機能＆センシング機能

鳥瞰図作成

1.文献調査

daily-tech.hatenablog.com

2.鳥瞰図変換ロジック作成

daily-tech.hatenablog.com

3.鳥瞰図変換補間ロジック作成

daily-tech.hatenablog.com

4.鳥瞰図同士の比較のための特徴点調査
5.鳥瞰図同士の比較ソフト作成
6.実験！

ステレオカメラ作成

1.OpenCV ですでに使用可能なステレオカメラ一覧を調べる
- Graph Cut Stereo
- Semi Global Matching Stereo
- Block Matching Stereo

daily-tech.hatenablog.com

2.Graph Cut Stereo を使用するために，OpenCV2.2 のコンパイルを通す．

daily-tech.hatenablog.com

3.ステレオカメラの Intrinsic キャリブレーション

4.ステレオカメラの Extrinsic キャリブレーション

5.ステレオ結果から3次元座標の生成

6.3次元座標からのトップビューの生成，z座標の値を見て走行可能エリアかどうか判定するマップの作成．

7.2次元の走行可能マップを使って，自己位置推定ができないかどうか検討．
- 二値化画像のテンプレートマッチング　（大体の位置精度が出せればOK．後は走行可能エリアの真ん中とか，わかりやすい指標を元に走れるとこをはしる．）
- 鳥瞰図変換結果を色でクラスタリングして走行可能エリア作成
- 鳥瞰図変換結果をエッジ検出する

自己位置推定実験 & ステレオカメラ実験

実験 with ロボット
実験 without ロボット
複数の連続する地点で２回カメラ撮影を実施する．
連続する地点で撮影した画像が自己位置推定に使えるか確認する．
新しいレンズの性能を見てみる
新しいレンズを使ってステレオカメラ作成

自己位置推定マップ

1.文献調査
2.ソフト作成

車体周辺の環境マップ作成

1.文献調査
2.ソフト作成

開発環境構築

1.Ubuntu のバージョン決定

daily-tech.hatenablog.com

2.環境設定
- ROS Kinetic Kame, PyCharm, Eclipse, git

daily-tech.hatenablog.com

- OpenCV 2.4 & OpenCV 3.1

daily-tech.hatenablog.com

デバイス周り

1.カメラ
- USB カメラセッティング

daily-tech.hatenablog.com
daily-tech.hatenablog.com

- カメラキャリブレーション

daily-tech.hatenablog.com

- V4L2 と OpenCV

daily-tech.hatenablog.com
daily-tech.hatenablog.com

プランナー機能作成

モーションコントロール機能

オドメトリ機能実装

1.文献調査
2.実オドメトリ機能作成
3.Visual Odometry 機能作成
4.実オドメトリと Visual Odometry の性能比較

プランナー作成

1.文献調査
2.センシング結果とロボット座標系の結びつけ
3.経路生成モジュールの作成

マイルストーン

つくばチャレンジ試走会 #1 (7/9 土)

1.マニュアル走行でのデータ取得
2.確認走行区間の自律走行

つくばチャレンジ試走会 #2 (9/22 土)

1.1km 区間の自律走行
2.確認走行区間の自律走行

つくばチャレンジ試走会 #3 (10/15 土)

つくばチャレンジ試走会 #4 (10/16 土)

1.全区間走行

つくばチャレンジ試走会 #5 (10/29 土)

1.全区間走行と探索対象の発見

つくばチャレンジ試走会 #6 (11/4 土)

つくばチャレンジ試走会 #7 (11/5 土)

つくばチャレンジ本走行 (11/6 日)

SI2016＠札幌 (12/15 - 12/17 水)

つくばチャレンジシンポジウム＠筑波大学 (11/5 木)

2016-05-29

TopView Transformation (鳥瞰図変換) 2

C++ Computer Vision つくばチャレンジ画像処理論文整理

TopView Transformation 1 のエントリで論文まとめを書いた鳥瞰図変換ですが，ここでは実際に鳥瞰図変換にトライしていきます．

実験設定

　自宅実験室（笑）にて，以前紹介した USB カメラを使って写真を撮ります．鳥瞰図変換用にカメラ三脚と角度計を購入．

高精度デジタル2軸角度計 DXL360 XY軸

出版社/メーカー: UMK
メディア:
この商品を含むブログを見る

【Amazon.co.jp限定】 Fotopro 三脚 DIGI-204 GM 4段小型 3WAY雲台アルミ製ガンメタリック DIGI-204GM

出版社/メーカー: Fotopro
発売日: 2014/04/26
メディア: Camera
この商品を含むブログを見る

実験の様子（笑）

f:id:rkoichi2001:20160529025845j:plain

結果

　結果ですが，カメラのキャリブレーションが甘いのか，カメラ設置があまいのか，直線が少し曲がってしまっているのですが，それでも鳥瞰図は出来上がりました．画像の端に行くに連れて対応するピクセルが疎になっている様子がわかります．線形補間等の処理を加えてあげる必要がありそうですが，それはまた今度．．．．

変換前写真（歪み補正前）

f:id:rkoichi2001:20160529133210p:plain

変換前写真（歪み補正後）

f:id:rkoichi2001:20160529133136p:plain

変換後写真

f:id:rkoichi2001:20160529134517p:plain

実装

　下記，実験に使ったコードです．

#include <iostream>
#include <math.h>
#include "opencv2/opencv.hpp"
//#include "opencv2/calib3d.hpp"

using namespace std;
using namespace cv;

int main(int argc, char** argv) {

	// 1. Load Raw Iamge
	string imagepath = "/home/koichi/my_photo-1.jpg";
	Mat rawImage = imread(imagepath, CV_LOAD_IMAGE_COLOR);
	namedWindow("Raw Image", WINDOW_AUTOSIZE);
	imshow("Raw Image", rawImage);

	// 2. Undistort Image Based on Calibration Matrix.
	Mat undistortImage(rawImage.rows, rawImage.cols, CV_8UC3);
	Matx33d K(	627.235434, 0,			654.957574,
				0,			630.482585,	494.346943,
				0,			0,			1			);
	cv::Vec4d D(-0.210146, 0.030563, 0.001172, -0.001306);
	cv::undistort(rawImage, undistortImage, K, D);
	namedWindow("Undistorted Image", WINDOW_AUTOSIZE);
	//line(undistortImage, Point(640, 0), Point(640, 960), Scalar(255, 255, 255), 10);
	//line(undistortImage, Point(0, 460), Point(1280, 460), Scalar(255, 255, 255), 10);
	imshow("Undistorted Image", undistortImage);

	// 3. Convert Image to Gray
	Mat grayImage(rawImage.rows, rawImage.cols, CV_8UC1);
	cvtColor(undistortImage, grayImage, COLOR_RGB2GRAY);
	namedWindow("Gray Image", WINDOW_AUTOSIZE);
	imshow("Gray Image", grayImage);

	// 4. Top View Conversion
	Mat topImage(rawImage.rows, rawImage.cols, CV_8UC3);
	topImage.setTo(Scalar(0));

	Mat topImageGray(rawImage.rows, rawImage.cols, CV_8UC1);
	topImageGray.setTo(Scalar(0));
	{
		double Hvc = 2;
		double Hc = 0.7;
		double Dvc = 1.7;
		double f = 630;
		double fp = f;
		double theta = 30.0 / 180.0 * M_PI;
		double s = sin(theta);
		double c = cos(theta);
		int cx = 640;
		int cy = 480;
		int cxp = 640;
		int cyp = 480;

		for (int y = 0; y < topImage.rows; y++) {
			for (int x = 0; x < topImage.cols; x++) {

				int xOrg = x - cx;
				int yOrg = - y + cy;

				int newX = fp / Hvc * Hc * xOrg / (f * s - yOrg * c);
				int newY = fp / Hvc * (Hc * (f * c + yOrg * s) / (f * s - yOrg * c) - Dvc);

				newX = newX + cxp;
				newY = -newY + cyp;

				if (newX < 0 || topImage.cols - 1 < newX || newY < 0 || topImage.rows - 1 < newY ) {
					continue;
				}

				topImageGray.data[newY * topImageGray.cols + newX] = grayImage.data[y * grayImage.cols + x];

				topImage.data[(newY * topImage.cols + newX) * topImage.channels()] = undistortImage.data[(y * topImage.cols + x) * topImage.channels()];
				topImage.data[(newY * topImage.cols + newX) * topImage.channels() + 1] = undistortImage.data[(y * topImage.cols + x) * topImage.channels() + 1];
				topImage.data[(newY * topImage.cols + newX) * topImage.channels() + 2] = undistortImage.data[(y * topImage.cols + x) * topImage.channels() + 2];

			}
		}
	}

	namedWindow("Top Image", WINDOW_AUTOSIZE);
	imshow("Top Image", topImage);
	namedWindow("Top Image Gray", WINDOW_AUTOSIZE);
	imshow("Top Image Gray", topImageGray);

	while(true) {
		waitKey(0);
	}
	return 0;
}

2016-05-29

TopView Transformation (鳥瞰図変換) 1

つくばチャレンジ Computer Vision 画像処理論文整理 C++

　いよいよつくばチャレンジに向けての具体的な”ロボット工作”的投稿です．突然ですが，最近の車についてるアラウンドビューモニタってご存じですか？駐車しようとしたときに，ナビに表示される車全体を上から見たような絵のことです．

アラウンドビューモニター | 日産｜技術開発の取り組み

　自分は画像処理の経験が浅いからかもしれませんが，これって本当にすごいと思います．実際にはカメラは車両のバンパやサイドミラーに”斜め下向き”についているので，カメラから得られる画像そのものはもちろんトップビューではないんですが，カメラと車の正確な位置情報をつかってえられた画像を変換してやることで，まるで地面を上から見ているような映像に変えることができるわけです．

　で，これがつくばチャレンジのロボットとなんの関係があるの？という話なんですが，ロボットの自己位置推定にトップビュー変換した画像を使えないかと考えているわけです．流れとしては，，，，

①手動で，つくばチャレンジ２０１６のコース上をロボット走行させる．
②走行中，適切な間隔で写真を撮っていく．
③走行後，撮影した写真を Top View 変換する．

①～③のステップで，理想的な走行経路から撮影した Top View 画像が取得できます．自動で走らせるときには，この事前に撮りためた Top View 画像と今まさに取得している写真の Top View 画像の比較をして，自動走行時のロボットがどこにいるかを判断します．
（上手くいくかどうかわかりませんが．．．）

　ということで，まず Top View 変換のプログラムを作ることにしました．OpenCV の WarpAffine という関数を呼べば割と簡単に作れそうだったのですが，とりあえず肝になるところだと思ったので勉強がてら論文読み＆サンプルコードを実装してみます．

文献

　参考にしたのは，下記の文献です．

Vehicle Detection Based on Perspective Transformation Using Rear-View Camera
A Vision Based Top-View Transformation Model for a Vehicle Parking Assistant
鳥瞰表示による駐車支援システム

技術概要

　Vehicle Detection Based on Perspective Transformation Using Rear-View Camera で用いられていた下記の図がわかりやすかったので，この図をベースに考えていきます．

f:id:rkoichi2001:20160526035336p:plain

座標変換

　視点変換をするにあたり，まずは座標変換を考えないといけません．いきなり撮像面間 (xy平面 - x'y'平面) の変換を考えるとこんがらがるので，カメラセンター座標間 (Oc座標 - Ovc座標) の変換を考えるところから始めます．元々のカメラ位置は Oc にあるわけですが，ここから見える風景を，あたかも Ovc から見たように変換する必要があります．つまり，「Oc から見た時の風景の各点の 3次元座標を，Ovcから見た時の 3次元座標に変換する」必要があるわけです．

$\vec{X_{O_{w}}} = (X_{O_{w}}, Y_{O_{w}}, Z_{O_{w}})^T$

　上記は空間内の一点で，それを世界座標系 $O_{w}$ から見た座標とします．カメラ座標系 $O_{c}$ ，仮想カメラ座標系 $O_{vc}$ から見たそれぞれの座標を下記のように定義します．

$\vec{X_{O_{c}}} = (X_{O_{c}}, Y_{O_{c}}, Z_{O_{c}})^T$
$\vec{X_{O_{vc}}} = (X_{O_{vc}}, Y_{O_{vc}}, Z_{O_{vc}})^T$

　また，仮想カメラ座標系から見たカメラ座標系の位置関係は下記のようになります．

回転関係 : ${}^{O_{vc}}R_{O_{c}}=\begin{bmatrix}1 &0 &0 \\ 0 &cos(90-\theta) &sin(90-\theta) \\ 0 &-sin(90-\theta) &cos(90-\theta) \end{bmatrix}$
並進関係 : $\vec{{}^{O_{vc}}Q_{O_{c}}} = (0, -D_{vc}, H_{vc} - H_{c})^T$

　上記の回転関係と並進関係を用いると，座標間の関係性は下記の変換行列で結ばれます．

$(X_{O_{vc}}, Y_{O_{vc}}, Z_{O_{vc}})^T\ =\ {}^{O_{vc}}T_{O_{c}}*(X_{O_{c}}, Y_{O_{c}}, Z_{O_{c}})^T$
${}^{O_{vc}}T_{O_{c}}\ = \ \begin{bmatrix} & & & \\ &{}^{O_{vc}}R_{O_{c}} & &{}^{O_{vc}}Q_{O_{c}} \\ & & & \\ 0 &0 &0 &1 \end{bmatrix}$

　上記をそのまま計算すると，下記の 3 つの関係式が導かれます．

1. $X_{O_{vc}}\ =\ X_{O_{c}}$
2. $Y_{O_{vc}}\ =\ Y_{O_{c}}*sin\theta\ +\ Z_{O_{c}}*cos\theta\ -\ D_{vc}$
$Z_{O_{vc}}\ =\ -Y_{O_{c}}*cos\theta\ +\ Z_{O_{c}}*sin\theta\ +\ H_{vc}\ -\ H_{c}$

※変換後のZ座標は常に $H_{vc}$ なので，上記の最後の式は最終的には下記のようになります．
3. $H_{vc}\ =\ -Y_{O_{c}}*cos\theta\ +\ Z_{O_{c}}*sin\theta\ +\ H_{vc}\ -\ H_{c}$

　また，三次元座標と投影座標の関係より，下記の 4 式が導かれます．

Camera ピクセル座標系
4. $x\ =\ f \displaystyle \frac{X_{O_{c}}}{Z_{O_{c}}}$
5. $y\ =\ f \displaystyle \frac{Y_{O_{c}}}{Z_{O_{c}}}$

Virtual Camera ピクセル座標系
6. $x'\ =\ f' \displaystyle \frac{X_{O_{vc}}}{Z_{O_{vc}}}$
7. $y'\ =\ f' \displaystyle \frac{Y_{O_{vc}}}{Z_{O_{vc}}}$

上記の７つの式から， $X_{O_{c}}\ Y_{O_{c}}\ Z_{O_{c}}\ X_{O_{vc}}\ Y_{O_{vc}}\ Z_{O_{vc}}$ を消去すると，論文に乗っている下記の式を得ることができます．

ふー．．．やっとまとまりました．最後の２式の変換を実際に実装していきます．
TopView Transformation 2 を好ご期待！！

2016-05-28

Windows Form とスレッド

C# のライブラリっていろいろと便利なものがそろってると思うんですが，どうも資料が不足してるような気がする．．．前回のエントリでも Bitmap を扱ったけど，いま Image とか Bitmap とかを扱うものを作ってて，一枚の画像を複数のスレッドからいじる必要があったんだけど，「別のスレッドがつかんでる」とか「Invalid Operation」とかの例外がたくさん上がってくるのでちょっと調べたのでメモ．

事前にちょっとググった結果，下記のようなコメント発見．

Windows フォームと複数スレッド
1. Windows フォームでスレッドを作成した場合，フォームやフォーム上のコントロールに対しては，そのスレッドからの操作は動作が保証されない．<- つまり，基本的には UI スレッドからしかいじるなということらしい．

ということで実験．下記，メインのコードです．
下記のコードではまず写真を GUI に表示して， 1 秒毎に写真を反転させます．

public partial class Form1 : Form
{
  private System.Timers.Timer myTimer;

  private void SetupTimer()
  {
    myTimer = new System.Timers.Timer();
    myTimer.Enabled = true;
    myTimer.AutoReset = true;
    myTimer.Interval = 1000;
    myTimer.Elapsed += new ElapsedEventHandler(OnTimerEvent);
  }

  delegate void FlipDelegate();

  internal void FlipImageByThread()
  {
    Image img = pictureBox1.Image;
    img.RotateFlip(RotateFlipType.Rotate180FlipX);
    pictureBox1.Refresh();
  }

  private void OnTimerEvent(object source, ElapsedEventArgs e)
  {
    FlipImageByThread();
    //Console.WriteLine("OnTimerEvent");
    //Invoke(new FlipDelegate(FlipImageByThread));        
  }

  private void DisplayBitmap()
  {
    Bitmap canvas = new Bitmap(pictureBox1.Width, pictureBox1.Height);
    Graphics g = Graphics.FromImage(canvas);
    g.InterpolationMode = System.Drawing.Drawing2D.InterpolationMode.NearestNeighbor;
    Bitmap pic = new Bitmap(@"C:\\Users\\Koichi\\Documents\\Visual Studio 2013\\Projects\\ConsoleApplication1\\OpenCVSharpSample\\img\\boeing777.jpg");
    g.DrawImage(pic, 0, 0, 500, 500);
    pic.Dispose();
    g.Dispose();
    pictureBox1.Image = canvas;           
  }

  public Form1()
  {
    InitializeComponent();
    DisplayBitmap();
    SetupTimer();            
  }
}

結果的に，上記のコードは例外がはかれてしまってうまく動きません．理由は最初に述べたとおり， UI スレッド以外から UI をいじろうとしたからみたいです．

f:id:rkoichi2001:20160528233945p:plain

で，それじゃ別のスレッドから UI をいじりたいとき（例えば，ユーザのマウスイベントを拾ってとか，上記の例みたいにタイマイベントをひろってとか．．．）にどうすればええんか？って話が出てきますが，Invoke というメソッドを使えば UI スレッドに処理を移譲できるみたい．

該当するコードは下記の部分です．

delegate void FlipDelegate();

internal void FlipImageByThread()
{
  Image img = pictureBox1.Image;
  img.RotateFlip(RotateFlipType.Rotate180FlipX);
  pictureBox1.Refresh();
}

private void OnTimerEvent(object source, ElapsedEventArgs e)
{
  // このラインをコメントアウト．
  // FlipImageByThread();

  // 下記の 2 行を有効化．
  Console.WriteLine("OnTimerEvent");
  Invoke(new FlipDelegate(FlipImageByThread));        
}

結果は，うまく写真が反転して出力されました．
まとめると，Windows フォームは UI スレッド以外からコントロールしてはいけない．コントロールが必要な場合は Invoke メソッドを用いて処理を UI スレッドに移譲する．

f:id:rkoichi2001:20160528234850p:plain

f:id:rkoichi2001:20160528234757p:plain

2016-05-26

OpenCVSharp BitMap <-> IplImage 間の変換

　いま携わっているプロジェクトで，BitMap形式の画像を IplImage に変換して OpenCVSharp を使って画像処理をやる必要があったんだけど，うまい変換方法がわからなかったので調べてみました．C# 自体そんなに強くない状態から始めたので，いろいろと勉強になってます．

　まずは，BitMap を表示するところから．

1. Windows Forms Project を生成する．

　「ファイル」->「新規作成」->「新規作成」->「プロジェクト」->「Windows フォームアプリケーション」

f:id:rkoichi2001:20160525234600p:plain

2. OpenCVSharp をプロジェクトに導入し，C# のフォーム上に写真を表示します．

　「ツール」->「NuGet パッケージマネージャ」->「ソリューションの NuGet パッケージの管理」

f:id:rkoichi2001:20160525234844p:plain

3. Form に Picture Box を追加します．

3.1 GUI コンポーネントの編集

　選択している「PictureBox」のドラッグ＆ドロップで完了．

f:id:rkoichi2001:20160525234935p:plain

3.2 コード編集

　ソリューションエクスプローラに作成したフォームアイテムが表示されるので，このアイテムをダブルクリックするとテキストベースのコードが現れます．このケースの場合「Form1.cs」が該当．

f:id:rkoichi2001:20160526000159p:plain

　このファイルにコードを記述して，外部から取り込んだ写真を表示．追加したコードは下記のとおり．もともと持っていた写真のサイズが少し大きかったので，それを縮小して表示する部分も加えました．詳細は下記のとおり．

1. Picture Box に追加する Bitmap オブジェクトを生成する．
　Form に追加した Picture Box と同じサイズではまる Bitmap を生成します．実際に Picture Box に表示されるのはこの canvas オブジェクトになります．

Bitmap canvas = new Bitmap(pictureBox1.Width, pictureBox1.Height);

2. canvas オブジェクト上の Image を操作するために，Graphics オブジェクトを取得します．

Graphics g = Graphics.FromImage(canvas);

3. canvas 上での Image 操作の際の補完方法を指定します．

g.InterpolationMode = System.Drawing.Drawing2D.InterpolationMode.NearestNeighbor;

4. 写真から Bitmap オブジェクトを生成します．
　実際に加工されるイメージの元となる Bitmap を生成します．こいつが canvas 上にコピーされて，canvas 上にコピーされたものに対していろいろと操作を加えるという流れになります．

Bitmap pic = new Bitmap(@"C:\\Users\\Koichi\\Documents\\Visual Studio 2013\\Projects\\ConsoleApplication1\\OpenCVSharpSample\\img\\boeing777.jpg");

5. オリジナルの Bitmap を canvas 上にコピーすると同時に拡大・縮小も実施します（されます）．

g.DrawImage(pic, 0, 0, 500, 500);

6. 元のイメージ，および Graphics オブジェクトは必要なくなったので，消去！

pic.Dispose();
g.Dispose();

7. 拡大・縮小したイメージを Picture Box に紐づける．

pictureBox1.Image = canvas;

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;


namespace WindowsFormsApplicationExample
{
    public partial class Form1 : Form
    {

        private void DisplayBitmap()
        {
            // 1. Image Object Creation.
            Bitmap canvas = new Bitmap(pictureBox1.Width, pictureBox1.Height);
            // 2. Generate Graphics Object from Image.
            Graphics g = Graphics.FromImage(canvas);
            // 3. Specify Interpolation Mode.
            g.InterpolationMode = System.Drawing.Drawing2D.InterpolationMode.NearestNeighbor;
            // 4. Generate Bitmap Object from picture.
            Bitmap pic = new Bitmap(@"C:\\Users\\Koichi\\Documents\\Visual Studio 2013\\Projects\\ConsoleApplication1\\OpenCVSharpSample\\img\\boeing777.jpg");
            // 5. Reduce Size of Picture and paint it to canvas.
            g.DrawImage(pic, 0, 0, 500, 500);
            // 6. Dispose unnecessary "pic" and graphics.
            pic.Dispose();
            g.Dispose();
            // 7. Display image.         
            pictureBox1.Image = canvas;
            
        }

        public Form1()
        {
            InitializeComponent();
            DisplayBitmap();
        }
    }
}

実行結果は下記のとおり．
f:id:rkoichi2001:20160526003811p:plain

3. System.Drawing.Bitmap -> IplImage 間の変換を実施

　ここからがやっと本題です．．．2 のステップで，Bitmap をフォーム上に表示するところまでは行きました．今度は，こいつを OpenCVSharp の画像形式に変換して OpenCVSharp の Window でも表示します．

1. 表示したイメージを取得する．

Image img = pictureBox1.Image;

2. 表示したイメージと同じサイズ・フォーマットの IplImage を用意する．

IplImage convertedImg = new IplImage(new CvSize(img.Width, img.Height), BitDepth.U8, 3);

3. Bitmap フォーマットから IplImage にコピーする．

convertedImg.CopyFrom(img as Bitmap);

4. OpenCV 流に表示する．

Cv.ShowImage("CvSharpImage", convertedImg);

　なんか，3 のステップだけを書いたらええやんけ！っていう気もしますが，自分の練習・備忘も含まれてるのでご容赦を！ちなみに，ここで紹介しているのは Bitmap -> IplImage の変換ですが，下記のようにすれば逆の変換もできます．

System.Drawing.Image img = iplImage.ToBitmap();

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;

using OpenCvSharp;
using OpenCvSharp.Extensions;

namespace WindowsFormsApplicationExample
{
    public partial class Form1 : Form
    {

        private void DisplayBitmap()
        {
            // 1. Image Object Creation.
            Bitmap canvas = new Bitmap(pictureBox1.Width, pictureBox1.Height);
            // 2. Generate Graphics Object from Image.
            Graphics g = Graphics.FromImage(canvas);
            // 3. Specify Interpolation Mode.
            g.InterpolationMode = System.Drawing.Drawing2D.InterpolationMode.NearestNeighbor;
            // 4. Generate Bitmap Object from picture.
            Bitmap pic = new Bitmap(@"C:\\Users\\Koichi\\Documents\\Visual Studio 2013\\Projects\\ConsoleApplication1\\OpenCVSharpSample\\img\\boeing777.jpg");
            // 5. Reduce Size of Picture and paint it to canvas.
            g.DrawImage(pic, 0, 0, 500, 500);
            // 6. Dispose unnecessary "pic" and graphics.
            pic.Dispose();
            g.Dispose();
            // 7. Display image.                        
            pictureBox1.Image = canvas;
            
        }

        private void DisplayPictureByOpenCVSharp()
        {
　　　　　　// 1. Get Displayed Image.
            Image img = pictureBox1.Image;

　　　　　　// 2. Instantiate OpenCV Image Class.
            IplImage convertedImg = new IplImage(new CvSize(img.Width, img.Height), BitDepth.U8, 3);

　　　　　　// 3. Copy data from Bitmap.            
            convertedImg.CopyFrom(img as Bitmap);

　　　　　　// 4. Display Image.
            Cv.ShowImage("CvSharpImage", convertedImg);

        }

        public Form1()
        {
            InitializeComponent();
            DisplayBitmap();
            DisplayPictureByOpenCVSharp();
        }
    }
}

実行結果は下記のとおり．
（若干端が黒くなっていてサイズが合ってないですが，，，とりあえず細かいことは置いといてということで．．．）

f:id:rkoichi2001:20160526010304p:plain