ROS kinetic 运行s_msckf和 vins

本文主要是介绍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.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的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！