お買い物2

ということで,回転行列のエントリがやっと書き終わったので,本当に書きたかったエントリを書きます.
表題の件,,,ことし最大の買い物をしてしまいました.30台中ごろの年齢で最大の買い物とかっていうと,おそらく「車」とか「家」とかってのが普通の回答なんでしょうが,オタクは違います!ええ.パソコン買っちゃいました(笑)

具体的には「沖縄三次元復元プロジェクト」に向けて,高性能PCとデジタル一眼レフを買ったのですが,60万以上使ってしまいました(笑)こんなアグレッシブなボーナスの使い方をしたのが初めてなのでAmazonクリックする手が震えましたが,本日頼んでいたすべてのものが届いてテンション高めです.来月10日のカード引き落とし日に後悔するのかもしれませんが.

ちなみに,今回初めて自作でパソコンを作ることにしました.
画像処理用の高性能PCが欲しかったので,だいぶ高くつきましたが,末永く(使えるものは)使っていきたいと思います.

1.PCケース

Corsair Carbite Air 540 E-ATX対応キューブPCケース CS5326 CC-9011030-WW

Corsair Carbite Air 540 E-ATX対応キューブPCケース CS5326 CC-9011030-WW

2.CPU

3.CPUクーラー

Corsair H100i V2 水冷一体型CPUクーラー FN1021 CW-9060025-WW

Corsair H100i V2 水冷一体型CPUクーラー FN1021 CW-9060025-WW

4.メモリ x 2

5.マザーボード

6.SSD

7.SSDラック

CREMAX HDD&SSDx4台搭載可能モジュールラック ホットスワップ

CREMAX HDD&SSDx4台搭載可能モジュールラック ホットスワップ

8.グラフィックボード

9.電源

10.BDドライブ

ということで,パソコンだけで45万しました...「マザーボードと電源,アンバランスにいいやつ買いすぎだろ!」って突っ込まれたんですが,まあ確かにそうかもですね...ただ,このマザーボードと電源なら,夢のグラフィックボード4枚搭載も可能でして,拡張性を優先して買った次第でございます.まあグラフィックボードあと三枚かったらそれだけで25万くらいかかってしまいますが,,,,この辺は実際に使っていく中で様子を見れればと思います.

で,

三次元復元に欠かせないもう一つの必須アイテム「カメラ」なんですが,個人的に写真はそんなに趣味じゃないんですが画像処理を極める!という目標的にも,ひとつそこそこいいカメラを持っておいてもいいかなと思い,初めてデジタル一眼レフを買いました.

生活を支えていただいている偉大なスポンサーP社製のカメラをもちろん買おうとしたのですが,どうしてもフルサイズのイメージャーに惹かれてしまい,,,すみません.浮気しました....

下記,ついついうれしくなって届いたものを並べてみました.
物欲強くないタイプだと思ってたんですが,ぜんぜんそんなことなかったですね(笑)

f:id:rkoichi2001:20171119033852j:plain


ということで,明日はPCくみ上げのエントリを書きます.
おやすみなさい....

オイラー角,固定角,回転行列...

仕事で回転行列でドはまりし,数日悩みました....
結局いろいろと教えてもらって解決したのですが,「沖縄三次元復元プロジェクト」にも大いに関係するのでちょうどいい機会と思い,座標変換をきちんと勉強して整理することにしました.

1.回転の表現方法

回転の表現方法には,固定角,オイラー角,クオータニオン等,いろいろとありますが,ここではひとまず固定角とオイラー角について考えます.

固定角

座標Aを回転変換させて,座標Bに変換することを考えた場合に,座標Aの各軸(固定)について回転することを前提に角度表現する方法です.
つまり,固定角表現で「X軸周りに30度,Y軸周りに45度,Z軸周りに90度」といった場合は,座標Aの各軸周りに回転させることになります.

オイラー

座標Aを回転変換させて,座標Bに変換することを考えた場合に,都度都度回転させた座標系の軸に関しての回転で角度表現する方法です.
つまり,オイラー角表現で「X軸周りに30度,Y軸周りに45度,Z軸周りに90度」といった場合は,まず座標AのX軸周りに30度回転,そのあと回転させた座標系A´のY軸に関して45度回転,そのあと回転させた座標系A´´のZ軸に関して90度回転という形になります.

簡単に二つの回転の表現方法を図示してみました.ちなみに,どちらの回転方法でも「どの順番でどの軸周りに回転させるか」で結果が変わってきます.下記の図では,固定角の場合にはZ軸,Y軸,X軸の順番に回転,オイラー角の場合はX軸,Y1軸,Z2軸の順番に回転させています.

f:id:rkoichi2001:20171119004406p:plain

座標系の変換をするときに,回転行列を連鎖させて変換をすると思います.例えば,座標系Aから座標系Bに変換し,座標系Bから座標系Cに変換したいとすると,
R_A2C = R_B2C * R_A2B
となります.自分の場合は座標間の相対角度があらかじめ分かっていて,そこからこの R_A2B を作る必要があったのですが,ここで固定角とオイラー角がごちゃごちゃになってしまってはまってしまいました.
固定角で考えても,オイラー角で考えても正しく変換すれば回転行列は一意に求まります.正しく変換すればですが...

2.固定角・オイラー角の関係

で,ここではハマった状況を考えます.
座標系AからBへの回転行列が求めたいのだけど,

A. 固定角表現での角度が分かっている.

この場合,「座標系AからBの変換は,X->Y->Zの順番に,固定角表現で30度,45度,90度回せば変換できるよ!」といわれれば回転行列が一意に求まります.

固定角表現が与えられた場合の回転行列(A->B)の求め方
RotX = \begin{pmatrix} 1&0&0 \\ 0&cos(\theta1)&-sin(\theta1) \\ 0&sin(\theta1)&cos(\theta1) \end{pmatrix}
RotY = \begin{pmatrix} cos(\theta2)&0&sin(\theta2) \\ 0&1&0 \\ -sin(\theta2)&0&cos(\theta2) \end{pmatrix}
RotZ = \begin{pmatrix} cos(\theta3)&-sin(\theta3)&0 \\ sin(\theta3)&cos(\theta3)&0  \\ 0&0&1 \end{pmatrix}
R_Fixed = RotZ * RotY * RotX

B. オイラー角表現での角度が分かっている.

この場合,「座標系AからBの変換は,X->Y1->Z2の順番に,オイラー角表現で30度,45度,90度回せば変換できるよ!」といわれれば回転行列が一意に求まります.
(このケースでは,上記の固定角表現と順番も角度も同じですが,当然ながら違う変換を表します.)

オイラー角表現で与えられた場合の回転行列(A->B)の求め方
RotX = \begin{pmatrix} 1&0&0 \\ 0&cos(\theta1)&-sin(\theta1) \\ 0&sin(\theta1)&cos(\theta1) \end{pmatrix}
RotY = \begin{pmatrix} cos(\theta2)&0&sin(\theta2) \\ 0&1&0 \\ -sin(\theta2)&0&cos(\theta2) \end{pmatrix}
RotZ = \begin{pmatrix} cos(\theta3)&-sin(\theta3)&0 \\ sin(\theta3)&cos(\theta3)&0  \\ 0&0&1 \end{pmatrix}

R_Euler = RotX * RotY * RotZ (固定角の場合と比べて,XとZの順番が入れ替わっていることに注目!)

C. 固定角とオイラー角の関係.

ここで,固定角とオイラー角の関係ですが,まず固定角でX->Y->Zの順番にx度,y度,z度回転させたときの回転行列を考えてみると,
R_Fixed = Rz(z) * Ry(y) * Rx(x)
となります.こいつと同じ回転行列をオイラー角表現であらわすと,Z->Y->Xの順番に z度,y度,x度回転させると,,,
R_Euler = Rz(z) * Ry(y) * Rx(x)
となります.つまり,固定角とオイラー角の関係は回転させる順番が逆であることだけだということがわかります.

D. 実例

そろそろ書いてる本人も混乱してきたので,簡単な実例を考えます.
下記の例,座標系AからBへの変換行列を考える際に,固定角で考えた場合とオイラー角で考えた場合を書いてますが,計算の順序を上記のようにすると最終結果も一致していることがわかります.

f:id:rkoichi2001:20171119022238p:plain

3.各軸周りの回転角度を求めるスクリプト

ということで,「2つの座標間の固定角が与えられたときの回転行列」,「2つの座標間のオイラー角が与えられたときの回転行列」,「回転行列が与えられたときの固定角」,「回転行列が与えられたときのオイラー角」を求めるスクリプトを作りました.ちょっと完全にテストできているわけではないので,実際に使うときは恐る恐る使ってください.ちなみに,どの順番で回転させるかで結果も変わってきますが,ひとまず xyz と zyx に関して作ってみました.ほかの順番でやりたいときは,”ShowRotMatInSymbol” でシンボルを表示してもらって,同じノリで方程式を解いていけばできると思います.

# -*- coding: utf-8 -*-

from sympy import *
from sympy import Matrix
import numpy as np
import math

class RotationMatrixConverter:
    
    def __init__(self):
        self.__thresh = 0.00001
    
    def ShowRotMatInSymbol(self, order):
        
        c1 = Symbol("c1")
        s1 = Symbol("s1")
        c2 = Symbol("c2")
        s2 = Symbol("s2")
        c3 = Symbol("c3")
        s3 = Symbol("s3")
        
        Rx = Matrix([[1, 0, 0], [0, c1, -s1], [0, s1, c1]])
        Ry = Matrix([[c2, 0, s2], [0, 1, 0], [-s2, 0, c2]])
        Rz = Matrix([[c3, -s3, 0], [s3, c3, 0], [0, 0, 1]])
        
        if order is "xyz":
            return (Rz * Ry * Rx)
        elif order is "xzy":
            return (Ry * Rz * Rx)
        elif order is "yxz":
            return (Rz * Rx * Ry)            
        elif order is "yzx":
            return (Rx * Rz * Ry)            
        elif order is "zxy":
            return (Ry * Rx * Rz)           
        elif order is "zyx":
            return (Rx * Ry * Rz)            
    
    def Euler2Rot(self, angleXYZ, order):

        RotX = np.matrix([[1, 0, 0], [0, math.cos(angleXYZ[0]), -math.sin(angleXYZ[0])], [0, math.sin(angleXYZ[0]), math.cos(angleXYZ[0])]])
        RotY = np.matrix([[math.cos(angleXYZ[1]), 0, math.sin(angleXYZ[1])], [0, 1, 0], [-math.sin(angleXYZ[1]), 0, math.cos(angleXYZ[1])]])
        RotZ = np.matrix([[math.cos(angleXYZ[2]), -math.sin(angleXYZ[2]), 0], [math.sin(angleXYZ[2]), math.cos(angleXYZ[2]), 0], [0, 0, 1]])
        
        if order is "xyz":
            return RotX * RotY * RotZ
        elif order is "zyx":
            return RotZ * RotY * RotX
        else:
            print("Specified order is not supported!")
            
    def Fixed2Rot(self, angleXYZ, order):

        RotX = np.matrix([[1, 0, 0], [0, math.cos(angleXYZ[0]), -math.sin(angleXYZ[0])], [0, math.sin(angleXYZ[0]), math.cos(angleXYZ[0])]])
        RotY = np.matrix([[math.cos(angleXYZ[1]), 0, math.sin(angleXYZ[1])], [0, 1, 0], [-math.sin(angleXYZ[1]), 0, math.cos(angleXYZ[1])]])
        RotZ = np.matrix([[math.cos(angleXYZ[2]), -math.sin(angleXYZ[2]), 0], [math.sin(angleXYZ[2]), math.cos(angleXYZ[2]), 0], [0, 0, 1]])
        
        if order is "xyz":
            return RotZ * RotY * RotX
        elif order is "zyx":
            return RotX * RotY * RotZ
        else:
            print("Specified order is not supported!")
    
    def Rot2Fixed(self, R, order):
        
        if order is "xyz":
            fixed = self.__Rot2Fixed_xyz(R)
        elif order is "zyx":
            fixed = self.__Rot2Fixed_zyx(R)
        else:
            print("Specified order is not supported!")
            
        return fixed
    
    def Rot2Euler(self, REuler, order):
    
        if order is "xyz":
            euler = self.__Rot2Fixed_zyx(REuler)
        elif order is "zyx":
            euler = self.__Rot2Fixed_xyz(REuler)
        else:
            print("Specified order is not supported!")
            
        return euler

    def __Rot2Fixed_xyz(self, R):
        
        if (abs(R[2, 0] - 1) > self.__thresh and abs(R[2, 0] + 1) > self.__thresh):
            
            theta2_0 = -math.asin(R[2, 0])
            theta2_1 = math.pi - theta2_0
            theta1_0 = math.atan2(R[2, 1] / math.cos(theta2_0), R[2, 2] / math.cos(theta2_0))
            theta1_1 = math.atan2(R[2, 1] / math.cos(theta2_1), R[2, 2] / math.cos(theta2_1))
            theta3_0 = math.atan2(R[1, 0] / math.cos(theta2_0), R[0, 0] / math.cos(theta2_0))
            theta3_1 = math.atan2(R[1, 0] / math.cos(theta2_1), R[0, 0] / math.cos(theta2_1))    
            FixedResXYZ = np.array([[theta1_0, theta2_0, theta3_0], [theta1_1, theta2_1, theta3_1]])
            
        else:
            theta3 = 0
            if (abs(R[2, 0] + 1) < self.__thresh):
                theta2 = math.pi / 2
                theta1 = theta3 + math.atan2(R[0, 1], R[0, 2])
            else:
                theta2 = -math.pi / 2
                theta1 = -theta3 + math.atan2(-R[0, 1], -R[0, 2])
    
            FixedResXYZ = np.array([theta1, theta2, theta3])
    
        return FixedResXYZ

    def __Rot2Fixed_zyx(self, R):

        if (abs(R[0, 2] - 1) > self.__thresh and abs(R[0, 2] + 1) > self.__thresh):
            
            theta2_0 = math.asin(R[0, 2])
            theta2_1 = math.pi - theta2_0
            theta1_0 = math.atan2(-R[1, 2] / math.cos(theta2_0), R[2, 2] / math.cos(theta2_0))
            theta1_1 = math.atan2(-R[1, 2] / math.cos(theta2_1), R[2, 2] / math.cos(theta2_1))
            theta3_0 = math.atan2(-R[0, 1] / math.cos(theta2_0), R[0, 0] / math.cos(theta2_0))
            theta3_1 = math.atan2(-R[0, 1] / math.cos(theta2_1), R[0, 0] / math.cos(theta2_1))    
            FixedResZYX = np.array([[theta1_0, theta2_0, theta3_0], [theta1_1, theta2_1, theta3_1]])
            
        else:
            theta3 = 0
            if (abs(R[0, 2] - 1) < self.__thresh):
                theta2 = math.pi / 2
                theta1 = -theta3 + math.atan2(R[2, 1], R[1, 1])
            else:
                theta2 = -math.pi / 2
                theta1 = theta3 + math.atan2(R[2, 1], R[1, 1])
    
            FixedResZYX = np.array([theta1, theta2, theta3])
    
        return FixedResZYX


if __name__ == "__main__":
    
    converter = RotationMatrixConverter()
    
    euler = np.array([45, 90, 135]) / 180 * math.pi
    
    # Rotation XYZ
    RotXYZ = converter.Euler2Rot(euler, "xyz")
    eulerRes = converter.Rot2Euler(RotXYZ, "xyz")
    print("Result XYZ")
    print("Rotation Mat : ")
    print(RotXYZ)
    print("Euler Angles : ", eulerRes / math.pi * 180)
    print(converter.Euler2Rot(eulerRes, "xyz"))
    print("")
    
    # Rotation ZYX
    RotZYX = converter.Euler2Rot(euler, "zyx")
    eulerRes = converter.Rot2Euler(RotZYX, "zyx")
    print("Result ZYX")
    print("Rotation Mat : ")
    print(RotZYX)
    print(eulerRes / math.pi * 180)
    print(converter.Euler2Rot(eulerRes, "zyx"))
    print("")

        
    fixed = np.array([180, 0, -90]) / 180 * math.pi
    RotXYZ = converter.Fixed2Rot(fixed, "xyz")
    print("Rotation Mat : ")
    print(RotXYZ)
    eulerRes = converter.Rot2Euler(RotXYZ, "xyz")
    
    print("")
    print("Result Euler XYZ : ")
    print(eulerRes / math.pi * 180)
    

イノベーション

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

www.youtube.com

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

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

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

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

つくばチャレンジ2017を終えて...

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

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

f:id:rkoichi2001:20171112201647p:plain

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

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

f:id:rkoichi2001:20171112205728p:plain
システム構成

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

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

www.youtube.com

12月中旬に仙台でSI2017の発表会に出て今年のつくばチャレンジ関連イベントは終わりになりますが,来年も応援よろしくお願いします!!

f:id:rkoichi2001:20171112221656j:plain

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





















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

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

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

思い起こせば,2017年は自己位置推定との戦いでした..

1.SWEEP(格安の2Dレーザスキャナ)を購入して,ROSのAMCLを用いてマップ作製・自己位置推定
―>屋外使用でスキャナのレンジが3mくらいしかなく,撃沈.


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

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

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

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

www.youtube.com

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

f:id:rkoichi2001:20171110063138p:plain

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

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化のとこを別のエントリでアップします.