Use your own equipment to run various VI-SLAM algorithms (1)
The equipment I use is a Hikvision monocular camera and an MTI inertial navigation system. The internal and external parameters have been calibrated with the kalibr tool in advance. I further tested it under different VI-SLAM algorithms and recorded the running commands and trajectory saving methods of various algorithms. .
All algorithms in this article are based on ROS operations.
Environment configuration
- Ubuntu 16.04
- ros kinetic
- own 3.3.7
- opencv 3.4.6
1 WINES-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'
Save track/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
Follow the example of plvins-show-linepoint.launch and modify the corresponding parameter configuration file 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 modifies the yaml configuration file and remaps the topic name
<?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>
Find imuOutput_ in the saved trajectory /rovio/include/rovio/RovioNode.hpp, corresponding to the output position and attitude.
// 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 requires that the input image be a grayscale image, and the image topic needs to be processed first.
4 MSCKF
catkin_make
roslaunch msckf_mono hikcam.launch
Similarly, follow the example of euroc.launch and modify the configuration parameter 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>
Save the track
and find the part that publishes Odometry in /msckf_mono/src/msckf_mono/src/ros_interface.cpp
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();
}