Zenn
🎞️

PCLで時系列点群データを可視化する

2024/10/03に公開
C++
pcl
点群処理
点群
tech

はじめに

PCL(Point Cloud Library)を使って時系列点群データ（動画のように撮影された3次元点群データの集まり）を可視化する方法を紹介します。

可視化のイメージはこんな感じです。

実装

pcl::visualization::PCLVisualizerを使います。

PCLが提供している可視化クラスは、一つの動画ファイルとして保存されるような時系列点群データには対応していません。

そのため、フレームごとにpcl::PointCloud型にデータを入れ、表示する点群データを都度切り替えることにより、コマ送り（あるいはパラパラ漫画）の要領で可視化を行います。

この発想を踏まえてpcl::visualization::PCLVisualizerのリファレンスを見てみると、updatePointCloudという関数が目に留まります。

updatePointCloudでは、既にaddPointCloudされた点群データのidを指定し、表示される点群データを上書きします。

さて、以上の情報をもとにプログラムを作成すると、次のようになります。

vis->spinOnceとstd::this_thread::sleep_forの時間は適当です。

main.cpp
using PointXYZRGB = pcl::PointXYZRGB;
using CloudXYZRGB = pcl::PointCloud<PointXYZRGB>;
using PCLVisualizer = pcl::visualization::PCLVisualizer;
using namespace std::chrono_literals;

void visualizeVideo(const std::vector<CloudXYZRGB::Ptr>& video) {

    const int size = video.size();
    if (size == 0) {
        return;
    }
	
    PCLVisualizer::Ptr vis(new PCLVisualizer);

    int index = 0;
    const std::string id_cloud = "cloud";
    vis->addPointCloud(video[index], id_cloud);

    const int x_pos = 100;
    const int y_pos = 100;
    const std::string id_text = "text";
    vis->addText(std::to_string(index + 1) + "/" + std::to_string(size), x_pos, y_pos, id_text);

    while (!vis->wasStopped()) {

        vis->spinOnce(15);
        std::this_thread::sleep_for(15ms);

        index++;
        if (index == size) {
            index = 0;
        }
        vis->updatePointCloud(video[index], id_cloud);
        vis->updateText(std::to_string(index + 1) + "/" + std::to_string(size), x_pos, y_pos, id_text);
    }
}

デモンストレーションのため、疑似的な動画を作成する関数も定義します。

main.cpp
void makeVideoExample(std::vector<CloudXYZRGB::Ptr>& video_out) {

    // parameters
    const int x_min = -30;
    const int x_max = 30;
    const int y_max = 30;
    const int frames = 100;
    const int cloud_width = x_max - x_min + 1;

    // define gaussian distribution
    const double pow_sigma = 100.0;
    const double coeff = 1.0 / sqrt(2.0 * M_PI * pow_sigma);
    auto calculateGauss = [&coeff, &pow_sigma](double val) -> double {
        return coeff * std::exp(-1.0 * val * val / (2.0 * pow_sigma));
    };
    const double normalize_coeff = 1.0 / calculateGauss(0.50 * (x_min + x_max));

    // resize output vector
    video_out.clear();
    video_out.resize(frames);

    // make pseudo-video
    const double dr = 0.50 / frames;
    double rate = 0.0;
    for (int i = 0; i < frames; i++, rate += dr) {

        // reset shared_ptr
        video_out[i].reset(new CloudXYZRGB);
        video_out[i]->resize(cloud_width);
        int id_cloud = 0;

        // write info
        for (int x = x_min; x <= x_max; x++, id_cloud++) {
            PointXYZRGB point;
            point.x = x;
            point.y = rate * calculateGauss(x) * normalize_coeff * y_max;
            point.r = 255;
            point.g = 255;
            point.b = 255;
            video_out[i]->at(id_cloud) = point;
        }
    }
}

以上の関数をmainの中で呼び出します。

main.cpp
int main() {

    std::vector<CloudXYZRGB::Ptr> video;
    makeVideoExample(video);
    visualizeVideo(video);
		
    return 0;
}

これで、冒頭の可視化が得られます。

おわりに

本記事では、PCLで時系列点群データを可視化する方法を紹介しました。

元々、1年以上前に同じことを試みたのですが、当時はC/C++の理解度とリファレンスを読む能力が足りずに挫折したので、プログラムを作ることができて自身の成長を感じました。

質問や疑問などは、コメント欄かX(Twitter)でご連絡ください。

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

Discussion