前言
Intel Realsense D435i在D435的基础上硬件融合了IMU，然而目前网上关于这款摄像头的资料非常少，本文主要介绍自己拿着d435i历经曲折最后成功运行VINS-Mono的过程。。。
准备工作
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得到或者自己标定
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.
可以直接用原来的，也可以改成1或者2让估计器自己优化。
4、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.80 # gravity magnitude
5、个人认为realsense d435i已经做好了硬件同步所以不需要在线估计同步时差，当然也可以不改
#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的参数尤其是重力加速度的问题。
最后欢迎各位一起交流！
在找到这个解决方法前本人还进行了很多其他尝试，各位也可以作为参考：
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所有信息，就不需要这样做了