ROS kinetic 运行s_msckf和 vins_fusion

s_msckf：采用多状态约束的双目vio系统

!!!注意

imuCallback：接收IMU数据，将IMU数据存到imu_msg_buffer中，这里只会利用开头200帧IMU数据进行静止初始化，不做其他处理。
featureCallback：接收双目特征，进行后端处理。利用IMU进行EKF Propagation，利用双目特征进行EKF Update。


静止初始化(initializeGravityAndBias)：将前200帧加速度和角速度求平均, 平均加速度的模值g作为重力加速度, 平均角速度作为陀螺仪的bias, 计算重力向量(0,0,-g)和平均加速度之间的夹角(旋转四元数), 标定初始时刻IMU系与world系之间的夹角. 因此MSCKF要求前200帧IMU是静止不动的

 

sudo apt-get install libsuitesparse-dev
cd ~/catkin_ws/src
git clone KumarRobotics/msckf_vio
cd ..
catkin_make --pkg msckf_vio --cmake-args -DCMAKE_BUILD_TYPE=Release
#激活环境变量很关键
source /devel/setup.bash

roslaunch msckf_vio msckf_vio_euroc.launch
#注意文件路径
rosrun rviz rviz -d rviz/rviz_euroc_config.rviz (改成你自己的rviz文件)
rosbag play ~/data/euroc/MH_04_difficult.bag(改成你自己的rosbag文件)

可以看到，s_msckf的输出是没有轨迹的，可以增加如下脚本，将/odom存为/path，在rviz订阅即可可视化轨迹 

脚本来自其issue：https://github.com/KumarRobotics/msckf_vio/issues/13

#!/usr/bin/env python

import rospy
from nav_msgs.msg import Odometry, Path
from geometry_msgs.msg import PoseStamped

class OdomToPath:
    def __init__(self):
        self.path_pub = rospy.Publisher('/slz_path', Path, latch=True, queue_size=10)
        self.odom_sub = rospy.Subscriber('/firefly_sbx/vio/odom', Odometry, self.odom_cb, queue_size=10)
        self.path = Path()

    def odom_cb(self, msg):
        cur_pose = PoseStamped()
        cur_pose.header = msg.header
        cur_pose.pose = msg.pose.pose
        self.path.header = msg.header
        self.path.poses.append(cur_pose)
        self.path_pub.publish(self.path)

if __name__ == '__main__':
    rospy.init_node('odom_to_path')
    odom_to_path = OdomToPath()
    rospy.spin()

或者增加一个draw_path的功能包：

cpp为：

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <ro