/*!
  \page log_091204_page 2009/12/04

  - 概要
    - \ref log_091204_schedule
    - \ref log_091204_affine
    - \ref log_091204_coordinate
    - \ref log_091204_poles
    - \ref log_091204_misc
    - \ref log_091204_next

  \section log_091204_schedule 開発スケジュールの送付

  実現したい項目毎に、仕様検討、シミュレーション、実機実行、のスパイラル開発を行う。\n
  １つの機能の動作が確認できた段階で、次の項目の実装を開始する。スパイラルで開発することで、どの開発段階でも、ロボットが動作する状態になっているようにする。

  \subsection log_091204_schedule_all 全体の開発予定

  \image html log_091204_all.jpg

  - 写真内の文字 (12 時の方向から時計回りの順で記述)
    - URG を用いた位置推定
    - 地図の作成
      - 走行モジュール + URG データ
      - 移動障害物の特定
      - 周囲画像の取得
    - URG データと走行モジュールの位置融合
    - 走行モジュール
      - A* による障害物の回避
      - 連続点列の経路追従
      - A* と連続点列を組合せについて
    - URG データを取得しているときの 3D 姿勢の取得
    - Wii リモコンを用いた位置、姿勢の推定

  これらの項目を、

  - 走る
  - 位置を知る
  - 地図を作る

  の項目に分けたときの、具体的な作業項目を以下に書き出す。\n
  ここで書き出した項目の３つを２週間で実装するペースで開発を行いたい。

  \subsection log_091204_schedule_run 走る

  \image html log_091204_run.jpg

  - 壁沿い走行
  - 点列の追従走行
  - 障害物の回避
  - A* を用いた障害物回避のモジュール化

  \subsection log_091204_schedule_position 位置を知る

  \image html log_091204_position.jpg

  - 壁を用いた位置推定
  - ICP マッチングを作って評価してみる
  - 走れる方向を知る
    - 交差点の検出など
  - ポールを用いた位置推定
  - 向き推定

  \subsection log_091204_schedule_map 地図を作る

  \image html log_091204_map.jpg

  - 移動物体の検出
    - 移動物体のトラッキング
  - 地図の画像生成
    - 画像に追加したデータからの経路、壁、ポール位置データの生成
  - URG データだけからの地図生成
  - データから壁沿い走行やポールを用いた位置推定を使い分けて走行する方法の検討

  \section log_091204_affine アフィン変換について

  ロボットの座標系の操作を実現するにあたり、アフィン変換について調査した。

  アフィン変換については、下記サイトが詳しい。
  http://ft-lab.ne.jp/cgi-bin/wiki.cgi?page=%A5%A2%A5%D5%A5%A3%A5%F3%CA%D1%B4%B9_3DCG

  ロボットの座標系をアフィン変換を用いて計算するにあたり、2D 版の座標系においては、「Z 軸を中心にした回転」と「並行移動」のみを用いる。

  \section log_091204_coordinate 座標系モジュールの仕様について

  地面の座標系にロボットが配置された状態や、ロボットの座標系にセンサが配置された状態を適切に表現するために、複数の座標系をツリー構造で管理できるようにする。

  \image html log_091204_coordinate_image.jpg 複数の座標系のイメージ

  座標系をツリー管理するにあたり、多分木なツリーで管理する。

  \image html log_091204_coordinate_tree.jpg 座標系のツリー管理

  親座標系は、子の座標系の原点がどの位置でどの向きにあるかを保持する。

  \image html log_091204_coordinate_child.jpg ロボット座標系上にセンサ座標系の原点を配置する

  この相対関係は、必要に応じて設定が変更できるようにする。

  座標系は、まずは 2D 版を実装し、必要に応じて 3D 版を開発、統合する。

  \subsection log_091204_coordinate_run 走行モジュールの座標系

  走行モジュールは、ロボットの存在する位置にもう１つの座標系を持つ。

  \image html log_091204_run_robot.jpg ロボット位置に座標系を持たせる

  センサなどは、このロボット位置を原点とした座標系に配置する。\n
  この座標系へは Running::robot() でアクセスする。

  Running::robot() の使用例
  \code
BeegoDevice run;

// ロボットの相対位置に設定した直線に追従
// 今の位置から 1 [m] 先で停止する
run.stopToLine(Position<long>(1000, 0, deg(0)), run.robot());

// ロボット上にセンサを配置する
// ロボットの前方 10 [cm] の位置に URG を配置する
UrgDevice urg;
urg.setOriginTo(run.robot(), Position<long>(100, 0, deg(0))); \endcode

  この、ロボット位置の座標系の位置を修正するには

  - Running::setRobotPosition(const Position<long>& position, coordinate = NULL);
    - ロボット位置を指定した座標系の指定位置に修正する

  - Running::addRobotPositionOffset(const Position<long>& offset, coordinate = NULL);
    - ロボットの位置を今の指定した座標系において、今の位置からずらす

  を用いる。

  \n
  \section log_091204_poles ２本のポールを使った環境での位置推定サンプル

  ポールが環境に２本あるときの位置推定について検討する。

  単に「ポールが２本ある」という情報がセンサから得られた場合、ロボットの位置はポール間を中心とした点対象の２箇所の位置に存在しうる。

  \image html log_091204_poles_noestimate.jpg 初期位置の情報がなければ、ロボットが存在しうる位置は一意に決まらない

  よって、今回はロボットの初期の位置が与えられた状態から位置推定を行う。\n
  初期位置を与えることにより、ポールがある予定の位置とセンサデータからのポールの位置を比較することで、現在のロボット位置が推定できる。

  \image html log_091204_poles_adjust.jpg 想定している位置とセンサで検出した位置のずれを補正する

  この検出した位置のずれを、現在位置に反映させることで、位置補正を行う。\n
  ずれの具体的な検出方法は、２つのポールによる線分を考えたときに

  -# 直線の傾きの差から、角度ずれの補正
  -# 端点の位置のずれから、位置ずれの補正

  を行う。

  推定された自己位置には誤差が含まれているため、現在の位置から徐々に推定された位置に修正されるようにする。修正にあたっては、推定された位置を反映させる割合 (0.1 程度) を定義し、

  next_position = (current_position * 0.9) + (estimate_position * 0.1)

  のような計算式で補正を行う。

  以下に、今回の方法で実装されたプログラムのシミュレーション動作、実機動作させたときの動画を示す。


  \subsection log_091204_poles_simulation シミュレーション動作

  シミュレータを実行するのに必要なファイルは、下記リンクからダウンロードできる。

  - <a href="http://sourceforge.jp/projects/qrobosdk/downloads/44787/positioning_poles.zip/">実行バイナリ</a>
  - <a href="http://sourceforge.jp/projects/qrobosdk/downloads/44787/qrk_dll.zip/">実行に必要な DLL</a>

  ロボットの初期位置は positioning_poles/simulation_settings.lua を修正することで変更できる。

  \verbatim
-- !!! 初期位置を X 軸方向に 100 [mm], 角度を 3 [deg] ずらす場合
-- デバイス位置を設定
local run = DeviceManager():model("run")
run:setPosition(Position(100, 0, deg(3)), nil, false) \endverbatim

  初期位置を変更した状態で positioning_poles.exe を実行すると、怪しげな位置補正をしながら、なんとなく経路走行するのが確認できる。


  \subsection log_091204_poles_real 実機動作

  実機を用いて実環境で走行実験を行った。

  - １回目 \n
    _movie_poles_091204_00_
    それっぽく走行して、停止。

  - ２回目 \n
    _movie_poles_091204_01_
    経路から、だんだんと外れていき、ロッカーに衝突して停止。ロッカーに衝突する少し前から、URG のデータが PC に記録されていなかった。


  \subsection log_091204_poles_notice 気付き

  シミュレーション動作、実機動作を行った際の気付き(主に反省点)を記す。

  - 実環境のロボットは、シミュレータの通りには動作しない。
    - シミュレータに誤差を付加することと、実機が理想的な動作をするよう調整することの、両方が必要。
    - 実機による実験をもっと多く行うべき。

  - 補正を行ったとき、大きく経路をはみ出してから本来の経路を走行している。
    - 補正と経路追従のどこかでおかしくなっている。この現象はシミュレータ動作でも確認できる。
    - 補正を行った後に走行コマンドを実行するサンプルを作り、問題を確認して解決する。

  - ロボットが衝突したときに、PC の電源が切れた状態になっていた。
    - その少し前から URG のデータが受信できない状態になっていたこともあり、何が起きていたかは不明。

  - ロボットの走行経路は画像データから生成しているが、交差する経路を表現できない。
    - いずれ、専用の経路エディタを作成することにする。


  \section log_091204_misc 雑多な報告

  - 2009/12/05 に galgar と会って打ち合わせを行った。有意義だった。

  - Robedo の Three が届いた
    - まずは、モータを回転させようと思う

  - つくチャレ 2009 プログラムの管理
    - プログラムをまとめて管理すべく、専用フォルダを作成して移動させた
      - qrobosdk/trunk/tsukuba_2009

  - 名言集
    - 柔道を真面目に練習していた頃の話をしていて
      - 「真面目にやると、それなりに上達できると思うよ」\n
        「なんで、ロボットは真面目に打ち込まないの？」


  \section log_091204_next 次回の予定

  - SH7045F の開発環境の構築
  - Robedo three のモータを回転させる
  - 自作ロボット作成予定の書き出し
  - ２本のポールを使った環境での位置推定サンプル、が実機でちゃんと動作するようにする
*/
