今回はChoeonoidで3次元点群データを取得する方法について解説します。
本記事は、ChoreonoidとPCLのインストールを行っていることが前提となりますので、済んでいない場合は、Choreonoid（以下のChoreonoid公式HPを参考）とPCLをインストールしてください。
https://choreonoid.org/ja/manuals/latest/install/build-ubuntu.html
以下のコマンドを実行し、PCL関連パッケージをインストールします。
sudo apt install libpcl-dev
sudo apt install pcl-tools
1. 使用した環境
・Ubuntu 18.04 LTS
・Choreonoid 1.8
・PCL 1.8.1
2. 3次元点群データとは
3次元点群データとは、一つ一つの点に3次元座標（X, Y, Z）と色の情報（R, G, B）を持ち、物体や地形などを点の集合体として表現するデータのことです。
3次元点群データを使用することで物体や地形を立体的に表現することができます。
点群データの取得例
3. プログラムとモデルのダウンロード
以下のリンクからRangeCameraSample.zipをダウンロードします。
https://rtc-fukushima.jp/wp/wp-content/uploads/2022/10/RangeCameraSample.zip
choreonoid/extディレクトリに解凍し、Choreonoidをビルドします。
3次元点群を取得できるカメラモデルとシンプルコントローラが使用できるようになります。「RangeCameraSample/project/RangeCameraSample.cnoid」を起動し、シミュレーションを実行します。「仮想ジョイスティック」ビューを選択後、[A]キーを押すと点群データを取得・保存できます。
4. 3次元点群データを取得するためのモデル
3次元点群データを取得するには、カメラモデルに点群を取得する設定を記述する必要があります。
設定方法は、以下のページの[Cameraノード]項目にあります。
https://choreonoid.org/ja/manuals/latest/handling-models/modelfile/yaml-reference.html#camera
カメラモデルの[format]項目（45行目）を”COLOR_DEPTH”に設定することで、色付きの3次元点群データを取得できるようになります。
実際のモデルには以下の様に記述します。
ダウンロードしたファイル「RangeCameraSample/model/RangeCameraSample.body」の40～53行目の部分を確認してください。
モデルを読み込むと以下の様なカメラモデルが読み込まれます。
・RangeCameraSample.body
format: ChoreonoidBody
formatVersion: 1.0
angleUnit: degree
name: RangeCameraSample
links:
-
name: CameraBody
jointType: fixed
mass: 100.0
centerOfMass: [ 0, 0, 0 ]
inertia: [ 1, 0, 0,
0, 1, 0,
0, 0, 1 ]
elements:
Transform:
translation: [ 0.25, 0, 0.02 ]
elements:
-
type: Shape
geometry: { type: Box, size: [0.25, 0.1, 0.1] }
appearance: &Rail_appearance
material:
diffuseColor: [ 0.1, 0.1, 0.8 ]
specularColor: [ 0.5, 0.5, 0.5 ]
shininess: 0.6
-
translation: [ 0.0, 0.05, 0.0 ]
type: Shape
geometry:
type: Cylinder
radius: 0.03
height: 0.02
appearance:
material:
diffuseColor: [ 0.8, 0.8, 0.8 ]
specularColor: [ 0.5, 0.5, 0.5 ]
shininess: 0.6
-
type: Camera
name: RangeCameraSample
translation: [ 0.0, 0.0, 0.0 ]
rotation: [ [ 1, 0, 0, 90 ] ]
id: 4
format: COLOR_DEPTH
lensType: Normal
on: true
width: 640
height: 480
fieldOfView: 57
nearClipDistance: 0.4
farClipDistance: 4.5
frameRate: 30
5. 3次元点群データを取得するためのプログラム
以下の[RangeCameraSample/controller/RangeCameraController.cpp]は、[A]キーを押すと点群データを取得・保存するプログラムになります。
・RangeCameraController.cpp
#include <cnoid/SimpleController>
#include <cnoid/Camera>
#include <cnoid/RangeCamera>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <cnoid/Joystick>
using namespace cnoid;
using namespace std;
class RangeCameraController : public SimpleController
{
RangeCamera* RangeCameraSample;
std::ostream* os;
SimpleControllerIO* io;
Joystick joystick;
bool PrevButtonState;
public:
virtual bool initialize(SimpleControllerIO* io) override
{
cout<<"start initalize"<<endl;
this->io = io;
os = &io->os();
RangeCameraSample = io->body()->findDevice<RangeCamera>("RangeCameraSample");
io->enableInput(RangeCameraSample);
return true;
}
virtual bool start() override
{
cout<<"start start"<<endl;
return true;
}
virtual bool control() override
{
joystick.readCurrentState();
bool buttonState = joystick.getButtonState(cnoid::Joystick::A_BUTTON);
if(buttonState && !PrevButtonState){
cout<<"push A_BUTTON"<<endl;
getPointCloud();
}
PrevButtonState = buttonState;
return true;
}
void getPointCloud()
{
// 現在のシーンの画像取得
const Image& RangeImage = RangeCameraSample->constImage();
// 現在のシーンの画像保存
RangeImage.save("pointcloud.png");
// 画像の高さと横幅
const int width = RangeImage.width();
const int height = RangeImage.height();
// 色データを取得
const unsigned char* pixels = RangeImage.pixels();
// Point Cloudの変数宣言
pcl::PointCloud<pcl::PointXYZRGB> cloud;
// Point Cloudの初期化
cloud.width = width;
cloud.height = height;
cloud.is_dense = false;
cloud.points.resize(cloud.width * cloud.height);
std::size_t i = 0;
// Point Cloudに各点の値（座標、色）を格納
for(const auto& e: RangeCameraSample->constPoints()) {
// X,Y,Zを格納
cloud[i].x = e(0);
cloud[i].y = e(1);
cloud[i].z = e(2);
// 色（R,G,B）を格納
cloud[i].r = pixels[3*i + 0];
cloud[i].g = pixels[3*i + 1];
cloud[i].b = pixels[3*i + 2];
++i;
}
//点群データを保存
pcl::io::savePCDFileBinaryCompressed ("pointcloud.pcd", cloud);
cout<<"save pointcloud.pcd"<<endl;
}
};
CNOID_IMPLEMENT_SIMPLE_CONTROLLER_FACTORY(RangeCameraController)
【補足解説】
3行目：Choreonoidで点群データを取得するためには、RangeCameraクラスの定義の読み込みが必要となります。
#include <cnoid/RangeCamera>
30-31行目：この記述でRangeCameraを読み込み、データが取得できるようになります。
RangeCamera = io->body()->findDevice<RangeCamera>("RangeCameraSample");
io->enableInput(RangeCamera);
75行目：constPoints関数で点群データを取得します。取得できるデータは各点の３次元座標（X, Y, Z）の情報です。
RangeCameraSample->constPoints();
62行目：色のデータはpixels関数で取得します。取得できるデータは各点のRGB情報です。戻り値が[unsigned char*]のため、1次元配列のように使用できます。
const unsigned char* pixels = RangeImage.pixels()
ここまでがChoreonoid内で3次元点群データを取得する方法です。
このあとで、取得したデータをPCL（Point Cloud Library）を使用して、扱いやすい形式に変換します。
変換後は、C++で点群データを使用できるようになります。
7-8行目：C++でPCLを使用するために、インクルードが必要となります。
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
65行目：プログラムでPCLを使用するためには、変数を宣言します。
pcl::PointCloud<pcl::PointXYZRGB> cloud;
89行目：データの保存を行います。
pcl::io::savePCDFileBinaryCompressed ("pointcloud.pcd", cloud);
"pointcloud.pcd" は、プログラムを実行した場所（Choreonoidを起動した場所）に保存されます。
6. 保存されたデータの確認
プログラムを実行した場所（Choreonoidを起動した場所）で以下のコマンドを実行してください。
pcl_viewer pointcloud.pcd
点群データを表示することができます。マウスで向きの変更が可能ですのでいろいろ動かしてみてください。
pcl_viewerの表示例1
pcl_viewerの表示例2
いかがでしたでしょうか。
これまで、Choreonoidで画像を取得する方法は説明がありましたが、点群データの取得方法についてはどこにも記載がありませんでしたので今回紹介させていただきました。
ぜひ、ご活用ください。