読者です 読者をやめる 読者になる 読者になる

パソコン環境設定 2017

今年もつくばチャレンジ2017に向けて、まずは環境のセットアップということで早速準備。
備忘録として、インストール一覧をメモメモ。

Windows
Chrome
Visual Studio 2015, Community
ITunes
Office
Anaconda
さくらエディタ

Linux
Chrome

  • sudo apt-get update
  • sudo apt-get install libgconf2-4 libnss3-1d libxss1

で、インストールした Chrome を実行。

ibus-mozc

  • sudo apt-get install ibus-mozc

で、再起動後に Text Entry Setting であらたに追加された Mozc を選択。

日本語キーボード対応

  • /usr/share/ibus/component/mozc.xml

のlayoutの部分を変更する。
default -> jp


ROS Kinetic
下記のチュートリアル通りに。
kinetic/Installation/Ubuntu - ROS Wiki

JDK
Eclipse を動かすための JAVA 環境のインストール。

  • sudo apt-get install default-jre

Eclipse
下記から Eclipse IDE for C/C++ Developers のインストール。Neonってのが一番新しいらしい。
http://www.eclipse.org/downloads/eclipse-packages/
Eclipse を /opt/ 以下に移動して、実行ファイルのパスを通す。
PATH="$PATH:/opt/eclipse/"

turtle-bot のシミュレーション関係一式
turtlebot/Tutorials/indigo/Turtlebot Installation - ROS Wiki

sudo apt-get install ros-kinetic-turtlebot ros-kinetic-turtlebot-apps ros-kinetic-turtlebot-interactions ros-kinetic-turtlebot-simulator ros-kinetic-kobuki-ftdi ros-kinetic-ar-track-alvar-msgs ros-kinetic-turtlebot-gazebo

Python リストのソートの仕方

機械学習の勉強にPythonを使ってるんですが,ちょっとしたデータ処理したりするの,めっちゃ使いやすいですね.
結構大きいデータ使ってるんですが,そこそこの速度で動いてくれるし.

ちょっと,Pythonのリストをソートする方法を調べてたんですが,備忘録としてメモしておきます.

例えば,名簿のリストがあったとします.
デフォルトでは,アルファベット順で並んでますが,同時に各人の身長のデータも持っていて,身長順にリストをソートしたいとします.
この時には,sorted関数を使えます.

['crues', 'hanks', 'mike', 'samp', 'tifanny', 'tom']

下記,名前が与えられたらその人の身長を返すメソッドだとします.

def orderFunc(k):
    
    if (k == "sam"):
        return 175
    elif (k == "mike"):
        return 180
    elif (k == "tom"):
        return 150
    elif (k == "hanks"):
        return 175
    elif (k == "crues"):
        return 190
    elif (k == "tifanny"):
        return 200
        
    return 0

で,上記のメソッドの名前を sorted 関数に渡してあげます.

In [180]: sorted(namelist, key=orderFunc, reverse=True)
Out[180]: ['tifanny', 'crues', 'mike', 'hanks', 'sam', 'tom']

めっちゃ便利ですね...

Pandas の使いかた

およそ半年ぶりの更新ですが,何事もなかったかのように平然とエントリをし始めましたが...
(つくばチャレンジにも参加しました.結果は残念でしたが...また取り組みをアップします.)

最近会社で機械学習の勉強をしていて,その際に Pandas というライブラリを使う必要があったので,概要と使い方を備忘録としてまとめておきます.
勉強に使ったのは下記の本です.一年半くらい前に買って,積読状態になっていたので,活用できる機会ができてよかった...

Pythonによるデータ分析入門 ―NumPy、pandasを使ったデータ処理

Pythonによるデータ分析入門 ―NumPy、pandasを使ったデータ処理


  • pandas のデータ構造

 大まかにいうと,Series と DataFrame という二種類のデータ構造がある.シリーズというのは,一次元の配列らしいが,Numpyの配列と異なるところは,インデックスというデータラベルの配列がつくところ.まず,IPython を使って Series で遊んでみます.


ライブラリのインポート.

from pandas import Series, DataFrame
import pandas as pd
  • シリーズオブジェクトの作成
In [131]: series = Series([1, 10, 100, 1000])

In [132]: series
Out[132]: 
0       1
1      10
2     100
3    1000
dtype: int64

上の出力を見てみると,Numpyの配列とは違って,勝手にインデックスが付与されていることがわかります.
また,このインデックスに関しては,自分で好きなものを付与することもできます.

  • インデックスラベルを指定したシリーズオブジェクトの作成
In [134]: series_w_index_specified = Series([1, 10, 100, 1000], index=['JPN', 'USA', 'GER', 'GBR'])

In [135]: series_w_index_specified
Out[135]: 
JPN       1
USA      10
GER     100
GBR    1000
dtype: int64
  • データフレームオブジェクトの作成

 シリーズが一次元の配列であるのに対し,こちらは二次元の配列というかテーブル風のデータ構造.

In [151]: data = {'state': ['Ohio', 'Ohio', 'Ohio', 'Nevada', 'Nevada'], 
     ...: 'year' : ['2000', '2001', '2002', '2001', '2002'], 
     ...: 'pop' :['1.5', '1.7', '3.6', '2.4', '2.9']}

In [152]: frame = DataFrame(data)

In [153]: frame

In [154]: Out[153]: 
   pop   state  year
0  1.5    Ohio  2000
1  1.7    Ohio  2001
2  3.6    Ohio  2002
3  2.4  Nevada  2001
4  2.9  Nevada  2002

 これが使いたかったんです.以前 SQL を使ってデータベースの処理をしていたことがあったんですが,テーブルの結合とかと等価の処理を自分で作るのは時間もかかるしめんどくさいし..ということで,pandas を使ってデータセットをマージする方法をメモ.

pandas.merge : データフレームに含まれる行を一つ以上のキーでマージできる.
pandas.concat : 複数の列のオブジェクトを貼り合わせたり,積み上げたりすることが可能.

    • pandas.merge を使ったデータフレームのマージ
In [9]: df1 = DataFrame({'key': ['b', 'b', 'a', 'c', 'a', 'a', 'b'],
   ...: 'data1': range(7)})

In [10]: df2 = DataFrame({'key': ['a', 'b', 'd'],
    ...: 'data2': range(3)})

In [11]: df1
Out[11]: 
   data1 key
0      0   b
1      1   b
2      2   a
3      3   c
4      4   a
5      5   a
6      6   b

In [12]: df2
Out[12]: 
   data2 key
0      0   a
1      1   b
2      2   d

上で作成したデータを,pandas.merge を使ってマージします.
超簡単ですね.ありがたやありがたや.

In [15]: pd.merge(df1, df2, on = 'key')
Out[15]: 
   data1 key  data2
0      0   b      1
1      1   b      1
2      6   b      1
3      2   a      0
4      4   a      0
5      5   a      0


結合する列の名前が左右のテーブルで異なるときもあると思いますが,こちらに関しても下記のようにすれば結合できるみたいです.
両方にキーが存在する行のみがマージされた結果に残されている様子が見て取れます.この結合を内部結合といい,結果に表れるキーは両者の共通集合になります.

In [22]: df3 = DataFrame({'lkey':['b', 'b', 'a', 'c', 'a', 'a', 'b'],
    ...: 'data1': range(7)})

In [23]: df4 = DataFrame({'rkey':['a', 'b', 'd'],
    ...: 'data2': range(3)})

In [24]: pd.merge(df3, df4, left_on='lkey', right_on='rkey')
Out[24]: 
   data1 lkey  data2 rkey
0      0    b      1    b
1      1    b      1    b
2      6    b      1    b
3      2    a      0    a
4      4    a      0    a
5      5    a      0    a

一方で,どちらか片方に存在するキーも消したくない場合もあると思います.この場合には,外部結合を実施すれば結合できます.
ただし,ペアとなるキーが相手方に存在しない場合はそのデータはNaNとなります.下記の実行結果を見てもわかる通り,結合ペアのキーのない箇所はデータが NaN になっています.

In [25]: pd.merge(df3, df4, left_on='lkey', right_on='rkey', how='outer')
Out[25]: 
   data1 lkey  data2 rkey
0    0.0    b    1.0    b
1    1.0    b    1.0    b
2    6.0    b    1.0    b
3    2.0    a    0.0    a
4    4.0    a    0.0    a
5    5.0    a    0.0    a
6    3.0    c    NaN  NaN
7    NaN  NaN    2.0    d

OpenCV Stereo Matching の性能比較 1

前回の投稿から少し時間が空いてしまいました...が,ブログ更新が飽きたというわけではなく,ちょっと更新するほどのまとまったネタができなかったわけです.というわけで,今日はつくばチャレンジで使うステレオマッチングメソッドの比較・選定を行いました.とりあえず,OpenCVでロジックが提供されている下記の3つのアルゴリズムを比較検討しました.

  1. Block Matching Method
  2. Stereo Block Matching Method
  3. Stereo Graph Cut Method

0. 実験に用いたベースデータ

 実験には,手軽に手に入る Tsukuba の視差画像を用いました.

左画像 サイズ 384 x 288
f:id:rkoichi2001:20160625230555p:plain
右画像 サイズ 384 x 288
f:id:rkoichi2001:20160625230603p:plain

1. Block Matching Method

 一番オーソドックスなアルゴリズムです.実験結果は下記の通り.
実行時間 3.3464ms (334.646 / 100)
f:id:rkoichi2001:20160625232749p:plain

2. Semi Global Matching Method

 一番良く使われているアルゴリズム?ではないでしょうか.
実行時間 16.6549ms (1665.49 / 100)
f:id:rkoichi2001:20160625231149p:plain

3. Stereo Graph Cut Method

 実行時間が結構長いです..が,結果も一番きれいです.解説にあるとおり,オフラインでの使用前提ですね.
実行時間 6664.27ms (66642.7 / 10)
f:id:rkoichi2001:20160625233306p:plain

実際には,パラメータを適切に調整して比較したわけではないので純粋に比較できているわけでは無いです.次のエントリではこの3つの手法を自分が撮影した画像に適用して見たいと思います.下記,実験に使ったソースコードをおいておきます.

1. Block Matching Method

#include <iostream>
using namespace std;

#include "opencv2/opencv.hpp"

int main() {
  const char *rightImg = "/home/koichi/data/tsukuba/scene1.row3.col4.ppm";
  const char *leftImg = "/home/koichi/data/tsukuba/scene1.row3.col2.ppm";

  // const char *rightImg = "/home/koichi/Desktop/Stereo Pair/pentR.bmp";
  // const char *leftImg = "/home/koichi/Desktop/Stereo Pair/pentL.bmp";

  IplImage *imgR, *imgL, *dst;
  IplImage *dispLeft, *dispRight;
  int disp = 32;
  //  CvStereoGCState *state = cvCreateStereoGCState(disp, 4);
  CvStereoBMState *state = cvCreateStereoBMState(CV_STEREO_BM_BASIC, disp);
  state->SADWindowSize = 9;

  // . Load Image.
  imgR = cvLoadImage(rightImg, CV_LOAD_IMAGE_GRAYSCALE);
  imgL = cvLoadImage(leftImg, CV_LOAD_IMAGE_GRAYSCALE);

  CvSize size;
  size.width = imgR->width;
  size.height = imgR->height;

  // . Allocate Memory
  dispLeft = cvCreateImage(size, IPL_DEPTH_16S, 1);
  dispRight = cvCreateImage(size, IPL_DEPTH_16S, 1);
  dst = cvCreateImage(size, IPL_DEPTH_8U, 1);

  {
    cv::TickMeter meter;
    meter.start();

    for (int i = 0; i < 100; i++) {
      // Calcuate Disparity
      cvFindStereoCorrespondenceBM(imgL, imgR, dispLeft, state);
    }

    meter.stop();
    std::cout << "Average Time for Calculation : "
              << meter.getTimeMilli() / 100.0 << std::endl;
  }
  double min, max;
  cvMinMaxLoc(dispLeft, &min, &max);

  cvConvertScale(dispLeft, dst, 255 / (max - min), 255 / min);
  // cvNormalize(dispLeft, dst, 1, 0, CV_MINMAX);

  const char *winName = "Stereo Correspondence";

  cvNamedWindow("Right Image", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("Left Image", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("Disparity", CV_WINDOW_AUTOSIZE);

  cvShowImage("Right Image", imgR);
  cvShowImage("Left Image", imgL);
  cvShowImage("Disparity", dst);

  cvWaitKey(0);

  // cvReleaseStereoGCState(&state);
  cvDestroyAllWindows();

  cvReleaseImage(&imgR);
  cvReleaseImage(&imgL);

  return 0;
}

2. Semi Global Matching Method

#include <iostream>
#include <string>
using namespace std;

#include "opencv2/opencv.hpp"

int main() {
  string rightImg = "/home/koichi/data/tsukuba/scene1.row3.col4.ppm";
  string leftImg = "/home/koichi/data/tsukuba/scene1.row3.col2.ppm";

  cv::Mat imgR = cv::imread(rightImg, CV_8UC1);
  cv::Mat imgL = cv::imread(leftImg, CV_8UC1);
  cv::Mat dispMat(imgR.rows, imgR.cols, CV_8UC1);

  int disp = 32;
  int minDisparity = 0;
  int numDisparities = 32;
  int SADWindowSize = 3;
  int P1 = 0;
  int P2 = 0;
  int disp12MaxDiff = 0;
  int preFilterCap = 0;
  int uniquenessRatio = 0;
  int speckleWindowSize = 0;
  int speckleRange = 0;
  bool fullDp = false;

  cv::StereoSGBM sgbm(minDisparity, numDisparities, SADWindowSize, P1, P2,
                      disp12MaxDiff, preFilterCap, uniquenessRatio,
                      speckleWindowSize, speckleRange, fullDp);

  {
    cv::TickMeter meter;
    meter.start();

    for (int i = 0; i < 100; i++) {
      sgbm(imgL, imgR, dispMat);
    }

    meter.stop();
    std::cout << "Average Time for Calculation : "
              << meter.getTimeMilli() / 100.0 << std::endl;
  }

  double min, max;
  cv::minMaxLoc(dispMat, &min, &max);
  cv::convertScaleAbs(dispMat, dispMat, 255 / (max - min), 255 / min);

  cv::namedWindow("Right Image", CV_WINDOW_AUTOSIZE);
  cv::namedWindow("Left Image", CV_WINDOW_AUTOSIZE);
  cv::namedWindow("Disparity", CV_WINDOW_AUTOSIZE);

  cv::imshow("Right Image", imgR);
  cv::imshow("Left Image", imgL);
  cv::imshow("Disparity", dispMat);

  cvWaitKey(0);
  cvDestroyAllWindows();

  return 0;
}

3. Stereo Graph Cut Method

#include <iostream>
using namespace std;

#include "opencv2/opencv.hpp"

int main() {
  const char *rightImg = "/home/koichi/data/tsukuba/scene1.row3.col4.ppm";
  const char *leftImg = "/home/koichi/data/tsukuba/scene1.row3.col2.ppm";

  // const char *rightImg = "/home/koichi/Desktop/Stereo Pair/pentR.bmp";
  // const char *leftImg = "/home/koichi/Desktop/Stereo Pair/pentL.bmp";

  IplImage *imgR, *imgL, *dst;
  IplImage *dispLeft, *dispRight;
  int disp = 32;
  CvStereoGCState *state = cvCreateStereoGCState(disp, 4);

  // . Load Image.
  imgR = cvLoadImage(rightImg, CV_LOAD_IMAGE_GRAYSCALE);
  imgL = cvLoadImage(leftImg, CV_LOAD_IMAGE_GRAYSCALE);

  CvSize size;
  size.width = imgR->width;
  size.height = imgR->height;

  // . Allocate Memory
  dispLeft = cvCreateImage(size, IPL_DEPTH_16S, 1);
  dispRight = cvCreateImage(size, IPL_DEPTH_16S, 1);
  dst = cvCreateImage(size, IPL_DEPTH_8U, 1);

  // Calcuate Disparity
  {
    cv::TickMeter meter;
    meter.start();
    int repeat = 10;

    for (int i = 0; i < repeat; i++) {
      cvFindStereoCorrespondenceGC(imgL, imgR, dispLeft, dispRight, state, 0);
    }

    meter.stop();
    std::cout << "Average Time for Calculation : "
              << meter.getTimeMilli() / repeat << std::endl;
  }

  cvConvertScale(dispLeft, dst, -256 / disp);

  const char *winName = "Stereo Correspondence";

  cvNamedWindow("Right Image", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("Left Image", CV_WINDOW_AUTOSIZE);
  cvNamedWindow("Disparity", CV_WINDOW_AUTOSIZE);

  cvShowImage("Right Image", imgR);
  cvShowImage("Left Image", imgL);
  cvShowImage("Disparity", dst);

  cvWaitKey(0);

  // cvReleaseStereoGCState(&state);
  cvDestroyAllWindows();

  cvReleaseImage(&imgR);
  cvReleaseImage(&imgL);

  return 0;
}

環境設定 - CppStyle

C++ Eclipse Tool 環境設定

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

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;
}

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

  • 5.ステレオ結果から3次元座標の生成
  • 6.3次元座標からのトップビューの生成,z座標の値を見て走行可能エリアかどうか判定するマップの作成.
  • 7.2次元の走行可能マップを使って,自己位置推定ができないかどうか検討.
    • 二値化画像のテンプレートマッチング (大体の位置精度が出せればOK.後は走行可能エリアの真ん中とか,わかりやすい指標を元に走れるとこをはしる.)
    • 鳥瞰図変換結果を色でクラスタリングして走行可能エリア作成
    • 鳥瞰図変換結果をエッジ検出する
自己位置推定実験 & ステレオカメラ実験
  • 実験 with ロボット
  • 実験 without ロボット
  • 複数の連続する地点で2回カメラ撮影を実施する.
  • 連続する地点で撮影した画像が自己位置推定に使えるか確認する.
  • 新しいレンズの性能を見てみる
  • 新しいレンズを使ってステレオカメラ作成
自己位置推定マップ
  • 1.文献調査
  • 2.ソフト作成
車体周辺の環境マップ作成
  • 1.文献調査
  • 2.ソフト作成

開発環境構築

  • 1.Ubuntu のバージョン決定

daily-tech.hatenablog.com

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

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 土)

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

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

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

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

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

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

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