From 6b743b6b1736a5c1795e767530a06afab47f590a Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Tue, 30 Jul 2024 11:39:42 -0700 Subject: [PATCH] tmp commits --- astrobee/config/localization/graph_vio.config | 2 +- astrobee/config/worlds/iss.config | 4 +++- .../ff_msgs/msg/VisualLandmarks.msg | 2 +- .../localization_common/src/utilities.cc | 3 +++ .../localization_node/src/localization.cc | 2 +- .../src/ros_pose_extrapolator_wrapper.cc | 2 +- .../live_measurement_simulator.h | 3 ++- .../scripts/parameter_sweeper.py | 21 ++++++++++++++++--- .../src/live_measurement_simulator.cc | 14 +++++++------ .../src/offline_replay.cc | 2 ++ 10 files changed, 40 insertions(+), 15 deletions(-) diff --git a/astrobee/config/localization/graph_vio.config b/astrobee/config/localization/graph_vio.config index 6f347bb858..68023589c7 100644 --- a/astrobee/config/localization/graph_vio.config +++ b/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 diff --git a/astrobee/config/worlds/iss.config b/astrobee/config/worlds/iss.config index 5d21853b3f..0eae756822 100755 --- a/astrobee/config/worlds/iss.config +++ b/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 -- diff --git a/communications/ff_msgs/msg/VisualLandmarks.msg b/communications/ff_msgs/msg/VisualLandmarks.msg index e5d5eeef00..507a920a4c 100644 --- a/communications/ff_msgs/msg/VisualLandmarks.msg +++ b/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 diff --git a/localization/localization_common/src/utilities.cc b/localization/localization_common/src/utilities.cc index 22a66cd65a..29fbfb36de 100644 --- a/localization/localization_common/src/utilities.cc +++ b/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"); diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc index 7ba01f1265..0c8944df50 100644 --- a/localization/localization_node/src/localization.cc +++ b/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()); diff --git a/localization/ros_pose_extrapolator/src/ros_pose_extrapolator_wrapper.cc b/localization/ros_pose_extrapolator/src/ros_pose_extrapolator_wrapper.cc index 7cd94489e2..39166c8f1d 100644 --- a/localization/ros_pose_extrapolator/src/ros_pose_extrapolator_wrapper.cc +++ b/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(imu_integrator_->size()) > 1) { + if (false && !standstill() && static_cast(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."); diff --git a/tools/localization_analysis/include/localization_analysis/live_measurement_simulator.h b/tools/localization_analysis/include/localization_analysis/live_measurement_simulator.h index f16503b5df..bfc6a8bd7c 100644 --- a/tools/localization_analysis/include/localization_analysis/live_measurement_simulator.h +++ b/tools/localization_analysis/include/localization_analysis/live_measurement_simulator.h @@ -60,6 +60,8 @@ class LiveMeasurementSimulator { boost::optional GetARMessage(const localization_common::Time current_time); boost::optional GetFlightModeMessage(const localization_common::Time current_time); + localization_node::Localizer map_feature_matcher_; + private: ff_msgs::Feature2dArray GenerateOFFeatures(const sensor_msgs::ImageConstPtr& image_msg); @@ -67,7 +69,6 @@ class LiveMeasurementSimulator { 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_; diff --git a/tools/localization_analysis/scripts/parameter_sweeper.py b/tools/localization_analysis/scripts/parameter_sweeper.py index 016ea60796..12c9e4c608 100755 --- a/tools/localization_analysis/scripts/parameter_sweeper.py +++ b/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") diff --git a/tools/localization_analysis/src/live_measurement_simulator.cc b/tools/localization_analysis/src/live_measurement_simulator.cc index 7377d53c65..6aed5203f8 100644 --- a/tools/localization_analysis/src/live_measurement_simulator.cc +++ b/tools/localization_analysis/src/live_measurement_simulator.cc @@ -41,7 +41,7 @@ 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()) { @@ -49,7 +49,7 @@ LiveMeasurementSimulator::LiveMeasurementSimulator(const LiveMeasurementSimulato exit(0); } - map_feature_matcher_.ReadParams(config); + // map_feature_matcher_.ReadParams(config); optical_flow_tracker_.ReadParams(&config); std::vector topics; topics.push_back(std::string("/") + TOPIC_HARDWARE_IMU); @@ -147,7 +147,8 @@ 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(); 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(); vl_buffer_.BufferMessage(*vl_features); } else if (string_ends_with(msg.getTopic(), kImageTopic_)) { @@ -155,12 +156,13 @@ bool LiveMeasurementSimulator::ProcessMessage() { 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); } } diff --git a/tools/localization_analysis/src/offline_replay.cc b/tools/localization_analysis/src/offline_replay.cc index 1f315016a0..64b14f98c1 100644 --- a/tools/localization_analysis/src/offline_replay.cc +++ b/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);