Merge pull request MIT-SPARK#135 from SPARK/feature/parse_gt_bias

option to parse gt bias from launch file

.github/ISSUE_TEMPLATE/error-report.md

@@ -23,7 +23,7 @@ about: Fill this out and upload your data (!!)
 Please attach all the files needed to reproduce the error.
 
 Please give also the following information:
-* SparkVio branch, tag or commit used
+* KimeraVIO branch, tag or commit used
 * GTSAM version used:
 * OpenGV version used:
 * OpenCV version used: type `opencv_version`

include/kimera_vio_ros/utils/UtilsRos.h

@@ -36,6 +36,7 @@ void msgCamInfoToCameraParams(const sensor_msgs::CameraInfoConstPtr& cam_info,
                               CameraParams* cam_params);
 
 void msgGtOdomToVioNavState(const nav_msgs::Odometry& gt_odom,
+                            const ros::NodeHandle& node_handle,
                             VioNavState* vio_navstate);
 
 }  // namespace utils

launch/kimera_vio_ros.launch

@@ -13,7 +13,7 @@
   <!-- Use CameraInfo topics to get parameters, instead of yaml files -->
   <arg name="use_online_cam_params" default="false"/>
 
-  <!-- If true, SparkVio will log output of all modules to the
+  <!-- If true, KimeraVIO will log output of all modules to the
        'log_output_path' location. -->
   <arg name="log_output" default="false"/>
   <arg name="log_output_path"
@@ -24,7 +24,7 @@
   <arg name="log_gt_data" default="false"/>
   <arg name="gt_topic"    default="ground_truth_odometry_topic"/>
 
-  <!-- If true, SparkVio will enable the LoopClosureDetector module. -->
+  <!-- If true, KimeraVIO will enable the LoopClosureDetector module. -->
   <arg name="use_lcd" default="false"/>
 
   <!-- Parameters -->

launch/kimera_vio_ros_euroc.launch

@@ -6,7 +6,7 @@
   <arg name="online" default="true" />
   <!-- Set use_sim_time to true if you use rosbag with clock argument -->
   <param name="use_sim_time" value="true"/>
-  <!-- Set log_output to true and SparkVio will log output of all modules to
+  <!-- Set log_output to true and KimeraVIO will log output of all modules to
        the log_output_path location. -->
   <arg name="log_output" default="false"/>
   <arg name="log_output_path"
@@ -16,6 +16,9 @@
   <arg name="log_gt_data" default="false"/>
   <arg name="gt_topic" default="/vicon/firefly_sbx/firefly_sbx"/>
 
+  <rosparam param="gt_gyro_bias"> [-0.002229, 0.0207, 0.07635] </rosparam>
+  <rosparam param="gt_accel_bias"> [-0.012492, 0.547666, 0.069073] </rosparam>
+
   <arg name="use_lcd"   default="false"/>
   <arg name="visualize" default="true"/>
 

scripts/retimestamp_rosbag.py

@@ -11,7 +11,7 @@
 # Usage    : python restamp_bag.py -i inbag.bag -o outbag.bag
 #
 #
-# This file has been modified to fit the needs of the SparkVIO project.
+# This file has been modified to fit the needs of the KimeraVIO project.
 # All original credit for this work goes to ETHZ.
 # ------------------------------------------------------------------------------
 

src/RosBagDataProvider.cpp

@@ -359,7 +359,7 @@ VioNavState RosbagDataProvider::getGroundTruthVioNavState(
   CHECK_LT(k_frame, rosbag_data_.gt_odometry_.size());
   nav_msgs::Odometry gt_odometry = *(rosbag_data_.gt_odometry_.at(k_frame));
   VioNavState vio_nav_state;
-  utils::msgGtOdomToVioNavState(gt_odometry, &vio_nav_state);
+  utils::msgGtOdomToVioNavState(gt_odometry, nh_private_, &vio_nav_state);
   return vio_nav_state;
 }
 

src/RosOnlineDataProvider.cpp

@@ -364,7 +364,9 @@ void RosOnlineDataProvider::callbackGtOdomOnce(
   LOG(WARNING) << "Using initial ground-truth state for initialization.";
   CHECK(gt_odom_msg);
   utils::msgGtOdomToVioNavState(
-      *gt_odom_msg, &vio_params_.backend_params_->initial_ground_truth_state_);
+      *gt_odom_msg,
+      nh_private_,
+      &vio_params_.backend_params_->initial_ground_truth_state_);
 
   // Signal receptance of ground-truth pose.
   gt_init_pose_received_ = true;

src/RosVisualizer.cpp

@@ -413,7 +413,7 @@ void RosVisualizer::publishResiliency(
   const gtsam::Matrix3& vel_cov =
       gtsam::sub(backend_output->state_covariance_lkf_, 6, 9, 6, 9);
 
-  // Create message type for quality of SparkVIO
+  // Create message type for quality of KimeraVIO
   std_msgs::Float64MultiArray resiliency_msg;
 
   // Publishing extra information:

src/utils/UtilsRos.cpp

@@ -19,9 +19,8 @@
 
 #include <gtsam/geometry/Pose3.h>
 
-#include <kimera-vio/frontend/CameraParams.h>
 #include <kimera-vio/common/VioNavState.h>
-
+#include <kimera-vio/frontend/CameraParams.h>
 
 namespace VIO {
 
@@ -59,7 +58,8 @@ void msgCamInfoToCameraParams(const sensor_msgs::CameraInfoConstPtr& cam_info,
   cam_params->camera_id_ = cam_info->header.frame_id;
   CHECK(!cam_params->camera_id_.empty());
 
-  cam_params->distortion_model_ = CameraParams::stringToDistortion(cam_info->distortion_model, "pinhole");
+  cam_params->distortion_model_ =
+      CameraParams::stringToDistortion(cam_info->distortion_model, "pinhole");
 
   const std::vector<double>& distortion_coeffs = cam_info->D;
   CHECK_EQ(distortion_coeffs.size(), 4);
@@ -101,6 +101,7 @@ void msgCamInfoToCameraParams(const sensor_msgs::CameraInfoConstPtr& cam_info,
 }
 
 void msgGtOdomToVioNavState(const nav_msgs::Odometry& gt_odom,
+                            const ros::NodeHandle& node_handle,
                             VioNavState* vio_navstate) {
   CHECK_NOTNULL(vio_navstate);
 
@@ -118,10 +119,22 @@ void msgGtOdomToVioNavState(const nav_msgs::Odometry& gt_odom,
 
   vio_navstate->pose_ = gtsam::Pose3(W_R_B, position);
   vio_navstate->velocity_ = velocity;
-  // TODO(Toni): how can we get the ground-truth biases? For sim, ins't it 0?
-  static const gtsam::Vector3 gyro_bias(0.0, 0.0, 0.0);
-  static const gtsam::Vector3 acc_bias(0.0, 0.0, 0.0);
-  vio_navstate->imu_bias_ = gtsam::imuBias::ConstantBias(acc_bias, gyro_bias);
+
+  // Get acceleration and gyro biases. Default is 0.
+  std::vector<double> parsed_acc_bias = {0.0, 0.0, 0.0};
+  node_handle.getParam("gt_accel_bias", parsed_acc_bias);
+  CHECK_EQ(parsed_acc_bias.size(), 3u);
+
+  std::vector<double> parsed_gyr_bias = {0.0, 0.0, 0.0};
+  node_handle.getParam("gt_gyro_bias", parsed_gyr_bias);
+  CHECK_EQ(parsed_gyr_bias.size(), 3u);
+
+  gtsam::Vector3 acc_bias(
+      parsed_acc_bias[0], parsed_acc_bias[1], parsed_acc_bias[2]);
+  gtsam::Vector3 gyr_bias(
+      parsed_gyr_bias[0], parsed_gyr_bias[1], parsed_gyr_bias[2]);
+
+  vio_navstate->imu_bias_ = gtsam::imuBias::ConstantBias(acc_bias, gyr_bias);
 }
 
 }  // namespace utils