use incremential triangulation for our active feature topic

ov_core/src/track/TrackBase.cpp

@@ -94,8 +94,8 @@ void TrackBase::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2,
       cv::circle(img_temp, pt_l, (is_small) ? 1 : 2, cv::Scalar(r1, g1, b1), cv::FILLED);
       // cv::putText(img_out, std::to_string(ids_left_last.at(i)), pt_l, cv::FONT_HERSHEY_SIMPLEX,0.5,cv::Scalar(0,0,255),1,cv::LINE_AA);
       // Draw rectangle around the point
-      cv::Point2f pt_l_top = cv::Point2f(pt_l.x - 5, pt_l.y - 5);
-      cv::Point2f pt_l_bot = cv::Point2f(pt_l.x + 5, pt_l.y + 5);
+      cv::Point2f pt_l_top = cv::Point2f(pt_l.x - 3, pt_l.y - 3);
+      cv::Point2f pt_l_bot = cv::Point2f(pt_l.x + 3, pt_l.y + 3);
       cv::rectangle(img_temp, pt_l_top, pt_l_bot, cv::Scalar(r2, g2, b2), 1);
     }
     // Draw what camera this is

ov_core/src/track/TrackBase.h

@@ -133,6 +133,18 @@ class TrackBase {
    */
   void change_feat_id(size_t id_old, size_t id_new);
 
+  /// Getter method for active features in the last frame (observations per camera)
+  std::unordered_map<size_t, std::vector<cv::KeyPoint>> get_last_obs() {
+    std::lock_guard<std::mutex> lckv(mtx_last_vars);
+    return pts_last;
+  }
+
+  /// Getter method for active features in the last frame (ids per camera)
+  std::unordered_map<size_t, std::vector<size_t>> get_last_ids() {
+    std::lock_guard<std::mutex> lckv(mtx_last_vars);
+    return ids_last;
+  }
+
   /// Getter method for number of active features
   int get_num_features() { return num_features; }
 

ov_msckf/launch/serial.launch

@@ -9,8 +9,8 @@
     <arg name="max_cameras" default="2" />
     <arg name="use_stereo"  default="true" />
     <arg name="bag_start"   default="0" /> <!-- v1-2: 0, mh1: 40, mh2: 35, mh3: 17.5, mh4-5: 15 -->
-    <arg name="dataset"     default="V1_02_medium" /> <!-- V1_01_easy, V1_02_medium, V1_03_difficult, V2_02_medium, dataset-room1_512_16 -->
-    <arg name="bag"         default="/media/patrick/RPNG_DATA_6/$(arg config)/$(arg dataset).bag" />
+    <arg name="dataset"     default="V2_02_medium" /> <!-- V1_01_easy, V1_02_medium, V1_03_difficult, V2_02_medium, dataset-room1_512_16 -->
+    <arg name="bag"         default="/media/patrick/RPNG FLASH 3/$(arg config)/$(arg dataset).bag" />
     <!-- <arg name="bag"         default="/home/patrick/datasets/$(arg config)/$(arg dataset).bag" /> -->
     <!-- <arg name="bag"         default="/datasets/$(arg config)/$(arg dataset).bag" /> -->
 

ov_msckf/src/core/VioManager.cpp

@@ -116,7 +116,6 @@ VioManager::VioManager(VioManagerOptions &params_) : thread_init_running(false),
   // Let's make a feature extractor
   // NOTE: after we initialize we will increase the total number of feature tracks
   // NOTE: we will split the total number of features over all cameras uniformly
-  trackDATABASE = std::make_shared<FeatureDatabase>();
   int init_max_features = std::floor((double)params.init_options.init_max_features / (double)params.state_options.num_cameras);
   if (params.use_klt) {
     trackFEATS = std::shared_ptr<TrackBase>(new TrackKLT(state->_cam_intrinsics_cameras, init_max_features,
@@ -205,9 +204,6 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector
 
   // Feed our simulation tracker
   trackSIM->feed_measurement_simulation(timestamp, camids, feats);
-  if (is_initialized_vio) {
-    trackDATABASE->append_new_measurements(trackSIM->get_feature_database());
-  }
   rT2 = boost::posix_time::microsec_clock::local_time();
 
   // Check if we should do zero-velocity, if so update the state with it
@@ -220,7 +216,6 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector
     }
     if (did_zupt_update) {
       assert_r(state->_timestamp == timestamp);
-      trackDATABASE->cleanup_measurements(timestamp);
       propagator->clean_old_imu_measurements(timestamp + state->_calib_dt_CAMtoIMU->value()(0) - 0.10);
       updaterZUPT->clean_old_imu_measurements(timestamp + state->_calib_dt_CAMtoIMU->value()(0) - 0.10);
       return;
@@ -274,16 +269,12 @@ void VioManager::track_image_and_update(const ov_core::CameraData &message_const
 
   // Perform our feature tracking!
   trackFEATS->feed_new_camera(message);
-  if (is_initialized_vio) {
-    trackDATABASE->append_new_measurements(trackFEATS->get_feature_database());
-  }
 
   // If the aruco tracker is available, the also pass to it
   // NOTE: binocular tracking for aruco doesn't make sense as we by default have the ids
   // NOTE: thus we just call the stereo tracking if we are doing binocular!
   if (is_initialized_vio && trackARUCO != nullptr) {
     trackARUCO->feed_new_camera(message);
-    trackDATABASE->append_new_measurements(trackARUCO->get_feature_database());
   }
   rT2 = boost::posix_time::microsec_clock::local_time();
 
@@ -297,7 +288,6 @@ void VioManager::track_image_and_update(const ov_core::CameraData &message_const
     }
     if (did_zupt_update) {
       assert_r(state->_timestamp == message.timestamp);
-      trackDATABASE->cleanup_measurements(message.timestamp);
       propagator->clean_old_imu_measurements(message.timestamp + state->_calib_dt_CAMtoIMU->value()(0) - 0.10);
       updaterZUPT->clean_old_imu_measurements(message.timestamp + state->_calib_dt_CAMtoIMU->value()(0) - 0.10);
       return;
@@ -584,7 +574,6 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message)
   // Cleanup any features older than the marginalization time
   if ((int)state->_clones_IMU.size() > state->_options.max_clone_size) {
     trackFEATS->get_feature_database()->cleanup_measurements(state->margtimestep());
-    trackDATABASE->cleanup_measurements(state->margtimestep());
     if (trackARUCO != nullptr) {
       trackARUCO->get_feature_database()->cleanup_measurements(state->margtimestep());
     }

ov_msckf/src/core/VioManager.h

@@ -185,9 +185,6 @@ class VioManager {
   /// Propagator of our state
   std::shared_ptr<Propagator> propagator;
 
-  /// Complete history of our feature tracks
-  std::shared_ptr<ov_core::FeatureDatabase> trackDATABASE;
-
   /// Our sparse feature tracker (klt or descriptor)
   std::shared_ptr<ov_core::TrackBase> trackFEATS;
 
@@ -239,10 +236,14 @@ class VioManager {
   std::shared_ptr<ov_core::FeatureInitializer> active_tracks_initializer;
 
   // Re-triangulated features 3d positions seen from the current frame (used in visualization)
+  // For each feature we have a linear system A * p_FinG = b we create and increment their costs
   double active_tracks_time = -1;
   std::unordered_map<size_t, Eigen::Vector3d> active_tracks_posinG;
   std::unordered_map<size_t, Eigen::Vector3d> active_tracks_uvd;
   cv::Mat active_image;
+  std::map<size_t, Eigen::Matrix3d> active_feat_linsys_A;
+  std::map<size_t, Eigen::Vector3d> active_feat_linsys_b;
+  std::map<size_t, int> active_feat_linsys_count;
 };
 
 } // namespace ov_msckf

ov_msckf/src/core/VioManagerHelper.cpp

@@ -190,109 +190,116 @@ bool VioManager::try_to_initialize(const ov_core::CameraData &message) {
 void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message) {
 
   // Start timing
-  boost::posix_time::ptime retri_rT1, retri_rT2, retri_rT3, retri_rT4, retri_rT5;
+  boost::posix_time::ptime retri_rT1, retri_rT2, retri_rT3;
   retri_rT1 = boost::posix_time::microsec_clock::local_time();
 
   // Clear old active track data
   active_tracks_time = state->_timestamp;
-  active_image = message.images.at(0).clone();
+  active_image = cv::Mat();
+  trackFEATS->display_active(active_image, 255, 255, 255, 255, 255, 255, " ");
+  if (!active_image.empty()) {
+    active_image = active_image(cv::Rect(0, 0, message.images.at(0).cols, message.images.at(0).rows));
+  }
   active_tracks_posinG.clear();
   active_tracks_uvd.clear();
 
-  // Get all features which are tracked in the current frame
-  // NOTE: This database should have all features from all trackers already in it
-  // NOTE: it also has the complete history so we shouldn't see jumps from deleting measurements
-  std::vector<std::shared_ptr<Feature>> active_features = trackDATABASE->features_containing_older(state->_timestamp);
-
-  // 0. Get all timestamps our clones are at (and thus valid measurement times)
-  std::vector<double> clonetimes;
-  for (const auto &clone_imu : state->_clones_IMU) {
-    clonetimes.emplace_back(clone_imu.first);
-  }
-
-  // 1. Clean all feature measurements and make sure they all have valid clone times
-  //    Also remove any that we are unable to triangulate (due to not having enough measurements)
-  auto it0 = active_features.begin();
-  while (it0 != active_features.end()) {
-
-    // Skip if it is a SLAM feature since it already is already going to be added
-    if (state->_features_SLAM.find((*it0)->featid) != state->_features_SLAM.end()) {
-      it0 = active_features.erase(it0);
-      continue;
-    }
-
-    // Clean the feature
-    (*it0)->clean_old_measurements(clonetimes);
-
-    // Count how many measurements
-    int ct_meas = 0;
-    for (const auto &pair : (*it0)->timestamps) {
-      ct_meas += (*it0)->timestamps[pair.first].size();
-    }
-
-    // Remove if we don't have enough and am not a SLAM feature which doesn't need triangulation
-    if (ct_meas < (int)std::max(4.0, std::floor(state->_options.max_clone_size * 2.0 / 5.0))) {
-      it0 = active_features.erase(it0);
-    } else {
-      it0++;
-    }
-  }
-  retri_rT2 = boost::posix_time::microsec_clock::local_time();
-
-  // Return if no features
-  if (active_features.empty() && state->_features_SLAM.empty())
-    return;
-
-  // 2. Create vector of cloned *CAMERA* poses at each of our clone timesteps
-  std::unordered_map<size_t, std::unordered_map<double, FeatureInitializer::ClonePose>> clones_cam;
-  for (const auto &clone_calib : state->_calib_IMUtoCAM) {
-
-    // For this camera, create the vector of camera poses
-    std::unordered_map<double, FeatureInitializer::ClonePose> clones_cami;
-    for (const auto &clone_imu : state->_clones_IMU) {
+  // Current active tracks in our frontend
+  // TODO: should probably assert here that these are at the message time...
+  auto last_obs = trackFEATS->get_last_obs();
+  auto last_ids = trackFEATS->get_last_ids();
+
+  // New set of linear systems that only contain the latest track info
+  std::map<size_t, Eigen::Matrix3d> active_feat_linsys_A_new;
+  std::map<size_t, Eigen::Vector3d> active_feat_linsys_b_new;
+  std::map<size_t, int> active_feat_linsys_count_new;
+
+  // Append our new observations for each camera
+  std::map<size_t, cv::Point2f> feat_uvs_in_cam0;
+  for (auto const &cam_id : message.sensor_ids) {
+
+    // IMU historical clone
+    Eigen::Matrix3d R_GtoI = state->_clones_IMU.at(state->_timestamp)->Rot();
+    Eigen::Vector3d p_IinG = state->_clones_IMU.at(state->_timestamp)->pos();
+
+    // Calibration for this cam_id
+    Eigen::Matrix3d R_ItoC = state->_calib_IMUtoCAM.at(cam_id)->Rot();
+    Eigen::Vector3d p_IinC = state->_calib_IMUtoCAM.at(cam_id)->pos();
+
+    // Convert current CAMERA position relative to global
+    Eigen::Matrix3d R_GtoCi = R_ItoC * R_GtoI;
+    Eigen::Vector3d p_CiinG = p_IinG - R_GtoCi.transpose() * p_IinC;
+
+    // Loop through each measurement
+    assert_r(last_obs.find(cam_id) != last_obs.end());
+    assert_r(last_ids.find(cam_id) != last_ids.end());
+    for (size_t i = 0; i < last_obs.at(cam_id).size(); i++) {
+
+      // Get the UV coordinate normal
+      cv::Point2f pt_d = last_obs.at(cam_id).at(i).pt;
+      cv::Point2f pt_n = state->_cam_intrinsics_cameras.at(cam_id)->undistort_cv(pt_d);
+      Eigen::Matrix<double, 3, 1> b_i;
+      b_i << pt_n.x, pt_n.y, 1;
+      b_i = R_GtoCi.transpose() * b_i;
+      b_i = b_i / b_i.norm();
+      Eigen::Matrix3d Bperp = skew_x(b_i);
+
+      // Record this feature uv if is seen from cam0
+      size_t featid = last_ids.at(cam_id).at(i);
+      if (cam_id == 0) {
+        feat_uvs_in_cam0[featid] = pt_d;
+      }
 
-      // Get current camera pose
-      Eigen::Matrix3d R_GtoCi = clone_calib.second->Rot() * clone_imu.second->Rot();
-      Eigen::Vector3d p_CioinG = clone_imu.second->pos() - R_GtoCi.transpose() * clone_calib.second->pos();
+      // Append to our linear system
+      Eigen::Matrix3d Ai = Bperp.transpose() * Bperp;
+      Eigen::Vector3d bi = Ai * p_CiinG;
+      if (active_feat_linsys_A.find(featid) == active_feat_linsys_A.end()) {
+        active_feat_linsys_A_new.insert({featid, Ai});
+        active_feat_linsys_b_new.insert({featid, bi});
+        active_feat_linsys_count_new.insert({featid, 1});
+      } else {
+        active_feat_linsys_A_new[featid] = Ai + active_feat_linsys_A[featid];
+        active_feat_linsys_b_new[featid] = bi + active_feat_linsys_b[featid];
+        active_feat_linsys_count_new[featid] = 1 + active_feat_linsys_count[featid];
+      }
 
-      // Append to our map
-      clones_cami.insert({clone_imu.first, FeatureInitializer::ClonePose(R_GtoCi, p_CioinG)});
+      // For this feature, recover its 3d position if we have enough observations!
+      if (active_feat_linsys_count_new.at(featid) > 3) {
+
+        // Recover feature estimate
+        Eigen::Matrix3d A = active_feat_linsys_A_new[featid];
+        Eigen::Vector3d b = active_feat_linsys_b_new[featid];
+        Eigen::MatrixXd p_FinG = A.colPivHouseholderQr().solve(b);
+        Eigen::MatrixXd p_FinCi = R_GtoCi * (p_FinG - p_CiinG);
+
+        // Check A and p_FinCi
+        Eigen::JacobiSVD<Eigen::Matrix3d> svd(A);
+        Eigen::MatrixXd singularValues;
+        singularValues.resize(svd.singularValues().rows(), 1);
+        singularValues = svd.singularValues();
+        double condA = singularValues(0, 0) / singularValues(singularValues.rows() - 1, 0);
+
+        // If we have a bad condition number, or it is too close
+        // Then set the flag for bad (i.e. set z-axis to nan)
+        if (std::abs(condA) <= params.featinit_options.max_cond_number && p_FinCi(2, 0) >= params.featinit_options.min_dist &&
+            p_FinCi(2, 0) <= params.featinit_options.max_dist && !std::isnan(p_FinCi.norm())) {
+          active_tracks_posinG[featid] = p_FinG;
+        }
+      }
     }
-
-    // Append to our map
-    clones_cam.insert({clone_calib.first, clones_cami});
   }
-  retri_rT3 = boost::posix_time::microsec_clock::local_time();
-
-  // 3. Try to triangulate all features that have measurements
-  auto it1 = active_features.begin();
-  while (it1 != active_features.end()) {
-
-    // Triangulate the feature and remove if it fails
-    bool success_tri = true;
-    if (active_tracks_initializer->config().triangulate_1d) {
-      success_tri = active_tracks_initializer->single_triangulation_1d(*it1, clones_cam);
-    } else {
-      success_tri = active_tracks_initializer->single_triangulation(*it1, clones_cam);
-    }
+  size_t total_triangulated = active_tracks_posinG.size();
 
-    // Remove the feature if not a success
-    if (!success_tri) {
-      it1 = active_features.erase(it1);
-      continue;
-    }
-    it1++;
-  }
-  retri_rT4 = boost::posix_time::microsec_clock::local_time();
+  // Update active set of linear systems
+  active_feat_linsys_A = active_feat_linsys_A_new;
+  active_feat_linsys_b = active_feat_linsys_b_new;
+  active_feat_linsys_count = active_feat_linsys_count_new;
+  retri_rT2 = boost::posix_time::microsec_clock::local_time();
 
   // Return if no features
-  if (active_features.empty() && state->_features_SLAM.empty())
+  if (active_tracks_posinG.empty() && state->_features_SLAM.empty())
     return;
 
-  // Points which we have in the global frame
-  for (const auto &feat : active_features) {
-    active_tracks_posinG[feat->featid] = feat->p_FinG;
-  }
+  // Append our SLAM features we have
   for (const auto &feat : state->_features_SLAM) {
     Eigen::Vector3d p_FinG = feat.second->get_xyz(false);
     if (LandmarkRepresentation::is_relative_representation(feat.second->_feat_representation)) {
@@ -325,13 +332,24 @@ void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message)
   //    We also will project the features into the current frame
   for (const auto &feat : active_tracks_posinG) {
 
-    // Project the current feature into the current frame of reference
+    // For now skip features not seen from current frame
+    // TODO: should we publish other features not tracked in cam0??
+    if (feat_uvs_in_cam0.find(feat.first) == feat_uvs_in_cam0.end())
+      continue;
+
+    // Calculate the depth of the feature in the current frame
+    // Project SLAM feature and non-cam0 features into the current frame of reference
     Eigen::Vector3d p_FinIi = R_GtoIi * (feat.second - p_IiinG);
     Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC;
     double depth = p_FinCi(2);
-    Eigen::Vector2d uv_norm, uv_dist;
-    uv_norm << p_FinCi(0) / depth, p_FinCi(1) / depth;
-    uv_dist = state->_cam_intrinsics_cameras.at(0)->distort_d(uv_norm);
+    Eigen::Vector2d uv_dist;
+    if (feat_uvs_in_cam0.find(feat.first) != feat_uvs_in_cam0.end()) {
+      uv_dist << (double)feat_uvs_in_cam0.at(feat.first).x, (double)feat_uvs_in_cam0.at(feat.first).y;
+    } else {
+      Eigen::Vector2d uv_norm;
+      uv_norm << p_FinCi(0) / depth, p_FinCi(1) / depth;
+      uv_dist = state->_cam_intrinsics_cameras.at(0)->distort_d(uv_norm);
+    }
 
     // Skip if not valid (i.e. negative depth, or outside of image)
     if (depth < 0.1) {
@@ -351,14 +369,13 @@ void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message)
     uvd << uv_dist, depth;
     active_tracks_uvd.insert({feat.first, uvd});
   }
-  retri_rT5 = boost::posix_time::microsec_clock::local_time();
+  retri_rT3 = boost::posix_time::microsec_clock::local_time();
 
   // Timing information
-  // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for cleaning\n" RESET, (retri_rT2-retri_rT1).total_microseconds() * 1e-6);
-  // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for triangulate setup\n" RESET, (retri_rT3-retri_rT2).total_microseconds() * 1e-6);
-  // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for triangulation\n" RESET, (retri_rT4-retri_rT3).total_microseconds() * 1e-6);
-  // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for re-projection\n" RESET, (retri_rT5-retri_rT4).total_microseconds() * 1e-6);
-  // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds total\n" RESET, (retri_rT5-retri_rT1).total_microseconds() * 1e-6);
+  PRINT_ALL(CYAN "[RETRI-TIME]: %.4f seconds for triangulation (%zu tri of %zu active)\n" RESET,
+            (retri_rT2 - retri_rT1).total_microseconds() * 1e-6, total_triangulated, active_feat_linsys_A.size());
+  PRINT_ALL(CYAN "[RETRI-TIME]: %.4f seconds for re-projection into current\n" RESET, (retri_rT3 - retri_rT2).total_microseconds() * 1e-6);
+  PRINT_ALL(CYAN "[RETRI-TIME]: %.4f seconds total\n" RESET, (retri_rT3 - retri_rT1).total_microseconds() * 1e-6);
 }
 
 cv::Mat VioManager::get_historical_viz_image() {

ov_msckf/src/ros/ROS1Visualizer.cpp

@@ -886,9 +886,8 @@ void ROS1Visualizer::publish_loopclosure_information() {
 
     // Create the images we will populate with the depths
     std::pair<int, int> wh_pair = {active_cam0_image.cols, active_cam0_image.rows};
-    cv::Mat depthmap_viz;
-    cv::cvtColor(active_cam0_image, depthmap_viz, cv::COLOR_GRAY2RGB);
     cv::Mat depthmap = cv::Mat::zeros(wh_pair.second, wh_pair.first, CV_16UC1);
+    cv::Mat depthmap_viz = active_cam0_image;
 
     // Loop through all points and append
     for (const auto &feattimes : active_tracks_uvd) {
@@ -898,7 +897,7 @@ void ROS1Visualizer::publish_loopclosure_information() {
       Eigen::Vector3d uvd = active_tracks_uvd.at(featid);
 
       // Skip invalid points
-      double dw = 3;
+      double dw = 4;
       if (uvd(0) < dw || uvd(0) > wh_pair.first - dw || uvd(1) < dw || uvd(1) > wh_pair.second - dw) {
         continue;
       }