more updates

astrobee/config/localization.config

@@ -60,7 +60,7 @@ teblid512_ransac_inlier_tolerance = 3
 teblid512_num_ransac_iterations = 1000
 teblid512_early_break_landmarks = 100
 teblid512_histogram_equalization = 3
-teblid512_check_essential_matrix = true 
+teblid512_check_essential_matrix = false 
 teblid512_essential_ransac_iterations = 2000
 teblid512_add_similar_images = true 
 teblid512_add_best_previous_image = true

astrobee/config/localization/graph_vio.config

@@ -48,7 +48,7 @@ gv_fa_vo_retriangulation_threshold = 1e-5
 gv_fa_vo_verbose_cheirality = false 
 ---- Spaced Feature Tracker
 gv_fa_vo_remove_undetected_feature_tracks = true 
-gv_fa_vo_measurement_spacing = 2 
+gv_fa_vo_measurement_spacing = 0 
 ---- Depth Odometry
 gv_fa_do_enabled = true 
 gv_fa_do_huber_k = world_huber_k

astrobee/config/worlds/iss.config

@@ -60,8 +60,8 @@ world_accel_sigma = 0.0005
 world_accel_bias_sigma = 0.0005
 world_gyro_bias_sigma = 0.0000035
 world_integration_variance = 0.0001
--- world_bias_acc_omega_int = 0.000001
-world_bias_acc_omega_int = 0.00015
+world_bias_acc_omega_int = 0.000001
+-- world_bias_acc_omega_int = 0.00015
 -- change to 0.000001?
 
 --------------------------

localization/localization_node/src/localization.cc

@@ -137,12 +137,12 @@ bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLa
     successes_.emplace_back(0);
     AdjustThresholds();
     // LOG(INFO) << "Failed to localize image.";
-    timer_.Stop();
+    timer_.StopAndLog();
     return false;
   }
   successes_.emplace_back(1);
   AdjustThresholds();
-  timer_.Stop();
+  timer_.StopAndLog();
 
   Eigen::Affine3d global_pose = camera.GetTransform().inverse();
   Eigen::Quaterniond quat(global_pose.rotation());

localization/sparse_mapping/scripts/make_teblid512_map.py

@@ -53,7 +53,7 @@ def make_teblid512_map(surf_map, world, robot_name, map_directory=None):
     if map_directory:
         os.chdir(map_directory)
     build_teblid512_map_command = (
-        "rosrun sparse_mapping build_map -rebuild -histogram_equalization -use_clahe -min_brisk_features 3000 -output_map "
+        "rosrun sparse_mapping build_map -rebuild -histogram_equalization -use_clahe -min_brisk_features 2000 -output_map "
         + teblid512_map_full_path
     )
     lu.run_command_and_save_output(

tools/localization_analysis/src/live_measurement_simulator.cc

@@ -92,6 +92,7 @@ LiveMeasurementSimulator::LiveMeasurementSimulator(const LiveMeasurementSimulato
 ff_msgs::Feature2dArray LiveMeasurementSimulator::GenerateOFFeatures(const sensor_msgs::ImageConstPtr& image_msg) {
   ff_msgs::Feature2dArray of_features;
   optical_flow_tracker_.OpticalFlow(image_msg, &of_features);
+  std::cout << "of feature count: " << of_features.feature_array.size();
   return of_features;
 }
 
@@ -145,11 +146,13 @@ bool LiveMeasurementSimulator::ProcessMessage() {
     // Always use ar features until have data with dock cam images
     const ff_msgs::VisualLandmarksConstPtr ar_features = msg.instantiate<ff_msgs::VisualLandmarks>();
     ar_buffer_.BufferMessage(*ar_features);
-  } else if (params_.use_bag_image_feature_msgs && string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_OF_FEATURES)) {
+  } else if (false && params_.use_bag_image_feature_msgs &&
+             string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_OF_FEATURES)) {
+    std::cout << "adding bag of msg!" << std::endl;
     const ff_msgs::Feature2dArrayConstPtr of_features = msg.instantiate<ff_msgs::Feature2dArray>();
     of_buffer_.BufferMessage(*of_features);
-  } else if (!teblid && params_.use_bag_image_feature_msgs && string_ends_with(msg.getTopic(),
-                        TOPIC_LOCALIZATION_ML_FEATURES)) {
+  } else if (!teblid && params_.use_bag_image_feature_msgs &&
+             string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_ML_FEATURES)) {
     std::cout << "adding bag vl msg!" << std::endl;
     const ff_msgs::VisualLandmarksConstPtr vl_features = msg.instantiate<ff_msgs::VisualLandmarks>();
     vl_buffer_.BufferMessage(*vl_features);
@@ -159,13 +162,17 @@ bool LiveMeasurementSimulator::ProcessMessage() {
       img_buffer_.emplace(localization_common::TimeFromHeader(image_msg->header), image_msg);
     }
     if (teblid) {  //! params_.use_bag_image_feature_msgs) {
-      // const ff_msgs::Feature2dArray of_features = GenerateOFFeatures(image_msg);
-      // of_buffer_.BufferMessage(of_features);
-
-      ff_msgs::VisualLandmarks vl_features;
-      if (GenerateVLFeatures(image_msg, vl_features)) {
-        std::cout << "generating vl feature!" << std::endl;
-        vl_buffer_.BufferMessage(vl_features);
+      if (true) {
+        std::cout << "generating of features!" << std::endl;
+        const ff_msgs::Feature2dArray of_features = GenerateOFFeatures(image_msg);
+        of_buffer_.BufferMessage(of_features);
+      }
+      if (true) {
+        ff_msgs::VisualLandmarks vl_features;
+        if (GenerateVLFeatures(image_msg, vl_features)) {
+          std::cout << "generating vl feature!" << std::endl;
+          vl_buffer_.BufferMessage(vl_features);
+        }
       }
     }
   }

tools/localization_analysis/src/map_matcher.cc

@@ -76,10 +76,12 @@ void MapMatcher::AddMapMatches() {
   topics.push_back(std::string("/") + image_topic_);
   rosbag::View view(input_bag_, rosbag::TopicQuery(topics));
   image_count_ = view.size();
+  int tmp_count = 0;
   for (const rosbag::MessageInstance msg : view) {
     if (string_ends_with(msg.getTopic(), image_topic_)) {
       sensor_msgs::ImageConstPtr image_msg = msg.instantiate<sensor_msgs::Image>();
       ff_msgs::VisualLandmarks vl_msg;
+      ++tmp_count;
       if (GenerateVLFeatures(image_msg, vl_msg)) {
         match_count_++;
         feature_averager_.Update(vl_msg.landmarks.size());
@@ -95,6 +97,10 @@ void MapMatcher::AddMapMatches() {
         const ros::Time timestamp = lc::RosTimeFromHeader(image_msg->header);
         nonloc_bag_.write(std::string("/") + image_topic_, timestamp, image_msg);
       }
+       std::stringstream ss;
+       ss << "Localized " << match_count_ << " / " << tmp_count << " images with mean of "
+          << feature_averager_.average() << " features";
+       ROS_INFO_STREAM(ss.str());
     }
   }
 }