ROS下运行ORB-SLAM2的数据采集存储和提取

ROS下运行ORB-SLAM2的数据采集存储和提取

前提：相机图像发布问题解决

目标：一边进行SLAM，一边存储数据流

问题：发布什么类型的数据内容？如何提取rosbag中的图片和点云数据？

手段：使用rosbag record 进行数据记录，后提取

—————————————————————正文————————————————————————

1. 采集数据

接上文，解决了图片发表的问题之后，在ORB-SLAM2与Realsense基础下进行SLAM，利用ORB-SLAM2进行相机轨迹计算。在terminal中运行以下命令，将各个部分启动（这里如果能集成一下就好了）

# launch realsense camera
roslaunch realsense2_camera rs_rgbd.launch
# run IR camera
rosrun tau2camera-ros tau2_raw_image_pub
# run ORB-SLAM2
rosrun ORB_SLAM2 RGBD /home/yang/Desktop/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/yang/Desktop/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/d415.yaml
# run rosbag for recording
rosbag record /topic_name

可以通过rosbag info 查看数据内容

path:        2019-12-23-16-33-48.bag
version:     2.0
duration:    2:22s (142s)
start:       Dec 23 2019 16:33:49.07 (1577090029.07)
end:         Dec 23 2019 16:36:11.88 (1577090171.88)
size:        29.7 GB
messages:    29009
compression: none [9783/9783 chunks]
types:       realsense2_camera/Extrinsics [3627b43073f4cd5dd6dc179a49eda2ad]
             sensor_msgs/CameraInfo       [c9a58c1b0b154e0e6da7578cb991d214]
             sensor_msgs/Image            [060021388200f6f0f447d0fcd9c64743]
             sensor_msgs/PointCloud2      [1158d486dd51d683ce2f1be655c3c181]
topics:      /camera/aligned_depth_to_color/camera_info    2192 msgs    : sensor_msgs/CameraInfo      
             /camera/aligned_depth_to_color/image_raw      1908 msgs    : sensor_msgs/Image           
             /camera/aligned_depth_to_infra1/camera_info   2482 msgs    : sensor_msgs/CameraInfo      
             /camera/aligned_depth_to_infra1/image_raw     2509 msgs    : sensor_msgs/Image           
             /camera/color/camera_info                     1848 msgs    : sensor_msgs/CameraInfo      
             /camera/color/image_raw                       1849 msgs    : sensor_msgs/Image           
             /camera/color/image_rect_color                1818 msgs    : sensor_msgs/Image           
             /camera/depth/camera_info                     1925 msgs    : sensor_msgs/CameraInfo      
             /camera/depth/image_rect_raw                  1885 msgs    : sensor_msgs/Image           
             /camera/depth_registered/points               2345 msgs    : sensor_msgs/PointCloud2     
             /camera/extrinsics/depth_to_color                1 msg     : realsense2_camera/Extrinsics
             /camera/extrinsics/depth_to_infra1               1 msg     : realsense2_camera/Extrinsics
             /camera/extrinsics/depth_to_infra2               1 msg     : realsense2_camera/Extrinsics
             /camera/infra1/camera_info                    1927 msgs    : sensor_msgs/CameraInfo      
             /camera/infra1/image_rect_raw                 1897 msgs    : sensor_msgs/Image           
             /camera/infra2/camera_info                    1931 msgs    : sensor_msgs/CameraInfo      
             /camera/infra2/image_rect_raw                 1952 msgs    : sensor_msgs/Image           
             /tau2_camera/image                             538 msgs    : sensor_msgs/Image

几个关键的数据流是：
/tau2_camera/image、/camera/color/image_raw、/camera/depth_registered/points

2. 提取数据，并以ROS时间对其命名

# extract pointcloud out of bag file
rosrun pcl_ros bag_to_pcd rosbag_name.bag /camera/depth_registered/points

# extract images(png) out of bag file
# switch the topic name inside the file to choose which type of data is extracted
python2 extract.py

提取pointcloud2类型的数据，可以用上面第一句话，会直接生成一个文件夹包含所有的pointcloud.pcd文件
提取图片数据采用python脚本的形式，参考了他的博客，这里注意的是，在python3下运行该脚本会报错，因此在python2下运行该脚本

import cv2
ImportError: /opt/ros/kinetic/lib/python2.7/dist-packages/cv2.so: undefined symbol: PyCObject_Type

#!/usr/bin/python2
# Extract images from a bag file.

#PKG = 'beginner_tutorials'
import roslib;   #roslib.load_manifest(PKG)
import rosbag
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError

# Reading bag filename from command line or roslaunch parameter.
#import os
#import sys


class ImageCreator():


    def __init__(self):
        self.bridge = CvBridge()
        with rosbag.Bag("2019-12-23-16-33-48.bag", "r") as bag:  
            for topic,msg,t in bag.read_messages():
                if topic == "/camera/color/image_raw":
                ##if topic == "/camera/aligned_depth_to_color/image_raw":
                #if topic == "/tau2_camera/image":
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")#color images
                            #cv_image = self.bridge.imgmsg_to_cv2(msg,"mono16")#tau2 images
                        except CvBridgeError as e:
                            print( e )
                        timestr = "%.6f" %  msg.header.stamp.to_sec()
                        print(timestr)
                        image_name = "color"+ timestr+ ".png" 
                        ##image_name = "aligned_depth_to_color"+ timestr+ ".png" 
                        #image_name = "tau2_camera"+ timestr+ ".png" 
                        cv2.imwrite(image_name, cv_image) 

if __name__ == '__main__':

    #rospy.init_node(PKG)

    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass

之后在本文件夹下有了以下的内容

total 31103580
-rw-rw-r-- 1 yang yang 31849779337 12月 23 16:36 2019-12-23-16-33-48.bag
drwxrwxr-x 2 yang yang 98304 12月 23 22:31 colorimage
-rw-rw-r-- 1 yang yang 259 12月 23 22:41 EXTRACTALL
-rwxrwxr-x 1 yang yang 1577 12月 23 22:27 extract.py
-rw-rw-r-- 1 yang yang 9672 12月 23 16:36 KeyFrameTrajectory.txt
drwxrwxr-x 2 yang yang 122880 12月 23 22:10 pcd
drwxrwxr-x 2 yang yang 36864 12月 23 22:24 tau2images

3. 后续工作

• 对齐数据流
• 点云融合
• 纹理赋值