Visual Studio Code を使った C++ のビルド&デバッグ方法(with CMAKE)

前回のエントリでC++のファイルをビルド・デバッグする方法を扱いました.自分で一からファイルを作って,作ったファイルをすべて g++ の引数に渡してビルドして....ということもあるかもですが,大体は規模が大きくなってくると CMake を使うと思います.このエントリでは,VSCODE と CMake を連携させて使う方法です.

1.インストール方法

まず,CMake と連携させて VSCODE を使うには,Extensionをインストールする必要があります.

f:id:rkoichi2001:20181117015620p:plain

インストールするのは,下記の2つです.
f:id:rkoichi2001:20181117015940p:plain

2.サンプルプロジェクトの作成

次に,サンプルプロジェクトのためのワークスペースを作ります.
f:id:rkoichi2001:20181117020318p:plain

f:id:rkoichi2001:20181117020354p:plain

次に,サンプルのプロジェクトを作成します.Ctrl + Shift + P でコマンドパレットを開き,CMakeと入力すると CMake 関連のコマンドがいっぱい出てきます.
f:id:rkoichi2001:20181117020527p:plain

次に,サンプルプロジェクトの名前を指定します.
f:id:rkoichi2001:20181117020858p:plain

最後に,該当のプロジェクトが executable を生成するか,library を生成するか選択します.
f:id:rkoichi2001:20181117021111p:plain

上記をすべて実施すると,サンプルの CMakeLists.txt と main.cpp ができます.これを改変していけば OK です!
f:id:rkoichi2001:20181117021347p:plain

3.C/C++コンフィグレーション・デバッグの設定

この設定方法は,前回のエントリで書いた内容と同じです.下記エントリの2.2と3を参考にしてください.
daily-tech.hatenablog.com

4.CMake Project のビルド

ビルドもとっても簡単です.CMake のビルドタスクはすでに CMake の Extension が compile_commands.json というファイルを作ってくれてます.
f:id:rkoichi2001:20181117021957p:plain

なので,コマンドパレットを開いて(Ctrl + Shift + P),CMake: Build a target を実行すれば,ビルドしてくれます.

下記は,ビルド実施時のターミナル出力です.

[build] Starting build
[proc] Executing command: /usr/bin/cmake --build /home/koichi/workspace/vs_code_sample_w_cmake/build --config Debug --target all -- -j 10
[build] Scanning dependencies of target vs_code_sample_w_cmake
[build] [ 50%] Building CXX object CMakeFiles/vs_code_sample_w_cmake.dir/main.cpp.o
[build] [100%] Linking CXX executable vs_code_sample_w_cmake
[build] [100%] Built target vs_code_sample_w_cmake
[build] Build finished with exit code 0

5.既存の CMake プロジェクトのロード

で,じゃあ既存の CMake プロジェクトはどうやってロードすんだよ?って話になると思うのですが,これも超簡単でした.まず,File -> New Window から新しい VSCODE を開きます.

f:id:rkoichi2001:20181117022646p:plain

次に,CMakeLists.txt を含んでいるプロジェクトを開きます.
f:id:rkoichi2001:20181117024955p:plain

CMakeLists.txt を含むフォルダを開くと,自動で Confugure してくれます.デフォルトでは CMakeLists.txt を含むフォルダ内に build ディレクトリができますが,このあたりは個人のホームディレクトリ直下にある .vscode/extensions/vector-of-bool.cmake-tools-1.1.2 のフォルダの下の "package.json" ファイルをいじれば変更できるかと思います.(が,試してません.)
f:id:rkoichi2001:20181117025107p:plain

ということで,VSCODEいいですね!eclipse より簡単かも!

Visual Studio Code を使った C++ のビルド&デバッグ方法

ということで,つくばチャレンジも一段落した今,ちょっと基礎的なエントリです.

仕事では Visual Studio を使って Windows メインで開発をしてるんですが,エディアとか IDE とかってなれるまでに時間がかかリますよね...なのでちょっと億劫だったんですが,流石に gedit でやるのも効率があんまり良くないかな..と思い,Linux でも何かエディタをつかうことにしました.C/C++ のエディタって,みんな eclipse CDT を使っているのかな?と思ってたんですが,最近はあまり eclipse ってつかわれてないんですかね?いろいろ記事を詠んでみると Visual Studio Code を扱っている記事が多かったので,ちょっと Eclipse CDT と Visual Studio Code を勉強&比較してみることにしました.

1.インストール方法

1.1 VSCODEのインストール

インストールは簡単でした.Ubuntu Softwareで検索窓に "vscode" って入れると Visual Studio Code が出てくるので,これをインストールします.

1.2 CPP extension のインストール

VSCODEでは,intellisense とか debugging のために extension をインストールする必要があります.ただ,これも VSCODE 経由で全部インストールできます.赤で囲ったボタンが Extension ボタンで,これを押すと同じく赤で囲った Extension の検索窓が出てきます.ここに,cpptoolsと入力すると,C/C++っていう一番上に出てきている Extension が検索結果として表示されるので,これをインストールします.
f:id:rkoichi2001:20181116081459p:plain

2.C++コードの作成とコンパイル

2.1 サンプルディレクトリとファイルの作成

まず,vscodeを開きます.この状態ではワークスペースとか何も設定されてない状態なので,サンプルのフォルダを作ります.青の "Open Folder" ってボタンを押せばフォルダチューザーが出てくるので,そこで適当なフォルダを選択します.フォルダを選択すると,選択したフォルダ "VS_CODE_SAMPLE_CPP" が起点になります.
f:id:rkoichi2001:20181116075226p:plain

で,次に src, include, build ディレクトリを作ってみます.ディレクトリもファイルの追加も vscode 上からできます.(VS_CODE_SAMPLE_CPPという表記の横に4つボタンが見えてますが,このボタンでファイル・フォルダの追加ができます.)
f:id:rkoichi2001:20181116075840p:plain

次に,作成した src ディレクトリにサンプルの cpp ファイル,include ディレクトリにサンプルの hpp ファイルを入れたらひとまず下準備完了です.

2.2 C/C++のコンフィグレーション設定

Ctrl + Shift + P を同時押しすると,コマンド窓?のようなものが表示されます.先ほどの C/C++ Extension がインストールされていれば,ここで "C/Cpp Edit Configuration" という選択肢が出てくると思います.下記の赤枠で囲った部分です.
f:id:rkoichi2001:20181116082016p:plain

この選択肢を実行すると,ルートフォルダの下に ".vscode" というディレクトリができます.このディレクトリにVSCODE関連の設定ファイルが入ってきます.と同時に c_cpp_properties.json というファイルができますが,この json ファイルが C/CPP extension が使う C/CPP の設定になります.
f:id:rkoichi2001:20181116082312p:plain

おそらく設定可能な変数は山のようにあると思うのですが,ひとまず下記の設定をすれば基本的な部分は抑えられるのかと.このワークスペースの構成では,ヘッダファイルは include フォルダに置くという前提で,"includePath" を workspaceFolder/include としています.

{
    "configurations": [
        {
            "name": "Linux",
            "includePath": [
                "${workspaceFolder}/include"
            ],
            "defines": [],
            "compilerPath": "/usr/bin/gcc",
            "cStandard": "c11",
            "cppStandard": "c++17",
            "intelliSenseMode": "clang-x64"
        }
    ],
    "version": 4
}
2.3 サンプルコードの作成

ここまで来ると,Intellisenseが動くようになっていると思うので,サンプルコードが俄然作りやすくなっていると思います.
f:id:rkoichi2001:20181116082820p:plain

完成版サンプルコード

#include <iostream>

int main(int argc, char const *argv[])
{
  std::cout << "Hello World!" << std::endl;

  for (int i = 0; i < 10; i++) {
    std::cout << "Sample For Loop : " << i << std::endl;
  }

  return 0;
}
2.4 ビルドタスクの作成

サンプルコードが完成したら,次にタスクを作ります.VSCODEでは,ビルドとかデバッグとかをタスクという単位で管理してます.なので,コンパイルとかビルドとかデバッグとかのためにタスクを作ってあげる必要があります.

2.4.1.Ctrl + Shift + P でバー表示,"Configure Task" を選択
f:id:rkoichi2001:20181116090549p:plain

2.4.2.”Create tasks.json” を選択
f:id:rkoichi2001:20181116090638p:plain

2.4.3.”Others” を選択
f:id:rkoichi2001:20181116090709p:plain

2.4.4."tasks.json" テンプレートが表示されるので,各々の環境に適した設定に変更
f:id:rkoichi2001:20181116090755p:plain

最終的に,自分の環境では下記のように tasks.json ファイルを更新しました.
下記 command がそのまんま g++ の実行ファイルになってて,args に引数を一通り入れて渡す感じですかね.ちなみに,"group" って書いてあるタグがあると思うんですが,ここで "build" を指定することによってこのタスクがビルドタスクであるとに認識されて,Ctrl + Shift + B ボタンを押すことによってビルドできるようになります.

{
  // See https://go.microsoft.com/fwlink/?LinkId=733558
  // for the documentation about the tasks.json format
  "version": "2.0.0",
  "tasks": [
    {
      "label": "echo",
      "type": "shell",
      "command": "g++",
      "args":[
          "-g", "${file}", "-o", "${fileBasenameNoExtension}"
      ],
      "group":{
        "kind": "build",
        "isDefault": true
      }
    }
  ]
}

2.4.5.ビルドの実行
ここまでくれば,ビルドの実行は簡単にできます.sample.cpp を開いた状態で,Ctrl + Shift + B を同時押ししてやると,ビルドが走って実行ファイルができます.下記,ターミナルの出力結果です.

> Executing task: g++ -g /home/koichi/workspace/vs_code_sample_cpp/src/sample1.cpp -o sample1 <
Terminal will be reused by tasks, press any key to close it.

でフォルダを見てみると,想定どおり実行ファイルができてます.

3 デバッグ環境の設定

サンプルコードのビルドがうまく行ったので,次はデバッグ環境の設定です.デバッグの設定に関しても,json ファイルを生成してあげる必要があります.

3.1 launch.jsonファイルの雛形生成
まず,デバッグボタンを押して,Configureボタンを押します.
f:id:rkoichi2001:20181117010219p:plain

3.2 launch.jsonファイルの修正.
3.1を実行すると,下記のようにlaunch.jsonファイルの雛形が出来上がります.
f:id:rkoichi2001:20181117010506p:plain
デバッグしたい実行ファイルを"program"に指定する必要があります.

{
  // Use IntelliSense to learn about possible attributes.
  // Hover to view descriptions of existing attributes.
  // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
  "version": "0.2.0",
  "configurations": [
    {
      "name": "(gdb) Launch",
      "type": "cppdbg",
      "request": "launch",
      "program": "${workspaceFolder}/${fileBasenameNoExtension}",
      "args": [],
      "stopAtEntry": false,
      "cwd": "${workspaceFolder}",
      "environment": [],
      "externalConsole": true,
      "MIMode": "gdb",
      "miDebuggerPath": "/usr/bin/gdb",
      "setupCommands": [
        {
          "description": "Enable pretty-printing for gdb",
          "text": "-enable-pretty-printing",
          "ignoreFailures": true
        }
      ]
    }
  ]
}

3.3 デバッグの実行
ここまでくればあとは簡単にデバッグできます.
デバッグボタンを押して,先ほど作った launch.json が (gdb) Launch として表示されるので,これを実行すれば始まります.

f:id:rkoichi2001:20181117011426p:plain

実際のデバッグ実行の様子.
f:id:rkoichi2001:20181117011949p:plain


ということで,VSCODEを使ったC++のビルド&デバッグ実行でした.今回のケースは一番シンプルなもので,外部ライブラリとのリンク等もありませんでしたが,それは次回に!

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

ということで,今年も終わりました.つくばチャレンジ本走行.
全く同じ書き出しでちょうど一年前に同じ記事を書いてました.一年たつの早いですね~.
daily-tech.hatenablog.com

今年の結果

今年はつくばチャレンジ第三ステージということで,昨年までとコースも大幅に変わりましたが,結果としては下記のような感じでした.
f:id:rkoichi2001:20181114003542p:plain

スタートして200mくらいの地点(上図の失敗ポイント1)が本走行の公式記録となりました.練習の時は無事に通過できていたので,まさかそこで終わってしまうとは思わなかったんですが,ちょっと通路が細くなっており,タイヤが脱輪してしまって復帰できませんでした.ただそのまま終わるのも悔しすぎたので,脱輪してコースに戻れないロボットをやさしく手押しして(笑)自動走行を続けました.結果的に,全長2キロのコースで自動走行が不能になってしまった箇所が上図の3箇所でした.

失敗ポイント1:通路が細く,通行可能なセメント路面と土路面との高低差が5cmくらいある.土の路面に脱輪してしまい走行不能になった.(<-本走行の公式記録)
失敗ポイント2:ウェイポイントがよくなかったのか,折り返し地点のコーンに衝突.
失敗ポイント3:高さのある構造物があまりないエリアで,自己位置推定が破綻して走行不能になった.

今年の成果

4月時点の下記エントリで書いていた3つの目標のうち,
daily-tech.hatenablog.com

1.ハードを変える.(完了)
2.レーザースキャナを使う.(完了)
3.画像ベースの3D地図を作成し,カメラで自己位置推定する.(未)

3は完全に未達になってしまいました.若干今年は不完全燃焼だったかな...という反省もあるのですが,ROS & Lidar の威力が半端なく,結果として自律走行のレベルは昨年よりも大幅に向上しました.(ROS を多分に使用したので,自分のコードをほとんど使わずに結構走れてしまいました.ここは,ちょっと複雑ですが...ただ,基本的な自律走行のやり方みたいな部分の理解はとても深まったので,得たものは多かったかと思います.)ちょっと年末まで ROS の基本パッケージの勉強&復習をして,来年はカメラを多分に用いて自律走行させたいと思います.

急遽安全管理責任者をお願いしたC氏.

サンキューでした!あんたがいなかったら確認走行はクリアできてなかったよ~.
f:id:rkoichi2001:20181114010028j:plain


f:id:rkoichi2001:20181114010105j:plain

ちなみに今年は,(毎年ですが)いろいろとつくばチャレンジ有志の方々,関連企業の方々に助けていただきました.ありがとうございました.
北陽電機株式会社:UTM-30LX-EW 貸出
i-Cart Mini プロジェクトの皆様:ハードウェアの流用
つくばチャレンジ実行委員の皆様,チーム大久保の皆様,会社同僚のC氏&S氏:安全管理責任者のお願い

システムのこととか,ROSの復習内容とかはまたおいおい備忘録的にブログで上げたいと思います.

ということで,毎年完全にやるやる詐欺になってますが,言わせてください.「来年は,完走します(゚Д゚)ノ」

ハフ変換

さあ,,,つくばチャレンジ本番までついに2週間です...つくばチャレンジ本番2週間前にこんな基礎的なことやってていいのかなあ...と思いつつ,どうしてもハフ変換が必要になったので,思い切って実装しました.OpenCVとかにも実装があるのですが,どうしても処理途中の中間データが必要だったので,思い切って作ってしまいました.ただ,計算速度が遅いので必要に応じて改善が必要です.実際のコードは下記のリポジトリに置いてあります.

github.com

0.ハフ変換とは

画像認識って,文字通り画像の中に写っているものを”認識”することだと思います.自動運転の車だったら道路を走っている人やほかの車を"認識"しなければいけません.これと同様に(だいぶ簡単ですが,,,,),今回のコードでは,画像の中にある"直線"を認識します.ハフ変換を使えば,円,楕円,直線などの特定の形状を認識することができます.

1.今回の画像

ペイントで書いた自分の絵ですが,今回は超シンプルに下記の絵の中に存在する黒い線をプログラムに認識させます.
f:id:rkoichi2001:20181027185319p:plain

2.考え方

2.1 直線の方程式

ハフ変換はいろんなサイトで説明されていると思うので,手短にちょっとだけ備忘のために書き残しておきます.一般に直線は下記の式で表現できます.
ax + by + c = 0
これは小学校で習う一次関数の式で,下記のイメージです.

f:id:rkoichi2001:20181027190130j:plain
平面内の直線と直線の方程式

ただし,今回の目的は直線らしきものが含まれている画像を Input としてうけとった時に,その直線を認識することが目的です.そのため,当たり前ですが事前に直線の式はわかりません.もらった画像を調べて,そこに映っているかもしれない直線を探す必要があります.ここで,上の直線の式の見方を変えると....
(a/c) x + (b/c) y + 1 = 0

上式の二つの変数要素 (a/c,b/c) が決まれば,直線は一意に決まります.つまり,画像をスキャンしていって直線(=二つの変数要素 (a/c, b/c))に対して投票することができれば,画像に含まれている直線を認識することができそうです.

2.2 (ρ,θ) を使った直線の表現方法

で,二つの変数要素 (a/c,b/c) は実際のところどういう意味があるのか?ということを説明します.
直線の式の見方を少し変えて,ハフ変換中心を (0, 0) として,中心から直線までの距離を ρ ,x軸とのなす角を θ とすると,下記のように図示できます.

f:id:rkoichi2001:20181027192758j:plain
直線の (ρ,θ) 空間表現

上図に赤で数式を書きましたが,このように表現すると結果的に数式が
(a/c) x + (b/c) y + 1 = 0
と同じ形になっていることがわかります.ということで,二つの変数要素 (a/c,b/c) は幾何学的には上記説明のような意味を持っており, (ρ,θ) が決まれば直線がただ一つ求まるということがわかります.

2.3 (ρ,θ) への投票

次に,どうやって (ρ,θ) 空間に投票するか?という問題ですが,ここは力ずくで投票します.下図で簡単に説明します.

1.画像の左上から右下まですべての画素を見る.
2.対象画素(下図の場合は4点)があった場合,下記の数式を用いて (ρ,θ) 空間に投票する.θは一解像度ずつ動かして,対象範囲(0 < θ < 180°)すべてをカバーする.
 ρ = x * cosθ + y * sinθ
3.(ρ,θ)空間内で一番投票スコアが大きいものを最も有力な直線として採用する.
f:id:rkoichi2001:20181027201024j:plain

結局2のステップがキモになってます.対象画素で投票する場合,この画素を通るすべての角度の直線に投票します.対象画素が上図のようなパターンだった場合は,赤でかかれた線分が結局同じ直線(ρ,θ)になるので,ほかの候補直線よりも投票値がおおきくなって選択される.という流れです.

で,投票のための探索範囲ですが,対象画像中で取りうる ρ の最大値と直線の対称性を考えると,下記の図のようになります.
f:id:rkoichi2001:20181027195034j:plain

3.処理の流れ

で,肝心のコードの話です.
3.1 二値化画像の生成
必ずしも二値化画像にする必要はないかもしれないですが,簡単のために二値化しています.最終的にハフ変換される画像に関しては,0以外の値が対象画素としてみなされています.

  cv::threshold(img, img, 50, 255, cv::THRESH_BINARY);
  cv::bitwise_not(img, img);

f:id:rkoichi2001:20181027202646p:plain
ハフ変換への入力画像

3.2 対象画素の投票
2.3で説明した(ρ,θ)空間への投票です.OpenCVだと,投票空間の結果が外部から取得できないので今回自作しました.

void HoughTrans::vote_rho_theta(cv::Mat &gray_img) {

  // Initialize Buffer.
  rho_theta = 0;

  unsigned char * const img_pnt = gray_img.data;
  int * const vote_st_pnt = reinterpret_cast<int *>(rho_theta.data);

  // Outer loop for image 
  for (int v = 0; v < gray_img.rows; v++) {
    for (int u = 0; u < gray_img.cols; u++) {

      unsigned char val = *(img_pnt + v * gray_img.cols + u);

      if (val >= vote_thresh) {

        // Inner loop for voting
        for (int theta_col = 0;  theta_col < rho_theta.cols; theta_col++) {

          // Rho-Theta Calculation
          double theta = theta_res * theta_col * M_PI / 180.0;
          double rho_abs = std::abs(u * cos(theta) + v * sin(theta));


          if (rho_abs <= rho_max) {
            // Hough Vote
            int rho_row = std::abs(static_cast<int>(rho_abs / rho_res));
            int *tgt_pnt = vote_st_pnt + rho_row * rho_theta.cols + theta_col;
            *tgt_pnt = *tgt_pnt + 1;
          }

        }
      }
    }
  }
}

f:id:rkoichi2001:20181027202556p:plain
(ρ,θ)投票空間の様子

3.3 上位直線の取得
(ρ,θ)空間の投票結果画像を用いて,投票結果が上位の直線を取得します.ちょっとここは書き方がいけてないかも.

void HoughTrans::get_top_ranked_lines(std::vector<LineElem> &lines) {

  lines.clear();

  int * const vote_st_pnt = reinterpret_cast<int *>(rho_theta.data);

  for (int v = 0; v < rho_theta.rows; v++) {
    for (int u = 0; u < rho_theta.cols; u++) {

      double rho = v * rho_res;
      double theta = u * theta_res;
      int score = *(vote_st_pnt + v * rho_theta.cols + u);

      if (score <= score_thresh) {
        continue;
      }
      LineElem line2(rho, theta, score);
      LineElem line = line2;
      lines.push_back(line);
    }
  }

  std::sort(lines.begin(), lines.end());  

}

3.4 (ρ,θ)の値を用いて,結果を描画.
終結果の描画です.

void calc_pnt_in_img(double rho, double theta, cv::Point2i &p1, cv::Point2i &p2) {

  int x1, y1, x2, y2;
  double theta_rad = theta * M_PI / 180.0;
  if ((theta < 45.0) || (135.0 < theta)) {
    y1 = -10000;
    y2 = 10000;
    x1 = -(sin(theta_rad)/cos(theta_rad)) * y1 + rho / cos(theta_rad);
    x2 = -(sin(theta_rad)/cos(theta_rad)) * y2 + rho / cos(theta_rad);
  } else {
    x1 = -10000;
    x2 = 10000;
    y1 = -(cos(theta_rad)/sin(theta_rad)) * x1 + rho / sin(theta_rad);
    y2 = -(cos(theta_rad)/sin(theta_rad)) * x2 + rho / sin(theta_rad);
  }
  p1.x = x1;
  p1.y = y1;
  p2.x = x2;
  p2.y = y2;
}

f:id:rkoichi2001:20181027202500p:plain
ハフ変換最終結

Semi Global Matching (SGM) と 動的計画法

LidarとStereoのフュージョンに向けて SGM を勉強したので,備忘録としてまとめておきます.

今回からGitHubにコードも公開していきたいと思います.ここで使ったサンプルコードは下記にあります.
github.com

Semi Global Matching

二枚の画像ペア(左カメラ画像,右カメラ画像)から視差画像を求めるためのステレオマッチングアルゴリズムで,「左の画像で写っているピクセルが右の画像のどこに対応するか」を計算します.

もう少し具体的に説明します.相対的な位置関係が既知の二つのカメラで人を撮影したとします.この時,人の鼻が左の画像で (xl, yl) に写っていた時に,右の画像で写っている位置 (xr, yr) を求めるのがこのアルゴリズムの役割です.それぞれの画像で鼻の移っている位置がわかれば,あとは下記図の下のほうに書いてある三角形の相似関係でカメラから人の鼻までの距離がわかります.
f:id:rkoichi2001:20181020103600j:plain

今回の実験には,Teddy画像を使います.
f:id:rkoichi2001:20181020104356p:plain

計算手順

計算手順は,大きく分けると下記の4ステップになります.

1. 画像の平滑化

左右の画像ペアでマッチングをとった時,ノイズを低減するためにまずガウスフィルタをかけます.この作業は必須ではないですが,視差画像のノイズが少なくなってなめらかになります.

平滑化後画像

f:id:rkoichi2001:20181020104706p:plain

該当コードは下記です.ガウスフィルタまで実装するのはしんどいので,OpenCVの力を借りてます.

  if (this->gauss_filt) {
    cv::GaussianBlur(left, left, cv::Size(3, 3), 3);
    cv::GaussianBlur(right, right, cv::Size(3, 3), 3);
  }
2. センサス変換

左カメラ画像と右カメラ画像のマッチングを求めることがこのアルゴリズムの出力になります.で,マッチングをするということは「左画像のピクセルと右画像のピクセルがどれだけ似ているか?」という評価式を作ってあげないといけません.この評価式にはいろいろなものがありますが,今回は一番メジャーな?センサス変換&ハミング距離を用いた方法でやってみました.センサス変換では,直近8近傍画素値を見て自分の画素値との大小関係を保存します.
f:id:rkoichi2001:20181020110959j:plain

センサス変換では,直近8近傍の大小関係が一つ一つのビットに保存されるだけなので,画素値そのものには直接的な意味はなくなってしまいますが,相対関係が保存されるので,撮影画像の明暗に影響を受けにくいという特徴があります.

センサス変換後画像

f:id:rkoichi2001:20181020104931p:plain

該当コードは下記です.入力画像 "img" に対して,センサス変換後画像 "census" を返します.

void Sgbm::census_transform(cv::Mat &img, cv::Mat &census)
{
  unsigned char * const img_pnt_st = img.data;
  unsigned char * const census_pnt_st = census.data;

  for (int row=1; row<rows-1; row++) {
    for (int col=1; col<cols-1; col++) {

      unsigned char *center_pnt = img_pnt_st + cols*row + col;
      unsigned char val = 0;
      for (int drow=-1; drow<=1; drow++) {
        for (int dcol=-1; dcol<=1; dcol++) {
          
          if (drow == 0 && dcol == 0) {
            continue;
          }
          unsigned char tmp = *(center_pnt + dcol + drow*cols);
          val = (val + (tmp < *center_pnt ? 0 : 1)) << 1;        
        }
      }
      *(census_pnt_st + cols*row + col) = val;
    }
  }
  return;
}
3. ピクセル毎のコストを計算する

2で作成した左右のセンサス変換画像から,ピクセル毎のコストを計算します.ここで,コストが小さいほどピクセルペアの一致度が高くなるようにコストを計算したいので,ハミング距離をコストとして採用します.ハミング距離の計算コードは下記です.

unsigned char Sgbm::calc_hamming_dist(unsigned char val_l, unsigned char val_r) {

  unsigned char dist = 0;
  unsigned char d = val_l ^ val_r;

  while(d) {
    d = d & (d - 1);
    dist++;
  }
  return dist;  
}

また,扱う画像が既にステレオ平行化されているという前提に立つと,ピクセル毎のコスト計算処理は,下記のようになります.
f:id:rkoichi2001:20181020115212j:plain

具体的にコードに書くと,下記のようになります.ピクセル毎のコストはあとの計算で使うので,すべて保存しておく必要があります.

void Sgbm::calc_pixel_cost(cv::Mat &census_l, cv::Mat &census_r, cost_3d_array &pix_cost) {

  unsigned char * const census_l_ptr_st = census_l.data;
  unsigned char * const census_r_ptr_st = census_r.data;

  for (int row = 0; row < this->rows; row++) {
    for (int col = 0; col < this->cols; col++) {
      unsigned char val_l = static_cast<unsigned char>(*(census_l_ptr_st + row*cols + col));
      for (int d = 0; d < this->d_range; d++) {
        unsigned char val_r = 0;
        if (col - d >= 0) {
          val_r = static_cast<unsigned char>(*(census_r_ptr_st + row*cols + col - d));
        }
        pix_cost[row][col][d] = calc_hamming_dist(val_l, val_r);
      }
    }
  }
}
4. ピクセル毎のコストを集約する

で,ここからがSemi Global Matchingの肝になります.3のステップですでにピクセル毎のマッチングコストを計算済なので,マッチングコストが最小のものを選択することはできるのですが,これだと制約条件が少なすぎてまともな視差画像ができません.実験的にピクセル毎のマッチングコストが最小になるものを使って作成した視差画像が下記になります.視差に連続性が全くない様子がわかると思います.
f:id:rkoichi2001:20181020120337p:plain

で,視差の連続性の観点でコストを導入します.隣り合う画素の視差違いによってペナルティを課します.ここで出てくるP1, P2という二つのパラメータがSGMの一番重要なパラメータになります.
ペナルティなし(0):隣り合う画素の視差が同じ
ペナルティ1(P1) :隣り合う画素の視差の違いが1
ペナルティ2(P2) :隣り合う画素の視差の違いが1以上

このようにペナルティを設けるということは,暗黙には下記の仮定があります.
仮定1:隣り合う画素は基本的には視差が同じになるべき.
仮定2:そうはいっても,斜めに写っている壁とかだと隣接する画素で視差が1くらいは違うときもあるんじゃね?
仮定3:視差が1以上のものに関しては大きなペナルティを貸す!ただし,物体の境界とかでは実際に視差が1以上になるのでまったくありえないわけではない.

というわけで,画像を左から右までスキャンしていったときに,上記3つのペナルティを課しながら総コストを計算していきます.ここでようやく,前回必死に勉強した動的計画法が登場します.ペナルティが3種類あるので,動的計画法の分岐は下記のようになります.

隣接する計算済ピクセルの視差値と同じ:(ペナルティ0)
隣接する計算済ピクセルの視差値より1小さい,もしくは大きい:(ペナルティP1)
隣接する計算済ピクセルの視差値より2以上変化する:(ペナルティP2)

これを数式で書くと,,,
L(p, d) = C(p, d) + min(L(p-r, d), L(p-r, d-1) + P1, L(p-r, d+1) + P1, L(p-r, i) + P2)
となります.ここで,,,,
L(p, d) : 現在着目しているピクセルで,視差値が d の時のコスト
C(p, d) : 現在着目しているピクセルで,視差値が d の時のピクセル毎に計算したコスト.
L(p-r, d) : 隣り合う一つ前(計算済)のピクセルで,視差値がdの時のコスト
L(p-r, d-1), L(p-r, d+1) : 隣り合う一つ前(計算済)のピクセルで,視差値がd-1, d+1の時のコスト
L(p-r, i) : 隣り合う一つ前(計算済)のピクセルで,視差値がi (一以上変わる) の時のコスト

ここで,(x, y) と表記せずに p, r とかって書いてあるので混乱してしまったんですが,SGMでは真横にスキャンするだけでなく,上下左右斜めの8方向(もしくは16方向)でスキャンしてコストを計算します.なので,p はそのまま画素位置と解釈して問題ないのですが,r はスキャンライン上の一要素分の移動ベクトルになります.8方向でコスト計算する場合,上記 L(p,d) の計算を8方向分行って,これをすべて足し算し,その結果最小となる視差値を採用します.(うーん.なんだかうまく説明できないので,コードを見てください.)
f:id:rkoichi2001:20181020124738j:plain

動的計画法を使って,コストを求めるコードです.

unsigned short Sgbm::aggregate_cost(int row, int col, int depth, int path, cost_3d_array &pix_cost, cost_4d_array &agg_cost) {

  // Depth loop for current pix.
  unsigned long val0 = 0xFFFF;
  unsigned long val1 = 0xFFFF;
  unsigned long val2 = 0xFFFF;
  unsigned long val3 = 0xFFFF;
  unsigned long min_prev_d = 0xFFFF;

  int dcol = this->scanlines.path8[path].dcol;
  int drow = this->scanlines.path8[path].drow;

  // Pixel matching cost for current pix.
  unsigned long indiv_cost = pix_cost[row][col][depth];

  if (row - drow < 0 || this->rows <= row - drow || col - dcol < 0 || this->cols <= col - dcol) {
    agg_cost[path][row][col][depth] = indiv_cost;
    return agg_cost[path][row][col][depth];
  }

  // Depth loop for previous pix.
  for (int dd = 0; dd < this->d_range; dd++) {
    unsigned long prev = agg_cost[path][row-drow][col-dcol][dd];
    if (prev < min_prev_d) {
      min_prev_d = prev;
    }
    
    if (depth == dd) {
      val0 = prev;
    } else if (depth == dd + 1) {
      val1 = prev + this->p1;
    } else if (depth == dd - 1) {
      val2 = prev + this->p1;
    } else {
      unsigned long tmp = prev + this->p2;
      if (tmp < val3) {
        val3 = tmp;
      }            
    }
  }

  // Select minimum cost for current pix.
  agg_cost[path][row][col][depth] = std::min(std::min(std::min(val0, val1), val2), val3) + indiv_cost - min_prev_d;
  //agg_cost[path][row][col][depth] = indiv_cost;

  return agg_cost[path][row][col][depth];
}
<||

8方向スキャンして,コスト計算するコードです.
>||
void Sgbm::aggregate_cost_for_each_scanline(cost_3d_array &pix_cost, cost_4d_array &agg_cost, cost_3d_array &sum_cost)
{
  // Cost aggregation for positive direction.
  for (int row = 0; row < this->rows; row++) {
    for (int col = 0; col < this->cols; col++) {
      for (int path = 0; path < this->scanlines.path8.size(); path++) {
        if (this->scanlines.path8[path].posdir) {
          //std::cout << "Pos : " << path << std::endl;
          for (int d = 0; d < this->d_range; d++) {
            sum_cost[row][col][d] += aggregate_cost(row, col, d, path, pix_cost, agg_cost);
          }
        }
      }
    }
  }

  // Cost aggregation for negative direction.
  for (int row = this->rows - 1; 0 <= row; row--) {
    for (int col = this->cols - 1; 0 <= col; col--) {
      for (int path = 0; path < this->scanlines.path8.size(); path++) {
        if (!this->scanlines.path8[path].posdir) {
          //std::cout << "Neg : " << path << std::endl;
          for (int d = 0; d < this->d_range; d++) {
            sum_cost[row][col][d] += aggregate_cost(row, col, d, path, pix_cost, agg_cost);
          }
        }
      }
    }
  }
  return;
}
5. 視差画像を求める.

でここまでくればあとは簡単です.コストが最小の画素値を採用して,視差画像を作成すれば完了です.
f:id:rkoichi2001:20181020100418p:plain

最小コストを選択して,視差画像作成

void Sgbm::calc_disparity(cost_3d_array &sum_cost, cv::Mat &disp_img)
{
  for (int row = 0; row < this->rows; row++) {
    for (int col = 0; col < this->cols; col++) {
      unsigned char min_depth = 0;
      unsigned long min_cost = sum_cost[row][col][min_depth];
      for (int d = 1; d < this->d_range; d++) {
        unsigned long tmp_cost = sum_cost[row][col][d];
        if (tmp_cost < min_cost) {
          min_cost = tmp_cost;
          min_depth = d;
        }
      }
      disp_img.at<unsigned char>(row, col) = min_depth;
    } 
  } 

  return;
}

スケーリングして,可視化用画像作成.

    cv::Mat tmp;
    disp.convertTo(tmp, CV_8U, 256.0/this->d_range);
    cv::imshow("Sgbm Result", tmp);
    cv::waitKey(0);

参考にさせていただいたサイト等々

atkg.hatenablog.com
github.com
github.com

一段落...

全く不本意ながら仕事が立て込みなかなかロボットできてなかったんですが,先週ようやく(&おそらく)一段落し平穏が訪れました.ということで,ここからロボットラストスパートです.結局今年はまだ試走会にも参加できておらず,11/11の本番まで残り一か月を切ってしまいました.

試走会

11/4(日)
11/9(金)
11/10(土)

本番

11/11(日)

あと3週間しかないので,あんまりいろいろなことはできませんが,ひとまず下記を目標に進めていきます.

目標

1. 確認走行区間の突破 @ 11/4
2. カメラとLidarのフュージョン

よし.頑張ろう..

イグジット(Exit)

"イグジット" という単語をググると,下記の定義が出てきます.

イグジットとは、ベンチャービジネスや企業再生などにおいて、創業者やファンド(ベンチャーキャピタルや再生ファンドなど)が株式を売却し、利益を手にすること。ハーベスティング(Harvesting、収穫)ともいう。

なにか新しいことを始めた時に,その成果が実って利益を手にする(=出口を見つける)という意味だと解釈すると,これって何もベンチャーIPOしてお金をゲットするという場合だけでなく,新しい技術をサービスに昇華させて,ビジネスとして価値あるものに変えるということにもそのまま当てはまると思います.製造業(主に自動車関連)で8年近く働いてきて感じることなんですが,日進月歩で技術はどんどん新しくなっていくんですが,これで金儲けをする(=多くの人に役立てる仕組みを考える)って本当に難しいです.「自動運転」とか「二足歩行ロボット」とか,十年以上前から新聞やメディアで扱われていますが,まだR&Dの投資回収をできた会社はないはずです.

なんですが,先日下記ビデオを見て,ロボットネタでエンターテインメント以外でついに「イグジット」する可能性が出てきたんじゃないかな?とちょっと興奮したのでブログにアップしました.竹中工務店ソフトバンクが実証実験をしているらしいですが,このくらい自由自在に移動できれば探査・調査目的に導入できるのではないかなと思います.警備ロボットとしても使えるかと.
www.youtube.com

今日はちょっと酔ってしまったので,もう寝ます...また明日...
www.youtube.com