ホーム / 技術系 /

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

2020年5月21日木曜日

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

t f B! P L

はじめに

PCLを用いて、3次元点群から様々なモデルをRANSACなどで抽出することができます。 (どのようなモデルが抽出できるのかはPCLの公式サイトを参考)

ただ抽出結果(coefficients)の日本語での値の意味の解説が見当たらなかったため、備忘録を兼ねてまとめておきます。

平面(SACMODEL_PLANE)以外は日本語での解説が皆無でめっちゃ調べるのに時間がかかったので。

直線(SACMODEL_LINE)抽出を実行して結果を確認するサンプルコードも忘れないようにまとめておきます。

点群の軸（直線）(SACMODEL_LINE)抽出時のモデル(coefficients)のデータ形式

setModelType(pcl::SACMODEL_LINE)とすることで点群の軸(直線)モデルが抽出できる。

直線モデル抽出時には、(引数に渡した)ModelCoefficients内部のfloat型配列valuesに結果が格納される。

その際インデックス0から2が三次元空間上の座標を、 インデックス3から5が各方向を表している。

つまり、配列valuesのデータを用いると、抽出した直線は

点$P(values[0], values[1], values[2])$を通り、方向ベクトル$\vec{a} = (values[3], values[4], values[5])$に平行な直線

となる。

また、媒介変数$t$を用いると、直線の方程式は

$$(x, y, z) = (values[0], values[1], values[2]) + t(values[3], values[4], values[5])$$

と表せられる。

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

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 addLinePoint(int point_num, float x0, float y0, float z0, float a, float b, float c)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i = 0; i < point_num; i++)
    {
        int rd = rand() % 10000;
        float rd2 = (float)rd / 10.0 - 500.0;

        float x = x0 + rd2 * a;
        float y = y0 + rd2 * b;
        float z = z0 + rd2 * 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, "line_coefficients_test");
    ros::NodeHandle nh;

    //固定seed値による乱数初期化
    srand(123456);

    //生成する点群数
    int point_num = 300000;
    //乱数による直線モデルの生成
    float x0 = (rand() % 100) / 10.0 - 5.0;
    float y0 = (rand() % 100) / 10.0 - 5.0;
    float z0 = (rand() % 100) / 10.0 - 5.0;
    float a = (rand() % 100) / 10.0 - 5.0;
    float b = (rand() % 100) / 10.0 - 5.0;
    float c = (rand() % 100) / 10.0 - 5.0;
    //点群生成元直線モデルの表示
    ROS_INFO("Input Model : [x0, y0, z0, a, b, c] = [%lf, %lf, %lf, %lf, %lf , %lf]", x0, y0, z0, a, b, c);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    //直線モデルに基づく点群の生成
    cloud_ptr = addLinePoint(point_num, x0, y0, z0, a, b, c);

    //RANSACを用いた直線モデル抽出処理
    //抽出結果保持用モデル
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    //モデル抽出時に使用された点群のインデックスを保存
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    //segmentationオブジェクトの生成
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    // RANSACにおいて最適化を実施するかどうか
    //trueの場合RANSACで推定された係数に対して最適化が実行され、すべての点に対しての平均二乗誤差が最小化される。
    seg.setOptimizeCoefficients(true);
    //抽出モデルに直線を指定
    seg.setModelType(pcl::SACMODEL_LINE);
    //抽出方法にRANSACを指定
    seg.setMethodType(pcl::SAC_RANSAC);
    //許容する誤差しきい値(直線の場合は直線に対する点群の振れ幅)
    seg.setDistanceThreshold(0.1);
    //RANSACの試行回数
    seg.setMaxIterations(1000);
    //モデル抽出対象点群のセット
    seg.setInputCloud(cloud_ptr);
    //モデル抽出の実行（引数に結果が保存される）
    seg.segment(*inliers, *coefficients);
    //抽出結果モデルの表示
    ROS_INFO("coefficients : [0, 1, 2, 3, 4, 5] = [%lf, %lf, %lf, %lf, %lf , %lf]", 
    coefficients->values[0], coefficients->values[1], coefficients->values[2], coefficients->values[3], coefficients->values[4], coefficients->values[5]);

    return 0;
}

実行結果

[ INFO] [1590028506.989304211]: Input Model : [x0, y0, z0, a, b, c] = [4.900000, -2.700000, 4.600000, -3.200000, -3.800000 , 4.600000]
[ INFO] [1590028507.066225221]: coefficients : [0, 1, 2, 3, 4, 5] = [4.118380, -3.628235, 5.723576, -0.472637, -0.561255 , 0.679416]

ここで

$$ \frac{4.118380 - 4.90}{-3.20} \fallingdotseq \frac{-3.628235 - (-2.70)}{-3.80}\fallingdotseq \frac{5.723576 - 4.60}{4.60} \fallingdotseq 0.2443 $$

かつ

$$ (-3.20, -3.80, 4.60) \fallingdotseq 6.77 (-0.472637, -0.561255 , 0.679416) $$

となるため、元の直線モデルを抽出出来ており、直線モデル抽出時の、ModelCoefficientsの構造と意味が確認出来た。

適当に乱数のseed値を変更することでほかの直線モデルの場合にどのようなモデルが抽出されるのかを確認することができます。

QooQ