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

前言

Intel Realsense D435i在D435的基础上硬件融合了IMU，然而目前网上关于这款摄像头的资料非常少，本文主要介绍自己拿着d435i历经曲折最后成功运行VINS-Mono的过程。。。

---

重要

最近官方更新了GitHub上的Realsense ROS Wrapper和librealsense，据反映在d435i的IMU数据获取上存在一些问题，如时间戳混乱、运行VINS的时候没有数据等。这些问题造成的现象包括但不限于：
1、roslaunch VINS后只能看到特征跟踪图像，没有world坐标系，终端也不报任何错误
2、终端一直提示imu message ins disorder!
由于其源码与本博客的差异较大，且在本博客的方法下这些问题并无法解决，我上传了自己当时版本的Realsense ROS Wrapper，供大家参考。（我的librealsense是2.17.1的，可能需要配套使用）
https://download.csdn.net/download/qq_41839222/11122224

如果一直报imu message ins disorder!且roslaunch /camera/imu后并看不出明显的时间戳混乱的，可以把源码上这行注释掉再试试效果~

---

准备工作

1、Intel Realsense D435i、Ubuntu 16.04
2、已经安装好Realsense驱动，如果没有的话可以参考：https://blog.csdn.net/qq_41839222/article/details/86503113
3、已经安装好VINS-Mono并且在数据集上正常工作
https://github.com/HKUST-Aerial-Robotics/VINS-Mono

一开始很神奇地发现VINS-Mono上居然有realsense的配置文件，以为事情非常简单，但运行以后发现并没有工作（因为不存在imu_topic: "/camera/imu/data_raw"啊）。

存在问题

运行realsense2_camera时，如

roslaunch realsense2_camera rs_camera.launch 

realsense d435i在ROS中发布的IMU数据分成了两个：
“/camera/gyro/sample” 发布角速度
“/camera/accel/sample” 发布线加速度

同时这两个的时间戳也不太一样，在launch文件中的频率也不一样：

  <arg name="gyro_fps"            default="400"/>
  <arg name="accel_fps"           default="250"/>

但是显然VINS-Mono是需要订阅一个同时具有角速度和线加速度信息的topic。

因此问题变成了如何让realsense2_camera发布这么一个topic。

解决方法：看源码

realsense2_camera\src\base_realsense_node.cpp就是写了如何发布所有topic，代码很长，一开始绕了不少弯路。

其中void BaseRealSenseNode::setupStreams()：

if (gyro_profile != _enabled_profiles.end() &&
            accel_profile != _enabled_profiles.end())
        {
            ROS_INFO("starting imu...");
            std::vector<rs2::stream_profile> profiles;
            profiles.insert(profiles.begin(), gyro_profile->second.begin(), gyro_profile->second.end());
            profiles.insert(profiles.begin(), accel_profile->second.begin(), accel_profile->second.end());
            auto& sens = _sensors[GYRO];
            sens.open(profiles);

            auto imu_callback_inner = [this](rs2::frame frame){
                imu_callback(frame);
            };

            auto imu_callback_sync_inner = [this](rs2::frame frame){
                imu_callback_sync(frame, _imu_sync_method);
            };

            if (_imu_sync_method > imu_sync_method::NONE)
            {
                std::string unite_method_str;
                int expected_fps(_fps[GYRO] + _fps[ACCEL]);
                unite_method_str = "COPY";
                if (_imu_sync_method == imu_sync_method::LINEAR_INTERPOLATION)
                {
                    unite_method_str = "LINEAR_INTERPOLATION";
                    expected_fps = 2 * std::min(_fps[GYRO], _fps[ACCEL]);
                }
                ROS_INFO_STREAM("Gyro and accelometer are enabled and combined to IMU message at " 
                                 << expected_fps << " fps by method:" << unite_method_str);
                sens.start(imu_callback_sync_inner);
            }
            else
            {
                sens.start(imu_callback_inner);

                if (_enable[GYRO])
                {
                    ROS_INFO_STREAM(_stream_name[GYRO] << " stream is enabled - " << "fps: " << _fps[GYRO]);
                    auto gyroInfo = getImuInfo(GYRO);
                    _info_publisher[GYRO].publish(gyroInfo);
		    
                }

                if (_enable[ACCEL])
                {
                    ROS_INFO_STREAM(_stream_name[ACCEL] << " stream is enabled - " << "fps: " << _fps[ACCEL]);
                    auto accelInfo = getImuInfo(ACCEL);
                    _info_publisher[ACCEL].publish(accelInfo);
                }
            }
        }

可以看到这里的条件判断：if (_imu_sync_method > imu_sync_method::NONE)，最后讲执行回调函数sens.start(imu_callback_sync_inner)；不然就执行_info_publisher[GYRO].publish(gyroInfo)和_info_publisher[ACCEL].publish(accelInfo)。

而我们一开始的launch文件就是采用了后面一种，最后直接发布了GYRO和ACCEL。
那么如何让_imu_sync_method不是NONE呢，在文件中搜索_imu_sync_method，直到看到：

std::string unite_imu_method_str("");
_pnh.param("unite_imu_method", unite_imu_method_str, DEFAULT_UNITE_IMU_METHOD);
if (unite_imu_method_str == "linear_interpolation")
	_imu_sync_method = imu_sync_method::LINEAR_INTERPOLATION;
else if (unite_imu_method_str == "copy")
	_imu_sync_method = imu_sync_method::COPY;
else
	_imu_sync_method = imu_sync_method::NONE;

当unite_imu_method_str 是 "copy"或者“linear_interpolation”就行了！
而这又是读取了launch文件中的unite_imu_method，所以在rs_camera.launch中直接修改：

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

重新运行realsense2_camera

roslaunch realsense2_camera rs_camera.launch 

此时可以看到发布的topic变成了"/camera/imu"

rostopic list
rostopic echo /camera/imu

大功告成！

p.s. 除此以外我还修改了enable_sync参数，是对相机和IMU进行同步的。

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

运行VINS

这里还需要修改一下配置文件：(在realsense_color_config.yaml基础上)
1、订阅的topic

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

2、相机的内参，通过读取camera_info得到或者自己标定，采用以下命令可以读取厂家的camera_info，但与实际可能存在差距。

rostopic echo /camera/color/camera_info

3、IMU到相机的变换矩阵

# 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和camera之间的外参矩阵建议使用Kalibr工具进行离线标定，也可以改成1或者2让估计器自己标定和优化。

4、IMU参数，这个需要对IMU的噪声和Bias进行标定，同时重力加速度对结果有影响。

#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

5、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)

6、相机曝光方式应为全局曝光

#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

实际效果

能够正常工作，但是绕房间一圈以后发现离起点有距离，还需要再调试一下！
还有就是如果突然图像跟踪失败并且没有重定位，轨迹会一直往前方走，这是因为只用到IMU数据时，IMU积分得到的位移、速度和角度值一定会发生漂移。对IMU进行参数标定和适当修改重力加速度的值会减少偏移的程度但不能去除。

最后欢迎各位一起交流！

---

在找到这个解决方法前本人还进行了很多其他尝试，各位也可以作为参考：

1、有人成功地实现了使用RealSense ZR300运行VINS，采用的驱动是eth-asl的maplab_realsense，这个程序还对IMU的陀螺仪、加速度计、图像的时间戳做了对齐处理。
但是我尝试了并没有读取到d435i的设备，应该是是有地方需要修改下。
Ubuntu 16.04 上用RealSense ZR300跑Vins Mono

2、在一开始看源码还没有发现它可以执行imu_callback_sync_inner回调函数的时候，以为就是在imu_callback获取传感器数据并发布的。它这里是不同时读取的，通过stream_index判断读取的内容是GYRO和ACCEL。

于是我用了一个很蠢的办法：可以看到被我注释的地方，我定义了一个静态数组imu_acc，在需要读取ACCEL数据的时候把数据存下来，在读取GYRO的时候取出来。最后的确可以让“/camera/gyro/sample”中具有加速度信息，最后可以运行VINS。

auto stream_index = (stream == GYRO.first)?GYRO:ACCEL;
    if (0 != _info_publisher[stream_index].getNumSubscribers() ||
        0 != _imu_publishers[stream_index].getNumSubscribers())
    {
        double elapsed_camera_ms = (/*ms*/ frame.get_timestamp() - /*ms*/ _camera_time_base) / 1000.0;
        ros::Time t(_ros_time_base.toSec() + elapsed_camera_ms);

        auto imu_msg = sensor_msgs::Imu();
        imu_msg.header.frame_id = _optical_frame_id[stream_index];
        imu_msg.orientation.x = 0.0;
        imu_msg.orientation.y = 0.0;
        imu_msg.orientation.z = 0.0;
        imu_msg.orientation.w = 0.0;
        imu_msg.orientation_covariance = { -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

        auto crnt_reading = *(reinterpret_cast<const float3*>(frame.get_data()));
        ConvertFromOpticalFrameToFrame(crnt_reading);
	//static float imu_acc[3] = {0,0,0};
        if (GYRO == stream_index)
        {
            imu_msg.angular_velocity.x = crnt_reading.x;
            imu_msg.angular_velocity.y = crnt_reading.y;
            imu_msg.angular_velocity.z = crnt_reading.z;
//imu_msg.linear_acceleration.x = imu_acc[0];
//imu_msg.linear_acceleration.y = imu_acc[1];
//imu_msg.linear_acceleration.z = imu_acc[2];
        }
        else if (ACCEL == stream_index)
        {
            imu_msg.linear_acceleration.x = crnt_reading.x;
            imu_msg.linear_acceleration.y = crnt_reading.y;
            imu_msg.linear_acceleration.z = crnt_reading.z;
//imu_acc[0]=imu_msg.linear_acceleration.x;
//imu_acc[1]=imu_msg.linear_acceleration.y;
//imu_acc[2]=imu_msg.linear_acceleration.z;	
        }

同时修改VINS的yaml文件

#common parameters
imu_topic: "/camera/gyro/sample"
image_topic: "/camera/color/image_raw"

需要注意的是这个方法在运行VINS前还需要有先订阅加速度的topic，比如：

rostopic echo /camera/accel/sample

然后再订阅gyro的topic，或是运行VINS，不然"/camera/gyro/sample"中还是只有角速度信息。

当然因为现在已经找到了imu_callback_sync_inner回调函数可以直接发布IMU所有信息，就不需要这样做了。