本文摘要：s_msckf是一种基于多状态约束的双目视觉惯性里程计(VIO)系统，通过IMUCallback接收并处理前200帧静止IMU数据以初始化重力加速度和陀螺仪偏差。在EKF框架下，该系统利用IMU数据进行预测，并结合双目特征信息完成状态更新。为了实现轨迹可视化，开发了Python脚本和C++节点将Odometry消息转换为Path消息发布至ROS主题。此外，文章还介绍了VINS-Fusion系统的搭建与运行方法，覆盖单目+IMU、立体+IMU以及立体相机等多种传感器配置下的KITTI和Euroc数据集测试场景。

转载文章

本篇文章为转载内容。原文链接：https://blog.csdn.net/slzlincent/article/details/104364909。

该文由互联网用户投稿提供，文中观点代表作者本人意见，并不代表本站的立场。

作为信息平台，本站仅提供文章转载服务，并不拥有其所有权，也不对文章内容的真实性、准确性和合法性承担责任。

如发现本文存在侵权、违法、违规或事实不符的情况，请及时联系我们，我们将第一时间进行核实并删除相应内容。

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.bashroslaunch 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 pythonimport rospy
from nav_msgs.msg import Odometry, Path
from geometry_msgs.msg import PoseStampedclass 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.headercur_pose.pose = msg.pose.poseself.path.header = msg.headerself.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 <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>nav_msgs::Path  path;ros::Publisher  path_pub;
ros::Subscriber odomSub;
ros::Subscriber odom_raw_Sub;void odomCallback(const nav_msgs::Odometry::ConstPtr& odom){geometry_msgs::PoseStamped this_pose_stamped;this_pose_stamped.header= odom->header;this_pose_stamped.pose = odom->pose.pose;//this_pose_stamped.pose.position.x = odom->pose.pose.position.x;//this_pose_stamped.pose.position.y = odom->pose.pose.position.y;//this_pose_stamped.pose.orientation = odom->pose.pose.orientation;//this_pose_stamped.header.stamp = ros::Time::now();//this_pose_stamped.header.frame_id = "world";//frame_id 是消息中与数据相关联的参考系id，例如在在激光数据中，frame_id对应激光数据采集的参考系 path.header= this_pose_stamped.header;path.poses.push_back(this_pose_stamped);//path.header.stamp = ros::Time::now();//path.header.frame_id= "world";path_pub.publish(path);//printf("path_pub ");//printf("odom %.3lf %.3lf\n",odom->pose.pose.position.x,odom->pose.pose.position.y);}int main (int argc, char **argv)
{ros::init (argc, argv, "showpath");ros::NodeHandle ph;path_pub = ph.advertise<nav_msgs::Path>("/trajectory",10, true);odomSub  = ph.subscribe<nav_msgs::Odometry>("/firefly_sbx/vio/odom", 10, odomCallback);//ros::Rate loop_rate(50);while (ros::ok()){ros::spinOnce();               // check for incoming messages//loop_rate.sleep();}return 0;
}

cmakelists.txt

cmake_minimum_required(VERSION 2.8.3)
project(draw)## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTSgeometry_msgsroscpprospystd_msgsmessage_generation
)catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES learning_communicationCATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)###########
## Build ##
###########include_directories(include${catkin_INCLUDE_DIRS}
)add_executable(draw_path draw.cpp)
target_link_libraries(draw_path ${catkin_LIBRARIES})

package.xml

<?xml version="1.0"?>
<package><name>draw</name><version>0.0.0</version><description>The learning_communication package</description><!-- One maintainer tag required, multiple allowed, one person per tag --><!-- Example:  --><!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --><maintainer email="hcx@todo.todo">hcx</maintainer><!-- One license tag required, multiple allowed, one license per tag --><!-- Commonly used license strings: --><!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --><license>TODO</license><!-- Url tags are optional, but multiple are allowed, one per tag --><!-- Optional attribute type can be: website, bugtracker, or repository --><!-- Example: --><!-- <url type="website">http://wiki.ros.org/learning_communication</url> --><!-- Author tags are optional, multiple are allowed, one per tag --><!-- Authors do not have to be maintainers, but could be --><!-- Example: --><!-- <author email="jane.doe@example.com">Jane Doe</author> --><!-- The *_depend tags are used to specify dependencies --><!-- Dependencies can be catkin packages or system dependencies --><!-- Examples: --><!-- Use build_depend for packages you need at compile time: --><!--   <build_depend>message_generation</build_depend> --><!-- Use buildtool_depend for build tool packages: --><!--   <buildtool_depend>catkin</buildtool_depend> --><!-- Use run_depend for packages you need at runtime: --><!--   <run_depend>message_runtime</run_depend> --><!-- Use test_depend for packages you need only for testing: --><!--   <test_depend>gtest</test_depend> --><buildtool_depend>catkin</buildtool_depend><build_depend>geometry_msgs</build_depend><build_depend>roscpp</build_depend><build_depend>rospy</build_depend><build_depend>std_msgs</build_depend><run_depend>geometry_msgs</run_depend><run_depend>roscpp</run_depend><run_depend>rospy</run_depend><run_depend>std_msgs</run_depend><build_depend>message_generation</build_depend><run_depend>message_runtime</run_depend><!-- The export tag contains other, unspecified, tags --><export><!-- Other tools can request additional information be placed here --></export>
</package>

vins_fusion: 双目vio等多系统

mkdir -p vins-catkin_ws/src
cd vins-catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
cd ..
catkin_make
source devel/setup.bash#按照readme
### 3.1 Monocualr camera + IMU```roslaunch vins vins_rviz.launchrosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml (optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml rosbag play YOUR_DATASET_FOLDER/MH_01_easy.bag
```### 3.2 Stereo cameras + IMU```roslaunch vins vins_rviz.launchrosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml (optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml rosbag play YOUR_DATASET_FOLDER/MH_01_easy.bag
```### 3.3 Stereo cameras```roslaunch vins vins_rviz.launchrosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml (optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml rosbag play YOUR_DATASET_FOLDER/MH_01_easy.bag
```<img src="https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/blob/master/support_files/image/euroc.gif" width = 430 height = 240 />## 4. KITTI Example
### 4.1 KITTI Odometry (Stereo)
Download [KITTI Odometry dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) to YOUR_DATASET_FOLDER. Take sequences 00 for example,
Open two terminals, run vins and rviz respectively. 
(We evaluated odometry on KITTI benchmark without loop closure funtion)
```roslaunch vins vins_rviz.launch(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/kitti_odom/kitti_config00-02.yamlrosrun vins kitti_odom_test ~/catkin_ws/src/VINS-Fusion/config/kitti_odom/kitti_config00-02.yaml YOUR_DATASET_FOLDER/sequences/00/ 
```
### 4.2 KITTI GPS Fusion (Stereo + GPS)
Download [KITTI raw dataset](http://www.cvlibs.net/datasets/kitti/raw_data.php) to YOUR_DATASET_FOLDER. Take [2011_10_03_drive_0027_synced](https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_10_03_drive_0027/2011_10_03_drive_0027_sync.zip) for example.
Open three terminals, run vins, global fusion and rviz respectively. 
Green path is VIO odometry; blue path is odometry under GPS global fusion.
```roslaunch vins vins_rviz.launchrosrun vins kitti_gps_test ~/catkin_ws/src/VINS-Fusion/config/kitti_raw/kitti_10_03_config.yaml YOUR_DATASET_FOLDER/2011_10_03_drive_0027_sync/ rosrun global_fusion global_fusion_node
```<img src="https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/blob/master/support_files/image/kitti.gif" width = 430 height = 240 />## 5. VINS-Fusion on car demonstration
Download [car bag](https://drive.google.com/open?id=10t9H1u8pMGDOI6Q2w2uezEq5Ib-Z8tLz) to YOUR_DATASET_FOLDER.
Open four terminals, run vins odometry, visual loop closure(optional), rviz and play the bag file respectively. 
Green path is VIO odometry; red path is odometry under visual loop closure.
```roslaunch vins vins_rviz.launchrosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/vi_car/vi_car.yaml (optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/vi_car/vi_car.yaml rosbag play YOUR_DATASET_FOLDER/car.bag

本篇文章为转载内容。原文链接：https://blog.csdn.net/slzlincent/article/details/104364909。

该文由互联网用户投稿提供，文中观点代表作者本人意见，并不代表本站的立场。

作为信息平台，本站仅提供文章转载服务，并不拥有其所有权，也不对文章内容的真实性、准确性和合法性承担责任。

如发现本文存在侵权、违法、违规或事实不符的情况，请及时联系我们，我们将第一时间进行核实并删除相应内容。

名词解释

作为当前文章的名词解释，仅对当前文章有效。

双目视觉惯性里程计（VIO, Visual-Inertial Odometry）：双目视觉惯性里程计是一种融合了视觉传感器（如双目相机）和惯性测量单元（IMU）数据的定位技术。在s_msckf系统中，VIO通过连续处理和匹配双目图像中的特征点以及IMU提供的加速度和角速度信息，利用扩展卡尔曼滤波器（EKF）进行状态估计与优化，实时计算并输出设备的位置、速度和姿态信息，从而实现对移动平台的高精度自主导航。

扩展卡尔曼滤波器（EKF, Extended Kalman Filter）：扩展卡尔曼滤波器是经典卡尔曼滤波器在非线性系统下的推广应用，它是一种常用的非线性状态估计方法。在s_msckf系统中，EKF Propagation阶段利用IMU数据预测系统的下一个状态，而EKF Update阶段则借助于双目视觉测得的特征信息对预测结果进行修正更新，以更准确地估算出系统的状态变量（例如位姿、速度等）。

静止初始化（Static Initialization）：静止初始化是s_msckf系统启动时的重要步骤，其目的是校准初始时刻IMU坐标系与世界坐标系之间的相对关系，并确定IMU传感器的零偏参数（如陀螺仪偏差）。具体做法是在系统启动后的前200帧IMU数据中，通过对加速度和角速度求平均值来估计重力加速度及其方向，进而确定重力向量和IMU的原始偏差，为后续VIO过程提供准确的初始条件。这个过程中要求IMU在采集这些数据时处于静止状态，以便准确提取出重力分量。