ROS orb_slam2_rosの結果をPython + matplotlibで表示 その1

こちらでORB SLAM2の結果を配信できるようになりました。
Ubuntu 18.04にorb_slam2_rosをインストール

結果をPythonプログラムで受け取り、matplotlibを使用して表示してみます。
Python matplotlibで3Dデータを可視化(散布図プロットと3次元座標での指定)

今回はdebug_imageで配信されるものと同等の内容が表示できるところまで試してみます。
ROS orb_slam2_rosによる車載カメラ動画の解析



パッケージの作成



こちらで作成したパッケージにプログラムを追加しました。
ROS Python + QtでGUIを作成1 簡単な画面の表示まで


$ roscd gui_tutorials
$ touch scripts/slam_basic1.py
$ chmod +x scripts/slam_basic1.py





最初の目標



配信されているオリジナル画像をmatplotlibで表示してみます。
今回、動画は「/videofile/image_raw」トピックで配信されているので、
これを監視して表示します。

こちらを参考にQtで表示してみます。
ROS Python + QtでGUIを作成5 配信された画像のARマーカー認識

・slam_basic1.py


  1. #!/usr/bin/env python
  2. # -*- coding:utf-8 -*-
  3. import rospy
  4. import sys
  5. import cv2
  6. from cv_bridge import CvBridge, CvBridgeError
  7. import numpy as np
  8. from sensor_msgs.msg import Image
  9. from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout, QLabel, QSizePolicy
  10. from PyQt5.QtGui import QImage, QPalette, QPixmap
  11. bridge = CvBridge()
  12. # GUI
  13. app = QApplication([])
  14. window = QWidget()
  15. window.setWindowTitle('Image View')
  16. # ファイルを読み込み
  17. imageLabel = QLabel()
  18. # スケールは1.0
  19. imageLabel.scaleFactor = 1.0
  20. layout = QVBoxLayout()
  21. layout.addWidget(imageLabel)
  22. window.setLayout(layout)
  23. window.resize(500, 400)
  24. def listener():
  25.     rospy.init_node('listener', anonymous=True)
  26.     rospy.Subscriber('/videofile/image_raw', Image, callback)
  27.     # spin() simply keeps python from exiting until this node is stopped
  28.     #rospy.spin()
  29.     window.show()
  30.     app.exec_()
  31. def callback(ros_data):
  32.     qimage = convert_qimage(ros_data)
  33.     # ラベルに読み込んだ画像を反映
  34.     imageLabel.setPixmap(QPixmap.fromImage(qimage))
  35. def convert_qimage(ros_data):
  36.     cv_rgb = bridge.imgmsg_to_cv2(ros_data, "rgb8")
  37.     # QImage変換
  38.     h, w = cv_rgb.shape[:2]
  39.     qimg = QImage(cv_rgb.flatten(), w, h, QImage.Format_RGB888)
  40.     return qimg
  41.     
  42. if __name__ == '__main__':
  43.     listener()



実行


$ rosrun gui_tutorials slam_basic1.py



動画が表示できました。

960_01.png




matplotlib



この画像に検出した座標データを上書きする必要があります。
自分で計算するのは面倒なので、matplotlibの背景に画像を指定して表示する方法を調べました。
PyQt5にmatplotlibを埋め込んでグラフを表示する

matplotlibでの表示に変更します。
処理が追いつかないので適当に間引いて表示しています。
ここらへんは今後の課題です。


  1. #!/usr/bin/env python
  2. # -*- coding:utf-8 -*-
  3. import rospy
  4. import sys
  5. import cv2
  6. from cv_bridge import CvBridge, CvBridgeError
  7. import numpy as np
  8. from sensor_msgs.msg import Image
  9. import matplotlib
  10. matplotlib.use('Qt5Agg')
  11. from PyQt5 import QtCore, QtWidgets
  12. from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
  13. from matplotlib.figure import Figure
  14. bridge = CvBridge()
  15. class ApplicationWindow(QtWidgets.QMainWindow):
  16.     def __init__(self):
  17.         QtWidgets.QMainWindow.__init__(self)
  18.         self.main_widget = QtWidgets.QWidget(self)
  19.         l = QtWidgets.QVBoxLayout(self.main_widget)
  20.         
  21.         # --------------------------------------------
  22.         # FigureCanvasに表示するグラフ
  23.         fig = Figure()
  24.         # グラフを表示するFigureCanvasを作成
  25.         self.fc = FigureCanvas(fig)
  26.         # グラフの設定
  27.         self.fc.axes = fig.add_subplot(111)
  28.         # 描画設定
  29.         self.fc.setParent(self.main_widget)
  30.         FigureCanvas.setSizePolicy(self.fc,
  31.                                  QtWidgets.QSizePolicy.Expanding,
  32.                                  QtWidgets.QSizePolicy.Expanding)
  33.         FigureCanvas.updateGeometry(self.fc)
  34.         # --------------------------------------------
  35.         # 作成したFigureCanvasを画面に追加
  36.         l.addWidget(self.fc)
  37.         # 中央に配置
  38.         self.setCentralWidget(self.main_widget)
  39.         self.drawing_counter = 0
  40.     def listener(self):
  41.         rospy.init_node('listener', anonymous=True)
  42.         rospy.Subscriber('/videofile/image_raw', Image, self.callback)
  43.         # spin() simply keeps python from exiting until this node is stopped
  44.         #rospy.spin()
  45.     def callback(self, ros_data):
  46.         self.drawing_counter += 1
  47.         if self.drawing_counter % 20 != 0:
  48.             return
  49.         im = bridge.imgmsg_to_cv2(ros_data, "rgb8")
  50.         self.fc.axes.imshow(im)
  51.         self.fc.draw()
  52.     
  53. if __name__ == '__main__':
  54.     qApp = QtWidgets.QApplication([])
  55.     aw = ApplicationWindow()
  56.     aw.setWindowTitle("matplotlib sample")
  57.     aw.listener()
  58.     aw.show()
  59.     qApp.exec_()



実行結果


$ rosrun gui_tutorials slam_basic1.py



960_02.png





センサーデータのプロット



ここまで作って気がついたのですが、どうもデバッグ画面で表示されるマーカーの情報は
トピックに流れていない模様。


/orb_slam2_r200_mono/map_points



ここにx, y, z座標のポイントデータが配信されています。
これを可視化してみようと思います。

よく理解できていないのですが、どうもSLAMでmapデータが出来上がる前は、
空のデータが設定されている模様。
ポイント数が多いので、1回だけプロットしています。

PointCloud2のデータ変換はこちらのソースが参考になりました。
https://github.com/udacity/RoboND-Perception-Exercises/blob/master/Exercise-3/sensor_stick/src/sensor_stick/pcl_helper.py


  1. #!/usr/bin/env python
  2. # -*- coding:utf-8 -*-
  3. import rospy
  4. import sys
  5. import cv2
  6. from cv_bridge import CvBridge, CvBridgeError
  7. import numpy as np
  8. from sensor_msgs.msg import CompressedImage
  9. from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout, QLabel, QSizePolicy
  10. from PyQt5.QtGui import QImage, QPalette, QPixmap
  11. # ARマーカー検出
  12. aruco = cv2.aruco
  13. dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
  14. # GUI
  15. app = QApplication([])
  16. window = QWidget()
  17. window.setWindowTitle('Image View')
  18. # ファイルを読み込み
  19. imageLabel = QLabel()
  20. # スケールは1.0
  21. imageLabel.scaleFactor = 1.0
  22. layout = QVBoxLayout()
  23. layout.addWidget(imageLabel)
  24. window.setLayout(layout)
  25. window.resize(500, 400)
  26. def listener():
  27.     rospy.init_node('listener', anonymous=True)
  28.     rospy.Subscriber("/camera/image/compressed", CompressedImage, callback)
  29.     # spin() simply keeps python from exiting until this node is stopped
  30.     #rospy.spin()
  31.     window.show()
  32.     app.exec_()
  33. def callback(ros_data):
  34.     
  35.     qimage = find_marker(ros_data)
  36.     # ラベルに読み込んだ画像を反映
  37.     imageLabel.setPixmap(QPixmap.fromImage(qimage))
  38. def find_marker(ros_data):
  39.     np_arr = np.fromstring(ros_data.data, np.uint8)
  40.     cv_bgr = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
  41.     
  42.     # マーカの検出
  43.     corners, ids, rejectedImgPoints = aruco.detectMarkers(cv_bgr, dictionary)
  44.     # 検出したマーカーを描画
  45.     aruco.drawDetectedMarkers(cv_bgr, corners, ids, (0,255,0))
  46.     # BGR -> RGB変換
  47.     cv_rgb = cv2.cvtColor(cv_bgr, cv2.COLOR_BGR2RGB)
  48.     # QImage変換
  49.     h, w = cv_rgb.shape[:2]
  50.     qimg = QImage(cv_rgb.flatten(), w, h, QImage.Format_RGB888)
  51.     return qimg
  52.     
  53. if __name__ == '__main__':
  54.     listener()



プロット結果はこちら。

960_03.png

何だろう?

別の動画ファイルでも解析してみます。
【素材】国道421号 起点~終点まで走行 #2
【素材】国道421号 起点~終点まで走行 #3

960_04.png

960_05.png

よくわからんですな。
付属のサンプルプログラムを見て、出直します。


【参考URL】

【素材】国道421号 起点~終点まで走行
【素材】国道421号 起点~終点まで走行 #2
【素材】国道421号 起点~終点まで走行 #3
https://github.com/udacity/RoboND-Perception-Exercises/blob/master/Exercise-3/sensor_stick/src/sensor_stick/pcl_helper.py

ROS orb_slam2_rosによる車載カメラ動画の解析

やっと道具が揃いました。
Ubuntu 18.04にorb_slam2_rosをインストール
ROS video_stream_opencvで動画(mp4)ファイルを再生する

動画ファイルを再生して、ORB SLAM 2で解析。
デバッグ画像を表示してみます。


orb_slam2_r200_mono.launch



orb_slam2の起動ファイルを編集し、動画が配信されているトピックをチェックするよう修正します。

・catkin_ws/src/orb_slam_2_ros/ros/launch/orb_slam2_r200_mono.launch

remap先を「/videofile/image_raw」に変更


  1. <launch>
  2. <node name="orb_slam2_r200_mono" pkg="orb_slam2_ros"
  3.      type="orb_slam2_ros_mono" args="
  4.          $(find orb_slam2_ros)/orb_slam2/Vocabulary/ORBvoc.txt
  5.          $(find orb_slam2_ros)/orb_slam2/config/RealSenseR200Mono.yaml"
  6.      output="screen">
  7.      <!-- <remap from="/camera/image_raw" to="/camera/rgb/image_raw" /> -->
  8.      <remap from="/camera/image_raw" to="/videofile/image_raw" />
  9.      <param name="publish_pointcloud" type="bool" value="true" />
  10.      <param name="pointcloud_frame_id" type="string" value="map" />
  11.      <param name="camera_frame_id" type="string" value="camera_link" />
  12. </node>
  13. </launch>





起動テスト



・コンソール1

orb_slam2を起動します。


$ roslaunch orb_slam2_ros orb_slam2_r200_mono.launch




・コンソール2

動画ファイルを再生


$ roslaunch video_file.launch



※video_file.launchの内容はこちらを参照
ROS video_stream_opencvで動画(mp4)ファイルを再生する

・コンソール3

画像ビューワーを起動します。


$ rqt_image_view




ちゃんと解析されているじゃないですか。

956_01.png

次はPythonで解析結果を処理する方法を調べてみます。


【参考URL】

【素材】国道421号 起点~終点まで走行

ROS video_stream_opencvで動画(mp4)ファイルを再生する

ROSでは、USBカメラの画像を処理するサンプルがよくありますが、
テストを行う時は動画ファイルの再生結果を入力としたい場合がよくあります。

Ubuntu 18.04にORB_SLAM2 + ROSをインストール
こちらでは、rosbagでログデータを再生していますが、
できれば撮影した動画ファイルをそのまま使用したい。


video_stream_opencv



このパッケージを使えば簡単に実現できそうです。
http://wiki.ros.org/video_stream_opencv

aptでインストール。


$ sudo apt install ros-melodic-video-stream-opencv




起動は、launchファイルを記載しないと行けない模様。
catkinワークスペースに「video_file.launch」というファイルを作成します。

内容は以下のようになりました。
video_stream_providerに再生したいmp4ファイルを絶対パスで指定します。


  1. <?xml version="1.0"?>
  2. <launch>
  3. <!-- launch video stream -->
  4. <include file="$(find video_stream_opencv)/launch/camera.launch" >
  5.         <!-- node name and ros graph name -->
  6.         <arg name="camera_name" value="videofile" />
  7.         <!-- full path to the video file -->
  8.         <!-- wget http://techslides.com/demos/sample-videos/small.mp4 -O /tmp/small.mp4 -->
  9.         <arg name="video_stream_provider" value="/home/baranche/nc556.mp4" />
  10.         <!-- set camera fps to (video files not affected) -->
  11.         <!-- <arg name="set_camera_fps" value="30"/> -->
  12.         <!-- set buffer queue size of frame capturing to -->
  13.         <arg name="buffer_queue_size" value="1000" />
  14.         <!-- throttling the querying of frames to -->
  15.         <arg name="fps" value="30" />
  16.         <!-- setting frame_id -->
  17.         <arg name="frame_id" value="videofile_frame" />
  18.         <!-- camera info loading, take care as it needs the "file:///" at the start , e.g.:
  19.         "file:///$(find your_camera_package)/config/your_camera.yaml" -->
  20.         <arg name="camera_info_url" value="" />
  21.         <!-- flip the image horizontally (mirror it) -->
  22.         <arg name="flip_horizontal" value="false" />
  23.         <!-- flip the image vertically -->
  24.         <arg name="flip_vertical" value="false" />
  25.         <!-- enable looping playback -->
  26.         <arg name="loop_videofile" value="true" />
  27.         <!-- visualize on an image_view window the stream generated -->
  28.         <arg name="visualize" value="false" />
  29. </include>
  30. </launch>




再生するファイルは、ニコニ・コモンズの車載カメラ動画
http://commons.nicovideo.jp/search/tag/%E8%BB%8A%E8%BC%89%E5%8B%95%E7%94%BB

こちらをお借りしています。
【素材】国道421号 起点~終点まで走行

launchファイルが準備できたら実行。


$ roslaunch video_file.launch




rqt_image_viewで内容確認。


$ rqt_image_view



ちゃんと再生されました。

955_01.png


【参考URL】

http://wiki.ros.org/video_stream_opencv
http://commons.nicovideo.jp/search/tag/%E8%BB%8A%E8%BC%89%E5%8B%95%E7%94%BB
【素材】国道421号 起点~終点まで走行

Ubuntu 18.04にorb_slam2_rosをインストール

Ubuntu Server 18.04にORB SLAM2をインストールしました。
Ubuntu 18.04にORB_SLAM2 + ROSをインストール

次は解析結果を扱うプログラムを。と思ったのですが、
C++でプログラムを書くのがめんどくさい。

何か良い用法はないか調べてみると、「orb_slam2_ros」というパッケージを見つけました。
http://wiki.ros.org/orb_slam2_ros

ざっと見た感じ、このノードを起動して画像を送りつけると
解析結果をPublishしてくれる模様。

これなら、解析結果を受け取るノードがPythonで記載できそうです。
Ubuntu Server 18.04にインストールして見ます。



インストール



こちらを参考にインストールを行いました。
https://github.com/appliedAI-Initiative/orb_slam_2_ros

C++やOpenCVはこちらの手順デーインストール済です。
Ubuntu 18.06 ServerにOpenCV 3.4.1をインストール(install-opencv.sh)

Eigen3をインストール。


$ sudo apt install libeigen3-dev




続いてorb_slam2_rosをダウンロードし、ビルドします。
catkinワークスペースのsrcディレクトリに移動。
※ワークスペースのパスは以下のコマンドで確認できます。


$ echo $ROS_PACKAGE_PATH



私の場合、ホームディレクトリのcatkin_ws/srcです。


$ cd ~catkin_ws/src




ソースを取得して、catkin_wsに移動。
catkin_makeを実行します。


$ git clone https://github.com/appliedAI-Initiative/orb_slam_2_ros.git
$ cd ../
$ catkin_make



Ubuntu 18.04にORB_SLAM2 + ROSをインストール
この時の苦労はなんだったんだというぐらい、あっさりインストールできました。




動作確認



単眼カメラ用のノードを起動します。


$ roslaunch orb_slam2_ros orb_slam2_r200_mono.launch



どんなトピックが作成されたか確認してみます。


$ rostopic list -v

Published topics:
* /orb_slam2_r200_mono/debug_image/compressed [sensor_msgs/CompressedImage] 1 publisher
* /orb_slam2_r200_mono/debug_image/theora [theora_image_transport/Packet] 1 publisher
* /orb_slam2_r200_mono/debug_image/compressed/parameter_updates [dynamic_reconfigure/Config] 1 publisher
* /orb_slam2_r200_mono/debug_image/compressed/parameter_descriptions [dynamic_reconfigure/ConfigDescription] 1 publisher
* /orb_slam2_r200_mono/parameter_updates [dynamic_reconfigure/Config] 1 publisher
* /rosout [rosgraph_msgs/Log] 1 publisher
* /rosout_agg [rosgraph_msgs/Log] 1 publisher
* /orb_slam2_r200_mono/map_points [sensor_msgs/PointCloud2] 1 publisher
* /orb_slam2_r200_mono/parameter_descriptions [dynamic_reconfigure/ConfigDescription] 1 publisher
* /orb_slam2_r200_mono/debug_image/theora/parameter_updates [dynamic_reconfigure/Config] 1 publisher
* /orb_slam2_r200_mono/debug_image/compressedDepth [sensor_msgs/CompressedImage] 1 publisher
* /orb_slam2_r200_mono/debug_image/compressedDepth/parameter_updates [dynamic_reconfigure/Config] 1 publisher
* /orb_slam2_r200_mono/debug_image [sensor_msgs/Image] 1 publisher
* /orb_slam2_r200_mono/debug_image/theora/parameter_descriptions [dynamic_reconfigure/ConfigDescription] 1 publisher
* /orb_slam2_r200_mono/debug_image/compressedDepth/parameter_descriptions [dynamic_reconfigure/ConfigDescription] 1 publisher

Subscribed topics:
* /camera/rgb/image_raw [sensor_msgs/Image] 1 subscriber
* /rosout [rosgraph_msgs/Log] 1 subscriber




/camera/rgb/image_rawに画像データを送れば、
/orb_slam2_r200_mono/debug_imageで結果が確認できそう。

Ubuntu 18.04にORB_SLAM2 + ROSをインストール
こちらのサンプルをそのまま使用します。


$ rosbag play LSD_room.bag /image_raw:=/camera/rgb/image_raw



ビューワーで結果を表示。


$ rqt_image_view



動いてくれました。

954_01.png

これはお手軽にORB SLAM 2の機能が使えて便利です。

Ubuntu 18.04にORB_SLAM2 + ROSをインストール

ORB-SLAM2
https://github.com/raulmur/ORB_SLAM2

これをUbuntu Server 18.04にインストールしてみます。
前提条件として、ROSやOpenCV3はインストール済です。
ROSをUbuntu 18.04.1にインストールする
Ubuntu 18.06 ServerにOpenCV 3.4.1をインストール(install-opencv.sh)


必要なライブラリ



ROSやOpenCVをインストール済だと不要かと思いますが、
一応以下のライブラリのインストールコマンドを実行しておきました。


$ sudo apt install libglew-dev cmake libjpeg-dev libtiff5-dev libopenexr-dev libpng-dev





Pangolinのインストール



依存しているPangolinをインストールします。
https://github.com/stevenlovegrove/Pangolin


$ git clone https://github.com/stevenlovegrove/Pangolin.git
$ cd Pangolin
$ mkdir build
$ cd build
$ cmake ..
$ make -j4
$ sudo make install



※make -j4
仮想環境だと「virtual memory exhausted: Cannot allocate memory」エラーが
発生したので、スレッド数を明示しています。
通常は「-j」オプションのみでOKだと思います。
KVM 仮想環境上でビルド時に「virtual memory exhausted: Cannot allocate memory」



ORB_SLAM2



ソースを取得してビルドすると以下のエラーが発生します。


error: ‘usleep’ was not declared in this scope



調べてみると、unistd.hのincludeが不足しているとのこと。
add #include to include/System.h to solve compiling errors

修正した資産をpull requestしている方がいましたので、
こちらをソースを使用してビルドすることにします。
https://github.com/ILab-01/ORB_SLAM2


$ git clone https://github.com/ILab-01/ORB_SLAM2.git
$ cd ORB_SLAM2
$ chmod +x build.sh



※仮想環境の場合の例外
ここでbuild.shを編集。
「make -j」となっている箇所を「make -j4」と変更しておきました。

変更が終わったらビルドを実行。


$ ./build.sh




続いてROSノードをビルドします。

まず、build_ros.shをエディタで開いて「make -j」の箇所を「make -j4」に変更。

ROS_PACKAGE_PATHに「ORB_SLAM2/Examples/ROS」のパスを追加。
build_ros.shに実行権限を付与して実行します。


$ export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/baranche/dev/ORB_SLAM2/Examples/ROS
$ chmod +x build_ros.sh
$ ./build_ros.sh



ここでエラー。


[rosbuild] Building package ORB_SLAM2
Failed to invoke /opt/ros/melodic/bin/rospack deps-manifests ORB_SLAM2
[rospack] Error: package 'ORB_SLAM2' depends on non-existent package 'tf' and
rosdep claims that it is not a system dependency. Check the ROS_PACKAGE_PATH or try calling 'rosdep update'



依存している「tf」が不足しているとのこと。
こちらが参考になりました。
https://github.com/raulmur/ORB_SLAM2/issues/386

tfを追加しビルド。


$ sudo apt install ros-melodic-tf



再度エラー


ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc:233:9:
error: ‘usleep’ was not declared in this scope usleep(mT*1000);



該当ソースに


  1. #include <unistd.h>



を追記して再度ビルドすると通ってくれました。



動作確認



テストデータをダウンロードし解凍します。


$ wget http://vmcremers8.informatik.tu-muenchen.de/lsd/LSD_room.bag.zip
$ unzip LSD_room.bag.zip



ターミナル1 - roscore起動


$ roscore



ターミナル2 - データの再生


$ rosbag play LSD_room.bag /image_raw:=/camera/image_raw



ターミナル3 - デモの実行


$ rosrun ORB_SLAM2 Mono dev/ORB_SLAM2/Vocabulary/ORBvoc.txt dev/ORB_SLAM2/Examples/Monocular/TUM1.yaml



※エラーになる場合、ORB_SLAM2/Examples/ROSにパスを通してから実行します。


$ export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/baranche/dev/ORB_SLAM2/Examples/ROS



やっと動いてくれました。

949_01.png


【参考URL】

https://github.com/raulmur/ORB_SLAM2
ROSのORB_SLAM2を動かしてみた。
【7Days自由研究】Visual SLAMとROSを使えるようになる話 Day-5
add #include to include/System.h to solve compiling errors
https://github.com/raulmur/ORB_SLAM2/issues/386

プロフィール

Author:symfo
blog形式だと探しにくいので、まとめサイト作成中です。
Symfoware まとめ

PR




検索フォーム

月別アーカイブ