ホーム / 技術系 /

【ROS】PCLを用いて平面モデル(SACMODEL_PLANE)を抽出した際の抽出結果モデル(coefficients)のデータ構造と意味

2020年5月23日土曜日

PCL ROS プログラミング 技術系

t f B! P L

はじめに

どのようなモデルが抽出できるのかはPCLの公式サイトに一覧があります。

ただ公式サイトを見ても抽出結果(coefficients)のデータ形式と値の意味が見当たらなかったため、備忘録を兼ねてまとめておきます。

今回は平面(SACMODEL_PLANE)の抽出と、平面上に点群を配置し、平面(SACMODEL_PLANE)抽出を実行して結果を確認するサンプルコードです。

点群の平面成分(SACMODEL_PLANE)

setModelType(pcl::SACMODEL_PLANE)とすることで点群の平面成分モデルが抽出できる。

平面モデル抽出時には、ModelCoefficients内部のfloat型配列valuesのインデックス0から3が、3次元空間上の平面を表す方程式

$$ax + by + cz + d = 0$$

の各係数$a$, $b$, $c$, $d$に対応している。

つまり、配列valuesを用いると平面の方程式は、

$$values[0] x + values[1] y + values[2] z + values[3] = 0$$

と表せられる。

平面抽出のサンプルコード及び実行結果

PCLのRANSACを用いた平面抽出のサンプルコード

#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <stdio.h>
#include <time.h>

//ある平面上に存在する点群を生成する
pcl::PointCloud<pcl::PointXYZ>::Ptr addPlanePoint(int point_num, float a, float b, float c, float d)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i = 0; i < point_num; i++)
    {
        float rd = (float)(rand() % 1000) / 10.0 - 50.0;
        float rd2 = (float)(rand() % 1000) / 10.0 - 50.0;
        //平面数式ax + by + cz + d = 0から、この平面を通る点を生成
        float x = rd;
        float y = rd2;
        float z = (-a * x - b * y - d) / c;
        cloud_ptr->points.push_back(pcl::PointXYZ(x, y, z));
    }
    return cloud_ptr;
}

int main(int argc, char **argv)
{
    //ROSの初期化処理
    ros::init(argc, argv, "plane_coefficients_test");
    ros::NodeHandle nh;
    //固定seed値による乱数初期化
    srand(123456);

    //生成する点群数
    int point_num = 30000;
    //乱数による平面モデルの生成
    float a = (rand() % 100) / 10.0 - 5.0;
    float b = (rand() % 100) / 10.0 - 5.0;
    float c = (rand() % 100) / 10.0 - 5.0;
    float d = (rand() % 100) / 10.0 - 5.0;
    //点群生成の元となる平面モデルの表示
    ROS_INFO("Input Model : [a, b, c, d] = [%3.1lf, %3.1lf, %3.1lf , %3.1lf]", a, b, c, d);
    //モデルに基づいた点群の生成
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    cloud_ptr = addPlanePoint(point_num, a, b, c, d);

    //RANSACを用いた平面モデル抽出処理
    //抽出結果保持用モデル
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    //モデル抽出時に使用された点群のインデックスを保持する変数のポインタ
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    //segmentationオブジェクトの生成
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    //RANSACにおいて最適化を実施するかどうか
    seg.setOptimizeCoefficients(true);
    //抽出モデルに平面を指定
    seg.setModelType(pcl::SACMODEL_PLANE);
    //抽出モデルにRANSACを指定
    seg.setMethodType(pcl::SAC_RANSAC);
    //許容する誤差しきい値
    seg.setDistanceThreshold(0.1);
    //モデル抽出対象点群のセット
    seg.setInputCloud(cloud_ptr);
    //モデル抽出の実行
    seg.segment(*inliers, *coefficients);
    //抽出結果モデルの表示
    ROS_INFO("coefficients : [0, 1, 2, 3] = [%5.3lf, %5.3lf, %5.3lf, %5.3lf]",
             coefficients->values[0], coefficients->values[1], coefficients->values[2], coefficients->values[3]);
    return 0;
}

実行結果

[ INFO] [1590114638.705926033]: Input Model : [a, b, c, d] = [4.9, -2.7, 4.6 , -3.2]
[ INFO] [1590114638.713621495]: coefficients : [0, 1, 2, 3] = [0.677, -0.373, 0.635, -0.442]

ここで

$$ (4.9, -2.7, 4.6, -3.2) \fallingdotseq 7.24 (0.677, -0.373, 0.635, -0.442) $$

となるため、元の平面モデル（方程式）と、抽出した平面モデルが一致していることがわかる。 また、平面モデル抽出時の、ModelCoefficientsの構造と意味が確認できた。

適当に乱数のseed値を変更した場合にも、点群生成元モデルと同じモデルが抽出できます。

QooQ