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

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

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

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

ヒストグラムのもととなる写真は,今をときめくSpace XのFalcon9です.
f:id:rkoichi2001:20170910155552j:plain

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

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

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

f:id:rkoichi2001:20170910162642p:plain

うーん..もう少し早くなると思ったんですが,半分くらいの速度にしかならなかったです.
おそらく,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)

Probabilistic Robotics (Intelligent Robotics and Autonomous Agents series)

  • 作者: Sebastian Thrun,Wolfram Burgard,Dieter Fox
  • 出版社/メーカー: The MIT Press
  • 発売日: 2005/08/19
  • メディア: ハードカバー
  • 購入: 1人 クリック: 6回
  • この商品を含むブログを見る

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

f:id:rkoichi2001:20170910122012p:plain

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

Localizer

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

Way Point Publisher

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

f:id:rkoichi2001:20170910124704p:plain

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

こちらに関しては,一応ちょこっと動くものはできているのですが,処理が多くて(+自分の実装がストレートすぎて,,,)全然リアルタイムで動かないので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実装をほぼコピーして必要な個所だけをアレンジすることにしました.

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

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

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

カートを押して,大清水公園のマップ(下記)を作成します.

f:id:rkoichi2001:20170906063527p:plain

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

f:id:rkoichi2001:20170906063653p:plain

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

f:id:rkoichi2001:20170906063745p:plain

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

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

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

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

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

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

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

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

f:id:rkoichi2001:20170901053051j:plain

f:id:rkoichi2001:20170901053057j:plain

f:id:rkoichi2001:20170901053104j:plain

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

事前にとっておいた地図を下記とします.
f:id:rkoichi2001:20170901053530j:plain

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

f:id:rkoichi2001:20170901053812j:plain

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

カルマンフィルタを使ったノイズ除去

で,現在行き詰まり中のロボット作成ですが,何とかステレオカメラを使ってマップを作らないといけないのでノイズの除去をする方法を検討してます.
解こうとしている問題は,,,

1.ステレオカメラで取得した視差情報を3次元の点群に変換.
2.3次元の点群を地面に投影して,地面に切ったグリッドの高さを求める.
3.地面に切ったグリッドの高さ,もしくは地面の傾きが一定以上大きい箇所は通行不可能なエリアとしてマーキングする.

で,結果的にはマップを「通行可能」「通行不可能」の二通りの値に分類して,これをマップとして生成します.
このためには,地面に切ったグリッドのセル高さを求めないといけないのですが,ステレオの視差情報はノイズが大きく,これをどうやって扱えばいいものかとずっと悩んでました.

f:id:rkoichi2001:20170826221543p:plain

上図はカメラと地面に切ったグリッドの簡易図ですが,青の三角形がカメラでその前のマス目が地面に定義したグリッドです.ステレオの視差情報から生成したポイントクラウドは上記のセルのどこかに属する形になりますが,このポイントクラウド一つ一つの高さ情報を使ってマス目一つの高さ情報を定義します.(マス目一つ一つが高さ情報を持っているイメージです.)

ただ,ステレオの視差情報はノイズによる影響が大きいので,同じマス目なのに高さが1.5mの時もあれば,2.5メートルとなってしまう時もあります.今回は,このノイズを取るために1次元のカルマンフィルタを使ってみることにしました.そんなにカルマンフィルタには詳しくなかったのでGoogle先生にいろいろと教えてもらって勉強しました.一番わかりやすくまとめられてたのが,下記のサイトです.

https://logics-of-blue.com/kalman-filter-concept/

で,上記サイトのサンプルプログラムを参考に,自分の問題にあてはめて Python コードを作ってみました.
問題設定は,,,,
1.ロボットが前方に動いて,新たなセルが視界に入ってくる.新たなセルの高さの真値は1mとする.
2.新たなセルであるため,当然高さ情報はなし.つまり初期値を 0m としてはじめる.
3.ノイズがいっぱい乗っているステレオの情報を繰り返し使って,真値の1mに近づける.

という問題設定です.ちなみに,カルマンフィルタを選択した時点でステレオノイズに含まれている誤差が平均値0の白色ノイズという前提になってしまうのですが,今回取り除きたいノイズがこのモデルに従うかどうかはわかりません...

下記 Python のサンプルコードです.

import numpy as np
from numpy.random import *
import matplotlib.pyplot as plt

def Kalman(y, xK1, pK1, sigmaW, sigmaV):

    # Predict State Variable
    xK = xK1

    # Update Sigma Variable
    pK = pK1 + sigmaW

    # Update Kalman Gain
    kGain = pK / (pK + sigmaV)

    # Filter State and Variable
    xFilt = xK + kGain * (y - xK)
    pFilt = (1 - kGain) * pK

    return xFilt, pFilt


def CreateFilteredData(measData, initPredVar, sigmaW, sigmaV):

    length = len(measData)
    filteredMeasData = np.zeros(length + 1)
    filteredPredVar = np.zeros(length + 1)
    filteredPredVar[0] = initPredVar

    for i in range(0, length):
        xFilt, pFilt = Kalman(measData[i], filteredMeasData[i], filteredPredVar[i], sigmaW, sigmaV)
        filteredMeasData[i + 1] = xFilt
        filteredPredVar[i + 1] = pFilt

    return np.delete(filteredMeasData, 0), np.delete(filteredPredVar, 0)

def CreateSampleData(height, var, size):

    indexes = [idx for idx in range(0, size)]
    heightListWithNoise = np.random.normal(height, var, size)

    return indexes, heightListWithNoise

if __name__ == '__main__':
    print("Sample Kalman Filter started.")

    height = 1.0
    var = 0.5
    size = 100

    indexes, data = CreateSampleData(height, var, size)
    filteredData, filteredVars = CreateFilteredData(data, 10000, 100, 10000)

    plt.plot(indexes, data)
    plt.plot(indexes, filteredData)
    #plt.plot(indexes, filteredVars)
    plt.show()

上記コードを実行した結果ですが,下記のようになりました.

f:id:rkoichi2001:20170826225002p:plain

縦軸がステレオの視差ベースで求めた高さのイメージです.横軸は単純にサイクルカウントです.図を見てわかるように,青の値が純粋な入力値であり,ノイズの影響でだいぶブレブレになってますが,フィルタをかけたあとの緑の値は 1m 付近をあるていどキープしているように見えます.ということで,取りあえずカルマンフィルタを使って高さ方向のノイズ除去をし,その値を用いてマップ作成をしてみます!結果はできれば明日にアップします!

お盆休みを終えて....

前回のブログ更新からちょうど一カ月...
さぼっていたわけでなく,割と作業をしてたんですが,,,,,行き詰まりました.

ROSのパッケージをなるべく使いまわすべく,下記を目標にやってました.

1. ステレオの結果をつかって, Local Map を生成.この Local Map に対して,仮想的なレーザースキャンを生成する.
2. 視差画像を鳥瞰図変換して,このLocalMapに対して仮想的なレーザースキャンを生成する.

が,やっぱり既存の amcl, gslam mapping をほとんど変更なしで使うのは無理そうです.
ステレオカメラと2Dレーザースキャナの長所と短所が完全に正反対で,ステレオカメラをレーザースキャンに変更した時点でステレオの長所を全部殺してしまってるみたいです.
ひょっとしたら gslam mapping のパラメータを変更して微調整をすればうまくいくかもしれないのですが,どうも筋が悪そうな気がしてたまらなくなってきたので別の攻め方で行くことにしました.

ステレオカメラ:
長所:画像情報なので,情報リッチ.3次元情報が取得できる.
短所:検知物体がカメラから離れるにしたがって,距離精度が距離の4乗(2乗?)に反比例して下がる.視野角が狭い(レーザスキャナに比べると).

2Dレーザスキャナ:
長所:検知物体からの距離が大きくなっても,ステレオほど急激に精度が劣化しない.視野角が広い.
短所:2Dスキャンなので,情報が限られている.一定の高さ以上の物体しかみつけられない.

下記,ちょっと自分の勉強もかねて整理したので備忘録代わりに残しておきます.

f:id:rkoichi2001:20170826124844p:plain

上記,ステレオの原理を簡単に図示したんですが,「左カメラ」「右カメラ」って書いてある下の長方形がカメラのイメージャー(画像素子)です.
実際には焦点の後ろに撮像素子があるのですが,前に書いても説明は成り立つので簡単のためにそう書いてます.

ステレオカメラで距離を取得するまでのステップを簡単に書くと,,

1. それぞれのカメラ(左カメラ,右カメラ)でほぼ同じ景色を撮影.

左カメラと右カメラで景色を取るわけですが,二つのカメラの距離が微妙に違う(横方向に20cm位)ので,景色の見え方が微妙に違います.

2. 左カメラで撮影した映像が,右カメラで見るとどの位置に来るかマッチングする.

例えば,湘南の海岸に行って映像を撮影したとして,左カメラで写した映像では左上から数えて横800pix,縦400pixのところにきれいなビキニのおねーちゃんが写っていたとして,
右カメラで撮った映像で同じきれいなおねーちゃんを探していくと,横600pix,縦400pixのとことかにあるわけです.
男がやると,このビキニのおねーちゃんの位置探しは絶対間違えないと思いますが,実際には題材はビキニのおねーちゃんではないことと(残念!),
めっちゃ早く(一秒間に10枚の映像ペア)対応するところを見つけないといけないので,この「左画像と右画像の対応点探し」をするアルゴリズムが必要になります.
これが世に言われる「ステレオマッチング」という問題で,この対応点をいかに正確に早く探せるかという研究が結構さかんに行われていました.
僕の場合は,「SGBM」というアルゴリズムを使って対応点を探してます.

3. 2でマッチングを取ることによって,左カメラ・右カメラに移っている同じ被写体の画像位置がわかるので,あとは上記の三角形の相似から距離を求める.

2のステップで対応点を見つけたら,あとは上記のスライドに書いてある三角形の相似の関係から,対応点までの距離がわかります.
ちなみに,スライドで書いてあるそれぞれの数字は,,,

B:左カメラ・右カメラの距離
cx, cy : カメラの主点座標
disp : 左カメラ・右カメラで写っているx座標の位置の差

です.で,ここで disp を使って距離を求めているわけですが,disp はピクセル座標の差なので,基本的に整数になります.
ビキニのおねーちゃんがあまりに遠くにいてしまって,左右の映像で写っている位置が同じになってしまうと disp = 0 になってしまってもはや距離を求めるすべがなくなってしまいます.
また,おねーちゃんの距離がカメラから離れるにしたがって,映像のなかのおねーちゃんのサイズも小さくなってしまうので,disp もどんどん小さくなってしまいます.
つまり(ちょっと強引ですが),,,,被写体が離れるにしたがって,Zの値があんまり正確ではなくなってしまいます.

で,カメラから離れるにしたがって,1mの正方形がどのくらい小さくなってしまうのかを考えてみたんですが,

f:id:rkoichi2001:20170826135823p:plain

20m離れた地点で 1m x 1m の正方形の面積が 34pix x 34 pix になりました.で,これがカメラからの距離によって被写体が小さく映ってしまう影響で,ステレオの場合はもう一つ
カメラから離れるにしたがって,距離の精度が下がってしまうという影響があります.下記,視差と距離の関係ですが,視差が大きい時(カメラの近くに被写体がいる)場合,1pixマッチングが
ずれても誤差は数センチなんですが,視差が小さいとき(カメラの遠くに被写体がいる)場合,1pixのマッチング誤差が数mの誤差を生んでしまいます.
(下の図の例でいうと,視差 54pix のところを間違えて 53pix と判断しても誤差は 2cm ですが,視差 4pix のところを 3pix と判断すると誤差は 7m 以上になります.)

f:id:rkoichi2001:20170826140631p:plain

ということで,ステレオにあった進め方をする必要があり...いま考え中です...ちょっと書くのが疲れたので,また新しいポストを上げます.

ROS Nodelet, Noise Remover, Gmapping....

おはようございます.一週間更新が滞ってしまいましたが,いろいろとやってました.
先週やったこととしては,,,,

1. ROS の Nodelet 化
2. SGBM の視差マップノイズ除去部品の作成
3. depthimage_to_laserscan と gmapping を使った地図の作成

1. ROS の Nodelet 化

 まず,ROS の Nodelet 化ですが,今まで一つ一つの部品は ROSの Node として作成していたのですが,Nodeをバンバン作ってプロセスとして立ち上げると,特に画像処理回りはノード間の通信量が大きくなりすぎてしまってPCがいっぱいいっぱいになってしまうので,今までプロセスとして立ち上げていたNodeをスレッドとして立ち上げてメモリ空間を共有するようにしました.(と書くと,僕が難しいことを成し遂げたみたいですが,Nodeletを使ったので自分は大したことはしてませんが(笑))
 ただ,Nodeletを使うのが初めてだったのでちょっとはまりました.

2. SGBM の視差マップノイズ除去部品の作成

 SGBMの視差マップですが,前回のエントリで書いた通り視差画像にはどうしてもノイズが入ってしまうので,これを除去するための部品を作りました.ノイズ除去するうえで用いた仮定は,,,
「視差は画面下から上に向かうにつれて,必ず小さくなっていく.」
「地表を最低面とする」
 結局,環境の三次元構造をとらえる必要はなく,地面からのモノの生え際がわかればいいのでこれでもいいかと.結果としては,下記のようになりました.写真を見てもわかるとおり,木の構造が地面から生える一本の柱みたいになってます.

ノイズ除去前の視差画像
f:id:rkoichi2001:20170726082530p:plain

ノイズ除去後の視差画像
f:id:rkoichi2001:20170726082613p:plain

3. depthimage_to_laserscan と gmapping を使った地図の作成

 で,最もやりたかった gmapping を使った Map の作成ですが,結果的には全然ダメでした(笑)やっぱりレーザースキャナとステレオ視差だとモデルが全然異なるので,もっと工夫しないと成立しないですね.自分が使っているステレオカメラの視野角がかなり広いので,10m位離れてしまうと距離の精度が50-100cm位になってしまって,全然きれいなマップができませんでした.

f:id:rkoichi2001:20170726083248p:plain
f:id:rkoichi2001:20170726084214p:plain

 うーん.やっぱりなかなかストレートにはいかないですね...ただ,SLAMのコードを自作するのはつらいので,何とかステレオからレーザースキャナの出力を生成する方法を考えてみます.今思いついているものとしては,

1. ステレオの結果をつかって, Local Map を生成.この Local Map に対して,仮想的なレーザースキャンを生成する.
2. 視差画像を鳥瞰図変換して,このLocalMapに対して仮想的なレーザースキャンを生成する.

 ということで,今週は LocalMap2LaserScan の作成に取り組みます.ということで,仕事行ってきます!