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

ROS kinetic 运行s_msckf和 vins_fusion相关推荐

Ubuntu 16.04下装ROS Kinetic问题若干、安装后首次运行
Ubuntu 16.04下装ROS Kinetic问题若干在Ubuntu 16.04下装ROS Kinetic中遇到一些问题,记录如下: 安装指引链接 kinetic版本安装指引链接其他版本R ...
的环境下 qt 运行在_Ubuntu16.04环境下运行vins mono（环境配置及编译）之ROS kinetic的安装...
所需环境:ubuntu16.04+ROS kinetic+opencv 3.3.1+eigen3.3.3+ceres solver 1.14 1.ROS Kinetic 的安装 (1)设置source ...
ROS入门笔记（五）：ROS中运行rqt_plot的问题（kinetic）
1 问题 rosrun rqt_plot rqt_plot ggk@ggk-virtual-machine:~$ rosrun rqt_plot rqt_plot /usr/lib/python2.7 ...
ROS kinetic安装、Kinect2驱动安装和配置
转载自:ROS kinetic安装.Kinect2驱动安装和配置直接看原帖,会更加舒适一些,粘过来没来得及将命令行单独排版. 为了以后的人可以少走弯路,我会尽可能的列出自己遇到的问题和解决方法. 提 ...
ROS学习（二）：在ubuntu 16.04安装ROS Kinetic
机器人操作系统 ROS 一般可以采用 apt-get的方式进行安装,可以根据自己的Ubuntu的具体版本下载相应的ROS版本进行安装. 安装前 1.配置软件库: Ubuntu repositories ...
Ubuntu16.04 + ROS kinetic + 激光slam-cartographer ROS + 数据仿真 + Turtlebot3仿真
一.Ubuntu16.04 +ROS kinetic的安装及问题解决 https://blog.csdn.net/GGY1102/article/details/107877937 二.Cartogr ...
ROS kinetic外接Realsense D435i跑ORB_SLAM2教程
本机环境:window10+vmware+ubuntu16.04+ROS kinetic+Intel Realsense D435i 基本步骤就四步: 1.配置好Realsense的SDK以及ro ...
ROS学习(开篇)Ubuntu16.04安装ROS Kinetic详细教程
文章目录前言一.添加ROS软件源(sources.list) 二.添加密钥三.更新apt功能包列表四.安装ROS 五.初始化 rosdep 六.将ROS环境变量添加到.bashrc文件中七. ...
北洋(HOKUYO)雷达在ROS Kinetic下使用
Ubuntu 16.04 ROS Kinetic 安装使用北阳 HOKUYO 雷达 utm-30 安装运行在ROS下还有一个包叫做hokuyo_node,但是只适用于indigo版本,官网后来又推 ...

ROS kinetic 运行s_msckf和 vins_fusion

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

cpp为：

cmakelists.txt

package.xml

vins_fusion: 双目vio等多系统

ROS kinetic 运行s_msckf和 vins_fusion相关推荐

最新文章

热门文章