orbslam-car,rosbag

清疚 2023-01-21 11:20 81阅读 0赞

# 做：alias linux #

# Linux alias命令用于设置指令的别名 #

alias 别名设置保存到文件：/root/.bashrc里面就可以了上述命令，使每次都能够自动生效。

若要修改用户（而非全部用户）自己的alias，修改~/.bashrc文件

sudo gedit ~/.bashrc
    //在最后加入这一行
    alias sd='source devel/setup.sh'

source ~/.bashrc

# 安装一下才有 pcl\_viewer 命令 #

sudo apt install pcl-tools

# 代下服务 #

github下载链接

[https://shrill-pond-3e81.hunsh.workers.dev][https_shrill-pond-3e81.hunsh.workers.dev]

把git中的右键拷贝压缩包下载路径到这个网站上

[http://gitd.cc/][http_gitd.cc]

把后缀.git的克隆路径直接放进来

# Astra相机的ROS环境 #

## 搭建ROS环境 ##

我们也可以直接使用官网提供的ros驱动进行开发，目前支持ROS Kinetic 和 Melodic

Astra相机的ros驱动源码地址：[https://github.com/orbbec/ros\_astra\_camera][https_github.com_orbbec_ros_astra_camera]，我们当前以 Melodic 为例（即Ubuntu18.04下的ROS环境），推荐到[Github代下服务][http_gitd.cc]高速下载源码。

1.  [安装ROS][ROS]
2.  安装依赖
    
    这里要确保通过 `echo $ROS_DISTRO` 可以输出 `melodic`

sudo apt install ros-$ROS_DISTRO-rgbd-launch \
    ros-$ROS_DISTRO-libuvc \
    ros-$ROS_DISTRO-libuvc-camera \
    ros-$ROS_DISTRO-libuvc-ros

创建工作空间

mkdir -p ~/catkin_ws/src
    cd ~/catkin_ws/
    catkin_make
    source devel/setup.bash

在ROS工作空间释放源码（可以直接[下载此链接][Link 1]解压到src目录）

cd ~/catkin_ws/src
    git clone https://github.com/orbbec/ros_astra_camera

执行脚本，添加相机设备rule

roscd astra_camera
    ./scripts/create_udev_rules

编译astra\_camera

cd ~/catkin_ws
    catkin_make --pkg astra_camera

## 启动节点 ##

在启动节点前，首先要根据设备的id，修改一下启动参数。我们打开launch文件 `launch/astrapro.launch` 其中有一部分配置如下：

//未加载内参，不能显示点云图
    roslaunch astra_camera astrapro.launch
    //加载内参，显示点云图
    roslaunch astra_camera astrapro.launch
    //加载内参，运行orbslam时用这个，会提共 /camera/image_raw 节点 ，在astrapro_su_rgb.launch中 ：把camera/image_raw 映射成了camera/rgb/image_ra可以显示点云，但是运行orbslam时需要 camera/image_raw需要这个节点，所以要注释掉，具体进launch文件看，用rqt_image_view查看时刷新一下节点
    roslaunch astra_camera astrapro_su_rgb.launch

<group ns="$(arg camera)">
    <node pkg="astra_camera" type="camera_node" name="$(arg camera)_rgb">
        
        <param name="vendor" value="0x2bc5"/>
        <param name="product" value="0x0501"/>
            ....
    </node>

这里是通过 vendor （贴牌）和 product （产品id） 来启动设备的，所以我们要使用 `lsusb` 命令，来查看相机的这两个信息：

ty@ty-PC:~/Workspace/ROS/catkin_ws$ lsusb
    Bus 002 Device 001: ID 1d6b:0003 Linux Foundation 3.0 root hub
    Bus 001 Device 003: ID 5986:066d Acer, Inc 
    Bus 001 Device 002: ID 8087:0a2a Intel Corp. 
    Bus 001 Device 007: ID 1532:0037 Razer USA, Ltd 
    Bus 001 Device 006: ID 2bc5:0502  
    Bus 001 Device 005: ID 2bc5:0403  
    Bus 001 Device 004: ID 05e3:0610 Genesys Logic, Inc. 4-port hub
    Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub

我们通过对比插上设备前和插上设备后可以发现这里多出两行设备及对应总线：

Bus 001 Device 006: ID 2bc5:0502  
    Bus 001 Device 005: ID 2bc5:0403

此时，这里 `2bc5:0502` 代表彩色摄像头，`2bc5:0403` 代表深度摄像头，我们修改文件 `launch/astrapro.launch` 的以下内容：

替换为：

保存后，执行以下命令启动：

roslaunch astra_camera astrapro.launch

此时还暂时不能看到图像，需要通过订阅话题查看图像

## 预览图像及点云图 ##

### 使用rqt\_image\_view 刷新一下节点 ###

rosrun rqt_image_view rqt_image_view

选择彩色RGB原始图话题`/camera/rgb/image_raw`

![Untitled.png][]

选择深度Depth原始图话题`/camera/depth/image_raw`：

### ![Astra/Untitled%201.png][Astra_Untitled_201.png] ###

## ##

### 配置相机内参文件 ###

由于相机本身需要一些畸变配置文件来进行去畸变，所以可以通过设置如下配置，设置彩色相机和深度相机的配置文件：

1、拷贝 camera.yaml 文件和 depth.yaml 文件到cfg目录

[camera.yaml][]

[depth.yaml][]

2、修改 launch/astrapro.launch 文件

### 使用rviz ###

拷贝 `astra_camera.rviz` 文件到项目的rviz目录

`astra_camera.rviz`

代码段 小部件

//加载了内参，可以显示点云图
    roslaunch astra_camera astrapro_su.launch
    //加载内参，运行orbslam时用这个，会提共 /camera/image_raw 节点 ，在astrapro_su_rgb.launch中 ：把camera/image_raw 映射成了camera/rgb/image_ra可以显示点云，但是运行orbslam时需要 camera/image_raw需要这个节点，所以要注释掉，具体进launch文件看;用rqt_image_view查看时刷新一下节点
    roslaunch astra_camera astrapro_su_rgb.launch

rviz -d src/slam_vslam/ros_astra_camera/rviz/astra_camera.rviz

![astra\_rviz][astra_rviz]

如果之前没有配置相机内参，则这里不会显示点云图

## 保存点云 ##

由于点云图像发布的话题为，我们可以通过订阅此话题来保存点云数据。

即使用`pcl_ros`包下的`pointcloud_to_pcd`节点实时捕获，[详见此链接][Link 2]

rosrun pcl_ros pointcloud_to_pcd input:=/camera/depth_registered/points

查看点云 上面命令捕捉到点云后会打印出点云的名字160482605021468.pcd

pcl_viewer 160482605021468.pcd

此命令会实时将指定话题的点云保存到当前目录，所以要注意使用Ctrl+C停止

![pcl\_ros][pcl_ros]

如上，可见该命令执行过之后，很快就保存了3个pcd文件到当前目录，我们可以通过pcl\_viewer工具预览点云。

\-------------------

orbslam安装

1.安装Pangolin

git clone https://github.com/stevenlovegrove/Pangolin.git
    cd Pangolin
    mkdir build
    cd build
    cmake -DCPP11_NO_BOOST=1 ..
    # 执行编译，这里的参数根据cpu核心数来，8核的就是-j8
    make -j4
    
    # 编译完之后，执行安装
    sudo make install

2.安装Eigen

sudo apt-get install libeigen3-dev

引用Eigen库只需要添加其头文件即可（不需要链接库文件），即在 CMakeLists.txt 文件中只要包含如下内容即可（当前开源项目已添加，不需要重复添加）：

include_directories("/usr/include/eigen3")

如果不能确定eigen3的实际安装位置，可以通过以下命令进行定位：

sudo updatedb
    locate eigen3

### 安装OpenCV ###

**ROS版已集成**

对于Ubuntu18.04的ROS环境melodic中，已经默认集成了`OpenCV-3.2.0`，ORB\_SLAM2需要的OpenCV版本>2.4.3即可，此时可以不用再独立安装OpenCV了

**独立运行环境**

如果不想依赖ROS环境，可以独立编译安装OpenCV，首先下载相关的源码：

虽然[OpenCV官方源码][OpenCV]在github上，但是我们建议在gitee上下载`OpenCV-3.4.9`的源码，速度快：[https://gitee.com/mirrors/opencv/repository/archive/3.4.9][https_gitee.com_mirrors_opencv_repository_archive_3.4.9]

同时下载OpenCV的扩展模块`opencv_contrib`包：[https://gitee.com/mirrors/opencv\_contrib/repository/archive/3.4.9][https_gitee.com_mirrors_opencv_contrib_repository_archive_3.4.9]

下载完成后，这里给出`OpenCV-3.4.9`的环境编译及安装的[文档链接][Link 3]。

## 下载编译ORB\_SLAM2 ##

git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2
    cd ORB_SLAM2
    chmod +x build.sh
    ./build.sh

由于作者未考虑Ubuntu18.04的兼容问题，因而需要手动在以下文件添加一个include头：

`include/System.h` 中添加：`#include <unistd.h>`

添加完成之后再重新执行编译脚本即可

如果正常编译完成，则lib目录下会生成`libORB_SLAM2.so`库，Examples目录下会生成对应的可执行程序 **mono\_tum, mono\_kitti, mono\_euroc, stereo\_kitti, stereo\_euroc, rgbd\_tum。**

双目相机和深度图相机的图像预处理：

![orbslam\_camera][orbslam_camera]

### 编译配置 ###

可以在执行cmake和make编译命令之前，在根目录的 `CMakeLists.txt` 文件中（12行）添加以下两行配置，

来屏蔽大量的代码过时警告（对编译结果没有影响），方便编译出错时候排查问题。

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations")
    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated")

# ORB\_SLAM2的ROS #

## 编译ROS节点 ##

首先编译以下节点：**mono, monoAR, stereo and RGB-D**

1.  添加节点的源码路径到 `ROS_PACKAGE_PATH` 中，打开文件 `.bashrc` 并将如下内容添加到末尾，这里**要将其中的 `PATH/ORB_SLAM2` 替换成你自己的源码路径**，并确保以下内容在文件的 `source /opt/ros/melodic/setup.bash` 之后，即
    
    编辑文件

gedit ~/.bashrc

追加以下内容

source /opt/ros/melodic/setup.bash
    export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/3rdparty/ORB_SLAM2/Examples/ROS

执行

source ~/.bashrc

使上述配置生效

执行源码目录下的 `build_ros.sh` 脚本

cd pwd /home/iimt/3rdparty/ORB_SLAM2
    chmod +x build_ros.sh
    ./build_ros.sh

在Ubuntu18.04之前的系统上没有问题，但是在Ubuntu18.04及以后的系统上，会出现编译报错，先解决保存问题，重新编译即可。

## 错误处理 ##

如果运行 `build_ros.sh` 报出如下错误

libboost

/usr/bin/ld: CMakeFiles/Stereo.dir/src/ros_stereo.cc.o: undefined reference to symbol '_ZN5boost6system15system_categoryEv'
    /usr/lib/x86_64-linux-gnu/libboost_system.so: error adding symbols: DSO missing from command line
    collect2: error: ld returned 1 exit status
    CMakeFiles/Stereo.dir/build.make:217: recipe for target '../Stereo' failed
    make[2]: *** [../Stereo] Error 1
    CMakeFiles/Makefile2:131: recipe for target 'CMakeFiles/Stereo.dir/all' failed
    make[1]: *** [CMakeFiles/Stereo.dir/all] Error 2
    make[1]: *** Waiting for unfinished jobs....

确保boost环境已安装

sudo apt-get install libboost-dev libboost-thread-dev libboost-filesystem-dev

打开 /home/iimt/3rdparty/ORB\_SLAM2/`Examples/ROS/ORB_SLAM2/CMakeLists.txt` 并在如下位置添加一行 -`lboost_system`

set(LIBS 
    ${OpenCV_LIBS} 
    ${EIGEN3_LIBS}
    ${Pangolin_LIBRARIES}
    ${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
    ${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
    ${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
    -lboost_system
    )

修改保存并重新编译即可

./build_ros.sh

## 运行节点 ##

运行如下节点 之前，我们要先开启相机设备节点，保证有相机数据发布在 `/camera/image_raw` 话题，这样我们才能看到图像内容并进行建图和定位。

在源码中 /home/iimt/3rdparty/ORB\_SLAM2/Examples/ROS/ORB\_SLAM2/src/ros\_mono.cc 可以看到订阅的话题，我们可以改成 `/camera/rgb/image_raw，就不需要改astrapro_su_rgb.launch中的话题名了`

//加载内参，运行orbslam时用这个，会提共 /camera/image_raw 节点 ，在astrapro_su_rgb.launch中 ：把camera/image_raw 映射成了camera/rgb/image_ra可以显示点云，但是运行orbslam时需要 camera/image_raw需要这个节点，所以要注释掉，具体进launch文件看;用rqt_image_view查看时刷新一下节点
    roslaunch astra_camera astrapro_su_rgb.launch

单目案例 必须在当前源码目录下 cd /home/iimt/3rdparty/ORB_SLAM2 右键打开控制台 然后执行下面命令，否则找不到 Mono Vocabulary/ORBvoc.txt 磁带数据 ，astra.yaml用

把相机内参放里面 astra.yaml

**单目案例**

rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/astra.yaml

建图时速度必须慢，且时连惯的，不然会出现帧丢失

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70][]

红色点是当时现在看到的特帧点点，黑色点是之前保存过点点只是现在没有看到;

勾选最后一个 localization时开启定位功能，不会再更新地图了

**单目AR案例**

rosrun ORB_SLAM2 MonoAR Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/astra.yaml

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 1][]

需要旋转平移初始话相机

insert cube 插入一个立方体

勾选 Draw Points 显示角点

由于是实时获取的深度数据，此时深度数据缩放因数DepthMapFactor要确保为1.0。

则我们把原来的 `./Examples/ROS/ORB_SLAM2/Asus.yaml` 复制一份为 `./Examples/ROS/ORB_SLAM2/astra.yaml` 。并且主要要修改以下四个值

Camera.fx
    Camera.fy
    Camera.cx
    Camera.cy

这四个值实际上是通过相机标定得来的，这个在相机相关章节0会有原理讲解及实现。

Astra奥比中光相机的 `astra.yaml` 参考配置文件如下：

%YAML:1.0
    
    #--------------------------------------------------------------------------------------------
    # Camera Parameters. Adjust them!
    #--------------------------------------------------------------------------------------------
    
    # Camera calibration and distortion parameters (OpenCV) 
    Camera.fx: 577.54679
    Camera.fy: 578.63325
    Camera.cx: 310.24326
    Camera.cy: 253.65539
    
    Camera.k1: 0.0
    Camera.k2: 0.0
    Camera.p1: 0.0
    Camera.p2: 0.0
    
    Camera.width: 640
    Camera.height: 480
    
    # Camera frames per second 
    Camera.fps: 30.0
    
    # IR projector baseline times fx (aprox.)
    Camera.bf: 40.0
    
    # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
    Camera.RGB: 1
    
    # Close/Far threshold. Baseline times.
    ThDepth: 40.0
    
    # Deptmap values factor
    DepthMapFactor: 1.0
    
    #--------------------------------------------------------------------------------------------
    # ORB Parameters
    #--------------------------------------------------------------------------------------------
    
    # ORB Extractor: Number of features per image
    ORBextractor.nFeatures: 1000
    
    # ORB Extractor: Scale factor between levels in the scale pyramid   
    ORBextractor.scaleFactor: 1.2
    
    # ORB Extractor: Number of levels in the scale pyramid  
    ORBextractor.nLevels: 8
    
    # ORB Extractor: Fast threshold
    # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
    # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
    # You can lower these values if your images have low contrast           
    ORBextractor.iniThFAST: 20
    ORBextractor.minThFAST: 7
    
    #--------------------------------------------------------------------------------------------
    # Viewer Parameters
    #--------------------------------------------------------------------------------------------
    Viewer.KeyFrameSize: 0.05
    Viewer.KeyFrameLineWidth: 1
    Viewer.GraphLineWidth: 0.9
    Viewer.PointSize:2
    Viewer.CameraSize: 0.08
    Viewer.CameraLineWidth: 3
    Viewer.ViewpointX: 0
    Viewer.ViewpointY: -0.7
    Viewer.ViewpointZ: -1.8
    Viewer.ViewpointF: 500

## RGBD ##

深度相机建图和导航

由于之前的单目相机Mono节点默认接收的RGB图像节点名称为 `/camera/image_raw` ，而深度相机RGB-D节点默认接收的RGB图像节点和深度Depth节点为 `/camera/rgb/image_raw` 和 `/camera/depth_registered/image_raw`，此时如果我们还是用之前的相机，就需要修改之前相机的RGB图像发布话题名称。

故，推荐将文件 `./Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc` 的以下部分内容：

ros::NodeHandle nh;
    
    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth_registered/image_raw", 1);

修改为：这样可以自己传入订阅的参数

//加上"~"号后可以传入参数
    ros::NodeHandle nh("~");
    
    std::string rgb_topic = nh.param<std::string>("rgb", "/camera/rgb/image_raw");
    std::string depth_topic = nh.param<std::string>("depth", "/camera/depth_registered/image_raw");
    
    cout << "rgb: " << rgb_topic << endl;
    cout << "depth: " << depth_topic << endl;
    
    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, rgb_topic, 1);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, depth_topic, 1);

然后在项目跟路径执行以下命令，重新编译ros环境：

./build_ros.sh

这样，我们就可以指定任意的rgb话题和depth话题了（当然，如果没有指定则会使用默认值）。

### 从相机发布数据 ###

**启动Astra相机**

执行以下命令启动相机

roslaunch astra_camera astrapro_su.launch

启动后，可以通过执行 `rosrun rqt_image_view rqt_image_view` 确保以下两种数据是否可见

1、RGB彩色数据：`/camera/rgb/image_raw`

2、Depth深度数据：`/camera/depth_registered/image_raw`

**启动RGBD节点**

此时，我们可以使用以下方式启动RGBD节点：

roslaunch astra_camera astrapro_su.launch

cd /home/iimt/3rdparty/ORB_SLAM2 右键打开控制台
    rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/astra.yaml _rgb:=/camera/rgb/image_raw _depth:=/camera/depth_registered/image_raw

或

roslaunch astra_camera astrapro_su_rgb.launch

cd /home/iimt/3rdparty/ORB_SLAM2 右键打开控制台
    rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/astra.yaml _rgb:=/camera/image_raw _depth:=/camera/depth_registered/image_raw

### 从bag中发布数据 ###

我们可以直接在该网站 [https://vision.in.tum.de/data/datasets/rgbd-dataset/download][https_vision.in.tum.de_data_datasets_rgbd-dataset_download] 中下载到bag数据。

我们以 [rgbd\_dataset\_freiburg1\_xyz.bag][rgbd_dataset_freiburg1_xyz.bag] 为例，下载后，可以使用 `rosbag info xxx.bag` 命令查看到彩色数据和深度数据：

ty@ty-PC:~/Workspace/SLAM/Res$ rosbag info rgbd_dataset_freiburg1_xyz.bag 
    path:         rgbd_dataset_freiburg1_xyz.bag
    version:      2.0
    duration:     30.4s
    start:        May 10 2011 20:38:18.38 (1305031098.38)
    end:          May 10 2011 20:38:48.81 (1305031128.81)
    size:         480.4 MB
    messages:     25626
    compression:  bz2 [1598/1598 chunks; 29.14%]
    uncompressed:   1.6 GB @ 54.1 MB/s
    compressed:   479.4 MB @ 15.8 MB/s (29.14%)
    types:        sensor_msgs/CameraInfo         [c9a58c1b0b154e0e6da7578cb991d214]
                  sensor_msgs/Image              [060021388200f6f0f447d0fcd9c64743]
                  sensor_msgs/Imu                [6a62c6daae103f4ff57a132d6f95cec2]
                  tf/tfMessage                   [94810edda583a504dfda3829e70d7eec]
                  visualization_msgs/MarkerArray [f10fe193d6fac1bf68fad5d31da421a7]
    topics:       /camera/depth/camera_info     798 msgs    : sensor_msgs/CameraInfo        
                  /camera/depth/image           798 msgs    : sensor_msgs/Image             
                  /camera/rgb/camera_info       798 msgs    : sensor_msgs/CameraInfo        
                  /camera/rgb/image_color       798 msgs    : sensor_msgs/Image             
                  /cortex_marker_array         3034 msgs    : visualization_msgs/MarkerArray
                  /imu                        15158 msgs    : sensor_msgs/Imu               
                  /tf                          4242 msgs    : tf/tfMessage

代码段 小部件可以看到彩色和深度分别对应的话题为：`/camera/rgb/image_color` 和 `/camera/depth/image`

**启动bag数据**

//需要进入数据所在目录
    cd /home/iimt/3rdparty/ORB_SLAM2/data
    rosbag play --pause -l rgbd_dataset_freiburg1_xyz.bag

这里启动之后，需要按下空格才会真正发布数据。`-l` 参数表示循环播放该数据包，可以去掉。按s会进一帧

\--pause：这个不指定的话，一开始就会执行发布数据; 指定这个参数后启动 按空格后执行读取后播放数据

\-l ：loop的缩写：论循重新播放不结束，不加-l会播放结束

**启动RGBD节点**

此时，我们可以使用以下方式启动RGBD节点，但是我们要注意先准备一个新的相机参数yaml文件

cd /home/iimt/3rdparty/ORB_SLAM2 右键打开控制台
    rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1_rosbag.yaml _rgb:=/camera/rgb/image_color _depth:=/camera/depth/image

注意这里我们修改了**相机配置文件**，**彩色图话题**，**深度图话题**。

如果深度的数据不动说明深度的配置有问题：更改深度值的缩放比例

这里的相机配置文件有所修改，我们要将 `Examples/RGB-D/TUM1.yaml` 复制一份为 `Examples/RGB-D/TUM1_rosbag.yaml` 将其中的一行DepthMapFactor改为如下值：

# Deptmap values factor 
    DepthMapFactor: 1.0

效果如下：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 2][]

# rosbag使用 #

## **osbag简介 //需要进入数据所在目录** ##

rosbag 既可以指命令行中数据包相关命令，也可以指 c++/python 的 rosbag 库。

rosbag 主要用于**记录、回放、分析 rostopic 中的数据**。它可以将指定 rostopic 中的数据记录到 .bag 后缀的数据包中，便于对其中的数据进行离线分析和处理。

对于订阅（ subscribe） 某个 topic 的节点来说，它无法区分这个 topic 中的数据到底是实时获取的数据还是从 rosbag 中回放的数据。这就有助于我们基于离线数据快速重现曾经的实际场景，进行可重复、低成本的分析和调试。

### 记录数据 ###

//需要进入存放数据的目录
    cd ~/bagfiles
    //记录所有topic的数据，建议不要一次性录制所有数据，深度数据可能会录制失败，数据量也非常大11s达到250m
    rosbag record -a

`-a` 选项表示将当前发布的所有 topic 数据都录制保存到一个 rosbag 文件中，录制的数据包名字为日期加时间。也可以只记录某些感兴趣的 topic

rosbag record /topic_name1 /topic_name2 /topic_name3

示例：录制参色图与深度图及内参

//需要进入存放数据的目录
    cd /home/iimt/3rdparty/ORB_SLAM2/data
    rosbag record /camera/rgb/image_raw /camera/depth_registered/image_raw /camera/rgb/camera_info /camera/depth/camera_info

ctrl+c 停止后会自动保存

如果要指定生成数据包的名字，则用-O /-o 参数，如下：

//需要进入存放数据的目录
    cd ~/bagfiles
    rosbag record -O filename.bag /topic_name1

如果在 launch 文件中使用 rosbag record 命令，如下：

### 查看数据包信息 ###

rosbag info指令可以显示数据包中的信息:

rosbag info filename.bag

以yaml格式查看

rosbag info -y filename.bag

### 回放数据 ###

回放数据包中的 topic

rosbag play <bagfile>

如果想改变消息的发布速率，可以用下面的命令，-r 后面的数字对应播放速率。

rosbag play -r 2 <bagfile>

如果希望 rosbag 循环播放，并且开始的时候暂停，可以用命令，-r播放速度会更快

rosbag play -l --pause <bagfile>  # -l== --loop

示例：

//需要进入数据所在目录
    cd /home/iimt/3rdparty/ORB_SLAM2/data
    //加载读取rosbag数据
    rosbag play --pause -l rgbd_dataset_freiburg1_xyz.bag
    
    
     //查看和使用视频数据
     rosrun rqt_image_view rqt_image_view
    cd /home/iimt/3rdparty/ORB_SLAM2 右键打开控制台
    rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1_rosbag.yaml _rgb:=/camera/rgb/image_color _depth:=/camera/depth/image

如果只播放感兴趣的 topic ，则用命令

rosbag play <bagfile> --topic /topic1

在上述播放命令执行期间，空格键可以暂停播放。

# OctoMap-基于八叉树的地图 #

八叉树是用于在3D视觉中细分空间的数据结构。每个立方体都可以逐级地细分为8个子立方体，直到达到了给定的最小体积（体素）尺寸。且该最小体积（体素）决定了八叉树的分辨率。

An Efficient Probabilistic 3D Mapping Framework Based on Octrees

**octomap的作用：**

*  通过点云生成占用空间小、内容紧凑的地图，**节省空间**
 *  合并了融合后的点云出现的重叠细节，**提高运算效率**
 *  方便用于**进行路径规划、导航、碰撞检测**

![d077e8166f2e5ec562d6d5297bc940fb.png][]

一般场景比较大时，采用较低的分辨率，意味着方块更少，且更大。场景较小时，采用高分辨率，提高精度

![e27be68bc9d57b11e47bbc730a361783.png][]

以上是一个八叉树的存储示例，左侧为体积模型，右侧为树状表示结构

![octomap\_demo.png][octomap_demo.png]

![octomap\_demo2.png][octomap_demo2.png]

## **使用方法** ##

源码地址：[https://github.com/OctoMap/octomap][https_github.com_OctoMap_octomap] 这里我们直接使用ros版的OctoMap，然后接收点云话题，将之转成octomap并显示。

### ROS**安装octomap** ###

这里 `$ROS_DISTRO` 代表你电脑上的ros版本，可以通过 `echo $ROS_DISTRO` 查看具体代表的版本，如果命令没有输出内容，可以根据不同的ubuntu系统，替换成字符串即可：

*  Ubuntu14.04 -> indigo
 *  Ubuntu16.04 -> kinetic
 *  Ubuntu18.04 -> melodic

sudo apt-get install ros-$ROS_DISTRO-octomap-ros 
    sudo apt-get install ros-$ROS_DISTRO-octomap-msgs
    sudo apt-get install ros-$ROS_DISTRO-octomap-server

给rviz安装插件用于显示octomap

sudo apt-get install ros-$ROS_DISTRO-octomap-rviz-plugins

### **下载并编译ROS节点** ###

cd catkin_ws/src
    git clone https://gitee.com/tangyang/pointcloud_publisher.git
    cd ..
    catkin_make

**pointcloud\_publisher**代码的作用：

1.  将点云文件内容数据发布到话题 `/pointcloud/output`
2.  使用octomap\_server\_node接收数据并生成Octomap
3.  使用rviz查看点云及对应的Octomap

### **启动节点并预览** ###

roslaunch pointcloud_publisher demo.launch

### **使用其他点云** ###

我们可以将其他.pcd文件拷贝到 `pointcloud_publisher/data` 目录下，然后修改 `launch/demo.launch` 中的path参数即可：

## **pointcloud\_mapping** ##

此章节我们学习实时将rosbag或是相机发布的rgb彩色图和depth深度图合成**点云图**，并根据相机的位姿信息对点云进行融合。

生成点云后，将点云数据发布到 `/pointcloud_mapping/Local/PointCloudOutput` 话题，通过 `octomap_server_node` 节点生成对应的**八叉树拓扑图**。

具体流程如下：

1、产生新的关键帧

2、构建关键帧对应的点云图

3、将新的点云图和旧的进行叠加

4、更新八叉树拓扑图

5、更新相机位姿

## **IO介绍** ##

**配置信息**

1.  相机内参
2.  配置信息

**输入数据：**

1.  彩色图 ：/RGBD/RGB/Image
2.  深度图：/RGBD/Depth/Image
3.  相机位姿：/RGBD/CameraPose

**输出数据：**

1.  变换后的点云
2.  点云转换得到的八叉树图

## **使用方式** ##

### **使用rosbag作为输入源** ###

**1、查看rosbag发布的数据** \`

该数据集可以在这个链接下载：

[https://vision.in.tum.de/rgbd/dataset/freiburg1/rgbd\_dataset\_freiburg1\_room.bag][https_vision.in.tum.de_rgbd_dataset_freiburg1_rgbd_dataset_freiburg1_room.bag]

使用命令：`rosbag info rgbd_dataset_freiburg1_room.bag`

ty@ty-PC:~/Lesson/SLAM/orbslam_01/Resource/data$ rosbag info rgbd_dataset_freiburg1_room.bag 
    path:         rgbd_dataset_freiburg1_room.bag
    version:      2.0
    duration:     49.3s
    start:        May 10 2011 20:51:46.96 (1305031906.96)
    end:          May 10 2011 20:52:36.24 (1305031956.24)
    size:         845.5 MB
    messages:     41803
    compression:  bz2 [2724/2724 chunks; 30.08%]
    uncompressed:   2.7 GB @ 56.9 MB/s
    compressed:   843.8 MB @ 17.1 MB/s (30.08%)
    types:        sensor_msgs/CameraInfo         [c9a58c1b0b154e0e6da7578cb991d214]
                  sensor_msgs/Image              [060021388200f6f0f447d0fcd9c64743]
                  sensor_msgs/Imu                [6a62c6daae103f4ff57a132d6f95cec2]
                  tf/tfMessage                   [94810edda583a504dfda3829e70d7eec]
                  visualization_msgs/MarkerArray [f10fe193d6fac1bf68fad5d31da421a7]
    topics:       /camera/depth/camera_info    1361 msgs    : sensor_msgs/CameraInfo        
                  /camera/depth/image          1360 msgs    : sensor_msgs/Image             
                  /camera/rgb/camera_info      1362 msgs    : sensor_msgs/CameraInfo        
                  /camera/rgb/image_color      1362 msgs    : sensor_msgs/Image             
                  /cortex_marker_array         4914 msgs    : visualization_msgs/MarkerArray
                  /imu                        24569 msgs    : sensor_msgs/Imu               
                  /tf                          6875 msgs    : tf/tfMessage

可见其中rgb图像话题对应

/camera/rgb/image_color      1362 msgs    : sensor_msgs/Image

depth深度图像话题对应

cd /home/iimt/3rdparty/ORB_SLAM2/data
    rosbag play -l --pause rgbd_dataset_freiburg1_room.bag

！注意：这里启动后数据暂时不会发布，等下边的接收及处理节点启动起来后，再到这个控制台**按下空格**，才真正发布数据！

**3、启动数据处理节点**

这里要使用添加新功能后的ORB\_SLAM2版本：[https://gitee.com/tangyang/ORB\_SLAM2][https_gitee.com_tangyang_ORB_SLAM2]

新加功能：

ros\_rgbd\_pose.cc与ros\_rgbd.cc对比 追踪rgbd进行彩色和深度图的地图的构建和定位修改后同时支持输出（4×4的位姿）相机到世界作表系或世界坐标系到相机的表达

指定输入的rgb话题和depth话题（刚刚通过rosbag info xxx.bag得到的）

cd /home/iimt/3rdparty/ORB_SLAM2
    rosrun ORB_SLAM2 RGBD_pose Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/TUM1_rosbag.yaml _rgb:=/camera/rgb/image_color _depth:=/camera/depth/image

过一小会儿，会有两个新的窗口打开，暂时没有数据内容，因为我们还未真正发布数据。

**4、启动点云及Octomap渲染节点**

由于我们启动的rosbag的名称`rgbd_dataset_freiburg1_room.bag`中包含数字1，说明对应的相机设备为tum1，故使用tum1.launch（其中不同的只是相机内参）

这里要先确认你的octomap环境已经配置好，参考链接：

[https://gitee.com/tangyang/pointcloud\_publisher][https_gitee.com_tangyang_pointcloud_publisher]

\# 下载代码

cd src 
    git clone https://gitee.com/tangyang/pointcloud_mapping.git
     cd .. 
    catkin_make -j4

roslaunch pointcloud_mapping tum1.launch

此时会有一个rviz窗口和一个点云的viewer窗口启动

**5、最后，回到rogbag控制台，按下空格！**

刚刚开启的那些窗口，应该开始进行实时建图与定位，且都会显示出数据了。

orb\_slam2：

![orbslam2.png][]

点云与Octomap：

![octomap.png][]

### **使用相机作为输入源** ###

**1、插上相机设备**

很多时候，忘了把相机连接电脑，也是出问题的原因。

**2、启动相机节点**

这里使用的是奥比中光的Astra相机（乐视体感相机），代码：[https://gitee.com/tangyang/ros\_astra\_camera][https_gitee.com_tangyang_ros_astra_camera]。

# roslaunch astra_camera astrapro.launch
    
    roslaunch astra_camera astrapro_su.launch

**3、启动数据处理节点**

这里同样要使用添加新功能后的ORB\_SLAM2版本：[https://gitee.com/tangyang/ORB\_SLAM2][https_gitee.com_tangyang_ORB_SLAM2]

指定输入的rgb话题和depth话题（可以通过rqt\_image\_view查看确认）

cd /home/iimt/3rdparty/ORB_SLAM2
    rosrun ORB_SLAM2 RGBD_pose Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/astra.yaml _rgb:=/camera/rgb/image_raw _depth:=/camera/depth_registered/image_raw

过一小会儿，会有两个新的窗口打开。

这里的`_rgb`和`_depth`参数其实也可以不写，因为代码中这两个的默认值就和我们指定的是一样的。

**4、启动点云及Octomap渲染节点**

这里要先确认你的octomap环境已经配置好，参考链接：[https://gitee.com/tangyang/pointcloud\_publisher][https_gitee.com_tangyang_pointcloud_publisher]

cd /home/iimt/workspace/workspace_ros_guidance
    catkin_make
    source devel/setup.sh
    roslaunch pointcloud_mapping astra.launch

如果没有数据就移动摄像头，因为没有数据更新

此时会有一个rviz窗口和一个点云的viewer窗口启动

ctrl+c 退出后保存不了数据，需要解决  
保存数据的路径配置：  
 <param name="node\_path" type="string" value="$(find pointcloud\_mapping)"/>

！注意：

> 如果点云没有更新或rviz界面没有变化，但是slam页面又是正常有数据的，那说明关键帧还没有生成，拿起摄像头转一些角度动一动就可以了。

### **查看融合后的点云** ###

在使用点云渲染节点 pointcloud\_mapping 后，会在其源码目录输出.pcd文件，我们可以使用pcl\_viewer进行查看

pcl_viewer src/pointcloud_mapping/resultPointCloudFile.pcd

## **其他** ##

可以在执行cmake和make编译命令之前，在根目录的 `CMakeLists.txt` 文件中（12行）添加以下两行配置，来屏蔽大量的代码过时警告（对编译结果没有影响），方便编译出错时候排查问题。

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations")
    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated")

[https_shrill-pond-3e81.hunsh.workers.dev]: https://shrill-pond-3e81.hunsh.workers.dev/
[http_gitd.cc]: http://gitd.cc/
[https_github.com_orbbec_ros_astra_camera]: https://github.com/orbbec/ros_astra_camera
[ROS]: http://wiki.ros.org/ROS/Installation
[Link 1]: https://shrill-pond-3e81.hunsh.workers.dev/https://github.com/orbbec/ros_astra_camera/archive/master.zip
[Untitled.png]: /images/20221020/aa86fa8e597b42dcbe242331336da244.png
[Astra_Untitled_201.png]: /images/20221020/c7574aa64a1146289bd83aba70dc9941.png
[camera.yaml]: http://robot.czxy.com/car/orbslam/assets/camera.yaml
[depth.yaml]: http://robot.czxy.com/car/orbslam/assets/depth.yaml
[astra_rviz]: /images/20221020/8edda5f9bff64626aacb45e6e215267c.png
[Link 2]: http://wiki.ros.org/pcl_ros#pointcloud_to_pcd
[pcl_ros]: /images/20221020/f7bde805cd0d46f4becdd610fb6b3ab4.png
[OpenCV]: https://github.com/opencv/opencv
[https_gitee.com_mirrors_opencv_repository_archive_3.4.9]: https://gitee.com/mirrors/opencv/repository/archive/3.4.9
[https_gitee.com_mirrors_opencv_contrib_repository_archive_3.4.9]: https://gitee.com/mirrors/opencv_contrib/repository/archive/3.4.9
[Link 3]: http://robot.czxy.com/docs/camera/env/01-opencv/
[orbslam_camera]: /images/20221020/a1cb06479d704b7abf021a2df6cd81a3.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70]: /images/20221020/a0f86002ce664cf38c4254778f821f4f.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 1]: /images/20221020/69ab32c1d4714181852130488fadc25c.png
[https_vision.in.tum.de_data_datasets_rgbd-dataset_download]: https://vision.in.tum.de/data/datasets/rgbd-dataset/download
[rgbd_dataset_freiburg1_xyz.bag]: https://vision.in.tum.de/rgbd/dataset/freiburg1/rgbd_dataset_freiburg1_xyz.bag
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 2]: /images/20221020/f4341bd6352f494b863c3ef784882118.png
[d077e8166f2e5ec562d6d5297bc940fb.png]: /images/20221020/3fa0bc38fc914d7ab817eb023b0e0423.png
[e27be68bc9d57b11e47bbc730a361783.png]: /images/20221020/f5995bcdec894512a4a70c02e3149e59.png
[octomap_demo.png]: /images/20221020/46c6ca0f9762416694d50f64cd34c77f.png
[octomap_demo2.png]: /images/20221020/c4d8c8b84e83486e813885d54a8f64b4.png
[https_github.com_OctoMap_octomap]: https://github.com/OctoMap/octomap
[https_vision.in.tum.de_rgbd_dataset_freiburg1_rgbd_dataset_freiburg1_room.bag]: https://vision.in.tum.de/rgbd/dataset/freiburg1/rgbd_dataset_freiburg1_room.bag
[https_gitee.com_tangyang_ORB_SLAM2]: https://gitee.com/tangyang/ORB_SLAM2
[https_gitee.com_tangyang_pointcloud_publisher]: https://gitee.com/tangyang/pointcloud_publisher
[orbslam2.png]: /images/20221020/874c316a8edd43628a7c051ce4816d68.png
[octomap.png]: /images/20221020/0d72cee1bf5645c7a84418633253d2e9.png
[https_gitee.com_tangyang_ros_astra_camera]: https://gitee.com/tangyang/ros_astra_camera

orbslam-car,rosbag

发表评论取消回复

还没有评论，来说两句吧...

相关阅读