search

jsk-ros-pkg / jsk_robot

jsk-ros-pkg/jsk_robot
https://github.com/jsk-ros-pkg/jsk_robot
73 stars 97 forks source link

[jsk_naoqi_robot] pepperの点群の出し方についてのご相談 #619

Closed kochigami closed 8 years ago

kochigami commented 8 years ago

画像認識がうまくいかないのは，点群も原因なのかなと思いここに相談を書かせていただきます． パッケージとは関係がなく申し訳ございません．

rviz のdepth_imageを見ると以下の様になっていますが． pepper_depth_image_from_rviz

以下のプログラム（depth_image_procを使って点群を出す）だと，画像が曲がってしまいます．（柱が折れ曲がっている） pepper_point_cloud_without_image_rect_color

<!-- -*- mode: xml -*- -->
<launch>
  <node pkg="tf" name="depth_to_top_camera_frame_publisher" type="static_transform_publisher" args="0.039 -0.044 0.035 0.000 0.000 0.000 CameraTop_optical_frame CameraDepth_optical_frame 10"/>

  <node pkg="nodelet" type="nodelet" name="register_depth" output="screen"
        args="standalone depth_image_proc/register" >
    <remap from="rgb/camera_info" to="/pepper_robot/camera/front/camera_info"/>
    <remap from="depth/camera_info" to="/pepper_robot/camera/depth/camera_info"/>
    <remap from="depth/image_rect" to="/pepper_robot/camera/depth/image_raw"/>
    <remap from="depth_registered/camera_info" to="/pepper_robot/camera/depth_registered/camera_info"/>
    <remap from="depth_registered/image_rect" to="/pepper_robot/camera/depth_registered/image_rect"/>
  </node>

  <node pkg="nodelet" type="nodelet" name="cloudify" output="screen"
        args="standalone depth_image_proc/point_cloud_xyzrgb" >
    <remap from="rgb/camera_info" to="/pepper_robot/camera/front/camera_info"/>
    <remap from="rgb/image_rect_color" to="/pepper_robot/camera/front/image_raw"/>
    <remap from="depth_registered/image_rect" to="/pepper_robot/camera/depth_registered/image_rect"/>
    <remap from="depth_registered/points" to="/pepper_robot/camera/depth/points"/>
  </node>
</launch>

image_proc を使ってimage_raw ではなく，image_rect_colorを使うようにしましたが，あまり変わりませんでした． pepper_point_cloud_with_image_rect_color

<!-- -*- mode: xml -*- -->
<launch>
  <node pkg="tf" name="depth_to_top_camera_frame_publisher" type="static_transform_publisher" args="0.039 -0.044 0.035 0.000 0.000 0.000 CameraTop_optical_frame CameraDepth_optical_frame 10"/>

  <node pkg="image_proc" type="image_proc" name="rgb_processing" output="screen" ns="/pepper_robot/camera/front">
  </node>

  <node pkg="nodelet" type="nodelet" name="register_depth" output="screen"
        args="standalone depth_image_proc/register" >
    <remap from="rgb/camera_info" to="/pepper_robot/camera/front/camera_info"/>
    <remap from="depth/camera_info" to="/pepper_robot/camera/depth/camera_info"/>
    <!-- <remap from="depth/image_rect" to="/pepper_robot/camera/depth/image_raw"/> -->
    <remap from="depth/image_rect" to="/pepper_robot/camera/depth/image_raw"/>
    <remap from="depth_registered/camera_info" to="/pepper_robot/camera/depth_registered/camera_info"/>
    <remap from="depth_registered/image_rect" to="/pepper_robot/camera/depth_registered/image_rect"/>
  </node>

  <node pkg="nodelet" type="nodelet" name="cloudify" output="screen"
        args="standalone depth_image_proc/point_cloud_xyzrgb" >
    <remap from="rgb/camera_info" to="/pepper_robot/camera/front/camera_info"/>
    <remap from="rgb/image_rect_color" to="/pepper_robot/camera/front/image_rect_color"/>
    <remap from="depth_registered/image_rect" to="/pepper_robot/camera/depth_registered/image_rect"/>
    <remap from="depth_registered/points" to="/pepper_robot/camera/depth/points"/>
  </node>
</launch>
k-okada commented 8 years ago

１． ペッパーのカメラつかっている？それとも追加でつけたデプスセンサ？ ペッパーのカメラのデプスはこのまえみたみたいにガタガタ．（そういえば，あの画像をキャプチャしてくれると嬉しい） 治そうとしているんだけど，たぶん，naoqiでデータを取ってる時か，データを送るときにjpeg圧縮しているんじゃないかと思うんだけど，心当たりないかな？

２． ずれているのは点群？それとも，点群の上のカメラの色情報？ まずは色無しで点群だけで 平面が平面に見えているか？ 線（柱）が線に見えているか？を確認する 色情報はここから点群の各点に色を追加している．これをregistrationという．これは色カメラのパラメータと距離カメラのパラメータから， どの点には，どの色カメラ画像の座標の色を使えばいいか，という計算して，点に色をつける．正確に言うとrectをつかわないとけない． ただ，rectとrawで http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration の http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration?action=AttachFile&do=get&target=mono_1.png と http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration?action=AttachFile&do=get&target=mono_2.png ぐらい違っていないと，どっちを使っても結果はそうかわらない．

◉ Kei Okada

2016-05-31 21:34 GMT+09:00 Kanae Kochigami notifications@github.com:

画像認識がうまくいかないのは，点群も原因なのかなと思いここに相談を書かせていただきます． パッケージとは関係がなく申し訳ございません．

rviz のdepth_imageを見ると以下の様になっていますが． [image: pepper_depth_image_from_rviz] https://cloud.githubusercontent.com/assets/7259671/15674129/07571a0c-2776-11e6-94dd-48223205173e.png

以下のプログラム（depth_image_procを使って点群を出す）だと，画像が曲がってしまいます．（柱が折れ曲がっている） [image: pepper_point_cloud_without_image_rect_color] https://cloud.githubusercontent.com/assets/7259671/15674229/a25cbe08-2776-11e6-8a63-81c8ae43df49.png

image_proc を使ってimage_raw ではなく，image_rect_colorを使うようにしましたが，あまり変わりませんでした． [image: pepper_point_cloud_with_image_rect_color] https://cloud.githubusercontent.com/assets/7259671/15674315/f60e8ed2-2776-11e6-82c4-bf180bb25296.png

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/jsk-ros-pkg/jsk_robot/issues/619, or mute the thread https://github.com/notifications/unsubscribe/AAeG3Atp0QEy8h5ULcy-qbCQtQ6ikN0kks5qHCrQgaJpZM4Iqe4I .

kochigami commented 8 years ago

ご教示ありがとうございます．

１． ペッパーのカメラつかっている？それとも追加でつけたデプスセンサ？

ペッパーのカメラです． rvizでdepth imageから点群を出した結果は以下になります．縦に伸びているのは壁です． pepper_point_cloud_from_depth_image

２． ずれているのは点群？それとも，点群の上のカメラの色情報？

ずれているのは点群の上の色情報ですので，デプスカメラとフロントカメラ間のtfを直します． pepper_camera_from_rviz

kochigami commented 8 years ago

<=> I made pcd file of wall, but showing it on Cloud viewer seemed to take a long time...

[to be continued]

kochigami commented 8 years ago

I created pcd file by getImageRemote NAOqi API function.

-- front view

pepper developer version pcd_wall_pepper_front

pepper for sale version pepper_for_sale_front

-- wall view from sky

pepper developer version pcd_wall_pepper

pepper for sale version pepper_for_sale_wall

-- table view from side

pepper developer version pcd_pepper_table

pepper for sale version pepper_for_sale_table

kochigami commented 8 years ago

現状の結論：

NAOqi API getImageRemote => Naoqi MSG => ROS MSG => publish ROS topic (naoqi_driver/src/converters) (naoqi_driver/src/converters) (naoqi_driver/src/publishers) 圧縮して，代入しているものはない（topicとして出してはいるけれど）

この時点でガタガタしている．

まだ理解できていないが理解したいこと：

参考画像：　xtion (height: 480 width: 640) xtion xtion_side

kochigami commented 8 years ago

とりあえずこのissueは閉じて，本当に困ったらフォーラムで質問します． ご教示ありがとうございました．

kochigami commented 8 years ago

カメラの視野角が狭い分，遠くへ行けば行くほど歪むのかもしれません． 近くを覗き見たら点群はまっすぐになりました．

The distance from Pepper to the table is 40 cm.
send *pepper* :head :neck-p :joint-angle 30

pepper_pointcloud_with_small_area

k-okada commented 8 years ago

これは同じロボット？だいぶ違うね． カメラの位置姿勢はいかが参考になるかな． http://doc.aldebaran.com/2-4/family/pepper_technical/video_3D_pep.html#d-camera-pepper http://doc.aldebaran.com/2-4/family/pepper_technical/video_pep.html

kochigami commented 8 years ago

ご教示ありがとうございました． カメラのことをあまり分かっていないので勉強します．

（以下，カラー画像と点群の重ね合わせのズレについて）

<node pkg="tf" name="depth_to_top_camera_frame_publisher" type="static_transform_publisher" args="0.039 0.074 0.030 0.000 0.000 0.000 CameraTop_optical_frame CameraDepth_optical_frame 10"/>

変更前 tf_modified_before

変更後 modified_tf

k-okada commented 8 years ago

2016-06-06 13:50 GMT+09:00 Kanae Kochigami notifications@github.com:

  • CameraTop_optical_frame CameraDepth_optical_frame間のtfを-0.044から目分量で0.074に直した

すばらしい．pepper_robot ? noaqi_driver に元コードがあるかな？そこにPR出しましょう．

◉ Kei Okada

k-okada commented 8 years ago

＞ros-naoqiのどこかでissueか何か書くところがあれば，後で書こうと思います． （そもそも上のxtion単体のカラー画像がずれているようなので，pepperのtfの問題ではないかもしれません．）

https://github.com/ros-naoqi/pepper_robot/blob/master/pepper_description/urdf/pepper1.0_generated_urdf/pepper_sensors.xacro#L120 を変えればいいのかな．？

kochigami commented 8 years ago

https://community.ald.softbankrobotics.com/en/forum/peppers-depth-image-compressed-publishing-10977

基本は320×240, 20fpsです． resolution:=0 (160×120), 1 (320×240), 2 (640×480) fps:=1, 5, 10, 20,30 でgetImageRemoteからpcdファイルの作成を試してみましたが， サイズが小さくなっただけで，歪みは変わらない気がしました． 点の数： resolution:=0, 1 => 各々160×120, 320×240 resolution:=2の時だけ点の数が4800になってしまった（また別の問題かと思います）

320×240　10fps　壁を横から resolution_320-240_framerate_10_side

160×120　10fps　壁を横から resolution_160-120_framerate_10_side

kochigami commented 8 years ago

https://github.com/ros-naoqi/pepper_robot/blob/master/pepper_description/urdf/pepper1.0_generated_urdf/pepper_sensors.xacro#L120 を変えればいいのかな．？

ご教示ありがとうございます． （=> https://github.com/ros-naoqi/naoqi_driver/blob/master/src/converters/joint_state.cpp でurdfを読み込み，tfを出していた．） ここで定義されているリンク同士のx, y, zの引き算と tf_echoの結果が合わないので，理由が分かったらプルリクエストを出そうと思います．

（以下自分用のメモ） Head -1- CameraTop_frame -2- CameraTop_optical_frame Head -3- CameraDepth_frame -4- CameraDepth_optical_frame

  1. xyz= 0.0868 0 0.1631
  2. xyz= 0.05138 0.039 0.1194　 （1と3の引き算でどうやって0.044, -0.035が出てくるかまだ分からない） => 小数第三位は四捨五入により切り捨てているようだ ↓回転している向きは同じ
  3. -1.57... 0 -1.57...
  4. -1.57... 0 -1.57...
rosrun tf tf_echo CameraTop_optical_frame CameraDepth_optical_frame 10
At time 1465299058.567
- Translation: [-0.039, 0.044, -0.035]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]
kochigami commented 8 years ago

コミュニティに質問しました

try kRawDepthColorSpace instead of kDepthColorSpace => xtion単体のように平らにならなかったが，ボコボコは少なくなった 変更： https://github.com/kochigami/naoqi_driver/blob/try-depth-raw/src/converters/camera.cpp#L129 colorspace_((camera_source_!=AL::kDepthCamera)?AL::kRGBColorSpace:AL::kRawDepthColorSpace ), https://github.com/kochigami/naoqi_driver/blob/try-depth-raw/src/tools/alvisiondefinitions.h#L81 const int kRawDepthColorSpace = 23;

結果： kDepthColorSpace (depth cloud in rviz) kdepthcolorspace_above

kRawDepthColorSpace (depth cloud in rviz) krawdepthcolorspace_above

kRawDepthColorSpace pcd file from getImageRemote 点の数は76800 (320*240) 320-240-framerate-15-rawdepth

カメラで今後やるべきことまとめ：

kochigami commented 8 years ago

kRawDepthColorSpaceで効果ありかもう一度確認する

効果はある． Rawの時点でボコボコしているので，きっとこの時点で圧縮されている． しかしkDepthColorSpaceを使うと，上半分くらいで点群の層が前に飛び出していて，もっとボコボコになる．（おそらくさらに圧縮している）

kDepthColorSpace 壁の点群の層が二段に分かれている kdepthcolorspace_splits_pointcloud kdepthcolorspace_splits_pointcloud_above

kRawDepthColorSpace 層レベルのボコボコは見られない krawdepthcolorspace_doesnot_spilit krawdepthcolorspace_doesnot_split_above

k-okada commented 8 years ago

:+1:

kochigami commented 8 years ago

時間が経ってしまったのですが、点群が歪んでしまう原因として

１．圧縮説 ２．私の計算違う説 ３．ペッパーの目の丸みで歪む説 があると思うのですが、

ir_front_depth_image_raw

roslaunch jsk_pepper_startup jsk_pepper_startup.launch
rosrun rviz rviz -d path/to/pepper_bringup/config/pepper.rviz

でデプスセンサと赤外線カメラ画像（同じデバイスから出てくるそうです） https://github.com/ros-naoqi/naoqi_driver/blob/master/src/converters/camera.cpp#L149 を比べたら、赤外線カメラ画像は歪んでいないので、３はなさそうです。（理解が間違っていたらすみません）

・　getRemoteImage というNAOqiの関数から得た点群が歪んでいる、 ・　kRawDepthColorSpace を使ってもやや歪んでいる ので、1が濃厚という結論で終わっていますが、 2ももう一度確認します。

k-okada commented 8 years ago

3はあるかもね．目のパーツ外せれば，一回外したい．

◉ Kei Okada

2016-09-07 19:53 GMT+09:00 Kanae Kochigami notifications@github.com:

時間が経ってしまったのですが、点群が歪んでしまう原因として

１．圧縮説 ２．私の計算違う説 ３．ペッパーの目の丸みで歪む説 があると思うのですが、

[image: ir_front_depth_image_raw] https://cloud.githubusercontent.com/assets/7259671/18307769/c78afbaa-752c-11e6-8b56-50c21a70f54f.png

roslaunch jsk_pepper_startup jsk_pepper_startup.launch rosrun rviz rviz -d path/to/pepper_bringup/config/pepper.rviz

でデプスセンサと赤外線カメラ画像（同じデバイスから出てくるそうです） https://github.com/ros-naoqi/naoqi_driver/blob/master/src/ converters/camera.cpp#L149 を比べたら、赤外線カメラ画像は歪んでいないので、３はなさそうです。（理解が間違っていたらすみません）

・ getRemoteImage というNAOqiの関数から得た点群が歪んでいる、 ・ kRawDepthColorSpace を使ってもやや歪んでいる ので、1が濃厚という結論で終わっていますが、 2ももう一度確認します。

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/jsk-ros-pkg/jsk_robot/issues/619#issuecomment-245245330, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3MVWXaMr2ngziTpXxSUJc0225C3Fks5qnpe0gaJpZM4Iqe4I .

furushchev commented 8 years ago

pepperにレーシックをしてみました。

img_5996 img_5997

手術前ではdepth画像の右上、左上に若干白くなってしまう部分があり、rvizでみるとその部分に何もないにも関わらずpepperの目から1.5mくらいのところに障害物があることになっていたのが、術後では治っているように思います。

before2 image

ただやはりまだ平面を見た時の点群はゆがんでいるようです。

pepper_pc_fail

そこで、https://github.com/kochigami/naoqi_driver/commit/d8ce96404251adf236ebe14084037a7a1a053112 のようにしてRAWのdepth画像を使ってみると、以下のようにxtion単体と比較しても遜色ない点群が出るようになったと思います。

pepper_raw

pepper_slam pepper_slam4

kochigami commented 8 years ago

手術ありがとうございます。 私も画像を確認してみました。

レーシック手術あり・なしの比較　（kRawDepthImageColorSpace使用） 点群の歪みがなくなっています。

レンズ摘出なし pepper_with_lens_krawdepthimagecolorspace_floor

レンズ摘出あり pepper_without_lens_krawdepthimagecolorspace_floor

kRawDepthImageColorSpaceとkDepthImageColorSpaceの比較　（レーシック手術なし）

手術前ではdepth画像の右上、左上に若干白くなってしまう部分があり、rvizでみるとその部分に何もないにも関わらずpepperの目から1.5mくらいのところに障害物があることになっていた

に関係しているように思います。

kDepthImageColorSpace pepper_with_lenses_kdepthimagecolorspace

kRawDepthImageColorSpace pepper_with_lens_krawdepthimagecolorspace_floor

@furushchev さんのコメントと合わせると、 レンズを取ると、歪みが取れる＋見えてはいけないものが見えなくなる kRawDepthImageColorSpaceを使っても、歪みが取れる＋見えてはいけないものが見えなくなる ２つを組み合わせるととても良いということが分かりました。

kochigami commented 8 years ago

@furushchev

すみません、 上のコメントでrtabmap_rosを使われていたとき、debファイルを使われていましたか。 それともソースから実行されましたか。

私はrosのindigoで、debの 0.11.8-0trusty-20161004-020811-0700 を使っていて、 RGB-D SLAM モードで試しているのですが、 地図が上のようにきれいにできないのと、自己位置推定にすぐ失敗するので困っています。

立ち上げるlaunchファイルはこのような感じでしたか。 もし覚えていらっしゃればご教示ください。ご迷惑をお掛けします。

(odom_frame_idを設定しないといけないのでしょうか。 そうであれば、ソースが必要で、今はビルドが通りません。)

<launch>
  <arg name="rviz" default="false" />
  <arg name="rtabmapviz" default="true" />
  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
    <arg name="rtabmap_args" value="--delete_db_on_start" />
    <arg name="frame_id" value="base_link"/>
    <arg name="depth_topic" value="/pepper_robot/camera/depth_registered/sw_registered/image_rect_raw" />
    <arg name="rgb_topic" value="/pepper_robot/camera/front/image_raw" />
    <arg name="camera_info_topic" value="/pepper_robot/camera/front/camera_info" />
    <arg name="rviz" value="$(arg rviz)" />
    <arg name="rtabmapviz" value="$(arg rtabmapviz)" />
  </include>
</launch>
furushchev commented 8 years ago

@kochigami aptからインストールしたと思います。 launchファイルもそれでいい気がします。 rtabmapvizに今までの履歴をリセットするボタンがあるので、それを最初に押してから始めると綺麗に取れると思います。 （あとちょっとでも激しく動くとダメでした。）

kochigami commented 8 years ago

@furushchev ご教示ありがとうございます。動かし方を変えてみます。