Switch to using gravity magnitude instead of specifying a vector, dis…

…parity-based zero-velocity update logic, other small variable renames to make static init code clearer

docs/gs-datasets.dox

@@ -158,7 +158,7 @@ The camera is 10 Hz, while the Xsens IMU is 100 Hz sensing rate.
 A groundtruth "baseline" trajectory is also provided which is the resulting output from fusion of the FoG, RKT GPS, and wheel encoders.
 
 @code{.shell-session}
-git clone https://github.com/irapkaist/file_player.git
+git clone https://github.com/rpng/file_player.git
 git clone https://github.com/irapkaist/irp_sen_msgs.git
 catkin build
 rosrun file_player file_player

docs/update-zerovelocity.dox

@@ -63,17 +63,28 @@ Most works boil down to simple thresholding and the approach is to try to determ
 There have been other works, @cite Wagstaff2017IPIN and @cite Ramanandan2011TITS, which have looked at more complicated methods and try to address the issue that this threshold can be dependent on the type of different motions (such as running vs walking) and characteristics of the platform which the sensor is mounted on (we want to ignore vehicle engine vibrations and other non-essential observed vibrations).
 
 
+@subsection update-zerovelocity-detect-imu Inertial-based Detection
+
 We approach this detection problem based on tuning of a \f$\chi^2\f$, chi-squared, thresholding based on the measurement model above.
 It is important to note that we also have a velocity magnitude check which is aimed at preventing constant velocity cases which have non-zero magnitude.
 More specifically, we perform the following threshold check to see if we are current at zero velocity:
 
 \f{align*}{
-\tilde{\mathbf{z}}^\top(\mathbf{H}\mathbf{P}\mathbf{H}^\top + \mathbf{R})^{-1}\tilde{\mathbf{z}} < \chi^2
+\tilde{\mathbf{z}}^\top(\mathbf{H}\mathbf{P}\mathbf{H}^\top + \alpha\mathbf{R})^{-1}\tilde{\mathbf{z}} < \chi^2
 \f}
 
-We found that in the real world experiments, typically the inertial measurement noise \f$\mathbf{R}\f$ needs to be inflated by 50-100 times to allow for proper detection.
+We found that in the real world experiments, typically the inertial measurement noise \f$\mathbf{R}\f$ needs to be inflated by \f$\alpha\in[50,100]\f$ times to allow for proper detection.
 This can hint that we are using overconfident inertial noises, or that there are additional frequencies (such as the vibration of motors) which inject additional noises.
 
 
+@subsection update-zerovelocity-detect-disp Disparity-based Detection
+
+We additionally have a detection method which leverages the visual feature tracks. Given two sequential images, the assumption is if there is very little disparity change between feature tracks then we will be stationary. Thus we calculate the average disparity and threshold on this value.
+
+\f{align*}{
+\frac{1}{N}\sum_{i=0}^N ||\mathbf{uv}_{k,i}-\mathbf{uv}_{k-1,i}|| < \Delta d
+\f}
+
+This seems to work reasonably well, but can fail if the environment is dynamic in nature, thus it can be useful to use both the inertial and disparity-based methods together in very dynamic environments.
 
 */

ov_core/src/feat/Feature.cpp

@@ -24,7 +24,7 @@
 
 using namespace ov_core;
 
-void Feature::clean_old_measurements(std::vector<double> valid_times) {
+void Feature::clean_old_measurements(const std::vector<double> &valid_times) {
 
   // Loop through each of the cameras we have
   for (auto const &pair : timestamps) {
@@ -53,6 +53,35 @@ void Feature::clean_old_measurements(std::vector<double> valid_times) {
   }
 }
 
+void Feature::clean_invalid_measurements(const std::vector<double> &invalid_times) {
+
+  // Loop through each of the cameras we have
+  for (auto const &pair : timestamps) {
+
+    // Assert that we have all the parts of a measurement
+    assert(timestamps[pair.first].size() == uvs[pair.first].size());
+    assert(timestamps[pair.first].size() == uvs_norm[pair.first].size());
+
+    // Our iterators
+    auto it1 = timestamps[pair.first].begin();
+    auto it2 = uvs[pair.first].begin();
+    auto it3 = uvs_norm[pair.first].begin();
+
+    // Loop through measurement times, remove ones that are in our timestamps
+    while (it1 != timestamps[pair.first].end()) {
+      if (std::find(invalid_times.begin(), invalid_times.end(), *it1) != invalid_times.end()) {
+        it1 = timestamps[pair.first].erase(it1);
+        it2 = uvs[pair.first].erase(it2);
+        it3 = uvs_norm[pair.first].erase(it3);
+      } else {
+        ++it1;
+        ++it2;
+        ++it3;
+      }
+    }
+  }
+}
+
 void Feature::clean_older_measurements(double timestamp) {
 
   // Loop through each of the cameras we have
@@ -81,3 +110,4 @@ void Feature::clean_older_measurements(double timestamp) {
     }
   }
 }
+

ov_core/src/feat/Feature.h

@@ -75,7 +75,16 @@ class Feature {
    *
    * @param valid_times Vector of timestamps that our measurements must occur at
    */
-  void clean_old_measurements(std::vector<double> valid_times);
+  void clean_old_measurements(const std::vector<double> &valid_times);
+
+  /**
+   * @brief Remove measurements that occur at the invalid timestamps
+   *
+   * Given a series of invalid timestamps, this will remove all measurements that have occurred at these times.
+   *
+   * @param invalid_times Vector of timestamps that our measurements should not
+   */
+  void clean_invalid_measurements(const std::vector<double> &invalid_times);
 
   /**
    * @brief Remove measurements that are older then the specified timestamp.

ov_core/src/feat/FeatureDatabase.h

@@ -314,6 +314,31 @@ class FeatureDatabase {
     }
   }
 
+  /**
+   * @brief This function will delete all feature measurements that are at the specified timestamp
+   */
+  void cleanup_measurements_exact(double timestamp) {
+    std::unique_lock<std::mutex> lck(mtx);
+    std::vector<double> timestamps = {timestamp};
+    for (auto it = features_idlookup.begin(); it != features_idlookup.end();) {
+      // Remove the older measurements
+      (*it).second->clean_invalid_measurements(timestamps);
+      // Count how many measurements
+      int ct_meas = 0;
+      for (const auto &pair : (*it).second->timestamps) {
+        ct_meas += (*it).second->timestamps.at(pair.first).size();
+      }
+      // If delete flag is set, then delete it
+      // NOTE: if we are using a shared pointer, then no need to do this!
+      if (ct_meas < 1) {
+        // delete (*it).second;
+        features_idlookup.erase(it++);
+      } else {
+        it++;
+      }
+    }
+  }
+
   /**
    * @brief Returns the size of the feature database
    */

ov_core/src/init/InertialInitializer.cpp

@@ -41,85 +41,87 @@ bool InertialInitializer::initialize_with_imu(double &time0, Eigen::Matrix<doubl
                                               Eigen::Matrix<double, 3, 1> &p_I0inG, bool wait_for_jerk) {
 
   // Return if we don't have any measurements
-  if (imu_data.empty()) {
+  if (imu_data.size() < 2) {
     return false;
   }
 
-  // Newest imu timestamp
+  // Newest and oldest imu timestamp
   double newesttime = imu_data.at(imu_data.size() - 1).timestamp;
+  double oldesttime = imu_data.at(0).timestamp;
+
+  // Return if we don't have enough for two windows
+  if(newesttime-oldesttime < 2 * _window_length) {
+    // printf(YELLOW "[INIT-IMU]: unable to select window of IMU readings, not enough readings\n" RESET);
+    return false;
+  }
 
   // First lets collect a window of IMU readings from the newest measurement to the oldest
-  std::vector<ImuData> window_newest, window_secondnew;
+  std::vector<ImuData> window_1to0, window_2to1;
   for (const ImuData &data : imu_data) {
     if (data.timestamp > newesttime - 1 * _window_length && data.timestamp <= newesttime - 0 * _window_length) {
-      window_newest.push_back(data);
+      window_1to0.push_back(data);
     }
     if (data.timestamp > newesttime - 2 * _window_length && data.timestamp <= newesttime - 1 * _window_length) {
-      window_secondnew.push_back(data);
+      window_2to1.push_back(data);
     }
   }
 
   // Return if both of these failed
-  if (window_newest.empty() || window_secondnew.empty()) {
-    // printf(YELLOW "InertialInitializer::initialize_with_imu(): unable to select window of IMU readings, not enough readings\n" RESET);
+  if (window_1to0.empty() || window_2to1.empty()) {
+    // printf(YELLOW "[INIT-IMU]: unable to select window of IMU readings, not enough readings\n" RESET);
     return false;
   }
 
-  // Calculate the sample variance for the newest one
-  Eigen::Matrix<double, 3, 1> a_avg = Eigen::Matrix<double, 3, 1>::Zero();
-  for (const ImuData &data : window_newest) {
-    a_avg += data.am;
+  // Calculate the sample variance for the newest window from 1 to 0
+  Eigen::Vector3d a_avg_1to0 = Eigen::Vector3d::Zero();
+  for (const ImuData &data : window_1to0) {
+    a_avg_1to0 += data.am;
+  }
+  a_avg_1to0 /= (int)window_1to0.size();
+  double a_var_1to0 = 0;
+  for (const ImuData &data : window_1to0) {
+    a_var_1to0 += (data.am - a_avg_1to0).dot(data.am - a_avg_1to0);
+  }
+  a_var_1to0 = std::sqrt(a_var_1to0 / ((int)window_1to0.size() - 1));
+
+  // Calculate the sample variance for the second newest window from 2 to 1
+  Eigen::Vector3d a_avg_2to1 = Eigen::Vector3d::Zero();
+  Eigen::Vector3d w_avg_2to1 = Eigen::Vector3d::Zero();
+  for (size_t i = 0; i < window_2to1.size(); i++) {
+    a_avg_2to1 += window_2to1.at(i).am;
+    w_avg_2to1 += window_2to1.at(i).wm;
   }
-  a_avg /= (int)window_newest.size();
-  double a_var = 0;
-  for (const ImuData &data : window_newest) {
-    a_var += (data.am - a_avg).dot(data.am - a_avg);
+  a_avg_2to1 = a_avg_2to1 / window_2to1.size();
+  w_avg_2to1 = w_avg_2to1 / window_2to1.size();
+  double a_var_2to1 = 0;
+  for (const ImuData &data : window_2to1) {
+    a_var_2to1 += (data.am - a_avg_2to1).dot(data.am - a_avg_2to1);
   }
-  a_var = std::sqrt(a_var / ((int)window_newest.size() - 1));
+  a_var_2to1 = std::sqrt(a_var_2to1 / ((int)window_2to1.size() - 1));
+  //printf(BOLDGREEN "[INIT-IMU]: IMU excitation, %.4f,%.4f\n" RESET, a_var_1to0, a_var_2to1);
 
   // If it is below the threshold and we want to wait till we detect a jerk
-  if (a_var < _imu_excite_threshold && wait_for_jerk) {
-    printf(YELLOW "InertialInitializer::initialize_with_imu(): no IMU excitation, below threshold %.4f < %.4f\n" RESET, a_var,
-           _imu_excite_threshold);
+  if (a_var_1to0 < _imu_excite_threshold && wait_for_jerk) {
+    printf(YELLOW "[INIT-IMU]: no IMU excitation, below threshold %.4f < %.4f\n" RESET, a_var_1to0, _imu_excite_threshold);
     return false;
   }
 
-  // Return if we don't have any measurements
-  // if(imu_data.size() < 200) {
-  //    return false;
-  //}
-
-  // Sum up our current accelerations and velocities
-  Eigen::Vector3d linsum = Eigen::Vector3d::Zero();
-  Eigen::Vector3d angsum = Eigen::Vector3d::Zero();
-  for (size_t i = 0; i < window_secondnew.size(); i++) {
-    linsum += window_secondnew.at(i).am;
-    angsum += window_secondnew.at(i).wm;
-  }
-
-  // Calculate the mean of the linear acceleration and angular velocity
-  Eigen::Vector3d linavg = Eigen::Vector3d::Zero();
-  Eigen::Vector3d angavg = Eigen::Vector3d::Zero();
-  linavg = linsum / window_secondnew.size();
-  angavg = angsum / window_secondnew.size();
-
-  // Calculate variance of the
-  double a_var2 = 0;
-  for (const ImuData &data : window_secondnew) {
-    a_var2 += (data.am - linavg).dot(data.am - linavg);
+  // We should also check that the old state was below the threshold!
+  // This is the case when we have started up moving, and thus we need to wait for a period of stationary motion
+  if (a_var_2to1 > _imu_excite_threshold && wait_for_jerk) {
+    printf(YELLOW "[INIT-IMU]: to much IMU excitation, above threshold %.4f > %.4f\n" RESET, a_var_2to1, _imu_excite_threshold);
+    return false;
   }
-  a_var2 = std::sqrt(a_var2 / ((int)window_secondnew.size() - 1));
 
   // If it is above the threshold and we are not waiting for a jerk
   // Then we are not stationary (i.e. moving) so we should wait till we are
-  if ((a_var > _imu_excite_threshold || a_var2 > _imu_excite_threshold) && !wait_for_jerk) {
-    printf(YELLOW "InertialInitializer::initialize_with_imu(): to much IMU excitation, above threshold %.4f,%.4f > %.4f\n" RESET, a_var,
-           a_var2, _imu_excite_threshold);
+  if ((a_var_1to0 > _imu_excite_threshold || a_var_2to1 > _imu_excite_threshold) && !wait_for_jerk) {
+    printf(YELLOW "[INIT-IMU]: to much IMU excitation, above threshold %.4f,%.4f > %.4f\n" RESET, a_var_1to0, a_var_2to1, _imu_excite_threshold);
     return false;
   }
 
   // Get z axis, which alines with -g (z_in_G=0,0,1)
-  Eigen::Vector3d z_axis = linavg / linavg.norm();
+  Eigen::Vector3d z_axis = a_avg_2to1 / a_avg_2to1.norm();
 
   // Create an x_axis
   Eigen::Vector3d e_1(1, 0, 0);
@@ -141,11 +143,11 @@ bool InertialInitializer::initialize_with_imu(double &time0, Eigen::Matrix<doubl
   Eigen::Matrix<double, 4, 1> q_GtoI = rot_2_quat(Ro);
 
   // Set our biases equal to our noise (subtract our gravity from accelerometer bias)
-  Eigen::Matrix<double, 3, 1> bg = angavg;
-  Eigen::Matrix<double, 3, 1> ba = linavg - quat_2_Rot(q_GtoI) * _gravity;
+  Eigen::Matrix<double, 3, 1> bg = w_avg_2to1;
+  Eigen::Matrix<double, 3, 1> ba = a_avg_2to1 - quat_2_Rot(q_GtoI) * _gravity;
 
   // Set our state variables
-  time0 = window_secondnew.at(window_secondnew.size() - 1).timestamp;
+  time0 = window_2to1.at(window_2to1.size() - 1).timestamp;
   q_GtoI0 = q_GtoI;
   b_w0 = bg;
   v_I0inG = Eigen::Matrix<double, 3, 1>::Zero();

ov_core/src/init/InertialInitializer.h

@@ -49,12 +49,14 @@ class InertialInitializer {
 public:
   /**
    * @brief Default constructor
-   * @param gravity Gravity in the global frame of reference
+   * @param gravity_mag Global gravity magnitude of the system (normally 9.81)
    * @param window_length Amount of time we will initialize over (seconds)
    * @param imu_excite_threshold Variance threshold on our acceleration to be classified as moving
    */
-  InertialInitializer(Eigen::Matrix<double, 3, 1> gravity, double window_length, double imu_excite_threshold)
-      : _gravity(gravity), _window_length(window_length), _imu_excite_threshold(imu_excite_threshold) {}
+  InertialInitializer(double gravity_mag, double window_length, double imu_excite_threshold)
+      : _window_length(window_length), _imu_excite_threshold(imu_excite_threshold) {
+    _gravity << 0.0, 0.0, gravity_mag;
+  }
 
   /**
    * @brief Feed function for inertial data

ov_core/src/track/TrackKLT.cpp

@@ -70,6 +70,7 @@ void TrackKLT::feed_monocular(double timestamp, cv::Mat &img, size_t cam_id) {
 
   // Lets track temporally
   perform_matching(img_pyramid_last[cam_id], imgpyr, pts_last[cam_id], pts_left_new, cam_id, cam_id, mask_ll);
+  assert(pts_left_new.size()==ids_last[cam_id].size());
   rT4 = boost::posix_time::microsec_clock::local_time();
 
   //===================================================================================
@@ -310,6 +311,13 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
 void TrackKLT::perform_detection_monocular(const std::vector<cv::Mat> &img0pyr, std::vector<cv::KeyPoint> &pts0,
                                            std::vector<size_t> &ids0) {
 
+  // First compute how many more features we need to extract from this image
+  int num_featsneeded = num_features - (int)pts0.size();
+
+  // If we don't need any features, just return
+  if (num_featsneeded < 0.2 * num_features)
+    return;
+
   // Create a 2D occupancy grid for this current image
   // Note that we scale this down, so that each grid point is equal to a set of pixels
   // This means that we will reject points that less then grid_px_size points away then existing features
@@ -335,13 +343,6 @@ void TrackKLT::perform_detection_monocular(const std::vector<cv::Mat> &img0pyr,
     it2++;
   }
 
-  // First compute how many more features we need to extract from this image
-  int num_featsneeded = num_features - (int)pts0.size();
-
-  // If we don't need any features, just return
-  if (num_featsneeded < 0.2 * num_features)
-    return;
-
   // Extract our features (use fast with griding)
   std::vector<cv::KeyPoint> pts0_ext;
   Grider_FAST::perform_griding(img0pyr.at(0), pts0_ext, num_featsneeded, grid_x, grid_y, threshold, true);
@@ -380,8 +381,8 @@ void TrackKLT::perform_detection_monocular(const std::vector<cv::Mat> &img0pyr,
 }
 
 void TrackKLT::perform_detection_stereo(const std::vector<cv::Mat> &img0pyr, const std::vector<cv::Mat> &img1pyr,
-                                        std::vector<cv::KeyPoint> &pts0, std::vector<cv::KeyPoint> &pts1, std::vector<size_t> &ids0,
-                                        std::vector<size_t> &ids1) {
+                                        std::vector<cv::KeyPoint> &pts0, std::vector<cv::KeyPoint> &pts1,
+                                        std::vector<size_t> &ids0, std::vector<size_t> &ids1) {
 
   // Create a 2D occupancy grid for this current image
   // Note that we scale this down, so that each grid point is equal to a set of pixels
@@ -574,7 +575,7 @@ void TrackKLT::perform_matching(const std::vector<cv::Mat> &img0pyr, const std::
   double max_focallength_img0 = std::max(camera_k_OPENCV.at(id0)(0, 0), camera_k_OPENCV.at(id0)(1, 1));
   double max_focallength_img1 = std::max(camera_k_OPENCV.at(id1)(0, 0), camera_k_OPENCV.at(id1)(1, 1));
   double max_focallength = std::max(max_focallength_img0, max_focallength_img1);
-  cv::findFundamentalMat(pts0_n, pts1_n, cv::FM_RANSAC, 1 / max_focallength, 0.999, mask_rsc);
+  cv::findFundamentalMat(pts0_n, pts1_n, cv::FM_RANSAC, 1.0 / max_focallength, 0.999, mask_rsc);
 
   // Loop through and record only ones that are valid
   for (size_t i = 0; i < mask_klt.size(); i++) {

ov_msckf/launch/pgeneva_ros_eth.launch

@@ -44,17 +44,19 @@
         <param name="dt_slam_delay"          type="double" value="3" />
         <param name="init_window_time"       type="double" value="$(arg init_window_time)" />
         <param name="init_imu_thresh"        type="double" value="$(arg init_imu_thresh)" />
-        <rosparam param="gravity">[0.0,0.0,9.81]</rosparam>
+        <param name="gravity_mag"            type="double" value="9.81" />
         <param name="feat_rep_msckf"         type="string" value="GLOBAL_3D" />
         <param name="feat_rep_slam"          type="string" value="ANCHORED_FULL_INVERSE_DEPTH" />
         <param name="feat_rep_aruco"         type="string" value="ANCHORED_FULL_INVERSE_DEPTH" />
 
         <!-- zero velocity update parameters -->
-        <param name="try_zupt"               type="bool"   value="false" />
-        <param name="zupt_chi2_multipler"    type="int"    value="2" />
-        <param name="zupt_max_velocity"      type="double" value="0.3" />
+        <!-- inertial and disparity based detection (inertial is key for dynamic environments) -->
+        <param name="try_zupt"               type="bool"   value="true" />
+        <param name="zupt_chi2_multipler"    type="double" value="0" /> <!-- set to 0 for only disp-based -->
+        <param name="zupt_max_velocity"      type="double" value="0.1" />
         <param name="zupt_noise_multiplier"  type="double" value="50" />
-        <param name="zupt_only_at_beginning" type="bool"   value="true" />
+        <param name="zupt_max_disparity"     type="double" value="0.5" /> <!-- set to 0 for only imu-based -->
+        <param name="zupt_only_at_beginning" type="bool"   value="false" />
 
         <!-- timing statistics recording -->
         <param name="record_timing_information"   type="bool"   value="$(arg dotime)" />
@@ -66,7 +68,7 @@
         <param name="fast_threshold"     type="int"    value="15" />
         <param name="grid_x"             type="int"    value="5" />
         <param name="grid_y"             type="int"    value="3" />
-        <param name="min_px_dist"        type="int"    value="5" />
+        <param name="min_px_dist"        type="int"    value="8" />
         <param name="knn_ratio"          type="double" value="0.70" />
         <param name="downsample_cameras" type="bool"   value="false" />
         <param name="multi_threading"    type="bool"   value="true" />