=============================================================================
自己／他者運動分離認識コンポーネント version 1.0 2011/08/25
東京大学　情報理工学系研究科　情報システム工学室　次世代プロジェクト担当
Mail: itngr@jsk.t.u-tokyo.ac.jp
=============================================================================

-----概要-----

推定された特徴点群の3次元フローと自己姿勢を元に
自己の位置や回転量を推定しながら，
特徴点群をラベリングして他者の位置や移動判定をするコンポーネント群です．

-----コンポーネントの使い方-----

-- コンパイル
../idlと../functionsでmakeした後，このディレクトリでmakeを行ってください．
以下のようなファイルが生成されます．
1. ./build/bin/MotionClassifierComp
2. ./build/lib/MotionClassifier.so
3. ./build/bin/MotionClassifierTest.conf (このディレクトリにあるファイルのコピー)
4. ./build/bin/rtc.conf (上位ディレクトリにあるファイルのコピー)

-- 起動方法
例えば以下のように実行できます．
% cd ./build/bin
% ./MotionClassifierComp
RTSystemEditorなどでActivateを行い，動作を実行できます．

-----コンポーネントの入出力-----

-- InPort

*TimedEstimatedMotionData in_inertia

推定された自己姿勢を入力します．

TimedEstimatedMotionDataは，

============================================================

  struct ThreeAxisData
  {
    double x;
    double y;
    double z;
  };

  struct Quaternion
  {
    double q1;
    double q2;
    double q3;
    double q4;
  };
  
  struct Motion3DSensorData
  {
    ThreeAxisData accs;
    ThreeAxisData gyros;
    ThreeAxisData mags;    
  };

  typedef double Mat44[4][4];

  typedef sequence<Motion3DSensorData> Motion3DSensorDataList;

  struct TimedMotion3DSensorData
  {
    // Time stamp.
    RTC::Time tm;
    // Sequence of motion sensor data.
    Motion3DSensorDataList motion3dsensors;
  };

  struct EstimatedMotionData
  {
    ThreeAxisData accel;
    ThreeAxisData angularvel;
    ThreeAxisData translationalvel;
    Quaternion quat;
    Mat44 quatcov;
    short isstatic;
  };

  struct TimedEstimatedMotionData
  {
    // Time stamp.
    RTC::Time tm;
    EstimatedMotionData estimatedmotion;
  };  

============================================================

のように定義されております。

*TimedFlow3D in_flow

特徴点群の3Dフローデータを入力します．

TimedFlow3Dは，

============================================================

  struct PointFloat3D
  {
    float x;
    float y;
    float z;
  };

  struct Flow3D
  {
    short count;
    PointFloat3D prev_point;
    PointFloat3D cur_point;
  };

  typedef sequence<Flow3D> Flow3DList;

  struct TimedFlow3D
  {
    // Time stamp.
    RTC::Time tm;
    Flow3DList flowdata;
  };  

============================================================

のように定義されております。

-- OutPort

*TimedWorldQuatCoords out_coords

推定した自己位置と姿勢クオータニオンを出力します。

TimedWorldQuatCoordsは，

============================================================

  struct Quaternion
  {
    double q1;
    double q2;
    double q3;
    double q4;
  };

  struct WorldQuatCoords
  {
    RTC::Vector3D worldpos;
    Quaternion quat;
  };

============================================================

のように定義されております．
WorldQuatCoordsにおいて，
worldposは自己位置をmm単位で表す3次元ベクトルの構造体です．
quatは自己姿勢を表すクオータニオンの構造体です．

*TimedLabeledPCL out_staticpcl
*TimedLabeledPCL out_movingpcl

推定した特徴点群情報を出力します．
特徴点の位置や含まれるラベル番号などの情報を持っています．

TimedLabeledPCLは，

============================================================

  struct LabeledPointCloudPoint
  {
    short count;
    short objectlabel;
    RTC::PointCloudPoint pc_world;
    RTC::PointCloudPoint pc_local;
  };

  typedef sequence<LabeledPointCloudPoint> LabeledPCL;

  struct TimedLabeledPCL
  {
    RTC::Time tm;
    LabeledPCL lpcl;
  };

============================================================

のように定義されております．
LabeledPointCloudPointにおいて，
countはその特徴点が見つかってから数えたフレーム数です．0なら無効，1以上なら有効な特徴点情報であることも示します．
objectlabelはラベル番号を表し，同じ動きをしている塊は同じラベル番号となります．
pc_worldはグローバル座標における特徴点位置を表します．
pc_localはローカル座標における特徴点位置を表します．

out_staticpclは静止していると認識されたラベル内の特徴点群が出力されます．
out_movingpclは移動していると認識されたラベル内の特徴点群が出力されます．
この2つの出力は，1つの特徴点情報の配列out_ftrs_を配列番号ごとにmovingかstaticかに振り分けているもので，
moving，staticそれぞれの配列の空席となる番号にはcount=0を書き込んでいます．
また，連続する2フレームにおけるout_ftrs_の同じ配列番号に有効な特徴点情報があり，countの値も連続していた場合，
2つの特徴点情報は同一の点によるものだと認識されていることになります．

-----ライセンス-----

本ソフトウェアは new BSD License に基づき配布されます．
詳しくは../LICENSE.txtファイルをご覧ください．
