用自己的设备跑各种VI-SLAM算法（1）

1 VINS-Fusion
2 PL-VINS
3 ROVIO
4 MSCKF

本人用的设备是一个海康单目相机和一个MTI惯导，已经预先用kalibr工具标定了内外参，进一步在不同VI-SLAM算法下进行测试，记录一下各种算法的运行命令和轨迹保存方法。

本文各类算法均基于ROS操作。

环境配置

Ubuntu 16.04
ros kinetic
eigen 3.3.7
opencv 3.4.6

1 VINS-Fusion

catkin_make
source devel/setup.bashroscore
roslaunch vins vins_rviz.launch
rosrun vins vins_node  '/home/slender/vins/VINS-FUSION/src/VINS-Fusion-master/config/hikcamera/hikcam-1080x720.yaml'

保存轨迹 /vins_estimator/src/utility/visualization.cpp

void pubOdometry(const Estimator &estimator, const std_msgs::Header &header)
{if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR){nav_msgs::Odometry odometry;odometry.header = header;odometry.header.frame_id = "world";odometry.child_frame_id = "world";Quaterniond tmp_Q;tmp_Q = Quaterniond(estimator.Rs[WINDOW_SIZE]);odometry.pose.pose.position.x = estimator.Ps[WINDOW_SIZE].x();odometry.pose.pose.position.y = estimator.Ps[WINDOW_SIZE].y();odometry.pose.pose.position.z = estimator.Ps[WINDOW_SIZE].z();odometry.pose.pose.orientation.x = tmp_Q.x();odometry.pose.pose.orientation.y = tmp_Q.y();odometry.pose.pose.orientation.z = tmp_Q.z();odometry.pose.pose.orientation.w = tmp_Q.w();odometry.twist.twist.linear.x = estimator.Vs[WINDOW_SIZE].x();odometry.twist.twist.linear.y = estimator.Vs[WINDOW_SIZE].y();odometry.twist.twist.linear.z = estimator.Vs[WINDOW_SIZE].z();pub_odometry.publish(odometry);geometry_msgs::PoseStamped pose_stamped;pose_stamped.header = header;pose_stamped.header.frame_id = "world";pose_stamped.pose = odometry.pose.pose;path.header = header;path.header.frame_id = "world";path.poses.push_back(pose_stamped);pub_path.publish(path);// write result to file// 输出结果ofstream foutC(VINS_RESULT_PATH, ios::app);foutC.setf(ios::fixed, ios::floatfield);foutC.precision(9);foutC << header.stamp.toSec()<< "  ";foutC.precision(5);foutC << estimator.Ps[WINDOW_SIZE].x() << "  "<< estimator.Ps[WINDOW_SIZE].y() << "  "<< estimator.Ps[WINDOW_SIZE].z() << "  "<< tmp_Q.x() << "  "<< tmp_Q.y() << "  "<< tmp_Q.z() << "  "<< tmp_Q.w() << "  "//<< estimator.Vs[WINDOW_SIZE].x() << ","//<< estimator.Vs[WINDOW_SIZE].y() << ","//<< estimator.Vs[WINDOW_SIZE].z() << "," << endl;foutC.close();Eigen::Vector3d tmp_T = estimator.Ps[WINDOW_SIZE];printf("time: %f, t: %f %f %f q: %f %f %f %f \n", header.stamp.toSec(), tmp_T.x(), tmp_T.y(), tmp_T.z(),tmp_Q.w(), tmp_Q.x(), tmp_Q.y(), tmp_Q.z());}
}

2 PL-VINS

仿照plvins-show-linepoint.launch,修改对应的参数配置文件yaml

roscore
roslaunch plvins_estimator hikcam1080-plvins.launch

hikcam1080-plvins.launch

<launch><arg name="config_path" default = "$(find feature_tracker)/../config/hikcam/hikcam_1080x720.yaml" /><arg name="vins_path" default = "$(find feature_tracker)/../config/../" /><node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find plvins_estimator)/../config/vins_rviz_config.rviz" /><node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log"><param name="config_file" type="string" value="$(arg config_path)" /><param name="vins_folder" type="string" value="$(arg vins_path)" /></node><node name="linefeature_tracker" pkg="feature_tracker" type="LineFeature_tracker" output="log"><param name="config_file" type="string" value="$(arg config_path)" /><param name="vins_folder" type="string" value="$(arg vins_path)" /></node>    <node name="plvins_estimator" pkg="plvins_estimator" type="plvins_estimator" output="log"><param name="config_file" type="string" value="$(arg config_path)" /><param name="vins_folder" type="string" value="$(arg vins_path)" /></node><node name="pose_graph" pkg="pose_graph" type="pose_graph" output="screen"><param name="config_file" type="string" value="$(arg config_path)" /><param name="visualization_shift_x" type="int" value="0" /><param name="visualization_shift_y" type="int" value="0" /><param name="skip_cnt" type="int" value="0" /><param name="skip_dis" type="double" value="0" /></node><node name="image_node_b" pkg="image_node_b" type="image_node_b" output="log"></node>
</launch>

3 ROVIO

catkin build rovio --cmake-args -DCMAKE_BUILD_TYPE=Release -DMAKE_SCENE=ONroslaunch rovio hikcam_node.launch

hikcam_node.launch修改yaml配置文件,remap话题名称

<?xml version="1.0" encoding="UTF-8"?>
<launch><node pkg="rovio" type="rovio_node" name="rovio" output="screen"><param name="filter_config" value="$(find rovio)/cfg/rovio.info"/><param name="camera0_config" value="/home/slender/vins/ROVIO/src/rovio/cfg/hikcam_1080x720.yaml"/><remap from="/imu0" to="/imu/data"/><remap from="/cam0/image_raw" to="/hikrobot_camera/gray"/></node>
</launch>

保存轨迹
/rovio/include/rovio/RovioNode.hpp中找到imuOutput_,对应输出位置和姿态.

// Obtain the save filter state.mtFilterState& filterState = mpFilter_->safe_;mtState& state = mpFilter_->safe_.state_;state.updateMultiCameraExtrinsics(&mpFilter_->multiCamera_);MXD& cov = mpFilter_->safe_.cov_;imuOutputCT_.transformState(state,imuOutput_);std::ofstream out_pose("/home/slender/vins/ROVIO/traj/rovio_traj.txt",std::ios::app);//save poseout_pose.setf(std::ios::fixed, std::ios::floatfield);out_pose.precision(9);out_pose << ros::Time(mpFilter_->safe_.t_) << " " ;out_pose.precision(5);out_pose << imuOutput_.WrWB()(0) << " " << imuOutput_.WrWB()(1) << " " << imuOutput_.WrWB()(2) << " " << imuOutput_.qBW().x() << " " << imuOutput_.qBW().y() << " " << imuOutput_.qBW().z() << " " << -imuOutput_.qBW().w() << std::endl;out_pose.close();

ROVIO要求输入图像为灰度图,还需要先对图像topic进行处理.

4 MSCKF

catkin_make roslaunch msckf_mono hikcam.launch

类似地，仿照euroc.launch,修改配置参数yaml
hikcam.launch

<?xml version="1.0"?>
<launch><param name="/use_sim_time" value="true"/><!-- Path to MH_03 kalibr calibration --><arg name="kalibr_yaml" default="$(find msckf_mono)/hikcam/hikcam-1080x720.yaml"/><group ns="msckf"><rosparam command="load" file="$(arg kalibr_yaml)"/><param name="kalibr_camera_name" value="cam0"/><param name="feature_cov" value="2"/><param name="imu_vars/w_var" value="1e-4"/><param name="imu_vars/dbg_var" value="3.6733e-5"/><param name="imu_vars/a_var" value="1e-2"/><param name="imu_vars/dba_var" value="7e-2"/><param name="imu_covars/p_init" value="1e-12"/><param name="imu_covars/q_init" value="1e-5"/><param name="imu_covars/v_init" value="1e-2"/><param name="imu_covars/ba_init" value="1e-2"/><param name="imu_covars/bg_init" value="1e-2"/><param name="max_gn_cost_norm" value="7"/><param name="translation_threshold" value="0.1"/><param name="keyframe_transl_dist" value="0.5"/><param name="keyframe_rot_dist" value="0.5"/><param name="min_track_length" value="5"/><param name="max_track_length" value="50"/><param name="max_cam_states" value="30"/><param name="ransac_threshold" value="0.00000002"/><param name="n_grid_rows" value="10"/><param name="n_grid_cols" value="10"/><node pkg="msckf_mono" name="msckf_mono_node" type="msckf_mono_node"><remap from="imu" to="/imu/data"/><remap from="image_mono" to="/hikrobot_camera/resize"/></node></group><node pkg="rviz" name="msckf_rviz" type="rviz" args="-d $(find msckf_mono)/launch/rviz.rviz"/></launch>

保存轨迹
找到/msckf_mono/src/msckf_mono/src/ros_interface.cpp中发布Odometry的部分

void RosInterface::publish_core(const ros::Time& publish_time){auto imu_state = msckf_.getImuState();nav_msgs::Odometry odom;odom.header.stamp = publish_time;odom.header.frame_id = "map";odom.twist.twist.linear.x = imu_state.v_I_G[0];odom.twist.twist.linear.y = imu_state.v_I_G[1];odom.twist.twist.linear.z = imu_state.v_I_G[2];odom.pose.pose.position.x = imu_state.p_I_G[0];odom.pose.pose.position.y = imu_state.p_I_G[1];odom.pose.pose.position.z = imu_state.p_I_G[2];Quaternion<float> q_out = imu_state.q_IG.inverse();odom.pose.pose.orientation.w = q_out.w();odom.pose.pose.orientation.x = q_out.x();odom.pose.pose.orientation.y = q_out.y();odom.pose.pose.orientation.z = q_out.z();odom_pub_.publish(odom);std::ofstream ofs_traj("/home/slender/vins/msckf_mono/traj/trajectory.txt",std::ios::app);ofs_traj.setf(std::ios::fixed, std::ios::floatfield);ofs_traj.precision(9);ofs_traj << odom.header.stamp <<" ";ofs_traj.precision(5);ofs_traj << odom.pose.pose.position.x << " " << odom.pose.pose.position.y << " " << odom.pose.pose.position.z << " " << odom.pose.pose.orientation.x << " " << odom.pose.pose.orientation.y << " " << odom.pose.pose.orientation.z << " " << odom.pose.pose.orientation.w << std::endl;ofs_traj.close();}

用自己的设备跑各种VI-SLAM算法(1)——VINS/PL-VINS/ROVIO/MSCKF相关推荐

AR设备单目视觉惯导SLAM算法综述与评价
点云PCL免费知识星球,点云论文速读. 标题:Survey and evaluation of monocular visual-inertial SLAM algorithms for augmen ...
八种常用激光雷达和视觉SLAM算法的评估与比较
文章:Evaluation and comparison of eight popular Lidar and Visual SLAM algorithms 作者:Bharath Garigipati ...
arduino设备跑 ros service server 的波折记
arduino设备跑 ros service server 的波折记. 引-- 参考前文https://blog.csdn.net/qq_38288618/article/details/104082 ...
ubuntu16.04 跑 rplidar Hector Slam
ubuntu16.04上 Ros Kinetic 跑 rplidar A1的 Hector Slam ROS 上跑rplidar Hector Slam 天了噜-这是我第一次写 CSDN .小白也是刚 ...
转：SLAM算法解析：抓住视觉SLAM难点，了解技术发展大趋势
SLAM(Simultaneous Localization and Mapping)是业界公认视觉领域空间定位技术的前沿方向,中文译名为"同步定位与地图构建",它主要用于解决机器 ...
自动驾驶/机器人 SLAM算法面经1
欢迎关注公众号:内推君SIR,加微信:neituijunsir 加入自动驾驶交流群:聚焦自动驾驶行业招聘信息 /技术发展 /行业动态. Case 1 一面项目相关 1.简历中的项目相关问题,项目 ...
一起自学SLAM算法：8.1 Gmapping算法
连载文章,长期更新,欢迎关注: 写在前面第1章-ROS入门必备知识第2章-C++编程范式第3章-OpenCV图像处理第4章-机器人传感器第5章-机器人主机第6章-机器人底盘第7章-SLA ...
SLAM算法评估中的轨迹拟合与外参求解
文章同步更新到github pages,欢迎收藏文章目录问题描述数学关系求解方法求解$_{gp}^{gv}T$ 求解$_{lp}^{lv}T$ 交替迭代优化时间戳对齐实验结果参考文献 ...
一起自学SLAM算法：7.1 SLAM发展简史
连载文章,长期更新,欢迎关注: 写在前面第1章-ROS入门必备知识第2章-C++编程范式第3章-OpenCV图像处理第4章-机器人传感器第5章-机器人主机第6章-机器人底盘第7章-SLA ...
一起自学SLAM算法：8.2 Cartographer算法
连载文章,长期更新,欢迎关注: 写在前面第1章-ROS入门必备知识第2章-C++编程范式第3章-OpenCV图像处理第4章-机器人传感器第5章-机器人主机第6章-机器人底盘第7章-SLA ...

用自己的设备跑各种VI-SLAM算法(1)——VINS/PL-VINS/ROVIO/MSCKF