ここでは、PCLプラグインの位置・姿勢推定機能について説明する。
PCLプラグインについてはPCL, PCL.rtcプラグインを参照のこと。

1.事前準備

CVFH特徴量を用いて位置・姿勢推定を行う場合は，事前に特徴量DBを作成する必要があります．
CVFH特徴量を用いた位置・姿勢推定方法:ObjectPoseRecognitionAlgorithm20150311.pdf
セグメンテーションアルゴリズム：SegmentationAlgorithm.pdf

1.1.PartialCloudGenratorのビルド

PartialCloudGeneratorをビルドしていない場合，以下の手順でビルドを行います．

 $ cd (choreonoidのディレクトリ)/extplugin/graspPlugin/PCL/PartialCloudGenerator
 $ ccmake .

 $ make

1.2.部分モデルの作成

対象モデルのある視点から見た部分モデルをポイントクラウド化したものを複数個作成します．
まず，モデルデータをply形式に変更します．VRML(wrl)からplyへの変換はmeshlabで行います．
meshlabで対象VRMLファイルを読み込み，"Export Mesh As"でply形式で保存します．
保存したplyファイルを(choreonoidのディレクトリ)/extplugin/graspPlugin/PCL/PartialCloudGeneratorにコピーします．
(以下，保存したplyファイルをmodel.plyとします．)

 $ cd (choreonoidのディレクトリ)/extplugin/graspPlugin/PCL/PartialCloudGenerator
 $ ./partial_cloud_generator -i model.ply -o (保存先ディレクトリ)

1.3.特徴量DBの作成

部分モデルに対して，CVFH特徴量等を算出します．
(1)Choreonoidを起動します．
(2)ツールバーのPCLバーにある「GenDes」ボタンを押します．
(3)ダイアログが表示されるので，「Partial model dir」の右側の「select...」ボタンを押し、先程作成した部分モデルがあるディレクリを選択します．
(4)作成時のパラメータを変更する場合は，「parameters」をクリックすると変更できるパラメータが表示されるので，これを変更してください．
　パラメータの意味は以下の通りです。
　　sample density:ダウンサンプリング時のvoxel_leafのサイズ
　　normal radius:特徴量算出に使用する法線を半径raidus_normalで計算を行う
(5)「OK」ボタンを押すと特徴量ファイル等がダイアログで指定したディレクトリ内に作成されます．
　boundingbox_X.txt：部分モデルのバウンディングボックス
　centroid_X_Y.txt：部分モデルの中心
　crh_X_Y.pcd:CRH特徴量
　crhext_X_Y.pcd:CRH特徴量(追加分)
　cvfh_X_Y.pcd:CVFH特徴量
　pcd_X.pcd：ダウンサンプリングした部分モデルポイントクラウド

1.4.YAMLファイルの作成

 # VRML(HRP)ファイルを指定します
 modelFile: modelHRP.wrl
 # CVFH特徴量ファイルがあるディレクトリを指定します
 descriptorDir: ./descriptor/model/ 

2.位置・姿勢推定

2.1使用方法

(1)Choreonoidを起動します．
(2)推定したいオブジェクトをSetObjectします．
　※CVFHを用いた位置・姿勢推定を行う場合は1.4の方法で作成したYAMLファイルを指定してください．
(3)ハンドに取り付けたカメラ等を使用する場合はSetCameraで使用カメラを選択します。
(4)PCLバーの「Est」ボタンを押します．
(5)パラメータを変更する場合は2.2の情報を参考に変更します．
(6)「OK」ボタンを押すと位置・姿勢推定が実行されます．
　成功したら対象オブジェクトが移動します．
　失敗した場合，対象オブジェクトは原点に移動します．
　失敗した場合は，探索範囲の変更，平面除去回数の変更等を試してください．

2.2パラメータ

「Initial alignment method」
　初期位置合わせに使用する方法を選択します．
「Capture」
　merge with previous captured pointcloudにチェックをいれると，前回キャプチャしたポイントクラウドとマージしたポイントクラウドで位置姿勢認識を行います。
「Regionタブ」
　探索範囲を指定します．
　"set target region"のチェックを外すとすべての範囲を探索します．
「PCD/Modelタブ」
　load scene from:チェックがついている場合，選択したファイルからポイントクラウドを読み込みます．
　save scene to:チェックがついている場合，選択したファイルにポイントクラウドを書き込みます．
　obj file path:チェックがついている場合，SetObjectしたオブジェクトではなく，選択したファイルについて推定を行います．
　　　　　　　チェックをつけた場合，推定されたオブジェクトがchoreonoidの左側アイテムタブに追加されます．
「Showタブ」
　推定後の表示を選択します．
　　scene：入力されたポイントクラウドをそのまま表示します．
　　segmented scene：セグメンテーション結果を表示します．
　　clipped scene and move object:推定後のオブジェクト部分を取り除いたポイントクラウドを表示します．
「Parameterタブ」
　「Sampling」
　sample density:ダウンサンプリング時のvoxel_leafのサイズ
　「Segmentation」
　dist threthold(plane remove):平面除去時の閾値
　number of planes(palne remove):平面除去回数
　number of candidate clusters: 平面除去後にできるクラスタに対し、いくつのクラスタを探索対象とするか
　「ICP」
　iteration：ICPの最大繰り返し回数
「SAC-IAタブ」
　線特徴量算出時の近傍点の基準を設定します。Radiusを選択した場合は距離で、K-nearestを選択した場合は点数で決定されます。
　normalが法線計算時、featureが特徴量計算時に使用する値です。
「CVFHタブ」
　Candidate pose：マッチング時の候補を何個選択するか
「calibrationタブ」
　キャリブレーション用です（後述）．

3.設定ファイル

 [ObjPoseEstimate]
 ;; 実行結果の表示方法。SCENE,SEGMENT,RESULTのいずれかを指定
 show_scene=SEGMENT 
 ;; ポイントクラウドの色表示
 show_color=true
 ;; trueのときpcd_loadfilepathのファイルからポイントクラウド読み込む
 ;; pcd_loadfilepathはconfig.iniからの相対パスまたは絶対パスで指定
 pcd_loadfile=true 
 pcd_loadfilepath=./test_pcd.pcd
 ;; trueのときpcd_savefilepathファイルへポイントクラウドを書き込む
 pcd_savefile=false
 pcd_savefilepath=./test_pcd.pcd
 ;; trueのときpcd_objfilepahtのオブジェクトを推定対象とする。falseのときはchoreonoid内setobjectしたものを対象とする
 pcd_loadobj=false
 pcd_objfilepath=../../hrgPlugin/Samples/CarParts3/8657033040.yaml
 ;; ダウンサンプリング時のleafサイズ[m]
 sampling_density=0.0025
 ;; 平面除去時のの閾値
 segmentation_distth=0.007
 ;; 平面除去回数
 segmentation_numplane=0
 ;; 推定対象クラスタ数
 segmentation_searchcluster=10
 
 ;;; セグメンテーション時のパラメータ
 ;; 使用するアルゴリズム true:LCCP、 false:EuclideanCluseteringベース
 use_lccp_segmentation=true
 ;; EuclideanCluseteringベースアルゴリズムのパラメータ
 ;; 　点間距離の視線ベクトルと水平方向成分がsegmentation_tolerance以内かつ
 ;; 　点間距離の視線ベクトルの垂直方向成分がsegmentation_vtolerance以内なら同一クラスタとする
 ;; 　リセグメント時の輪郭点からsegmentaion_resegment_radius以内の点を削除する。
 ;; 　リセグメントはクラスタの細い部分で分離するがこの値で分離する細さが変化する
 ;; 　segmentation_boundary_radiusは輪郭点を求める時のパラメータ。この値が大きすぎると凹んだところの輪郭点が無視される。
 ;; 　この値が小さすぎると欠落部分のまわりが輪郭点になってしまう。
 segmentation_tolerance=0.004
 segmentation_vtolerance=0.004
 segmentation_resegment_radius=0.01
 segmentation_boundary_radius=0.0075
 ;; LCCPアルゴリズムのパラメータ
 ;;   supervoxel(http://pointclouds.org/documentation/tutorials/supervoxel_clustering.php#supervoxel-clustering)
 ;;   作成時のパラメータR_seedに相当
 segmentation_lccp_seedsize=0.01
 ;;   supervoxel作成時のパラメータW_sに相当
 segmentation_lccp_spatialcoeff=0.4
 ;;   supervoxel作成時のパラメータW_nに相当
 segmentation_lccp_normalcoeff=1.0
 ;;   クラスタとなるための最小の点の数
 segmentation_lccp_minclustersize=20
 ;;   凹みと判断する角度(degree)。この値が小さくすると少しの凹みでも分離される。大きくすると分離されにくくなる。
 segmentation_lccp_concavitytolerance=10
 
 ;; SACIAのパラメータ
 SACIA_use_radius=true
 SACIA_radius_normal=0.001
 SACIA_radius_feature=0.002
 SACIA_use_knearest=false
 SACIA_knearest_normal=10
 SACIA_knearest_feature=10
 ;; ICP回数
 ICP_iteration=15
 ;;　region_enableがtrueのときポイントクラウドをクリッピングする 
 region_enable=true
 region_xmax=0.60
 region_xmin=0.30
 region_ymax=0.0
 region_ymin=-0.26
 region_zmax=2.0
 region_zmin=0.74
 ;; CVFHで特徴量が類似している上位CVFH_NN個のポーズに対し認識を行う．
 CVFH_NN=15

以下は古い情報です。

4.キャリブレーション方法

(1)PCL/calibtools/calibmat.txtのキャリブレーション行列を単位行列に変更します．
(2)choreonoidを起動します．
(３)PCLバーの「Est」ボタンを押します．ダイアログが表示されます．
(４)calibrationタブを選択します．
(5)apply trans matのチェックをはずします．
(６)selectボタンをクリックし，キャプチャしたポイントクラウドの保存先を決定します．
(７)"capture and save"ボタンを押すと，ポイントクラウドが表示されます．（同時にファイルに保存されます．）
(8)表示されたポイントクラウドから実際の座標との対応点を４つ見つけ，そのペアを記録します．
　ハンドカメラの場合はcalibrationタブで"show palm pos"ボタンを押すとpalmの位置・姿勢がメッセージウィンドウに表示されるので，これも記録します．
(9)固定カメラの場合はPCL/calibtools/caliration.mに(8)で記録した座標で上書きし，calibmat.txtを作成します．
　ハンドカメラの場合はPCL/calibtools/calirationhandcamera.mに(8)で記録した座標とpalmの位置姿勢を上書きし，calibmat.txtを作成します．
(10)choreonoidを一旦終了させ、再度起動します．
（１１）PCLバーの「Est」ボタンを押します．ダイアログのcalibrationタブを選択し，selectボタンをクリックし，先ほど保存したポイントクラウドファイルを選択します．
（１２）loadボタンを押すとワールド座標系に変換されたポイントクラウドが表示されますので，正しいか確認します．ハンドカメラの場合はhand cameraのチェックをいれます．
　　※ハンドカメラの場合はロボットをポイントクラウドキャプチャ時の姿勢にセットしておきます．
（１３）ずれている場合はポイントクラウドと実際の座標との対応点を見つけcaliration.mまたはcalirationhandcamera.mを更新し，calibmat.txtを作成します．

パラメータの設定

法線の可視化

初期位置合わせ時の法線を見る方法を説明します。
まずObjectPoseEstimator.cppに
　#define DEBUG_MODE
の記述を追加しビルドします。
choreonoidを起動しPCLプラグインの位置・姿勢推定機能を実行します。
extplugin/graspPlugin/PCL/の下にobj.pcdとscene.pcdが生成されます。
これは、オブジェクトと環境の法線付きポイントクラウドです。

次にこのpcdファイルを描画します。
[windowsの場合]
コマンドプロンプトを起動し、PCLがインストールされているフォルダに移動します。

 > bin/pcd_viewer_release.exe -normals 1 -normal_scale 0.005 "対象pcdファイルのパス”

 $ sudo apt-get install libpcl-1.6-bin

 $ pcd_viewer -normals 1 -normal_scale 0.005 "対象pcdファイルのパス” 

動作確認

choreonoidを起動します。
PCLバーのEstをクリックするとダイアログが表示されます（使用方法を参照）。
ダイアログの中段PCD/modelの「load scene from」の右側の「select...」ボタンを押し、extplugin/graspPlugin/PCD/PoseEstimationPCD/poseestimation_sample.pcdを選択します。
ダイアログの中段PCD/modelの「obj file path」の右側の「select...」ボタンを押し、extplugin/graspPlugin/Samples/Object2/110418_ELPA_lowhrp.wrlを選択します。
ダイアログの中段Parmetersの「number of planes(plane remove)」を2に設定し、OKボタンを押します。
下図のようなポイントクラウドが表示されます。

アイテムタブに「110418_ELPA_Merged」が追加されているのでこれにチェックをいれます。下図のように箱の真ん中にリモコンが表示されます（向き等は下図と違う場合があります。）

choreonoidを終了します。