PCLで三次元物体認識

どうもこんにちは、こんな自動航行船のシミュレータを作っている者です。
現在次回大会へ向けて鋭意作業中なわけですが、この大会割とフィールド内においてあるものの形状が決まっています。
そこでstlファイルを読み込んで点群処理をして勝手に三次元物体認識をしてくれるノードがほしいなあ・・・ということで実際に作ってみました。
こちらのパッケージの動作実験の様子は

こんな感じになっています。

#アルゴリズム

こちらの三次元物体認識パッケージの動作原理は以下のアルゴリズムで動作しています。
1.見つけたい物体をモデリングした.stlファイルをロードして三次元特徴量(SHOT特徴量)を記述
2.LRFからロボット周辺の点群を取得し三次元特徴量(SHOT特徴量)を記述
3.pcl::Hough3DGroupingを用いて見つけたい物体の特徴量とボット周辺の点群から得られた特徴量を比較、物体を検出
4.２と３を繰り返す

このPCLのサンプルを参考に実装しました。

SHOT特徴量

SHOT特徴量は2010年にによってTombarらによって発表された姿勢変化に不変な三次元特徴量です。
Local Reference Frameを設定し注目点の近辺を主成分分析して基準座標系を算出、Local Reference Frameを元に周囲の空間を32分割してヒストグラムを算出します。
こちらの資料のp.61にある解説が非常に明快で分かりやすかったです。
PCLにおいてはSHOT特徴量は以下の手順で計算することができます。
ソースコードはPCLのサンプルより使用させていただいています。
1.対象とする点群の法線を計算

sample.cpp

//
//  Compute Normals
//
pcl::PointCloud<PointType>::Ptr model (new pcl::PointCloud<PointType> ());
pcl::PointCloud<NormalType>::Ptr model_normals (new pcl::PointCloud<NormalType> ());

pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
norm_est.setKSearch (10);
norm_est.setInputCloud (model);
norm_est.compute (*model_normals);

2.点群をダウンサンプルし、特徴点を抽出

sample.cpp

//
//  Downsample Clouds to Extract keypoints
//
pcl::PointCloud<PointType>::Ptr model_keypoints (new pcl::PointCloud<PointType> ());

pcl::UniformSampling<PointType> uniform_sampling;
uniform_sampling.setInputCloud (model);
uniform_sampling.setRadiusSearch (model_ss_);
uniform_sampling.filter (*model_keypoints);
std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;

3.特徴点からSHOT特徴量を計算

sample.cpp

//
//  Compute Descriptor for keypoints
//
pcl::PointCloud<DescriptorType>::Ptr model_descriptors (new pcl::PointCloud<DescriptorType> ());
  
pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;
descr_est.setRadiusSearch (descr_rad_);
descr_est.setInputCloud (model_keypoints);
descr_est.setInputNormals (model_normals);
descr_est.setSearchSurface (model);
descr_est.compute (*model_descriptors);

#特徴点のマッチングとクラスタリング
1.特徴点同士をマッチング

sample.cpp

//
//  Find Model-Scene Correspondences with KdTree
//
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
cl::KdTreeFLANN<DescriptorType> match_search;
match_search.setInputCloud (model_descriptors);

//  For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector.
for (size_t i = 0; i < scene_descriptors->size (); ++i)
{
  std::vector<int> neigh_indices (1);
  std::vector<float> neigh_sqr_dists (1);
  if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs
  {
    continue;
  }
  int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
  if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) //  add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
  {
    pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
    model_scene_corrs->push_back (corr);
  }
}
std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;

KdTree構造で表現されたSHOT特徴量の記述子を探索し、マッチングを行っています。
シーンの点群データにふくまれる一つ一つ特徴点をモデルの点群データの特徴点とマッチングさせています。

2.クラスタリング

sample.cpp

//
//  Actual Clustering
//
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
std::vector<pcl::Correspondences> clustered_corrs;
//
//  Compute (Keypoints) Reference Frames only for Hough
//
pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ());
pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ());

pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
rf_est.setFindHoles (true);
rf_est.setRadiusSearch (rf_rad_);

rf_est.setInputCloud (model_keypoints);
rf_est.setInputNormals (model_normals);
rf_est.setSearchSurface (model);
rf_est.compute (*model_rf);

rf_est.setInputCloud (scene_keypoints);
rf_est.setInputNormals (scene_normals);
rf_est.setSearchSurface (scene);
rf_est.compute (*scene_rf);

//  Clustering
pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
clusterer.setHoughBinSize (cg_size_);
clusterer.setHoughThreshold (cg_thresh_);
clusterer.setUseInterpolation (true);
clusterer.setUseDistanceWeight (false);

clusterer.setInputCloud (model_keypoints);
clusterer.setInputRf (model_rf);
clusterer.setSceneCloud (scene_keypoints);
clusterer.setSceneRf (scene_rf);
clusterer.setModelSceneCorrespondences (model_scene_corrs);

//clusterer.cluster (clustered_corrs);
clusterer.recognize (rototranslations, clustered_corrs);

ハフ変換を利用したクラスタリングアルゴリズムを実行しています。
シーンと対象物の対応関係を3次元ハフ空間で投票を行い、最終的な物体とシーンの対応関係を決定します。

以上のようなアルゴリズムを組み合わせて三次元物体認識を行えるようなROSノードを作ることができました。