Pepperめも～SLAMに挑戦～ (前編)

いきなりですが…

• この記事は前編という事で完結してません。
• 記事を書いたのは去年の12月前半くらいから年明けにかけて…
• その後別の作業を始めたので一旦保留してる内容です。
• これの直前にあたるROSの記事が500Viewとか11ストックされていたので、みんな気になるROSなら、この状態でも誰かの参考程度にはなるのかなーと下書きを公開。
  • べ、べつに諦めたわけじゃないんだからね！

• ちなみに、最後の記事以降の言葉に出来てないあたりの永い試行錯誤の中で、結構いい感じのところまでは辿り着いたのですが、どうも、Pepper君のDepthセンサの画像は歪んでいる気がする…というところでドハマリ中です。誰か知ってたら教えて…あとでアルデバランの人が居るときに聞いてみたい…

• 2016.9.13 超久しぶりに更新！

• 東大の方々(だと思う?けど何も知らずにgithubウォッチしてる)が、あの謎の歪みを解決したらしいです！ やはり奴のカバーが問題だったのか…写真見る限りガムテでレーシック手術できるのかなぁ

• https://github.com/jsk-ros-pkg/jsk_robot/issues/619#issuecomment-246576442

• 忙しすぎて試す暇無いので試せないけど試したいなぁ…(遠い目…)

SLAMって？

• Simultaneous localization and mapping
• 自分の位置をトラッキングしてしかもマッピングしちゃうぞ！っていうやつ。
• ROSを利用する。ROSは前の投稿でPepper君でSLAM試すところまでは実現済み。

何やるつもり？

• 単眼動画ベースのSLAMでLSD-SLAMというのがあって多分使いこなすと凄そう。
  • 前回、うまく使いこなせるまで行かなかった
  • さらにある情報を知ってひとまず保留することに…
• 前回の投稿後、Pepper君のタグ関連の投稿を漁ってたら…
  • PepperくんでPoint Cloud
  • こんなナイスな投稿が！
  • PointCloudLibrary 通称PCL。世の中にはこんな良い環境が＆Pepper君のデプスセンサを取ってくる方法が紹介されてる！
  • ちょっといつの日にか…と諦め気味だったKinect系のSLAMが俄然身近になった！

というわけで

• Pepper君のKinectのお仲間のデプスセンサを使ってPointCloudなSLAMしちゃおうという挑戦することにした！

まずは情報収集

• http://answers.ros.org/question/63129/slam-with-pcl-data/
• ふむふむ
• http://wiki.ros.org/ethzasl_icp_configuration
• なるほど。PCLじゃないみたいだけど、なんか独立してるよとある。色々手軽そうかも！
• http://ros.org/wiki/ethzasl_icp_mapping
• でも別の情報も調べよう。
• http://wiki.ros.org/pcl
• PCLそのもののページ
• http://felixendres.github.io/rgbdslam_v2/
• 有名なSLAMのV2らしい。PCLとかのグループの創設にもかかわってそうな人達らしい

まずはRGBD-SLAM-V2を入れてみる

• さすがROSらしく選択肢は結構ありそうなのでまずは、有名どころと思われるものの後継のプロジェクトのモノを入れてみる

• ただし…Hydro用と明記してあるのでIndigoに素直に入るかは試してみるしかなさそう…

• パッケージのビルドにはcatkin

  • ROSの新しいビルドシステムcatkinについて
  • こんな良い記事があるのでcatkinって？という人は読んでおくと幸せになれる

• 前回、catkinのワークスペースの設定は終わらせてるので…

cd ~/ros_catkin_ws/indigo/src/
wstool set --git rgbdslam_v2 https://github.com/felixendres/rgbdslam_v2.git
wstool update
cd ..
catkin_make

• これでmakeまではさくっと完了！
  • そしてエラーの嵐！
• catkin参考リンク ROSチュートリアル第五回 - catkinのパッケージをコンパイルしてみる

RGBD-SLAM-V2のエラーを何とかしよう

コンパイルエラー１個目(indigoとの互換性関連)

/home/haiatto/ros_catkin_ws/indigo/src/rgbdslam_v2/src/openni_listener.cpp:272:91: error: ‘const struct tf::tfMessage_<std::allocator<void> >’ has no member named ‘__connection_header’
           boost::shared_ptr<std::map<std::string, std::string> > msg_header_map = tf_msg->__connection_header;

• indigoでは __connection_header が削除されてた。
  • tfというのは座標変換などを扱う基礎中の基礎のライブラリらしい
  • その中の、tfMessageどうも最新のtfにtfMessageが見当たらないのでエラーを起こした周りをコメントアウトで対処

リンクエラー１個目(libglew関連)

/usr/bin/ld: -lGLEW が見つかりません

• libglewみたいなものを、入れればいいのかなと適当に sudo apt-get install libglew-dev
  • うん。これは通ったっぽい

コンパイルエラー２個目(OpenCV関連)

/home/haiatto/ros_catkin_ws/indigo/src/rgbdslam_v2/src/misc.cpp:40:39: fatal error: opencv2/nonfree/nonfree.hpp: そのようなファイルやディレクトリはありません
 #include "opencv2/nonfree/nonfree.hpp"

• これは一目瞭然。opencvのnonfree.hppが無いと。
• たしかlsd-slamのREADMEに何か書いてあった気が。あ、あった！

Note for Ubuntu 14.04: The packaged OpenCV for Ubuntu 14.04 does not include the nonfree module, which is required for openFabMap (which requires SURF features). You need to get a full version of OpenCV with nonfree module, which is easiest by compiling your own version. We suggest to use the 2.4.8 version, to assure compatibility with the current indigo open-cv package

• 2.8とコンパチだから、それのフルバージョン入れれば良いよ、カンタンダヨって書いてある。リンクもあったのでそこから落としてきて…

  • でも素直にcmakeしただけじゃ全然ダメだった

• 試行錯誤の結果うまくいった方法はこれ！

sudo apt-get install build-essential cmake 
sudo apt-get install libjpeg-dev libtiff4-dev libjasper-dev 
sudo apt-get install libgtk2.0-dev 
sudo apt-get install libavcodec-dev libavformat-dev libswscale-dev libv4l-dev

cd ~ 
wget https://github.com/Itseez/opencv/archive/2.4.8.zip
unzip 2.4.8.zip 
cd opencv-2.4.8 
mkdir build
cd build
cmake -D CMAKE_BUILD_TYPE=RELEASE -D WITH_TBB=ON -D BUILD_NEW_PYTHON_SUPPORT=ON -D WITH_V4L=ON -D INSTALL_C_EXAMPLES=ON -D INSTALL_PYTHON_EXAMPLES=ON -D BUILD_EXAMPLES=ON -D WITH_QT=ON -D WITH_OPENGL=ON ..

cmake -D WITH_TBB=ON -D BUILD_NEW_PYTHON_SUPPORT=ON -D WITH_V4L=ON -D INSTALL_C_EXAMPLES=ON -D INSTALL_PYTHON_EXAMPLES=ON -D BUILD_EXAMPLES=ON -D WITH_QT=ON -D WITH_OPENGL=ON -D MAKE_INSTALL_PREFIX=/usr/local ..
make
sudo make install

echo "/usr/local/lib" | sudo tee /etc/ld.so.conf.d/opencv.conf
sudo ldconfig

export PKG_CONFIG_PATH=/usr/local/lib/pkgconfig:$PKG_CONFIG_PATH
export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH
export PKG_CONFIG_PATH=/usr/local/lib/pkgconfig:$PKG_CONFIG_PATH

※OpenCVコンパイルするときの試行錯誤中のリアルタイム記録

• 古いubuntuに2.4.8入れる記事が見つかったので、その通りやってみるも、色々依存関係がオカシイとか言われるので一旦情報収集
• ubuntuに2.4.9入れる記事を見つけたので、見比べて必要最低限っぽいところをチョイス

sudo apt-get install build-essential cmake 
sudo apt-get install libjpeg-dev libtiff4-dev libjasper-dev 
sudo apt-get install libgtk2.0-dev 
sudo apt-get install libavcodec-dev libavformat-dev libswscale-dev libv4l-dev

• 以下の不要そうなものはひとまず無視しておくことに。一気に入れすぎは不安定な時の原因切り分けとかの害悪の元！

libopenexr-dev 
python-dev python-numpy python-tk 
libtbb-dev # OpenCV on Matlab 
libeigen3-dev # 線形代数とかのC++ライブラリ
yasm # アセンブラらしい
libfaac-dev libopencore-amrnb-dev libopencore-amrwb-dev 
libtheora-dev libvorbis-dev # 音声系のコーデック？
libxvidcore-dev libx264-dev # xvidとx264
libqt4-dev libqt4-opengl-dev # qt4？入ってない？
sphinx-common # ドキュメント生成ツールらしい
texlive-latex-extra # tex
libdc1394-22-dev #IEEE1394 isight向けなのかな。まあいいや
default-jdk # javaいらない
ant # antもjava要らないなら不要だろうきっと。
libvtk5-qt4-dev # まあいいや

• cmake設定も見比べて、とりあえず要りそうなものをチョイスしてみた

cmake 
-D CMAKE_BUILD_TYPE=RELEASE 
-D WITH_TBB=ON 
-D BUILD_NEW_PYTHON_SUPPORT=ON 
-D WITH_V4L=ON 
-D INSTALL_C_EXAMPLES=ON 
-D INSTALL_PYTHON_EXAMPLES=ON 
-D BUILD_EXAMPLES=ON 
-D WITH_QT=ON 
-D WITH_OPENGL=ON 
..

よしこれでOK！
以上、リアルタイム記録でした

ここで衝撃の事実が発覚

• indigoで通そうとしてる人達 いるじゃん！
• あいやぁ… ここの質問みて、この質問者、突き放されてるから情報無いのかなぁと早とちりしたのが間違いだった…
• opencv頑張らなくてもよかったのかも。まあいいや。最初に読んでても多分理解が追い付かなかったし…けっかおーらい

Makeエラー1個目

make[2]: *** `/home/haiatto/ros_catkin_ws/indigo/devel/lib/rgbdslam/rgbdslam' に必要なターゲット `/usr/lib/libcxsparse.so.2.2.3' を make するルールがありません.  中止.

CMakeLists.txt

-SET(LIBS_LINK GL GLU cholmod /usr/lib/libcxsparse.so.2.2.3 ${G2O_LIBRARIES} ${QT_LIBRARIES} ${QT_QTOPE
+SET(LIBS_LINK GL GLU cholmod cxsparse ${G2O_LIBRARIES} ${QT_LIBRARIES} ${QT_QTOPENGL_LIBRARY} ${GLUT_L

• お次は、indigoで通そうとしてる人達の情報に従ってやってみたら、これにて通った。これは単にお行儀の問題？
• ついでに、↑の情報に従って、CMakeLists.txt を書き換えてみる。
• ただ、ここだけはコンパイルエラーが起こるので(autoキーワード…型推論とか使ってるソースがあるので)どうも微妙だったのでやめておいた。
  • あとから追記。これは重要な意味があった…それを知るのはもっと先だけど。

-SET(CMAKE_CXX_FLAGS "-ggdb -O3 -fPIC -std=c++0x")
+#SET(CMAKE_CXX_FLAGS "-ggdb -O3 -fPIC -std=c++0x")
+SET(CMAKE_CXX_FLAGS "-ggdb -O3 -fPIC")

リンクエラー2個目

CMakeFiles/rgbdslam.dir/src/feature_adjuster.o: 関数 `DetectorAdjuster::detectImpl(cv::Mat const&, std::vector<cv::KeyPoint, std::allocator<cv::KeyPoint> >&, cv::Mat const&) const' 内:
/home/haiatto/ros_catkin_ws/indigo/src/rgbdslam_v2/src/feature_adjuster.cpp:76: `cv::SURF::SURF(double, int, int, bool, bool)' に対する定義されていない参照です
～中略～
collect2: error: ld returned 1 exit status
make[2]: *** [/home/haiatto/ros_catkin_ws/indigo/devel/lib/rgbdslam/rgbdslam] エラー 1
make[1]: *** [rgbdslam_v2/CMakeFiles/rgbdslam.dir/all] エラー 2
make: *** [all] エラー 2

• これはほんとにさっぱり。何がさっぱりって色々やってるうちに治ってしまったという気持ち悪さ。
• find_package(OpenCV)で共有の方を探してしまうのが原因ぽかったけど、それじゃ、共有の方を試しにリネームしたら…とかやってたら通ったものの、戻してbuildフォルダ全消しとかしても結局エラーが起きなくなってしまった。
• まあいいや。きっとターミナルの再起動とかが足りなかったのだろうそうに違いない。

Make通った！けど実行時エラー

~/ros_catkin_ws/indigo$ rosrun rgbdslam rgbdslam 
Segmentation fault (コアダンプ)

もう…気難しいんだから（涙
いいよ、デバッグしちゃうよ。もう。ほんと覚えることが多くて素敵！（楽しいけど）
一応、indigoで通そうとしてる人達こちらに同様のエラーが載ってたけど、解決して無いっぽい？

ROSでデバッグしてみる

• http://d.hatena.ne.jp/meison_amsl/20140107/1389099065
• なるほど。gdbでやればいいのか。初gdb!

(gdb) bt
#0  0x00007f5194264a8c in boost::math::lanczos::lanczos_initializer<boost::math::lanczos::lanczos17m64, long double>::init::init() ()
   from /usr/lib/libpcl_io.so.1.7
#1  0x00007f51941ef186 in ?? () from /usr/lib/libpcl_io.so.1.7
#2  0x00007f5194a4f13a in call_init (l=<optimized out>, argc=argc@entry=1, 
    argv=argv@entry=0x7fff74bb5998, env=env@entry=0x7fff74bb59a8)
    at dl-init.c:78
#3  0x00007f5194a4f223 in call_init (env=<optimized out>, 
    argv=<optimized out>, argc=<optimized out>, l=<optimized out>)
    at dl-init.c:36

• うわぁ…スタートアップルーチンの中で落ちてるじゃん…
• call_init って調べるとグローバルコンストラクタの呼び出し…
• っていうかこれ、もれなくlibpcl_io.so.1.7使ったら落ちないとおかしいんじゃないだろか。…それともメモリ破壊か未初期化系？問題の個所でググると何か出てきそうだなぁこれ…
• うん。出てきた。
  • フィーリングで英文を読むに、ROS関係ないんじゃないかね？関係するの全部リムって見たけど起きるぜよ！って書いてある気がする。つまりPCLのバグ…
  • そして原因は"-std=c++0x"だから外してね！PCLまだ対応して無いの。今ベリーハードワークなう！って書いてある。…あれ？
  • そういえば、…コンパイルエラー起きるからってindigoで通そうとしてる人達の情報に従わず、除外したところじゃん！
  • こんなところに影響があったなんて…驚愕！

再度CMakeLists.txt調整とソースを修正

CMakeLists.txt

-SET(CMAKE_CXX_FLAGS "-ggdb -O3 -fPIC -std=c++0x")
+#SET(CMAKE_CXX_FLAGS "-ggdb -O3 -fPIC -std=c++0x")
+SET(CMAKE_CXX_FLAGS "-ggdb -O3 -fPIC")

feature_adjuster.cpp

bool VideoGridAdaptedFeatureDetector::empty() const
{
//@@c++0x@@    for(auto detector : detectors){
//@@c++0x@@      if(detector->empty()) return true;
//@@c++0x@@    }
    for(std::vector<cv::Ptr<StatefulFeatureDetector> >::const_iterator detector=detectors.begin() ; detector!=detectors.end();++detector){
      if(detector->empty()) return true;
    }
    return false;
}

よし、落ちなくなった！ここまで1日かかったけど出来たよー

やっとSLAM

• さて…RGBD-SLAM v2動いたので、次はPepper君のデプス情報を取り込むところを作ろう！

調査フェーズ開始！

• RGBDSLAMv2を動かすために必要な情報はなにか？

  • 画像を受け取る最初の段階を扱ってる、openni_listener.cppを読むと、openniの名前の通り前提はopenniを使うことみたいだけど、別になくても汎用のトピックを使っているようなのでopenni自体は不要。
  • デプスセンサ値は必須で、あとはカラーカメラの画像と情報は必須らしい。+αとしてPCLのデータも付加して渡すことが出来るらしい 不要。
  • 実際にポイントに変換していると思われる、createXYZRGBPointCloud()を読むと、変換にはカメラ情報は使って無いっぽい。

• Pepper君が扱える情報は何か？

  • Pepper君の従兄のROMEO君用ノード を見ると、Depth画像は取れるけど、IR画像は現在取れないらしい。(ROMEO君は真黒ダミー画像作って公開してた)
  • nao用カメラのlaunchファイルと引数解釈してるところをじっくり読むと、なんとDepth値画像がちゃんと用意されているので、こちらは特別なノードは不要でDepth値を扱える！
  • Pepper君はRGBカメラを2個持っている。

推理フェーズ開始！

• Pepper君のXtionPro3DセンサはRGBカメラ付いてない版なので、まずはデプスだけを取り込むようなノードを書けば行けそうな予感

• Pepper君にはRGBカメラが付いているのでカラーを得る事は可能。

  • デプスセンサとは位置がズレているので、ズレを直さないとならない。
  • ズレを直すのはデプス値とデプスカメラとRGBカメラの相対位置があれば、自ずとRGBカメラへ実際の空間上のポイントからレイを飛ばす感覚で求まる。(三角測量の逆)
  • まあでも、ひとまず無くても動きはするはず(結果の出来上がるモデルは淡泊になるけど…)

• ということは、真黒ダミーカメラを作って公開するノードだけ作ればあとはありもの組み合わせていける！

  • 後で画像をズラシをするのに差し変えてもよし。

作成フェーズ開始！

ノード作ってみる

cd ~/ros_catkin_ws/indigo/src/
catkin_create_pkg dummy_camera std_msgs rospy roscpp

• 公式チュートリアルを参考にて空の dummy_camera パッケージを作成！
  • ちなみに引数の2番目以降は依存パッケージ。C言語でいうところのstdlib.h みたいなものみたい。

cd ~/ros_catkin_ws/indigo/src/dummy_camera
mkdir scripts

• scriptsフォルダを作成して、そこに

cd scripts
echo "" >> dummy_camera_node.py

• dummy_camera_node.py を作成して、
  • ちなみに最初は dummy_camera.py という名前で作っていたら、生成されたpythonのパッケージをインポートする際に邪魔するという罠が待っていたので名前を変更…

dummy_camera_node.py

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Image, CameraInfo
import camera_info_manager
from sensor_msgs.msg._CameraInfo import CameraInfo

from cv_bridge import CvBridge, CvBridgeError
import cv2
import numpy as np

def dummy_camera():
    rospy.init_node('dummy_camera')
    bridge = CvBridge()
    pub_cimg = rospy.Publisher('~image_color', Image)
    pub_cinf = rospy.Publisher('~camera_info', CameraInfo)

    cimg = Image()
    cinf = CameraInfo()
    r = rospy.Rate(15)
    while not rospy.is_shutdown():

        if pub_cimg.get_num_connections() == 0:
            r.sleep()
            continue

        colorimg = np.zeros((240,320,3), np.uint8)
        try:
          cimg = bridge.cv2_to_imgmsg(colorimg, "bgr8")
          cimg.header.stamp = rospy.Time.now()
          cimg.header.frame_id =  "1"

          cinf = CameraInfo() 
          cinf.header.stamp = cimg.header.stamp
          cinf.header.frame_id =  "1"

          pub_cimg.publish(cimg)
          pub_cinf.publish(cinf)

        except CvBridgeError, e:
          print e

        r.sleep()


if __name__ == "__main__":
    try:
        dummy_camera()
    except RuntimeError as e:
        rospy.logerr('Something went wrong: %s' % str(e) )

• こんなのを作ってみた、そして依存ファイルを書いたりするpackage.xmlをいじって、

package.xml

<?xml version="1.0"?>
<package>
  <name>dummy_camera</name>
  <version>0.0.0</version>
  <description>The dummy_camera package</description>

～略～

  <!--   <test_depend>gtest</test_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>cv_bridge</build_depend><!-- 追加！ -->

  <run_depend>roscpp</run_depend>
  <run_depend>rospy</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>cv_bridge</run_depend> <!-- 追加！ -->

～略～

</package>- 

• あ、あと忘れずに

chmod +x ~/ros_catkin_ws/indigo/src/dummy_camera/scripts/dummy_camera_node.py

• 実行できるようにして、あとはmake

cd ~/ros_catkin_ws/indigo/
catkin_make

• これでノードは出来上がり！

組み合わせてみる

• launchフォルダを作って、

rgbdslam+dummy_camera+naoqi_depth_cam.launch

<!-- This file shows the most important parameters in their default settings,
     to make them easily available for beginners.

     The openni driver has to be started seperately, e.g. with "roslaunch openni_launch openni.launch" -->
<launch>
  <arg name="nao_ip"/> 

  <node name="dummy_camera" pkg="dummy_camera" type="dummy_camera_node.py" output="screen"> 
  </node>

  <include file="$(find naoqi_sensors)/launch/camera.launch"> 
      <arg name="nao_ip"      value="$(arg nao_ip)" />
      <arg name="source"      value="2" /> 
      <arg name="brightness"  value="0" /> 
      <arg name="color_space" value="17" /> 
  </include> 

  <node pkg="rgbdslam" type="rgbdslam" name="rgbdslam" cwd="node" required="true" output="screen"> 
    <!-- Input data settings-->
    <param name="config/subscriber_queue_size"         value="10"/>
    <param name="config/topic_image_mono"              value="/dummy_camera/image_color"/> 
    <param name="config/camera_info_topic"             value="/dummy_camera/camera_info"/>
    <param name="config/topic_image_depth"             value="/camera/image_raw"/>
    <param name="config/topic_points"                  value=""/> <!--if empty, poincloud will be reconstructed from image and depth -->

    <param name="config/feature_detector_type"         value="SURF"/><!-- also available: SIFT, SURF, GFTT (good features to track), ORB. -->
    <param name="config/feature_extractor_type"        value="SURF"/><!-- also available: SIFT, SURF, SURF128 (extended SURF), ORB. -->
    <param name="config/nn_distance_ratio"             value="0.5"/> <!-- Feature correspondence is valid if distance to nearest neighbour is smaller than this parameter times the distance to the 2nd neighbour -->
    <param name="config/max_keypoints"                 value="600"/><!-- Extract no more than this many keypoints (not honored by SIFTGPU)-->
    <param name="config/ransac_iterations"             value="200"/>
    <param name="config/cloud_creation_skip_step"      value="1"/><!-- subsample the images' pixels (in both, width and height), when creating the cloud -->
    <param name="config/data_skip_step"                value="1"/><!-- subsample the image frames (in time) -->

  </node>
</launch>

roslaunch dummy_camera rgbdslam+dummy_camera+naoqi_depth_cam.launch

• できた？ …うーん起動してるけどちゃんと動いてない…orz
• もしかしてRGB画像が必須なの？

RGBD-SLAMのソースを読む

• 諦めてソースをちゃんと読んでみることに

mainからたどる

• main.cpp:122 main()

  • ここはOpenNIListener のインスタンスを生成して読んで後は待つだけ。
  • その他の部分はパラメタとかGUIとかの準備っぽい

• openni_listener.cpp:123 OpenNIListener(GraphManager*

  • ここは、3種のデータを受け取ってコールバック呼び出しをするところ。message_filters::Synchronizer。入力しているトピックのタイムスタンプが大体一致したら1回コールバックを呼ぶ、という事を繰り返すモノが主役。
  • その他は、各種準備だけ。

• openni_listener.cpp:485 OpenNIListener::noCloudCallback (

  • ここも更なる関数を呼ぶ準備だけ。

• openni_listener.cpp:665 OpenNIListener::noCloudCameraCallback

  • ここからコールバックの中身。RGBDSLAMの中核になる部分。
  • retrieveTransformations 何か準備してるみたいけどあまり重要でない？
  • callProcessing Nodeクラスをnewして、cameraCallback呼ぶらしい毎回newするっぽい
  • その後、cameraCallback

辿り着いた

• node.cpp:113 Node::Node(
  • 画像から生成するバージョンのコンストラクタの中をみてみると…

detector->detect( gray_img, feature_locations_2d_, detection_mask);// fill 2d locations

• ああやっぱり。画像の方を使って特徴点マッチしてるよ
  • その後の処理の projectTo3D とかみると、見つけた点を前提に処理してる…
  • つまり軽量化のためか最初は画像を使ってマッチングすると…
  • ICPとかの記述もあるからその後、ポイントの雲同士の位置合わせもするんだろうけど、最初は画像が無い事には、そこまでの処理が始まらない

画像が必要だと判明し、３歩進んで２歩戻る

• 画像をちゃんと与えないとダメっぽいのでノード作り直さないと…orz
• ああ、年越しそう！

一応その後も追ってみた

• openni_listener.cpp:686 OpenNIListener::callProcessing

  • GUIとの分岐をしたりしつつ、processNodeを呼ぶのがお仕事

• openni_listener.cpp:715 OpenNIListener::processNode

  • graph_mgr_->addNode(new_node); これしかしてないっぽい。
  • それ以外はGUIの表示とかだけ…

• graph_manager.cpp:723 GraphManager::addNode

  • nodeComparisons と localizationUpdate の呼び出しがメインっぽい

• graph_manager.cpp:416 GraphManager::nodeComparisons

  • nodeComparisons
  • 深かったけどここら辺が、RGBDSLAMの真価を発揮してる部分っぽい。おかげでさっぱりわからない！

やり直し

カメラについてしっかり学ぶ

• 学んでおかないと逆算出来ないのである程度覚える
  • rosの資料１
  • rosの資料２
  • opencvの資料
  • どこかの大学の資料
  • 自分的には最後のpdfが一番わかりやすかった。
• 前回のカメラパラメータの計算は間違ってたことを理解した（気がする）
• fx,fy の意味…
  • fx,fyは 焦点距離…で仮想の投影面への距離。fx,fy分かれているけど、正方ピクセルなら両方一緒。
  • 前回はこれが画像のアスペクト比に比例すると思い込んでた…けど違ったらしい。
  • なので普通両方いっしょ。

というわけで調査フェーズ

• まずはPepper君の情報

Sensor       X (m)   Y (m)  Z (m)  VFOV   HFOV   Rotation
CameraTop    0.0868  0.0000 0.1631 44.30° 57.20° No 
CameraBottom 0.0936  0.0000 0.0616 44.30° 57.20° rotationY(40.0*TO_RAD) 
DepthCamera  0.05138 0.0390 0.1194 45.00° 58.00° 

• そしてxtion proのパラメータ(参考になる資料よりそのまま)

fx = 525
fy = 525
cy = 239.5 
cx = 319.5

• ためしにxtion proのパラメータ(自分で計算してみた)

fx = 1/tan(hfov/2) * (640/2) = 577.295282
fy = 1/tan(vfov/2) * (480/2) = 577.295282
cy = 240 
cx = 320

…自分で計算したのと合わない

やっぱり、計算間違ってるのかと調べた結果…

http://wiki.ros.org/kinect_calibration/technical こちらで判明。

RGBカメラの値が 525 で、IRカメラは計算通りの約580…
という事はデプス画像ってRGBカメラに合わせて補正かけた結果の画像という事らしい。
さっきの資料にもRGBより大きくしてステレオカメラの要領で計算と書いてある。
XtionPro(LiveじゃないのはRGBカメラなし==Pepperのセンサ)でも一緒という事らしい。

• …でもそれじゃ相対位置出す際のカメラの中心位置ってどう扱えば…素直にトライアンドエラー？
  • 可能性１ RGBカメラのあるであろう位置にキャリブレーションされてる
  • 可能性２ IRカメラの位置に居るけどFOVが変わってる
  • 可能性３ 実はFOVはそのままで525じゃない(可能性薄そう)

naoのtopのcameraのパラメータ(自分で計算してみた)
fx = 1/tan(hfov/2) * (320/2) = 293.46  
fy = 1/tan(vfov/2) * (240/2) = 293.46 
cy = 240/2 
cx = 320/2

             奥行     横      高さ
CameraTop    0.08680  0.0000 0.1631 
DepthCamera  0.05138  0.0390 0.1194 
offsToTo     0.03542 -0.0390 0.0447

推理フェーズ

• やりたいことは
  • デプス画像の各画素に相当するRGB値を得たい。
• 問題点
  • RGBカメラとデプス画像はズレがある
  • デプス画像はIRカメラそのものではないようなので中心位置がIRカメラと同じとは限らない。
• 仮定してみる
  • 少なくともIRカメラとの距離の差は確実に存在するはず。
  • 仮想のRGBカメラの位置に合わせてデプス画像が作られていた場合、内側にずれる。
• 結論
  • まずはIRカメラと同じ位置と仮定してズレを補正
  • その後、内側にずらしてみて様子を見る

作成フェーズ

• デプス画像の空間での画像にTopカメラの画像をマッピングするプログラム
  • こちらの方の記事を雛形して作成！ PepperくんでPoint Cloud

# Camera parameters (only tested on Pepper)

# DepthCam Focal length
DepthCamFX = 525.0 / 2
DepthCamFY = 525.0 / 2

# DepthCam Optical center
DepthCamCX = 319.5 / 2
DepthCamCY = 239.5 / 2

# TopCam Focal length
TopCamFX = 293.46  
TopCamFY = 293.46 

# TopCam Optical center
TopCamCX = 160
TopCamCY = 120 

# meter to millimeter
UNIT_SCALING = 0.001

#
TopCamToDCamOfsZ =  0.03542 
TopCamToDCamOfsX = -0.03900 
TopCamToDCamOfsY =  0.04470

width        = 320
height       = 240
depthImgData     = 
rgbImgData       = 
mappedRgbImgData = 

for v in range(height):
    for u in range(width):
        offset = (v * width + u) * 2
        # Depthカメラ
        depth = ord(depthImgData[offset]) + ord(depthImgData[offset+1]) * 256 

        x = (u - DepthCamCX) * depth * UNIT_SCALING / DepthCamFX
        y = (v - DepthCamCY) * depth * UNIT_SCALING / DepthCamFY
        z = depth * UNIT_SCALING

        # Topカメラの空間での投影された座標を計算
        x = x - TopCamToDCamOfsX
        y = y - TopCamToDCamOfsY
        z = z - TopCamToDCamOfsZ
        scrX = TopCamFx * x / z + TopCamCX 
        scrY = TopCamFy * y / z + TopCamCY

        # 補正後画像を作成
        if scrX < 0 or scrY < 0 oe scrX > width or scrY > height:
            # 範囲外は真黒に 
            mappedRgbImgData[offset/2*3+0] = 0
            mappedRgbImgData[offset/2*3+1] = 0
            mappedRgbImgData[offset/2*3+2] = 0
        else:        
            mappedRgbImgData[offset/2*3+0] = rgbImgData[(scrX * width + scrY)*3+0]
            mappedRgbImgData[offset/2*3+1] = rgbImgData[(scrX * width + scrY)*3+1]
            mappedRgbImgData[offset/2*3+2] = rgbImgData[(scrX * width + scrY)*3+2]

• こんな感じで作っていったけどどうしても合わない…

調整って大変だよね

• うーんうまくいかないので調整しながらやろう
• でもビルドしなおすのは面倒…
• 調てみるとROSには動的な調整のいい方法があるらしい
• http://wiki.ros.org/ja/dynamic_reconfigure/Tutorials
• チュートリアルを参考に… cfg/Camera.cfg を作成
  • 重要な点
  • exit(gen.generate(PACKAGE, "dummy_camera", "Camera")) 最後の名前はファイル名と一緒にする

• チュートリアルを参考に… CMakeLists.txt に追記

  • 重要な点
  • generate_dynamic_reconfigure_options(cfg/Camera.cfg) これがpythonの dummy_camera/cfg/CameraConfig.py モジュールを自動生成する。
  • ここでパッケージ名とかぶるようなファイル(dummy_camera.pyみたいな名前で)を作ってたりすると検索パスの順番の関係か no module name とか言われるてハマる(ハマった…)

• [編集中メモ]あとでここにgithubのリンク貼る！

後編へつづく！

• といいな…

言葉にできない

• ラーラーラー ことーばにーできなーい
• という事で文章では表現が難しいのでホントは写真でもあるといいけど言葉にしてみる現在どはまり中の問題を書いておきます。(この記事が前編になった理由)

やはりPepper君のモノクロデプス画像は間違っている。

• どうにもXtionとか世の中のKinectの参考画像やデータと俺の青春ラブ…じゃなくてPepper君のモノクロデブス画像は何かが違うような気がします。
• うちのペッパー君の返すデータはどうも天井とかが中心などがボコボコに歪んでる…端っこも歪みまくってるようです…
• ただ世の中のデプス画像はOpenNIの公式ドライバから得たデータだと思うので補正がかかってるのかもしれないし、使えない範囲をクリップしてるとか自分の計算式が間違っているのかもしれない。
• 個人的には、PepperEYEに被さってる赤外線透過するカバー、あれ、像をゆがめてないよね…と疑ってますが、検証方法が凄く難しいので積んでます。（聞いてみたらあれを外すまで分解するのはほぼ無理だといってました）
  • まあ計算式が間違ってるに１００ガバス
  • IR画像が取れれば即判明なんですけど…どっちにしてもキャリブレーションしないと辛い程度のデプス値しかとれてません
  • 世の中の旧KinectとかXtionの動画は驚くほど平面が平面として取れてる画像ばかりなので、あんな画像が取れればSLAMもぴったり合いそうなんですけど…単にセンサの性能が家の環境だとその程度しか取れない可能性も…

その他