イノベーション

技術ブログなので，あんまり自分の思いを書いたりするつもりはなかったのですが．．．（炎上すると怖いので（笑））
ちょっとある動画を見つけてしまい，エントリを書いてみました．

www.youtube.com

これ，ありえないですよね．ちょっと数年前までは二足歩行で歩くことだけでも結構すごかったのに．．．．

最近特に思うのですが，概念的に新しいもの・サービス（IPhone，Itunes，GPU&AI，AWS）を初めておおきなお金を稼ぐのも外国企業，既存のモノ（自動車，ロケット，二足歩行ロボット）の延長線上にあって，みんなできたらいいなと思っている（完全自動運転，民間でのロケット開発・運用，鉄腕アトム）けど，技術的ハードルが高すぎて到達できなさそうに見えるもののハードルを超えるのも外国企業になってしまっていますよね．

ここ数年，世界で起こっている大きなトレンドで日本の企業が起こしたものって，本当に一つもないような気がします．（唯一あるとしたら，ハイブリッドカーでしょうか．もうEVに移行しかかってますが．．．）これ，なんでですかね．リーマンショックが起こった時に，資本主義では短期的な結果を求めすぎるから云々，とかってことがよく議論になりましたけど，少なくとも製造業・IT業界では外国企業のほうがはるかに長期的視野で物事を考えているような気がします．
（上記動画のBoston Dynamicsは2017年からはソフトバンクグループなのかもしれませんが．．．）

確かに，上の動画のロボットもマニタイズっていう観点で見ると「で，それ売れるの？金稼げるの？」っていう質問にはまだ答えられないレベルだと思うので，まだ詰めるところは無数にあるとはおもうのですが，ひとつづつ着実にハードルを越えていってる感じですね．．．．気が付いたら，今のスマホみたいに寡占状態になってなければいいんですが．

つくばチャレンジ２０１７を終えて．．．

ということで，今年も終わりました．つくばチャレンジ本走行．
当初の目標（２キロ完走）からはほど遠い結果（２００メートル走行）になってしまいましたが，いろいろと得るものは多い一年だったと思います．

簡単に言うと，結果としては下記のような感じでした．

上記写真の赤色の星マークのところまでの自走でリタイアでした．
上記の図だけをにみると，「いやいや，全然やん！」という感じだと思うのですが，（まあ，実際そうなのですが（笑））
自己位置推定がある程度できるようになって，自分で進む方向を都度自律判断して動けるようになったのは一つ進歩だったかと思います．
もっと距離を延ばすためには，人・移動物・障害物を検知して停止する・よけるという部分も作らないといけないですが，この辺は来年に向けて作りこみます．

最終的なシステム構成は下記の感じになりました．思ったほどＲＯＳのライブラリをうまく使いまわすことができず，結構自作する形になりましたが，このあたりは
かえって勉強になってよかったかなと思います．

システム構成

３年目にしてようやくいろんなこと（ハード・電源周りの組み方・ソフトの作り方・自律移動ロボットの難しさ・課題）がなんとなくわかってきたような気がします．
ということで，なんだか毎年やるやる詐欺になってますが（笑），宣言させてください．「来年は完走します！！！」

Denzelさんも，「Without commitment, you'll never start」「Without consistency, you'll never finish」って言ってますし（笑）

www.youtube.com

１２月中旬に仙台でＳＩ２０１７の発表会に出て今年のつくばチャレンジ関連イベントは終わりになりますが，来年も応援よろしくお願いします！！

上記写真、今年から導入した自分のテントです。二十代前半の若者に混じって三十代のおっさんが大真面目にパソコンに向き合っている絵もなかなかシュールですが笑 来年も繰り返されます。

で，，，ロボットばっかり詰めてやったせいもあってちょっと飽きたので（笑），来年のつくばチャレンジのためのネタ探しもかねて，２月末までちょっと自由研究します．
テーマは「沖縄（首里城＆国際通り）を三次元復元する！」ということで，詳しくは次のポストに書きますが，三次元復元にチャレンジします．
ボーナスをはたいて，一眼レフと高性能ＰＣも買ってしまいました（笑）
しばらくはこちらの話題を詳しく書いていくので，こうご期待！！！

ステレオカメラを用いた自己位置推定

前回の更新からほぼ二か月空いてしまいました．．．結局１０月は一度もブログを更新することなく終えてしまいましたが，今年のつくばチャレンジも終わりました．
結果は別のエントリで書くとして，以前のエントリで自己位置推定のことをちょこっと書いてましたが，ようやく完成して使えるレベルになったので記事に起こしておきます．

思い起こせば，２０１７年は自己位置推定との戦いでした．．

１．SWEEP（格安の2Dレーザスキャナ）を購入して，ROSのAMCLを用いてマップ作製・自己位置推定
―＞屋外使用でスキャナのレンジが３ｍくらいしかなく，撃沈．

２．ステレオの出力を疑似的にレーザースキャンに変更，ROSのAMCLを用いて自己位置推定にトライ
―＞ステレオ視差情報から求まる位置の距離に対する精度劣化が大きく，変換しても使えず．．．

３．ステレオ位置情報から求まる位置情報を路面のグリッドに投影．グリッド毎に代表高さを決めてやり，グリッド間勾配を見て通行可能／不可能のマップを作製．
自動走行時には上記のマップに対してパターンマッチングをして自己位置推定．
―＞この実装でつくばチャレンジに参加．

ということで，３の方法に落ち着きました．結局作りこみが甘く，大清水公園内でのリタイアになってしまいましたが，自己位置推定はできていたように思います．

下記，ステレオデータを使って実際に自己位置推定している様子です．ビデオを早送り編集できなかったので，ちょっと無駄に長い動画になってしまいましたが．．
左側が事前に取得しておいた地図で，右側の小さい正方形がロボットが自走しているときの周辺地図です．この周辺地図を事前に取得した地図にテンプレートマッチング
することで自己位置推定しています．

www.youtube.com

で，最終的に得られた結果が下記になります．青色の線がデッドレコニングの結果，赤色の点がテンプレートマッチングの結果，黄緑色の線がその二つを拡張カルマンフィルタで
統合した結果です．割といい感じに自己位置推定できていると思います．自画自賛ですが．．．ただ，現状はカルマンフィルタを使って統合しているだけというのもあって，
一度自分の位置を見失うと，復帰できないので，この辺りはパーティクルフィルタとかほかのアルゴリズムを実装してちょっと作りこまないといけないと思います．

つくばチャレンジの結果はまた次のポストで書きます！

CUDA によるヒストグラム生成高速化

自分のロボット部品のCUDA化する必要があったので，CUDAの勉強もかねてGPUを使ったヒストグラム作成のプログラムを作りました．
内容としては，下記の本の９章のサンプルコードを参考に作成しました．下記の本，基本的なことが一通り説明されてまして，個人的には結構おすすめでした．

CUDA by Example 汎用GPUプログラミング入門

ヒストグラムのもととなる写真は，今をときめくSpace XのFalcon9です．

Falcon9の垂直着陸シーン．（これ，ほんとすごいですよね！酒飲んだら毎回必ず興奮して，着陸シーンを友達に見せつけてしまいます．自分がなにかやったわけでは無いですが．．．）
www.youtube.com

で，だいぶ派手な動画の後にだいぶ地味なエントリ内容で恐縮ですが，GPUとCPUでヒストグラムを計算した実行時間の結果ですが，下記のようになりました．

実験条件：
添付写真　（1500pix x 1000pix，グレースケール）のヒストグラム計算
実行回数　10000回のヒストグラム計算 x 10Set
実行環境　CPU : Intel Core i7-4700HQ, GPU : NVIDIA GeForce GT 740M (64bit)
実行結果　

うーん．．もう少し早くなると思ったんですが，半分くらいの速度にしかならなかったです．
おそらく，GPUのスペックがだいぶ低いからだと思うのですが，Jetsonだともって早くなってくれるはず．．．

サンプルコード（一応，こちらがホスト側のコードです．）

//
#include <iostream>
#include <cuda_runtime.h>
#include <cstdlib>
#include <sys/time.h>
#include <stdio.h>
#include <assert.h>

//
#include "opencv2/highgui.hpp"
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"

//


#define REPEAT_NUM (10000)

static void createHistogramWithCPU(int width, int height, const unsigned char* buff, unsigned int *histo);
extern void createHistogramWithGPU(int width, int height, const unsigned char* buff, unsigned int *histo);

static void createHistogramWithCPU(int width, int height, const unsigned char* buff, unsigned int *histo) {

  const unsigned char *pnt = buff;

  for (int j = 0; j < height; j++) {
    for (int i = 0; i < width; i++) {
      histo[*pnt]++;
      pnt++;
    }
  }
}

static void drawHistogram(const unsigned int* histo, cv::Mat &histoImg) {

  int max = 0;
  {
    const unsigned int *pnt = histo;
    for (int i = 0; i < 256; i++) {
      unsigned int value = *pnt;
      pnt++;
      if (max < value) {
        max = value;
      }
    }
  }

  {
    const unsigned int *pnt = histo;
    histoImg = cv::Mat(512, 512, CV_8UC1);
    histoImg = 255;
    for (int i = 0; i < 512; i+=2) {
      unsigned int value = *pnt;
      pnt++;
      unsigned int thresh = 512 - (512 / (float)max * value);
      for (int j = 0; j < 512; j++) {
        if (j > thresh) {
          histoImg.at<unsigned char>(j, i) = 0;
          histoImg.at<unsigned char>(j, i + 1) = 0;
        }
      }
    }
  }

}

int main(int argc, char** argv)
{
  std::cout << "Cuda Sample Started!" << std::endl;

  unsigned int *histoCpu = new unsigned int[256];
  unsigned int *histoGpu = new unsigned int[256];


  cv::Mat src = cv::imread("./falcon_9.jpg", cv::IMREAD_GRAYSCALE);
  cv::resize(src, src, cv::Size(), 0.5, 0.5);

  for (int k = 0; k < 10; k++) {

    std::cout << "Histogram Create" << std::endl;
    {
      struct timeval t1, t2;
      gettimeofday(&t1, NULL);
      for (int i = 0; i < REPEAT_NUM; i++) {
      //for (int i = 0; i < 1; i++) {
        memset(histoCpu, 0, 256 * sizeof(int));
        createHistogramWithCPU(src.cols, src.rows, (unsigned char *)src.data, histoCpu);
      }
      gettimeofday(&t2, NULL);
      printf("Time CPU %f\n", (t2.tv_sec - t1.tv_sec) + (t2.tv_usec - t1.tv_usec)*1.0E-6);
    }

    {
      struct timeval t1, t2;
      gettimeofday(&t1, NULL);
      for (int i = 0; i < REPEAT_NUM; i++) {
        memset(histoGpu, 0, 256 * sizeof(int));
        createHistogramWithGPU(src.cols, src.rows, (unsigned char *)src.data, histoGpu);
      }
      gettimeofday(&t2, NULL);
      printf("Time GPU %f\n", (t2.tv_sec - t1.tv_sec) + (t2.tv_usec - t1.tv_usec)*1.0E-6);
    }

    cv::Mat histoImgCpu, histoImgGpu;
    drawHistogram(histoCpu, histoImgCpu);
    drawHistogram(histoGpu, histoImgGpu);

    cv::imshow("sample", src);
    cv::imshow("CPU", histoImgCpu);
    cv::imshow("GPU", histoImgGpu);
    cv::waitKey(1000);

  }

  delete histoCpu;
  delete histoGpu;

  return 0;
}

サンプルコード（こちらがデバイス側のコードです．一部ホスト側のコードも入ってますが．．）

#include <cuda_runtime.h>
#include <stdio.h>

static void HandleError( cudaError_t err,
                         const char *file,
                         int line ) {
    if (err != cudaSuccess) {
        printf( "%s in %s at line %d\n", cudaGetErrorString( err ),
                file, line );
        exit( EXIT_FAILURE );
    }
}
#define HANDLE_ERROR( err ) (HandleError( err, __FILE__, __LINE__ ))

__global__ void createHistogramKernel(unsigned char* buffer, long size, unsigned int* histo) {
	
	__shared__ unsigned int temp[256];
	temp[threadIdx.x] = 0;
	__syncthreads();
	
	int i = threadIdx.x + blockIdx.x * blockDim.x;
	int stride = blockDim.x * gridDim.x;
	
	while (i < size) {
		atomicAdd( &temp[buffer[i]], 1 );
		//atomicAdd( &(histo[buffer[i]]), 1 );
		i += stride;
	}
	
	__syncthreads();
	atomicAdd( &(histo[threadIdx.x]), temp[threadIdx.x] );
}

void createHistogramWithGPU(int width, int height, const unsigned char* buff, unsigned int *histo) {
	
	unsigned char *devBuffer;
	unsigned int *devHisto;
	
	// Memory Allocation.
	HANDLE_ERROR( cudaMalloc( (void **)&devBuffer, width * height ));
	HANDLE_ERROR( cudaMalloc( (void **)&devHisto, 256 * sizeof(unsigned int) ));
	
	HANDLE_ERROR( cudaMemcpy( devBuffer, buff, width * height, cudaMemcpyHostToDevice ));
	HANDLE_ERROR( cudaMemset( devHisto, 0, 256 * sizeof(unsigned int) ));

	cudaDeviceProp prop;
	cudaGetDeviceProperties( &prop, 0 );
	int blocks = prop.multiProcessorCount;
	//createHistogramKernel<<< blocks * 2, 256 >>>( devBuffer, width * height, devHisto );
	createHistogramKernel<<< 6 * 2, 256 >>>( devBuffer, width * height, devHisto );
	
	HANDLE_ERROR( cudaMemcpy( histo, devHisto, 256 * sizeof(unsigned int), cudaMemcpyDeviceToHost));

	// Memory Free
	HANDLE_ERROR( cudaFree( devBuffer ));
	HANDLE_ERROR( cudaFree( devHisto ));

}

システム構成 ~ 試走会2回目に向けて．

「近くの実験場」でロボットを走らせたかったのですが，当然間に合うわけもなく．．．
今週はロボット開発者のバイブルこと「確率ロボティックス」にもう一度目を通してどうするか考えていました．

日本語版

確率ロボティクス (プレミアムブックス版)

英語版

Probabilistic Robotics (Intelligent Robotics and Autonomous Agents series)

で，ひとまず自律で動かすまでにあと何が必要なのかを再度洗い出して見ました．（当初の，「できるだけROSを使う」という目論見からもちょっと変わってしまったので．．．）

新しく作らないといけない部品（上図中で黄色の箱）

Localizer

Localizerは前回のポストであったように，あらかじめ保持している地図中において，「ロボットがどこを走っているか？」を判別する部品です．
オドメトリの結果とマップマッチングの結果を統合する必要があるのですが，この統合の方法としてどのアルゴリズムを使えばよいのかずっと悩んでいたのですが，
単純にEKFを使うことにしました．詳細はまた別のエントリでアップします．

Way Point Publisher

この部品は，ロボットが進んでほしい軌跡をコントローラに対してリクエストする部品です．具体的には，作成した地図（下記）で，通過してほしいポイントを事前に決めておきます．
下図の赤い〇がWay Pointを表してます．で，赤の〇の中にロボットが入ったら次の赤の〇をターゲット座標として動きます．これを繰り返してゴールを目指します．

実装に手を入れないといけない部品

こちらに関しては，一応ちょこっと動くものはできているのですが，処理が多くて（+自分の実装がストレートすぎて，，，）全然リアルタイムで動かないのでCUDA実装してJETSONで動かします．
一応先週CUDAの復習もしたので，すんなりいけばいいのですが，，，またアップします．

Noise Remover (CUDA化)

Global Map Generator (ROS化 + CUDA化)

Local Map 2 Laser Scan (ROS化 + CUDA化)

あとちょっとのように見えていて，実はまだいっぱいやることありますね．．．なんとか9/23には間に合わせられるように頑張ります．
今日はCUDA化のとこを別のエントリでアップします．

テンプレートマッチング with CUDA

昨晩の深酒のせいで，昼まで寝てしまい．．．．掃除・洗濯もろもろしたらもう17時．．．．
9/23に試走会2回目があるのに，この感じだと間に合わないなあと言ってても始まらないので，タイトルどおり今からテンプレートマッチングの部品を作ります．
0から作るのはしんどいので，OpenCVのCUDA実装をほぼコピーして必要な個所だけをアレンジすることにしました．

完成したらエントリ更新します．

ーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーーー

上記投稿からはや４日．．．テンプレートマッチング試してみました．
CUDAの実装方法をちょっと復習したり（若干モチベーションが上がらなかったり．．．）してたので，思ったより時間がかかってしまいました．
上記テンプレートマッチングですが，なんとかうまく行きそうな気がしてます．

カートを押して，大清水公園のマップ（下記）を作成します．

で，もう一回大清水公園をカートを押してマップを作りながら，いま事前に作っていた地図の中でどこを走っているかを調べます（自己位置推定）

実際には，赤の正方形（10m x 10m）の中心にロボットがいると仮定します．で，赤の正方形だけを切り出したものが下記です．

で，上記の正方形領域をもともと作っていた地図に対して照らしあわせて，一番結果がマッチしたところにロボットがいると判断します．
そのマッチング結果が下記になります．一番白が強い箇所が一番マッチが強い箇所です．

下記のマッチング結果はCUDA実装の一部を変更した自作バージョンのマッチングスコア

下記のマッチング結果はCUDA実装にもともとあるTM_CCOEFFを使ってマッチングしたスコア

CUDA実装にあるテンプレートマッチング方式のほうがいい感じにマッチングスコアが出たのが意外でしたが，灰色（Unknown）領域ではマッチングしたくないので，ここは少し手を入れる必要があると思います．あと，実際にはオドメトリからロボットが走っている位置はだいたいわかっているので，マップ全体に対してテンプレートマッチングする必要はなく，実際には今ロボットがいると思っている位置±3m，±5degくらいでマッチングをして一番スコアが高い位置にロボットがいると判定し，そこから経路を引いてロボットをすすめる．という流れになるかと思います．ひとまず，土曜までには自己位置推定する部品を完成させ，日曜には「近所の実験場」で実際にロボットを走らせて見たいのですが，，，，また状況アップします．

ステレオカメラを用いた OGM の作成３ @ 大清水公園

というわけで，ちょっと時間がかかってしまいましたが，大清水公園のマップを作成しました．
実際には OGM というよりも，TGM?とでもいうべきなのでしょうか？マップは対象のグリッドが通行可能かどうか「Traversable or not」の条件で作ってあります．

具体的には，グリッド間の高さの差が20cm以上ある箇所は通行不可能として黒色に，そうでない場合は通行可能であるとして白色に，検知範囲に入っておらずわからない場所は灰色に表示しています．

まず，グリッドの高さを濃淡で表したマップが下記です．

一部緑になってしまってますが，これは変換ミスです．
一応，同じ日に3回走行したデータを用いてマップを作っているのですが，見た感じ再現性がありそうなので自律走行に使えるのではないか．．と思ってます．
灰色の箇所はUnknownということでマッチングに使用せず，白と黒の領域だけを使ってテンプレートマッチングを実施してマップ内部でどこにロボットが存在するか判定しようと考えています．具体的には，，，

事前にとっておいた地図を下記とします．

で，ロボットが自律走行しているときに作成している地図を下記とします．

赤のエリアをロボットの周辺10m x 10mの正方形とします．この正方形の位置を事前にとっておいた地図の中で探します．
具体的には，事前にとっておいた地図の領域に対して，赤の正方形でテンプレートマッチングを実施して一番スコアが高い箇所を現在の位置とします．
ステレオノイズの影響もあって，地図自体がまだ完璧なレベルではないですが，時間がもうないので，ひとまず現状のものを使って実験してみます．
ということで，今週末はテンプレートマッピングの部品を作ります！