【点群処理】pcl::VoxelGridのボクセルにアクセスする

はじめに

PCL(Point Cloud Library)で点群処理をしていて、pcl::VoxelGridをかけた後、点群の番号ではなくボクセルグリッドの番号でデータにアクセスしたいことがありました。

詳細はこちらのクラスリファレンスに書いてありますが、クラスの使い方を把握するのには、多少時間がかかります。

そこで、私がリファレンスから読み取ったことを以下にまとめます。

ボクセルへのアクセス

目的

$(i, j, k)$をボクセルグリッドにおける$x$方向、$y$方向、$z$方向の番号だとすると、$(i, j, k)$を与えたとき、それに対応するボクセルに格納されているデータを得ることができれば、本記事の目的は達成です。

リファレンスを読む

カプセル化の原則から考えて、pcl::VoxelGridの処理結果はゲッタを介して得られると予想できます。

そこで、クラスリファレンスのうちgetで始まる関数を上から順番に見ていきます。
すると早速、一番上のgetCentroidIndexに次のようなコメントがあることが確認できます。

for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed, and that the point is inside the grid, to avoid invalid access (or use getGridCoordinates+getCentroidIndexAt)

【和訳】
効率のため、leaf layoutの保存を有効にしてからフィルタ処理を行い、点がグリッド内にあることを確認し、無効なアクセスを避けてください。
または、getGridCoordinatesとgetCentroidIndexAtを使ってください。

その次の関数であるgetCentroidIndexAtには、次のように書かれています。

Returns the index in the downsampled cloud corresponding to a given set of coordinates.

【和訳】
ダウンサンプリングされた点群データにおける、与えられた座標に対応するデータの番号を返します。

この記述から、ボクセル位置$(i, j, k)$に対応する点群データの番号は、getCentroidIndexAtを用いて得られることが分かりました。

また、はじめのgetCentroidIndexのコメントを受けてsetから始まるセッタを見ていくと、setSaveLeafLayoutという関数が目に留まります。
同関数には、以下のコメントが書かれています。

Set to true if leaf layout information needs to be saved for later access.

【和訳】
後のアクセスのためにleaf layout情報を保存する必要がある場合はtrueに設定してください。

この記述から、フィルタリングをする前にsetSaveLeafLayoutにtrueを渡すべきであると分かりました。

プログラムを書く

本記事では、名前空間seoi::に記述されたラッパーを、点群の入力と可視化のために使います。
これらの方法が分からない方は、インターネットで別途調べるか、PCLのチュートリアルをご覧ください。

情報が出揃ったので、プログラムを書いていきます。

まずは、点群データを読み込みます。
本記事で使用している点群データは、こちらで配布されているスタンフォードバニーです。

using PointXYZ = pcl::PointXYZ;
using CloudXYZ = pcl::PointCloud<PointXYZ>;
using PointXYZRGB = pcl::PointXYZRGB;
using CloudXYZRGB = pcl::PointCloud<PointXYZRGB>;

int main() {

    // read stanford bunny
    std::string data_path = "hogehoge";
    CloudXYZ::Ptr cloud_mono = seoi::io::loadXYZ(data_path);
    CloudXYZRGB::Ptr cloud(new CloudXYZRGB);
    pcl::copyPointCloud(*cloud_mono, *cloud);
    for (int i = 0; i < cloud->width; i++) {
        cloud->at(i).r = 255.0;
        cloud->at(i).g = 255.0;
        cloud->at(i).b = 255.0;
    }
    seoi::util::visualize(cloud);

    return 0;
}

これを実行すると、次のような出力が得られます。

次に、pcl::VoxelGridを点群データに適用します。
setSaveLeafLayoutを忘れないようにしてください。

int main() {

    // read stanford bunny into "cloud" as mentioned above

    // apply voxel grid filter
    const double leaf_size = 0.02;
    CloudXYZRGB::Ptr filtered_cloud(new CloudXYZRGB);
    pcl::VoxelGrid<PointXYZRGB>::Ptr grid(new pcl::VoxelGrid<PointXYZRGB>);
    grid->setInputCloud(cloud);
    grid->setLeafSize(leaf_size, leaf_size, leaf_size);
    grid->setSaveLeafLayout(true);
    grid->filter(*filtered_cloud);
    
    seoi::util::visualize(filtered_cloud);

    return 0;
}

これを実行すると、次のような出力が得られます。
オリジナルデータより点の数が減っています。

さて、本題のボクセルへのアクセスに取り掛かります。

本記事の目的を踏まえ、任意のボクセル位置$(i, j, k)$を指定し、それに対応する点群データの番号をgetCentroidIndexAtで取得します。
この関数は、ボクセル位置を示すベクトルEigen::Vector3iを引数に取ります。
また、そのボクセル位置にデータが格納されていない場合、この関数は-1を返します。

constexpr int VOXEL_GRID_EMPTY = -1;
int main() {

    // apply voxel grid filter as mentioned above

    // access to a voxel
    Eigen::Vector3i ijk(-4, 8, -3);
    int cloud_index = grid->getCentroidIndexAt(ijk);
    PointXYZRGB point;
    if (cloud_index == VOXEL_GRID_EMPTY) {
        std::cout << ijk << " is invalid access!" << std::endl;
    }
    else {
        point = filtered_cloud->at(cloud_index);
    }
    std::cout << "point = " << point << std::endl;
    
    return 0;
}

本プログラムと同じデータ、同じパラメータ設定であれば、以下のように出力されると思います。
ボクセル位置に基づいて点群データにアクセスし、情報を取り出すことができました。

point = (-0.070192,0.176532,-0.048774 - 255,255,255)

setSaveLeafLayoutにtrueを渡し忘れた場合、ボクセル位置を指定してアクセスすることはできません。
したがって、上記プログラムでは異なるメッセージが出力されます。

-4
08
-3 is invalid access!
point = (0,0,0 - 0,0,0)

ボクセル位置$(i, j, k)$の補足

本記事の目的は既に達成されましたが、$(i, j, k)$について補足をしておきます。

フィルタ適用後の点群データに対するボクセル位置は、pcl::VoxelGridのメンバ関数getGridCoordinatesを用いて取得できます。

int main() {

    // apply voxel grid filter as mentioned above

    // get all ijk
    for (int i = 0; i < filtered_cloud->width; i++) {
        double x = filtered_cloud->at(i).x;
        double y = filtered_cloud->at(i).y;
        double z = filtered_cloud->at(i).z;
        Eigen::Vector3i current_ijk = grid->getGridCoordinates(x, y, z);
        std::cout << "i = " << i << std::endl << current_ijk << std::endl;
    }

    return 0;
}

for文を用いてボクセルを走査したいときは、ボクセル位置の下限・上限が必要です。
感覚的には下限は0のような気がしますが、上記プログラムにあるとおり、点群データによっては負値もあり得ます。
そこで、pcl::VoxelGridのメンバ関数getMinBoxCoordinates、getMaxBoxCoordinatesを用いて下限・上限値を取得します。

int main() {

    // apply voxel grid filter as mentioned above

    // get min and max coordinates
    Eigen::Vector3i min = grid->getMinBoxCoordinates();
    Eigen::Vector3i max = grid->getMaxBoxCoordinates();
    std::cout << "min =" << std::endl << min << std::endl;
    std::cout << "max =" << std::endl << max << std::endl;

    return 0;
}

本記事と同じ条件であれば、以下の出力が得られると思います。

min =
-5
01
-3
max =
2
9
2

minとmaxの各要素には、0番目から順に$x$方向、$y$方向、$z$方向のボクセル位置の下限・上限値が格納されています。
そのため、for文の始まりと終わりにこれらの数値を使えば、ボクセルを走査することができます。

おわりに

本記事では、pcl::VoxelGridの使い方について私がリファレンスから読み取ったことをまとめてみました。

ご質問やご指摘などは、コメント欄かX(Twitter)でお願いします。

本記事が少しでもお役に立てば幸いです。