視覚センサのシミュレーション¶

視覚センサ ¶

Choreonoidでは、視覚センサとして、以下のデバイス型が定義されています。

Camera
RangeCamera
RangeSensor

Cameraはビデオカメラに対応するデバイスです。これは一定のフレームレートで連続して２次元画像データを取得します。

RangeCameraはCameraを拡張したもので、２次元画像に加えて、それに対応する深度マップ（画像内の物体表面までの視点からの距離を格納したもの）も取得します。このタイプに該当するセンサの例として、Kinectが挙げられます。

RangeSensorはレーザを用いた３次元計測装置を想定したデバイスです。通常は一ライン分計測した１次元の距離データを出力しますが、RangeCameraの深度マップと同様の２次元の距離データを出力可能なものもあります。

このような視覚センサはロボットに搭載するセンサとして一般的なもので、シミュレーションについても需要が多いものです。以下では、Choreonoidでこれらのセンサのシミュレーションを行う方法について解説します。

視覚センサの追加 ¶

視覚センサを利用するためには、まずボディモデルにおいて使用したいセンサがデバイスとして定義されている必要があります。

Choreonoidのモデルファイル（拡張子body)においては、各種センサ・デバイスを定義するノードの仕様に従ってセンサを記述しておきます。

視覚センサのシミュレーション機能は、通常サブシミュレータとして実装され、シミュレータアイテムと組み合わせて利用します。そのようなサブシミュレータをChoreonoidでは「ビジョンシミュレータ」と呼んでいます。標準で利用可能なビジョンシミュレータとして「GLビジョンシミュレータ」があります。これはシーンビューの描画に用いているものと同じ描画機能を用いて、視覚センサのデータを生成するというものです。この描画機能はOpenGLというAPIを用いて実装されていますので、GLビジョンシミュレータという名称となっています。

ビジョンシミュレータは、シミュレータアイテムの子アイテムとして配置することで機能します。GLビジョンシミュレータを利用する場合は、メインメニューの「ファイル」-「新規」-「GLビジョンシミュレータ」によってそのアイテムを生成することができますので、これを対象となるシミュレータアイテムの子アイテムとして配置してください。

そのようにしておけば、シミュレータアイテムが対象とする仮想世界に対して、視覚センサのシミュレーションも行われるようになります。具体的には、Cameraデバイスの画像データやRangeCameraの深度マップ、RangeSensorの距離データについて、デバイスに設定されたフレームレートでデータが更新されることになります。

GLビジョンシミュレータの設定項目 ¶

視覚センサのシミュレーションを行うための基本的な設定は上記のとおりですが、GLビジョンシミュレータアイテムのプロパティによって細かい部分の設定を行うことができます。設定に関わる各プロパティの内容を以下に示します。

プロパティ	意味
対象ボディ	視覚センサシミュレーションの対象となるボディモデルを名前で指定します。カンマで区切って複数指定することも可能です。何も指定しない場合は全てのモデルが対象となります。シミュレーション対象のボディモデルを限定したい場合のみ、この項目を設定します。対象モデルを限定することで、シミュレーション速度が向上する場合があります。
対象センサ	シミュレーション対象の視覚センサを名前で指定します。カンマで区切って複数指定することが可能です。「対象ボディ」と同様に、シミュレーション対象のセンサを限定したい場合のみ、この項目を設定します。
最大フレームレート	全てのセンサに対して、センサの仕様値に関わらず、ここに設定したフレームレートが最大となります。フレームレートを落とすことでシミュレーション速度を向上させたい場合にこの項目を設定します。
最大レイテンシ	センサが計測を開始してからその結果をデータとして出力可能になるまでの時間（レイテンシ）に最大値を設定します。全センサに対して、センサの仕様に関わらず、この時間が経過するとデータが出力可能となります。この値を小さくすると、シミュレーションが遅くなる場合があります。
ビジョンデータの記録	カメラ画像や距離データ等、視覚センサを用いて取得したデータを、シミュレーション結果の記録におけるデバイス状態の記録に含めるかを設定します。これらのデータは一般的にサイズが大きく、短時間の記録でも多くのメモリを消費してしまうため、通常は含めないようにします。
センサ個別スレッド	複数の視覚センサをシミュレーションする状況で、各センサごとに専用のスレッドを割り当てるかどうかを設定します。通常はtrueにしておきますが、センサの数や使用しているGPUによってはfalseにした方がシミュレーション速度が向上する場合もあります。
ベストエフォート	視覚センサにはフレームレートが設定されており、そのフレームレートの間隔でデータを更新するようになっています。ベストエフォートをtrueとすると、そのフレームレート内に更新が間に合わなくてもよいものとします。実際の間隔は、シミュレータ内部でのデータ生成処理に依存します。逆にfalseにすると、設定されたフレームレートに従って更新が行われます。ただしデータ生成処理がその時間内に終了しない場合も、終了を待つ必要があるため、その分シミュレーション速度が遅くなる可能性があります。従って、フレームレートを守ることよりシミュレーション速度の向上が重要である場合は、この項目をtrueとしてください。
全てのシーンオブジェクト	3DCGとして表示可能なオブジェクトを「シーンオブジェクト」と呼びます。プロジェクトアイテムに関しては、アイテムツリービュー上でチェックを入れるとシーンビュー上に表示されるものが「シーンオブジェクト」です。この項目では、視覚センサから見える仮想世界に、ボディアイテム以外のシーンオブジェクトを含めるかどうかを設定します。ボディアイテム以外のシーンオブジェクトとしては、例えばシーンアイテムがあります。これはシミュレーションにおける力学的な挙動には影響しませんが、仮想世界の見た目の要素として利用することができます。
レンジセンサ精度係数	レンジセンサの距離データは、OpenGLの深度バッファを用いて生成しています。本項目では、距離センサの解像度に対する深度バッファの解像度の割合を設定します。値を大きくすると距離データの精度が向上します。
深度エラー	レンジセンサの距離データに一定のオフセットを付加します。本項目はまだ実験的なものですので、積極的な利用は控えて下さい。
ヘッドライト	常に視点から視線方向に向けて光を照らす光源のことを「ヘッドライト」とし、カメラ画像の生成においてこの光源を有効にするかどうかを設定します。
追加のライト	ボディモデルに含まれる光源（ライト）を「追加のライト」とし、カメラ画像の生成においてこの光源を有効にするかどうかを設定します。ライトのシミュレーションを行いたい場合はこれをtureにしておきます。

デフォルトの設定で視覚センサのシミュレーションはひととおり機能しますので、上記の項目は必要に応じて設定してもらうということでOKです。

複数センサのシミュレーション ¶

シミュレーション対象の仮想世界に複数の視覚センサが含まれる場合も、GLビジョンシミュレータアイテムをひとつ用意しておけばデフォルトでそれらが全てシミュレートされます。シミュレートするセンサを限定したい場合は、上に挙げた「対象ボディ」や「対象センサ」のプロパティを設定しておきます。

それぞれのセンサに対して、上で挙げたプロパティを独立して設定したい場合もあるかもしれません。例えばカメラについてはシミュレーション速度になるべく影響を与えないようベストエフォートモードとしたいが、レンジセンサについてはフレームレートを落とさずに計測したい、といったことが考えられます。そのような場合は、複数のGLビジョンシミュレータアイテムを用意し、それぞれの「対象ボディ」「対象センサ」を分けた上で、それぞれ必要な設定を行えばOKです。どちらもシミュレータアイテムの子アイテムとして配置しておけば、シミュレーション中に同時に処理されることになります。

センサ情報の利用 ¶

シミュレートされた画像データや距離データは、シミュレータ内部では対応するDeviceオブジェクトのデータとして格納されています。このデータを何らかの手法で取得することで、センサデータを利用します。

実際にセンサの情報を利用するのは、通常はロボットのコントローラです。コントローラに関しては、各コントローラアイテムがデバイスへのアクセス方法をそれぞれ規定していますので、視覚センサに関してもそれに従ってデータを取得してください。これに関しては、力センサ、レートジャイロ、加速度センサといった他のセンサと同様です。実際のアクセス方法は各コントローラアイテムのマニュアル等を参照してください。

視覚センサの利用例 ¶

以下では視覚センサを利用する例として、ロボットが有するカメラにコントローラからアクセスし、その画像データをファイルに出力するというサンプルを紹介します。

ロボットモデルの用意 ¶

まず、対象とするロボットモデルとして、Cameraデバイスを有するものを用意します。そのようなモデルであれば何でもよいのですが、以下ではSR1モデルを用いることにします。

SR1モデルでは、そのモデルファイル "SR1.body" において視覚センサが以下のように定義されています。

-
  type: Camera
  name: LeftCamera
  translation: [ 0.15, 0.05, 0.15 ]
  rotation: [ [ 0, 1, 0, -90 ], [ 0, 0, 1, -90 ] ]
  id: 0
  format: COLOR
  nearClipDistance: 0.1
  elements: &CameraShape
    Transform:
      rotation: [ 1, 0, 0, 90 ]
      elements:
        Shape:
          geometry: { type: Cylinder, radius: 0.02, height: 0.025 }
          appearance:
            material: { diffuseColor: [ 1, 0, 0 ] }
-
  type: Camera
  name: RightCamera
  translation: [ 0.15, -0.05, 0.15 ]
  rotation: [ [ 0, 1, 0, -90 ], [ 0, 0, 1, -90 ] ]
  id: 1
  format: COLOR
  nearClipDistance: 0.1
  elements: *CameraShape

ここではロボットの左目、右目に対応する2つのCameraデバイスのノードが定義されています。それらのformatは "COLOR" となっており、カラーのカメラ画像を取得することが可能です。

シミュレーションプロジェクトの作成 ¶

次に、このモデルを対象としたシミュレーションプロジェクトを作成しましょう。これも何でもよいのですが、以下ではSR1のサンプルプロジェクトのひとつである"SR1Liftup.cnoid"をベースとして用いることにします。

プロジェクトを読み込んだら、メインメニューの「ファイル」-「新規」から「GLビジョンシミュレータ」を選択し、GLビジョンシミュレータアイテムを生成してください。デフォルトの名前は"GLVisionSimulator"となります。これをアイテムツリービュー上で以下のように配置します。

+ World

+ SR1

+ SR1LiftupController

+ box2

+ Floor

+ AISTSimulator

+ GLVisionSimulator

このように、GLビジョンシミュレータアイテムをシミュレータアイテムの子アイテムとして配置してください。これにより、GLビジョンシミュレータによる視覚センサシミュレーション機能が有効となります。この設定でシミュレーションを行うと、SR1モデルが有する"LeftCamera"と"RightCamera"の２つのカメラについて、対応するDeviceオブジェクトの画像データが更新されるようになります。

サンプルコントローラ ¶

カメラ画像にアクセスするコントローラのサンプルとして、"CameraSampleController" を用いることにします。このコントローラは、ロボットが有するCameraデバイスの一覧をまず表示し、その画像データを一秒ごとにファイルに出力するというものです。

注釈

このコントローラのソースは"sample/SimpleController/CameraSampleController.cpp"になります。SimpleControllerの他のサンプルがビルドされていれば、このサンプルもビルドされているはずです。

プロジェクトにこのコントローラを追加します。コントローラアイテムの生成、コントローラ本体のセットの例と同様に、「シンプルコントローラ」アイテムを生成して、以下のような配置にします。

+ World

+ SR1

+ SR1LiftupController

+ CameraSampleController

+ box2

+ Floor

+ AISTSimulator

+ GLVisionSimulator

追加したコントローラアイテムの名前をここでは"CameraSampleController"としています。

このアイテムを"SR1LiftupController"の子アイテムとして配置していることに注意してください。このようにすることで、２つのコントローラを組み合わせて動作させることができます。CameraSampleControllerはカメラの使用に特化したコントローラで、これだけだとロボットが崩れ落ちてしまいますので、今回はこのように組み合わせています。SR1LiftupControllerの部分は、ロボットの身体を制御する任意のコントローラと置き換えることが可能です。

注釈

このようにネストさせたコントローラアイテムをまとめて動作させる機能は、シンプルコントローラアイテム特有の機能です。ベースとなるコントローラアイテムの子や孫として追加していくことで、任意個のコントローラを組み合わせることが可能です。内部的にはそれらのコントローラの制御関数がツリー探索順（深さ優先）で実行され、それらの間の入出力も統合されます。

注釈

ボディアイテムの直下に複数のコントローラアイテムを並列に配置することでも、それらを組み合わせて実行することが可能です。この方法は任意のコントローラアイテム型に対応しています。ただしこの場合は入出力が各コントローラで独立に行われることになり、うまく統合されない場合があるので、注意が必要です。

次に、追加したコントローラアイテムの「コントローラ」プロパティに"CameraSampleController"と記述して、コントローラの本体をセットしてください。

シミュレーションの実行 ¶

以上の状態でシミュレーションを開始してください。するとまずメッセージビューに以下のメッセージが表示されます。

Sensor type: Camera, id: 0, name: LeftCamera
Sensor type: Camera, id: 1, name: RightCamera

これは対象のモデルが有しているCameraデバイスをリストアップしたもので、それぞれの実際の型、デバイスid、および名前を表示しています。

その後シミュレーション中に、

The image of LeftCamera has been saved to "LeftCamera.png".
The image of RightCamera has been saved to "RightCamera.png".

という表示と共にそれぞれのカメラ画像がファイルとして保存されます。保存先はChoreonoidを起動したカレントディレクトリで、名前は"センサ名.png"となります。これが１秒おきに最新の画像で更新されます。

保存された画像を適当な画像ビューアで表示させてみてください。保存される画像は、ロボットの左目、右目に対応するカメラの画像をシミュレートしたものです。それぞれの例を以下に示します。

これにより、カメラ画像のシミュレーションができていて、それをコントローラ側で取得できていることが分かります。

注釈

画像ビューアの中には、ファイルの更新を自動的に検知して表示を更新する機能を有するものがあります。例えば、Linux上で動作する画像ビューアの "gThumb" はこの機能を有しています。(Ubuntuでは "apt-get install gthumb" でインストールできます。）そのようなビューアを用いると、シミュレーションの進行に伴ってカメラ画像が更新されていく様子が確認できます。

今回対象としているデバイスは通常のCamera型となりますが、モデルファイル中のCameraノードにおいて format に COLOR_DEPTH を指定すると、距離画像データも取得可能なRangeCameraとすることができます。その場合、距離画像データについても画像データと同様にアクセスできますので、興味があればSR1モデルやサンプルコントローラを改良して試してみて下さい。

サンプルコントローラの実装内容 ¶

CameraSampleControllerのソースコードを以下に示します。

#include <cnoid/SimpleController>
#include <cnoid/Camera>

using namespace cnoid;

class CameraSampleController : public SimpleController
{
    DeviceList<Camera> cameras;
    double timeCounter;
    double timeStep;
    std::ostream* os;

public:
    virtual bool initialize(SimpleControllerIO* io) override
    {
        os = &io->os();

        cameras << io->body()->devices();

        for(size_t i=0; i < cameras.size(); ++i){
            Device* camera = cameras[i];
            io->enableInput(camera);
            *os << "Device type: " << camera->typeName()
                << ", id: " << camera->id()
                << ", name: " << camera->name() << std::endl;
        }

        timeCounter = 0.0;
        timeStep = io->timeStep();

        return true;
    }

    virtual bool control() override
    {
        timeCounter += timeStep;
        if(timeCounter >= 1.0){
            for(size_t i=0; i < cameras.size(); ++i){
                Camera* camera = cameras[i];
                std::string filename = camera->name() + ".png";
                camera->constImage().save(filename);
                *os << "The image of " << camera->name()
                    << " has been saved to \"" << filename << "\"." << std::endl;
            }
            timeCounter = 0.0;
        }
        return false;
    }
};

CNOID_IMPLEMENT_SIMPLE_CONTROLLER_FACTORY(CameraSampleController)

Cameraデバイスの使用については、

#include <cnoid/Camera>

によってCameraクラスの定義を取り込み、

DeviceList<Camera> cameras;

に対して

cameras << io->body()->devices();

とすることでロボットモデルが有する全てのCameraデバイスを取得しています。

このようにして取得したCameraデバイスに関して、initialize関数のforループ内で

io->enableInput(camera);

とすることで、各カメラからの入力を有効化しています。また、カメラの情報をテキストメッセージとして出力しています。

control関数内では

camera->constImage().save(filename);

によってカメラの画像データをファイルに出力しています。ここでは取得した画像データを編集することはないため、constImage()関数を用いています。

Cameraデバイスと関連する部分は以上です。これ以外の部分については、コントローラの実装と共通する部分も多いので、そちらの解説を参考にしてください。

視覚センサのシミュレーション¶