用自己的设备跑各种VI-SLAM算法(1)——VINS/PL-VINS/ROVIO/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.bash

roscore
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=ON

roslaunch 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 pose
        out_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();
  }