RealSense D435i数据录制 VINS离线运行

Posted 2022-12-05 猛龙过江ing

    花间一壶酒，独酌无相亲。举杯邀明月，对影成三人。月既不解饮，影徒随我身。暂伴月将影，行乐须及春。我歌月徘徊，我舞影零乱。醒时相交欢，醉后各分散。永结无情游，相期邈云汉。 ——（唐）李白《月下独酌四首 其一》

    最近科研做的很不顺利，各种情况层出不穷。先是电脑固态硬盘坏了，换了一块硬盘，重装了Windows系统；接着手机连接相机录制数据一直不成功，手机死活没反应；再有电脑连接相机录制数据运行程序也不行，发布的topic一直对不上；再按照网上教程尝试若干次都不行之后，系统留下了大量残留删不掉；干脆重装Ubuntu，结果捅了一个更大的蚂蜂窝；三个系统安装盘，要么就是终端连不上网，要么就是整个连不上网，还有一个更狠——直接黑屏无法安装；在修改DNS解决联网问题之后，接着照着网上的教程折腾，还是各种错误；之后发现我用的库的版本和教程的不一样，果断更换版本，只不过是重头再来；这一回顺畅了许多，但是网上的教程只能跑在线的，并没有离线的教程；马上百度、谷歌用起来，找到了ROS有一个录制的功能，最后终于实现了，也算有了那么一点点进展。这段时间用蒙曼老师的话说就是：“人生嘛，就是这样，旧的问题解决了，新的问题又会出现”，用李云龙的话说就是：“人要是倒霉，放屁都砸脚后跟”。下面掌声有请蒙曼老师和歇后语小王子李云龙出场：

     好了，说了这么多废话，言归正传，用这篇博客记录一下实现的过程，下次要是再出问题了，按图索骥就可以了。

    零、安装环境

    系统：Ubuntu 16.04 LTS
    ROS： Kinetic
    传感器：Intel Realsense D435i

    一、准备工作

    Realsense驱动，本文选择2.17.1版本。下载地址附上：https://github.com/IntelRealSense/librealsense/releases/tag/v2.17.1

    Realsense ROS Wrapper，版本号需要和驱动对应，本文选择2.1.3版本。下载地址附上：https://github.com/IntelRealSense/realsense-ros/releases/tag/2.1.3

    二、安装Realsense SDK

    1、下载source

    下载地址见上文。

    2、安装依赖项

sudo apt-get install libudev-dev pkg-config libgtk-3-dev
sudo apt-get install libusb-1.0-0-dev pkg-config
sudo apt-get install libglfw3-dev
sudo apt-get install libssl-dev

    3、安装位于librealsense源目录中的Intel Realsense权限脚本

sudo cp config/99-realsense-libusb.rules /etc/udev/rules.d/
sudo udevadm control --reload-rules && udevadm trigger

    4、构建和应用补丁内核模块:

    这一步需要把相机拔下来（其实一直就没有插上）。根据不同的ubuntu版本安装：对于Ubuntu 14/16/18 LTS：

lsmod | grep videobuf2_core
sudo modprobe -r uvcvideo
lsmod | grep videobuf2_core
./scripts/patch-realsense-ubuntu-lts.sh
sudo dmesg | tail -n 50

    第一行的命令很慢，很耗时。

    5、对SDK进行编译

    进入librealsense目录，之后：

mkdir build
cd build
cmake ../ -DBUILD_EXAMPLES=true
make
sudo make install

    -DBUILD_EXAMPLES=true 这个选项表示带演示和教程的。

    6、检验安装是否成功

    进入/librealsense/build/examples/capture，试一下效果。

./rs-capture 

    三、安装ROS Wrapper

    1、建立workspace，已经有的可以跳过

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src/
catkin_init_workspace 
cd ..
catkin_make
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

    2、在catkin_ws/src/下载源程序，下载地址见上文，并catkin_make

    3、检验

    启动相机节点：

roslaunch realsense2_camera rs_rgbd.launch

    查看发布的topic：

rostopic list

    结果如下图所示：

    此时还可以直接获取相机的内参。当然最好还是自己标定了。

rostopic echo /camera/color/camera_info 
rostopic echo /camera/aligned_depth_to_color/camera_info

    四、在线运行VINS

    1、修改相机相关参数

    由于相机发布的topic将IMU的加速度计和陀螺仪的数据分开了，所以需要进行改动。另外，还需要进行时间戳的同步。

    修改～/catkin_ws/src/realsense-ros-2.1.3/realsense2_camera/launch中的rs_camera.launch文件的两个属性，具体如下：

<arg name="enable_sync"           default="true"/>
<arg name="unite_imu_method"      default="copy"/>

    修改后最好另存为一个其他文件。重新运行realsense2_camera，获取topic列表，就可以看到topic：“/camera/imu”。

roslaunch realsense2_camera rs_camera.launch（或者自己另存为的文件名） 
rostopic list
rostopic echo /camera/imu

    2、修改VINS相关参数

    在realsense_color_config.yaml基础上修改订阅的topic：

#common parameters
imu_topic: "/camera/imu"
image_topic: "/camera/color/image_raw"

    修改相机内参，IMU到相机的外参标定选择2，让程序自己在线标定吧：

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0   # 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.
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.                        
#If you choose 0 or 1, you should write down the following matrix.

    IMU参数，这里全都使用默认值：

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.2          # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.05         # gyroscope measurement noise standard deviation.     #0.05
acc_w: 0.02         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 4.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.81       # gravity magnitude

    realsense d435i说是已经做好了硬件同步所以不需要在线估计同步时差（但是用kalibr标定出来和在线估计出来都存在大概-0.06的时间差）：

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

    相机曝光方式应为全局曝光：

#rolling shutter parameters
rolling_shutter: 0                      # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0               # unit: s. rolling shutter read out time per frame (from data sheet). 

    然后就可以运行了：

roslaunch realsense2_camera rs_camera.launch 
roslaunch vins_estimator realsense_color.launch 
roslaunch vins_estimator vins_rviz.launch

    来一张效果图：

    图之所以这样主要是因为我移动的很随意，有遮挡、剧烈运动等，并不是出现了bug。

    五、离线运行VINS

    虽然现在可以在线运行了，但是这并不是我想要的最终结果。之后尝试了使用相机SDK自带的数据录制功能，发现加速度计和陀螺仪的数据依然是分开的，没有因为上面对参数的修改而发生改变，并且在图形界面也没有发现可以进行修改的地方。幸运的是，通过查阅相关资料发现，ROS提供了录制特定topic的API，这下事情就好办了。

    一方面连接相机、发布数据：

roslaunch realsense2_camera rs_camera.launch 

    另一方面录制想要的topic，正好还可以将不需要的数据剔除：

rosbag record /camera/imu /camera/color/image_raw

    录制完成时，直接Ctrl+C终断即可，录制的bag文件会以时间命名，保存在home目录。

    六、展示结果

    和启动VINS运行数据集的数据类似，启动ROS、RVIZ运行录制的数据：

source ~/catkin_ws/devel/setup.bash
roslaunch vins_estimator realsense_color.launch 

source ~/catkin_ws/devel/setup.bash
roslaunch vins_estimator vins_rviz.launch

source ~/catkin_ws/devel/setup.bash
rosbag play ~/catkin_ws/Dataset/1.bag

    本人喜欢设置好环境变量，并且把vins_rviz.launch和相关文件合并，这样就两步就可以了：

cd catkin_ws && catkin_make && roslaunch vins_estimator realsense_color.launch

rosbag play ~/1.bag

    最后，运行结果奉上：

    掌握了这个就可以愉快地采集数据进行实验了，别看VINS面对公开数据集威风凛凛，在实际应用中能不能行还真不一定。希望未来越来越顺利，能不断地有新进展。

参考：

1、如何用Realsense D435i运行VINS-Mono等VIO算法 获取IMU同步数据

2、Realsense D435i 在ubuntu上安装SDK与ROS Wrapper 运行ORB-SLAM2、RTAB和VINS-Mono

3、rosbag录制和回放消息实用总结