SI2017に参加してきました．

年末は仕事でバタバタしてしまいなかなか更新できていませんでしたが，ようやく冬休みに入ったということでたまっていた分をガンガン更新したいと思います．

昨年（札幌），一昨年（名古屋）に引き続き，今年度もSI2017に参加してきました．今年は仙台開催でした．
相変わらず素朴な発表であることは変わりませんでしたが，初めてちょっと中身のある発表ができたかと思います．
2017年はつくばチャレンジのステージ2の最終年で，来年以降コースや課題が変わるのですが，そのワーキンググループにも少し参加させていただき，若干コミュニティに入れたのでは？？
という感じがします．ハッピーです（笑）

Si2017 チームイエスマン 発表スライド from Robotics Engineer

www.slideshare.net

ということで，スライドの最終ページにも書いてますが，

来年は完走します！！

仙台は大学受験滑って以来，実に10年ぶりでしたが，相変わらずきれいな街でした．
観光スポットはそれほど多くないですが，町も整備されているし，食べ物もおいしいしで，素敵でした．
下記は仙台駅の写真ですが，これ，三次元復元してみました！詳しくは次のエントリで書きます．

CMake 変数全表示

ミニ投稿で恐縮ですが，，，CMake使ってて，変数の設定がどうなっているか，どの変数がCMakeLists.txtで使えそうかって結構調べたい時ありますよね？下記のコード片使えば，全部表示できるみたいです．

get_cmake_property(_variableNames VARIABLES)
foreach (_variableName ${_variableNames})
    message(STATUS "${_variableName}=${${_variableName}}")
endforeach()

Doxygen, GraphViz の導入

SFMのサンプルコードを理解するにあたり，コードを読んでノートにメモって．．という作業をしようとしてたんですが，関数のコールグラフくらいは既存のツールで作れないかな？と思い，DoxygenとGraphVizを使ってみることにしました．下記，使い方です．ただ，どうも自分が使いこなせていないためか，おもったほど便利じゃないですね．．．

インストール

$ sudo apt-get update
$ sudo apt-get install doxygen
$ sudo apt-get install graphviz

ドキュメント生成

1. ソースがあるフォルダへ移動し，doxygen コマンドを実行します．

$ cd workspace/reps/mastering_opencv/Chapter4_StructureFromMotion/
$ doxygen -g

2. 生成された設定ファイル "Doxyfile" を必要に応じて変更します．自分の場合は下記を変更しました．

EXTRACT_ALL = YES
EXTRACT_PRIVATE = YES
EXTRACT_STATIC = YES
RECURSIVE = YES
CALL_GRAPH = YES
CALLER_GRAPH = YES
DOT_MULTI_TARGETS = YES

3. ファイル生成

$ doxygen Doxyfile

上記のステップでコールグラフが生成されました．SFM処理のメイン関数のコールグラフを添付します．

うーん．やっぱり銀の弾丸ではないですね．

Structure From Motion 事始め

PCも組み終わり，SFMの勉強に入っていきます．
まずは理論の勉強からですが，いきなり数式とにらめっこするのはつらいので，サンプルのアプリを落としてそのコードと教科書を往復して勉強をすることにしました．

題材

１．教科書
この本，ググるとPDFでまんま落ちてます．そのリンクを張るのはさすがに怖かったのでアマゾンリンクにしましたが．．．この教科書の４章にSFMの概要とサンプルコードに対しての説明が書かれてます．

Mastering OpenCV With Practical Computer Vision Projects: Step-by-step Tutorials to Solve Common Real-world Computer Vision Problems for Desktop or Mobile, from Augmented Reality and Number Plate Recognition

２．サンプルコード
上記教科書にはチャプター毎にサンプルコードがあるようなのですが，このコードもGITに落ちてます．自分は４章のSFMを使います．
github.com

サンプルコードコンパイルまで

コマンド開始時点のフォルダははすべて workspace というフォルダのイメージで書いてます．

１．サンプルコードを落とします．

mkdir reps/mastering_opencv/build
cd reps/mastering_opencv
git clone https://github.com/MasteringOpenCV/code.git

２．OpenCV2.4.11を落とします．

上記サンプルコードは OpenCV3 系には未対応ということで，2.4.11を落としてコンパイルします．
gstreamer 周りのコンパイルが通らなかったので，ひとまずOFFでコンパイルしました．

mkdir reps/opencv-2.4.11 reps/opencv-2.4.11/install reps/opencv-2.4.11/installd reps/opencv-2.4.11/build
cd reps/opencv-2.4.11
git clone https://github.com/opencv/opencv.git
cd opencv
git checkout 2.4.11
cmake -DCMAKE_INSTALL_PREFIX=../install -DBUILD_EXAMPLES=ON -DINSTALL_C_EXAMPLES=ON -DWITH_OPENGL=ON -DWITH_V4L=ON -DWITH_GSTREAMER=OFF -DWITH_QT=ON -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DWITH_CUDA=YES -DCUDA_GENERATION=Auto ../opencv
make install -j8
cmake -DCMAKE_INSTALL_PREFIX=../installd -DBUILD_EXAMPLES=ON -DINSTALL_C_EXAMPLES=ON -DWITH_OPENGL=ON -DWITH_V4L=ON -DWITH_GSTREAMER=OFF -DWITH_QT=ON -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Debug -DWITH_CUDA=YES -DCUDA_GENERATION=Auto ../opencv
make install -j8

３．SSBAを落とします．

サンプルコード内でバンドル調整をしているみたいのなので，必要なコードを落とします．

mkdir reps/SSBA/build
cd reps/SSBA
git clone https://github.com/chzach/SSBA.git
cd build
cmake -DCMAKE_BUILD_TYPE=Release ../SSBA-4.0/

4．Mastering OpenCV のサンプルコードを一部改変します．

これは自分の環境依存のせいかもしれないですが，そのままコンパイルが通らなかったので，一部 CMakeLists.txt とコードを改変しました．
1. CMakeLists.txt の変更
SSBA_LIBRARY_DIR, SSBA_INCLUDE_DIR, OpenCV_DIRを自分がビルド／インストールしたとこにポイントします．
自分の場合は，，，，

set(OpenCV_DIR "${CMAKE_SOURCE_DIR}/../../opencv-2.4.11/install/share/OpenCV")

set(SSBA_LIBRARY_DIR "${CMAKE_SOURCE_DIR}/../../SSBA/build")
include_directories(${SSBA_LIBRARY_DIR}/../SSBA-4.0)

2. GPUSURFFeatureMatcher.h の変更
SURF 特徴量が nonfree パッケージに入っているので，下記の include を追記します．

#include <opencv2/nonfree/gpu.hpp>

3. Visualization.cpp
PCL, ROS メッセージ変換のメソッドのヘッダファイルのパスが異なっていたので，追記します．

#include <pcl/ros/conversions.h>

4. graphcuts.cpp
Cuda8.0 からはGraphCutsがサポートされていないらしく，下記の記述を追記します．

#if !defined (HAVE_CUDA) || defined (CUDA_DISABLER)

から

#if !defined (HAVE_CUDA) || defined (CUDA_DISABLER)  || (CUDART_VERSION >= 8000)

に変更します．

５．Mastering OpenCV のサンプルコードをビルドします．

cd reps/mastering_opencv/build
cmake -G "Eclipse CDT4 - Unix Makefiles" -DCMAKE_BUILD_TYPE:STRING="Release" ../Chapter4_StructureFromMotion
make -j8

と，ここまでくればひとまず eclipse でコードを見ながら実行できるようになります．
次のエントリでは中身を見ていきます．

自作PCくみ上げ

今日から１２月ですね．つくばチャレンジが終わったら，今度は仕事がバタバタしてきて，少し更新が滞ってしまいました．
が，PC組み終わりました．OSのインストールでちょっとはまってしまい，先週末をつぶしてしまいましたが，，，，

PCを自作したのは初めてだったのですが，くみ上げ自体は思いのほかすんなり終わりました．
一つ一つ写真を撮ってブログに上げようと思ったのですが，ちょっと時間がかかりすぎてそれは断念です．

くみ上げ後．
（最近のPCって，マザーボード光るんですね！ちょっとテンション上がりました．）

ということで，いよいよSFMをやる準備が整いました！今週からはSFMの記事をアップしていきます！

お買い物２

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

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

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

１．PCケース

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

２．CPU

Intel CPU Core i7-7820X 3.6GHz 11Mキャッシュ 8コア/16スレッド LGA2066 BX80673I77820X 【BOX】

３．CPUクーラー

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

４．メモリ x 2

CORSAIR DDR4 デスクトップPC用 メモリモジュール VENGEANCE LPX Series 16GB×2枚キット CMK32GX4M2A2666C16

５．マザーボード

ASUS Intel X299搭載 マザーボード LGA2066対応 ROG RAMPAGE VI EXTREME【EATX 】

６．SSD

Crucial [Micron製] 内蔵SSD 2.5インチ MX300 2TB ( 3D TLC NAND / SATA 6Gbps / 3年保証 )国内正規品 CT2050MX300SSD1/JP

７．SSDラック

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

８．グラフィックボード

GIGABYTE ビデオカード NVIDIA GeForce GTX 1080搭載 オーバークロック ゲーミングモデル GV-N1080G1 GAMING-8GD

９．電源

Corsair AXi Series AX1500i 1500 Watt (1500W) Fully Modular Digital Power Supply 80+ Titanium Certified [並行輸入品]

１０．BDドライブ

Pioneer パイオニア Windows10対応 BD-R 16倍速書込 S-ATA接続 ブラックトレー仕様 BD/DVD/CDライター ソフト無 バルク品 BDR-209BK2

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

で，

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

ソニー SONY ミラーレス一眼 α7 II ズームレンズキット FE 28-70mm F3.5-5.6 OSS ILCE-7M2K

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

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

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

オイラー角，固定角，回転行列．．．

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

１．回転の表現方法

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

固定角

座標Aを回転変換させて，座標Bに変換することを考えた場合に，座標Aの各軸（固定）について回転することを前提に角度表現する方法です．
つまり，固定角表現で「X軸周りに３０度，Y軸周りに４５度，Z軸周りに９０度」といった場合は，座標Aの各軸周りに回転させることになります．

オイラー角

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

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

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

２．固定角・オイラー角の関係

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

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

この場合，「座標系AからBの変換は，X->Y->Zの順番に，固定角表現で３０度，４５度，９０度回せば変換できるよ！」といわれれば回転行列が一意に求まります．

固定角表現が与えられた場合の回転行列（A->B）の求め方



R_Fixed = RotZ * RotY * RotX

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

この場合，「座標系AからBの変換は，X->Y1->Z2の順番に，オイラー角表現で３０度，４５度，９０度回せば変換できるよ！」といわれれば回転行列が一意に求まります．
（このケースでは，上記の固定角表現と順番も角度も同じですが，当然ながら違う変換を表します．）

オイラー角表現で与えられた場合の回転行列（A->B）の求め方

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への変換行列を考える際に，固定角で考えた場合とオイラー角で考えた場合を書いてますが，計算の順序を上記のようにすると最終結果も一致していることがわかります．

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

ということで，「２つの座標間の固定角が与えられたときの回転行列」，「２つの座標間のオイラー角が与えられたときの回転行列」，「回転行列が与えられたときの固定角」，「回転行列が与えられたときのオイラー角」を求めるスクリプトを作りました．ちょっと完全にテストできているわけではないので，実際に使うときは恐る恐る使ってください．ちなみに，どの順番で回転させるかで結果も変わってきますが，ひとまず 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)