ROSでPCLのサンプルを動かす - 点群の差分検出(octree change detection)

23445 ワード

PCL ROS PCL テキストリンク

はじめに

3Dセンサから取得された点群から認識したい対象を環境から抽出する方法を調べました.
PCLのOctreePointCloudChangeDetectorというクラスを使用するとその様な処理ができ保存しておいた点群から現在の点群で増加している部分の抽出ができます.
今回はOctreePointCloudChangeDetectorクラスを使用しているPCLのサンプルコードの動作と解説を記載しました.

環境

ubuntu20.04
ROS noetic

コード

下記URLから引用

octree_change_detection.cpp

#include <pcl/point_cloud.h>
#include <pcl/octree/octree_pointcloud_changedetector.h>

#include <iostream>
#include <vector>
#include <ctime>

int
main (int argc, char** argv)
{
  srand ((unsigned int) time (NULL));

  // Octree resolution - side length of octree voxels
  float resolution = 32.0f;

  // Instantiate octree-based point cloud change detection class
  pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ> );

  // Generate pointcloud data for cloudA
  cloudA->width = 128;
  cloudA->height = 1;
  cloudA->points.resize (cloudA->width * cloudA->height);

  for (std::size_t i = 0; i < cloudA->size (); ++i)
  {
    (*cloudA)[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
    (*cloudA)[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
    (*cloudA)[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
  }

  // Add points from cloudA to octree
  octree.setInputCloud (cloudA);
  octree.addPointsFromInputCloud ();

  // Switch octree buffers: This resets octree but keeps previous tree structure in memory.
  octree.switchBuffers ();

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );

  // Generate pointcloud data for cloudB 
  cloudB->width = 128;
  cloudB->height = 1;
  cloudB->points.resize (cloudB->width * cloudB->height);

  for (std::size_t i = 0; i < cloudB->size (); ++i)
  {
    (*cloudB)[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
    (*cloudB)[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
    (*cloudB)[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
  }

  // Add points from cloudB to octree
  octree.setInputCloud (cloudB);
  octree.addPointsFromInputCloud ();

  std::vector<int> newPointIdxVector;

  // Get vector of point indices from octree voxels which did not exist in previous buffer
  octree.getPointIndicesFromNewVoxels (newPointIdxVector);

  // Output points
  std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
  for (std::size_t i = 0; i < newPointIdxVector.size (); ++i)
    std::cout << i << "# Index:" << newPointIdxVector[i]
              << "  Point:" << (*cloudB)[newPointIdxVector[i]].x << " "
              << (*cloudB)[newPointIdxVector[i]].y << " "
              << (*cloudB)[newPointIdxVector[i]].z << std::endl;

}

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(pcl_sample)

find_package(catkin REQUIRED)
find_package(PCL 1.8 REQUIRED)

catkin_package(
)

include_directories(
 include
 ${PCL_INCLUDE_DIRS}
 #${catkin_INCLUDE_DIRS}
)

add_executable(change_detection src/change_detection.cpp)

target_link_libraries(change_detection
  ${PCL_LIBRARIES}
  #${catkin_LIBRARIES}
)

解説

まずOctreePointCloudChangeDetectorクラスのインスタンスを作成しそのvoxel resolutionを定義する.

  srand ((unsigned int) time (NULL));

  // Octree resolution - side length of octree voxels
  float resolution = 32.0f;

  // Instantiate octree-based point cloud change detection class
  pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);

そしてランダムなポイントデータで初期化されたcloudAというpoint cloudインスタンスを作成.作成した点群データは1つのoctree構造体の作成に使用されます.

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ> );

  // Generate pointcloud data for cloudA
  cloudA->width = 128;
  cloudA->height = 1;
  cloudA->points.resize (cloudA->width * cloudA->height);

  for (size_t i = 0; i < cloudA->points.size (); ++i)
  {
    cloudA->points[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
    cloudA->points[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
    cloudA->points[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
  }

  // Add points from cloudA to octree
  octree.setInputCloud (cloudA);
  octree.addPointsFromInputCloud ();

cloudAは参照するための点群でそのoctree構造体がその空間分布を表現します.OctreePointCloudChangeDetectionクラスはOctree2BufBaseクラスを継承しており2つのoctreeを同時にメモリー内で保存したり管理することができます."octree.switchBuffers()"を使用すると過去のoctree構造体をメモリーに保存したままoctreeクラスをリセットすることができます.

  // Switch octree buffers: This resets octree but keeps previous tree structure in memory.
  octree.switchBuffers ();

次に2つ目の点群のインスタンス"cloudB"を作成し,ランダムなデータで満たします.この点群は新しいoctree構造体の作成に使用されます.

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );

  // Generate pointcloud data for cloudB 
  cloudB->width = 128;
  cloudB->height = 1;
  cloudB->points.resize (cloudB->width * cloudB->height);

  for (size_t i = 0; i < cloudB->points.size (); ++i)
  {
    cloudB->points[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
    cloudB->points[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
    cloudB->points[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
  }

  // Add points from cloudB to octree
  octree.setInputCloud (cloudB);
  octree.addPointsFromInputCloud ();

現在のoctree構造体(cloudBから作成された
)で過去のoctree構造体(cloudAから作成された)に存在しないvoxelの点を取得するためにpoint indicesのベクトルを返す"getPointIndicesFromNewVoxelsというメソッドを呼びます.

  std::vector<int> newPointIdxVector;

  // Get vector of point indices from octree voxels which did not exist in previous buffer
  octree.getPointIndicesFromNewVoxels (newPointIdxVector);

最後にstd::cout streamで結果を出力します.

  // Output points
  std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
  for (size_t i = 0; i < newPointIdxVector.size (); ++i)
    std::cout << i << "# Index:" << newPointIdxVector[i]
              << "  Point:" << cloudB->points[newPointIdxVector[i]].x << " "
              << cloudB->points[newPointIdxVector[i]].y << " "
              << cloudB->points[newPointIdxVector[i]].z << std::endl;

結果

以下のようにcloudAベースのoctreeのボクセルとcloudBベースのoctreeのボクセルを比較し増加しているボクセルのインデックスとその座標が表示されます.

result

Output from getPointIndicesFromNewVoxels:
0# Index:3  Point:20.3452 1.81795 17.8999
1# Index:29  Point:15.6769 1.23849 30.9812
2# Index:94  Point:19.2824 0.975498 33.1067
3# Index:36  Point:50.6501 0.0791533 20.7237
4# Index:60  Point:39.7209 0.0998207 15.1994
5# Index:89  Point:40.9676 1.40058 9.41052
6# Index:86  Point:55.8706 1.69124 6.36924
7# Index:33  Point:60.9991 50.969 62.419

参考

解説などは以下URLの内容を参考にしました.

ROS環境の構築やビルドの方法などは以下URLの内容を参照ください.

Author And Source

この問題について(ROSでPCLのサンプルを動かす - 点群の差分検出(octree change detection)), 我々は、より多くの情報をここで見つけました https://qiita.com/hoshianaaa/items/66aab13f44c9bf3e91a2

著者帰属：元の著者の情報は、元のURLに含まれています。著作権は原作者に属する。

Content is automatically searched and collected through network algorithms . If there is a violation . Please contact us . We will adjust (correct author information ,or delete content ) as soon as possible .

自分の夢を追う--------補助制作第三十三課:倉庫にN個の物品を預ける対象アセンブリ実現コード

1962 ProblemD単語置換