cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

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

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

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

ライブラリの概要はこちらのPDFが最も分かり易かったです。
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で適当に表示してみた例。
f:id:cvl-robot:20180928232557p:plain

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

思いのほか面倒くさいです。データアクセスの方法は、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());
    }

f:id:cvl-robot:20181002210210p:plain

今日の本文

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

GARAGE PARADISE 1 (SPコミックス)

GARAGE PARADISE 1 (SPコミックス)

GARAGE PARADISE 2 (SPコミックス)

GARAGE PARADISE 2 (SPコミックス)

GARAGE PARADISE 3 (SPコミックス)

GARAGE PARADISE 3 (SPコミックス)