octomapのoctovisのMSVCインストールメモ

以前、octomapをopenFrameworksで使うためにインストールしたことがありましたが、その際、可視化プログラムoctovisはQtを使うことが億劫でインストールしませんでした。
必要が出てインストールしてみたのですが、案の定ちょっと面倒くさかったのでメモを残します。
基本的には/octomap/octovis/README,mdに書いてあります。

QGLViewerのビルド

octomapのoctovisのソース中にQGLViewerというフォルダがあり、これを先にビルドする必要があります。

/octomap/octovis/src/extern/QGLViewer

1.Qt5をインストール
QtのダウンロードサイトDownload Qt: Choose commercial or open sourceから、インストーラをダウンロードして実行します。ライセンス要項が適合するならOpenSource版で構いません。
実行すると、アカウント登録を求められますので登録して進みます。次にインストールするバージョンを選択できますので、安定版Qt 5.11.2のMSVC 2017 64-bitなど好きなものを選んでインストールします。
2.環境変数Pathにqmake.exeの場所を追加
例として

E:\Qt\5.11.2\msvc2017_64\bin

3.VS2017の開発者用コマンドプロンプトを開く

cd octomap/octovis/src/extern/QGLViewer
qmake -t vclib QGLViewer.pro -spec win32-msvc

4.Visual StudioでQGLViewer.vcxprojを開く
x64プラットフォームの場合は、x86を基に構成マネージャーで追加する。あとは普通にビルド。

octomapのビルド

5. CMakeを通す
OCTOMAP_OMP, OCTOVIS_QT5, QT_QMAKE_EXECUTABLE, QGLViewer_LIBRARY_DIR_WINDOWSを設定して一度configureすると、上手く見つからなかった場合、Qt関係のDIRを指定するように指示されますのでこれを指定していきます。例. Qt5Core_DIR \Qt\5.11.2\msvc2017_64\lib\cmake\Qt5Coreなど。都合5個聞かれます。
自分の場合の例はこんな感じ。

6.ビルド
cmakeで生成されたslnファイルをVisual Studioで開いてビルドします。octovis-sharedでエラーが出ます。
まず、octovisのプロジェクトのプロパティを開いて、リンカー→入力→追加の依存ファイルをすべてコピーします。
次に、octovis-sharedのプロジェクトのプロパティを開いて、リンカー→入力→追加の依存ファイルにすべて追加します。先行入力されていたものとダブリがあっても名寄せされますので、気にせずとも大丈夫です。
これでビルドが通るようになっているかと思います。

自分用読むべき論文メモ 2018年11月版

An Open-Source C++ Library for Robotics and Optimal Control
https://bitbucket.org/adrlab/ct
https://adrlab.bitbucket.io/ct/v2.3/ct_doc/doc/html/index.html
ROSが邪魔ー
adrl:software [LAB]
このLabのライブラリが面白い。

qiita.com
読まない理由がない。

タコの心身問題――頭足類から考える意識の起源

上記のブルックスのエッセイでも紹介されているタコのkindle版はまだないみたい。

docs.google.com
勉強になります。

自分用読むべき論文メモ 2018年10月版

Randomly distributed embedding making short-term high-dimensional data predictable | PNAS
www.iis.u-tokyo.ac.jp
www.atmarkit.co.jp
今月、いろいろすごいニュースがあったけれど、これが一番凄そう

www.preferred-networks.jp
robotstart.info
eetimes.jp
news.mynavi.jp
それぞれの要素技術は分かる。でもそれがもう実時間で普通に動くアプリケーションが実装されているのが驚異的。

Shape Completion Enabled Grasping
2.5次元から3次元の復元と、DEX-NET的なグラスピング戦略。

[1810.04891] Dense Object Reconstruction from RGBD Images with Embedded Deep Shape Representations
密な3次元モデルの再構成。マージング

shiropen.com
息を吸いながら囁いて入力できる音声入力。さすが福本さん的な独創性の高い音声インターフェース。近い将来、すべてのスマホに標準採用されそうですね。

Luminar
トヨタが自動運転に採用、22歳が開発した「高性能センサー」の実力｜WIRED.jp
自動運転の大激戦を制する？ トヨタ注目のスタートアップが開発した「新しいセンサー」の正体｜WIRED.jp
他のワンショット3D Lidarとかとの違いはどうなんだろう？

www.cepton.com
quanergy.com
TetraVue | Home
ムービー見ると、TetraVueすごいけど自動走行向けスペックとはちょっと違うような。

www.symphotony.com

wired.jp
wired.jp

pathak22.github.io

Curiosity driven exploration from Takuya Minagawa

www.slideshare.net
speakerdeck.com

blogs.unity3d.com

mabonki0725.hatenablog.com
mabonki0725.hatenablog.com

Curiosity-driven Exploration by Self-supervised Prediction · Issue #308 · arXivTimes/arXivTimes · GitHub
「AIに好奇心を与えるとスーパーマリオやDoomが上達」、カリフォルニア大学バークレー校のチームが研究成果を発表 - Engadget 日本版
好奇心　深層強化学習

magiclantern.fm
有志のユーザ製のキヤノンの一眼レフデジカメ用ファームウェア。標準化してほしいぐらいの高機能。

topology-tool-kit.github.io
Topology Tool Kit。Qtが邪魔だなぁ

igl | Interactive Geometry Lab | ETH Zurich | Code

NDTのD2D Registrationを試してみる

NDTについて

NDT(Normal Distribution Transform)は、大規模な点群データをざっくりと扱いたいときに使う特徴量で、空間を適当なGridで分割してGrid内の点群分布を正規分布に表したものです。点群が多くなっても高速に計算できる、応用範囲が広く、多くのアルゴリズムで精度が思いのほか高いなどのメリットがあります。
NDTを使って位置合わせを計算するアルゴリズムもいくつか提案されていて、NDTと点群(P2D: Point to Distribution)、NDT-MCL(パーティクルフィルタ)、NDTとNDT(D2D: Distribution to Distribution)などが知られています。ROSのSLAMの位置合わせアルゴリズムとしてよく使われていたり、国内では、名古屋大学のAutoware[1], 豊田中央研究所[2], 九州大学の倉爪研究室[3]などが力を入れているようです。

NDT-D2D Registration

NDT-P2D, NDT-MCLはなんだか無駄が多い気がして使う気にならなかったのですが、NDT-D2DはNDT同士を対応付けて位置姿勢をニュートン法で求めるというシンプルな方法をとるので良さそうに見えます。
元の論文は[4]で、実装例はスウェーデンのエーレブルー大学にあります。
github.com
Linux環境でROSが使える人は、ROSパッケージが出ていますので素直にこちらを参照してください。
ndt_registration - ROS Wiki

NDT-D2DをWindowsで試したい

Orebro大のソースコードは一見大きなライブラリに見えますが、NDT-D2Dの部分だけを部分的に取り出しても動かすことができるようです。
意味づけラベルとNDTを一緒に使うというzaganidisさんの提案のse_ndtのコードの内、oru_minimalが参考になります、が、OcTreeなども省いてしまっているようなので、これを参考にオリジナルからソースコードを切り出してみます。
Semantic-assisted 3D normal distributions transform for scan registration in environments with limited structure - IEEE Conference Publication
github.com

NDT-D2DをWindowsのopenFrameworksで試す

github.com
に切り出したソースコードを置きます。gettimeofdayなどLinux用関数を強制的に置き換えていますので、Windowsでしか動かない部分があると思います。変更を加えたソースコードの周りには//////（スラッシュ五個）を置いていますので、修正時の参考にしてください。
依存ライブラリとして、PCLとopenCVが必要です。
cvl-robot.hateblo.jp
を参考に

vcpkg install pcl:x64-windows
vcpkg install opencv:x64-windows

を通してください。vcpkg integrate installをしてしまうと、openFrameworksが面倒くさくなるのでしない方が良いです。インストールフォルダに手動でインクルードパスとライブラリパスを通してください。

include path: C:\vcpkg\installed\x64-windows\include
library path: C:\vcpkg\installed\x64-windows\lib

ライブラリ名を手動で列挙するのは苦行なので、こちらの便利なアプリを使うと良いでしょう。
neno-garden.com

実行結果

（多分ライブラリの実装上の制限で）スケールを合わせる必要がありますが、ちゃんと位置合わせ出来ています。
初期位置

赤い点が位置合わせ結果

ICPじゃ初期位置合わせを厳密にやらないと収束しそうもないデータでも（ぴったりじゃないけど）収束している。

赤い丸が位置合わせ結果

[1] autoware.ai
[2] https://ipsj.ixsq.nii.ac.jp/ej/index.php?action=pages_view_main&active_action=repository_action_common_download&item_id=107049&item_no=1&attribute_id=1&file_no=1&page_id=13&block_id=8
[3] https://www.jstage.jst.go.jp/article/jrsj/31/9/31_31_896/_pdf
[4] http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.818.9757&rep=rep1&type=pdf

WindowsでPCLをインストールする2018年度版

Sugiuraさんのブログにまとめられています。が、ＷＥＢへのアクセスがとても遅いので注意が必要です。
Point Cloud Library is available in Vcpkg – Summary?Blog

Vcpkgのインストール

2018年現在、もっとも簡単なWindowsへのインストール方法は、Vcpkgを使う方法のようです。
github.com
からソースをダウンロードして分かり易いパスに展開します。例：e:\vcpkg
batファイルを実行するとビルドが開始されます。

bootstrap-vcpkg.bat

pathを通す

環境変数Pathにインストールディレクトリを追加します。例：e:\vcpkg

PCLのインストール

Unanancyowenのブログの記述に従って、コマンドプロンプトからvcpkgを実行します。
例えば、x64静的ライブラリが欲しい場合、次を実行します。

# x64 static link library
vcpkg install pcl:x64-windows-static

インストールとビルドが進むのですが、途中でエラーが出て止まってしまいます。
https://eiichiromomma.github.io/2017/10/13/vcpkg-ninja/
こちらの記事によると、英語パックのインストールが必要とのことですので、こちらをインストールします。
Download Microsoft Visual Studio 2015 Language Pack from Official Microsoft Download Center

再度、vcpkg installを実行すると処理が進みますが、大きなライブラリ群なので時間がかなりかかります。

追加ライブラリをインストールする場合

# Build PCL with OpenNI2, Qt and PCAP
vcpkg install pcl[openni2,qt,pcap]:x64-windows --featurepackages

VisualStudio環境から使えるように設定する、設定をはずす

# Integrate
vcpkg integrate install

# UnIntegrate
vcpkg integrate remove

今日の登山用品

地震や台風で停電が各地で頻発しているのを口実に、キャンプ用品を買いたいなと考えています。無駄にチタンが良いな。

EPI(イーピーアイ) ATSチタンクッカー TYPE-3 セット TS-203

EPI(イーピーアイ) REVO-3700ストーブ(日本製) S-1028

octomapを使って、実データからマインクラフトみたいな空間を切り出す（その１）

マインクラフトを遊んだことが無いのでゲームはよくわかってないのですが、マインクラフトみたいに全てがボクセルで表現された空間を作りたいと思います。

3次元空間を表現しようとする場合、単純な3次元配列を使うととんでもなくデータが多くなるので、オクトツリーなどのデータ構造を使ってデータ量を減らすことが一般的です。octomapはそのようなライブラリの最も有名なものの一つで、ロボットや自動運転の車のSLAM地図を表現するのによく使われています。また、ROSで最も便利に使われるライブラリの一つでもあります、が、このページではROSを介さずに使います。
OctoMap - 3D occupancy mapping

ライブラリの概要はこちらのＰＤＦが最も分かり易かったです。
http://www2.informatik.uni-freiburg.de/~hornunga/pub/hornung13roscon.pdf

octomap関係なく地図のデータ表現の基礎は、いつものMy Enigmaさんの記事が勉強になります。
自律移動ロボットのためのグリッドマップ作成MATLABサンプルプログラム - MyEnigma

ノード、リーフ、ルートなどのツリー型データ構造の用語等は、こちらのWikipediaにまとまっています。
木構造 (データ構造) - Wikipedia

インストール

octomapのインストールは、githubからソースを取得して、cmake、ビルドを普通に通せば完了します。
github.com
今回はoctovisを使いません。可視化アプリのoctovisをビルドしようとするとQt4が必要になり、ちょっと面倒くさいので、必要になったらoFで自前で作ることにします。

基礎知識

ボクセルのデータ表現

octomapの空間（ノード）は、大雑把に3種類で表現されます。作ったばかりの空間は、Unknownで満たされていて、データを追加するたびにリーフにOccupiedが追加されていきます。

• Unknown: 未知の空間。NULLデータが入っている。
• Free: 中に何もないことが分かっている空間。
• Occupied: 何かしらのデータが入っている空間。0~1の確率で表現される。

（追加する、予定。予定を忘れがち。）

使い方の例

空間を作る

一辺が0.05の長さのボクセルを最小単位にして空間を作ります。

std::shared_ptr<octomap::ColorOcTree> tree;
tree = std::make_shared<octomap::ColorOcTree>(0.05);

レイを一つ飛ばして空間を削り出す。

光線を一つの線分として、線分と衝突した空間のボクセルのデータを書き換えます。

octomap::point3d origin(x0, y0, z0); // 計測原点。カメラの3次元座標。
octomap::point3d end(x1, y1, z1); // 計測した1点の3次元座標。
tree->insertRay(origin, end); // レイを飛ばして空間を削り出す。

空間の更新

レイの挿入が一通り終わったら、空間全体を更新します。

tree->updateInnerOccupancy();

データの保存

コマンド一発でファイルに吐いてくれます。

tree->writeBinary("map.bt");

ただし、拡張子によってデータ表現がいくつかあるので難しいことをし始めたら注意が必要です。

.graph: openGraph();
.bt: openTree();
.ot: openOcTree();
.hot: openMapCollection();
.dat: openPointcloud();

未知空間のボクセルの中心座標を取得する

int lv = tree->getTreeDepth(); // 調べたいデータ階層の深さ。例として、データ最深階層を指定。ただし、0を指定するとさらにもう一つ細かいみたい。
double offset = 0.5 * tree->getResolution();
octomap::point3d pmin(-1. - offset, -1. - offset, -1. - offset); // 調べたい領域のバウンディングボックスの最小端の座標
octomap::point3d pmax(1. - offset, 1. - offset, 1. - offset); // 調べたい領域のバウンディングボックスの最大端の座標
octomap::point3d_list node_centers; // 結果を格納するリスト
tree->getUnknownLeafCenters(node_centers, pmin, pmax, lv);

for (octomap::point3d_list::iterator it = node_centers.begin(); it != node_centers.end(); ++it) {
  std::cout << it->x() << " " << it->y() << " " << it->z() << std::endl;
  // ofDrawSphere(it->x(), it->y(), it->z(), tree->getResolution());
}

ランダムにレイを8000個ほど飛ばして、1×1×1の大きさの0.05解像度の空間を削り出してみた結果、残ったunknown領域をoFで適当に表示してみた例。

占有空間のボクセルの中心座標を取得する

思いのほか面倒くさいです。データアクセスの方法は、octomapチュートリアルのtest_iteratorsにまとめられています。
まず、再帰的にボクセルにアクセスするための補助関数を2つ用意します。

void computeChildCenter(const unsigned int& pos, const float& center_offset, 
    const octomap::point3d& parent_center, octomap::point3d& child_center) {
    // x-axis
    if (pos & 1) child_center(0) = parent_center(0) + center_offset;
    else     child_center(0) = parent_center(0) - center_offset;
    // y-axis
    if (pos & 2) child_center(1) = parent_center(1) + center_offset;
    else   child_center(1) = parent_center(1) - center_offset;
    // z-axis
    if (pos & 4) child_center(2) = parent_center(2) + center_offset;
    else   child_center(2) = parent_center(2) - center_offset;
}

/// mimics old deprecated behavior to compare against
void getLeafNodesRecurs(std::list<octomap::OcTreeVolume>& voxels, unsigned int max_depth, 
    octomap::OcTreeNode* node, unsigned int depth, const octomap::point3d& parent_center,
    const octomap::point3d& tree_center, octomap::OcTree* tree, bool occupied)
{
    if ((depth <= max_depth) && (node != NULL)) {
        if (tree->nodeHasChildren(node) && (depth != max_depth)) {
            float center_offset = float(tree_center(0) / pow(2., (double)depth + 1));
                octomap::point3d search_center;
                for (unsigned int i = 0; i < 8; i++) {
                    if (tree->nodeChildExists(node, i)) {
                        computeChildCenter(i, center_offset, parent_center, search_center);
                        getLeafNodesRecurs(voxels, max_depth, tree->getNodeChild(node, i), depth + 1, 
                                 search_center, tree_center, tree, occupied);
                    }
              }
         }
        else {
            if (tree->isNodeOccupied(node) == occupied) {
                   double voxelSize = tree->getResolution() * pow(2., double(16 - depth));
                   voxels.push_back(std::make_pair(parent_center - tree_center, voxelSize));
            }
        }
    }
}

リーフノードを再帰的に探索して、データをリストに格納します。

    std::list<octomap::OcTreeVolume> list_depr;
    std::list<octomap::OcTreeVolume> list_iterator;

    /**
     * get all occupied leafs
     */
    unsigned char maxDepth = 16;
    const unsigned int tree_max_val(32768);
    octomap::point3d tree_center;
    tree_center(0) = tree_center(1) = tree_center(2)
            = (float)(((double)tree_max_val) * tree->getResolution());

    getLeafNodesRecurs(list_depr, maxDepth, (octomap::OcTreeNode*)tree->getRoot(), 0,
             tree_center, tree_center, (octomap::OcTree*)&(*tree), true);

    for (octomap::ColorOcTree::iterator it = tree->begin(maxDepth), end = tree->end(); it != end; ++it) {
        if (tree->isNodeOccupied(*it))
        {
             list_iterator.push_back(octomap::OcTreeVolume(it.getCoordinate(), it.getSize()));
        }
    }

取得したリストを使用して、結果を描画する例はこんな感じです。

    ofSetColor(ofColor::yellow);
    for (std::list<octomap::OcTreeVolume>::iterator it = list_iterator.begin();
         it != list_iterator.end(); ++it) {
        ofDrawSphere(it->first.x(), it->first.y(), it->first.z(), tree->getResolution());
    }

参考資料

[1] OctoMap - 3D occupancy mapping
[2] http://www2.informatik.uni-freiburg.de/~hornunga/pub/hornung13roscon.pdf
[3] ソースコードの参考に。ただし、ソース付属のチュートリアルとほぼ一緒です。
SLAM拾萃(1)：octomap - 半闲居士 - 博客园

ROS関連
[4] Octomapで遊んでみた
[5] Octomapについての簡単に3次元地図を作る時のメモ - My Research Memo + Silly Story
OctoMap-3D についてのメモ - My Research Memo + Silly Story

今日の本文

沼津が誇る大漫画家西風先生の待望の新作が出ました！車が好きな人、車を勉強したい人に、とてもおすすめです。正しい偏った車知識が自然と身に付きます。

GARAGE PARADISE 1 (SPコミックス)

GARAGE PARADISE 2 (SPコミックス)

GARAGE PARADISE 3 (SPコミックス)

トラ技2018年10月号で紹介されている無限スプライン関数の計算アルゴリズムを試してみる

データストリームに対してリアルタイムで逐次に3次スプライン補間を計算できるアルゴリズムだそうです。

トランジスタ技術 2018年 10 月号

SSDAC特設サイト

#include <iostream>
#include <math.h>
#include <vector>
#include <iterator>
#include <algorithm>

#include <omp.h>

double calc_coefficient_b(double *d, int n)
{
  static const double alpha = -2. + sqrt(3.);
  static const double a = -3. * (sqrt(3.) - 1.);
  static const double b = 3. * (2. * sqrt(3.) - 3.);
  double e(0.), f(0.);
  for(int i=n; i>0; i--){
    f = d[n+i] + alpha * f;
    e = d[n-i] + alpha * e;
  }
  return a * d[n] + b * (f + e);
}

int main() {
  static const int n = 9;
  static const int multiples = 64;
  std::vector<double> output;
  output.clear();

  // input-data preparation
  double samples[] = {0., 2., 3., 5., 0., -1., 2., -3., -4., -3., 0., 3., 3., 6., 0., 9., 1.};
  std::vector<double> data(samples, std::end(samples));
  int n_samples = data.size();
  data.insert(data.begin(), n, 0.);
  data.insert(data.end(), n, 0.);

  // calc-preparation
  std::vector<double> x3, x2, x1;
  x3.clear();
  x2.clear();
  x1.clear();
  for(int i = 0; i < multiples; i++){
    double x = static_cast<double>(i) / static_cast<double>(multiples);
    x1.push_back(x);
    x2.push_back(pow(x, 2));
    x3.push_back(pow(x, 3));
  }
  std::vector<double> interpolated;
  interpolated.resize(multiples);
  
  int start_idx = 0;
  double bj, bj_1, dj, dj_1;
  bj = calc_coefficient_b(&data[start_idx], n);
  dj = data[start_idx];
  for(int j = start_idx; j < n_samples; j++){
    // calc coefficients
    bj_1 = calc_coefficient_b(&data[j + 1], n);
    dj_1 = data[j + n + 1];
    double aj = (bj_1 - bj) / 3.;
    double cj = dj_1 - dj - 2. * bj / 3. - bj_1 / 3.;

    // calc oversampling
#pragma omp parallel for
    for(int i = 0; i < multiples; i++){
      interpolated[i] = aj * x3[i] + bj * x2[i] + cj * x1[i] + dj;
    }
    std::copy(interpolated.begin(), interpolated.end(), std::back_inserter(output));
    
    bj = bj_1;
    dj = dj_1;
  }

  // outputs
  std::cout << "samples:" << std::endl;
  for(int i = start_idx; i < n_samples; i++){
    std::cout << samples[i] << std::endl;
    for(int j = 0; j < multiples - 1; j++){
      std::cout << 0. << std::endl;
    }
  }
  std::cout << "output:" << std::endl;
  for(int i=0; i<output.size(); i++){
    std::cout << output[i] << std::endl;
  }

  return 0;
}

適当な入力データに対する計算結果はこんな感じ。

b:
2.1531

• 2.03546

2.98887

• 6.91988

3.69088
4.1564

• 8.31614

5.10831

• 0.117074

1.35999
0.677115

• 4.06843

6.59671

• 13.3183

19.6765

• 20.3877

10.8744

• 2.10998

きれいにスプライン補間できていそうですね。
あまりにもアルゴリズムが簡単すぎて、これが本当に新しくて凄いものなのかピンと来ていません。

[1] https://www.tezukuri-amp.org/bunkakai/dac/cgi-bin/img-box/img20170804205124.pdf

GitHub - kritzikratzi/ofxAvCodec: openFrameworks wrapper for FFmpeg/libavcodec
GitHub - roymacdonald/ofxSoundObjects: openFrameworks addon implementation for the ofSoundObject implementaion developed at the YCAM '13 ofDevCon, yet never released