tmp commits

astrobee/config/localization/graph_vio.config

@@ -29,7 +29,7 @@ gv_fa_standstill_pose_between_factor_rotation_stddev = 0.01
 --- VO
 gv_fa_vo_enabled = true
 gv_fa_vo_huber_k = world_huber_k 
-gv_fa_vo_max_num_factors = 8 
+gv_fa_vo_max_num_factors = 13 
 gv_fa_vo_min_num_points_per_factor = 2 
 gv_fa_vo_max_num_points_per_factor = 6 
 gv_fa_vo_min_avg_distance_from_mean = 0.075

astrobee/config/worlds/iss.config

@@ -60,7 +60,9 @@ 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.00015
+world_bias_acc_omega_int = 0.000001
+-- world_bias_acc_omega_int = 0.00015
+-- change to 0.000001?
 
 --------------------------
 --       PHYSICS        --

communications/ff_msgs/msg/VisualLandmarks.msg

@@ -22,4 +22,4 @@ Header header # header with timestamp
 uint32 camera_id # image ID, associated with registration pulse
 geometry_msgs/Pose pose # estimated camera pose from features
 ff_msgs/VisualLandmark[] landmarks # list of all landmarks
-float32 runtime # Time it took to calculate the pose and matching landmarks
+#float32 runtime # Time it took to calculate the pose and matching landmarks

localization/localization_common/src/utilities.cc

@@ -83,9 +83,12 @@ void LoadGraphLocalizerConfig(config_reader::ConfigReader& config, const std::st
 }
 
 void LoadGraphVIOConfig(config_reader::ConfigReader& config, const std::string& path_prefix) {
+  config.AddFile((path_prefix + "localization/localization.config").c_str());
   config.AddFile((path_prefix + "localization/graph_vio.config").c_str());
   config.AddFile((path_prefix + "localization/imu_integrator.config").c_str());
   config.AddFile((path_prefix + "localization/imu_filter.config").c_str());
+  config.AddFile((path_prefix + "localization/imu_bias_initializer.config").c_str());
+  config.AddFile((path_prefix + "localization/ros_graph_vio.config").c_str());
   config.AddFile("transforms.config");
   config.AddFile("cameras.config");
   config.AddFile("geometry.config");

localization/localization_node/src/localization.cc

@@ -147,7 +147,7 @@ bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLa
   Eigen::Affine3d global_pose = camera.GetTransform().inverse();
   Eigen::Quaterniond quat(global_pose.rotation());
 
-  vl->runtime = timer_.last_value();
+  // vl->runtime = timer_.last_value();
   vl->pose.position = msg_conversions::eigen_to_ros_point(global_pose.translation());
   vl->pose.orientation = msg_conversions::eigen_to_ros_quat(quat);
   assert(landmarks.size() == observations.size());

localization/ros_pose_extrapolator/src/ros_pose_extrapolator_wrapper.cc

@@ -118,7 +118,7 @@ RosPoseExtrapolatorWrapper::LatestExtrapolatedStateAndCovariances() {
   // Extrapolate VIO data with latest IMU measurements.
   // Don't add IMU data if at standstill to avoid adding noisy IMU measurements to
   // extrapolated state. Avoid adding IMU data if too few measurements ( < 2) are in imu integrator.
-  if (!standstill() && static_cast<int>(imu_integrator_->size()) > 1) {
+  if (false && !standstill() && static_cast<int>(imu_integrator_->size()) > 1) {
     const auto latest_extrapolated_state = imu_integrator_->ExtrapolateLatest(*latest_extrapolated_vio_state_);
     if (!latest_extrapolated_state) {
       LogError("LatestExtrapolatedCombinedNavStateAndCovariances: Failed to extrapolate latest vio state.");

tools/localization_analysis/include/localization_analysis/live_measurement_simulator.h

@@ -60,14 +60,15 @@ class LiveMeasurementSimulator {
   boost::optional<ff_msgs::VisualLandmarks> GetARMessage(const localization_common::Time current_time);
   boost::optional<ff_msgs::FlightMode> GetFlightModeMessage(const localization_common::Time current_time);
 
+  localization_node::Localizer map_feature_matcher_;
+
  private:
   ff_msgs::Feature2dArray GenerateOFFeatures(const sensor_msgs::ImageConstPtr& image_msg);
 
   bool GenerateVLFeatures(const sensor_msgs::ImageConstPtr& image_msg, ff_msgs::VisualLandmarks& vl_features);
 
   rosbag::Bag bag_;
   sparse_mapping::SparseMap map_;
-  localization_node::Localizer map_feature_matcher_;
   depth_odometry::DepthOdometryWrapper depth_odometry_wrapper_;
   LiveMeasurementSimulatorParams params_;
   lk_optical_flow::LKOpticalFlow optical_flow_tracker_;

tools/localization_analysis/scripts/parameter_sweeper.py

@@ -85,6 +85,10 @@ def test_values(
     config_path = os.path.join(
         rospkg.RosPack().get_path("astrobee"), "config/localization"
     )
+    config_path2 = os.path.join(
+        rospkg.RosPack().get_path("astrobee"), "config"
+    )
+
     make_config_file(
         "graph_localizer.config", new_output_dir, config_path, values, value_names
     )
@@ -97,6 +101,17 @@ def test_values(
     make_config_file(
         "imu_filter.config", new_output_dir, config_path, values, value_names
     )
+    make_config_file(
+        "imu_bias_initializer.config", new_output_dir, config_path, values, value_names
+    )
+    make_config_file(
+        "ros_graph_vio.config", new_output_dir, config_path, values, value_names
+    )
+    make_config_file(
+        "localization.config", new_output_dir, config_path2, values, value_names
+    )
+
+
     output_bag = os.path.join(new_output_dir, "results.bag")
     output_stats_file = os.path.join(new_output_dir, "graph_stats.csv")
     graph_config_prefix = new_output_dir_prefix + "/"
@@ -215,13 +230,13 @@ def parameter_sweep(
 def make_value_ranges():
     value_ranges = []
     value_names = []
-    steps = 10
+    steps = 1
 
     # tune num smart factors
     # value_ranges.append(np.logspace(-3, -5, steps, endpoint=True))
     # value_names.append("ii_accel_bias_sigma")
-    value_ranges.append(np.logspace(-3, -1, steps, endpoint=True))
-    value_names.append("gv_fa_do_point_noise_scale")
+    value_ranges.append(np.logspace(2, 2, steps, endpoint=True))
+    value_names.append("teblid256_num_ransac_iterations")
     # value_ranges.append(np.logspace(-3, -7, steps, endpoint=True))
     # value_names.append("ii_bias_acc_omega_int")
 

tools/localization_analysis/src/live_measurement_simulator.cc

@@ -41,15 +41,15 @@ LiveMeasurementSimulator::LiveMeasurementSimulator(const LiveMeasurementSimulato
   config_reader::ConfigReader config;
   config.AddFile("cameras.config");
   config.AddFile("geometry.config");
-  config.AddFile("localization.config");
+  // config.AddFile("localization.config");
   config.AddFile("optical_flow.config");
 
   if (!config.ReadFiles()) {
     LogError("Failed to read config files.");
     exit(0);
   }
 
-  map_feature_matcher_.ReadParams(config);
+  // map_feature_matcher_.ReadParams(config);
   optical_flow_tracker_.ReadParams(&config);
   std::vector<std::string> topics;
   topics.push_back(std::string("/") + TOPIC_HARDWARE_IMU);
@@ -147,20 +147,22 @@ bool LiveMeasurementSimulator::ProcessMessage() {
   } else if (params_.use_bag_image_feature_msgs && string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_OF_FEATURES)) {
     const ff_msgs::Feature2dArrayConstPtr of_features = msg.instantiate<ff_msgs::Feature2dArray>();
     of_buffer_.BufferMessage(*of_features);
-  } else if (params_.use_bag_image_feature_msgs && string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_ML_FEATURES)) {
+  } else if (false) {  // params_.use_bag_image_feature_msgs && string_ends_with(msg.getTopic(),
+                       // TOPIC_LOCALIZATION_ML_FEATURES)) {
     const ff_msgs::VisualLandmarksConstPtr vl_features = msg.instantiate<ff_msgs::VisualLandmarks>();
     vl_buffer_.BufferMessage(*vl_features);
   } else if (string_ends_with(msg.getTopic(), kImageTopic_)) {
     sensor_msgs::ImageConstPtr image_msg = msg.instantiate<sensor_msgs::Image>();
     if (params_.save_optical_flow_images) {
       img_buffer_.emplace(localization_common::TimeFromHeader(image_msg->header), image_msg);
     }
-    if (!params_.use_bag_image_feature_msgs) {
-      const ff_msgs::Feature2dArray of_features = GenerateOFFeatures(image_msg);
-      of_buffer_.BufferMessage(of_features);
+    if (true) {  //! 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);
       }
     }

tools/localization_analysis/src/offline_replay.cc

@@ -67,6 +67,8 @@ OfflineReplay::OfflineReplay(const std::string& bag_name, const std::string& map
   // TODO(rsoussan): clean this up
   params.use_bag_image_feature_msgs = use_bag_image_feature_msgs;
   live_measurement_simulator_.reset(new LiveMeasurementSimulator(params));
+  live_measurement_simulator_->map_feature_matcher_.ReadParams(config);
+
 
   GraphLocalizerSimulatorParams graph_loc_params;
   LoadGraphLocalizerSimulatorParams(config, graph_loc_params);