【在实习期的快乐生活】D435i&Rtabmap&Vins

最新推荐文章于 2024-05-14 20:37:32 发布

原味小辣鸡

最新推荐文章于 2024-05-14 20:37:32 发布

阅读量3.1k

点赞数 7

分类专栏：笔记文章标签： slam intel

本文链接：https://blog.csdn.net/qq_15536485/article/details/121783692

版权

笔记专栏收录该内容

2 篇文章 0 订阅

订阅专栏

D435i&Rtabmap&Vins

高刷新率：彩色图像和深度信息都可以在848 × 480分辨率下以90fps的高刷新率进行传输。
高精度：近距离误差范围1厘米左右,3米处误差范围3厘米。
出厂自带相机标定信息：可在rostopic下的/camera/(哪个摄像头)/camera_info获取（自己重新标定了一下，感觉出厂的标定数据挺准的）

d435i驱动安装

// An highlighted block
#################
# 安装d435i驱动 #
#################
# 安装依赖
sudo apt-get install ros-melodic-realsense2-camera

sudo apt-get update && sudo apt-get upgrade && sudo apt-get dist-upgrade

sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE

sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" -u || sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u

sudo apt-get install librealsense2-dkms
sudo apt-get install librealsense2-utils 
sudo apt-get install librealsense2-dev
# 出错不用管

mkdir -p ~/catkin_d435i/src
cd ~/catkin_d435i/src/
git clone https://github.com/IntelRealSense/realsense-ros.git
cd ..
catkin_make

# 测试d435i
realsense-viewer

D435i launch文件重要参数解析

文件为rs_rgbd.launch

单个摄像头开启参数1和2分别对应左右，也可以自己在rviz里分别挡住，一个一个试试
  <arg name="infra_width"         default="640"/>
  <arg name="infra_height"        default="480"/>
  <arg name="enable_infra1"       default="false"/>
  <arg name="enable_infra2"       default="false"/>
深度和图像都可以在848 × 480分辨率下以90fps运行 
  <arg name="depth_fps"           default="-1"/>
<arg name="infra_fps"           default="-1"/>
  <arg name="color_fps"           default="-1"/>
这个是imu的陀螺仪和加速度，可以上到200fps
  <arg name="gyro_fps"            default="-1"/>
  <arg name="accel_fps"           default="-1"/>
  <arg name="enable_gyro"         default="false"/>
  <arg name="enable_accel"        default="false"/>
开启相机和IMU的同步
  <arg name="enable_sync"         default="true"/>
开启深度和彩色的同步
  <arg name="align_depth"         default="true"/>
发布tf坐标，用于rviz的可视化，rate=30就够了
  <arg name="publish_tf"          default="true"/>
  <arg name="tf_publish_rate"     default="0"/> <!-- 0 - static transform -->

RTAB-MAP

RTAB-MAP？

和orb-slam一样，都是vslam中必须先学的经典算法。
orb和vins使用的是稀疏点云，rtab是稠密点云，建图后的视觉效果会更好点。
已经集成到ros的deb包里了，安装只使用到sudo apt-get install，操作特别简单。

RTAB-MAP原理

（1）被提出的目的：
提供一个与时间和尺度无关的基于外观的定位与构图解决方案，针对大型环境中的在线闭环检测问题。
（2）思想：
为满足实时性的一些限制，闭环检测是仅仅利用有限数量的一些定位点，同时在需要的时候又能够访问到整个地图的定位点。当地图中定位点的数目使得找到定位匹配的时间超过某个阀值时，RTAB-MAP就将WM(Working Memory)中不太可能形成闭环的定位点转移到LTM（Long-Term Memory）中，这样这些被转移的位置点就不参与下次闭环检测的运算。当一个闭环被检测到时，其领接定位点又能够重新的从LTM中取回放入WM中，用于将来的闭环检测。
由于LTM中的定位点并不参与闭环检测，因此选择WM中的哪些定点转移到LTM中非常重要。假设更频繁的被访问的定位点比其他的定位点更易于形成闭环。这样一个定位点被连续访问的次数就可以用来衡量其易于形成闭环的权重。当需要从WM转移定位点到LTM中时，优先选择具有最低权重的定位点。如果具有最低权重的定位点又有多个时，优先选择被存储时间最长的那一个。
在这里插入图片描述

以上为RTAB-Map ROS节点的框图。
（3）算法输入：
TF：用于定义传感器相对于机器人底座的位置；来自任何来源的里程计(可以是3DoF或6DoF)；
相机输入：(一个或多个RGB-D图像，或双目立体图像)，且带有相应的校准消息。
可选输入∶2D激光的雷达扫描，或3D激光的点云。
来自这些输入的所有消息被同步并传递给graph-SLAM算法。
（4）算法输出：
Map Data：包含最新添加的节点（带有压缩传感器数据)和Graph；
Map Graph：没有任何数据的纯图；
TF：矫正过的里程计；
可选的OctoMap (3D占用栅格地图)；
可选的稠密点云地图；
可选的2D占用栅格地图。

参考链接: https://blog.csdn.net/xmy306538517/article/details/78771104.

RTABMAP安装

##################
# 安装rtabmap_ros#
##################
# 使用deb快速安装
sudo apt-get install ros-melodic-rtabmap-ros

我们采用rgbd的方式建图，rtabmap是支持双目建图的，但是都有深度信息了干嘛还用双目在这里插入图片描述
，自我感觉的建图效果排序rgbd（彩色深度图像） > stereo（双目） > mono（单目）

# 新建终端，开启d435i rgbd
cd ~/catkin_d435i
source devel/setup.bash
roslaunch realsense2_camera rs_rgbd.launch tf_publish_rate:=30
命令解析:
发布tf坐标，为了在rviz可视化显示中，获取tf转换信息

# 新建终端，开启rtabmap
# 自带可视化界面
roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" rviz:=false rtabmapviz:=true rgb_topic:=/camera/color/image_raw depth_topic:=/camera/aligned_depth_to_color/image_raw camera_info_topic:=/camera/color/camera_info
命令解析:
参数---delete_db_on_start将使rtabmap在启动时删除数据库（默认位于~/.ros/rtabmap.db）。如果想让机器人继续从之前的映射会话中进行映射，应该删除 --delete_db_on_start。
rgb_topic 彩色图像topic
depth_topic 深度图像topic
camera_info_topic 相机内参外参

# rviz可视化界面，在这里需要自行在rviz中设定topic，讲map改为OctoMap
roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" rviz:=true rtabmapviz:=false rgb_topic:=/camera/color/image_raw depth_topic:=/camera/aligned_depth_to_color/image_raw camera_info_topic:=/camera/color/camera_info

在这里插入图片描述

#查看刚刚建立的地图特征点信息
rtabmap-databaseViewer ~/.ros/rtabmap.db

在这里插入图片描述

RTABMAP实测效果总结

在没有外部odom数据输入时，它依靠的是自身的视觉的里程计，如果在b点把摄像头绑架到a点，那它会认为自己还在b点，并把a点附近的特征点和之前b点的作比较。如果ab两点相近还好说，没准有匹配的特征点，相离较远的话，直接完蛋。
所以对于这个demo来说，效果不是很好，这个算是rtabmap的缺陷，不过在现实机器人中，是有实时的外部odom输入到rtabmap中的，效果会好很多。在realsense官方的realsense2_camera目录下，有个rtabmap的demo，里面同时用到了d435i和t265，d435i提供rgbd信息，t265提供位姿信息，侧面验证了rtabmap对外部odom的依赖。

RTABMAP launch文件重要参数解析

文件为rtabmap.launch，可以使用roscd rtabmap/launch以找到该文件的目录。

（1）Rgbd相关
<!—rgbd 和深度topic -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
<!—摄像头内外参topic -->
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
（2）双目相关
<!—左右摄像头topic -->
<arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
<arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />      <!—左右摄像头内外参topic -->
<arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
<arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />

Rtabmap内部运行关键参数

如果需要在自己的机器人上做进行进一步的改进，就得看这个了
参考链接:http://wiki.ros.org/rtabmap_ros/Tutorials/Advanced%20Parameter%20Tuning

VINS-Fusion

VINS-Fusion解析

（1）简介
港科大在2019年1月12号发布了Vins-fusion，从2017年发布的Vins-mono：单目+IMU即单目惯导，这次晋级了vins-fusion，相比其他的vslam，VINS算是比较新的了，在demo中主要给出了四个版本：
（1）单目+imu
（2）纯双目
（3）双目+imu
（4）双目+imu+GPS
我们使用的是d435i，正好可以使用方法（3）进行建图，有imu不用真是可惜了。

（2）VINS 为什么采用「视觉 + IMU」融合？
单一的传感器不能适用所有的场景，比如视觉传感器在大多数纹理丰富的场景中效果很好，但是如果遇到玻璃，白墙等特征较少的场景，基本上无法工作，而通过多个传感器的融合可以达到理想的定位效果。
（3）原理
有点难理解，俺看不太懂。。。在这里插入图片描述

VINS-Fusion安装

（1）安装依赖

# CMake
sudo apt-get install cmake
# google-glog + gflags
sudo apt-get install libgoogle-glog-dev libgflags-dev
# BLAS & LAPACK
sudo apt-get install libatlas-base-dev
# Eigen3
sudo apt-get install libeigen3-dev
# SuiteSparse and CXSparse (optional)
sudo apt-get install libsuitesparse-dev

（2）安装ceres-solver依赖

# 首先下载ceres-solver
http://ceres-solver.org/ceres-solver-2.0.0.tar.gz
# 解压
tar zxf ceres-solver-2.0.0.tar.gz
mkdir ceres-bin
cd ceres-bin
cmake ../ceres-solver-2.0.0
make -j3
# 测试
make test
# 安装
sudo make install

（3）安装VINS-Fusion

cd ~/catkin_d435i/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
cd ../
catkin_make
source ~/catkin_ d435i/devel/setup.bash

（4）配置launch文件
这次我们采用的是双目+IMU。
新建rs_camera_vins.launch，将它保存在~/catkin_d435i/src/realsense-ros/realsense2_camera/launch/目录下，参数说明在上文的 D435i launch文件重要参数解析，文件内容如下：

<launch>
  <arg name="serial_no"           default=""/>
  <arg name="usb_port_id"         default=""/>
  <arg name="device_type"         default=""/>
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="camera"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>
  <arg name="external_manager"    default="false"/>
  <arg name="manager"             default="realsense2_camera_manager"/>

  <arg name="fisheye_width"       default="640"/>
  <arg name="fisheye_height"      default="480"/>
  <arg name="enable_fisheye"      default="false"/>

  <arg name="depth_width"         default="640"/>
  <arg name="depth_height"        default="480"/>
  <arg name="enable_depth"        default="true"/>

  <arg name="infra_width"        default="640"/>
  <arg name="infra_height"       default="480"/>
  <arg name="enable_infra1"       default="true"/>
  <arg name="enable_infra2"       default="true"/>

  <arg name="color_width"         default="640"/>
  <arg name="color_height"        default="480"/>
  <arg name="enable_color"        default="true"/>

  <arg name="fisheye_fps"         default="30"/>
  <arg name="depth_fps"           default="30"/>
  <arg name="infra_fps"           default="30"/>
  <arg name="color_fps"           default="30"/>
  <arg name="gyro_fps"            default="200"/>
  <arg name="accel_fps"           default="250"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>

  <arg name="enable_pointcloud"         default="false"/>
  <arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
  <arg name="pointcloud_texture_index"  default="0"/>

  <arg name="enable_sync"               default="true"/>
  <arg name="align_depth"               default="true"/>

  <arg name="publish_tf"                default="true"/>
  <arg name="tf_publish_rate"           default="0"/>

  <arg name="filters"                   default=""/>
  <arg name="clip_distance"             default="-2"/>
  <arg name="linear_accel_cov"          default="0.01"/>
  <arg name="initial_reset"             default="false"/>
  <arg name="unite_imu_method"          default="linear_interpolation"/>
  <arg name="topic_odom_in"             default="odom_in"/>
  <arg name="calib_odom_file"           default=""/>
  <arg name="publish_odom_tf"           default="true"/>
  <arg name="allow_no_texture_points"   default="false"/>
  <arg name="emitter_enable"   		default="false"/>

<!-- rosparam set /camera/stereo_module/emitter_enabled false -->
<!-- 下面这段用于关闭激光发射器，防止干扰两个红外摄像头。 -->
<rosparam>
  /camera/stereo_module/emitter_enabled: 0
</rosparam>

<rosparam if="$(arg emitter_enable)">
  /camera/stereo_module/emitter_enabled: 1
</rosparam>

  <group ns="$(arg camera)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
      <arg name="external_manager"         value="$(arg external_manager)"/>
      <arg name="manager"                  value="$(arg manager)"/>
      <arg name="serial_no"                value="$(arg serial_no)"/>
      <arg name="usb_port_id"              value="$(arg usb_port_id)"/>
      <arg name="device_type"              value="$(arg device_type)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>

      <arg name="enable_pointcloud"        value="$(arg enable_pointcloud)"/>
      <arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
      <arg name="pointcloud_texture_index"  value="$(arg pointcloud_texture_index)"/>
      <arg name="enable_sync"              value="$(arg enable_sync)"/>
      <arg name="align_depth"              value="$(arg align_depth)"/>

      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
      <arg name="enable_fisheye"           value="$(arg enable_fisheye)"/>

      <arg name="depth_width"              value="$(arg depth_width)"/>
      <arg name="depth_height"             value="$(arg depth_height)"/>
      <arg name="enable_depth"             value="$(arg enable_depth)"/>

      <arg name="color_width"              value="$(arg color_width)"/>
      <arg name="color_height"             value="$(arg color_height)"/>
      <arg name="enable_color"             value="$(arg enable_color)"/>

      <arg name="infra_width"              value="$(arg infra_width)"/>
      <arg name="infra_height"             value="$(arg infra_height)"/>
      <arg name="enable_infra1"            value="$(arg enable_infra1)"/>
      <arg name="enable_infra2"            value="$(arg enable_infra2)"/>

      <arg name="fisheye_fps"              value="$(arg fis<launch>
	<include file="$(find realsense2_camera)/launch/rs_camera_vins.launch"/>	
	<include file="$(find vins)/launch/vins_rviz.launch"/>
	<!--node pkg="vins" type="vins_node" name="vins_node">
		<param name="config_file" value="~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml""/>
	</node-->	
</launch>heye_fps)"/>
      <arg name="depth_fps"                value="$(arg depth_fps)"/>
      <arg name="infra_fps"                value="$(arg infra_fps)"/>
      <arg name="color_fps"                value="$(arg color_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>

      <arg name="publish_tf"               value="$(arg publish_tf)"/>
      <arg name="tf_publish_rate"          value="$(arg tf_publish_rate)"/>

      <arg name="filters"                  value="$(arg filters)"/>
      <arg name="clip_distance"            value="$(arg clip_distance)"/>
      <arg name="linear_accel_cov"         value="$(arg linear_accel_cov)"/>
      <arg name="initial_reset"            value="$(arg initial_reset)"/>
      <arg name="unite_imu_method"         value="$(arg unite_imu_method)"/>
      <arg name="topic_odom_in"            value="$(arg topic_odom_in)"/>
      <arg name="calib_odom_file"          value="$(arg calib_odom_file)"/>
      <arg name="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
      <arg name="allow_no_texture_points"  value="$(arg allow_no_texture_points)"/>
    </include>
  </group>
</launch>

（5）配置yaml文件
文件位于src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml
内容如下：


%YAML:1.0

#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; 
imu: 1         
num_of_cam: 2  

imu_topic: "/camera/imu"
image0_topic: "/camera/infra1/image_rect_raw"
image1_topic: "/camera/infra2/image_rect_raw"
output_path: "/home/god/output/"

cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 480
   

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.

body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 1, 0, 0, -0.00552,
           0, 1, 0, 0.0051,
           0, 0, 1, 0.01174,
           0, 0, 0, 1 ]

body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 1, 0, 0, 0.0446571,
           0, 1, 0, 0.0051,
           0, 0, 1, 0.01174,
           0, 0, 0, 1 ]

#Multiple thread support
multiple_thread: 1

#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 30            # min distance between two features 
freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
F_threshold: 1.0        # ransac threshold (pixel)
show_track: 1           # publish tracking image as topic
flow_back: 1            # perform forward and backward optical flow to improve feature tracking accuracy

#optimization parameters
max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
max_num_iterations: 8   # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.1          # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 0.01         # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 0.001         # accelerometer bias random work noise standard deviation.  #0.002
gyr_w: 0.0001       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.805         # gravity magnitude

#unsynchronization parameters
estimate_td: 1                      # online estimate time offset between camera and imu
td: 0.00                             # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

#loop closure parameters
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/god/output/pose_graph/" # save and load path
save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0

（6）yaml文件中需要注意的参数

路径
output_path: "/home/(用户名)/output/" 
pose_graph_save_path: "/home/(用户名)/output/pose_graph/" # save and load path

程序是否参考imu与摄像头标定数据（即body_T_cam0，body_T_cam1），0是完全相信初始参数，1是在初始参数基础上，在运行过程中做出调整，2是完全不相信初始参数，由程序自我标定，在初始化的时候需要让摄像头先转转。
estimate_extrinsic: 1

camera 与IMU 的联合标定的参数，下文有标定教程
body_T_cam0
body_T_cam1
回环检测开关
save_image: 1    
    
以上就是几个重要的参数了，如果需要进一步了解，官方的英文注释写的也是挺全的。

在VINS中D435i需要标定的参数以及原理

以下两个参数分别对应imu对于左摄像头，imu对于右摄像头
body_T_cam0
body_T_cam1
说实话，d435i的品控还是可以的，我在使用自己相机内外参标定数据，和其他人标定的数据做了对比，误差都在10以下，基本可以认作是偶然误差。
所以，别人的d435i标定数据按理是可以直接拿来用的，可以保留我已经标定好的。
如果不放心可以按照下面这四个教程来标定，需要说明的是，教程是对RGBD进行标定，我们需要参照流程对两个摄像头分别标定：
D435i标定摄像头和IMU教程一（配置环境篇）
参考链接:https://blog.csdn.net/qq_35616298/article/details/116171823

D435i标定摄像头和IMU教程二（RGB摄像头标定篇）
参考链接:https://blog.csdn.net/qq_35616298/article/details/116174615

D435i标定摄像头和IMU教程三（IMU标定篇）
参考链接:https://blog.csdn.net/qq_35616298/article/details/116190164

D435i标定摄像头和IMU教程四（RGB摄像头和IMU联合标定篇）
参考链接:https://blog.csdn.net/qq_35616298/article/details/116197526

cam与imu标定原理的地址就放这里了，反正我这个调包程序员是看不懂了哈哈哈哈
参考链接:https://zhuanlan.zhihu.com/p/358232587

开始愉快的玩耍吧

这次我们采用的是双目+IMU数据
cd ~/catkin_d435i
roslaunch realsense2_camera rs_camera_vins.launch
roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_d435i/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml

效果图

在这里插入图片描述

原味小辣鸡

关注

7
点赞
踩
82

收藏

觉得还不错? 一键收藏
1
评论
【在实习期的快乐生活】D435i&Rtabmap&Vins

D435i&Rtabmap&Vins目录D435id435i驱动安装D435i launch文件重要参数解析RTAB-MAPRTAB-MAP？RTAB-MAP原理RTABMAP安装RTABMAP实测效果总结RTABMAP launch文件重要参数解析Rtabmap内部运行关键参数VINS-FusionVINS-Fusion解析VINS-Fusion安装在VINS中D435i需要标定的参数以及原理开始愉快的玩耍吧效果图目录你好！这是你第一次使用 Markdown编辑器所展示的欢迎页。
复制链接

扫一扫