diff --git a/astrobee/config/gnc.config b/astrobee/config/gnc.config index 805c472bd9..408f444bdd 100644 --- a/astrobee/config/gnc.config +++ b/astrobee/config/gnc.config @@ -18,6 +18,8 @@ require "common_lua" require "geometry" +-- Velocity Feedforward Gain +tun_vel_gain = 0.5; -- Accel Feedforward Gains tun_accel_gain = vec3(1.0, 1.0, 1.0); tun_alpha_gain = vec3(1.0, 1.0, 1.0); diff --git a/astrobee/config/graph_localizer.config b/astrobee/config/graph_localizer.config deleted file mode 100644 index a999c4ecf0..0000000000 --- a/astrobee/config/graph_localizer.config +++ /dev/null @@ -1,196 +0,0 @@ --- Copyright (c) 2017, United States Government, as represented by the --- Administrator of the National Aeronautics and Space Administration. --- --- All rights reserved. --- --- The Astrobee platform is licensed under the Apache License, Version 2.0 --- (the "License"); you may not use this file except in compliance with the --- License. You may obtain a copy of the License at --- --- http://www.apache.org/licenses/LICENSE-2.0 --- --- Unless required by applicable law or agreed to in writing, software --- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT --- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the --- License for the specific language governing permissions and limitations --- under the License. -require "context" - --- Graph Value Options -ideal_duration = 3.25 --- Don't leave less than min_num_states in window if possible -min_num_states = 3 -max_num_states = 20 -min_num_factors_per_feature = 2 --- Graph Options -max_iterations = 4 --- cholesky (faster but less robust) or qr (slower but more robust) -marginals_factorization = "qr" --- 62.5 measurements per second -num_bias_estimation_measurements = 100 -limit_imu_factor_spacing = false -max_imu_factor_spacing = 0.1 -add_priors = true -add_marginal_factors = false -loc_nav_cam_noise_stddev = 0.1 -optical_flow_nav_cam_noise_stddev = 0.1 -loc_dock_cam_noise_stddev = 0.1 -huber_k = world_huber_k --- Only applicable for non zero gravity, causes gravity to be detected as an accelerometer bias instead of --- subtracted from accelerometer measurements which requires the use of global orientation estimates. --- Only works when robot is always gravity aligned. Recommended when testing in the lab. -ignore_gravity = true -estimate_world_T_dock_using_loc = true --- Applies to factor adders but not standstill detection --- Sanity Checker options --- TODO(rsoussan): Create seperate config for sanity checker? -check_pose_difference = true -num_consecutive_pose_difference_failures_until_insane = 3 -max_sane_position_difference = 1.5 -check_position_covariance = false -check_orientation_covariance = false -position_covariance_threshold = 0 -orientation_covariance_threshold = 0 --- Starting Prior Noise -starting_prior_translation_stddev = 0.02 -starting_prior_quaternion_stddev = 0.01 -starting_prior_velocity_stddev = 0.01 -starting_prior_accel_bias_stddev = 0.001 -starting_prior_gyro_bias_stddev = 0.001 --- Threshold Bias Uncertainies -threshold_bias_uncertainty = true -accel_bias_stddev_threshold = 0.025 -gyro_bias_stddev_threshold = 0.025 --- Lost Threshold -position_cov_log_det_lost_threshold = 0 -orientation_cov_log_det_lost_threshold = 0 --- Feature Tracker -feature_tracker_sliding_window_duration = 3.25 --- Standstill -max_standstill_feature_track_avg_distance_from_mean = 0.075 -standstill_min_num_points_per_track = 4 -standstill_feature_track_duration = 1 -standstill_adder_add_velocity_prior = true -standstill_adder_add_pose_between_factor = true -standstill_adder_prior_velocity_stddev = 0.01 -standstill_adder_pose_between_factor_translation_stddev = 0.01 -standstill_adder_pose_between_factor_rotation_stddev = 0.01 --- Optical Flow Smart Factors -smart_projection_adder_enabled = true --- Gives min threshold for distance traveled for optical flow tracks to avoid degenerate cases with no/too small of baseline -smart_projection_adder_min_avg_distance_from_mean = 0.075 -smart_projection_adder_enable_EPI = false -smart_projection_adder_landmark_distance_threshold = 500 -smart_projection_adder_dynamic_outlier_rejection_threshold = 50 -smart_projection_adder_retriangulation_threshold = 1e-5 -smart_projection_adder_verbose_cheirality = false -smart_projection_adder_robust = true -smart_projection_adder_max_num_factors = 13 -smart_projection_adder_min_num_points = 2 -smart_projection_adder_max_num_points_per_factor = 7 -smart_projection_adder_measurement_spacing = 2 -smart_projection_adder_feature_track_min_separation = 0 -smart_projection_adder_rotation_only_fallback = true -smart_projection_adder_splitting = true -smart_projection_adder_scale_noise_with_num_points = true -smart_projection_adder_noise_scale = 2 -smart_projection_adder_use_allowed_timestamps = true --- Optical Flow Projection Factors -projection_adder_enabled = false -projection_adder_enable_EPI = false -projection_adder_landmark_distance_threshold = 50 -projection_adder_dynamic_outlier_rejection_threshold = 5 -projection_adder_max_num_features = 7 -projection_adder_min_num_measurements_for_triangulation = 3 -projection_adder_add_point_priors = true -projection_adder_point_prior_translation_stddev = 0.1 --- Loc Factors -loc_adder_add_pose_priors = false -loc_adder_add_projections = true -loc_adder_prior_translation_stddev = 0.06 -loc_adder_prior_quaternion_stddev = 0.06 -loc_adder_min_num_matches = 5 -loc_adder_max_num_factors = 50 -loc_adder_scale_pose_noise_with_num_landmarks = true -loc_adder_scale_projection_noise_with_num_landmarks = false -loc_adder_pose_noise_scale = 7 --- Change to 9 if scale with inverse distance -loc_adder_projection_noise_scale = 60 -loc_adder_max_inlier_weighted_projection_norm = 30 -loc_adder_weight_projections_with_distance = false -loc_adder_add_prior_if_projections_fail = true --- AR Tag Loc Projection Factors -ar_tag_loc_adder_add_pose_priors = false -ar_tag_loc_adder_add_projections = true -ar_tag_loc_adder_prior_translation_stddev = 0.01 -ar_tag_loc_adder_prior_quaternion_stddev = 0.01 -ar_tag_loc_adder_min_num_matches = 4 -ar_tag_loc_adder_max_num_factors = 50 -ar_tag_loc_adder_scale_pose_noise_with_num_landmarks = false -ar_tag_loc_adder_scale_projection_noise_with_num_landmarks = false -ar_tag_loc_adder_pose_noise_scale = 1 -ar_tag_loc_adder_projection_noise_scale = 30 -ar_tag_loc_adder_max_inlier_weighted_projection_norm = 5 -ar_tag_loc_adder_weight_projections_with_distance = false -ar_tag_loc_adder_add_prior_if_projections_fail = true --- Rotation Factor -rotation_adder_enabled = false -rotation_adder_min_avg_disparity = 0.2 -rotation_adder_rotation_stddev = 0.1 -rotation_adder_max_percent_outliers = 0.13 --- Handrail Factor -handrail_adder_enabled = true -handrail_adder_min_num_line_matches = 1 -handrail_adder_min_num_plane_matches = 1 -handrail_adder_point_to_line_stddev = 0.1 -handrail_adder_point_to_plane_stddev = 0.1 -handrail_adder_use_silu_for_point_to_line_segment_factor = false --- Depth Odometry Factor -depth_odometry_adder_enabled = true -depth_odometry_adder_noise_scale = 10 -depth_odometry_adder_use_points_between_factor = false -depth_odometry_adder_point_to_point_error_threshold = 10.0 -depth_odometry_adder_position_covariance_threshold = 1 -depth_odometry_adder_orientation_covariance_threshold = 1 -depth_odometry_adder_pose_translation_norm_threshold = 3.0 -depth_odometry_adder_max_num_points_between_factors = 10 --- IMU integration --- none, ButterOxSxLpxNx, where O is order, S is sample rate, Lp is low pass cutoff, N is notch --- Note that notch depends on the sample rate and aliasing -imu_filter_quiet_accel = "ButterO3S125Lp3N33_33" -imu_filter_quiet_ang_vel = "ButterO1S125Lp3N33_33" -imu_filter_nominal_accel = "ButterO3S125Lp3N41_66" -imu_filter_nominal_ang_vel = "ButterO1S125Lp3N41_66" -imu_filter_aggressive_accel = "ButterO3S125Lp3N46_66" -imu_filter_aggressive_ang_vel = "ButterO1S125Lp3N46_66" -imu_augmentor_standstill = true -gyro_sigma = world_gyro_sigma -accel_sigma = world_accel_sigma -accel_bias_sigma = world_accel_bias_sigma -gyro_bias_sigma = world_gyro_bias_sigma -integration_variance = world_integration_variance -bias_acc_omega_int = world_bias_acc_omega_int --- Graph Localizer Nodelet --- Buffer max should be at least 10 and a max of 2 seconds of data --- Imu ~62.5 Hz -max_imu_buffer_size = 125 --- OF ~10 Hz -max_optical_flow_buffer_size = 20 --- Vl ~1 Hz -max_vl_buffer_size = 10 --- AR ~1 Hz -max_ar_buffer_size = 10 --- Depth Odometry ~2 Hz -max_depth_odometry_buffer_size = 10 --- DL ~1 Hz -max_dl_buffer_size = 10 --- Other -verbose = false -fatal_failures = true -log_on_destruction = true -print_factor_info = false -use_ceres_params = false -publish_localization_graph = false -save_localization_graph_dot_file = false -log_rate = 10 -imu_bias_file = resolve_resource("imu_bias.config"); diff --git a/astrobee/config/localization/depth_odometry.config b/astrobee/config/localization/depth_odometry.config index be72977dbb..68fa53037f 100644 --- a/astrobee/config/localization/depth_odometry.config +++ b/astrobee/config/localization/depth_odometry.config @@ -32,6 +32,13 @@ orientation_covariance_threshold = 100 -- icp or image_feature depth_odometry_method = "image_feature" +-- Buffer size for images and point clouds +max_buffer_size = 5 + +-- Max number of depth images to create in a single iteration +-- Limits depth odometry runtime +max_depth_images = 2 + -- ICP options -- Search radius for each point in ICP @@ -122,9 +129,18 @@ clahe_clip_limit = 40 -- Other min_x_distance_to_border = 10 min_y_distance_to_border = 10 -min_num_inliers = 5 +min_num_correspondences = 5 +-- Don't compute relative pose and just return 3D point correspondences +-- Only applicable for image_feature approach +only_correspondences = true -- Refine estimate with PointToPlaneICP refine_estimate = true +-- Filter outliers using essential matrix estimation +filter_outliers = false +-- ransac, lmeds, rho (rho is actual prosac) +filter_method = "rho" +inlier_threshold = 1.0 +inlier_probability = 0.999 -- Point cloud with known correspondences aligner pcwkca_max_num_iterations = 100 diff --git a/astrobee/config/localization/graph_localizer.config b/astrobee/config/localization/graph_localizer.config new file mode 100644 index 0000000000..8e321984ee --- /dev/null +++ b/astrobee/config/localization/graph_localizer.config @@ -0,0 +1,82 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. +require "context" + +-- Graph Localizer (gl) +-- Factor Adders (fa) +--- Loc +---- Sparse Map (sm) +gl_fa_loc_sm_enabled = true +gl_fa_loc_sm_huber_k = world_huber_k +gl_fa_loc_sm_add_pose_priors = false +gl_fa_loc_sm_add_projection_factors = true +gl_fa_loc_sm_add_prior_if_projection_factors_fail = true +gl_fa_loc_sm_prior_translation_stddev = 0.06 +gl_fa_loc_sm_prior_quaternion_stddev = 0.06 +gl_fa_loc_sm_scale_pose_noise_with_num_landmarks = false +gl_fa_loc_sm_scale_projection_noise_with_num_landmarks = false +gl_fa_loc_sm_scale_projection_noise_with_landmark_distance = false +--- change to <=1 w/ new vio cov +gl_fa_loc_sm_pose_noise_scale = 0.001 +gl_fa_loc_sm_projection_noise_scale = 1.5 +gl_fa_loc_sm_max_num_projection_factors = 25 +gl_fa_loc_sm_min_num_matches_per_measurement = 5 +gl_fa_loc_sm_max_valid_projection_error = 30 +gl_fa_loc_sm_nav_cam_noise_stddev = 0.1 +---- AR Tag (ar) +gl_fa_loc_ar_enabled = true +gl_fa_loc_ar_huber_k = world_huber_k +gl_fa_loc_ar_add_pose_priors = false +gl_fa_loc_ar_add_projection_factors = true +gl_fa_loc_ar_add_prior_if_projection_factors_fail = true +gl_fa_loc_ar_prior_translation_stddev = 0.06 +gl_fa_loc_ar_prior_quaternion_stddev = 0.06 +gl_fa_loc_ar_scale_pose_noise_with_num_landmarks = false +gl_fa_loc_ar_scale_projection_noise_with_num_landmarks = false +gl_fa_loc_ar_scale_projection_noise_with_landmark_distance = false +gl_fa_loc_ar_pose_noise_scale = 0.01 +-- Change to 9 if scale with inverse distance +gl_fa_loc_ar_projection_noise_scale = 10 +gl_fa_loc_ar_max_num_projection_factors = 25 +gl_fa_loc_ar_min_num_matches_per_measurement = 5 +gl_fa_loc_ar_max_valid_projection_error = 30 +gl_fa_loc_ar_dock_cam_noise_stddev = 0.1 +-- Node Adders (na) +--- Pose +gl_na_pose_starting_prior_translation_stddev = 0.02 +gl_na_pose_starting_prior_quaternion_stddev = 0.01 +gl_na_pose_huber_k = world_huber_k +gl_na_pose_add_priors = true +gl_na_pose_ideal_duration = 4 +gl_na_pose_min_num_states = 3 +gl_na_pose_max_num_states = 5 +gl_na_pose_model_huber_k = world_huber_k +-- Optimizer (op) +--- Nonlinear +gl_op_nl_marginals_factorization = "qr" +gl_op_nl_max_iterations = 10 +gl_op_nl_verbose = false +gl_op_nl_use_ceres_params = false +-- Graph Optimizer (go) +--- Sliding Window +gl_go_sw_huber_k = world_huber_k +gl_go_sw_log_stats_on_destruction = false +gl_go_sw_print_after_optimization = false +gl_go_sw_add_marginal_factors = false +gl_go_sw_slide_window_before_optimization = true +-- Other +gl_max_vio_measurement_gap = 3 diff --git a/astrobee/config/localization/graph_vio.config b/astrobee/config/localization/graph_vio.config new file mode 100644 index 0000000000..6f347bb858 --- /dev/null +++ b/astrobee/config/localization/graph_vio.config @@ -0,0 +1,90 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. +require "context" + +-- Graph VIO (gv) +-- Factor Adders (fa) +--- Standstill +gv_fa_standstill_enabled = true +gv_fa_standstill_huber_k = world_huber_k +gv_fa_standstill_add_velocity_prior = true +gv_fa_standstill_add_pose_between_factor = true +gv_fa_standstill_prior_velocity_stddev = 0.01 +gv_fa_standstill_pose_between_factor_translation_stddev = 0.01 +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_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 +gv_fa_vo_robust = true +gv_fa_vo_rotation_only_fallback = true +gv_fa_vo_fix_invalid_factors = true +gv_fa_vo_scale_noise_with_num_points = true +gv_fa_vo_noise_scale = 2 +gv_fa_vo_nav_cam_noise_stddev = 0.1 +---- Smart Factor +gv_fa_vo_rotation_only_fallback = true +gv_fa_vo_enable_EPI = false +gv_fa_vo_landmark_distance_threshold = 500 +gv_fa_vo_dynamic_outlier_rejection_threshold = 50 +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 +---- Depth Odometry +gv_fa_do_enabled = true +gv_fa_do_huber_k = world_huber_k +gv_fa_do_use_points_between_factor = true +gv_fa_do_pose_covariance_scale = 1 +gv_fa_do_point_noise_scale = 0.01 +gv_fa_do_scale_point_between_factors_with_inverse_distance = true +gv_fa_do_scale_point_between_factors_with_estimate_error = false +gv_fa_do_reject_large_point_to_point_error = false +gv_fa_do_point_to_point_error_threshold = 10.0 +gv_fa_do_reject_large_translation_norm = false +gv_fa_do_pose_translation_norm_threshold = 3.0 +gv_fa_do_max_num_points_between_factors = 30 +-- Node Adders (na) +--- Combined Nav State +gv_na_cns_huber_k = world_huber_k +gv_na_cns_add_priors = true +gv_na_cns_ideal_duration = 3.25 +gv_na_cns_min_num_states = 3 +gv_na_cns_max_num_states = 8 +--- Combined Nav State Model +gv_na_cns_model_huber_k = world_huber_k +-- Optimizer (op) +--- Nonlinear +gv_op_nl_marginals_factorization = "qr" +gv_op_nl_max_iterations = 6 +gv_op_nl_verbose = false +gv_op_nl_use_ceres_params = false +-- Graph Optimizer (go) +--- Sliding Window +gv_go_sw_huber_k = world_huber_k +gv_go_sw_log_stats_on_destruction = false +gv_go_sw_print_after_optimization = false +gv_go_sw_add_marginal_factors = false +gv_go_sw_slide_window_before_optimization = true +-- Standstill +gv_standstill_min_num_points_per_track = 4 +gv_standstill_duration = 1 +gv_standstill_max_avg_distance_from_mean = 0.075 diff --git a/astrobee/config/tools/graph_bag.config b/astrobee/config/localization/imu_bias_initializer.config similarity index 60% rename from astrobee/config/tools/graph_bag.config rename to astrobee/config/localization/imu_bias_initializer.config index 3008537967..8e42670469 100644 --- a/astrobee/config/tools/graph_bag.config +++ b/astrobee/config/localization/imu_bias_initializer.config @@ -14,27 +14,8 @@ -- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -- License for the specific language governing permissions and limitations -- under the License. +require "context" --- imu -imu_msg_delay = 0 -imu_min_msg_spacing = 0 --- flight_mode -flight_mode_msg_delay = 0 -flight_mode_min_msg_spacing = 0 --- depth odometry -depth_odometry_msg_delay = 0 -depth_odometry_min_msg_spacing = 0 --- optical flow -of_msg_delay = 0 -of_min_msg_spacing = 0 --- sparse mapping -vl_msg_delay = 0 -vl_min_msg_spacing = 0 --- april tag -ar_msg_delay = 0 -ar_min_msg_spacing = 0 --- graph_localizer_simulator -optimization_time = 0.30 --- Other -save_optical_flow_images = false -log_relative_time = false +-- IMU bias initializer (ibi) +ibi_imu_bias_filename = resolve_resource("imu_bias.config"); +ibi_num_bias_estimation_measurements = 100 diff --git a/astrobee/config/localization/imu_filter.config b/astrobee/config/localization/imu_filter.config new file mode 100644 index 0000000000..6d6fbe86b6 --- /dev/null +++ b/astrobee/config/localization/imu_filter.config @@ -0,0 +1,27 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. +require "context" + +-- IMU Filter (if) +-- none, ButterOxSxLpxNx, where O is order, S is sample rate, Lp is low pass cutoff, N is notch +-- Note that notch depends on the sample rate and aliasing +if_quiet_accel = "ButterO3S125Lp3N33_33" +if_quiet_ang_vel = "ButterO1S125Lp3N33_33" +if_nominal_accel = "ButterO3S125Lp3N41_66" +if_nominal_ang_vel = "ButterO1S125Lp3N41_66" +if_aggressive_accel = "ButterO3S125Lp3N46_66" +if_aggressive_ang_vel = "ButterO1S125Lp3N46_66" diff --git a/astrobee/config/localization/imu_integrator.config b/astrobee/config/localization/imu_integrator.config new file mode 100644 index 0000000000..782056284c --- /dev/null +++ b/astrobee/config/localization/imu_integrator.config @@ -0,0 +1,27 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. +require "context" + +-- IMU Integrator (ii) +ii_gyro_sigma = world_gyro_sigma +ii_accel_sigma = world_accel_sigma +ii_accel_bias_sigma = world_accel_bias_sigma +ii_gyro_bias_sigma = world_gyro_bias_sigma +ii_integration_variance = world_integration_variance +ii_bias_acc_omega_int = world_bias_acc_omega_int +ii_gravity = world_gravity_vector +ii_ignore_gravity = true diff --git a/astrobee/config/localization/ros_graph_localizer.config b/astrobee/config/localization/ros_graph_localizer.config new file mode 100644 index 0000000000..a9cbdae498 --- /dev/null +++ b/astrobee/config/localization/ros_graph_localizer.config @@ -0,0 +1,44 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. +require "context" + +-- Ros graph localizer (rgl) +-- Measurement buffer size in between loc messages +rgl_max_relative_vio_buffer_size = 2000 +-- Max duration (s) between vl msgs, if surpassed localizer is reset to avoid VIO msg overflow issues +rgl_max_duration_between_vl_msgs = 1000 +-- Buffer max should be at least 10 and a max of 2 seconds of data +--- VIO ~15 Hz +rgl_max_graph_vio_state_buffer_size = 10 +--- Vl ~1 Hz +rgl_max_vl_matched_projections_buffer_size = 10 +rgl_max_ar_tag_matched_projections_buffer_size = 10 +--- Imu ~62.5 Hz +rgl_max_imu_buffer_size = 125 +--- Feature point ~10 Hz +rgl_max_feature_point_buffer_size = 20 +--- Depth Odometry ~5 Hz +rgl_max_depth_odom_buffer_size = 5 +-- Set depth cloud and image buffer sizes to 1 so only the latest +-- data is used for inline depth odometry +rgl_max_depth_image_buffer_size = 1 +rgl_max_depth_cloud_buffer_size = 1 +-- Others +rgl_run_depth_odometry = true +rgl_publish_depth_odometry = true +rgl_subscribe_to_depth_odometry = false +rgl_extrapolate_dock_pose_with_imu = true diff --git a/astrobee/config/localization/ros_graph_vio.config b/astrobee/config/localization/ros_graph_vio.config new file mode 100644 index 0000000000..b12360c1a4 --- /dev/null +++ b/astrobee/config/localization/ros_graph_vio.config @@ -0,0 +1,25 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. +require "context" + +-- Ros Graph VIO (rgv) +--- Starting state noise +rgv_starting_pose_translation_stddev = 0.00000001 +rgv_starting_pose_quaternion_stddev = 0.00000001 +rgv_starting_velocity_stddev_scale = 200 +rgv_starting_accel_bias_stddev_scale = 200 +rgv_starting_gyro_bias_stddev_scale = 200 diff --git a/astrobee/config/localization/ros_pose_extrapolator.config b/astrobee/config/localization/ros_pose_extrapolator.config new file mode 100644 index 0000000000..e91c7e1173 --- /dev/null +++ b/astrobee/config/localization/ros_pose_extrapolator.config @@ -0,0 +1,21 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. +require "context" + +-- Ros Pose Extrapolator +rpe_standstill_enabled = true; +rpe_max_relative_vio_buffer_size = 2000; diff --git a/astrobee/config/management/cpu_mem_monitor.config b/astrobee/config/management/cpu_mem_monitor.config index 512be2f818..5004f88b14 100644 --- a/astrobee/config/management/cpu_mem_monitor.config +++ b/astrobee/config/management/cpu_mem_monitor.config @@ -32,7 +32,7 @@ llp = { {name="llp_gnc"}, {name="llp_i2c"}, {name="llp_imu"}, - {name="llp_imu_aug"}, + {name="llp_pose_extr"}, {name="llp_lights"}, {name="llp_pmc"}, {name="llp_serial"}}, diff --git a/astrobee/config/management/fault_table.config b/astrobee/config/management/fault_table.config index aab193af50..3e5d19ff0e 100644 --- a/astrobee/config/management/fault_table.config +++ b/astrobee/config/management/fault_table.config @@ -181,9 +181,9 @@ subsystems={ {id=59, warning=false, blocking=false, response=command("unloadNodelet", "ctl", ""), key="INITIALIZATION_FAILED", description="Control Initialization Failed"}, {id=140, warning=false, blocking=false, response=command("noOp"), key="KINETIC_ENERGY_INCREASING", description="Kinetic energy increasing in stopping mode"}, }}, - {name="imu_aug", faults={ + {name="pose_extr", faults={ {id=16, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from IMU Augmentor", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=61, warning=false, blocking=true, response=command("unloadNodelet", "imu_aug", ""), key="INITIALIZATION_FAILED", description="IMU Augmentor Initialization Failed"}, + {id=61, warning=false, blocking=true, response=command("unloadNodelet", "pose_extr", ""), key="INITIALIZATION_FAILED", description="Pose Extrapolator Initialization Failed"}, }}, {name="graph_loc", faults={ {id=15, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from Graph Localizer", heartbeat={timeout_sec=3.1, misses=1.0}}, diff --git a/astrobee/config/management/sys_monitor.config b/astrobee/config/management/sys_monitor.config index 050fffb979..581c44c26a 100644 --- a/astrobee/config/management/sys_monitor.config +++ b/astrobee/config/management/sys_monitor.config @@ -30,7 +30,7 @@ heartbeat_pub_rate_sec = 5 -- Name of node used to calculate time difference between the mlp and the llp. -- The node specified should be running on the llp as the system monitor runs -- on the mlp -time_diff_node = "imu_aug" +time_diff_node = "pose_extr" -- Threshold used to trigger time drift fault time_drift_thres_sec = 0.25 @@ -55,10 +55,10 @@ nodelet_info={ {name="flashlight_aft", manager="llp_i2c", type="flashlight/FlashlightNodelet"}, {name="flashlight_front", manager="llp_i2c", type="flashlight/FlashlightNodelet"}, {name="framestore", manager="mlp_mobility", type="mobility/FrameStore"}, - {name="graph_loc", manager="mlp_graph_localization", type="graph_localizer/GraphLocalizerNodelet"}, + {name="graph_loc", manager="mlp_graph_localization", type="ros_graph_localizer/RosGraphLocalizerNodelet"}, {name="handrail_detect", manager="mlp_depth_cam", type="handrail_detect/HandrailDetect"}, {name="image_sampler", manager="mlp_localization", type="image_sampler/ImageSampler"}, - {name="imu_aug", manager="llp_imu_aug", type="imu_augmentor/ImuAugmentorNodelet"}, + {name="pose_extr", manager="llp_pose_extr", type="ros_pose_extrapolator/RosPoseExtrapolatorNodelet"}, {name="laser", manager="llp_i2c", type="laser/LaserNodelet"}, {name="light_flow", manager="llp_lights", type="light_flow/LightFlowNodelet"}, {name="llp_cpu_mem_monitor", manager="llp_monitors", type="cpu_mem_monitor/CpuMemMonitor"}, diff --git a/astrobee/config/tools/offline_replay.config b/astrobee/config/tools/offline_replay.config new file mode 100644 index 0000000000..da59096a2e --- /dev/null +++ b/astrobee/config/tools/offline_replay.config @@ -0,0 +1,58 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. + +-- LiveMeasurementSimulator +--- imu +or_imu_msg_delay = 0 +or_imu_min_msg_spacing = 0 +--- flight_mode +or_flight_mode_msg_delay = 0 +or_flight_mode_min_msg_spacing = 0 +--- depth odometry +or_depth_odometry_msg_delay = 0 +or_depth_odometry_min_msg_spacing = 0 +--- depth image +or_depth_image_msg_delay = 0 +or_depth_image_min_msg_spacing = 0 +--- depth cloud +or_depth_cloud_msg_delay = 0 +or_depth_cloud_min_msg_spacing = 0 +--- optical flow +or_of_msg_delay = 0 +or_of_min_msg_spacing = 0 +--- sparse mapping +or_vl_msg_delay = 0 +or_vl_min_msg_spacing = 0 +--- april tag +or_ar_msg_delay = 0 +or_ar_min_msg_spacing = 0 +--- vio +or_vio_msg_delay = 0 +or_vio_min_msg_spacing = 0 + +-- graph_localizer_simulator +or_loc_optimization_time = 0.05 + +-- graph_vio_simulator +or_vio_optimization_time = 0.30 + +-- Other +or_save_optical_flow_images = false +or_use_bag_depth_odom_msgs = false +or_log_relative_time = false +or_ar_min_num_landmarks = 5 +or_sparse_mapping_min_num_landmarks = 5 diff --git a/astrobee/config/worlds/granite.config b/astrobee/config/worlds/granite.config index 15b5cdd112..620b3e5309 100755 --- a/astrobee/config/worlds/granite.config +++ b/astrobee/config/worlds/granite.config @@ -47,12 +47,12 @@ world_vision_ar_tag_filename = "dock_markers_world.config" -- Distance of handrail from wall plane world_handrail_wall_min_gap = 0.11 -- Huber loss function cutoff point to switch from quadratic to linear -world_huber_k = 0.5 -world_gyro_sigma = 0.005 -world_accel_sigma = 0.03 -world_accel_bias_sigma = 0.01 -world_gyro_bias_sigma = 0.001 -world_integration_variance = 0.05 +world_huber_k = 1.345 +world_gyro_sigma = 0.00001 +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.0001 -------------------------- diff --git a/astrobee/config/worlds/iss.config b/astrobee/config/worlds/iss.config index 1a60f27ee3..5d21853b3f 100755 --- a/astrobee/config/worlds/iss.config +++ b/astrobee/config/worlds/iss.config @@ -55,14 +55,12 @@ world_handrail_wall_min_gap = 0.06 -- Huber loss function cutoff point to switch from quadratic to linear world_huber_k = 1.345 -- From gtsam: Angular and velocity random walk expressed in degrees respectively m/s per sqrt(hr) --- (0.5 * M_PI / 180) / 60; // 0.5 degree ARW world_gyro_sigma = 0.00001; --- 0.1 / 60; // 10 cm VRW -world_accel_sigma = 0.00015 -world_accel_bias_sigma = 0.0077 -world_gyro_bias_sigma = 0.0001 +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.000015 +world_bias_acc_omega_int = 0.00015 -------------------------- -- PHYSICS -- diff --git a/astrobee/launch/offline_localization/loc_only.launch b/astrobee/launch/offline_localization/loc_only.launch index 95a468bd90..0b6c8f51ad 100644 --- a/astrobee/launch/offline_localization/loc_only.launch +++ b/astrobee/launch/offline_localization/loc_only.launch @@ -32,7 +32,7 @@ <env name="ASTROBEE_RESOURCE_DIR" value="$(find astrobee)/resources" /> <!-- Nodes to run --> - <include file="$(find graph_localizer)/launch/graph_localizer.launch"> + <include file="$(find ros_graph_localizer)/launch/ros_graph_localizer.launch"> <arg name="terminal" value=""/> </include> </launch> diff --git a/astrobee/launch/offline_localization/localization_from_bag.launch b/astrobee/launch/offline_localization/localization_from_bag.launch index 54fbd81976..eea8a3e9ea 100644 --- a/astrobee/launch/offline_localization/localization_from_bag.launch +++ b/astrobee/launch/offline_localization/localization_from_bag.launch @@ -24,7 +24,7 @@ <!-- Command line arguments --> <arg name="bagfile"/> <arg name="record" default="false" /> - <arg name="imu_augmentor" default="true" /> + <arg name="pose_extrapolator" default="true" /> <arg name="depth_odometry" default="false" /> <arg name="robot" default="bumble" /> <arg name="world" default="iss" /> @@ -46,51 +46,51 @@ <group if="$(arg record)"> <arg name="output_bagfile"/> <include file="$(find astrobee)/launch/offline_localization/record_localization.launch"> - <arg name="terminal" value="terminator -x"/> + <arg name="terminal" value="xterm -hold -e"/> <arg name="output_bagfile" value="$(arg output_bagfile)"/> </include> </group> - <!-- Run imu augmentor if required --> - <group if="$(arg imu_augmentor)"> - <include file="$(find imu_augmentor)/launch/imu_augmentor.launch"> - <arg name="terminal" value="terminator -x"/> + <!-- Run pose extrapolator if required --> + <group if="$(arg pose_extrapolator)"> + <include file="$(find ros_pose_extrapolator)/launch/ros_pose_extrapolator.launch"> + <arg name="terminal" value="xterm -hold -e"/> </include> </group> - <include file="$(find imu_bias_tester)/launch/imu_bias_tester.launch"> - <arg name="terminal" value="terminator -x"/> - </include> + <!-- include file="$(find imu_bias_tester)/launch/imu_bias_tester.launch"> + <arg name="terminal" value="xterm -hold -e"/> + </include --> <!-- Nodes to run --> <group unless="$(arg image_features)"> <include file="$(find lk_optical_flow)/launch/lk_optical_flow.launch"> - <arg name="terminal" value="terminator -x"/> + <arg name="terminal" value="xterm -hold -e"/> </include> <include file="$(find localization_node)/launch/localization_node.launch"> - <arg name="terminal" value="terminator -x"/> + <arg name="terminal" value="xterm -hold -e"/> </include> </group> - <include file="$(find graph_localizer)/launch/graph_localizer.launch"> - <arg name="terminal" value="terminator -x"/> + <include file="$(find ros_graph_localizer)/launch/ros_graph_localizer.launch"> + <arg name="terminal" value="xterm -hold -e"/> </include> <group if="$(arg localization_manager)"> <include file="$(find localization_manager)/launch/localization_manager.launch"> - <arg name="terminal" value="terminator -x"/> + <arg name="terminal" value="xterm -hold -e"/> </include> </group> <!-- Run depth odometry if required --> <group if="$(arg depth_odometry)"> <include file="$(find depth_odometry)/launch/depth_odometry.launch"> - <arg name="terminal" value="terminator -x"/> + <arg name="terminal" value="xterm -hold -e"/> </include> </group> <!-- Play bagfile --> <include file="$(find astrobee)/launch/offline_localization/replay_localization.launch"> <arg name="bagfile" value="$(arg bagfile)"/> - <arg name="terminal" value="terminator -x"/> + <arg name="terminal" value="xterm -hold -e"/> <arg name="image_features" value="$(arg image_features)"/> </include> diff --git a/astrobee/launch/offline_localization/record_localization.launch b/astrobee/launch/offline_localization/record_localization.launch index a5ab1c75d8..19c7730113 100644 --- a/astrobee/launch/offline_localization/record_localization.launch +++ b/astrobee/launch/offline_localization/record_localization.launch @@ -18,5 +18,5 @@ <launch> <arg name="output_bagfile"/> <arg name="terminal" default="" /> - <node pkg="rosbag" type="record" name="recorder" output="screen" launch-prefix="$(arg terminal)" args="/graph_loc/state /graph_loc/graph /sparse_mapping/pose /loc/ml/features /loc/of/features /ar_tag/pose /gnc/ekf /imu_bias_tester/pose /loc/depth/odom -O $(arg output_bagfile)"/> + <node pkg="rosbag" type="record" name="recorder" output="screen" launch-prefix="$(arg terminal)" args="/graph_vio/state /graph_loc/state /graph_loc/graph /sparse_mapping/pose /loc/ml/features /loc/of/features /ar_tag/pose /gnc/ekf /imu_bias_tester/pose /loc/depth/odom -O $(arg output_bagfile)"/> </launch> diff --git a/astrobee/launch/offline_localization/replay_localization.launch b/astrobee/launch/offline_localization/replay_localization.launch index 7f822936c9..d1dab21f1b 100644 --- a/astrobee/launch/offline_localization/replay_localization.launch +++ b/astrobee/launch/offline_localization/replay_localization.launch @@ -18,8 +18,11 @@ <launch> <arg name="bagfile"/> <arg name="image_features" default="false" /> + <arg name="depth_odom" default="false" /> <arg name="image_feature_topics" default="" unless="$(arg image_features)"/> <arg name="image_feature_topics" value="/loc/ml/features /loc/of/features" if="$(arg image_features)"/> + <arg name="depth_odom_topics" default="" unless="$(arg depth_odom)"/> + <arg name="depth_odom_topics" value="/loc/depth/odom" if="$(arg depth_odom)"/> <arg name="terminal" default="" /> @@ -29,5 +32,5 @@ <remap from="/mgt/img_sampler/nav_cam/image_record" to="/hw/cam_nav"/> <remap from="/gnc/ekf" to="/bag/gnc/ekf"/> - <node pkg="rosbag" type="play" name="player" output="screen" launch-prefix="$(arg terminal)" args="--clock $(arg bagfile) --topics /hw/imu /hw/cam_nav /hw/depth_haz/extended/amplitude_int /hw/depth_haz/points /mgt/img_sampler/nav_cam/image_record /mob/flight_mode /loc/ar/features /loc/depth/odom $(arg image_feature_topics)" /> + <node pkg="rosbag" type="play" name="player" output="screen" launch-prefix="$(arg terminal)" args="--clock $(arg bagfile) --rate 2 --topics /hw/imu /hw/cam_nav /hw/depth_haz/extended/amplitude_int /hw/depth_haz/points /mgt/img_sampler/nav_cam/image_record /mob/flight_mode /loc/ar/features $(arg image_feature_topics) $(arg depth_odom_topics)" /> </launch> diff --git a/astrobee/launch/robot/LLP.launch b/astrobee/launch/robot/LLP.launch index fd0c5d3a79..ceda9ce578 100644 --- a/astrobee/launch/robot/LLP.launch +++ b/astrobee/launch/robot/LLP.launch @@ -31,7 +31,7 @@ <node pkg="nodelet" type="nodelet" args="manager" name="llp_gnc" output="$(arg output)"/> <node pkg="nodelet" type="nodelet" args="manager" - name="llp_imu_aug" output="$(arg output)"/> + name="llp_pose_extr" output="$(arg output)"/> <node pkg="nodelet" type="nodelet" args="manager" name="llp_monitors" output="$(arg output)"/> <node pkg="nodelet" type="nodelet" args="manager" @@ -49,9 +49,9 @@ <!-- Launch GNC nodes --> <group unless="$(arg gtloc)"> <include file="$(find ff_util)/launch/ff_nodelet.launch"> - <arg name="class" value="imu_augmentor/ImuAugmentorNodelet" /> - <arg name="name" value="imu_aug" /> - <arg name="manager" value="llp_imu_aug" /> + <arg name="class" value="ros_pose_extrapolator/RosPoseExtrapolatorNodelet" /> + <arg name="name" value="pose_extr" /> + <arg name="manager" value="llp_pose_extr" /> <arg name="spurn" value="$(arg spurn)" /> <arg name="nodes" value="$(arg nodes)" /> <arg name="extra" value="$(arg extra)" /> diff --git a/astrobee/launch/robot/MLP.launch b/astrobee/launch/robot/MLP.launch index 37b473170a..4d6f954072 100644 --- a/astrobee/launch/robot/MLP.launch +++ b/astrobee/launch/robot/MLP.launch @@ -100,7 +100,7 @@ <!-- Run ground_truth_localizer, otherwise run graph localizer --> <group unless="$(arg gtloc)"> <include file="$(find ff_util)/launch/ff_nodelet.launch"> - <arg name="class" value="graph_localizer/GraphLocalizerNodelet" /> + <arg name="class" value="ros_graph_localizer/RosGraphLocalizerNodelet" /> <arg name="name" value="graph_loc" /> <arg name="manager" value="mlp_graph_localization" /> <arg name="spurn" value="$(arg spurn)" /> @@ -109,6 +109,7 @@ <arg name="debug" value="$(arg debug)" /> <arg name="default" value="true" /> </include> + </group> <group if="$(arg gtloc)"> diff --git a/tools/localization_analysis/scripts/imu_measurements.py b/communications/ff_msgs/msg/CombinedNavState.msg similarity index 61% rename from tools/localization_analysis/scripts/imu_measurements.py rename to communications/ff_msgs/msg/CombinedNavState.msg index dbd3b2bb58..f1d68b7ee2 100644 --- a/tools/localization_analysis/scripts/imu_measurements.py +++ b/communications/ff_msgs/msg/CombinedNavState.msg @@ -1,5 +1,3 @@ -#!/usr/bin/python -# # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # @@ -16,17 +14,9 @@ # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the # License for the specific language governing permissions and limitations # under the License. - -import vector3ds - - -class ImuMeasurements: - def __init__(self): - self.accelerations = vector3ds.Vector3ds() - self.angular_velocities = vector3ds.Vector3ds() - self.times = [] - - def add_imu_measurement(self, msg): - self.accelerations.add_vector3d(msg.linear_acceleration) - self.angular_velocities.add_vector3d(msg.angular_velocity) - self.times.append(msg.header.stamp.secs + 1e-9 * msg.header.stamp.nsecs) +# +Header header +geometry_msgs/PoseWithCovarianceStamped pose +VelocityWithCovariance velocity +ImuBiasWithCovariance imu_bias +PoseCovarianceStamped[] correlation_covariances diff --git a/communications/ff_msgs/msg/CombinedNavStateArray.msg b/communications/ff_msgs/msg/CombinedNavStateArray.msg new file mode 100644 index 0000000000..cfa5ceb98a --- /dev/null +++ b/communications/ff_msgs/msg/CombinedNavStateArray.msg @@ -0,0 +1,19 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +Header header +CombinedNavState[] combined_nav_states diff --git a/communications/ff_msgs/msg/GraphLocState.msg b/communications/ff_msgs/msg/GraphLocState.msg new file mode 100644 index 0000000000..9f70ce4356 --- /dev/null +++ b/communications/ff_msgs/msg/GraphLocState.msg @@ -0,0 +1,32 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +std_msgs/Header header # header with timestamp +string child_frame_id # frame ID +# Latest Pose Estimate +geometry_msgs/PoseWithCovariance pose # world_T_body +# Stats +uint32 num_detected_ar_features +uint32 num_detected_ml_features +uint32 optimization_iterations +float32 optimization_time +float32 update_time # Include optimization_time and other operations to add data to graph +float32 duration +uint32 num_factors +uint32 num_ml_projection_factors +uint32 num_ml_pose_factors +uint32 num_states diff --git a/communications/ff_msgs/msg/GraphState.msg b/communications/ff_msgs/msg/GraphState.msg index a58fa306dc..623a808647 100644 --- a/communications/ff_msgs/msg/GraphState.msg +++ b/communications/ff_msgs/msg/GraphState.msg @@ -16,6 +16,7 @@ # under the License. # +# Deprecated, kept for backwards compatability std_msgs/Header header # header with timestamp string child_frame_id # frame ID # State Estimates diff --git a/tools/localization_analysis/scripts/velocities.py b/communications/ff_msgs/msg/GraphVIOState.msg similarity index 54% rename from tools/localization_analysis/scripts/velocities.py rename to communications/ff_msgs/msg/GraphVIOState.msg index 5fad11c6f7..c0c58f7f0a 100644 --- a/tools/localization_analysis/scripts/velocities.py +++ b/communications/ff_msgs/msg/GraphVIOState.msg @@ -1,5 +1,3 @@ -#!/usr/bin/python -# # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # @@ -16,20 +14,22 @@ # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the # License for the specific language governing permissions and limitations # under the License. - -import vector3ds - - -class Velocities(object): - def __init__(self, velocity_type, topic): - self.velocities = vector3ds.Vector3ds() - self.times = [] - self.velocity_type = velocity_type - self.topic = topic - - def add_velocity(self, velocity_msg, timestamp, bag_start_time=0): - self.velocities.add(velocity_msg.x, velocity_msg.y, velocity_msg.z) - self.times.append(timestamp.secs + 1e-9 * timestamp.nsecs - bag_start_time) - - def add_msg(self, msg, timestamp, bag_start_time=0): - self.add_velocity(msg.vector, timestamp, bag_start_time) +# +std_msgs/Header header # header with timestamp +string child_frame_id # frame ID +# CombinedNavState estimates currently in the graph +CombinedNavStateArray combined_nav_states +# Stats +uint32 num_detected_of_features +uint32 optimization_iterations +float32 optimization_time +float32 update_time # Include optimization_time and other operations to add data to graph +float32 duration +uint32 num_factors +uint32 num_of_factors +uint32 num_depth_factors +uint32 num_states +# Status +bool standstill +# TODO(rsoussan): Fill this in! Estimating bias expected for gnc ekf message! +bool estimating_bias # Are we busy estimating the bias? diff --git a/tools/localization_analysis/scripts/pose.py b/communications/ff_msgs/msg/ImuBiasWithCovariance.msg similarity index 66% rename from tools/localization_analysis/scripts/pose.py rename to communications/ff_msgs/msg/ImuBiasWithCovariance.msg index 2fe84b49d3..cb46d9b26b 100644 --- a/tools/localization_analysis/scripts/pose.py +++ b/communications/ff_msgs/msg/ImuBiasWithCovariance.msg @@ -1,5 +1,3 @@ -#!/usr/bin/python -# # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # @@ -16,14 +14,12 @@ # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the # License for the specific language governing permissions and limitations # under the License. +# - -class Pose: - def __init__(self, orientation, position): - self.orientation = orientation - self.position = position - - def __mul__(self, pose_b): - new_orientation = self.orientation * pose_b.orientation - new_position = self.orientation.apply(pose_b.position) + self.position - return Pose(new_orientation, new_position) +geometry_msgs/Vector3 accelerometer_bias +geometry_msgs/Vector3 gyroscope_bias +# Ordered as accelerometer then gyroscope bias covarainces +# Top left 3x3 block is the accelerometer bias covariances, +# bottom right 3x3 block is the gyroscope bias covariances, +# and other blocks are accelerometer/gyroscope covariances. +float64[36] covariance diff --git a/tools/localization_analysis/scripts/vector3d.py b/communications/ff_msgs/msg/PoseCovarianceStamped.msg similarity index 85% rename from tools/localization_analysis/scripts/vector3d.py rename to communications/ff_msgs/msg/PoseCovarianceStamped.msg index ee422308b8..88272170c4 100644 --- a/tools/localization_analysis/scripts/vector3d.py +++ b/communications/ff_msgs/msg/PoseCovarianceStamped.msg @@ -1,5 +1,3 @@ -#!/usr/bin/python -# # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # @@ -16,10 +14,7 @@ # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the # License for the specific language governing permissions and limitations # under the License. +# - -class Vector3d: - def __init__(self, x, y, z): - self.x = x - self.y = y - self.z = z +float64[36] covariance +time time diff --git a/communications/ff_msgs/msg/LocalizationGraph.msg b/communications/ff_msgs/msg/SerializedGraph.msg similarity index 100% rename from communications/ff_msgs/msg/LocalizationGraph.msg rename to communications/ff_msgs/msg/SerializedGraph.msg diff --git a/tools/localization_analysis/scripts/multiprocessing_helpers.py b/communications/ff_msgs/msg/VelocityWithCovariance.msg similarity index 62% rename from tools/localization_analysis/scripts/multiprocessing_helpers.py rename to communications/ff_msgs/msg/VelocityWithCovariance.msg index e46960992b..23b8a5e6be 100644 --- a/tools/localization_analysis/scripts/multiprocessing_helpers.py +++ b/communications/ff_msgs/msg/VelocityWithCovariance.msg @@ -14,20 +14,7 @@ # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the # License for the specific language governing permissions and limitations # under the License. +# -# Forward errors so we can recover failures -# even when running commands through multiprocessing -# pooling -def full_traceback(func): - import functools - import traceback - - @functools.wraps(func) - def wrapper(*args, **kwargs): - try: - return func(*args, **kwargs) - except Exception as e: - msg = "{}\n\nOriginal {}".format(e, traceback.format_exc()) - raise type(e)(msg) - - return wrapper +geometry_msgs/Vector3 velocity +float64[9] covariance diff --git a/gnc/ctl/include/ctl/ctl.h b/gnc/ctl/include/ctl/ctl.h index b252c53da4..89ae651977 100644 --- a/gnc/ctl/include/ctl/ctl.h +++ b/gnc/ctl/include/ctl/ctl.h @@ -132,6 +132,7 @@ class Control { Eigen::Matrix<float, 4, 4> OmegaMatrix(Eigen::Vector3f input); + float tun_vel_gain; Eigen::Vector3f tun_accel_gain; Eigen::Vector3f tun_alpha_gain; float tun_ctl_stopping_omega_thresh; diff --git a/gnc/ctl/src/ctl.cc b/gnc/ctl/src/ctl.cc index afd9320672..15578ab8d7 100644 --- a/gnc/ctl/src/ctl.cc +++ b/gnc/ctl/src/ctl.cc @@ -190,7 +190,8 @@ void Control::FindBodyForceCmd(const ControlState & state, ControlOutput* out) { Eigen::Vector3f v = Eigen::Vector3f::Zero() - state.est_V_B_ISS_ISS; if (out->ctl_status > 1) { // find desired velocity from position error - v += target_vel + (Kp_lin.array() * out->pos_err.array()).matrix() + out->pos_err_int; + v = tun_vel_gain * (target_vel - state.est_V_B_ISS_ISS) + (Kp_lin.array() * out->pos_err.array()).matrix() + + out->pos_err_int; } Eigen::Vector3f a = RotateVectorAtoB(v, state.est_quat_ISS2B); a = (a.array() * state.vel_kd.array()).matrix(); @@ -350,6 +351,8 @@ void Control::Initialize(void) { } void Control::ReadParams(config_reader::ConfigReader* config) { + if (!config->GetReal("tun_vel_gain", &tun_vel_gain)) + ROS_FATAL("Unspecified tun_vel_gain."); Eigen::Vector3d temp; if (!msg_conversions::config_read_vector(config, "tun_accel_gain", &temp)) ROS_FATAL("Unspecified tun_accel_gain."); diff --git a/localization/depth_odometry/CMakeLists.txt b/localization/depth_odometry/CMakeLists.txt index cf1c85095b..531977b2a6 100644 --- a/localization/depth_odometry/CMakeLists.txt +++ b/localization/depth_odometry/CMakeLists.txt @@ -36,7 +36,8 @@ find_package(catkin2 REQUIRED COMPONENTS ) # Find OpenCV -find_package(OpenCV 4.0 REQUIRED) +LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") +find_package(OpenCV4WithXFeatures REQUIRED) find_package(Eigen3 REQUIRED) @@ -90,28 +91,23 @@ if(CATKIN_ENABLE_TESTING) ${PROJECT_NAME} ${catkin_LIBRARIES} ) - # Ignore these tests if OpenCV 4 installed as cv_bridge will cause a runtime segfault - # since it uses opencv4 on newer ros versions and the rest of the code uses opencv3 - find_package(OpenCV 4 QUIET) - if(NOT OpenCV_FOUND) - add_rostest_gtest(test_image_features_with_known_correspondences_aligner_depth_odometry - test/test_image_features_with_known_correspondences_aligner_depth_odometry.test - test/test_image_features_with_known_correspondences_aligner_depth_odometry.cc - test/test_utilities.cc - ) - target_link_libraries(test_image_features_with_known_correspondences_aligner_depth_odometry - ${PROJECT_NAME} ${catkin_LIBRARIES} - ) - - add_rostest_gtest(test_depth_odometry_wrapper - test/test_depth_odometry_wrapper.test - test/test_depth_odometry_wrapper.cc - test/test_utilities.cc - ) - target_link_libraries(test_depth_odometry_wrapper - ${PROJECT_NAME} ${catkin_LIBRARIES} - ) - endif() + add_rostest_gtest(test_image_features_with_known_correspondences_aligner_depth_odometry + test/test_image_features_with_known_correspondences_aligner_depth_odometry.test + test/test_image_features_with_known_correspondences_aligner_depth_odometry.cc + test/test_utilities.cc + ) + target_link_libraries(test_image_features_with_known_correspondences_aligner_depth_odometry + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + + add_rostest_gtest(test_depth_odometry_wrapper + test/test_depth_odometry_wrapper.test + test/test_depth_odometry_wrapper.cc + test/test_utilities.cc + ) + target_link_libraries(test_depth_odometry_wrapper + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) endif() diff --git a/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper.h b/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper.h index d0292511a2..8190654f6b 100644 --- a/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper.h +++ b/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper.h @@ -21,9 +21,9 @@ #include <depth_odometry/depth_odometry.h> #include <depth_odometry/depth_odometry_wrapper_params.h> #include <ff_msgs/DepthOdometry.h> -#include <localization_common/measurement_buffer.h> #include <localization_common/time.h> #include <localization_common/timer.h> +#include <localization_common/timestamped_set.h> #include <pcl/point_cloud.h> #include <pcl/point_types.h> @@ -48,8 +48,8 @@ class DepthOdometryWrapper { std::vector<ff_msgs::DepthOdometry> ProcessDepthImageIfAvailable(); std::unique_ptr<DepthOdometry> depth_odometry_; DepthOdometryWrapperParams params_; - localization_common::MeasurementBuffer<sensor_msgs::PointCloud2ConstPtr> point_cloud_buffer_; - localization_common::MeasurementBuffer<sensor_msgs::ImageConstPtr> image_buffer_; + localization_common::TimestampedSet<sensor_msgs::PointCloud2ConstPtr> point_cloud_buffer_; + localization_common::TimestampedSet<sensor_msgs::ImageConstPtr> image_buffer_; localization_common::Timer timer_ = localization_common::Timer("Depth Odometry"); }; } // namespace depth_odometry diff --git a/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper_params.h b/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper_params.h index 956fd4202f..16d8c2d30a 100644 --- a/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper_params.h +++ b/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper_params.h @@ -34,6 +34,10 @@ struct DepthOdometryWrapperParams { Eigen::Affine3d haz_cam_A_haz_depth; PointToPlaneICPDepthOdometryParams icp; ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams image_features; + // Used for image and point cloud buffer + double max_buffer_size; + // Max number of depth images to generate per cycle + double max_depth_images; }; } // namespace depth_odometry diff --git a/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry_params.h b/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry_params.h index 3416822159..4d47dcaf09 100644 --- a/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry_params.h +++ b/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry_params.h @@ -18,6 +18,7 @@ #ifndef DEPTH_ODOMETRY_IMAGE_FEATURES_WITH_KNOWN_CORRESPONDENCES_ALIGNER_DEPTH_ODOMETRY_PARAMS_H_ #define DEPTH_ODOMETRY_IMAGE_FEATURES_WITH_KNOWN_CORRESPONDENCES_ALIGNER_DEPTH_ODOMETRY_PARAMS_H_ +#include <camera/camera_params.h> #include <depth_odometry/depth_odometry_params.h> #include <depth_odometry/point_to_plane_icp_depth_odometry_params.h> #include <point_cloud_common/point_cloud_with_known_correspondences_aligner_params.h> @@ -41,9 +42,16 @@ struct ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams : public // Other double min_x_distance_to_border; double min_y_distance_to_border; - int min_num_inliers; + int min_num_correspondences; + bool only_correspondences; bool refine_estimate; PointToPlaneICPDepthOdometryParams point_to_plane_icp; + Eigen::Matrix3d cam_intrinsics; + std::shared_ptr<camera::CameraParameters> cam_params; + bool filter_outliers; + int filter_method; + double inlier_threshold; + double inlier_probability; }; } // namespace depth_odometry diff --git a/localization/depth_odometry/src/depth_odometry_wrapper.cc b/localization/depth_odometry/src/depth_odometry_wrapper.cc index 11117402af..36452090c7 100644 --- a/localization/depth_odometry/src/depth_odometry_wrapper.cc +++ b/localization/depth_odometry/src/depth_odometry_wrapper.cc @@ -58,6 +58,9 @@ void DepthOdometryWrapper::Initialize(const DepthOdometryWrapperParams& params) } else { LogFatal("DepthOdometryWrapper: Invalid depth odometry method selected."); } + + point_cloud_buffer_ = localization_common::TimestampedSet<sensor_msgs::PointCloud2ConstPtr>(params.max_buffer_size); + image_buffer_ = localization_common::TimestampedSet<sensor_msgs::ImageConstPtr>(params.max_buffer_size); } std::vector<ff_msgs::DepthOdometry> DepthOdometryWrapper::PointCloudCallback( @@ -75,27 +78,39 @@ std::vector<ff_msgs::DepthOdometry> DepthOdometryWrapper::ProcessDepthImageIfAva std::vector<lm::DepthImageMeasurement> depth_image_measurements; boost::optional<lc::Time> latest_added_point_cloud_msg_time; boost::optional<lc::Time> latest_added_image_msg_time; + int added_depth_images = 0; // Point clouds and depth images for the same measurement arrive on different topics. // Correlate pairs of these if possible. - for (const auto& image_msg : image_buffer_.measurements()) { - const auto image_msg_timestamp = image_msg.first; - const auto point_cloud_msg = - point_cloud_buffer_.GetNearby(image_msg_timestamp, params_.max_image_and_point_cloud_time_diff); - if (point_cloud_msg) { + // Only add up to max_depth_images per cycle. + for (auto image_msg_it = image_buffer_.set().rbegin(); + image_msg_it != image_buffer_.set().rend() && added_depth_images < params_.max_depth_images; ++image_msg_it) { + const auto image_msg_timestamp = image_msg_it->first; + const auto point_cloud_msg = point_cloud_buffer_.Closest(image_msg_timestamp); + if (point_cloud_msg && + std::abs(point_cloud_msg->timestamp - image_msg_timestamp) <= params_.max_image_and_point_cloud_time_diff) { const auto depth_image_measurement = - lm::MakeDepthImageMeasurement(*point_cloud_msg, image_msg.second, params_.haz_cam_A_haz_depth); + lm::MakeDepthImageMeasurement(point_cloud_msg->value, image_msg_it->second, params_.haz_cam_A_haz_depth); if (!depth_image_measurement) { LogError("ProcessDepthImageIfAvailable: Failed to create depth image measurement."); continue; } depth_image_measurements.emplace_back(*depth_image_measurement); - latest_added_point_cloud_msg_time = lc::TimeFromHeader((*point_cloud_msg)->header); - latest_added_image_msg_time = image_msg_timestamp; + ++added_depth_images; + if (!latest_added_point_cloud_msg_time) latest_added_point_cloud_msg_time = point_cloud_msg->timestamp; + if (!latest_added_image_msg_time) latest_added_image_msg_time = image_msg_timestamp; } } - if (latest_added_point_cloud_msg_time) point_cloud_buffer_.EraseUpToAndIncluding(*latest_added_point_cloud_msg_time); - if (latest_added_image_msg_time) image_buffer_.EraseUpToAndIncluding(*latest_added_image_msg_time); + // Remove any measuremets older than measurements used for depth image creation + // Also remove measurements used for depth image creation + if (latest_added_point_cloud_msg_time) { + point_cloud_buffer_.RemoveOldValues(*latest_added_point_cloud_msg_time); + point_cloud_buffer_.Remove(*latest_added_point_cloud_msg_time); + } + if (latest_added_image_msg_time) { + image_buffer_.RemoveOldValues(*latest_added_image_msg_time); + image_buffer_.Remove(*latest_added_image_msg_time); + } std::vector<ff_msgs::DepthOdometry> depth_odometry_msgs; for (const auto& depth_image_measurement : depth_image_measurements) { diff --git a/localization/depth_odometry/src/image_features_with_known_correspondences_aligner_depth_odometry.cc b/localization/depth_odometry/src/image_features_with_known_correspondences_aligner_depth_odometry.cc index 4357a913a1..faf0e79b5f 100644 --- a/localization/depth_odometry/src/image_features_with_known_correspondences_aligner_depth_odometry.cc +++ b/localization/depth_odometry/src/image_features_with_known_correspondences_aligner_depth_odometry.cc @@ -24,6 +24,9 @@ #include <vision_common/lk_optical_flow_feature_detector_and_matcher.h> #include <vision_common/surf_feature_detector_and_matcher.h> +#include <opencv2/calib3d.hpp> +#include <opencv2/core/eigen.hpp> + namespace depth_odometry { namespace lc = localization_common; namespace lm = localization_measurements; @@ -63,12 +66,13 @@ ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::DepthImageCallback( latest_depth_image_features_and_points_.reset(new DepthImageFeaturesAndPoints( depth_image_measurement.depth_image, *(feature_detector_and_matcher_->detector()), clahe_, normals_required_)); latest_timestamp_ = depth_image_measurement.timestamp; - if (params_.refine_estimate) point_to_plane_icp_depth_odometry_->DepthImageCallback(depth_image_measurement); + if (!params_.only_correspondences && params_.refine_estimate) + point_to_plane_icp_depth_odometry_->DepthImageCallback(depth_image_measurement); return boost::none; } const lc::Time timestamp = depth_image_measurement.timestamp; if (timestamp < latest_timestamp_) { - LogWarning("DepthImageCallback: Out of order measurement received."); + LogError("DepthImageCallback: Out of order measurement received."); return boost::none; } @@ -80,12 +84,13 @@ ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::DepthImageCallback( const double time_diff = latest_timestamp_ - previous_timestamp_; if (time_diff > params_.max_time_diff) { - LogWarning("DepthImageCallback: Time difference too large, time diff: " << time_diff); + LogError("DepthImageCallback: Time difference too large, time diff: " << time_diff); return boost::none; } const auto& matches = feature_detector_and_matcher_->Match(previous_depth_image_features_and_points_->feature_image(), latest_depth_image_features_and_points_->feature_image()); + // Get 3d points and required normals for matches // Continue if any of these, including image points, are invalid std::vector<Eigen::Vector2d> source_image_points; @@ -106,7 +111,7 @@ ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::DepthImageCallback( if (!Valid3dPoint(source_point_3d) || !Valid3dPoint(target_point_3d)) continue; const Eigen::Vector3d source_landmark = pc::Vector3d(*source_point_3d); const Eigen::Vector3d target_landmark = pc::Vector3d(*target_point_3d); - if (normals_required_) { + if (normals_required_ && !params_.only_correspondences) { const auto target_normal = latest_depth_image_features_and_points_->Normal(target_landmark, params_.aligner.normal_search_radius); if (!target_normal) continue; @@ -125,11 +130,62 @@ ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::DepthImageCallback( target_landmarks.emplace_back(target_landmark); } - if (target_landmarks.size() < 4) { - LogError("DepthImageCallback: Too few points provided, need 4 but given " << target_landmarks.size() << "."); + if (target_landmarks.size() < params_.min_num_correspondences) { + LogDebug("DepthImageCallback: Too few points provided, need " << params_.min_num_correspondences << " but given " + << target_landmarks.size() << "."); return boost::none; } + if (params_.filter_outliers) { + std::vector<unsigned char> inliers; + std::vector<cv::Point2d> source_points, target_points; + // Undistort and store image points as CV points + for (int i = 0; i < source_image_points.size(); ++i) { + Eigen::Vector2d undistorted_source, undistorted_target; + params_.cam_params->Convert<camera::DISTORTED, camera::UNDISTORTED_C>(source_image_points[i], + &undistorted_source); + params_.cam_params->Convert<camera::DISTORTED, camera::UNDISTORTED_C>(target_image_points[i], + &undistorted_target); + source_points.emplace_back(cv::Point2d(undistorted_source.x(), undistorted_source.y())); + target_points.emplace_back(cv::Point2d(undistorted_target.x(), undistorted_target.y())); + } + cv::Mat intrinsics; + cv::eigen2cv(params_.cam_intrinsics, intrinsics); + // TODO(rsoussan): Pass iterations limit if OpenCV version upgraded + cv::findEssentialMat(source_points, target_points, intrinsics, params_.filter_method, params_.inlier_probability, + params_.inlier_threshold, inliers); + // Filter inliers in source and target point sets + source_image_points.erase(std::remove_if(source_image_points.begin(), source_image_points.end(), + [&source_image_points, &inliers](const Eigen::Vector2d& p) { + return static_cast<int>(inliers[(&p - &*source_image_points.begin())]) == + 0; + }), + source_image_points.end()); + target_image_points.erase(std::remove_if(target_image_points.begin(), target_image_points.end(), + [&target_image_points, &inliers](const Eigen::Vector2d& p) { + return static_cast<int>(inliers[(&p - &*target_image_points.begin())]) == + 0; + }), + target_image_points.end()); + source_landmarks.erase(std::remove_if(source_landmarks.begin(), source_landmarks.end(), + [&source_landmarks, &inliers](const Eigen::Vector3d& p) { + return static_cast<int>(inliers[(&p - &*source_landmarks.begin())]) == 0; + }), + source_landmarks.end()); + target_landmarks.erase(std::remove_if(target_landmarks.begin(), target_landmarks.end(), + [&target_landmarks, &inliers](const Eigen::Vector3d& p) { + return static_cast<int>(inliers[(&p - &*target_landmarks.begin())]) == 0; + }), + target_landmarks.end()); + } + + if (params_.only_correspondences) { + return PoseWithCovarianceAndCorrespondences( + lc::PoseWithCovariance(Eigen::Isometry3d::Identity(), lc::PoseCovariance()), + lm::DepthCorrespondences(source_image_points, target_image_points, source_landmarks, target_landmarks), + previous_timestamp_, latest_timestamp_); + } + // TODO(rsoussan): This isn't required with std:optional, remove when upgrade to c++17 and change normals // containers to be boost::optional types boost::optional<const std::vector<Eigen::Vector3d>&> source_normals_ref = @@ -142,7 +198,7 @@ ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::DepthImageCallback( auto target_T_source = aligner_.ComputeRelativeTransform(source_landmarks, target_landmarks, source_normals_ref, target_normals_ref); if (!target_T_source) { - LogWarning("DepthImageCallback: Failed to get relative transform."); + LogError("DepthImageCallback: Failed to get relative transform."); return boost::none; } @@ -155,7 +211,7 @@ ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::DepthImageCallback( if (!lc::PoseCovarianceSane(source_T_target.covariance, params_.position_covariance_threshold, params_.orientation_covariance_threshold)) { - LogWarning("DepthImageCallback: Sanity check failed - invalid covariance."); + LogError("DepthImageCallback: Sanity check failed - invalid covariance."); return boost::none; } diff --git a/localization/depth_odometry/src/parameter_reader.cc b/localization/depth_odometry/src/parameter_reader.cc index 929ffba968..c26ce440e9 100644 --- a/localization/depth_odometry/src/parameter_reader.cc +++ b/localization/depth_odometry/src/parameter_reader.cc @@ -18,11 +18,15 @@ #include <depth_odometry/parameter_reader.h> #include <localization_common/logger.h> +#include <localization_common/utilities.h> #include <msg_conversions/msg_conversions.h> #include <point_cloud_common/parameter_reader.h> #include <vision_common/parameter_reader.h> +#include <opencv2/calib3d.hpp> + namespace depth_odometry { +namespace lc = localization_common; namespace mc = msg_conversions; namespace pc = point_cloud_common; namespace vc = vision_common; @@ -34,6 +38,8 @@ void LoadDepthOdometryWrapperParams(config_reader::ConfigReader& config, DepthOd params.haz_cam_A_haz_depth = Eigen::Affine3d::Identity(); LoadPointToPlaneICPDepthOdometryParams(config, params.icp); LoadImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(config, params.image_features); + params.max_buffer_size = mc::LoadDouble(config, "max_buffer_size"); + params.max_depth_images = mc::LoadDouble(config, "max_depth_images"); } void LoadDepthOdometryParams(config_reader::ConfigReader& config, DepthOdometryParams& params) { @@ -83,8 +89,23 @@ void LoadImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams( params.clahe_clip_limit = mc::LoadDouble(config, "clahe_clip_limit"); params.min_x_distance_to_border = mc::LoadDouble(config, "min_x_distance_to_border"); params.min_y_distance_to_border = mc::LoadDouble(config, "min_y_distance_to_border"); - params.min_num_inliers = mc::LoadInt(config, "min_num_inliers"); + params.min_num_correspondences = mc::LoadInt(config, "min_num_correspondences"); + params.only_correspondences = mc::LoadBool(config, "only_correspondences"); params.refine_estimate = mc::LoadBool(config, "refine_estimate"); + params.cam_intrinsics = lc::LoadCameraIntrinsicsMatrix(config, "haz_cam"); + params.cam_params.reset(new camera::CameraParameters(&config, "haz_cam")); + params.filter_outliers = mc::LoadBool(config, "filter_outliers"); + const std::string method = mc::LoadString(config, "filter_method"); + if (method == "ransac") + params.filter_method = cv::RANSAC; + else if (method == "lmeds") + params.filter_method = cv::LMEDS; + else if (method == "rho") + params.filter_method = cv::RHO; + else + LogFatal("Invalid outlier filter method used"); + params.inlier_threshold = mc::LoadDouble(config, "inlier_threshold"); + params.inlier_probability = mc::LoadDouble(config, "inlier_probability"); if (params.refine_estimate) LoadPointToPlaneICPDepthOdometryParams(config, params.point_to_plane_icp); LoadDepthOdometryParams(config, params); } diff --git a/localization/depth_odometry/src/utilities.cc b/localization/depth_odometry/src/utilities.cc index a17048e8e7..5e8f059fdd 100644 --- a/localization/depth_odometry/src/utilities.cc +++ b/localization/depth_odometry/src/utilities.cc @@ -32,7 +32,7 @@ ff_msgs::DepthOdometry DepthOdometryMsg(const PoseWithCovarianceAndCorrespondenc depth_odometry_msg.correspondences = CorrespondencesMsg(sensor_F_source_T_target.depth_correspondences); depth_odometry_msg.valid_image_points = sensor_F_source_T_target.depth_correspondences.valid_image_points; depth_odometry_msg.valid_points_3d = sensor_F_source_T_target.depth_correspondences.valid_3d_points; - lc::TimeToHeader(sensor_F_source_T_target.target_time, depth_odometry_msg.header); + lc::TimeToHeader(sensor_F_source_T_target.source_time, depth_odometry_msg.header); lc::TimeToMsg(sensor_F_source_T_target.source_time, depth_odometry_msg.odometry.source_time); lc::TimeToMsg(sensor_F_source_T_target.target_time, depth_odometry_msg.odometry.target_time); depth_odometry_msg.runtime = runtime; diff --git a/localization/depth_odometry/test/test_utilities.cc b/localization/depth_odometry/test/test_utilities.cc index 9e9d075cce..225c597898 100644 --- a/localization/depth_odometry/test/test_utilities.cc +++ b/localization/depth_odometry/test/test_utilities.cc @@ -234,8 +234,10 @@ DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams() { params.clahe_clip_limit = 40; params.min_x_distance_to_border = 0; params.min_y_distance_to_border = 0; - params.min_num_inliers = 0; + params.min_num_correspondences = 0; + params.only_correspondences = false; params.refine_estimate = false; + params.filter_outliers = false; DefaultDepthOdometryParams(params); return params; } @@ -254,6 +256,8 @@ DepthOdometryWrapperParams DefaultDepthOdometryWrapperParams() { params.haz_cam_A_haz_depth = Eigen::Affine3d::Identity(); params.icp = DefaultPointToPlaneICPDepthOdometryParams(); params.image_features = DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(); + params.max_buffer_size = 10; + params.max_depth_images = 10; return params; } } // namespace depth_odometry diff --git a/localization/doc/astroloc_library_quickstart.pdf b/localization/doc/astroloc_library_quickstart.pdf new file mode 100644 index 0000000000..d4393b939e Binary files /dev/null and b/localization/doc/astroloc_library_quickstart.pdf differ diff --git a/localization/doc/images/graph_localizer.png b/localization/doc/images/graph_localizer.png new file mode 100644 index 0000000000..362abb331f Binary files /dev/null and b/localization/doc/images/graph_localizer.png differ diff --git a/localization/doc/images/graph_optimizer.png b/localization/doc/images/graph_optimizer.png new file mode 100644 index 0000000000..804ab5e3fd Binary files /dev/null and b/localization/doc/images/graph_optimizer.png differ diff --git a/localization/doc/images/graph_vio.png b/localization/doc/images/graph_vio.png new file mode 100644 index 0000000000..334528651b Binary files /dev/null and b/localization/doc/images/graph_vio.png differ diff --git a/localization/doc/images/sliding_window_graph_optimizer.png b/localization/doc/images/sliding_window_graph_optimizer.png new file mode 100644 index 0000000000..484355719f Binary files /dev/null and b/localization/doc/images/sliding_window_graph_optimizer.png differ diff --git a/localization/doc/src/absolute_pose_factor_adder.tex b/localization/doc/src/absolute_pose_factor_adder.tex new file mode 100644 index 0000000000..10c6523a29 --- /dev/null +++ b/localization/doc/src/absolute_pose_factor_adder.tex @@ -0,0 +1,45 @@ +\begin{minted}[]{c++} +#include <factor_adders/absolute_pose_factor_adder_params.h> +#include <factor_adders/single_measurement_based_factor_adder.h> +#include <localization_common/time.h> +#include <localization_measurements/timestamped_pose_with_covariance.h> + +class AbsolutePoseFactorAdder + : public SingleMeasurementBasedFactorAdder< + localization_measurements:: + TimestampedPoseWithCovariance> { + public: + AbsolutePoseFactorAdder( + const AbsolutePoseFactorAdderParams& params, + const std::shared_ptr<RelativePoseNodeAdder> node_adder) + : SingleMeasurementBasedFactorAdder< + localization_measurements:: + TimestampedPoseWithCovariance>(params), + params_(params), + node_adder_(node_adder) {} + + private: + int AddFactorsForSingleMeasurement( + const localization_measurements:: + TimestampedPoseWithCovariance& measurement, + gtsam::NonlinearFactorGraph& factors) final { + node_adder_->AddNode(measurement.timestamp, factors); + const auto keys = node_adder_->Keys(measurement.timestamp); + // First key is pose key + const auto& pose_key = keys[0]; + const auto pose_noise = + gtsam::Prior<gtsam::Pose3>::shared_ptr pose_prior_factor( + new gtsam::Prior<gtsam::Pose3>( + pose_key, measurement.pose.pose, pose_noise)); + factors.push_back(pose_prior_factor); + } + + bool CanAddFactor( + const localization_common::Time time) const final { + return node_adder_->CanAddNode(time); + } + + std::shared_ptr<PoseNodeAdderType> node_adder_; + AbsolutePoseFactorAdderParams params_; +}; +\end{minted} \ No newline at end of file diff --git a/localization/doc/src/bib.bib b/localization/doc/src/bib.bib new file mode 100644 index 0000000000..e9e19e6870 --- /dev/null +++ b/localization/doc/src/bib.bib @@ -0,0 +1,10 @@ +@article{kaess2012isam2, + title={iSAM2: Incremental smoothing and mapping using the Bayes tree}, + author={Kaess, Michael and Johannsson, Hordur and Roberts, Richard and Ila, Viorela and Leonard, John J and Dellaert, Frank}, + journal={The International Journal of Robotics Research}, + volume={31}, + number={2}, + pages={216--235}, + year={2012}, + publisher={Sage Publications Sage UK: London, England} +} diff --git a/localization/doc/src/commands.tex b/localization/doc/src/commands.tex new file mode 100644 index 0000000000..b8623c2814 --- /dev/null +++ b/localization/doc/src/commands.tex @@ -0,0 +1,63 @@ +\newcommand{\pluseq}{\mathrel{+}=} +\newcommand{\timeseq}{\mathrel{*}=} +\newcommand{\vect}[1]{\boldsymbol{#1}} +\newcommand{\matr}[1]{\boldsymbol{#1}} +\newcommand{\tra}[1]{{\textrm{Tr}}({#1})} +\newcommand{\trafo}[2]{\substack{\scriptscriptstyle{{#1}} \\ \scriptscriptstyle{{#2}}}\textbf{T}} +\newcommand{\rot}[2]{\substack{\scriptscriptstyle{{#1}} \\ \scriptscriptstyle{{#2}}}\textbf{R}} +\newcommand{\trans}[2]{\substack{\scriptscriptstyle{{#1}} \\ \scriptscriptstyle{{#2}}}\textbf{t}} +\newcommand{\rotlie}[2]{\substack{\scriptscriptstyle{{#1}} \\ \scriptscriptstyle{{#2}}}\boldsymbol{\phi}} +\newcommand{\rotliepert}[0]{\rotlie{b}{\bar{b}}_{i}} +\newcommand{\translie}[2]{\substack{\scriptscriptstyle{{#1}} \\ \scriptscriptstyle{{#2}}}\boldsymbol{\rho}} +\newcommand{\transliepert}[0]{\translie{b}{\bar{b}}_{i}} +\newcommand{\trafolie}[2]{\substack{\scriptscriptstyle{{#1}} \\ \scriptscriptstyle{{#2}}}\boldsymbol{\varphi}} +\newcommand{\trafoliepert}[0]{\trafolie{b}{\bar{b}}_{i}} +\newcommand{\trafolietrafo}[0]{\trafo{b}{\bar{b}}} + +%\newcommand{\trafo}[2]{\substack{\scriptscriptstyle{#1} \\ \scriptscriptstyle{#2}}T} +%\newcommand{\rot}[2]{\substack{\scriptscriptstyle{#1} \\ \scriptscriptstyle{#2}}R} +\newcommand{\expmap}[1]{\exp(#1)} +%\newcommand{\Expmap}[1]{Exp(#1)} +\newcommand{\logmap}[1]{\log(#1)} +\newcommand{\Frame}[1]{\textbf{#1}} +\newcommand{\NavState}[0]{S} +\newcommand{\liealg}[2]{\substack{\scriptscriptstyle{\textbf{#1}} \\ \scriptscriptstyle{\textbf{#2}}}\vect{\phi}} +\newcommand{\soIII}{so(3)} +\newcommand{\SOIII}{SO(3)} +\newcommand{\seIII}{se(3)} +\newcommand{\SEIII}{SE(3)} +\newcommand{\unitquat}[2]{\substack{\scriptscriptstyle{\textbf{#1}} \\ \scriptscriptstyle{\textbf{#2}}}\Phi} +\newcommand{\Vee}[1]{{\left[#1\right]}^{\textrm{v}}} +\newcommand{\skewhat}[1]{{{\left[#1\right]}^{\wedge}}} +\newcommand{\Rn}[1]{\mathbb{R}^{#1}} +\newcommand{\ith}{\textrm{i}^{\textrm{th}}} +\newcommand{\inv}[1]{#1^{-1}} +\newcommand{\ithindex}[1]{\begin{bmatrix}#1 \end{bmatrix}_{i}} +\newcommand{\fourblock}[4]{\renewcommand\arraystretch{1.3} +\mleft[ +\begin{array}{c|c} + #1 & #2 \\ \hline + #3 & #4 +\end{array}\mright]} +\newcommand{\trafoblock}[2]{\fourblock{#1}{#2}{0}{1}} +\newcommand{\vectortwodblock}[2]{\renewcommand\arraystretch{1.3} +\mleft[ +\begin{array}{c} + #1 \\ \hline + #2 +\end{array}\mright]} +\newcommand{\rowvectortwodblock}[2]{ +\mleft[ +\begin{array}{c|c} + #1 +\end{array}\mright]} +\newcommand{\blocklog}[1]{\log\left[#1\right]} +\newcommand{\adjoint}[1]{\textrm{Ad}\left[#1\right]} +\newcommand{\homonorm}[1]{h\left(\vect{#1}\right)} +\newcommand{\projection}[1]{\pi\left({#1}\right)} +\newcommand{\camintrinsics}{\matr{C}} + + + +%\newcommand{\boxplus}{\mathbin{\mathop\amssymbboxplus}} +%\newcommand{\boxminus}{\mathbin{\mathop\amssymbboxminus}} \ No newline at end of file diff --git a/localization/doc/src/factor_adder.pdf b/localization/doc/src/factor_adder.pdf new file mode 100644 index 0000000000..d944d8ce4c Binary files /dev/null and b/localization/doc/src/factor_adder.pdf differ diff --git a/localization/doc/src/graph_localizer_h.tex b/localization/doc/src/graph_localizer_h.tex new file mode 100644 index 0000000000..22dae2da01 --- /dev/null +++ b/localization/doc/src/graph_localizer_h.tex @@ -0,0 +1,42 @@ +\begin{minted}[]{c++} +#include <localization_measurements/pose_measurement.h> +#include <sliding_window_graph_optimizer/sliding_window_graph_optimizer.h> + +#include "absolute_pose_factor_adder.h" +#include "localizer_params.h" +#include "relative_pose_node_adder.h" + +class Localizer : public sliding_window_graph_optimizer:: + SlidingWindowGraphOptimizer { + public: + explicit Localizer(const LocalizerParams& params) + : params_(params) { + // Register factor and node adders + AddFactorAdder(factor_adder_); + AddSlidingWindowNodeAdder(node_adder_); + } + + void AddRelativePoseMeasurement( + const localization_measurements:: + TimestampedPoseWithCovariance& measurement) { + node_adder_->AddMeasurement(measurement); + } + + void AddAbsolutePoseMeasurement( + const localization_measurements:: + TimestampedPoseWithCovariance& measurement) { + factor_adder_->AddMeasurement(measurement); + } + + const TimestampedNodes<gtsam::Pose3>& timestamped_nodes() + const { + return node_adder_->nodes(); + } + + private: + LocalizerParams params_; + + std::shared_ptr<AbsolutePoseFactorAdder> factor_adder_; + std::shared_ptr<RelativePoseNodeAdder> node_adder_; +}; +\end{minted} \ No newline at end of file diff --git a/localization/doc/src/graph_opt.pdf b/localization/doc/src/graph_opt.pdf new file mode 100644 index 0000000000..ea93fd069f Binary files /dev/null and b/localization/doc/src/graph_opt.pdf differ diff --git a/localization/doc/src/interfacing_cc.tex b/localization/doc/src/interfacing_cc.tex new file mode 100644 index 0000000000..56da479b12 --- /dev/null +++ b/localization/doc/src/interfacing_cc.tex @@ -0,0 +1,27 @@ +\begin{minted}[]{c++} +#include "localizer.h" + +int main() { + Localizer localizer(LocalizerParams()); + // Add relative and absolute pose measurements at successive + // timestamps + for (int i = 0; i < 10; ++i) { + localizer.AddRelativePoseMeasurement( + RandomPoseMeasurement(i)); + localizer.AddAbsolutePoseMeasurement( + RandomPoseMeasurement(i)); + } + + localizer.Update(); + + // Access optimized timestamped nodes + const auto& timestamped_nodes = localizer.timestamped_nodes(); + // Access optimized GTSAM values + const auto& values = localizer.values(); + // Access GTSAM factors + const auto& factors = localizer.factors(); + // Compute covariance for a node at timestamp 1 + const auto keys = timestamped_nodes.Keys(1); + const auto covariance = localizer.Covariance(keys[0]); +} +\end{minted} \ No newline at end of file diff --git a/localization/doc/src/loc_graph.pdf b/localization/doc/src/loc_graph.pdf new file mode 100644 index 0000000000..6305b793a2 Binary files /dev/null and b/localization/doc/src/loc_graph.pdf differ diff --git a/localization/doc/src/node_adder.pdf b/localization/doc/src/node_adder.pdf new file mode 100644 index 0000000000..38fbf4917b Binary files /dev/null and b/localization/doc/src/node_adder.pdf differ diff --git a/localization/doc/src/overview.tex b/localization/doc/src/overview.tex new file mode 100644 index 0000000000..9afce9a2ff --- /dev/null +++ b/localization/doc/src/overview.tex @@ -0,0 +1,59 @@ +\section{AstroLoc Library Overview} +The AstroLoc library offers useful tools for localization, odometry, calibration, mapping, and more using GTSAM graph-based optimization. +The library consists of graph optimizer and sliding window graph optimizer classes that use node adders and factor adders for node and factor creation, where nodes are state parameters optimized in the graph. +\begin{figure}[ht] + \centering +\includegraphics[width=0.9\textwidth]{graph_opt.pdf} + \caption{A graph optimizer contains node adders that create nodes and relative factors connecting them and factor adders that create measurement-based factors for certain nodes in the graph.} + \label{img:graph_opt} +\end{figure} +The library contains packages for each of these objects and has useful factor and node adders already implemented to help perform localization, odometry, calibration, and mapping. +\subsection{Optimizers}\label{sec:overview} +Nonlinear and ISAM2 optimizers can be used for graph optimization. +ISAM2 \cite{kaess2012isam2} is particularly helpful for mapping when keeping a large history of nodes to optimize, whereas often nonlinear optimization is sufficient for performing sliding window optimization. +\subsection{Graph Optimizer and Sliding Window Graph Optimizer} +\subsubsection{Graph Optimizer} +The graph optimizer shown in Figure ~\ref{img:graph_opt} contains a GTSAM factor graph and factor and node adders that process input measurements and output the nodes and factors used for optimization. +It contains functions to perform optimization and access covariances for its nodes. +\subsubsection{Sliding Window Graph Optimizer} +The sliding window graph optimizer in Figure ~\ref{img:sliding_window_graph_opt} extends the graph optimizer for sliding window optimization and uses sliding window node adders that remove old nodes and factors that fall outside of the window. +It uses an Update() function call that adds factors from factor adders up to the latest measurements, optimizes the graph, and slides the window as required. +The sliding window size is defined in both a maximum duration and number of nodes and is configured in the sliding window parameters. +\begin{figure}[ht] + \centering +\includegraphics[width=0.9\textwidth]{sliding_window_graph_opt.pdf} + \caption{The sliding window graph optimizer uses sliding window node adders that remove old nodes and factors as required.} + \label{img:sliding_window_graph_opt} +\end{figure} +\subsection{Node Adders} +The node adders displayed in Figure ~\ref{img:node_adder} define the node type to be optimized and are in charge of adding relative factors and nodes to the graph for those types. +A node adder is templated on measurement, node, timestamped nodes, and node adder model types. +\begin{figure}[H] + \centering +\includegraphics[width=0.9\textwidth]{node_adder.pdf} + \caption{A node adder taking measurements and using a node adder model to generate timestamped nodes and relative factors.} + \label{img:node_adder} +\end{figure} +\subsubsection{Measurement Type} +The measurement is passed to the node adder model and contains the information necessary to create future nodes and relative factors. +\subsubsection{Node and TimestampedNodes Types} +The node type specifies the state parameter optimized in the graph optimizer. +The timestamped nodes type specifies the container for interfacing with the nodes in the optimizer. +For simple, single-typed timestamped values, such as a pose or point, the TimestampedNodes class should be used. +For nodes containing multiple types (i.e. a pose, velocity, and bias for VIO), use the TimestampedCombinedNodes class and make sure to override the required functions for adding and accessing nodes. +See CombinedNavStateNodes in the nodes package for an example. +Multiple node adders can exist in a single graph optimizer that handle different node types, and all these are optimized together in the same graph. +\subsubsection{Node Adder Models} +The node adder model handles adding nodes and relative factors and stores measurements used for creating these. +Prefer using the BetweenFactorMeasurementBasedTimestampedNodeAdderModel class which adds relative factors using gtsam::BetweenFactors for a single-valued node type. +For more complicated relative factors, use MeasurementBasedTimestampedNodeAdderModel and customize the functions for adding relative nodes and factors as desired. +See the CombinedNavStateNodeAdder in the node adders package for an example. +\subsection{Factor Adders} +The factor adders illustrated in Figure ~\ref{img:factor_adder} add measurement-based factors that can depend on single or multiple node values. +For factor creation that depends on a single measurement at a time, use SingleMeasurementBasedFactorAdder, otherwise use MeasurementBasedFactorAdder. +\begin{figure}[h] + \centering +\includegraphics[width=0.9\textwidth]{factor_adder.pdf} + \caption{A factor adder taking measurements and generating factors. Nodes are created for the graph at the measurement timestamp using a shared node adder.} + \label{img:factor_adder} +\end{figure} \ No newline at end of file diff --git a/localization/doc/src/quickstart.tex b/localization/doc/src/quickstart.tex new file mode 100644 index 0000000000..c867faeea3 --- /dev/null +++ b/localization/doc/src/quickstart.tex @@ -0,0 +1,8 @@ +Here we provide an overview of the AstroLoc library and present several examples to help explain the code structure and abilities of the library. +For an overview of the code base and library objects, see ~\ref{sec:overview}. +For a simple localization example, see ~\ref{sec:simple_localizer}. +For more examples, see the various sections after ~\ref{sec:simple_localizer}. + +\input{overview} +\input{simple_localizer} + diff --git a/localization/doc/src/relative_pose_node_adder_h.tex b/localization/doc/src/relative_pose_node_adder_h.tex new file mode 100644 index 0000000000..e876f481e9 --- /dev/null +++ b/localization/doc/src/relative_pose_node_adder_h.tex @@ -0,0 +1,15 @@ +\begin{minted}[]{c++} +#include <localization_measurements/timestamped_pose_with_covariance.h> +#include <node_adders/measurement_based_timestamped_node_adder.h> +#include <nodes/timestamped_nodes.h> + +#include <gtsam/geometry/Pose3.h> + +"relative_pose_node_adder_model.h" + + using RelativePoseNodeAdder = + MeasurementBasedTimestampedNodeAdder< + localization_measurements::TimestampedPoseWithCovariance, + gtsam::Pose3, nodes::TimestampedNodes<gtsam::Pose3>, + RelativePoseNodeAdderModel>; +\end{minted} \ No newline at end of file diff --git a/localization/doc/src/relative_pose_node_adder_model_h.tex b/localization/doc/src/relative_pose_node_adder_model_h.tex new file mode 100644 index 0000000000..1d205eeac3 --- /dev/null +++ b/localization/doc/src/relative_pose_node_adder_model_h.tex @@ -0,0 +1,68 @@ +\begin{minted}[]{c++} +#include <localization_common/pose_with_covariance_interpolater.h> +#include <localization_measurements/timestamped_pose_with_covariance.h> +#include <node_adders/between_factor_node_adder_model.h> +#include <nodes/timestamped_nodes.h> + +#include <gtsam/geometry/Pose3.h> +#include <gtsam/inference/Key.h> + +#include <utility> + +class RelativePoseNodeAdderModel + : public BetweenFactorMeasurementBasedTimestampedNodeAdderModel< + localization_measurements::TimestampedPoseWithCovariance, + gtsam::Pose3> { + public: + using NodesType = nodes::TimestampedNodes<gtsam::Pose3>; + using Params = TimestampedNodeAdderModelParams; + + explicit PoseNodeAdderModel(const Params& params) + : Base(params) {} + + gtsam::KeyVector AddNode( + const localization_common::Time timestamp, + NodesType& nodes) const final { + const auto pose = interpolator_.Interpolate(timestamp); + return nodes.Add(timestamp, pose->pose); + } + + boost::optional< + std::pair<gtsam::Pose3, gtsam::SharedNoiseModel>> + RelativeNodeAndNoise( + const localization_common::Time timestamp_a, + const localization_common::Time timestamp_b) const final { + const auto relative_pose = + interpolator_.Relative(timestamp_a, timestamp_b); + const auto relative_pose_noise = + gtsam::noiseModel::Gaussian::Covariance( + relative_pose->covariance); + return std::pair<gtsam::Pose3, gtsam::SharedNoiseModel>( + relative_pose->pose, relative_pose_noise); + } + + void AddMeasurement( + const localization_measurements:: + TimestampedPoseWithCovariance& measurement) { + interpolator_.Add(measurement.time, + measurement.pose_with_covariance); + } + + void RemoveMeasurements( + const localization_common::Time oldest_allowed_time) { + // Keep lower bound so future measurements can be + // interpolated using it. + interpolator_.RemoveBelowLowerBoundValues( + oldest_allowed_time); + } + bool CanAddNode( + const localization_common::Time timestamp) const final { + return interpolator_.WithinBounds(timestamp); + ; + } + + private: + localization_common::PoseWithCovarianceInterpolater + interpolator_; +}; +\end{minted} \ No newline at end of file diff --git a/localization/doc/src/root.tex b/localization/doc/src/root.tex new file mode 100644 index 0000000000..aa0ac1bb5d --- /dev/null +++ b/localization/doc/src/root.tex @@ -0,0 +1,34 @@ +\documentclass{article} +\usepackage[utf8]{inputenc} + +\usepackage[noend]{algorithmic} +\usepackage{amsmath} +\usepackage{amsfonts} +\usepackage{amssymb} +\usepackage{graphicx} +\usepackage{mathtools} +\usepackage{hyperref} +\usepackage{physics} +\usepackage[thinc]{esdiff} +\usepackage{listings} +\usepackage{mleftright} +\usepackage{mathbbol} +\usepackage{xcolor} +\usepackage{minted} +\input{commands} +\setcounter{tocdepth}{4} +\setcounter{secnumdepth}{4} + +\title{AstroLoc Library Quickstart} +\author{Ryan Soussan} +\date{April 2023} + +\begin{document} + +\maketitle +\input{quickstart} + +\bibliographystyle{IEEEtran} +\bibliography{bib} + +\end{document} diff --git a/localization/doc/src/simple_localizer.pdf b/localization/doc/src/simple_localizer.pdf new file mode 100644 index 0000000000..a4752cf844 Binary files /dev/null and b/localization/doc/src/simple_localizer.pdf differ diff --git a/localization/doc/src/simple_localizer.tex b/localization/doc/src/simple_localizer.tex new file mode 100644 index 0000000000..41ead5a1b1 --- /dev/null +++ b/localization/doc/src/simple_localizer.tex @@ -0,0 +1,53 @@ +\section{Simple Localizer}\label{sec:simple_localizer} +Here we will create a simple localizer taking relative odometry pose measurements and absolute map-based pose measurements as input. +The structure of the localizer is shown in Figure ~\ref{img:simple_localizer}. + +\begin{figure}[ht] + \centering +\includegraphics[width=0.9\textwidth]{simple_localizer.pdf} + \caption{Localizer structure.} + \label{img:simple_localizer} +\end{figure} +The localizer takes relative and absolute pose measurements and passes these to the node adder and factor adder respectively. +These are described in more detail below. \par + +The graph structure is shown in Figure ~\ref{img:loc_graph}. + +\begin{figure}[ht] + \centering +\includegraphics[width=0.9\textwidth]{loc_graph.pdf} + \caption{Localization graph structure. Optimized pose nodes are connected by pose between factors. An initial prior constrains the first node and absolute measurement priors are added to future nodes.} + \label{img:loc_graph} +\end{figure} + +Pose nodes are optimized at various timestamps. +An initial pose prior factor is added to the first pose node, while pose between factors connect successive pose nodes. +Absolute pose priors are added using incoming measurements that trigger the creation of new pose nodes and relative factors at the same timestamp. + +\subsection{Localizer Sliding Window Graph Optimizer} +We will make the localizer a sliding-window graph optimizer so that old nodes and factors are removed. +\input{graph_localizer_h.tex} +The graph localizer includes both the relative pose node adder and absolute pose factor adder. + +\subsection{Relative Pose Node Adder} +The pose node adder uses timestamped relative pose measurements to create gtsam::Pose3 nodes. +It relies on the pose node adder model for node and relative factor creation. +\input{relative_pose_node_adder_h.tex} +\subsubsection{Pose Measurement} +A timestamped pose with covariance is used to generate values for future node and relative factors. +\subsubsection{Pose Node and TimestampedNodes} +The node type is a gtsam::Pose3 and the timestamped nodes type is a TimestampedNodes object since the node type is a single value. +\subsubsection{Relative Pose Node Adder Model} +\input{relative_pose_node_adder_model_h.tex} +The pose node adder model uses a pose interpolator to generate required relative poses and covariances using input relative pose measurements. +Since the model is a between factor adder model, it inserts gtsam::Pose3 between factors as relative factors. +\subsection{Absolute Pose Factor Adder} +The absolute pose factor adder creates a GTSAM pose prior factor for each absolute pose measurement. +Since each created factor uses a single measurement, the factor adder is a SingleMeasurementBasedFactorAdder. +When adding the prior factor, it uses the relative pose node adder to add a pose node at the same timestamp as the prior factor. +\input{absolute_pose_factor_adder.tex} +\subsection{Interfacing with Localizer} +Measurements are passed to the node and factor adders at various timestamps. +Optimization, factor and node creation, and sliding the window occur during the Update() call. +Various helper functions exist for accessing optimized values and covariances from the localizer. +\input{interfacing_cc} \ No newline at end of file diff --git a/localization/doc/src/sliding_window_graph_opt.pdf b/localization/doc/src/sliding_window_graph_opt.pdf new file mode 100644 index 0000000000..99fde3e653 Binary files /dev/null and b/localization/doc/src/sliding_window_graph_opt.pdf differ diff --git a/localization/factor_adders/CMakeLists.txt b/localization/factor_adders/CMakeLists.txt new file mode 100644 index 0000000000..e9298be4ee --- /dev/null +++ b/localization/factor_adders/CMakeLists.txt @@ -0,0 +1,101 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(factor_adders) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + graph_factors + localization_common + localization_measurements + node_adders + vision_common +) + +# Find GTSAM +find_package(GTSAM REQUIRED) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +# include ff_nodelet to get ff_util header files since these aren't exposed elsewhere +catkin_package( + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS graph_factors localization_common localization_measurements node_adders vision_common +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${GLOG_INCLUDE_DIRS} +) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + # Loc factor adder test + add_rostest_gtest(test_loc_factor_adder + test/test_loc_factor_adder.test + test/test_loc_factor_adder.cc + ) + target_link_libraries(test_loc_factor_adder + ${catkin_LIBRARIES} + ) + # Single measurement based factor adder test + add_rostest_gtest(test_single_measurement_based_factor_adder + test/test_single_measurement_based_factor_adder.test + test/test_single_measurement_based_factor_adder.cc + ) + target_link_libraries(test_single_measurement_based_factor_adder + ${catkin_LIBRARIES} + ) + # Standstill factor adder test + add_rostest_gtest(test_standstill_factor_adder + test/test_standstill_factor_adder.test + test/test_standstill_factor_adder.cc + ) + target_link_libraries(test_standstill_factor_adder + ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_vo_smart_projection_factor_adder + test/test_vo_smart_projection_factor_adder.test + test/test_vo_smart_projection_factor_adder.cc + ) + target_link_libraries(test_vo_smart_projection_factor_adder + ${catkin_LIBRARIES} + ) +endif() + +############# +## Install ## +############# + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/factor_adders/include/factor_adders/depth_odometry_factor_adder.h b/localization/factor_adders/include/factor_adders/depth_odometry_factor_adder.h new file mode 100644 index 0000000000..9075bc883f --- /dev/null +++ b/localization/factor_adders/include/factor_adders/depth_odometry_factor_adder.h @@ -0,0 +1,136 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef FACTOR_ADDERS_DEPTH_ODOMETRY_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_DEPTH_ODOMETRY_FACTOR_ADDER_H_ + +#include <factor_adders/depth_odometry_factor_adder_params.h> +#include <factor_adders/single_measurement_based_factor_adder.h> +#include <graph_factors/point_to_point_between_factor.h> +#include <localization_common/time.h> +#include <localization_common/utilities.h> +#include <localization_measurements/depth_odometry_measurement.h> + +#include <gtsam/linear/NoiseModel.h> +#include <gtsam/slam/BetweenFactor.h> + +namespace factor_adders { +// Adds GTSAM Pose Between factors for depth odometry relative pose measurements +// point to point between factors for depth odometry correspondence measurements. +// Adds pose nodes using PoseNodeAdder at the same timestamps as the measurements. +template <class PoseNodeAdderType> +class DepthOdometryFactorAdder + : public SingleMeasurementBasedFactorAdder<localization_measurements::DepthOdometryMeasurement> { + public: + DepthOdometryFactorAdder(const DepthOdometryFactorAdderParams& params, + const std::shared_ptr<PoseNodeAdderType> node_adder); + + private: + // Creates a pose between factor or point to point between factors and pose nodes for the given measurement. + int AddFactorsForSingleMeasurement(const localization_measurements::DepthOdometryMeasurement& measurement, + gtsam::NonlinearFactorGraph& factors) final; + + bool CanAddFactor(const localization_measurements::DepthOdometryMeasurement& measurement) const final; + + std::shared_ptr<PoseNodeAdderType> node_adder_; + DepthOdometryFactorAdderParams params_; +}; + +template <class PoseNodeAdderType> +DepthOdometryFactorAdder<PoseNodeAdderType>::DepthOdometryFactorAdder( + const DepthOdometryFactorAdderParams& params, const std::shared_ptr<PoseNodeAdderType> node_adder) + : SingleMeasurementBasedFactorAdder<localization_measurements::DepthOdometryMeasurement>(params), + params_(params), + node_adder_(node_adder) {} + +template <class PoseNodeAdderType> +int DepthOdometryFactorAdder<PoseNodeAdderType>::AddFactorsForSingleMeasurement( + const localization_measurements::DepthOdometryMeasurement& measurement, gtsam::NonlinearFactorGraph& factors) { + const double translation_norm = measurement.odometry.sensor_F_source_T_target.pose.translation().norm(); + if (params_.reject_large_translation_norm && translation_norm > params_.pose_translation_norm_threshold) { + LogDebug("AddFactors: Ignoring pose with large translation norm. Norm: " + << translation_norm << ", threshold: " << params_.pose_translation_norm_threshold); + return 0; + } + + if (!node_adder_->AddNode(measurement.odometry.source_time, factors) || + !node_adder_->AddNode(measurement.odometry.target_time, factors)) { + LogError("AddFactorsForSingleMeasurement: Failed to add nodes at source and target time."); + return 0; + } + const auto keys_a = node_adder_->Keys(measurement.odometry.source_time); + // First key is pose key + const auto& pose_key_a = keys_a[0]; + const auto keys_b = node_adder_->Keys(measurement.odometry.target_time); + const auto& pose_key_b = keys_b[0]; + + if (params_.use_points_between_factor) { + int num_between_factors = 0; + for (int i = 0; i < measurement.correspondences.source_3d_points.size() && + num_between_factors < params_.max_num_points_between_factors; + ++i) { + const Eigen::Vector3d& sensor_t_point_source = measurement.correspondences.source_3d_points[i]; + const Eigen::Vector3d& sensor_t_point_target = measurement.correspondences.target_3d_points[i]; + + double noise_sigma = 1.0; + if (params_.scale_point_between_factors_with_inverse_distance) { + noise_sigma = sensor_t_point_source.norm(); + } else if (params_.scale_point_between_factors_with_estimate_error) { + const Eigen::Vector3d estimate_error = + sensor_t_point_source - measurement.odometry.sensor_F_source_T_target.pose * sensor_t_point_target; + noise_sigma = estimate_error.norm(); + } + if (params_.reject_large_point_to_point_error) { + const Eigen::Vector3d estimate_error = + sensor_t_point_source - measurement.odometry.sensor_F_source_T_target.pose * sensor_t_point_target; + if (estimate_error.norm() > params_.point_to_point_error_threshold) { + continue; + } + } + + const auto points_between_factor_noise = localization_common::Robust( + gtsam::noiseModel::Isotropic::Sigma(3, noise_sigma * params_.point_noise_scale), params_.huber_k); + gtsam::PointToPointBetweenFactor::shared_ptr points_between_factor( + new gtsam::PointToPointBetweenFactor(sensor_t_point_source, sensor_t_point_target, params_.body_T_sensor, + points_between_factor_noise, pose_key_a, pose_key_b)); + factors.push_back(points_between_factor); + ++num_between_factors; + } + LogDebug("AddFactors: Added " << num_between_factors << " points between factors."); + return num_between_factors; + } else { + const auto relative_pose_noise = localization_common::Robust( + gtsam::noiseModel::Gaussian::Covariance(params_.pose_covariance_scale * + measurement.odometry.body_F_source_T_target.covariance), + params_.huber_k); + const gtsam::BetweenFactor<gtsam::Pose3>::shared_ptr pose_between_factor(new gtsam::BetweenFactor<gtsam::Pose3>( + pose_key_a, pose_key_b, localization_common::GtPose(measurement.odometry.body_F_source_T_target.pose), + relative_pose_noise)); + factors.push_back(pose_between_factor); + return 1; + } +} + +template <class PoseNodeAdderType> +bool DepthOdometryFactorAdder<PoseNodeAdderType>::CanAddFactor( + const localization_measurements::DepthOdometryMeasurement& measurement) const { + return node_adder_->CanAddNode(measurement.odometry.source_time) && + node_adder_->CanAddNode(measurement.odometry.target_time); +} +} // namespace factor_adders + +#endif // FACTOR_ADDERS_DEPTH_ODOMETRY_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/depth_odometry_factor_adder_params.h b/localization/factor_adders/include/factor_adders/depth_odometry_factor_adder_params.h similarity index 61% rename from localization/graph_localizer/include/graph_localizer/depth_odometry_factor_adder_params.h rename to localization/factor_adders/include/factor_adders/depth_odometry_factor_adder_params.h index 95dc518a7b..f16f7e3b16 100644 --- a/localization/graph_localizer/include/graph_localizer/depth_odometry_factor_adder_params.h +++ b/localization/factor_adders/include/factor_adders/depth_odometry_factor_adder_params.h @@ -16,24 +16,27 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_DEPTH_ODOMETRY_FACTOR_ADDER_PARAMS_H_ -#define GRAPH_LOCALIZER_DEPTH_ODOMETRY_FACTOR_ADDER_PARAMS_H_ +#ifndef FACTOR_ADDERS_DEPTH_ODOMETRY_FACTOR_ADDER_PARAMS_H_ +#define FACTOR_ADDERS_DEPTH_ODOMETRY_FACTOR_ADDER_PARAMS_H_ -#include <graph_optimizer/factor_adder_params.h> +#include <factor_adders/factor_adder_params.h> #include <gtsam/geometry/Pose3.h> -namespace graph_localizer { -struct DepthOdometryFactorAdderParams : public graph_optimizer::FactorAdderParams { - double noise_scale; +namespace factor_adders { +struct DepthOdometryFactorAdderParams : public FactorAdderParams { + double pose_covariance_scale; + double point_noise_scale; bool use_points_between_factor; - double position_covariance_threshold; - double orientation_covariance_threshold; - double point_to_point_error_threshold; + bool scale_point_between_factors_with_inverse_distance; + bool scale_point_between_factors_with_estimate_error; + bool reject_large_translation_norm; double pose_translation_norm_threshold; + bool reject_large_point_to_point_error; + double point_to_point_error_threshold; int max_num_points_between_factors; gtsam::Pose3 body_T_sensor; }; -} // namespace graph_localizer +} // namespace factor_adders -#endif // GRAPH_LOCALIZER_DEPTH_ODOMETRY_FACTOR_ADDER_PARAMS_H_ +#endif // FACTOR_ADDERS_DEPTH_ODOMETRY_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/factor_adders/include/factor_adders/factor_adder.h b/localization/factor_adders/include/factor_adders/factor_adder.h new file mode 100644 index 0000000000..d15e2c762a --- /dev/null +++ b/localization/factor_adders/include/factor_adders/factor_adder.h @@ -0,0 +1,60 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_FACTOR_ADDER_H_ + +#include <factor_adders/factor_adder_params.h> +#include <localization_common/time.h> + +#include <gtsam/nonlinear/NonlinearFactorGraph.h> + +#include <boost/serialization/serialization.hpp> + +#include <vector> + +namespace factor_adders { +// Adds factors to a graph. Base class for measurement-based factor adder. +class FactorAdder { + public: + explicit FactorAdder(const FactorAdderParams& params) : params_(params) {} + + // Default constructor for serialization only + FactorAdder() = default; + + virtual ~FactorAdder() = default; + + // Add factors in valid time range to existing factor graph. + // Returns number of added factors. + virtual int AddFactors(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, gtsam::NonlinearFactorGraph& factors) = 0; + + protected: + FactorAdderParams params_; + + private: + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_NVP(params_); + } +}; +} // namespace factor_adders + +#endif // FACTOR_ADDERS_FACTOR_ADDER_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/factor_adder_params.h b/localization/factor_adders/include/factor_adders/factor_adder_params.h similarity index 80% rename from localization/graph_optimizer/include/graph_optimizer/factor_adder_params.h rename to localization/factor_adders/include/factor_adders/factor_adder_params.h index 73b14436d0..fa54195e4e 100644 --- a/localization/graph_optimizer/include/graph_optimizer/factor_adder_params.h +++ b/localization/factor_adders/include/factor_adders/factor_adder_params.h @@ -16,14 +16,14 @@ * under the License. */ -#ifndef GRAPH_OPTIMIZER_FACTOR_ADDER_PARAMS_H_ -#define GRAPH_OPTIMIZER_FACTOR_ADDER_PARAMS_H_ +#ifndef FACTOR_ADDERS_FACTOR_ADDER_PARAMS_H_ +#define FACTOR_ADDERS_FACTOR_ADDER_PARAMS_H_ -namespace graph_optimizer { +namespace factor_adders { struct FactorAdderParams { bool enabled; double huber_k; }; -} // namespace graph_optimizer +} // namespace factor_adders -#endif // GRAPH_OPTIMIZER_FACTOR_ADDER_PARAMS_H_ +#endif // FACTOR_ADDERS_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/factor_adders/include/factor_adders/loc_factor_adder.h b/localization/factor_adders/include/factor_adders/loc_factor_adder.h new file mode 100644 index 0000000000..d6473937ed --- /dev/null +++ b/localization/factor_adders/include/factor_adders/loc_factor_adder.h @@ -0,0 +1,243 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_LOC_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_LOC_FACTOR_ADDER_H_ + +#include <factor_adders/single_measurement_based_factor_adder.h> +#include <factor_adders/loc_factor_adder_params.h> +#include <graph_factors/loc_pose_factor.h> +#include <graph_factors/loc_projection_factor.h> +#include <localization_common/averager.h> +#include <localization_common/logger.h> +#include <localization_common/utilities.h> +#include <localization_measurements/matched_projections_measurement.h> + +#include <vector> + +namespace factor_adders { +// SingleMeasurementBasedFactorAdder that adds either loc projection factors and loc pose priors for given matched +// projection measurements. +template <typename PoseNodeAdderType> +class LocFactorAdder + : public SingleMeasurementBasedFactorAdder<localization_measurements::MatchedProjectionsMeasurement> { + using Base = SingleMeasurementBasedFactorAdder<localization_measurements::MatchedProjectionsMeasurement>; + + public: + LocFactorAdder(const LocFactorAdderParams& params, const std::shared_ptr<PoseNodeAdderType> node_adder); + + // Default constructor for serialization only. + LocFactorAdder() = default; + + private: + // Adds loc projection factor or loc pose factor depending on params. + // If the loc projection factor is selected but fails due to a reprojection error, adds a loc pose factor + // as a fallback. + int AddFactorsForSingleMeasurement( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement, + gtsam::NonlinearFactorGraph& factors) final; + + // Returns whether the node adder can add a node at the provided time. + bool CanAddFactor(const localization_measurements::MatchedProjectionsMeasurement& measurement) const final; + + // Adds a loc pose factor (pose prior) + int AddLocPoseFactor(const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement, + gtsam::NonlinearFactorGraph& factors); + + // Adds a loc projection factor. If a reprojection error occurs, adds a loc pose factor. + int AddLocProjectionFactor( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement, + gtsam::NonlinearFactorGraph& factors); + + // Helper function to add a pose node if needed and return the node's key. + boost::optional<gtsam::Key> AddPoseNode(const localization_common::Time timestamp, + gtsam::NonlinearFactorGraph& factors); + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(node_adder_); + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(num_landmarks_averager_); + ar& BOOST_SERIALIZATION_NVP(pose_prior_noise_sigmas_); + } + + std::shared_ptr<PoseNodeAdderType> node_adder_; + LocFactorAdderParams params_; + localization_common::Averager num_landmarks_averager_ = localization_common::Averager("Num Landmarks"); + gtsam::Vector6 pose_prior_noise_sigmas_; +}; + +// Implementation +template <typename PoseNodeAdderType> +LocFactorAdder<PoseNodeAdderType>::LocFactorAdder(const LocFactorAdderParams& params, + std::shared_ptr<PoseNodeAdderType> node_adder) + : Base(params), params_(params), node_adder_(node_adder) { + pose_prior_noise_sigmas_ = (gtsam::Vector(6) << params_.prior_translation_stddev, params_.prior_translation_stddev, + params_.prior_translation_stddev, params_.prior_quaternion_stddev, + params_.prior_quaternion_stddev, params_.prior_quaternion_stddev) + .finished(); +} + +template <typename PoseNodeAdderType> +int LocFactorAdder<PoseNodeAdderType>::AddFactorsForSingleMeasurement( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement, + gtsam::NonlinearFactorGraph& factors) { + if (matched_projections_measurement.matched_projections.empty()) { + LogDebug("AddFactorsForSingleMeasurement: Empty measurement."); + return 0; + } + + if (static_cast<int>(matched_projections_measurement.matched_projections.size()) < + params_.min_num_matches_per_measurement) { + LogDebug("AddFactorsForSingleMeasurement: Not enough matches in projection measurement."); + return 0; + } + + const int num_landmarks = matched_projections_measurement.matched_projections.size(); + num_landmarks_averager_.Update(num_landmarks); + int num_loc_projection_factors = 0; + int num_loc_pose_factors = 0; + if (params_.add_projection_factors) { + num_loc_projection_factors = AddLocProjectionFactor(matched_projections_measurement, factors); + // Add loc pose factors as a fallback if all projection factors failed and fallback + // enabled + if (num_loc_projection_factors == 0 && params_.add_prior_if_projection_factors_fail) { + num_loc_pose_factors = AddLocPoseFactor(matched_projections_measurement, factors); + } + } + // Add loc pose factors if enabled and fallback hasn't already been triggered + if (params_.add_pose_priors && num_loc_pose_factors == 0) { + num_loc_pose_factors = AddLocPoseFactor(matched_projections_measurement, factors); + } + return num_loc_pose_factors + num_loc_projection_factors; +} + +template <typename PoseNodeAdderType> +bool LocFactorAdder<PoseNodeAdderType>::CanAddFactor( + const localization_measurements::MatchedProjectionsMeasurement& measurement) const { + return node_adder_->CanAddNode(measurement.timestamp); +} + +template <typename PoseNodeAdderType> +int LocFactorAdder<PoseNodeAdderType>::AddLocProjectionFactor( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement, + gtsam::NonlinearFactorGraph& factors) { + double noise_scale = params_.projection_noise_scale; + if (params_.scale_projection_noise_with_num_landmarks) { + const int num_landmarks = matched_projections_measurement.matched_projections.size(); + noise_scale *= std::pow((num_landmarks_averager_.average() / static_cast<double>(num_landmarks)), 2); + } + const auto pose_key = AddPoseNode(matched_projections_measurement.timestamp, factors); + if (!pose_key) { + LogError("AddLocProjectionFactors: Failed to get pose key."); + return 0; + } + const auto world_T_body = node_adder_->nodes().template Value<gtsam::Pose3>(*pose_key); + if (!world_T_body) { + LogError("AddLocProjectionFactors: Failed to get world_T_body at timestamp " + << matched_projections_measurement.timestamp << "."); + return 0; + } + + int num_loc_projection_factors = 0; + for (const auto& matched_projection : matched_projections_measurement.matched_projections) { + gtsam::SharedNoiseModel noise; + // Use the landmark distance from the camera to inversely scale the noise if desired. + if (params_.scale_projection_noise_with_landmark_distance) { + const Eigen::Vector3d& world_t_landmark = matched_projection.map_point; + const Eigen::Isometry3d nav_cam_T_world = + localization_common::EigenPose(*world_T_body * params_.body_T_cam).inverse(); + const gtsam::Point3 nav_cam_t_landmark = nav_cam_T_world * world_t_landmark; + // Don't use robust cost here to more effectively correct a drift occurance + noise = gtsam::SharedIsotropic( + gtsam::noiseModel::Isotropic::Sigma(2, params_.projection_noise_scale * 1.0 / nav_cam_t_landmark.z())); + } else { + noise = localization_common::Robust( + gtsam::SharedIsotropic(gtsam::noiseModel::Isotropic::Sigma(2, noise_scale * params_.cam_noise->sigma())), + params_.huber_k); + } + gtsam::LocProjectionFactor<>::shared_ptr loc_projection_factor( + new gtsam::LocProjectionFactor<>(matched_projection.image_point, matched_projection.map_point, noise, *pose_key, + params_.cam_intrinsics, params_.body_T_cam)); + // Check for errors, discard factor if too large of projection error occurs + // or a cheirality error occurs + const auto error = (loc_projection_factor->evaluateError(*world_T_body)).norm(); + if (error > params_.max_valid_projection_error) continue; + const auto cheirality_error = loc_projection_factor->cheiralityError(*world_T_body); + if (cheirality_error) continue; + factors.push_back(loc_projection_factor); + ++num_loc_projection_factors; + if (num_loc_projection_factors >= params_.max_num_projection_factors) break; + } + + LogDebug("AddFactorsForSingleMeasurement: Added " << num_loc_projection_factors + << " loc projection factors at timestamp " + << matched_projections_measurement.timestamp << "."); + return num_loc_projection_factors; +} + +template <typename PoseNodeAdderType> +int LocFactorAdder<PoseNodeAdderType>::AddLocPoseFactor( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement, + gtsam::NonlinearFactorGraph& factors) { + double noise_scale = params_.pose_noise_scale; + if (params_.scale_pose_noise_with_num_landmarks) { + const int num_landmarks = matched_projections_measurement.matched_projections.size(); + noise_scale *= std::pow((num_landmarks_averager_.average() / static_cast<double>(num_landmarks)), 2); + } + const auto pose_key = AddPoseNode(matched_projections_measurement.timestamp, factors); + if (!pose_key) { + LogError("AddLocProjectionFactors: Failed to get pose key."); + return 0; + } + + const auto pose_noise = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(noise_scale * pose_prior_noise_sigmas_)), + params_.huber_k); + + gtsam::LocPoseFactor::shared_ptr pose_prior_factor(new gtsam::LocPoseFactor( + *pose_key, matched_projections_measurement.global_T_cam * params_.body_T_cam.inverse(), pose_noise)); + factors.push_back(pose_prior_factor); + LogDebug("AddLocPoseFactor: Added loc pose prior factor at timestamp " << matched_projections_measurement.timestamp + << "."); + return 1; +} + +template <typename PoseNodeAdderType> +boost::optional<gtsam::Key> LocFactorAdder<PoseNodeAdderType>::AddPoseNode(const localization_common::Time timestamp, + gtsam::NonlinearFactorGraph& factors) { + if (!node_adder_->AddNode(timestamp, factors)) { + LogError("AddLocPoseFactor: Failed to add node for timestamp " << timestamp << "."); + return boost::none; + } + const auto keys = node_adder_->Keys(timestamp); + if (keys.empty()) { + LogError("AddLocPoseFactor: Failed to get keys for timestamp " << timestamp << "."); + return boost::none; + } + + // Assumes first key is pose + const auto& pose_key = keys[0]; + return pose_key; +} +} // namespace factor_adders + +#endif // FACTOR_ADDERS_LOC_FACTOR_ADDER_H_ diff --git a/localization/factor_adders/include/factor_adders/loc_factor_adder_params.h b/localization/factor_adders/include/factor_adders/loc_factor_adder_params.h new file mode 100644 index 0000000000..dfae8cc094 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/loc_factor_adder_params.h @@ -0,0 +1,68 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_LOC_FACTOR_ADDER_PARAMS_H_ +#define FACTOR_ADDERS_LOC_FACTOR_ADDER_PARAMS_H_ + +#include <factor_adders/factor_adder_params.h> + +#include <gtsam/geometry/Cal3_S2.h> +#include <gtsam/geometry/Pose3.h> +#include <gtsam/linear/NoiseModel.h> + +namespace factor_adders { +struct LocFactorAdderParams : public FactorAdderParams { + // Add pose prior factors using estimated pose from matched + // projections measurement. + bool add_pose_priors; + // Add projections factor using matched projections. + bool add_projection_factors; + // Add pose prior factors if all projection factors fail. + bool add_prior_if_projection_factors_fail; + // Translation stddev noise for prior factors. + double prior_translation_stddev; + // Quaternion stddev noise for prior factors. + double prior_quaternion_stddev; + // Inversely scale noise with the number of matched projections when creating a pose factor. + bool scale_pose_noise_with_num_landmarks; + // Scale projection noise with the square of the ratio of the average number of matches per measurement + // to the current number of matches when creating a projections factor. + // Yields lower noise for measurements with more than average matches. + bool scale_projection_noise_with_num_landmarks; + // Scale projection factor noise using the inverse of the distance to a landmark. + bool scale_projection_noise_with_landmark_distance; + // Relative pose noise scale. + double pose_noise_scale; + // Relative projections noise scale. + double projection_noise_scale; + // Max num projection factors to add for a matched projections measurement. + int max_num_projection_factors; + // Min number of matched projections to create a factor (prior or projection). + int min_num_matches_per_measurement; + // Max projection error norm for a projection factor to be valid. + double max_valid_projection_error; + // Camera extrinsics. + gtsam::Pose3 body_T_cam; + // Camera intrinsics. + boost::shared_ptr<gtsam::Cal3_S2> cam_intrinsics; + // Camera noise used for projection factor noise. + gtsam::SharedIsotropic cam_noise; +}; +} // namespace factor_adders + +#endif // FACTOR_ADDERS_LOC_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/factor_adders/include/factor_adders/measurement_based_factor_adder.h b/localization/factor_adders/include/factor_adders/measurement_based_factor_adder.h new file mode 100644 index 0000000000..165da4dbf2 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/measurement_based_factor_adder.h @@ -0,0 +1,135 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_MEASUREMENT_BASED_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_MEASUREMENT_BASED_FACTOR_ADDER_H_ + +#include <factor_adders/factor_adder.h> +#include <localization_common/timestamped_set.h> + +namespace factor_adders { +template <typename MeasurementType> +// FactorAdder that stores measurements in a measurement buffer +// and uses these to create factors given a time range. +class MeasurementBasedFactorAdder : public FactorAdder { + public: + explicit MeasurementBasedFactorAdder(const FactorAdderParams& params); + virtual ~MeasurementBasedFactorAdder() = default; + + // Adds factors then removes old measurements. + // Requires implementation of AddMeasurementBasedFactors. + int AddFactors(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, gtsam::NonlinearFactorGraph& factors) final; + + // Add measurement to measurement buffer. + void AddMeasurement(const MeasurementType& measurement); + + // Remove old measurements from measurement buffer. + void RemoveOldMeasurements(const localization_common::Time oldest_allowed_time); + + protected: + // Helper function to process measurements in valid time range and remove processed ones + // from the measurement buffer. + // Removes the measurement if the process_measurement_function returns true. + void ProcessMeasurements( + const localization_common::Time oldest_allowed_time, const localization_common::Time newest_allowed_time, + const std::function<bool(const MeasurementType&, gtsam::NonlinearFactorGraph&)>& process_measurement_function, + gtsam::NonlinearFactorGraph& factors); + + // Wrapper for ProcessMeasurements that doesn't use factors. + void ProcessMeasurements(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, + const std::function<bool(const MeasurementType&)>& process_measurement_function); + + localization_common::TimestampedSet<MeasurementType> measurements_; + + private: + // Adds factors based on measurements. + // Removes used measurements from measurement buffer. + virtual int AddMeasurementBasedFactors(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, + gtsam::NonlinearFactorGraph& factors) = 0; + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(FactorAdder); + ar& BOOST_SERIALIZATION_NVP(measurements_); + } +}; + +// Implementation +template <typename MeasurementType> +MeasurementBasedFactorAdder<MeasurementType>::MeasurementBasedFactorAdder(const FactorAdderParams& params) + : FactorAdder(params) {} + +template <typename MeasurementType> +int MeasurementBasedFactorAdder<MeasurementType>::AddFactors(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, + gtsam::NonlinearFactorGraph& factors) { + const int num_added_factors = AddMeasurementBasedFactors(oldest_allowed_time, newest_allowed_time, factors); + RemoveOldMeasurements(oldest_allowed_time); + return num_added_factors; +} + +template <typename MeasurementType> +void MeasurementBasedFactorAdder<MeasurementType>::AddMeasurement(const MeasurementType& measurement) { + measurements_.Add(measurement.timestamp, measurement); +} + +template <typename MeasurementType> +void MeasurementBasedFactorAdder<MeasurementType>::RemoveOldMeasurements( + const localization_common::Time oldest_allowed_timestamp) { + measurements_.RemoveOldValues(oldest_allowed_timestamp); +} + +template <typename MeasurementType> +void MeasurementBasedFactorAdder<MeasurementType>::ProcessMeasurements( + const localization_common::Time oldest_allowed_time, const localization_common::Time newest_allowed_time, + const std::function<bool(const MeasurementType&, gtsam::NonlinearFactorGraph&)>& process_measurement_function, + gtsam::NonlinearFactorGraph& factors) { + auto& measurements = measurements_.set(); + for (auto it = measurements.begin(); it != measurements.end();) { + const auto& measurement = it->second; + const auto timestamp = it->first; + if (timestamp >= oldest_allowed_time && timestamp <= newest_allowed_time && + process_measurement_function(measurement, factors)) { + // Remove used measurements. + it = measurements.erase(it); + } else { + ++it; + } + } +} + +template <typename MeasurementType> +void MeasurementBasedFactorAdder<MeasurementType>::ProcessMeasurements( + const localization_common::Time oldest_allowed_time, const localization_common::Time newest_allowed_time, + const std::function<bool(const MeasurementType&)>& process_measurement_function) { + gtsam::NonlinearFactorGraph dummy_factors; + ProcessMeasurements( + oldest_allowed_time, newest_allowed_time, + [&process_measurement_function](const MeasurementType& measurement, gtsam::NonlinearFactorGraph& factors) { + return process_measurement_function(measurement); + }, + dummy_factors); +} +} // namespace factor_adders + +#endif // FACTOR_ADDERS_MEASUREMENT_BASED_FACTOR_ADDER_H_ diff --git a/localization/factor_adders/include/factor_adders/pose_factor_adder.h b/localization/factor_adders/include/factor_adders/pose_factor_adder.h new file mode 100644 index 0000000000..64e4de173c --- /dev/null +++ b/localization/factor_adders/include/factor_adders/pose_factor_adder.h @@ -0,0 +1,80 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef FACTOR_ADDERS_POSE_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_POSE_FACTOR_ADDER_H_ + +#include <factor_adders/single_measurement_based_factor_adder.h> +#include <localization_common/time.h> +#include <localization_measurements/pose_with_covariance_measurement.h> +#include <node_adders/pose_node_adder.h> + +#include <gtsam/linear/NoiseModel.h> +#include <gtsam/slam/PriorFactor.h> + +namespace factor_adders { +using PoseFactorAdderParams = FactorAdderParams; + +// Adds GTSAM Pose Prior factors for absolute pose measurements. +// Adds pose nodes using PoseNodeAdder at the same timestamps as the measurements. +template <class PoseNodeAdderType> +class PoseFactorAdder + : public SingleMeasurementBasedFactorAdder<localization_measurements::PoseWithCovarianceMeasurement> { + public: + PoseFactorAdder(const PoseFactorAdderParams& params, const std::shared_ptr<PoseNodeAdderType> node_adder); + + private: + // Creates a pose factor and pose node for the given measurement. + int AddFactorsForSingleMeasurement(const localization_measurements::PoseWithCovarianceMeasurement& measurement, + gtsam::NonlinearFactorGraph& factors) final; + + // Able to add a factor if the node adder can create a node for the provided measurement. + bool CanAddFactor(const localization_measurements::PoseWithCovarianceMeasurement& measurement) const final; + + std::shared_ptr<PoseNodeAdderType> node_adder_; + PoseFactorAdderParams params_; +}; + +template <class PoseNodeAdderType> +PoseFactorAdder<PoseNodeAdderType>::PoseFactorAdder(const PoseFactorAdderParams& params, + const std::shared_ptr<PoseNodeAdderType> node_adder) + : SingleMeasurementBasedFactorAdder<localization_measurements::PoseWithCovarianceMeasurement>(params), + params_(params), + node_adder_(node_adder) {} + +template <class PoseNodeAdderType> +int PoseFactorAdder<PoseNodeAdderType>::AddFactorsForSingleMeasurement( + const localization_measurements::PoseWithCovarianceMeasurement& measurement, gtsam::NonlinearFactorGraph& factors) { + node_adder_->AddNode(measurement.timestamp, factors); + const auto keys = node_adder_->Keys(measurement.timestamp); + // First key is pose key + const auto& pose_key = keys[0]; + const auto pose_noise = gtsam::noiseModel::Gaussian::Covariance(measurement.covariance); + const gtsam::PriorFactor<gtsam::Pose3>::shared_ptr pose_prior_factor( + new gtsam::PriorFactor<gtsam::Pose3>(pose_key, measurement.pose, pose_noise)); + factors.push_back(pose_prior_factor); + return 1; +} + +template <class PoseNodeAdderType> +bool PoseFactorAdder<PoseNodeAdderType>::CanAddFactor( + const localization_measurements::PoseWithCovarianceMeasurement& measurement) const { + return node_adder_->CanAddNode(measurement.timestamp); +} +} // namespace factor_adders + +#endif // FACTOR_ADDERS_POSE_FACTOR_ADDER_H_ diff --git a/localization/factor_adders/include/factor_adders/relative_pose_factor_adder.h b/localization/factor_adders/include/factor_adders/relative_pose_factor_adder.h new file mode 100644 index 0000000000..e851100d99 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/relative_pose_factor_adder.h @@ -0,0 +1,88 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef FACTOR_ADDERS_RELATIVE_POSE_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_RELATIVE_POSE_FACTOR_ADDER_H_ + +#include <factor_adders/relative_pose_factor_adder_params.h> +#include <factor_adders/single_measurement_based_factor_adder.h> +#include <localization_common/time.h> +#include <localization_measurements/relative_pose_with_covariance_measurement.h> + +#include <gtsam/linear/NoiseModel.h> +#include <gtsam/slam/BetweenFactor.h> + +namespace factor_adders { +// Adds GTSAM Pose Between factors for relative pose measurements. +// Adds pose nodes using PoseNodeAdder at the same timestamps as the measurements. +template <class PoseNodeAdderType> +class RelativePoseFactorAdder + : public SingleMeasurementBasedFactorAdder<localization_measurements::RelativePoseWithCovarianceMeasurement> { + public: + RelativePoseFactorAdder(const RelativePoseFactorAdderParams& params, + const std::shared_ptr<PoseNodeAdderType> node_adder); + + private: + // Creates a pose between factor and pose nodes for the given measurement. + int AddFactorsForSingleMeasurement( + const localization_measurements::RelativePoseWithCovarianceMeasurement& measurement, + gtsam::NonlinearFactorGraph& factors) final; + + bool CanAddFactor(const localization_measurements::RelativePoseWithCovarianceMeasurement& measurement) const final; + + std::shared_ptr<PoseNodeAdderType> node_adder_; + RelativePoseFactorAdderParams params_; +}; + +template <class PoseNodeAdderType> +RelativePoseFactorAdder<PoseNodeAdderType>::RelativePoseFactorAdder(const RelativePoseFactorAdderParams& params, + const std::shared_ptr<PoseNodeAdderType> node_adder) + : SingleMeasurementBasedFactorAdder<localization_measurements::RelativePoseWithCovarianceMeasurement>(params), + params_(params), + node_adder_(node_adder) {} + +template <class PoseNodeAdderType> +int RelativePoseFactorAdder<PoseNodeAdderType>::AddFactorsForSingleMeasurement( + const localization_measurements::RelativePoseWithCovarianceMeasurement& measurement, + gtsam::NonlinearFactorGraph& factors) { + if (!node_adder_->AddNode(measurement.timestamp_a, factors) || + !node_adder_->AddNode(measurement.timestamp_b, factors)) { + LogError("AddFactorsForSingleMeasurement: Failed to add nodes at respective times."); + return 0; + } + + const auto keys_a = node_adder_->Keys(measurement.timestamp_a); + // First key is pose key + const auto& pose_key_a = keys_a[0]; + const auto keys_b = node_adder_->Keys(measurement.timestamp_b); + const auto& pose_key_b = keys_b[0]; + const auto relative_pose_noise = + gtsam::noiseModel::Gaussian::Covariance(params_.covariance_scale * measurement.covariance); + const gtsam::BetweenFactor<gtsam::Pose3>::shared_ptr pose_between_factor( + new gtsam::BetweenFactor<gtsam::Pose3>(pose_key_a, pose_key_b, measurement.relative_pose, relative_pose_noise)); + factors.push_back(pose_between_factor); + return 1; +} + +template <class PoseNodeAdderType> +bool RelativePoseFactorAdder<PoseNodeAdderType>::CanAddFactor( + const localization_measurements::RelativePoseWithCovarianceMeasurement& measurement) const { + return node_adder_->CanAddNode(measurement.timestamp_a) && node_adder_->CanAddNode(measurement.timestamp_b); +} +} // namespace factor_adders + +#endif // FACTOR_ADDERS_RELATIVE_POSE_FACTOR_ADDER_H_ diff --git a/localization/factor_adders/include/factor_adders/relative_pose_factor_adder_params.h b/localization/factor_adders/include/factor_adders/relative_pose_factor_adder_params.h new file mode 100644 index 0000000000..719c7037b6 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/relative_pose_factor_adder_params.h @@ -0,0 +1,30 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_RELATIVE_POSE_FACTOR_ADDER_PARAMS_H_ +#define FACTOR_ADDERS_RELATIVE_POSE_FACTOR_ADDER_PARAMS_H_ + +#include <factor_adders/factor_adder_params.h> + +namespace factor_adders { +struct RelativePoseFactorAdderParams : public FactorAdderParams { + double covariance_scale; +}; +} // namespace factor_adders + +#endif // FACTOR_ADDERS_RELATIVE_POSE_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/factor_adders/include/factor_adders/single_measurement_based_factor_adder.h b/localization/factor_adders/include/factor_adders/single_measurement_based_factor_adder.h new file mode 100644 index 0000000000..47adf9a7af --- /dev/null +++ b/localization/factor_adders/include/factor_adders/single_measurement_based_factor_adder.h @@ -0,0 +1,81 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_SINGLE_MEASUREMENT_BASED_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_SINGLE_MEASUREMENT_BASED_FACTOR_ADDER_H_ + +#include <factor_adders/measurement_based_factor_adder.h> + +namespace factor_adders { +template <typename MeasurementType> +// MeasurementBasedFactorAdder that adds factors using single measurements at a time +// for measurements in a provided time range. +class SingleMeasurementBasedFactorAdder : public MeasurementBasedFactorAdder<MeasurementType> { + public: + explicit SingleMeasurementBasedFactorAdder(const FactorAdderParams& params); + virtual ~SingleMeasurementBasedFactorAdder() = default; + + private: + // Add factors for all measurements in valid time range to existing factor graph. + // Calls AddFactors(measurement) for each measurement in range. + // Subsequently removes measurements that are addable (for example, if a corresponding + // node adder can create a node at it's timestamp) as determined by CanAddFactor(). + // Returns number of added factors. + int AddMeasurementBasedFactors(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, + gtsam::NonlinearFactorGraph& factors) final; + + // Add factors given a single measurement. + // Returns number of added factors. + virtual int AddFactorsForSingleMeasurement(const MeasurementType& measurement, + gtsam::NonlinearFactorGraph& factors) = 0; + + // Whether a factor can be added for the given measurement. + virtual bool CanAddFactor(const MeasurementType& measurement) const = 0; + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(MeasurementBasedFactorAdder<MeasurementType>); + } +}; + +// Implementation +template <typename MeasurementType> +SingleMeasurementBasedFactorAdder<MeasurementType>::SingleMeasurementBasedFactorAdder(const FactorAdderParams& params) + : MeasurementBasedFactorAdder<MeasurementType>(params) {} + +template <typename MeasurementType> +int SingleMeasurementBasedFactorAdder<MeasurementType>::AddMeasurementBasedFactors( + const localization_common::Time oldest_allowed_time, const localization_common::Time newest_allowed_time, + gtsam::NonlinearFactorGraph& factors) { + int num_added_factors = 0; + this->ProcessMeasurements( + oldest_allowed_time, newest_allowed_time, + [this, &num_added_factors](const MeasurementType& measurement, gtsam::NonlinearFactorGraph& factors) { + if (!CanAddFactor(measurement)) return false; + num_added_factors += AddFactorsForSingleMeasurement(measurement, factors); + return true; + }, + factors); + return num_added_factors; +} +} // namespace factor_adders + +#endif // FACTOR_ADDERS_SINGLE_MEASUREMENT_BASED_FACTOR_ADDER_H_ diff --git a/localization/factor_adders/include/factor_adders/standstill_factor_adder.h b/localization/factor_adders/include/factor_adders/standstill_factor_adder.h new file mode 100644 index 0000000000..f58d93bb07 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/standstill_factor_adder.h @@ -0,0 +1,173 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_STANDSTILL_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_STANDSTILL_FACTOR_ADDER_H_ + +#include <factor_adders/single_measurement_based_factor_adder.h> +#include <factor_adders/standstill_factor_adder_params.h> +#include <localization_common/utilities.h> +#include <localization_measurements/standstill_measurement.h> + +#include <gtsam/slam/BetweenFactor.h> + +#include <vector> + +namespace factor_adders { +// SingleMeasurementBasedFactorAdder that generates standstill factors (zero velocity prior and zero relative pose +// between factors) based on provided params. +template <typename PoseVelocityNodeAdderType> +class StandstillFactorAdder + : public SingleMeasurementBasedFactorAdder<localization_measurements::StandstillMeasurement> { + using Base = SingleMeasurementBasedFactorAdder<localization_measurements::StandstillMeasurement>; + + public: + StandstillFactorAdder(const StandstillFactorAdderParams& params, + const std::shared_ptr<PoseVelocityNodeAdderType> node_adder); + + // Default constructor for serialization only + StandstillFactorAdder() = default; + + private: + // Adds zero velocity and/or zero relative pose factors depending on params. + int AddFactorsForSingleMeasurement(const localization_measurements::StandstillMeasurement& standstill_measurement, + gtsam::NonlinearFactorGraph& factors) final; + + // Returns whether the node adder can add a node at the provided time. + bool CanAddFactor(const localization_measurements::StandstillMeasurement& measurement) const final; + + // Adds a velocity prior at the provided timestamp with zero velocity. + bool AddZeroVelocityPrior(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors); + // Adds a relative pose between factor with zero relative movement between nodes at timestamp a and b. + bool AddZeroRelativePoseFactor(const localization_common::Time timestamp_a, + const localization_common::Time timestamp_b, gtsam::NonlinearFactorGraph& factors); + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(node_adder_); + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(zero_velocity_noise_); + ar& BOOST_SERIALIZATION_NVP(zero_relative_pose_noise_); + } + + std::shared_ptr<PoseVelocityNodeAdderType> node_adder_; + StandstillFactorAdderParams params_; + gtsam::SharedNoiseModel zero_velocity_noise_; + gtsam::SharedNoiseModel zero_relative_pose_noise_; +}; + +// Implementation +template <typename PoseVelocityNodeAdderType> +StandstillFactorAdder<PoseVelocityNodeAdderType>::StandstillFactorAdder( + const StandstillFactorAdderParams& params, const std::shared_ptr<PoseVelocityNodeAdderType> node_adder) + : Base(params), params_(params), node_adder_(node_adder) { + // Create zero velocity noise + const gtsam::Vector3 velocity_prior_noise_sigmas( + (gtsam::Vector(3) << params_.prior_velocity_stddev, params_.prior_velocity_stddev, params_.prior_velocity_stddev) + .finished()); + zero_velocity_noise_ = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(velocity_prior_noise_sigmas)), + params_.huber_k); + + // Create zero relative pose noise + const gtsam::Vector6 pose_between_noise_sigmas( + (gtsam::Vector(6) << params_.pose_between_factor_rotation_stddev, params_.pose_between_factor_rotation_stddev, + params_.pose_between_factor_rotation_stddev, params_.pose_between_factor_translation_stddev, + params_.pose_between_factor_translation_stddev, params_.pose_between_factor_translation_stddev) + .finished()); + zero_relative_pose_noise_ = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(pose_between_noise_sigmas)), params_.huber_k); +} + +template <typename PoseVelocityNodeAdderType> +int StandstillFactorAdder<PoseVelocityNodeAdderType>::AddFactorsForSingleMeasurement( + const localization_measurements::StandstillMeasurement& standstill_measurement, + gtsam::NonlinearFactorGraph& factors) { + int num_factors_added = 0; + if (params_.add_pose_between_factor) { + if (AddZeroRelativePoseFactor(standstill_measurement.previous_timestamp, standstill_measurement.timestamp, factors)) + ++num_factors_added; + } + if (params_.add_velocity_prior) { + if (AddZeroVelocityPrior(standstill_measurement.timestamp, factors)) ++num_factors_added; + } + return num_factors_added; +} + +template <typename PoseVelocityNodeAdderType> +bool StandstillFactorAdder<PoseVelocityNodeAdderType>::CanAddFactor( + const localization_measurements::StandstillMeasurement& measurement) const { + return node_adder_->CanAddNode(measurement.timestamp); +} + +template <typename PoseVelocityNodeAdderType> +bool StandstillFactorAdder<PoseVelocityNodeAdderType>::AddZeroVelocityPrior(const localization_common::Time timestamp, + gtsam::NonlinearFactorGraph& factors) { + if (!node_adder_->AddNode(timestamp, factors)) { + return false; + } + const auto keys = node_adder_->Keys(timestamp); + if (keys.empty()) { + LogError("AddZeroVelocityPrior: Failed to get keys for timestamp."); + return false; + } + // Assumes second key is velocity + const auto& velocity_key = keys[1]; + + // Create factor + gtsam::PriorFactor<gtsam::Velocity3>::shared_ptr velocity_prior_factor( + new gtsam::PriorFactor<gtsam::Velocity3>(velocity_key, gtsam::Velocity3::Zero(), zero_velocity_noise_)); + factors.push_back(velocity_prior_factor); + LogDebug("AddFactorsForSingleMeasurement: Added standstill velocity prior factor."); + return true; +} + +template <typename PoseVelocityNodeAdderType> +bool StandstillFactorAdder<PoseVelocityNodeAdderType>::AddZeroRelativePoseFactor( + const localization_common::Time timestamp_a, const localization_common::Time timestamp_b, + gtsam::NonlinearFactorGraph& factors) { + if (!(node_adder_->AddNode(timestamp_a, factors) && node_adder_->AddNode(timestamp_b, factors))) { + return false; + } + + const auto keys_a = node_adder_->Keys(timestamp_a); + if (keys_a.empty()) { + LogError("AddZeroVelocityPrior: Failed to get keys for timestamp_a."); + return false; + } + const auto keys_b = node_adder_->Keys(timestamp_b); + if (keys_b.empty()) { + LogError("AddZeroVelocityPrior: Failed to get keys for timestamp_b."); + return false; + } + // Assumes first key is pose + const auto& pose_key_a = keys_a[0]; + const auto& pose_key_b = keys_b[0]; + + gtsam::BetweenFactor<gtsam::Pose3>::shared_ptr pose_between_factor(new gtsam::BetweenFactor<gtsam::Pose3>( + pose_key_a, pose_key_b, gtsam::Pose3::identity(), zero_relative_pose_noise_)); + factors.push_back(pose_between_factor); + LogDebug("AddFactorsForSingleMeasurement: Added standstill pose between factor."); + return true; +} +} // namespace factor_adders + +#endif // FACTOR_ADDERS_STANDSTILL_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/standstill_factor_adder_params.h b/localization/factor_adders/include/factor_adders/standstill_factor_adder_params.h similarity index 72% rename from localization/graph_localizer/include/graph_localizer/standstill_factor_adder_params.h rename to localization/factor_adders/include/factor_adders/standstill_factor_adder_params.h index 1bd7689710..d0b20ab263 100644 --- a/localization/graph_localizer/include/graph_localizer/standstill_factor_adder_params.h +++ b/localization/factor_adders/include/factor_adders/standstill_factor_adder_params.h @@ -16,19 +16,19 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_PARAMS_H_ -#define GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_PARAMS_H_ +#ifndef FACTOR_ADDERS_STANDSTILL_FACTOR_ADDER_PARAMS_H_ +#define FACTOR_ADDERS_STANDSTILL_FACTOR_ADDER_PARAMS_H_ -#include <graph_optimizer/factor_adder_params.h> +#include <factor_adders/factor_adder_params.h> -namespace graph_localizer { -struct StandstillFactorAdderParams : public graph_optimizer::FactorAdderParams { +namespace factor_adders { +struct StandstillFactorAdderParams : public FactorAdderParams { bool add_velocity_prior; bool add_pose_between_factor; double prior_velocity_stddev; double pose_between_factor_translation_stddev; double pose_between_factor_rotation_stddev; }; -} // namespace graph_localizer +} // namespace factor_adders -#endif // GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_PARAMS_H_ +#endif // FACTOR_ADDERS_STANDSTILL_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/factor_adders/include/factor_adders/vo_smart_projection_factor_adder.h b/localization/factor_adders/include/factor_adders/vo_smart_projection_factor_adder.h new file mode 100644 index 0000000000..ab88e97a10 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/vo_smart_projection_factor_adder.h @@ -0,0 +1,344 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_VO_SMART_PROJECTION_FACTOR_ADDER_H_ +#define FACTOR_ADDERS_VO_SMART_PROJECTION_FACTOR_ADDER_H_ + +#include <factor_adders/measurement_based_factor_adder.h> +#include <factor_adders/vo_smart_projection_factor_adder_params.h> +#include <graph_factors/robust_smart_projection_pose_factor.h> +#include <localization_common/utilities.h> +#include <localization_measurements/feature_points_measurement.h> +#include <vision_common/spaced_feature_tracker.h> +#include <vision_common/utilities.h> + +#include <vector> + +namespace factor_adders { +using SmartFactorCalibration = gtsam::Cal3_S2; +using SmartFactorCamera = gtsam::PinholePose<SmartFactorCalibration>; +using RobustSmartFactor = gtsam::RobustSmartProjectionPoseFactor<SmartFactorCalibration>; +using SharedRobustSmartFactor = boost::shared_ptr<RobustSmartFactor>; +template <typename PoseNodeAdderType> +// Adds visual-odometry (VO) smart factors for feature tracks using GTSAM smart factors. +// Smart factors avoid bundle-adjustment by marginalizing out triangulated feature track +// landmark points before adding for optimization, making them much more efficient at the expense +// of accuracy. Since the landmark position for the point is not included during optimization, +// GTSAM performs retriangulation of the landmark during optimization if poses in the smart +// factor have changed by more than a set threshold. Retriangulation also triggers relinearization of the smart factor. +// Measurements are downsampled in the feature tracker, avoiding issues of downsampling +// from the latest measurement which may introduce many new timestamped pose nodes to +// the graph in addition to the latest measurement's timestamp. +class VoSmartProjectionFactorAdder + : public MeasurementBasedFactorAdder<localization_measurements::FeaturePointsMeasurement> { + using Base = MeasurementBasedFactorAdder<localization_measurements::FeaturePointsMeasurement>; + + public: + VoSmartProjectionFactorAdder(const VoSmartProjectionFactorAdderParams& params, + std::shared_ptr<PoseNodeAdderType> node_adder); + + // Default constructor for serialization only. + VoSmartProjectionFactorAdder() = default; + + // Const accesor to feature tracker + const vision_common::SpacedFeatureTracker& feature_tracker() const; + + private: + // Add factors using either set measurement spacing or max spacing. + int AddMeasurementBasedFactors(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, + gtsam::NonlinearFactorGraph& factors) final; + + // Add factors using downsampled measurements, determined by the measurement_spacing param. + // Measurements are in affect downsampled before being used for feature tracks. + int AddFactorsUsingDownsampledMeasurements(gtsam::NonlinearFactorGraph& factors) const; + + // Helper function to add a smart factor given a set of feature track points. + // Assumes points are ordered from oldest to latest, so oldest points are + // added first and prioritized over later points given a max number of points to add. + bool AddSmartFactor(const vision_common::FeaturePoints& feature_track_points, + gtsam::NonlinearFactorGraph& factors) const; + + // Checks the average distance from the mean and the number of points in the track. + bool ValidTrack(const vision_common::FeaturePoints& feature_track) const; + + // Fixes invalid smart factors (that suffer cheirality errors) by removing individual measurements and if that fails + // by removing measurement sequences. + // If both fail, the smart factor is left in the graph in case it becomes + // valid over the course of optimization. + void FixSmartFactors(const gtsam::Values& values, gtsam::NonlinearFactorGraph& factors) const; + + // Attempts to fix smart factors by removing individual measurements. + boost::optional<SharedRobustSmartFactor> FixSmartFactorByRemovingIndividualMeasurements( + const gtsam::Values& values, const RobustSmartFactor& smart_factor) const; + + // Attempts to fix smart factors by removing measurement sequences. + boost::optional<SharedRobustSmartFactor> FixSmartFactorByRemovingMeasurementSequence( + const gtsam::Values& values, const RobustSmartFactor& smart_factor) const; + + // Helper function for creating and testing a fixed smart factor + boost::optional<SharedRobustSmartFactor> FixedSmartFactor( + const gtsam::Values& values, gtsam::PinholePose<gtsam::Cal3_S2>::MeasurementVector& measurements, + gtsam::KeyVector& keys) const; + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(node_adder_); + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(feature_tracker_); + } + + std::shared_ptr<PoseNodeAdderType> node_adder_; + VoSmartProjectionFactorAdderParams params_; + std::shared_ptr<vision_common::SpacedFeatureTracker> feature_tracker_; +}; + +// Implementation +template <typename PoseNodeAdderType> +VoSmartProjectionFactorAdder<PoseNodeAdderType>::VoSmartProjectionFactorAdder( + const VoSmartProjectionFactorAdderParams& params, std::shared_ptr<PoseNodeAdderType> node_adder) + : Base(params), params_(params), node_adder_(node_adder) { + feature_tracker_.reset(new vision_common::SpacedFeatureTracker(params.spaced_feature_tracker)); +} + +template <typename PoseNodeAdderType> +const vision_common::SpacedFeatureTracker& VoSmartProjectionFactorAdder<PoseNodeAdderType>::feature_tracker() const { + return *feature_tracker_; +} + +template <typename PoseNodeAdderType> +int VoSmartProjectionFactorAdder<PoseNodeAdderType>::AddMeasurementBasedFactors( + const localization_common::Time oldest_allowed_time, const localization_common::Time newest_allowed_time, + gtsam::NonlinearFactorGraph& factors) { + // Update feature tracker with new measurements and remove these from the measurement buffer if nodes can be created + // for them. + ProcessMeasurements(oldest_allowed_time, newest_allowed_time, + [this](const localization_measurements::FeaturePointsMeasurement& measurement) { + // Don't add measurements until pose nodes can be created at their timestamp + if (!node_adder_->CanAddNode(measurement.timestamp)) return false; + feature_tracker_->Update(measurement.feature_points); + return true; + }); + // Remove old feature track points before adding factors + feature_tracker_->RemoveOldPoints(oldest_allowed_time); + // Remove old smart factors before adding new ones, since otherwise there would be repeat factors + // for already existing feature tracks + localization_common::DeleteFactors<RobustSmartFactor>(factors); + + // Create smart factors based on feature tracks + int num_added_factors = 0; + num_added_factors = AddFactorsUsingDownsampledMeasurements(factors); + // Attempt to fix broken factors if enabled + if (params_.fix_invalid_factors) FixSmartFactors(node_adder_->nodes().gtsam_values(), factors); + return num_added_factors; +} + +template <typename PoseNodeAdderType> +int VoSmartProjectionFactorAdder<PoseNodeAdderType>::AddFactorsUsingDownsampledMeasurements( + gtsam::NonlinearFactorGraph& factors) const { + int num_added_factors = 0; + const auto spaced_feature_tracks = feature_tracker_->SpacedFeatureTracks(); + for (const auto& feature_track : spaced_feature_tracks) { + if (num_added_factors >= params_.max_num_factors) break; + if (ValidTrack(feature_track)) { + if (AddSmartFactor(feature_track, factors)) ++num_added_factors; + } + } + return num_added_factors; +} + +template <typename PoseNodeAdderType> +bool VoSmartProjectionFactorAdder<PoseNodeAdderType>::AddSmartFactor( + const vision_common::FeaturePoints& feature_track_points, gtsam::NonlinearFactorGraph& factors) const { + SharedRobustSmartFactor smart_factor; + const int num_feature_track_points = feature_track_points.size(); + // Optionally scale the noise with the number of points, as a longer track the relies on a + // longer history of poses tends to have higher error + const double noise_scale = + params_.scale_noise_with_num_points ? params_.noise_scale * num_feature_track_points : params_.noise_scale; + const auto noise = gtsam::noiseModel::Isotropic::Sigma(2, noise_scale * params_.cam_noise->sigma()); + smart_factor = + boost::make_shared<RobustSmartFactor>(noise, params_.cam_intrinsics, params_.body_T_cam, params_.smart_factor, + params_.rotation_only_fallback, params_.robust, params_.huber_k); + + for (int i = 0; i < static_cast<int>(feature_track_points.size()); ++i) { + // Start in reverse so most recent measurements are added first + const auto& feature_point = feature_track_points[feature_track_points.size() - 1 - i]; + if (i >= params_.max_num_points_per_factor) break; + const auto& timestamp = feature_point.timestamp; + // Add or get pose key + if (!node_adder_->AddNode(timestamp, factors)) { + LogError("AddSmartFactor: Failed to add node for timestamp " << std::setprecision(15) << timestamp << "."); + return false; + } + const auto keys = node_adder_->Keys(timestamp); + if (keys.empty()) { + LogError("AddSmartFactor: Failed to get keys for timestamp " << std::setprecision(15) << timestamp << "."); + return false; + } + // Assumes first key is pose + const auto& pose_key = keys[0]; + + smart_factor->add(SmartFactorCamera::Measurement(feature_point.image_point), pose_key); + } + factors.push_back(smart_factor); + return true; +} + +template <typename PoseNodeAdderType> +bool VoSmartProjectionFactorAdder<PoseNodeAdderType>::ValidTrack( + const vision_common::FeaturePoints& feature_track) const { + const double average_distance_from_mean = vision_common::AverageDistanceFromMean(feature_track); + if (static_cast<int>(feature_track.size()) < params_.min_num_points_per_factor) return false; + return (average_distance_from_mean >= params_.min_avg_distance_from_mean); +} + +template <typename PoseNodeAdderType> +void VoSmartProjectionFactorAdder<PoseNodeAdderType>::FixSmartFactors(const gtsam::Values& values, + gtsam::NonlinearFactorGraph& factors) const { + for (auto& factor : factors) { + auto smart_factor = dynamic_cast<RobustSmartFactor*>(factor.get()); + if (!smart_factor) continue; + // Can't remove measurements if there are only 2 or fewer + if (smart_factor->measured().size() <= 2) continue; + const auto point = smart_factor->triangulateSafe(smart_factor->cameras(values)); + if (point.valid()) continue; + // Invalid factor, first attempt to fix by removing individiual measurements. + { + const auto fixed_smart_factor = FixSmartFactorByRemovingIndividualMeasurements(values, *smart_factor); + if (fixed_smart_factor) { + factor = *fixed_smart_factor; + continue; + } + } + // If removing individiual measurements fails, attempt to fix by removing measurement + // sequences. + { + const auto fixed_smart_factor = FixSmartFactorByRemovingMeasurementSequence(values, *smart_factor); + if (fixed_smart_factor) { + factor = *fixed_smart_factor; + continue; + } + } + // Don't remove factor in case it becomes valid over the course of optimization. + LogDebug("SplitSmartFactorsIfNeeded: Failed to fix smart factor"); + } +} + +template <typename PoseNodeAdderType> +boost::optional<SharedRobustSmartFactor> +VoSmartProjectionFactorAdder<PoseNodeAdderType>::FixSmartFactorByRemovingIndividualMeasurements( + const gtsam::Values& values, const RobustSmartFactor& smart_factor) const { + // TODO(rsoussan): Make this more efficient by enabled removal of measurements and keys in smart factor + const auto original_measurements = smart_factor.measured(); + const auto original_keys = smart_factor.keys(); + int measurement_index_to_remove; + // Start with latest measurement + for (measurement_index_to_remove = original_measurements.size() - 1; measurement_index_to_remove >= 0; + --measurement_index_to_remove) { + gtsam::PinholePose<gtsam::Cal3_S2>::MeasurementVector measurements_to_add; + gtsam::KeyVector keys_to_add; + for (int i = 0; i < static_cast<int>(original_measurements.size()); ++i) { + if (i == measurement_index_to_remove) continue; + measurements_to_add.emplace_back(original_measurements[i]); + keys_to_add.emplace_back(original_keys[i]); + } + const auto fixed_smart_factor = FixedSmartFactor(values, measurements_to_add, keys_to_add); + if (fixed_smart_factor) { + LogDebug("FixSmartFactorByRemovingIndividualMeasurements: Fixed by removing measurement " + << measurement_index_to_remove << ", num original measurements: " << original_measurements.size()); + return fixed_smart_factor; + } + } + return boost::none; +} + +template <typename PoseNodeAdderType> +boost::optional<SharedRobustSmartFactor> +VoSmartProjectionFactorAdder<PoseNodeAdderType>::FixSmartFactorByRemovingMeasurementSequence( + const gtsam::Values& values, const RobustSmartFactor& smart_factor) const { + constexpr int min_num_measurements = 2; + // TODO(rsoussan): Make this more efficient by enabled removal of measurements and keys in smart factor + const auto original_measurements = smart_factor.measured(); + const auto original_keys = smart_factor.keys(); + int num_measurements_to_add = original_measurements.size() - 1; + // Try to remove min number of most recent measurements + while (num_measurements_to_add >= min_num_measurements) { + gtsam::PinholePose<gtsam::Cal3_S2>::MeasurementVector measurements_to_add; + gtsam::KeyVector keys_to_add; + for (int i = 0; i < num_measurements_to_add; ++i) { + measurements_to_add.emplace_back(original_measurements[i]); + keys_to_add.emplace_back(original_keys[i]); + } + const auto fixed_smart_factor = FixedSmartFactor(values, measurements_to_add, keys_to_add); + if (fixed_smart_factor) { + LogDebug( + "FixSmartFactorByRemovingMeasurementSequence: Fixed smart factor by removing most recent " + "measurements. Original " + "measurement size: " + << original_measurements.size() << ", new size: " << num_measurements_to_add); + return fixed_smart_factor; + } else { + --num_measurements_to_add; + } + } + num_measurements_to_add = original_measurements.size() - 1; + // Try to remove min number of oldest measurements + while (num_measurements_to_add >= min_num_measurements) { + gtsam::PinholePose<gtsam::Cal3_S2>::MeasurementVector measurements_to_add; + gtsam::KeyVector keys_to_add; + for (int i = original_measurements.size() - num_measurements_to_add; + i < static_cast<int>(original_measurements.size()); ++i) { + measurements_to_add.emplace_back(original_measurements[i]); + keys_to_add.emplace_back(original_keys[i]); + } + const auto fixed_smart_factor = FixedSmartFactor(values, measurements_to_add, keys_to_add); + if (fixed_smart_factor) { + LogDebug( + "FixSmartFactorByRemovingMeasurementSequence: Fixed smart factor by removing oldest measurements. " + "Original " + "measurement size: " + << original_measurements.size() << ", new size: " << num_measurements_to_add); + return fixed_smart_factor; + } else { + --num_measurements_to_add; + } + } + // Failed to fix smart factor + return boost::none; + // TODO(rsoussan): delete factor if fail to find acceptable new one? + // TODO(rsoussan): attempt to make a second factor with remaining measuremnts!!! +} + +template <typename PoseNodeAdderType> +boost::optional<SharedRobustSmartFactor> VoSmartProjectionFactorAdder<PoseNodeAdderType>::FixedSmartFactor( + const gtsam::Values& values, gtsam::PinholePose<gtsam::Cal3_S2>::MeasurementVector& measurements, + gtsam::KeyVector& keys) const { + auto new_smart_factor = boost::make_shared<RobustSmartFactor>( + params_.cam_noise, params_.cam_intrinsics, params_.body_T_cam, params_.smart_factor, params_.rotation_only_fallback, + params_.robust, params_.huber_k); + new_smart_factor->add(measurements, keys); + const auto new_point = new_smart_factor->triangulateSafe(new_smart_factor->cameras(values)); + if (!new_point.valid()) return boost::none; + return new_smart_factor; +} +} // namespace factor_adders +#endif // FACTOR_ADDERS_VO_SMART_PROJECTION_FACTOR_ADDER_H_ diff --git a/localization/factor_adders/include/factor_adders/vo_smart_projection_factor_adder_params.h b/localization/factor_adders/include/factor_adders/vo_smart_projection_factor_adder_params.h new file mode 100644 index 0000000000..7624b75501 --- /dev/null +++ b/localization/factor_adders/include/factor_adders/vo_smart_projection_factor_adder_params.h @@ -0,0 +1,78 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef FACTOR_ADDERS_VO_SMART_PROJECTION_FACTOR_ADDER_PARAMS_H_ +#define FACTOR_ADDERS_VO_SMART_PROJECTION_FACTOR_ADDER_PARAMS_H_ + +#include <factor_adders/factor_adder_params.h> +#include <vision_common/spaced_feature_tracker_params.h> + +#include <gtsam/geometry/Pose3.h> +#include <gtsam/geometry/Cal3_S2.h> +#include <gtsam/linear/NoiseModel.h> +#include <gtsam/slam/SmartFactorParams.h> + +namespace factor_adders { +struct VoSmartProjectionFactorAdderParams : public FactorAdderParams { + vision_common::SpacedFeatureTrackerParams spaced_feature_tracker; + // Maximum number of smart factors to include in a graph at a time. + int max_num_factors; + // Minimum number of points for a feature track to be used for a smart factor. + int min_num_points_per_factor; + // Maximum number of points in a feature track to include in a smart factor. + int max_num_points_per_factor; + // Minimum average deviation for points in a feature track to be used. + // A higher deviation provides a larger baseline between points and is less likely + // to yield numerical errors during triangulation and optimization. + double min_avg_distance_from_mean; + // Use a robust loss for the factor. + bool robust; + // Minimum distance in image space between the latest measurement in a feature track + // and already included latest measurements in other feature tracks for the feature track + // to be used as a smart factor. + /* double feature_track_min_separation;*/ + // If triangulation fails, use a rotation-only version of the smart factor. + // Otherwise, the smart factor is disabled. + bool rotation_only_fallback; + // Attempt to fix smart factors that are invalid by removing individual measurements + // and if this fails measurement sequences. + bool fix_invalid_factors; + // Scale the noise with the number of points, as a longer track the relies on a + // longer history of poses tends to have higher error + bool scale_noise_with_num_points; + // Relative noise scale for all smart factors. + double noise_scale; + // Camera extrinsics. + gtsam::Pose3 body_T_cam; + // Camera intrinsics. + boost::shared_ptr<gtsam::Cal3_S2> cam_intrinsics; + // Camera noise. + gtsam::SharedIsotropic cam_noise; + // GTSAM Smart factor params, see GTSAM documentation and below for more details. + // enableEPI: Refine triangulation using Levenberg-Marquardt Optimization. + // verboseCheirality: Print error on cheirality errors + // landmarkDistanceThreshold: Maximum valid distance for a triangulated point. + // dynamicOutlierRejectionThreshold: Maximum valid reprojection error for a valid triangulated point. + // retriangulationThreshold: Equality threshold for poses in a smart factor to trigger retriangulation + // of a landmark point. If any of the poses vary by more than this threshold + // compared to their previous value, retriangulation is triggered. + gtsam::SmartProjectionParams smart_factor; +}; +} // namespace factor_adders + +#endif // FACTOR_ADDERS_VO_SMART_PROJECTION_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/factor_adders/package.xml b/localization/factor_adders/package.xml new file mode 100644 index 0000000000..7269c605eb --- /dev/null +++ b/localization/factor_adders/package.xml @@ -0,0 +1,27 @@ +<package> + <name>factor_adders</name> + <version>1.0.0</version> + <description> + The factor adders package + </description> + <license> + Apache License, Version 2.0 + </license> + <author email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </author> + <maintainer email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </maintainer> + <buildtool_depend>catkin</buildtool_depend> + <build_depend>graph_factors</build_depend> + <build_depend>localization_common</build_depend> + <build_depend>localization_measurements</build_depend> + <build_depend>node_adders</build_depend> + <build_depend>vision_common</build_depend> + <run_depend>graph_factors</run_depend> + <run_depend>localization_common</run_depend> + <run_depend>localization_measurements</run_depend> + <run_depend>node_adders</run_depend> + <run_depend>vision_common</run_depend> +</package> diff --git a/localization/factor_adders/readme.md b/localization/factor_adders/readme.md new file mode 100644 index 0000000000..096ca049dc --- /dev/null +++ b/localization/factor_adders/readme.md @@ -0,0 +1,22 @@ +\page factoradders Factor Adders + +# Package Overview +The factor adders package provides a set of factor adders that add factors to a graph optimizer given a range of timestamps. + +## FactorAdder +Base class for adding factors given a timestamp range. + +## MeasurementBasedFactorAdder +Extends the FactorAdder and stores measurements in a buffer before using these for factor creation. + +## SingleMeasurementBasedFactorAdder +MeasurementBasedFactorAdder that adds factors using single measurements at a time for measurements in a provided time range. + +## LocFactorAdder +SingleMeasurementBasedFactorAdder that takes map-based image feature measurements and generates either LocProjectionFactors or LocPoseFactors. Optionally adds loc pose factors as a fallback for projection factors that suffer cheirality errors. Uses a pose node adder for required node creation. + +### VOSmartProjectionFactorAdder +Generates visual odometry smart factors using image feature tracks. Downsamples measurements as desired for smart factors. See gtsam::SmartProjectionPoseFactor for more information on smart factors. Uses a pose node adder for required node creation. + +### StandstillFactorAdder +Creates a zero velocity prior and a zero tranform between factor for successive nodes when standstill is detected. Uses a pose and velocity node adder for required node creation. diff --git a/localization/factor_adders/test/test_loc_factor_adder.cc b/localization/factor_adders/test/test_loc_factor_adder.cc new file mode 100644 index 0000000000..a73d3026b7 --- /dev/null +++ b/localization/factor_adders/test/test_loc_factor_adder.cc @@ -0,0 +1,520 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <factor_adders/loc_factor_adder.h> +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <localization_measurements/matched_projections_measurement.h> +#include <node_adders/node_adder.h> +#include <node_adders/utilities.h> +#include <nodes/timestamped_nodes.h> +#include <nodes/values.h> + +#include <gtsam/geometry/Pose3.h> + +#include <gtest/gtest.h> + +namespace fa = factor_adders; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace na = node_adders; +namespace no = nodes; + +// Test node adder that just returns keys that should be used. +// Key values are calculated using the integer timestamps passed. +class SimplePoseNodeAdder : public na::NodeAdder { + public: + SimplePoseNodeAdder() : values_(std::make_shared<no::Values>()), nodes_(values_) {} + + void AddInitialNodesAndPriors(gtsam::NonlinearFactorGraph& graph) final{}; + + bool AddNode(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final { + if (!CanAddNode(timestamp)) return false; + if (!values_) std::cout << "no values!" << std::endl; + values().Add(gtsam::Pose3::identity()); + return true; + } + + bool CanAddNode(const localization_common::Time timestamp) const final { + return timestamp <= latest_measurement_time_; + } + + // Assumes integer timestamps that perfectly cast to ints. + // First key is pose key. + gtsam::KeyVector Keys(const localization_common::Time timestamp) const final { + gtsam::KeyVector keys; + // Offset by 1 since node keys start at 1 + keys.emplace_back(gtsam::Key(static_cast<int>(timestamp + 1))); + return keys; + } + + const no::Values& values() const { return *values_; } + + no::Values& values() { return *values_; } + + no::TimestampedNodes<gtsam::Pose3>& nodes() { return nodes_; } + + // Simulate measurement delay for node adder and control end of measurements time. + int latest_measurement_time_ = 10; + + private: + std::shared_ptr<no::Values> values_; + no::TimestampedNodes<gtsam::Pose3> nodes_; +}; + +class LocFactorAdderTest : public ::testing::Test { + public: + LocFactorAdderTest() { node_adder_.reset(new SimplePoseNodeAdder()); } + + void SetUp() final { AddMeasurements(); } + + void AddMeasurements() { + for (int time = 0; time < num_times_; ++time) { + lm::MatchedProjectionsMeasurement measurement; + measurement.global_T_cam = lc::RandomPose(); + measurement.timestamp = time; + for (int i = 0; i < num_projections_per_measurement_; ++i) { + const lm::ImagePoint image_point(i, i + 1); + const lm::MapPoint map_point(i, i + 1, i + 2); + const lm::MatchedProjection matched_projection(image_point, map_point, time); + measurement.matched_projections.emplace_back(matched_projection); + } + measurements_.emplace_back(measurement); + } + } + + void Initialize(const fa::LocFactorAdderParams& params) { + factor_adder_.reset(new fa::LocFactorAdder<SimplePoseNodeAdder>(params, node_adder_)); + params_ = params; + pose_prior_noise_sigmas_ = (gtsam::Vector(6) << params_.prior_translation_stddev, params_.prior_translation_stddev, + params_.prior_translation_stddev, params_.prior_quaternion_stddev, + params_.prior_quaternion_stddev, params_.prior_quaternion_stddev) + .finished(); + } + + fa::LocFactorAdderParams DefaultParams() { + fa::LocFactorAdderParams params; + params.add_pose_priors = false; + params.add_projection_factors = false; + params.add_prior_if_projection_factors_fail = false; + params.prior_translation_stddev = 0.1; + params.prior_quaternion_stddev = 0.2; + params.scale_pose_noise_with_num_landmarks = false; + params.scale_projection_noise_with_num_landmarks = false; + params.scale_projection_noise_with_landmark_distance = false; + params.pose_noise_scale = 2; + params.projection_noise_scale = 2; + params.max_num_projection_factors = 3; + params.min_num_matches_per_measurement = 1; + params.max_valid_projection_error = 1e6; + params.body_T_cam = gtsam::Pose3::identity(); + params.cam_intrinsics = boost::make_shared<gtsam::Cal3_S2>(); + params.cam_noise = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); + params.huber_k = 1.345; + params.enabled = true; + return params; + } + + void EXPECT_SAME_POSE_FACTOR(const int factor_index, const int measurement_index) { + const auto pose_factor = dynamic_cast<gtsam::LocPoseFactor*>(factors_[factor_index].get()); + ASSERT_TRUE(pose_factor); + const gtsam::Pose3 factor_pose = pose_factor->prior(); + const gtsam::Pose3 measurement_pose = measurements_[measurement_index].global_T_cam * params_.body_T_cam.inverse(); + EXPECT_MATRIX_NEAR(factor_pose, measurement_pose, 1e-6); + } + + void EXPECT_SAME_PROJECTION_FACTOR(const int factor_index, const int measurement_time, const int measurement_index) { + const auto projection_factor = dynamic_cast<gtsam::LocProjectionFactor<>*>(factors_[factor_index].get()); + ASSERT_TRUE(projection_factor); + const auto& measurement = measurements_[measurement_index].matched_projections[measurement_index]; + EXPECT_MATRIX_NEAR(projection_factor->measured(), measurement.image_point, 1e-6); + EXPECT_MATRIX_NEAR(projection_factor->landmark_point(), measurement.map_point, 1e-6); + EXPECT_MATRIX_NEAR(projection_factor->body_P_sensor()->matrix(), params_.body_T_cam, 1e-6); + EXPECT_MATRIX_NEAR(projection_factor->calibration()->matrix(), params_.cam_intrinsics->matrix(), 1e-6); + } + + void EXPECT_SAME_NOISE(const int factor_index, const gtsam::SharedNoiseModel& noise) { + const auto projection_factor = dynamic_cast<gtsam::LocProjectionFactor<>*>(factors_[factor_index].get()); + ASSERT_TRUE(projection_factor); + EXPECT_MATRIX_NEAR(na::Covariance(projection_factor->noiseModel()), na::Covariance(noise), 1e-6); + } + + void EXPECT_SAME_NOISE_NON_ROBUST(const int factor_index, const gtsam::SharedNoiseModel& noise) { + const auto projection_factor = dynamic_cast<gtsam::LocProjectionFactor<>*>(factors_[factor_index].get()); + ASSERT_TRUE(projection_factor); + const auto factor_noise = + dynamic_cast<gtsam::noiseModel::Gaussian*>(projection_factor->noiseModel().get())->covariance(); + const auto expected_noise = dynamic_cast<gtsam::noiseModel::Gaussian*>(noise.get())->covariance(); + EXPECT_MATRIX_NEAR(factor_noise, expected_noise, 1e-6); + } + + void EXPECT_SAME_POSE_NOISE(const int factor_index, const gtsam::SharedNoiseModel& noise) { + const auto pose_factor = dynamic_cast<gtsam::LocPoseFactor*>(factors_[factor_index].get()); + ASSERT_TRUE(pose_factor); + EXPECT_MATRIX_NEAR(na::Covariance(pose_factor->noiseModel()), na::Covariance(noise), 1e-6); + } + + std::unique_ptr<fa::LocFactorAdder<SimplePoseNodeAdder>> factor_adder_; + std::shared_ptr<SimplePoseNodeAdder> node_adder_; + gtsam::NonlinearFactorGraph factors_; + const int num_times_ = 10; + const int num_projections_per_measurement_ = 3; + std::vector<lm::MatchedProjectionsMeasurement> measurements_; + fa::LocFactorAdderParams params_; + gtsam::Vector6 pose_prior_noise_sigmas_; +}; + +TEST_F(LocFactorAdderTest, ProjectionFactors) { + auto params = DefaultParams(); + params.add_projection_factors = true; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), num_projections_per_measurement_); + EXPECT_EQ(factors_.size(), 3); + EXPECT_SAME_PROJECTION_FACTOR(0, 0, 0); + EXPECT_SAME_PROJECTION_FACTOR(1, 0, 1); + EXPECT_SAME_PROJECTION_FACTOR(2, 0, 2); + + // Add second factors + factor_adder_->AddMeasurement(measurements_[1]); + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), num_projections_per_measurement_); + EXPECT_EQ(factors_.size(), 6); + // t0 factors + EXPECT_SAME_PROJECTION_FACTOR(0, 0, 0); + EXPECT_SAME_PROJECTION_FACTOR(1, 0, 1); + EXPECT_SAME_PROJECTION_FACTOR(2, 0, 2); + // t1 factors + EXPECT_SAME_PROJECTION_FACTOR(3, 1, 0); + EXPECT_SAME_PROJECTION_FACTOR(4, 1, 1); + EXPECT_SAME_PROJECTION_FACTOR(5, 1, 2); + + // Repeat add factors with no new measurements, nothing should change + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 0); + EXPECT_EQ(factors_.size(), 6); +} + +TEST_F(LocFactorAdderTest, MaxProjectionFactors) { + auto params = DefaultParams(); + params.add_projection_factors = true; + params.max_num_projection_factors = 2; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factors, only max_num_projection_factors should be added + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), params.max_num_projection_factors); + EXPECT_EQ(factors_.size(), params.max_num_projection_factors); + EXPECT_SAME_PROJECTION_FACTOR(0, 0, 0); + EXPECT_SAME_PROJECTION_FACTOR(1, 0, 1); +} + +TEST_F(LocFactorAdderTest, MinNumMeasurementsPerFactor) { + auto params = DefaultParams(); + params.add_projection_factors = true; + params.add_pose_priors = true; + params.min_num_matches_per_measurement = 10; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factors, none should be added + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 0); + EXPECT_EQ(factors_.size(), 0); +} + +TEST_F(LocFactorAdderTest, ProjectionNoise) { + auto params = DefaultParams(); + params.add_projection_factors = true; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), params.max_num_projection_factors); + const auto expected_noise = + localization_common::Robust(gtsam::SharedIsotropic(gtsam::noiseModel::Isotropic::Sigma( + 2, params_.projection_noise_scale * params_.cam_noise->sigma())), + params_.huber_k); + EXPECT_SAME_NOISE(0, expected_noise); +} + +TEST_F(LocFactorAdderTest, ScaleProjectionNoiseWithNumLandmarks) { + auto params = DefaultParams(); + params.add_projection_factors = true; + params.scale_projection_noise_with_num_landmarks = true; + Initialize(params); + // Add a measurement with only one match + auto measurement_0 = measurements_[0]; + // Remove all measurements after the first + measurement_0.matched_projections.erase(measurement_0.matched_projections.begin() + 1, + measurement_0.matched_projections.end()); + factor_adder_->AddMeasurement(measurement_0); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 1); + { + // Ratio is 1 for the first measurement + const double num_landmarks_ratio = 1; + const double noise_scale = params_.projection_noise_scale * std::pow(num_landmarks_ratio, 2); + const auto expected_noise = localization_common::Robust( + gtsam::SharedIsotropic(gtsam::noiseModel::Isotropic::Sigma(2, noise_scale * params_.cam_noise->sigma())), + params_.huber_k); + EXPECT_SAME_NOISE(0, expected_noise); + } + // Add second measurement with all matches + factor_adder_->AddMeasurement(measurements_[1]); + // Add second factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 3); + EXPECT_EQ(factors_.size(), 4); + { + // New average is (1+3)/2 = 2, so ratio is 2/3 for the second measurement + const double num_landmarks_ratio = 2.0 / 3.0; + const double noise_scale = params_.projection_noise_scale * std::pow(num_landmarks_ratio, 2); + const auto expected_noise = localization_common::Robust( + gtsam::SharedIsotropic(gtsam::noiseModel::Isotropic::Sigma(2, noise_scale * params_.cam_noise->sigma())), + params_.huber_k); + EXPECT_SAME_NOISE(1, expected_noise); + EXPECT_SAME_NOISE(2, expected_noise); + EXPECT_SAME_NOISE(3, expected_noise); + } +} + +TEST_F(LocFactorAdderTest, ScaleProjectionNoiseWithLandmarkDistance) { + auto params = DefaultParams(); + params.add_projection_factors = true; + params.scale_projection_noise_with_landmark_distance = true; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), params.max_num_projection_factors); + const auto world_T_body = node_adder_->values().Value<gtsam::Pose3>(factors_[0]->keys()[0]); + ASSERT_TRUE(world_T_body != boost::none); + // Check first factor noise + { + const Eigen::Vector3d& world_t_landmark = measurements_[0].matched_projections[0].map_point; + const Eigen::Isometry3d nav_cam_T_world = + localization_common::EigenPose(*world_T_body * params_.body_T_cam).inverse(); + const gtsam::Point3 nav_cam_t_landmark = nav_cam_T_world * world_t_landmark; + // Don't use robust cost here to more effectively correct a drift occurance + const auto expected_noise = gtsam::SharedIsotropic( + gtsam::noiseModel::Isotropic::Sigma(2, params_.projection_noise_scale * 1.0 / nav_cam_t_landmark.z())); + EXPECT_SAME_NOISE_NON_ROBUST(0, expected_noise); + } + // Check second factor noise + { + const Eigen::Vector3d& world_t_landmark = measurements_[0].matched_projections[1].map_point; + const Eigen::Isometry3d nav_cam_T_world = + localization_common::EigenPose(*world_T_body * params_.body_T_cam).inverse(); + const gtsam::Point3 nav_cam_t_landmark = nav_cam_T_world * world_t_landmark; + // Don't use robust cost here to more effectively correct a drift occurance + const auto expected_noise = gtsam::SharedIsotropic( + gtsam::noiseModel::Isotropic::Sigma(2, params_.projection_noise_scale * 1.0 / nav_cam_t_landmark.z())); + EXPECT_SAME_NOISE_NON_ROBUST(1, expected_noise); + } + // Check third factor noise + { + const Eigen::Vector3d& world_t_landmark = measurements_[0].matched_projections[2].map_point; + const Eigen::Isometry3d nav_cam_T_world = + localization_common::EigenPose(*world_T_body * params_.body_T_cam).inverse(); + const gtsam::Point3 nav_cam_t_landmark = nav_cam_T_world * world_t_landmark; + // Don't use robust cost here to more effectively correct a drift occurance + const auto expected_noise = gtsam::SharedIsotropic( + gtsam::noiseModel::Isotropic::Sigma(2, params_.projection_noise_scale * 1.0 / nav_cam_t_landmark.z())); + EXPECT_SAME_NOISE_NON_ROBUST(2, expected_noise); + } +} + +TEST_F(LocFactorAdderTest, MinReprojectionError) { + auto params = DefaultParams(); + params.add_projection_factors = true; + params.max_valid_projection_error = 1e6; + Initialize(params); + // Add a measurement with only one match + auto measurement_0 = measurements_[0]; + // Remove all measurements after the first + measurement_0.matched_projections.erase(measurement_0.matched_projections.begin() + 1, + measurement_0.matched_projections.end()); + factor_adder_->AddMeasurement(measurement_0); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + const auto world_T_body = node_adder_->values().Value<gtsam::Pose3>(factors_[0]->keys()[0]); + ASSERT_TRUE(world_T_body != boost::none); + double projection_error; + // Check projection error + { + const auto projection_factor = dynamic_cast<gtsam::LocProjectionFactor<>*>(factors_[0].get()); + ASSERT_TRUE(projection_factor); + projection_error = projection_factor->evaluateError(*world_T_body).norm(); + } + + const double epsilon = 1e-3; + // Redo adding factors, but set projection error threshold slightly below previous error. + // Factor shouldn't be added. + { + factors_ = gtsam::NonlinearFactorGraph(); + auto params = DefaultParams(); + params.add_projection_factors = true; + params.max_valid_projection_error = projection_error - epsilon; + Initialize(params); + // Add a measurement with only one match + auto measurement_0 = measurements_[0]; + // Remove all measurements after the first + measurement_0.matched_projections.erase(measurement_0.matched_projections.begin() + 1, + measurement_0.matched_projections.end()); + factor_adder_->AddMeasurement(measurement_0); + // Factor should fail due to reprojection error + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 0); + } + // Redo adding factors, but set projection error threshold slightly above previous error. + // Factor should be added. + { + factors_ = gtsam::NonlinearFactorGraph(); + auto params = DefaultParams(); + params.add_projection_factors = true; + params.max_valid_projection_error = projection_error + epsilon; + Initialize(params); + // Add a measurement with only one match + auto measurement_0 = measurements_[0]; + // Remove all measurements after the first + measurement_0.matched_projections.erase(measurement_0.matched_projections.begin() + 1, + measurement_0.matched_projections.end()); + factor_adder_->AddMeasurement(measurement_0); + // Factor should succeed + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_SAME_PROJECTION_FACTOR(0, 0, 0); + } +} + +TEST_F(LocFactorAdderTest, PoseFactors) { + auto params = DefaultParams(); + params.add_pose_priors = true; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factor + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 1); + EXPECT_SAME_POSE_FACTOR(0, 0); + const auto pose_noise = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas( + Eigen::Ref<const Eigen::VectorXd>(params_.pose_noise_scale * pose_prior_noise_sigmas_)), + params_.huber_k); + EXPECT_SAME_POSE_NOISE(0, pose_noise); + + factor_adder_->AddMeasurement(measurements_[1]); + // Add second factor + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_POSE_FACTOR(0, 0); + EXPECT_SAME_POSE_NOISE(0, pose_noise); + EXPECT_SAME_POSE_FACTOR(1, 1); + EXPECT_SAME_POSE_NOISE(1, pose_noise); +} + +TEST_F(LocFactorAdderTest, PoseFactorsTooSoon) { + auto params = DefaultParams(); + params.add_pose_priors = true; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factor + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 1); + EXPECT_SAME_POSE_FACTOR(0, 0); + // Add factor too soon, set node adder latest measurement before factor newest_allowed_time + // so factor can't be created. + node_adder_->latest_measurement_time_ = 0.5; + factor_adder_->AddMeasurement(measurements_[1]); + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 0); + EXPECT_EQ(factors_.size(), 1); + EXPECT_SAME_POSE_FACTOR(0, 0); + // Change node adder latest measurement time back, make sure measurement for factor + // adder hasn't been removed and factor can still be created. + node_adder_->latest_measurement_time_ = 2; + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_POSE_FACTOR(0, 0); + EXPECT_SAME_POSE_FACTOR(1, 1); +} + +TEST_F(LocFactorAdderTest, ScalePoseNoiseWithNumLandmarks) { + auto params = DefaultParams(); + params.add_pose_priors = true; + params.scale_pose_noise_with_num_landmarks = true; + Initialize(params); + // Add a measurement with only one match + auto measurement_0 = measurements_[0]; + // Remove all measurements after the first + measurement_0.matched_projections.erase(measurement_0.matched_projections.begin() + 1, + measurement_0.matched_projections.end()); + factor_adder_->AddMeasurement(measurement_0); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 1); + { + // Ratio is 1 for the first measurement + const double num_landmarks_ratio = 1; + const double noise_scale = params_.pose_noise_scale * std::pow(num_landmarks_ratio, 2); + const auto expected_noise = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(noise_scale * pose_prior_noise_sigmas_)), + params_.huber_k); + EXPECT_SAME_POSE_NOISE(0, expected_noise); + } + // Add second measurement with all matches + factor_adder_->AddMeasurement(measurements_[1]); + // Add second factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 2); + { + // New average is (1+3)/2 = 2, so ratio is 2/3 for the second measurement + const double num_landmarks_ratio = 2.0 / 3.0; + const double noise_scale = params_.pose_noise_scale * std::pow(num_landmarks_ratio, 2); + const auto expected_noise = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(noise_scale * pose_prior_noise_sigmas_)), + params_.huber_k); + EXPECT_SAME_POSE_NOISE(1, expected_noise); + } +} + +TEST_F(LocFactorAdderTest, PoseFallback) { + auto params = DefaultParams(); + params.add_projection_factors = true; + params.add_pose_priors = false; + params.add_prior_if_projection_factors_fail = true; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), num_projections_per_measurement_); + EXPECT_EQ(factors_.size(), 3); + EXPECT_SAME_PROJECTION_FACTOR(0, 0, 0); + EXPECT_SAME_PROJECTION_FACTOR(1, 0, 1); + EXPECT_SAME_PROJECTION_FACTOR(2, 0, 2); + + // Force cheirality error for projections, add second factor, expect pose prior to be added + auto measurement_1 = measurements_[1]; + for (int i = 0; i < num_projections_per_measurement_; ++i) { + measurement_1.matched_projections[i].map_point = lm::MapPoint(-100 + i, -99 + i, -98 + i); + } + factor_adder_->AddMeasurement(measurement_1); + EXPECT_EQ(factor_adder_->AddFactors(0, 1, factors_), 1); + EXPECT_EQ(factors_.size(), 4); + EXPECT_SAME_PROJECTION_FACTOR(0, 0, 0); + EXPECT_SAME_PROJECTION_FACTOR(1, 0, 1); + EXPECT_SAME_PROJECTION_FACTOR(2, 0, 2); + EXPECT_SAME_POSE_FACTOR(3, 1); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/graph_localizer/test/test_rotation_factor_adder.test b/localization/factor_adders/test/test_loc_factor_adder.test similarity index 92% rename from localization/graph_localizer/test/test_rotation_factor_adder.test rename to localization/factor_adders/test/test_loc_factor_adder.test index 2ff3ec849b..ee6e741f07 100644 --- a/localization/graph_localizer/test/test_rotation_factor_adder.test +++ b/localization/factor_adders/test/test_loc_factor_adder.test @@ -16,5 +16,5 @@ <!-- permissions and limitations under the License. --> <launch> - <test pkg="graph_localizer" type="test_rotation_factor_adder" test-name="test_rotation_factor_adder" /> + <test pkg="factor_adders" type="test_loc_factor_adder" test-name="test_loc_factor_adder" /> </launch> diff --git a/localization/factor_adders/test/test_single_measurement_based_factor_adder.cc b/localization/factor_adders/test/test_single_measurement_based_factor_adder.cc new file mode 100644 index 0000000000..16fb321d60 --- /dev/null +++ b/localization/factor_adders/test/test_single_measurement_based_factor_adder.cc @@ -0,0 +1,209 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <factor_adders/single_measurement_based_factor_adder.h> +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <localization_measurements/pose_measurement.h> + +#include <gtest/gtest.h> + +namespace fa = factor_adders; +namespace lc = localization_common; +namespace lm = localization_measurements; + +struct SimpleAdderParams : public fa::FactorAdderParams { + SimpleAdderParams() { + huber_k = 1.345; + enabled = true; + } +}; + +class SimpleAdder : public fa::SingleMeasurementBasedFactorAdder<lm::PoseMeasurement> { + public: + explicit SimpleAdder(const SimpleAdderParams& params = SimpleAdderParams()) + : fa::SingleMeasurementBasedFactorAdder<lm::PoseMeasurement>(params), params_(params) { + // Create pose noise + const double translation_stddev = 0.1; + const double rotation_stddev = 0.2; + const gtsam::Vector6 pose_noise_sigmas((gtsam::Vector(6) << rotation_stddev, rotation_stddev, rotation_stddev, + translation_stddev, translation_stddev, translation_stddev) + .finished()); + pose_noise_ = lc::Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(pose_noise_sigmas)), + params_.huber_k); + } + + int end_time_ = 10; + + private: + int AddFactorsForSingleMeasurement(const lm::PoseMeasurement& measurement, + gtsam::NonlinearFactorGraph& factors) final { + gtsam::PriorFactor<gtsam::Pose3>::shared_ptr pose_prior_factor( + new gtsam::PriorFactor<gtsam::Pose3>(gtsam::Key(key_value_++), measurement.pose, pose_noise_)); + factors.push_back(pose_prior_factor); + return 1; + } + + // Simulate some node adder delay and don't add factors if past end time. + bool CanAddFactor(const lm::PoseMeasurement& measurement) const final { return measurement.timestamp <= end_time_; } + + int key_value_ = 0; + gtsam::SharedNoiseModel pose_noise_; + SimpleAdderParams params_; +}; + +class SingleMeasurementBasedFactorAdderTest : public ::testing::Test { + public: + SingleMeasurementBasedFactorAdderTest() {} + + void SetUp() final { + constexpr int kNumMeasurements = 10; + for (int i = 0; i < kNumMeasurements; ++i) { + const lm::PoseMeasurement measurement(lc::RandomPose(), lc::Time(i)); + measurements_.emplace_back(measurement); + } + } + + void AddMeasurements() { + for (const auto& measurement : measurements_) factor_adder_.AddMeasurement(measurement); + } + + // TODO(rsoussan): Unify this with pose_node_adder test! + void EXPECT_SAME_PRIOR_FACTOR(const int index, const Eigen::Isometry3d& pose) { + const auto pose_prior_factor = dynamic_cast<gtsam::PriorFactor<gtsam::Pose3>*>(factors_[index].get()); + ASSERT_TRUE(pose_prior_factor); + EXPECT_MATRIX_NEAR(pose_prior_factor->prior(), pose, 1e-6); + } + + void EXPECT_SAME_PRIOR_FACTOR(const int index, const gtsam::Pose3& pose) { + EXPECT_SAME_PRIOR_FACTOR(index, lc::EigenPose(pose)); + } + + void EXPECT_SAME_PRIOR_FACTOR(const int index) { EXPECT_SAME_PRIOR_FACTOR(index, pose(index)); } + + void EXPECT_SAME_PRIOR_FACTOR(const int factor_index, const int pose_index) { + EXPECT_SAME_PRIOR_FACTOR(factor_index, pose(pose_index)); + } + + lc::Time time(int index) { return measurements_[index].timestamp; } + + const gtsam::Pose3& pose(const int index) { return measurements_[index].pose; } + + SimpleAdder factor_adder_; + std::vector<lm::PoseMeasurement> measurements_; + gtsam::NonlinearFactorGraph factors_; + + private: +}; + +TEST_F(SingleMeasurementBasedFactorAdderTest, AddMeasurements) { + AddMeasurements(); + EXPECT_EQ(factor_adder_.AddFactors(-100, -10, factors_), 0); + EXPECT_EQ(factors_.size(), 0); + // Add factor 0 + EXPECT_EQ(factor_adder_.AddFactors(-100.1, time(0), factors_), 1); + EXPECT_EQ(factors_.size(), 1); + EXPECT_SAME_PRIOR_FACTOR(0); + // Add factors 1, 2, 3 + EXPECT_EQ(factor_adder_.AddFactors(time(1) - 0.1, time(3) + 0.1, factors_), 3); + EXPECT_EQ(factors_.size(), 4); + EXPECT_SAME_PRIOR_FACTOR(0); + EXPECT_SAME_PRIOR_FACTOR(1); + EXPECT_SAME_PRIOR_FACTOR(2); + EXPECT_SAME_PRIOR_FACTOR(3); + // Add factors 1, 2, 3 again, should have no affect + EXPECT_EQ(factor_adder_.AddFactors(time(1) - 0.1, time(3) + 0.1, factors_), 0); + EXPECT_EQ(factors_.size(), 4); + EXPECT_SAME_PRIOR_FACTOR(0); + EXPECT_SAME_PRIOR_FACTOR(1); + EXPECT_SAME_PRIOR_FACTOR(2); + EXPECT_SAME_PRIOR_FACTOR(3); + // Add factors 4, 5 + EXPECT_EQ(factor_adder_.AddFactors(time(4) - 0.1, time(5), factors_), 2); + EXPECT_EQ(factors_.size(), 6); + EXPECT_SAME_PRIOR_FACTOR(0); + EXPECT_SAME_PRIOR_FACTOR(1); + EXPECT_SAME_PRIOR_FACTOR(2); + EXPECT_SAME_PRIOR_FACTOR(3); + EXPECT_SAME_PRIOR_FACTOR(4); + EXPECT_SAME_PRIOR_FACTOR(5); + // Old and used measurements should be removed up, expect adding factors 0,1,2 to fail + EXPECT_EQ(factor_adder_.AddFactors(time(0) - 0.1, time(2), factors_), 0); + EXPECT_EQ(factors_.size(), 6); + EXPECT_SAME_PRIOR_FACTOR(0); + EXPECT_SAME_PRIOR_FACTOR(1); + EXPECT_SAME_PRIOR_FACTOR(2); + EXPECT_SAME_PRIOR_FACTOR(3); + EXPECT_SAME_PRIOR_FACTOR(4); + EXPECT_SAME_PRIOR_FACTOR(5); + // Add factors 8, 9 + EXPECT_EQ(factor_adder_.AddFactors(time(8) - 0.1, time(9), factors_), 2); + EXPECT_EQ(factors_.size(), 8); + EXPECT_SAME_PRIOR_FACTOR(0); + EXPECT_SAME_PRIOR_FACTOR(1); + EXPECT_SAME_PRIOR_FACTOR(2); + EXPECT_SAME_PRIOR_FACTOR(3); + EXPECT_SAME_PRIOR_FACTOR(4); + EXPECT_SAME_PRIOR_FACTOR(5); + EXPECT_SAME_PRIOR_FACTOR(6, 8); + EXPECT_SAME_PRIOR_FACTOR(7, 9); + // Add factors 6,7 - should fail since already used an oldest allowed time (8) newer + // than each of these and therefore measurements were removed + EXPECT_EQ(factor_adder_.AddFactors(time(6) - 0.1, time(7), factors_), 0); + EXPECT_EQ(factors_.size(), 8); + EXPECT_SAME_PRIOR_FACTOR(0); + EXPECT_SAME_PRIOR_FACTOR(1); + EXPECT_SAME_PRIOR_FACTOR(2); + EXPECT_SAME_PRIOR_FACTOR(3); + EXPECT_SAME_PRIOR_FACTOR(4); + EXPECT_SAME_PRIOR_FACTOR(5); + EXPECT_SAME_PRIOR_FACTOR(6, 8); + EXPECT_SAME_PRIOR_FACTOR(7, 9); + // Can't add factors newer than measurements + EXPECT_EQ(factor_adder_.AddFactors(time(9) + 0.1, time(9) + 1000.1, factors_), 0); + EXPECT_EQ(factors_.size(), 8); +} + +TEST_F(SingleMeasurementBasedFactorAdderTest, AddFactorsTooSoon) { + AddMeasurements(); + // Add factor 0 + EXPECT_EQ(factor_adder_.AddFactors(-100.1, time(0), factors_), 1); + EXPECT_EQ(factors_.size(), 1); + EXPECT_SAME_PRIOR_FACTOR(0); + // Make factors unaddable, make sure none are added + factor_adder_.end_time_ = 0.5; + EXPECT_EQ(factor_adder_.AddFactors(time(1), time(3), factors_), 0); + EXPECT_EQ(factors_.size(), 1); + EXPECT_SAME_PRIOR_FACTOR(0); + // Increase end time, measurements shouldn't have been removed and + // now factors should be added. + factor_adder_.end_time_ = 10; + EXPECT_EQ(factor_adder_.AddFactors(time(1), time(3), factors_), 3); + EXPECT_EQ(factors_.size(), 4); + EXPECT_SAME_PRIOR_FACTOR(0); + EXPECT_SAME_PRIOR_FACTOR(1); + EXPECT_SAME_PRIOR_FACTOR(2); + EXPECT_SAME_PRIOR_FACTOR(3); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/factor_adders/test/test_single_measurement_based_factor_adder.test b/localization/factor_adders/test/test_single_measurement_based_factor_adder.test new file mode 100644 index 0000000000..55c3f342bd --- /dev/null +++ b/localization/factor_adders/test/test_single_measurement_based_factor_adder.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="factor_adders" type="test_single_measurement_based_factor_adder" test-name="test_single_measurement_based_factor_adder" /> +</launch> diff --git a/localization/factor_adders/test/test_standstill_factor_adder.cc b/localization/factor_adders/test/test_standstill_factor_adder.cc new file mode 100644 index 0000000000..9183c2c51f --- /dev/null +++ b/localization/factor_adders/test/test_standstill_factor_adder.cc @@ -0,0 +1,249 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <factor_adders/standstill_factor_adder.h> +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <localization_measurements/standstill_measurement.h> +#include <node_adders/node_adder.h> +#include <node_adders/utilities.h> + +#include <gtest/gtest.h> + +namespace fa = factor_adders; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace na = node_adders; + +// Test node adder that just returns keys that should be used. +// Key values are calculated using the integer timestamps passed, where the +// first key is a pose key and second is a velocity key. +// Pose keys are 2*timestamp and velocity keys are 2*timestamp + 1. +class SimplePoseVelocityNodeAdder : public na::NodeAdder { + public: + void AddInitialNodesAndPriors(gtsam::NonlinearFactorGraph& graph) final{}; + + bool AddNode(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final { return true; } + + bool CanAddNode(const localization_common::Time timestamp) const final { return true; } + + // Assumes integer timestamps that perfectly cast to ints. + // First key is pose key, second is velocity key + gtsam::KeyVector Keys(const localization_common::Time timestamp) const final { + gtsam::KeyVector keys; + keys.emplace_back(gtsam::Key(static_cast<int>(timestamp) * 2)); + keys.emplace_back(gtsam::Key(static_cast<int>(timestamp) * 2 + 1)); + return keys; + } +}; + +class StandstillFactorAdderTest : public ::testing::Test { + public: + StandstillFactorAdderTest() { node_adder_.reset(new SimplePoseVelocityNodeAdder()); } + + void SetUp() final {} + + void AddMeasurements() { + constexpr int kNumMeasurements = 10; + for (int i = 0; i < kNumMeasurements; ++i) { + const lm::StandstillMeasurement measurement(i + 1, i); + measurements_.emplace_back(measurement); + factor_adder_->AddMeasurement(measurement); + } + } + + void Initialize(const fa::StandstillFactorAdderParams& params) { + factor_adder_.reset(new fa::StandstillFactorAdder<SimplePoseVelocityNodeAdder>(params, node_adder_)); + // Create zero velocity noise + const gtsam::Vector3 velocity_prior_noise_sigmas( + (gtsam::Vector(3) << params.prior_velocity_stddev, params.prior_velocity_stddev, params.prior_velocity_stddev) + .finished()); + zero_velocity_noise_ = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(velocity_prior_noise_sigmas)), + params.huber_k); + + // Create zero relative pose noise + const gtsam::Vector6 pose_between_noise_sigmas( + (gtsam::Vector(6) << params.pose_between_factor_rotation_stddev, params.pose_between_factor_rotation_stddev, + params.pose_between_factor_rotation_stddev, params.pose_between_factor_translation_stddev, + params.pose_between_factor_translation_stddev, params.pose_between_factor_translation_stddev) + .finished()); + zero_relative_pose_noise_ = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(pose_between_noise_sigmas)), + params.huber_k); + } + + fa::StandstillFactorAdderParams DefaultParams() { + fa::StandstillFactorAdderParams params; + params.enabled = true; + params.huber_k = 1.345; + params.add_velocity_prior = true; + params.add_pose_between_factor = true; + params.prior_velocity_stddev = 0.1; + params.pose_between_factor_translation_stddev = 0.2; + params.pose_between_factor_rotation_stddev = 0.3; + return params; + } + + // TODO(rsoussan): Get from common location, share with pose_node_adder test + template <typename FactorPtrType> + void EXPECT_SAME_NOISE(const FactorPtrType factor, const gtsam::Matrix& covariance) { + EXPECT_MATRIX_NEAR(na::Covariance(factor->noiseModel()), covariance, 1e-6); + } + + template <typename FactorPtrType> + void EXPECT_SAME_NOISE(const FactorPtrType factor, const gtsam::SharedNoiseModel noise) { + EXPECT_SAME_NOISE(factor, na::Covariance(noise)); + } + + void EXPECT_SAME_VELOCITY_PRIOR_FACTOR(const int factor_index, const int key_index) { + const auto velocity_prior_factor = + dynamic_cast<gtsam::PriorFactor<gtsam::Velocity3>*>(factors_[factor_index].get()); + ASSERT_TRUE(velocity_prior_factor); + EXPECT_MATRIX_NEAR(velocity_prior_factor->prior(), Eigen::Vector3d::Zero(), 1e-6); + EXPECT_EQ(velocity_prior_factor->key(), gtsam::Key(key_index)); + EXPECT_SAME_NOISE(velocity_prior_factor, zero_velocity_noise_); + } + + void EXPECT_SAME_POSE_BETWEEN_FACTOR(const int factor_index, const int key_index) { + const auto pose_between_factor = dynamic_cast<gtsam::BetweenFactor<gtsam::Pose3>*>(factors_[factor_index].get()); + ASSERT_TRUE(pose_between_factor); + EXPECT_MATRIX_NEAR(pose_between_factor->measured(), Eigen::Isometry3d::Identity(), 1e-6); + EXPECT_EQ(pose_between_factor->key1(), gtsam::Key(key_index)); + EXPECT_EQ(pose_between_factor->key2(), gtsam::Key((key_index + 2))); + EXPECT_SAME_NOISE(pose_between_factor, zero_relative_pose_noise_); + } + + lc::Time time(int index) { return measurements_[index].timestamp; } + + std::unique_ptr<fa::StandstillFactorAdder<SimplePoseVelocityNodeAdder>> factor_adder_; + std::shared_ptr<SimplePoseVelocityNodeAdder> node_adder_; + gtsam::NonlinearFactorGraph factors_; + gtsam::SharedNoiseModel zero_velocity_noise_; + gtsam::SharedNoiseModel zero_relative_pose_noise_; + + private: + std::vector<lm::StandstillMeasurement> measurements_; +}; + +TEST_F(StandstillFactorAdderTest, PoseAndVelocityFactors) { + auto params = DefaultParams(); + Initialize(params); + AddMeasurements(); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(time(0), time(0), factors_), 2); + EXPECT_EQ(factors_.size(), 2); + // Keys and their indices: + // pose_0: 0, velocity_0: 1 + // pose_1: 2, velocity_1: 3 + // Factors and their indices: + // pose_between: 0, velocity_prior: 1 + EXPECT_SAME_POSE_BETWEEN_FACTOR(0, 0); + // Use velocity_1 key since velocity prior is added to most recent timestamp + // in standstill measurement + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(1, 3); + // Add 2nd and 3rd factors + EXPECT_EQ(factor_adder_->AddFactors((time(0) + time(1)) / 2.0, (time(2) + time(3)) / 2.0, factors_), 4); + EXPECT_EQ(factors_.size(), 6); + // Keys and their indices: + // pose_0: 0, velocity_0: 1 + // pose_1: 2, velocity_1: 3 + // pose_2: 4, velocity_1: 5 + // pose_3: 6, velocity_1: 7 + // Factors and their indices: + // pose_between: 0, velocity_prior: 1 + // pose_between: 2, velocity_prior: 3 + // pose_between: 4, velocity_prior: 5 + EXPECT_SAME_POSE_BETWEEN_FACTOR(0, 0); + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(1, 3); + EXPECT_SAME_POSE_BETWEEN_FACTOR(2, 2); + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(3, 5); + EXPECT_SAME_POSE_BETWEEN_FACTOR(4, 4); + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(5, 7); +} + +TEST_F(StandstillFactorAdderTest, PoseOnlyFactors) { + auto params = DefaultParams(); + params.add_velocity_prior = false; + Initialize(params); + AddMeasurements(); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(time(0), time(0), factors_), 1); + EXPECT_EQ(factors_.size(), 1); + // Keys and their indices: + // pose_0: 0, velocity_0: 1 + // pose_1: 2, velocity_1: 3 + // Factors and their indices: + // pose_between: 0 + EXPECT_SAME_POSE_BETWEEN_FACTOR(0, 0); + // Add 2nd and 3rd factors + EXPECT_EQ(factor_adder_->AddFactors((time(0) + time(1)) / 2.0, (time(2) + time(3)) / 2.0, factors_), 2); + EXPECT_EQ(factors_.size(), 3); + // Keys and their indices: + // pose_0: 0, velocity_0: 1 + // pose_1: 2, velocity_1: 3 + // pose_2: 4, velocity_1: 5 + // pose_3: 6, velocity_1: 7 + // Factors and their indices: + // pose_between: 0 + // pose_between: 1 + // pose_between: 2 + EXPECT_SAME_POSE_BETWEEN_FACTOR(0, 0); + EXPECT_SAME_POSE_BETWEEN_FACTOR(1, 2); + EXPECT_SAME_POSE_BETWEEN_FACTOR(2, 4); +} + +TEST_F(StandstillFactorAdderTest, VelocityOnlyFactors) { + auto params = DefaultParams(); + params.add_pose_between_factor = false; + Initialize(params); + AddMeasurements(); + // Add first factors + EXPECT_EQ(factor_adder_->AddFactors(time(0), time(0), factors_), 1); + EXPECT_EQ(factors_.size(), 1); + // Keys and their indices: + // pose_0: 0, velocity_0: 1 + // pose_1: 2, velocity_1: 3 + // Factors and their indices: + // velocity_prior: 0 + // Use velocity_1 key since velocity prior is added to most recent timestamp + // in standstill measurement + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(0, 3); + // Add 2nd and 3rd factors + EXPECT_EQ(factor_adder_->AddFactors((time(0) + time(1)) / 2.0, (time(2) + time(3)) / 2.0, factors_), 2); + EXPECT_EQ(factors_.size(), 3); + // Keys and their indices: + // pose_0: 0, velocity_0: 1 + // pose_1: 2, velocity_1: 3 + // pose_2: 4, velocity_1: 5 + // pose_3: 6, velocity_1: 7 + // Factors and their indices: + // velocity_prior: 0 + // velocity_prior: 1 + // velocity_prior: 2 + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(0, 3); + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(1, 5); + EXPECT_SAME_VELOCITY_PRIOR_FACTOR(2, 7); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/graph_localizer/test/test_depth_odometry_factor_adder.test b/localization/factor_adders/test/test_standstill_factor_adder.test similarity index 91% rename from localization/graph_localizer/test/test_depth_odometry_factor_adder.test rename to localization/factor_adders/test/test_standstill_factor_adder.test index 7af2876610..1f4d38543b 100644 --- a/localization/graph_localizer/test/test_depth_odometry_factor_adder.test +++ b/localization/factor_adders/test/test_standstill_factor_adder.test @@ -16,5 +16,5 @@ <!-- permissions and limitations under the License. --> <launch> - <test pkg="graph_localizer" type="test_depth_odometry_factor_adder" test-name="test_depth_odometry_factor_adder" /> + <test pkg="factor_adders" type="test_standstill_factor_adder" test-name="test_standstill_factor_adder" /> </launch> diff --git a/localization/factor_adders/test/test_vo_smart_projection_factor_adder.cc b/localization/factor_adders/test/test_vo_smart_projection_factor_adder.cc new file mode 100644 index 0000000000..3d91bd1a1c --- /dev/null +++ b/localization/factor_adders/test/test_vo_smart_projection_factor_adder.cc @@ -0,0 +1,592 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <factor_adders/vo_smart_projection_factor_adder.h> +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <localization_measurements/feature_points_measurement.h> +#include <node_adders/node_adder.h> +#include <node_adders/utilities.h> +#include <nodes/timestamped_nodes.h> +#include <nodes/values.h> +#include <vision_common/feature_point.h> + +#include <gtest/gtest.h> + +namespace fa = factor_adders; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace na = node_adders; +namespace no = nodes; +namespace vc = vision_common; + +// Test node adder that just returns keys that should be used. +// Key values are calculated using the integer timestamps passed. +class SimplePoseNodeAdder : public na::NodeAdder { + public: + SimplePoseNodeAdder() : values_(std::make_shared<no::Values>()), nodes_(values_) {} + void AddInitialNodesAndPriors(gtsam::NonlinearFactorGraph& graph) final{}; + + bool AddNode(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final { return true; } + + bool CanAddNode(const localization_common::Time timestamp) const final { + return timestamp <= latest_measurement_time_; + } + + // Assumes integer timestamps that perfectly cast to ints. + // First key is pose key. + gtsam::KeyVector Keys(const localization_common::Time timestamp) const final { + if (!CanAddNode(timestamp)) return gtsam::KeyVector(); + gtsam::KeyVector keys; + keys.emplace_back(gtsam::Key(static_cast<int>(timestamp))); + return keys; + } + + const no::Values& values() const { return *values_; } + + no::Values& values() { return *values_; } + + no::TimestampedNodes<gtsam::Pose3>& nodes() { return nodes_; } + + // Simulate measurement delay for node adder and control end of measurements time. + double latest_measurement_time_ = 10; + + private: + std::shared_ptr<no::Values> values_; + no::TimestampedNodes<gtsam::Pose3> nodes_; +}; + +class VoSmartProjectionFactorAdderTest : public ::testing::Test { + public: + VoSmartProjectionFactorAdderTest() { node_adder_.reset(new SimplePoseNodeAdder()); } + + void SetUp() final { CreateMeasurements(); } + + // Add measurements for each track at multiple timestamps + void CreateMeasurements() { + for (int time = 0; time < num_measurements_; ++time) { + lm::FeaturePointsMeasurement measurement; + measurement.timestamp = time; + for (int track_id = 0; track_id < num_tracks_; ++track_id) { + // Make image points different for different measurements + const vc::FeaturePoint p(track_id * 10 + time, track_id * 10 + time + 1, time + 1, track_id, time); + measurement.feature_points.emplace_back(p); + } + measurements_.emplace_back(measurement); + } + } + + void EXPECT_SAME_FACTOR(const int factor_index, const std::vector<int>& timestamps, + const std::vector<lm::FeaturePointsMeasurement>& measurements, + const std::vector<int>& measurement_indices) { + const auto factor = factors_[factor_index]; + const auto smart_factor = dynamic_cast<const fa::RobustSmartFactor*>(factor.get()); + ASSERT_TRUE(smart_factor); + const auto& keys = factor->keys(); + EXPECT_EQ(keys.size(), timestamps.size()); + const auto& factor_measurements = smart_factor->measured(); + EXPECT_EQ(factor_measurements.size(), timestamps.size()); + for (int i = 0; i < keys.size(); ++i) { + // Factors store measurements in reverse order + int measurement_index = keys.size() - 1 - i; + EXPECT_EQ(keys[measurement_index], gtsam::Key(timestamps[i])); + EXPECT_MATRIX_NEAR(factor_measurements[measurement_index], + measurements[measurement_indices[i]].feature_points[factor_index].image_point, 1e-6); + } + + // Check factor noise + // Uses inv sigma scaling, applies optional robust loss to whitened noise after + const double noise_scale = + params_.scale_noise_with_num_points ? params_.noise_scale * measurements.size() : params_.noise_scale; + const double expected_sigma = noise_scale * params_.cam_noise->sigma(); + EXPECT_NEAR(smart_factor->noise_inv_sigma(), 1.0 / expected_sigma, 1e-6); + EXPECT_EQ(smart_factor->robust(), params_.robust); + } + + void EXPECT_SAME_FACTOR(const int factor_index, const std::vector<int>& timestamps) { + std::vector<int> measurement_indices; + for (const auto& time : timestamps) { + measurement_indices.emplace_back(time); + } + EXPECT_SAME_FACTOR(factor_index, timestamps, measurements_, measurement_indices); + } + + lc::Time timestamp(const int measurement_index) { return measurements_[measurement_index].timestamp; } + + void Initialize(const fa::VoSmartProjectionFactorAdderParams& params) { + factor_adder_.reset(new fa::VoSmartProjectionFactorAdder<SimplePoseNodeAdder>(params, node_adder_)); + params_ = params; + } + + fa::VoSmartProjectionFactorAdderParams DefaultParams() { + fa::VoSmartProjectionFactorAdderParams params; + // Feature Tracker Params + params.spaced_feature_tracker.measurement_spacing = 0; + params.spaced_feature_tracker.remove_undetected_feature_tracks = true; + params.max_num_factors = 2; + params.min_num_points_per_factor = 2; + params.max_num_points_per_factor = 3; + params.min_avg_distance_from_mean = 1e-9; + params.robust = true; + params.rotation_only_fallback = false; + params.fix_invalid_factors = false; + params.scale_noise_with_num_points = false; + params.noise_scale = 1; + params.body_T_cam = gtsam::Pose3::identity(); + params.cam_intrinsics = boost::make_shared<gtsam::Cal3_S2>(); + params.cam_noise = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); + // Smart Factor Params + params.smart_factor.verboseCheirality = false; + params.smart_factor.setRankTolerance(1e-9); + params.smart_factor.setLandmarkDistanceThreshold(3); + params.smart_factor.setDynamicOutlierRejectionThreshold(1); + params.smart_factor.setRetriangulationThreshold(0.1); + params.smart_factor.setDegeneracyMode(gtsam::DegeneracyMode::HANDLE_INFINITY); + params.smart_factor.setEnableEPI(false); + params.enabled = true; + params.huber_k = 1.345; + return params; + } + + std::unique_ptr<fa::VoSmartProjectionFactorAdder<SimplePoseNodeAdder>> factor_adder_; + std::shared_ptr<SimplePoseNodeAdder> node_adder_; + gtsam::NonlinearFactorGraph factors_; + std::vector<lm::FeaturePointsMeasurement> measurements_; + fa::VoSmartProjectionFactorAdderParams params_; + int num_measurements_ = 10; + int num_tracks_ = 3; +}; + +TEST_F(VoSmartProjectionFactorAdderTest, AddFactors) { + auto params = DefaultParams(); + Initialize(params); + const int max_factors = std::min(params.max_num_factors, num_tracks_); + // Add first measurement + // No factors should be added since there are too few measurements for each factor + factor_adder_->AddMeasurement(measurements_[0]); + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(0), factors_), 0); + // Track: Measurement Timestamps + // 0: 0 + // 1: 0 + // 2: 0 + EXPECT_EQ(factors_.size(), 0); + // Add second measurement + factor_adder_->AddMeasurement(measurements_[1]); + // No factors added if don't include second measurement timestamp + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), (timestamp(0) + timestamp(1)) / 2.0, factors_), 0); + // Track: Measurement Timestamps + // 0: 0 + // 1: 0 + // 2: 0 + EXPECT_EQ(factors_.size(), 0); + // All factors added if include second measurement timestamp + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(1), factors_), max_factors); + // Track: Measurement Timestamps + // 0: 0, 1 + // 1: 0, 1 + // 2: 0, 1 + EXPECT_SAME_FACTOR(0, {0, 1}); + EXPECT_SAME_FACTOR(1, {0, 1}); + EXPECT_EQ(factors_.size(), max_factors); + // Try to add factors from t: 1->2, but no measurement added yet for t2, so not enough + // measurements to add factors + EXPECT_EQ(factor_adder_->AddFactors(timestamp(1), timestamp(2), factors_), 0); + // Track: Measurement Timestamps + // 0: 1 + // 1: 1 + // 2: 1 + EXPECT_EQ(factors_.size(), 0); + // Add 3rd measurement + factor_adder_->AddMeasurement(measurements_[2]); + // Add factors from t: 1->2 + EXPECT_EQ(factor_adder_->AddFactors(timestamp(1), timestamp(2), factors_), 2); + // Track: Measurement Timestamps + // 0: 1, 2 + // 1: 1, 2 + // 2: 1, 2 + EXPECT_SAME_FACTOR(0, {1, 2}); + EXPECT_SAME_FACTOR(1, {1, 2}); + EXPECT_EQ(factors_.size(), 2); + // Add 4th and 5th Measurement + factor_adder_->AddMeasurement(measurements_[3]); + factor_adder_->AddMeasurement(measurements_[4]); + // Add factors from t: 5->6 + // Should erase all measurements and add no factors + EXPECT_EQ(factor_adder_->AddFactors(timestamp(5), timestamp(6), factors_), 0); + // Track: Measurement Timestamps + // 0: + // 1: + // 2: + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(5), factors_), 0); + factor_adder_->AddMeasurement(measurements_[5]); + factor_adder_->AddMeasurement(measurements_[6]); + factor_adder_->AddMeasurement(measurements_[7]); + factor_adder_->AddMeasurement(measurements_[8]); + factor_adder_->AddMeasurement(measurements_[9]); + // Add factors from t: 5->8 + EXPECT_EQ(factor_adder_->AddFactors(timestamp(5), timestamp(8), factors_), 2); + // Track: Measurement Timestamps + // 0: 5, 6, 7, 8 + // 1: 5, 6, 7, 8 + // 2: 5, 6, 7, 8 + EXPECT_EQ(factors_.size(), 2); + // Since max points per factor is 3, expect only last three measurements to be used. + EXPECT_SAME_FACTOR(0, {6, 7, 8}); + EXPECT_SAME_FACTOR(1, {6, 7, 8}); +} + +TEST_F(VoSmartProjectionFactorAdderTest, AddSpacedFactors) { + auto params = DefaultParams(); + params.spaced_feature_tracker.measurement_spacing = 1; + Initialize(params); + const int max_factors = std::min(params.max_num_factors, num_tracks_); + // Add first measurement + // No factors should be added since there are too few measurements for each factor + factor_adder_->AddMeasurement(measurements_[0]); + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(0), factors_), 0); + // Track: Measurement Timestamps + // 0: 0 + // 1: 0 + // 2: 0 + EXPECT_EQ(factors_.size(), 0); + // Add second measurement + // No factors added since skip second measurement due to measurement spacing + factor_adder_->AddMeasurement(measurements_[1]); + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(1), factors_), 0); + // Track: Measurement Timestamps (skipped measurements in parenthesis) + // 0: 0, (1) + // 1: 0, (1) + // 2: 0, (1) + EXPECT_EQ(factors_.size(), 0); + // Add 3rd measurement + // Add factors from t: 0->3 + factor_adder_->AddMeasurement(measurements_[2]); + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(2), factors_), 2); + // Track: Measurement Timestamps + // 0: 0, (1), 2 + // 1: 0, (1), 2 + // 2: 0, (1), 2 + EXPECT_SAME_FACTOR(0, {0, 2}); + EXPECT_SAME_FACTOR(1, {0, 2}); + // Adding repeat factors shouldn't change graph + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(2), factors_), 2); + // Track: Measurement Timestamps + // 0: 0, (1), 2 + // 1: 0, (1), 2 + // 2: 0, (1), 2 + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_FACTOR(0, {0, 2}); + EXPECT_SAME_FACTOR(1, {0, 2}); + // Add measurements from 3->7 + factor_adder_->AddMeasurement(measurements_[3]); + factor_adder_->AddMeasurement(measurements_[4]); + factor_adder_->AddMeasurement(measurements_[5]); + factor_adder_->AddMeasurement(measurements_[6]); + factor_adder_->AddMeasurement(measurements_[7]); + // Add factors from 3 -> 7 + // Only timestamps 4 and 6 should be used + EXPECT_EQ(factor_adder_->AddFactors(timestamp(3), timestamp(7), factors_), 2); + // Track: Measurement Timestamps + // 0: (3), 4, (5), 6, (7) + // 1: (3), 4, (5), 6, (7) + // 2: (3), 4, (5), 6, (7) + EXPECT_SAME_FACTOR(0, {4, 6}); + EXPECT_SAME_FACTOR(1, {4, 6}); +} + +TEST_F(VoSmartProjectionFactorAdderTest, AvgDistFromMean) { + auto params = DefaultParams(); + // Each point pair is separated by (1, 1), so for 3 pairs: + // (10, 11), (11, 12), (12, 13) -> mean: (11, 12) + // avg dist from mean: 2*|((1,1)|/3 = 2sqrt(2)/3 ~= 0.9428 + // Use a slightly larger min so all tracks are invalid + const double expected_avg_distance_from_mean = 2.0 * std::sqrt(2) / 3.0; + const double epsilon = 1e-6; + params.min_avg_distance_from_mean = expected_avg_distance_from_mean + epsilon; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + factor_adder_->AddMeasurement(measurements_[1]); + factor_adder_->AddMeasurement(measurements_[2]); + // A factors should fail to be added due to avg distance from mean failure. + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(2), factors_), 0); + + // Repeat with slightly smaller distance, all factors should be added + params.min_avg_distance_from_mean = expected_avg_distance_from_mean - epsilon; + Initialize(params); + factor_adder_->AddMeasurement(measurements_[0]); + factor_adder_->AddMeasurement(measurements_[1]); + factor_adder_->AddMeasurement(measurements_[2]); + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(2), factors_), 2); +} + +TEST_F(VoSmartProjectionFactorAdderTest, ValidFactor) { + auto params = DefaultParams(); + params.fix_invalid_factors = true; + params.min_avg_distance_from_mean = 0; + Initialize(params); + const int track_id = 0; + const int times = 3; + // Add same measurement at different timestamps + // Populate graph values with valid poses with translation spread + std::vector<lm::FeaturePointsMeasurement> measurements; + gtsam::KeyVector added_keys; + // Start with time 1 since first node in nodes has a key of 1 + for (int time = 1; time <= times; ++time) { + lm::FeaturePointsMeasurement measurement; + measurement.timestamp = time; + const vc::FeaturePoint p(0, 0, time, track_id, time); + measurement.feature_points.emplace_back(p); + measurements.emplace_back(measurement); + const gtsam::Pose3 pose(gtsam::Rot3::identity(), gtsam::Point3(time, time, 0)); + // Add node here since not actually added in simple node adder + const auto key = node_adder_->values().Add(pose); + added_keys.emplace_back(key); + } + factor_adder_->AddMeasurement(measurements[0]); + factor_adder_->AddMeasurement(measurements[1]); + factor_adder_->AddMeasurement(measurements[2]); + // All measurements should be valid + EXPECT_EQ(factor_adder_->AddFactors(1, 3, factors_), 1); + // Keys 1,2,3, should match to measurements 0,1,2 + EXPECT_SAME_FACTOR(0, {1, 2, 3}, measurements, {0, 1, 2}); +} + +TEST_F(VoSmartProjectionFactorAdderTest, InvalidLastMeasurementFactor) { + auto params = DefaultParams(); + params.fix_invalid_factors = true; + params.min_avg_distance_from_mean = 0; + params.max_num_points_per_factor = 4; + Initialize(params); + const int track_id = 0; + const int times = 4; + // Add same measurement at different timestamps + // Populate graph values with valid poses with translation spread + std::vector<lm::FeaturePointsMeasurement> measurements; + gtsam::KeyVector added_keys; + // Start with time 1 since first node in nodes has a key of 1 + for (int time = 1; time <= times; ++time) { + lm::FeaturePointsMeasurement measurement; + measurement.timestamp = time; + const vc::FeaturePoint p(0, 0, time, track_id, time); + measurement.feature_points.emplace_back(p); + measurements.emplace_back(measurement); + gtsam::Rot3 rotation = gtsam::Rot3::identity(); + gtsam::Point3 translation(time, time, 0); + // Make last measurement invalid. + // Turn the camera in the opposite direction so point can't be triangulated + if (time == times) { + rotation = gtsam::Rot3::Ypr(M_PI / 2, 0., -M_PI / 2); + } + const gtsam::Pose3 pose(rotation, translation); + // Add node here since not actually added in simple node adder + const auto key = node_adder_->values().Add(pose); + added_keys.emplace_back(key); + } + + factor_adder_->AddMeasurement(measurements[0]); + factor_adder_->AddMeasurement(measurements[1]); + factor_adder_->AddMeasurement(measurements[2]); + factor_adder_->AddMeasurement(measurements[3]); + // Valid factor since with removed measurement should stil have two points. + EXPECT_EQ(factor_adder_->AddFactors(0, 5, factors_), 1); + // Middle measurement should be removed. + // Keys 1,2,3 should match to measurements 0,1,2 + EXPECT_SAME_FACTOR(0, {1, 2, 3}, measurements, {0, 1, 2}); +} + +TEST_F(VoSmartProjectionFactorAdderTest, InvalidMiddleMeasurementFactor) { + auto params = DefaultParams(); + params.fix_invalid_factors = true; + params.min_avg_distance_from_mean = 0; + params.max_num_points_per_factor = 4; + Initialize(params); + const int track_id = 0; + const int times = 4; + // Add same measurement at different timestamps + // Populate graph values with valid poses with translation spread + std::vector<lm::FeaturePointsMeasurement> measurements; + gtsam::KeyVector added_keys; + // Start with time 1 since first node in nodes has a key of 1 + for (int time = 1; time <= times; ++time) { + lm::FeaturePointsMeasurement measurement; + measurement.timestamp = time; + const vc::FeaturePoint p(0, 0, time, track_id, time); + measurement.feature_points.emplace_back(p); + measurements.emplace_back(measurement); + gtsam::Rot3 rotation = gtsam::Rot3::identity(); + gtsam::Point3 translation(time, time, 0); + // Make middle measurement invalid. + // Turn the camera in the opposite direction so point can't be triangulated + if (time == 3) { + rotation = gtsam::Rot3::Ypr(M_PI / 2, 0., -M_PI / 2); + } + const gtsam::Pose3 pose(rotation, translation); + // Add node here since not actually added in simple node adder + const auto key = node_adder_->values().Add(pose); + added_keys.emplace_back(key); + } + + factor_adder_->AddMeasurement(measurements[0]); + factor_adder_->AddMeasurement(measurements[1]); + factor_adder_->AddMeasurement(measurements[2]); + factor_adder_->AddMeasurement(measurements[3]); + // Valid factor since with removed measurement should stil have two points. + EXPECT_EQ(factor_adder_->AddFactors(1, 4, factors_), 1); + // Middle measurement should be removed. + // Keys 1,2,4 should match to measurements 0,1,3 + EXPECT_SAME_FACTOR(0, {1, 2, 4}, measurements, {0, 1, 3}); +} + +TEST_F(VoSmartProjectionFactorAdderTest, InvalidLastTwoMeasurementsFactor) { + auto params = DefaultParams(); + params.fix_invalid_factors = true; + params.min_avg_distance_from_mean = 0; + params.max_num_points_per_factor = 5; + Initialize(params); + const int track_id = 0; + const int times = 5; + // Add same measurement at different timestamps + // Populate graph values with valid poses with translation spread + std::vector<lm::FeaturePointsMeasurement> measurements; + gtsam::KeyVector added_keys; + // Start with time 1 since first node in nodes has a key of 1 + for (int time = 1; time <= times; ++time) { + lm::FeaturePointsMeasurement measurement; + measurement.timestamp = time; + const vc::FeaturePoint p(0, 0, time, track_id, time); + measurement.feature_points.emplace_back(p); + measurements.emplace_back(measurement); + gtsam::Rot3 rotation = gtsam::Rot3::identity(); + gtsam::Point3 translation(time, time, 0); + // Make last two measurements invalid. + // Turn the camera in the opposite direction so point can't be triangulated + if (time >= 4) { + rotation = gtsam::Rot3::Ypr(M_PI / 2, 0., -M_PI / 2); + } + const gtsam::Pose3 pose(rotation, translation); + // Add node here since not actually added in simple node adder + const auto key = node_adder_->values().Add(pose); + added_keys.emplace_back(key); + } + + factor_adder_->AddMeasurement(measurements[0]); + factor_adder_->AddMeasurement(measurements[1]); + factor_adder_->AddMeasurement(measurements[2]); + factor_adder_->AddMeasurement(measurements[3]); + factor_adder_->AddMeasurement(measurements[4]); + // Valid factor since with removed measurement should still have two points. + EXPECT_EQ(factor_adder_->AddFactors(1, 5, factors_), 1); + // Middle measurement should be removed. + // Keys 1,2,3 should match to measurements 0,1,2 + EXPECT_SAME_FACTOR(0, {1, 2, 3}, measurements, {0, 1, 2}); +} + +TEST_F(VoSmartProjectionFactorAdderTest, InvalidFirstTwoMeasurementsFactor) { + auto params = DefaultParams(); + params.fix_invalid_factors = true; + params.min_avg_distance_from_mean = 0; + params.max_num_points_per_factor = 5; + Initialize(params); + const int track_id = 0; + const int times = 5; + // Add same measurement at different timestamps + // Populate graph values with valid poses with translation spread + std::vector<lm::FeaturePointsMeasurement> measurements; + gtsam::KeyVector added_keys; + // Start with time 1 since first node in nodes has a key of 1 + for (int time = 1; time <= times; ++time) { + lm::FeaturePointsMeasurement measurement; + measurement.timestamp = time; + const vc::FeaturePoint p(0, 0, time, track_id, time); + measurement.feature_points.emplace_back(p); + measurements.emplace_back(measurement); + gtsam::Rot3 rotation = gtsam::Rot3::identity(); + gtsam::Point3 translation(time, time, 0); + // Make first two measurements invalid. + // Turn the camera in the opposite direction so point can't be triangulated + if (time < 3) { + rotation = gtsam::Rot3::Ypr(M_PI / 2, 0., -M_PI / 2); + } + const gtsam::Pose3 pose(rotation, translation); + // Add node here since not actually added in simple node adder + const auto key = node_adder_->values().Add(pose); + added_keys.emplace_back(key); + } + + factor_adder_->AddMeasurement(measurements[0]); + factor_adder_->AddMeasurement(measurements[1]); + factor_adder_->AddMeasurement(measurements[2]); + factor_adder_->AddMeasurement(measurements[3]); + factor_adder_->AddMeasurement(measurements[4]); + // Valid factor since with removed measurement should still have two points. + EXPECT_EQ(factor_adder_->AddFactors(1, 5, factors_), 1); + // First two measurements should be removed. + // Keys 3,4,5 should match to measurements 2,3,4 + EXPECT_SAME_FACTOR(0, {3, 4, 5}, measurements, {2, 3, 4}); +} + +TEST_F(VoSmartProjectionFactorAdderTest, TooSoonFactors) { + auto params = DefaultParams(); + Initialize(params); + const int max_factors = std::min(params.max_num_factors, num_tracks_); + // Add first measurement + // No factors should be added since there are too few measurements for each factor + factor_adder_->AddMeasurement(measurements_[0]); + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(0), factors_), 0); + // Track: Measurement Timestamps + // 0: 0 + // 1: 0 + // 2: 0 + EXPECT_EQ(factors_.size(), 0); + // Add second measurement + factor_adder_->AddMeasurement(measurements_[1]); + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(1), factors_), max_factors); + // Track: Measurement Timestamps + // 0: 0, 1 + // 1: 0, 1 + // 2: 0, 1 + EXPECT_SAME_FACTOR(0, {0, 1}); + EXPECT_SAME_FACTOR(1, {0, 1}); + EXPECT_EQ(factors_.size(), max_factors); + // Add factor too soon, set node adder latest measurement before factor newest_allowed_time + // so factor can't be created. + factor_adder_->AddMeasurement(measurements_[2]); + node_adder_->latest_measurement_time_ = 1.5; + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(2), factors_), 2); + // Track: Measurement Timestamps + // 0: 0, 1, 2 + // 1: 0, 1, 2 + // 2: 0, 1, 2 + EXPECT_SAME_FACTOR(0, {0, 1}); + EXPECT_SAME_FACTOR(0, {0, 1}); + EXPECT_EQ(factors_.size(), max_factors); + // Change node adder latest measurement time back, make sure measurement for factor + // adder hasn't been removed and latest measurements are still added to factor. + node_adder_->latest_measurement_time_ = 2; + EXPECT_EQ(factor_adder_->AddFactors(timestamp(0), timestamp(2), factors_), 2); + EXPECT_SAME_FACTOR(0, {0, 1, 2}); + EXPECT_SAME_FACTOR(1, {0, 1, 2}); + EXPECT_EQ(factors_.size(), max_factors); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/graph_localizer/test/test_combined_nav_state_node_updater.test b/localization/factor_adders/test/test_vo_smart_projection_factor_adder.test similarity index 91% rename from localization/graph_localizer/test/test_combined_nav_state_node_updater.test rename to localization/factor_adders/test/test_vo_smart_projection_factor_adder.test index 2a05e9ef11..2a4ef70dc0 100644 --- a/localization/graph_localizer/test/test_combined_nav_state_node_updater.test +++ b/localization/factor_adders/test/test_vo_smart_projection_factor_adder.test @@ -16,5 +16,5 @@ <!-- permissions and limitations under the License. --> <launch> - <test pkg="graph_localizer" type="test_combined_nav_state_node_updater" test-name="test_combined_nav_state_node_updater" /> + <test pkg="factor_adders" type="test_vo_smart_projection_factor_adder" test-name="test_vo_smart_projection_factor_adder" /> </launch> diff --git a/localization/graph_factors/CMakeLists.txt b/localization/graph_factors/CMakeLists.txt new file mode 100644 index 0000000000..41c2761245 --- /dev/null +++ b/localization/graph_factors/CMakeLists.txt @@ -0,0 +1,137 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(graph_factors) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + localization_common + localization_measurements + vision_common +) + +# Find GTSAM +find_package(GTSAM REQUIRED) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +# include ff_nodelet to get ff_util header files since these aren't exposed elsewhere +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS localization_common localization_measurements vision_common +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${GLOG_INCLUDE_DIRS} +) + +file(GLOB cc_files + "src/*.cc" +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + ${cc_files} +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_inverse_depth_projection_factor + test/test_inverse_depth_projection_factor.test + test/test_inverse_depth_projection_factor.cc + ) + target_link_libraries(test_inverse_depth_projection_factor + graph_factors ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_point_to_line_factor + test/test_point_to_line_factor.test + test/test_point_to_line_factor.cc + ) + target_link_libraries(test_point_to_line_factor + graph_factors ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_point_to_line_segment_factor + test/test_point_to_line_segment_factor.test + test/test_point_to_line_segment_factor.cc + ) + target_link_libraries(test_point_to_line_segment_factor + graph_factors ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_point_to_plane_factor + test/test_point_to_plane_factor.test + test/test_point_to_plane_factor.cc + ) + target_link_libraries(test_point_to_plane_factor + graph_factors ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_point_to_point_between_factor + test/test_point_to_point_between_factor.test + test/test_point_to_point_between_factor.cc + ) + target_link_libraries(test_point_to_point_between_factor + graph_factors ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_point_to_handrail_endpoint_factor + test/test_point_to_handrail_endpoint_factor.test + test/test_point_to_handrail_endpoint_factor.cc + ) + target_link_libraries(test_point_to_handrail_endpoint_factor + graph_factors ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_silu + test/test_silu.test + test/test_silu.cc + ) + target_link_libraries(test_silu + graph_factors ${catkin_LIBRARIES} + ) +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/graph_factors/include/graph_factors/cumulative_factor.h b/localization/graph_factors/include/graph_factors/cumulative_factor.h new file mode 100644 index 0000000000..6f3c2192dd --- /dev/null +++ b/localization/graph_factors/include/graph_factors/cumulative_factor.h @@ -0,0 +1,40 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_FACTORS_CUMULATIVE_FACTOR_H_ +#define GRAPH_FACTORS_CUMULATIVE_FACTOR_H_ + +#include <gtsam/nonlinear/NonlinearFactor.h> + +#include <unordered_set> + +namespace gtsam { +// Base case for cumulative factors which have a variable-sized set of measurements and keys accumulated +// over time. When using with a sliding window graph, old measurements and keys should +// be removed instead of removing the entire factor. +class CumulativeFactor { + public: + virtual ~CumulativeFactor() = default; + + // Returns a copy of the factor with keys in keys_to_remove removed along with + // any associated measurements. + virtual boost::shared_ptr<NonlinearFactor> PrunedCopy(const std::unordered_set<gtsam::Key>& keys_to_remove) const = 0; +}; +} // namespace gtsam + +#endif // GRAPH_FACTORS_CUMULATIVE_FACTOR_H_ diff --git a/localization/graph_localizer/include/graph_localizer/inverse_depth_projection_factor.h b/localization/graph_factors/include/graph_factors/inverse_depth_projection_factor.h similarity index 97% rename from localization/graph_localizer/include/graph_localizer/inverse_depth_projection_factor.h rename to localization/graph_factors/include/graph_factors/inverse_depth_projection_factor.h index 6cf6cac688..b698c7eae1 100644 --- a/localization/graph_localizer/include/graph_localizer/inverse_depth_projection_factor.h +++ b/localization/graph_factors/include/graph_factors/inverse_depth_projection_factor.h @@ -16,8 +16,8 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_INVERSE_DEPTH_PROJECTION_FACTOR_H_ -#define GRAPH_LOCALIZER_INVERSE_DEPTH_PROJECTION_FACTOR_H_ +#ifndef GRAPH_FACTORS_INVERSE_DEPTH_PROJECTION_FACTOR_H_ +#define GRAPH_FACTORS_INVERSE_DEPTH_PROJECTION_FACTOR_H_ #include <vision_common/inverse_depth_measurement.h> @@ -145,4 +145,4 @@ struct traits<InverseDepthProjectionFactor> : public Testable<InverseDepthProjec } // namespace gtsam -#endif // GRAPH_LOCALIZER_INVERSE_DEPTH_PROJECTION_FACTOR_H_ +#endif // GRAPH_FACTORS_INVERSE_DEPTH_PROJECTION_FACTOR_H_ diff --git a/localization/graph_localizer/include/graph_localizer/loc_pose_factor.h b/localization/graph_factors/include/graph_factors/loc_pose_factor.h similarity index 92% rename from localization/graph_localizer/include/graph_localizer/loc_pose_factor.h rename to localization/graph_factors/include/graph_factors/loc_pose_factor.h index c51d525c2b..c3a578a011 100644 --- a/localization/graph_localizer/include/graph_localizer/loc_pose_factor.h +++ b/localization/graph_factors/include/graph_factors/loc_pose_factor.h @@ -16,8 +16,8 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_LOC_POSE_FACTOR_H_ -#define GRAPH_LOCALIZER_LOC_POSE_FACTOR_H_ +#ifndef GRAPH_FACTORS_LOC_POSE_FACTOR_H_ +#define GRAPH_FACTORS_LOC_POSE_FACTOR_H_ #include <gtsam/geometry/Pose3.h> #include <gtsam/slam/PriorFactor.h> @@ -50,4 +50,4 @@ class LocPoseFactor : public PriorFactor<Pose3> { }; } // namespace gtsam -#endif // GRAPH_LOCALIZER_LOC_POSE_FACTOR_H_ +#endif // GRAPH_FACTORS_LOC_POSE_FACTOR_H_ diff --git a/localization/graph_localizer/include/graph_localizer/loc_projection_factor.h b/localization/graph_factors/include/graph_factors/loc_projection_factor.h similarity index 95% rename from localization/graph_localizer/include/graph_localizer/loc_projection_factor.h rename to localization/graph_factors/include/graph_factors/loc_projection_factor.h index 8590d59324..1f39bee668 100644 --- a/localization/graph_localizer/include/graph_localizer/loc_projection_factor.h +++ b/localization/graph_factors/include/graph_factors/loc_projection_factor.h @@ -16,8 +16,8 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_LOC_PROJECTION_FACTOR_H_ -#define GRAPH_LOCALIZER_LOC_PROJECTION_FACTOR_H_ +#ifndef GRAPH_FACTORS_LOC_PROJECTION_FACTOR_H_ +#define GRAPH_FACTORS_LOC_PROJECTION_FACTOR_H_ #include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Pose3.h> @@ -40,7 +40,6 @@ class LocProjectionFactor : public NoiseModelFactor1<POSE> { // Keep a copy of measurement and calibration for I/O Point2 measured_; ///< 2D measurement LANDMARK landmark_point_; ///< Landmark point - Pose3 world_T_cam_; ///< world_T_cam estimate if available boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame @@ -186,15 +185,11 @@ class LocProjectionFactor : public NoiseModelFactor1<POSE> { return false; } - void setWorldTCam(const gtsam::Pose3& world_T_cam) { world_T_cam_ = world_T_cam; } - /** return the measurement */ const Point2& measured() const { return measured_; } const LANDMARK& landmark_point() const { return landmark_point_; } - const Pose3& world_T_cam() const { return world_T_cam_; } - LANDMARK& landmark_point() { return landmark_point_; } /** return the calibration object */ @@ -216,7 +211,6 @@ class LocProjectionFactor : public NoiseModelFactor1<POSE> { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar& BOOST_SERIALIZATION_NVP(measured_); ar& BOOST_SERIALIZATION_NVP(landmark_point_); - ar& BOOST_SERIALIZATION_NVP(world_T_cam_); ar& BOOST_SERIALIZATION_NVP(K_); ar& BOOST_SERIALIZATION_NVP(body_P_sensor_); ar& BOOST_SERIALIZATION_NVP(throwCheirality_); @@ -234,4 +228,4 @@ struct traits<LocProjectionFactor<POSE, LANDMARK, CALIBRATION>> } // namespace gtsam -#endif // GRAPH_LOCALIZER_LOC_PROJECTION_FACTOR_H_ +#endif // GRAPH_FACTORS_LOC_PROJECTION_FACTOR_H_ diff --git a/localization/graph_localizer/include/graph_localizer/point_to_handrail_endpoint_factor.h b/localization/graph_factors/include/graph_factors/point_to_handrail_endpoint_factor.h similarity index 96% rename from localization/graph_localizer/include/graph_localizer/point_to_handrail_endpoint_factor.h rename to localization/graph_factors/include/graph_factors/point_to_handrail_endpoint_factor.h index 163c2c71c0..c31b0b912b 100644 --- a/localization/graph_localizer/include/graph_localizer/point_to_handrail_endpoint_factor.h +++ b/localization/graph_factors/include/graph_factors/point_to_handrail_endpoint_factor.h @@ -16,8 +16,8 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_POINT_TO_HANDRAIL_ENDPOINT_FACTOR_H_ -#define GRAPH_LOCALIZER_POINT_TO_HANDRAIL_ENDPOINT_FACTOR_H_ +#ifndef GRAPH_FACTORS_POINT_TO_HANDRAIL_ENDPOINT_FACTOR_H_ +#define GRAPH_FACTORS_POINT_TO_HANDRAIL_ENDPOINT_FACTOR_H_ #include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Point3.h> @@ -123,4 +123,4 @@ class PointToHandrailEndpointFactor : public NoiseModelFactor1<Pose3> { }; } // namespace gtsam -#endif // GRAPH_LOCALIZER_POINT_TO_HANDRAIL_ENDPOINT_FACTOR_H_ +#endif // GRAPH_FACTORS_POINT_TO_HANDRAIL_ENDPOINT_FACTOR_H_ diff --git a/localization/graph_localizer/include/graph_localizer/point_to_line_factor.h b/localization/graph_factors/include/graph_factors/point_to_line_factor.h similarity index 91% rename from localization/graph_localizer/include/graph_localizer/point_to_line_factor.h rename to localization/graph_factors/include/graph_factors/point_to_line_factor.h index 48c12c8563..38fd276dbd 100644 --- a/localization/graph_localizer/include/graph_localizer/point_to_line_factor.h +++ b/localization/graph_factors/include/graph_factors/point_to_line_factor.h @@ -16,10 +16,10 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_POINT_TO_LINE_FACTOR_H_ -#define GRAPH_LOCALIZER_POINT_TO_LINE_FACTOR_H_ +#ifndef GRAPH_FACTORS_POINT_TO_LINE_FACTOR_H_ +#define GRAPH_FACTORS_POINT_TO_LINE_FACTOR_H_ -#include <graph_localizer/point_to_line_factor_base.h> +#include <graph_factors/point_to_line_factor_base.h> #include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Point3.h> @@ -63,4 +63,4 @@ class PointToLineFactor : public PointToLineFactorBase { }; } // namespace gtsam -#endif // GRAPH_LOCALIZER_POINT_TO_LINE_FACTOR_H_ +#endif // GRAPH_FACTORS_POINT_TO_LINE_FACTOR_H_ diff --git a/localization/graph_localizer/include/graph_localizer/point_to_line_factor_base.h b/localization/graph_factors/include/graph_factors/point_to_line_factor_base.h similarity index 96% rename from localization/graph_localizer/include/graph_localizer/point_to_line_factor_base.h rename to localization/graph_factors/include/graph_factors/point_to_line_factor_base.h index 5d4949ac17..5e7a929417 100644 --- a/localization/graph_localizer/include/graph_localizer/point_to_line_factor_base.h +++ b/localization/graph_factors/include/graph_factors/point_to_line_factor_base.h @@ -16,8 +16,8 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_POINT_TO_LINE_FACTOR_BASE_H_ -#define GRAPH_LOCALIZER_POINT_TO_LINE_FACTOR_BASE_H_ +#ifndef GRAPH_FACTORS_POINT_TO_LINE_FACTOR_BASE_H_ +#define GRAPH_FACTORS_POINT_TO_LINE_FACTOR_BASE_H_ #include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Point3.h> @@ -112,4 +112,4 @@ class PointToLineFactorBase : public NoiseModelFactor1<gtsam::Pose3> { }; } // namespace gtsam -#endif // GRAPH_LOCALIZER_POINT_TO_LINE_FACTOR_BASE_H_ +#endif // GRAPH_FACTORS_POINT_TO_LINE_FACTOR_BASE_H_ diff --git a/localization/graph_localizer/include/graph_localizer/point_to_line_segment_factor.h b/localization/graph_factors/include/graph_factors/point_to_line_segment_factor.h similarity index 89% rename from localization/graph_localizer/include/graph_localizer/point_to_line_segment_factor.h rename to localization/graph_factors/include/graph_factors/point_to_line_segment_factor.h index 9aff7448cf..5e701805c0 100644 --- a/localization/graph_localizer/include/graph_localizer/point_to_line_segment_factor.h +++ b/localization/graph_factors/include/graph_factors/point_to_line_segment_factor.h @@ -16,11 +16,11 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_POINT_TO_LINE_SEGMENT_FACTOR_H_ -#define GRAPH_LOCALIZER_POINT_TO_LINE_SEGMENT_FACTOR_H_ +#ifndef GRAPH_FACTORS_POINT_TO_LINE_SEGMENT_FACTOR_H_ +#define GRAPH_FACTORS_POINT_TO_LINE_SEGMENT_FACTOR_H_ -#include <graph_localizer/point_to_line_factor_base.h> -#include <graph_localizer/silu.h> +#include <graph_factors/point_to_line_factor_base.h> +#include <graph_factors/silu.h> #include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Point3.h> @@ -78,10 +78,10 @@ class PointToLineSegmentFactor : public PointToLineFactorBase { } else { if (H) { Matrix11 d_silu_d_z; - error.z() = graph_localizer::SiluWithOffsetTwoWay(error.z(), line_length_ / 2.0, d_silu_d_z); + error.z() = graph_factors::SiluWithOffsetTwoWay(error.z(), line_length_ / 2.0, d_silu_d_z); H->block<1, 6>(2, 0) = d_silu_d_z * H->block<1, 6>(2, 0); } - error.z() = graph_localizer::SiluWithOffsetTwoWay(error.z(), line_length_ / 2.0); + error.z() = graph_factors::SiluWithOffsetTwoWay(error.z(), line_length_ / 2.0); } return error; } @@ -103,4 +103,4 @@ class PointToLineSegmentFactor : public PointToLineFactorBase { }; } // namespace gtsam -#endif // GRAPH_LOCALIZER_POINT_TO_LINE_SEGMENT_FACTOR_H_ +#endif // GRAPH_FACTORS_POINT_TO_LINE_SEGMENT_FACTOR_H_ diff --git a/localization/graph_localizer/include/graph_localizer/point_to_plane_factor.h b/localization/graph_factors/include/graph_factors/point_to_plane_factor.h similarity index 96% rename from localization/graph_localizer/include/graph_localizer/point_to_plane_factor.h rename to localization/graph_factors/include/graph_factors/point_to_plane_factor.h index 3996523e31..b54e181743 100644 --- a/localization/graph_localizer/include/graph_localizer/point_to_plane_factor.h +++ b/localization/graph_factors/include/graph_factors/point_to_plane_factor.h @@ -16,8 +16,8 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_POINT_TO_PLANE_FACTOR_H_ -#define GRAPH_LOCALIZER_POINT_TO_PLANE_FACTOR_H_ +#ifndef GRAPH_FACTORS_POINT_TO_PLANE_FACTOR_H_ +#define GRAPH_FACTORS_POINT_TO_PLANE_FACTOR_H_ #include <localization_measurements/plane.h> @@ -104,4 +104,4 @@ class PointToPlaneFactor : public NoiseModelFactor1<Pose3> { }; } // namespace gtsam -#endif // GRAPH_LOCALIZER_POINT_TO_PLANE_FACTOR_H_ +#endif // GRAPH_FACTORS_POINT_TO_PLANE_FACTOR_H_ diff --git a/localization/graph_localizer/include/graph_localizer/point_to_point_between_factor.h b/localization/graph_factors/include/graph_factors/point_to_point_between_factor.h similarity index 95% rename from localization/graph_localizer/include/graph_localizer/point_to_point_between_factor.h rename to localization/graph_factors/include/graph_factors/point_to_point_between_factor.h index c2c6b4d618..13385c969d 100644 --- a/localization/graph_localizer/include/graph_localizer/point_to_point_between_factor.h +++ b/localization/graph_factors/include/graph_factors/point_to_point_between_factor.h @@ -16,8 +16,8 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_POINT_TO_POINT_BETWEEN_FACTOR_H_ -#define GRAPH_LOCALIZER_POINT_TO_POINT_BETWEEN_FACTOR_H_ +#ifndef GRAPH_FACTORS_POINT_TO_POINT_BETWEEN_FACTOR_H_ +#define GRAPH_FACTORS_POINT_TO_POINT_BETWEEN_FACTOR_H_ #include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Point3.h> @@ -89,4 +89,4 @@ class PointToPointBetweenFactor : public NoiseModelFactor2<Pose3, Pose3> { }; } // namespace gtsam -#endif // GRAPH_LOCALIZER_POINT_TO_POINT_BETWEEN_FACTOR_H_ +#endif // GRAPH_FACTORS_POINT_TO_POINT_BETWEEN_FACTOR_H_ diff --git a/localization/graph_localizer/include/graph_localizer/pose_rotation_factor.h b/localization/graph_factors/include/graph_factors/pose_rotation_factor.h similarity index 95% rename from localization/graph_localizer/include/graph_localizer/pose_rotation_factor.h rename to localization/graph_factors/include/graph_factors/pose_rotation_factor.h index 46479cf437..9ef0c9251d 100644 --- a/localization/graph_localizer/include/graph_localizer/pose_rotation_factor.h +++ b/localization/graph_factors/include/graph_factors/pose_rotation_factor.h @@ -16,8 +16,8 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_POSE_ROTATION_FACTOR_H_ -#define GRAPH_LOCALIZER_POSE_ROTATION_FACTOR_H_ +#ifndef GRAPH_FACTORS_POSE_ROTATION_FACTOR_H_ +#define GRAPH_FACTORS_POSE_ROTATION_FACTOR_H_ #include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Rot3.h> @@ -95,4 +95,4 @@ class PoseRotationFactor : public NoiseModelFactor2<gtsam::Pose3, gtsam::Pose3> }; } // namespace gtsam -#endif // GRAPH_LOCALIZER_POSE_ROTATION_FACTOR_H_ +#endif // GRAPH_FACTORS_POSE_ROTATION_FACTOR_H_ diff --git a/localization/graph_localizer/include/graph_localizer/robust_smart_projection_pose_factor.h b/localization/graph_factors/include/graph_factors/robust_smart_projection_pose_factor.h similarity index 88% rename from localization/graph_localizer/include/graph_localizer/robust_smart_projection_pose_factor.h rename to localization/graph_factors/include/graph_factors/robust_smart_projection_pose_factor.h index faf1955887..69ed2b2ed2 100644 --- a/localization/graph_localizer/include/graph_localizer/robust_smart_projection_pose_factor.h +++ b/localization/graph_factors/include/graph_factors/robust_smart_projection_pose_factor.h @@ -16,8 +16,10 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_ROBUST_SMART_PROJECTION_POSE_FACTOR_H_ -#define GRAPH_LOCALIZER_ROBUST_SMART_PROJECTION_POSE_FACTOR_H_ +#ifndef GRAPH_FACTORS_ROBUST_SMART_PROJECTION_POSE_FACTOR_H_ +#define GRAPH_FACTORS_ROBUST_SMART_PROJECTION_POSE_FACTOR_H_ + +#include <graph_factors/cumulative_factor.h> #include <gtsam/slam/SmartProjectionPoseFactor.h> @@ -26,7 +28,7 @@ namespace gtsam { template <class CALIBRATION> -class RobustSmartProjectionPoseFactor : public SmartProjectionPoseFactor<CALIBRATION> { +class RobustSmartProjectionPoseFactor : public SmartProjectionPoseFactor<CALIBRATION>, public CumulativeFactor { typedef PinholePose<CALIBRATION> Camera; typedef SmartFactorBase<Camera> Base; typedef typename Camera::Measurement Z; @@ -49,6 +51,8 @@ class RobustSmartProjectionPoseFactor : public SmartProjectionPoseFactor<CALIBRA const bool rotation_only_fallback = false, const bool robust = true, const double huber_k = 1.0) : SmartProjectionPoseFactor<CALIBRATION>(sharedNoiseModel, K, body_P_sensor, params), + sharedNoiseModel_(sharedNoiseModel), + params_(params), rotation_only_fallback_(rotation_only_fallback), robust_(robust), huber_k_(huber_k) { @@ -60,6 +64,19 @@ class RobustSmartProjectionPoseFactor : public SmartProjectionPoseFactor<CALIBRA triangulation_params_ = params.triangulation; } + // Helper function to return copy of factor with only measurements for allowed keys. + boost::shared_ptr<NonlinearFactor> PrunedCopy(const std::unordered_set<gtsam::Key>& keys_to_remove) const final { + boost::shared_ptr<RobustSmartProjectionPoseFactor> pruned_factor( + new RobustSmartProjectionPoseFactor(sharedNoiseModel_, this->calibration(), this->body_P_sensor(), params_, + rotation_only_fallback_, robust_, huber_k_)); + for (int i = 0; i < this->keys().size(); ++i) { + if (keys_to_remove.count(this->keys()[i]) == 0) pruned_factor->add(this->measured()[i], this->keys()[i]); + } + // If too few measurements, factor is invalid + if (pruned_factor->measured().size() < 2) return nullptr; + return pruned_factor; + } + boost::shared_ptr<GaussianFactor> linearize(const Values& values) const override { typename Base::Cameras cameras = this->cameras(values); // if (!this->triangulateForLinearize(cameras)) return boost::make_shared<JacobianFactorSVD<Dim, 2>>(this->keys()); @@ -233,7 +250,10 @@ class RobustSmartProjectionPoseFactor : public SmartProjectionPoseFactor<CALIBRA double noise_inv_sigma_; // TODO(rsoussan): Remove once result_ serialization bug in gtsam fixed TriangulationParameters triangulation_params_; + // Copies stored here to access for empty measurement copy creation + SharedNoiseModel sharedNoiseModel_; + SmartProjectionParams params_; }; } // namespace gtsam -#endif // GRAPH_LOCALIZER_ROBUST_SMART_PROJECTION_POSE_FACTOR_H_ +#endif // GRAPH_FACTORS_ROBUST_SMART_PROJECTION_POSE_FACTOR_H_ diff --git a/localization/graph_localizer/include/graph_localizer/silu.h b/localization/graph_factors/include/graph_factors/silu.h similarity index 89% rename from localization/graph_localizer/include/graph_localizer/silu.h rename to localization/graph_factors/include/graph_factors/silu.h index c7321ba2be..01fe7cf987 100644 --- a/localization/graph_localizer/include/graph_localizer/silu.h +++ b/localization/graph_factors/include/graph_factors/silu.h @@ -16,12 +16,12 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_SILU_H_ -#define GRAPH_LOCALIZER_SILU_H_ +#ifndef GRAPH_FACTORS_SILU_H_ +#define GRAPH_FACTORS_SILU_H_ #include <gtsam/base/OptionalJacobian.h> -namespace graph_localizer { +namespace graph_factors { // Sigmoid linear unit function double Silu(const double x, gtsam::OptionalJacobian<1, 1> d_silu_d_x = boost::none); @@ -32,6 +32,6 @@ double SiluWithOffset(const double x, const double offset, gtsam::OptionalJacobi // centered at x = 0 rather than a single ramp _/ Note that this is discontinous at x = 0. double SiluWithOffsetTwoWay(const double x, const double offset, gtsam::OptionalJacobian<1, 1> d_silu_d_x = boost::none); -} // namespace graph_localizer +} // namespace graph_factors -#endif // GRAPH_LOCALIZER_SILU_H_ +#endif // GRAPH_FACTORS_SILU_H_ diff --git a/localization/graph_factors/package.xml b/localization/graph_factors/package.xml new file mode 100644 index 0000000000..ac4252ebe6 --- /dev/null +++ b/localization/graph_factors/package.xml @@ -0,0 +1,23 @@ +<package> + <name>graph_factors</name> + <version>1.0.0</version> + <description> + The graph factors package + </description> + <license> + Apache License, Version 2.0 + </license> + <author email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </author> + <maintainer email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </maintainer> + <buildtool_depend>catkin</buildtool_depend> + <build_depend>localization_common</build_depend> + <build_depend>localization_measurements</build_depend> + <build_depend>vision_common</build_depend> + <run_depend>localization_common</run_depend> + <run_depend>localization_measurements</run_depend> + <run_depend>vision_common</run_depend> +</package> diff --git a/localization/graph_factors/readme.md b/localization/graph_factors/readme.md new file mode 100644 index 0000000000..b4627100ca --- /dev/null +++ b/localization/graph_factors/readme.md @@ -0,0 +1,30 @@ +\page graphfactors Graph Factors + +# Package Overview +The graph factors package provides a set of graph factors for use with a GTSAM graph optimizer. See the GraphOptimizer package for more information on their usage. + +## Factors + +### LocPoseFactor +The LocPoseFactor is simply a gtsam::PriorFactor\<gtsam::Pose3\> that enables the differention of a pose prior from a localization map-based image feature factor. + +### LocProjectionFactor +The LocProjectionFactor is almost a direct copy of the gtsam::ProjectionFactor except it does not optimize for the 3D feature point location. + +### PoseRotationFactor +The PoseRotationFactor constrains two gtsam::Pose3 nodes using their relative rotation. + +### PointToHandrailEndpointFactor.h +The PointToHandrailEndpointFactor constrains a gtsam::Pose3 using a handrail endpoint detection in the sensor frame compared with the closest handrail endpoint from a know handrail. + +### PointToLineFactor.h +The PointToLineFactor constrains a gtsam::Pose3 using a point detection in the sensor frame compared with a line in the world frame. + +### PointToLineSegmentFactor.h +The PointToLineSegmentFactor constrains a gtsam::Pose3 using a point detection in the sensor frame compared with a line segment in the world frame. This factor contains a discontinuity in the Jacobian as there is zero error along the line segment axis if the point is between line segment endpoints and non-zero error otherwise. An option to use a SILU (Sigmoid Linear Unit) approximation is provided for this case. + +###PointToPlaneFactor.h +The PointToPlaneFactor constrains a gtsam::Pose3 using a point detection in the sensor frame compared with a plane in the world frame. + +### RobustSmartProjectionFactor +The RobustSmartProjectionFactor adds to the gtsam::SmartProjectionFactor by providing a robust huber kernel. Additionally, it fixes some issues in the SmartProjectionFactor allowing for a rotation-only fallback when using the JacobianSVD option and allows for proper serialization of the factor. diff --git a/localization/graph_localizer/src/silu.cc b/localization/graph_factors/src/silu.cc similarity index 94% rename from localization/graph_localizer/src/silu.cc rename to localization/graph_factors/src/silu.cc index 5fbeca83b9..bb8f03b378 100644 --- a/localization/graph_localizer/src/silu.cc +++ b/localization/graph_factors/src/silu.cc @@ -16,11 +16,11 @@ * under the License. */ -#include <graph_localizer/silu.h> +#include <graph_factors/silu.h> #include <cmath> -namespace graph_localizer { +namespace graph_factors { double Silu(const double x, gtsam::OptionalJacobian<1, 1> d_silu_d_x) { const double silu_x = x / (1.0 + std::exp(-1.0 * x)); if (d_silu_d_x) { @@ -40,4 +40,4 @@ double SiluWithOffsetTwoWay(const double x, const double offset, gtsam::Optional const double silu_inverted_x = SiluWithOffset(inverted_x, offset, d_silu_d_x); return -1.0 * silu_inverted_x; } -} // namespace graph_localizer +} // namespace graph_factors diff --git a/localization/graph_localizer/test/test_inverse_depth_projection_factor.cc b/localization/graph_factors/test/test_inverse_depth_projection_factor.cc similarity index 99% rename from localization/graph_localizer/test/test_inverse_depth_projection_factor.cc rename to localization/graph_factors/test/test_inverse_depth_projection_factor.cc index 047f17a205..9ada1f7a2c 100644 --- a/localization/graph_localizer/test/test_inverse_depth_projection_factor.cc +++ b/localization/graph_factors/test/test_inverse_depth_projection_factor.cc @@ -16,7 +16,7 @@ * under the License. */ -#include <graph_localizer/inverse_depth_projection_factor.h> +#include <graph_factors/inverse_depth_projection_factor.h> #include <localization_common/logger.h> #include <localization_common/test_utilities.h> #include <localization_common/utilities.h> diff --git a/localization/graph_localizer/test/test_inverse_depth_projection_factor.test b/localization/graph_factors/test/test_inverse_depth_projection_factor.test similarity index 91% rename from localization/graph_localizer/test/test_inverse_depth_projection_factor.test rename to localization/graph_factors/test/test_inverse_depth_projection_factor.test index 0278f5a615..ad2a4bf727 100644 --- a/localization/graph_localizer/test/test_inverse_depth_projection_factor.test +++ b/localization/graph_factors/test/test_inverse_depth_projection_factor.test @@ -16,5 +16,5 @@ <!-- permissions and limitations under the License. --> <launch> - <test pkg="graph_localizer" type="test_inverse_depth_projection_factor" test-name="test_inverse_depth_projection_factor" /> + <test pkg="graph_factors" type="test_inverse_depth_projection_factor" test-name="test_inverse_depth_projection_factor" /> </launch> diff --git a/localization/graph_localizer/test/test_point_to_handrail_endpoint_factor.cc b/localization/graph_factors/test/test_point_to_handrail_endpoint_factor.cc similarity index 98% rename from localization/graph_localizer/test/test_point_to_handrail_endpoint_factor.cc rename to localization/graph_factors/test/test_point_to_handrail_endpoint_factor.cc index c026b3a661..bf09a16143 100644 --- a/localization/graph_localizer/test/test_point_to_handrail_endpoint_factor.cc +++ b/localization/graph_factors/test/test_point_to_handrail_endpoint_factor.cc @@ -16,7 +16,7 @@ * under the License. */ -#include <graph_localizer/point_to_handrail_endpoint_factor.h> +#include <graph_factors/point_to_handrail_endpoint_factor.h> #include <localization_common/logger.h> #include <localization_common/test_utilities.h> diff --git a/localization/graph_localizer/test/test_point_to_handrail_endpoint_factor.test b/localization/graph_factors/test/test_point_to_handrail_endpoint_factor.test similarity index 91% rename from localization/graph_localizer/test/test_point_to_handrail_endpoint_factor.test rename to localization/graph_factors/test/test_point_to_handrail_endpoint_factor.test index 08235f5291..5229c12165 100644 --- a/localization/graph_localizer/test/test_point_to_handrail_endpoint_factor.test +++ b/localization/graph_factors/test/test_point_to_handrail_endpoint_factor.test @@ -16,5 +16,5 @@ <!-- permissions and limitations under the License. --> <launch> - <test pkg="graph_localizer" type="test_point_to_handrail_endpoint_factor" test-name="test_point_to_handrail_endpoint_factor" /> + <test pkg="graph_factors" type="test_point_to_handrail_endpoint_factor" test-name="test_point_to_handrail_endpoint_factor" /> </launch> diff --git a/localization/graph_localizer/test/test_point_to_line_factor.cc b/localization/graph_factors/test/test_point_to_line_factor.cc similarity index 99% rename from localization/graph_localizer/test/test_point_to_line_factor.cc rename to localization/graph_factors/test/test_point_to_line_factor.cc index 82fdd219f3..7e76120d1c 100644 --- a/localization/graph_localizer/test/test_point_to_line_factor.cc +++ b/localization/graph_factors/test/test_point_to_line_factor.cc @@ -16,7 +16,7 @@ * under the License. */ -#include <graph_localizer/point_to_line_factor.h> +#include <graph_factors/point_to_line_factor.h> #include <localization_common/logger.h> #include <localization_common/test_utilities.h> diff --git a/localization/graph_localizer/test/test_point_to_line_factor.test b/localization/graph_factors/test/test_point_to_line_factor.test similarity index 92% rename from localization/graph_localizer/test/test_point_to_line_factor.test rename to localization/graph_factors/test/test_point_to_line_factor.test index eb49f31bc6..7a3e23b464 100644 --- a/localization/graph_localizer/test/test_point_to_line_factor.test +++ b/localization/graph_factors/test/test_point_to_line_factor.test @@ -16,5 +16,5 @@ <!-- permissions and limitations under the License. --> <launch> - <test pkg="graph_localizer" type="test_point_to_line_factor" test-name="test_point_to_line_factor" /> + <test pkg="graph_factors" type="test_point_to_line_factor" test-name="test_point_to_line_factor" /> </launch> diff --git a/localization/graph_localizer/test/test_point_to_line_segment_factor.cc b/localization/graph_factors/test/test_point_to_line_segment_factor.cc similarity index 99% rename from localization/graph_localizer/test/test_point_to_line_segment_factor.cc rename to localization/graph_factors/test/test_point_to_line_segment_factor.cc index 5ae810eb29..1922d23341 100644 --- a/localization/graph_localizer/test/test_point_to_line_segment_factor.cc +++ b/localization/graph_factors/test/test_point_to_line_segment_factor.cc @@ -16,7 +16,7 @@ * under the License. */ -#include <graph_localizer/point_to_line_segment_factor.h> +#include <graph_factors/point_to_line_segment_factor.h> #include <localization_common/logger.h> #include <localization_common/test_utilities.h> diff --git a/localization/graph_localizer/test/test_point_to_line_segment_factor.test b/localization/graph_factors/test/test_point_to_line_segment_factor.test similarity index 91% rename from localization/graph_localizer/test/test_point_to_line_segment_factor.test rename to localization/graph_factors/test/test_point_to_line_segment_factor.test index b5c48d63ae..f9e7fb9327 100644 --- a/localization/graph_localizer/test/test_point_to_line_segment_factor.test +++ b/localization/graph_factors/test/test_point_to_line_segment_factor.test @@ -16,5 +16,5 @@ <!-- permissions and limitations under the License. --> <launch> - <test pkg="graph_localizer" type="test_point_to_line_segment_factor" test-name="test_point_to_line_segment_factor" /> + <test pkg="graph_factors" type="test_point_to_line_segment_factor" test-name="test_point_to_line_segment_factor" /> </launch> diff --git a/localization/graph_localizer/test/test_point_to_plane_factor.cc b/localization/graph_factors/test/test_point_to_plane_factor.cc similarity index 86% rename from localization/graph_localizer/test/test_point_to_plane_factor.cc rename to localization/graph_factors/test/test_point_to_plane_factor.cc index a3ddf6c491..63422c1e76 100644 --- a/localization/graph_localizer/test/test_point_to_plane_factor.cc +++ b/localization/graph_factors/test/test_point_to_plane_factor.cc @@ -16,8 +16,7 @@ * under the License. */ -#include <graph_localizer/point_to_plane_factor.h> -#include <graph_localizer/test_utilities.h> +#include <graph_factors/point_to_plane_factor.h> #include <localization_common/logger.h> #include <localization_common/test_utilities.h> #include <localization_measurements/plane.h> @@ -28,14 +27,20 @@ #include <gtest/gtest.h> -namespace gl = graph_localizer; namespace lc = localization_common; namespace lm = localization_measurements; namespace sym = gtsam::symbol_shorthand; + +localization_measurements::Plane RandomPlane() { + gtsam::Point3 point = lc::RandomPoint3d(); + gtsam::Vector3 normal = lc::RandomVector3d().normalized(); + return localization_measurements::Plane(point, normal); +} + TEST(PointToPlaneFactorTester, Jacobian) { for (int i = 0; i < 500; ++i) { const gtsam::Point3 sensor_t_point = lc::RandomPoint3d(); - const lm::Plane world_T_handrail_plane = gl::RandomPlane(); + const lm::Plane world_T_handrail_plane = RandomPlane(); const gtsam::Pose3 body_T_sensor = lc::RandomPose(); const gtsam::Pose3 world_T_body = lc::RandomPose(); const auto noise = gtsam::noiseModel::Unit::Create(1); diff --git a/localization/graph_localizer/test/test_point_to_plane_factor.test b/localization/graph_factors/test/test_point_to_plane_factor.test similarity index 92% rename from localization/graph_localizer/test/test_point_to_plane_factor.test rename to localization/graph_factors/test/test_point_to_plane_factor.test index d706fbd26f..961c1a8276 100644 --- a/localization/graph_localizer/test/test_point_to_plane_factor.test +++ b/localization/graph_factors/test/test_point_to_plane_factor.test @@ -16,5 +16,5 @@ <!-- permissions and limitations under the License. --> <launch> - <test pkg="graph_localizer" type="test_point_to_plane_factor" test-name="test_point_to_plane_factor" /> + <test pkg="graph_factors" type="test_point_to_plane_factor" test-name="test_point_to_plane_factor" /> </launch> diff --git a/localization/graph_localizer/test/test_point_to_point_between_factor.cc b/localization/graph_factors/test/test_point_to_point_between_factor.cc similarity index 97% rename from localization/graph_localizer/test/test_point_to_point_between_factor.cc rename to localization/graph_factors/test/test_point_to_point_between_factor.cc index e43d8347d0..356c548757 100644 --- a/localization/graph_localizer/test/test_point_to_point_between_factor.cc +++ b/localization/graph_factors/test/test_point_to_point_between_factor.cc @@ -16,8 +16,7 @@ * under the License. */ -#include <graph_localizer/point_to_point_between_factor.h> -#include <graph_localizer/test_utilities.h> +#include <graph_factors/point_to_point_between_factor.h> #include <localization_common/logger.h> #include <localization_common/test_utilities.h> #include <localization_common/utilities.h> @@ -28,7 +27,6 @@ #include <gtest/gtest.h> -namespace gl = graph_localizer; namespace lc = localization_common; namespace sym = gtsam::symbol_shorthand; diff --git a/localization/graph_localizer/test/test_point_to_point_between_factor.test b/localization/graph_factors/test/test_point_to_point_between_factor.test similarity index 91% rename from localization/graph_localizer/test/test_point_to_point_between_factor.test rename to localization/graph_factors/test/test_point_to_point_between_factor.test index 375a442c11..caaae6b4f4 100644 --- a/localization/graph_localizer/test/test_point_to_point_between_factor.test +++ b/localization/graph_factors/test/test_point_to_point_between_factor.test @@ -16,5 +16,5 @@ <!-- permissions and limitations under the License. --> <launch> - <test pkg="graph_localizer" type="test_point_to_point_between_factor" test-name="test_point_to_point_between_factor" /> + <test pkg="graph_factors" type="test_point_to_point_between_factor" test-name="test_point_to_point_between_factor" /> </launch> diff --git a/localization/graph_localizer/test/test_rotation_factor.cc b/localization/graph_factors/test/test_rotation_factor.cc similarity index 98% rename from localization/graph_localizer/test/test_rotation_factor.cc rename to localization/graph_factors/test/test_rotation_factor.cc index fefc35f0f0..e67d6c3caf 100644 --- a/localization/graph_localizer/test/test_rotation_factor.cc +++ b/localization/graph_factors/test/test_rotation_factor.cc @@ -16,7 +16,7 @@ * under the License. */ -#include <graph_localizer/pose_rotation_factor.h> +#include <graph_factors/pose_rotation_factor.h> #include <localization_common/logger.h> #include <localization_common/test_utilities.h> diff --git a/localization/graph_localizer/test/test_rotation_factor.test b/localization/graph_factors/test/test_rotation_factor.test similarity index 93% rename from localization/graph_localizer/test/test_rotation_factor.test rename to localization/graph_factors/test/test_rotation_factor.test index 1535f0f22e..78500e974d 100644 --- a/localization/graph_localizer/test/test_rotation_factor.test +++ b/localization/graph_factors/test/test_rotation_factor.test @@ -16,5 +16,5 @@ <!-- permissions and limitations under the License. --> <launch> - <test pkg="graph_localizer" type="test_rotation_factor" test-name="test_rotation_factor" /> + <test pkg="graph_factors" type="test_rotation_factor" test-name="test_rotation_factor" /> </launch> diff --git a/localization/graph_localizer/test/test_silu.cc b/localization/graph_factors/test/test_silu.cc similarity index 78% rename from localization/graph_localizer/test/test_silu.cc rename to localization/graph_factors/test/test_silu.cc index 3bd5332550..1e47aeb513 100644 --- a/localization/graph_localizer/test/test_silu.cc +++ b/localization/graph_factors/test/test_silu.cc @@ -16,7 +16,7 @@ * under the License. */ -#include <graph_localizer/silu.h> +#include <graph_factors/silu.h> #include <localization_common/logger.h> #include <localization_common/test_utilities.h> @@ -24,15 +24,15 @@ #include <gtest/gtest.h> -namespace gl = graph_localizer; +namespace gf = graph_factors; namespace lc = localization_common; TEST(SiluTester, Jacobian) { for (int i = 0; i < 500; ++i) { const double x = lc::RandomDouble(); gtsam::Matrix H; - gl::Silu(x, H); + gf::Silu(x, H); const auto numerical_H = gtsam::numericalDerivative11<double, double>( - boost::function<double(const double)>(boost::bind(&gl::Silu, _1, boost::none)), x); + boost::function<double(const double)>(boost::bind(&gf::Silu, _1, boost::none)), x); ASSERT_TRUE(gtsam::equal_with_abs_tol(numerical_H, H.matrix(), 1e-6)); } } @@ -42,9 +42,9 @@ TEST(SiluWithOffsetTester, Jacobian) { const double x = lc::RandomDouble(); const double offset = lc::RandomPositiveDouble(); gtsam::Matrix H; - gl::SiluWithOffset(x, offset, H); + gf::SiluWithOffset(x, offset, H); const auto numerical_H = gtsam::numericalDerivative21<double, double, double>( - boost::function<double(const double, const double)>(boost::bind(&gl::SiluWithOffset, _1, _2, boost::none)), x, + boost::function<double(const double, const double)>(boost::bind(&gf::SiluWithOffset, _1, _2, boost::none)), x, offset); ASSERT_TRUE(gtsam::equal_with_abs_tol(numerical_H, H.matrix(), 1e-6)); } @@ -53,42 +53,42 @@ TEST(SiluWithOffsetTester, Jacobian) { TEST(SiluWithOffsetTester, XMuchGreaterThanOffset) { const double x = 3001.3; const double offset = 5.1; - const double silu_x = gl::SiluWithOffset(x, offset); + const double silu_x = gf::SiluWithOffset(x, offset); EXPECT_NEAR(x - offset, silu_x, 1e-6); } TEST(SiluWithOffsetTester, XGreaterThanOffset) { const double x = 13.2; const double offset = 5.1; - const double silu_x = gl::SiluWithOffset(x, offset); + const double silu_x = gf::SiluWithOffset(x, offset); EXPECT_NEAR(x - offset, silu_x, 1e-2); } TEST(SiluWithOffsetTester, XSlightlyGreaterThanOffset) { const double x = 5.2; const double offset = 5.1; - const double silu_x = gl::SiluWithOffset(x, offset); + const double silu_x = gf::SiluWithOffset(x, offset); EXPECT_NEAR(x - offset, silu_x, 5e-2); } TEST(SiluWithOffsetTester, XSlightlyLessThanOffset) { const double x = 5.0; const double offset = 5.1; - const double silu_x = gl::SiluWithOffset(x, offset); + const double silu_x = gf::SiluWithOffset(x, offset); EXPECT_NEAR(0.0, silu_x, 5e-2); } TEST(SiluWithOffsetTester, XLessThanOffset) { const double x = 1.0; const double offset = 5.1; - const double silu_x = gl::SiluWithOffset(x, offset); + const double silu_x = gf::SiluWithOffset(x, offset); EXPECT_NEAR(0.0, silu_x, 1e-1); } TEST(SiluWithOffsetTester, XMuchLessThanOffset) { const double x = 3.0; const double offset = 1003.3; - const double silu_x = gl::SiluWithOffset(x, offset); + const double silu_x = gf::SiluWithOffset(x, offset); EXPECT_NEAR(0.0, silu_x, 1e-6); } @@ -97,9 +97,9 @@ TEST(SiluWithOffsetTwoWayTester, Jacobian) { const double x = lc::RandomDouble(); const double offset = lc::RandomPositiveDouble(); gtsam::Matrix H; - gl::SiluWithOffsetTwoWay(x, offset, H); + gf::SiluWithOffsetTwoWay(x, offset, H); const auto numerical_H = gtsam::numericalDerivative21<double, double, double>( - boost::function<double(const double, const double)>(boost::bind(&gl::SiluWithOffsetTwoWay, _1, _2, boost::none)), + boost::function<double(const double, const double)>(boost::bind(&gf::SiluWithOffsetTwoWay, _1, _2, boost::none)), x, offset); ASSERT_TRUE(gtsam::equal_with_abs_tol(numerical_H, H.matrix(), 1e-6)); } @@ -108,77 +108,77 @@ TEST(SiluWithOffsetTwoWayTester, Jacobian) { TEST(SiluWithOffsetTwoWayTester, XMuchGreaterThanOffset) { const double x = 3001.3; const double offset = 5.1; - const double silu_x = gl::SiluWithOffsetTwoWay(x, offset); + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); EXPECT_NEAR(x - offset, silu_x, 1e-6); } TEST(SiluWithOffsetTwoWayTester, XGreaterThanOffset) { const double x = 13.2; const double offset = 5.1; - const double silu_x = gl::SiluWithOffsetTwoWay(x, offset); + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); EXPECT_NEAR(x - offset, silu_x, 1e-2); } TEST(SiluWithOffsetTwoWayTester, XSlightlyGreaterThanOffset) { const double x = 5.2; const double offset = 5.1; - const double silu_x = gl::SiluWithOffsetTwoWay(x, offset); + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); EXPECT_NEAR(x - offset, silu_x, 5e-2); } TEST(SiluWithOffsetTwoWayTester, XSlightlyLessThanOffset) { const double x = 5.0; const double offset = 5.1; - const double silu_x = gl::SiluWithOffsetTwoWay(x, offset); + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); EXPECT_NEAR(0.0, silu_x, 5e-2); } TEST(SiluWithOffsetTwoWayTester, XLessThanOffset) { const double x = 1.0; const double offset = 5.1; - const double silu_x = gl::SiluWithOffsetTwoWay(x, offset); + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); EXPECT_NEAR(0.0, silu_x, 1e-1); } TEST(SiluWithOffsetTwoWayTester, XMuchLessThanOffset) { const double x = 3.0; const double offset = 1003.3; - const double silu_x = gl::SiluWithOffsetTwoWay(x, offset); + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); EXPECT_NEAR(0.0, silu_x, 1e-6); } TEST(SiluWithOffsetTwoWayTester, NegativeXLessThanOffset) { const double x = -13.2; const double offset = 5.1; - const double silu_x = gl::SiluWithOffsetTwoWay(x, offset); + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); EXPECT_NEAR(x + offset, silu_x, 1e-2); } TEST(SiluWithOffsetTwoWayTester, NegativeXSlightlyLessThanOffset) { const double x = -5.2; const double offset = 5.1; - const double silu_x = gl::SiluWithOffsetTwoWay(x, offset); + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); EXPECT_NEAR(x + offset, silu_x, 5e-2); } TEST(SiluWithOffsetTwoWayTester, NegativeXSlightlyGreaterThanOffset) { const double x = -5.0; const double offset = 5.1; - const double silu_x = gl::SiluWithOffsetTwoWay(x, offset); + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); EXPECT_NEAR(0.0, silu_x, 5e-2); } TEST(SiluWithOffsetTwoWayTester, NegativeXGreaterThanOffset) { const double x = -1.0; const double offset = 5.1; - const double silu_x = gl::SiluWithOffsetTwoWay(x, offset); + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); EXPECT_NEAR(0.0, silu_x, 1e-1); } TEST(SiluWithOffsetTwoWayTester, NegativeXMuchGreaterThanOffset) { const double x = -3.0; const double offset = 1003.3; - const double silu_x = gl::SiluWithOffsetTwoWay(x, offset); + const double silu_x = gf::SiluWithOffsetTwoWay(x, offset); EXPECT_NEAR(0.0, silu_x, 1e-6); } diff --git a/localization/graph_localizer/test/test_silu.test b/localization/graph_factors/test/test_silu.test similarity index 94% rename from localization/graph_localizer/test/test_silu.test rename to localization/graph_factors/test/test_silu.test index 4df780b897..c896939e47 100644 --- a/localization/graph_localizer/test/test_silu.test +++ b/localization/graph_factors/test/test_silu.test @@ -16,5 +16,5 @@ <!-- permissions and limitations under the License. --> <launch> - <test pkg="graph_localizer" type="test_silu" test-name="test_silu" /> + <test pkg="graph_factors" type="test_silu" test-name="test_silu" /> </launch> diff --git a/localization/graph_localizer/CMakeLists.txt b/localization/graph_localizer/CMakeLists.txt index 5683213d8c..79376275b8 100644 --- a/localization/graph_localizer/CMakeLists.txt +++ b/localization/graph_localizer/CMakeLists.txt @@ -29,17 +29,12 @@ find_package(Eigen3 REQUIRED) ## Find catkin macros and libraries find_package(catkin2 REQUIRED COMPONENTS - nodelet - camera - config_reader - ff_util - ff_msgs - graph_optimizer - imu_integration + factor_adders + graph_factors localization_common localization_measurements - msg_conversions - vision_common + node_adders + sliding_window_graph_optimizer ) # Find OpenCV @@ -49,17 +44,12 @@ catkin_package( LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} CATKIN_DEPENDS - nodelet - camera - config_reader - ff_util - ff_msgs - graph_optimizer - imu_integration + factor_adders + graph_factors localization_common localization_measurements - msg_conversions - vision_common + node_adders + sliding_window_graph_optimizer ) ########### @@ -83,88 +73,6 @@ add_library(${PROJECT_NAME} add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(test_combined_nav_state_node_updater - test/test_combined_nav_state_node_updater.test - test/test_combined_nav_state_node_updater.cc - ) - target_link_libraries(test_combined_nav_state_node_updater - graph_localizer ${catkin_LIBRARIES} - ) - add_rostest_gtest(test_depth_odometry_factor_adder - test/test_depth_odometry_factor_adder.test - test/test_depth_odometry_factor_adder.cc - ) - target_link_libraries(test_depth_odometry_factor_adder - graph_localizer ${catkin_LIBRARIES} - ) - add_rostest_gtest(test_inverse_depth_projection_factor - test/test_inverse_depth_projection_factor.test - test/test_inverse_depth_projection_factor.cc - ) - target_link_libraries(test_inverse_depth_projection_factor - graph_localizer ${catkin_LIBRARIES} - ) - add_rostest_gtest(test_point_to_line_factor - test/test_point_to_line_factor.test - test/test_point_to_line_factor.cc - ) - target_link_libraries(test_point_to_line_factor - graph_localizer ${catkin_LIBRARIES} - ) - add_rostest_gtest(test_point_to_line_segment_factor - test/test_point_to_line_segment_factor.test - test/test_point_to_line_segment_factor.cc - ) - target_link_libraries(test_point_to_line_segment_factor - graph_localizer ${catkin_LIBRARIES} - ) - add_rostest_gtest(test_point_to_plane_factor - test/test_point_to_plane_factor.test - test/test_point_to_plane_factor.cc - ) - target_link_libraries(test_point_to_plane_factor - graph_localizer ${catkin_LIBRARIES} - ) - add_rostest_gtest(test_point_to_point_between_factor - test/test_point_to_point_between_factor.test - test/test_point_to_point_between_factor.cc - ) - target_link_libraries(test_point_to_point_between_factor - graph_localizer ${catkin_LIBRARIES} - ) - add_rostest_gtest(test_point_to_handrail_endpoint_factor - test/test_point_to_handrail_endpoint_factor.test - test/test_point_to_handrail_endpoint_factor.cc - ) - target_link_libraries(test_point_to_handrail_endpoint_factor - graph_localizer ${catkin_LIBRARIES} - ) - add_rostest_gtest(test_rotation_factor_adder - test/test_rotation_factor_adder.test - test/test_rotation_factor_adder.cc - ) - target_link_libraries(test_rotation_factor_adder - graph_localizer ${catkin_LIBRARIES} - ) - add_rostest_gtest(test_rotation_factor - test/test_rotation_factor.test - test/test_rotation_factor.cc - ) - target_link_libraries(test_rotation_factor - graph_localizer ${catkin_LIBRARIES} - ) - add_rostest_gtest(test_silu - test/test_silu.test - test/test_silu.cc - ) - target_link_libraries(test_silu - graph_localizer ${catkin_LIBRARIES} - ) -endif() - ############# ## Install ## ############# @@ -176,20 +84,9 @@ install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) -# Mark nodelet_plugin for installation -install(FILES nodelet_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - # Mark cpp header files for installation install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE ) - -# Mark launch files for installation -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - PATTERN ".svn" EXCLUDE) - diff --git a/localization/graph_localizer/include/graph_localizer/combined_nav_state_graph_values.h b/localization/graph_localizer/include/graph_localizer/combined_nav_state_graph_values.h deleted file mode 100644 index f8a41482d6..0000000000 --- a/localization/graph_localizer/include/graph_localizer/combined_nav_state_graph_values.h +++ /dev/null @@ -1,139 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_COMBINED_NAV_STATE_GRAPH_VALUES_H_ -#define GRAPH_LOCALIZER_COMBINED_NAV_STATE_GRAPH_VALUES_H_ - -#include <graph_optimizer/graph_values.h> -#include <graph_localizer/combined_nav_state_graph_values_params.h> -#include <localization_common/combined_nav_state.h> -#include <localization_common/logger.h> -#include <localization_common/time.h> -#include <localization_measurements/feature_point.h> - -#include <gtsam/inference/Symbol.h> -#include <gtsam/nonlinear/NonlinearFactorGraph.h> -#include <gtsam/nonlinear/Values.h> - -#include <boost/optional.hpp> -#include <boost/serialization/unordered_map.hpp> - -#include <map> -#include <unordered_map> -#include <utility> -#include <vector> - -namespace graph_localizer { -namespace sym = gtsam::symbol_shorthand; -class CombinedNavStateGraphValues : public graph_optimizer::GraphValues { - public: - CombinedNavStateGraphValues( - const CombinedNavStateGraphValuesParams& params = CombinedNavStateGraphValuesParams(), - std::shared_ptr<gtsam::Values> values = std::shared_ptr<gtsam::Values>(new gtsam::Values())); - - // Add timestamp and keys to timestamp_key_index_map, and values to values - bool AddCombinedNavState(const localization_common::CombinedNavState& combined_nav_state, const int key_index); - - boost::optional<localization_common::CombinedNavState> LatestCombinedNavState() const; - - boost::optional<localization_common::CombinedNavState> OldestCombinedNavState() const; - - boost::optional<int> OldestCombinedNavStateKeyIndex() const; - - boost::optional<int> LatestCombinedNavStateKeyIndex() const; - - boost::optional<std::pair<gtsam::imuBias::ConstantBias, localization_common::Time>> LatestBias() const; - - // Returns the oldest time that will be in graph values once the window is slid using params - boost::optional<localization_common::Time> SlideWindowNewOldestTime() const final; - - boost::optional<int> KeyIndex(const localization_common::Time timestamp) const; - - int RemoveOldCombinedNavStates(const localization_common::Time oldest_allowed_time); - - gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time, - const gtsam::NonlinearFactorGraph& graph) const final; - - boost::optional<gtsam::Key> PoseKey(const localization_common::Time timestamp) const; - - boost::optional<gtsam::Key> GetKey(graph_optimizer::KeyCreatorFunction key_creator_function, - const localization_common::Time timestamp) const final; - - boost::optional<localization_common::Time> OldestTimestamp() const final; - - boost::optional<localization_common::Time> LatestTimestamp() const final; - - boost::optional<localization_common::Time> ClosestPoseTimestamp(const localization_common::Time timestamp) const; - - // Assumes timestamp is within bounds of graph values timestamps. - std::pair<boost::optional<localization_common::Time>, boost::optional<localization_common::Time>> - LowerAndUpperBoundTimestamp(const localization_common::Time timestamp) const; - - boost::optional<localization_common::CombinedNavState> LowerBoundOrEqualCombinedNavState( - const localization_common::Time timestamp) const; - - bool HasKey(const localization_common::Time timestamp) const; - - template <class FACTOR> - static bool ContainsCombinedNavStateKey(const FACTOR& factor, const int key_index) { - if (factor.find(sym::P(key_index)) != factor.end()) return true; - if (factor.find(sym::V(key_index)) != factor.end()) return true; - if (factor.find(sym::B(key_index)) != factor.end()) return true; - return false; - } - - boost::optional<localization_common::CombinedNavState> GetCombinedNavState( - const localization_common::Time timestamp) const; - - double Duration() const; - - int NumStates() const; - - boost::optional<localization_common::Time> Timestamp(const int key_index) const; - - const CombinedNavStateGraphValuesParams& params() const; - - std::vector<localization_common::Time> Timestamps() const; - - boost::optional<localization_common::Time> Timestamp(graph_optimizer::KeyCreatorFunction key_creator_function, - const gtsam::Key key) const; - - private: - // Removes keys from timestamp_key_index_map, values from values - bool RemoveCombinedNavState(const localization_common::Time timestamp); - - bool Empty() const; - - boost::optional<localization_common::Time> LowerBoundOrEqualTimestamp( - const localization_common::Time timestamp) const; - - // Serialization function - friend class boost::serialization::access; - template <class ARCHIVE> - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(graph_optimizer::GraphValues); - ar& BOOST_SERIALIZATION_NVP(timestamp_key_index_map_); - ar& BOOST_SERIALIZATION_NVP(params_); - } - - CombinedNavStateGraphValuesParams params_; - std::map<localization_common::Time, int> timestamp_key_index_map_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_COMBINED_NAV_STATE_GRAPH_VALUES_H_ diff --git a/localization/graph_localizer/include/graph_localizer/combined_nav_state_node_updater.h b/localization/graph_localizer/include/graph_localizer/combined_nav_state_node_updater.h deleted file mode 100644 index 971eab604c..0000000000 --- a/localization/graph_localizer/include/graph_localizer/combined_nav_state_node_updater.h +++ /dev/null @@ -1,111 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_COMBINED_NAV_STATE_NODE_UPDATER_H_ -#define GRAPH_LOCALIZER_COMBINED_NAV_STATE_NODE_UPDATER_H_ - -#include <graph_localizer/combined_nav_state_graph_values.h> -#include <graph_localizer/combined_nav_state_node_updater_params.h> -#include <graph_optimizer/key_info.h> -#include <graph_optimizer/node_updater_with_priors.h> -#include <imu_integration/latest_imu_integrator.h> -#include <localization_common/combined_nav_state.h> - -namespace graph_localizer { -class CombinedNavStateNodeUpdater - : public graph_optimizer::NodeUpdaterWithPriors<localization_common::CombinedNavState, - localization_common::CombinedNavStateNoise> { - public: - CombinedNavStateNodeUpdater(const CombinedNavStateNodeUpdaterParams& params, - std::shared_ptr<imu_integration::LatestImuIntegrator> latest_imu_integrator, - std::shared_ptr<gtsam::Values> values); - CombinedNavStateNodeUpdater() = default; - - void AddInitialValuesAndPriors(gtsam::NonlinearFactorGraph& factors); - - void AddInitialValuesAndPriors(const localization_common::CombinedNavState& global_N_body, - const localization_common::CombinedNavStateNoise& noise, - gtsam::NonlinearFactorGraph& factors) final; - - void AddPriors(const localization_common::CombinedNavState& global_N_body, - const localization_common::CombinedNavStateNoise& noise, gtsam::NonlinearFactorGraph& factors) final; - - bool Update(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final; - - bool SlideWindow(const localization_common::Time oldest_allowed_timestamp, - const boost::optional<gtsam::Marginals>& marginals, const gtsam::KeyVector& old_keys, - const double huber_k, gtsam::NonlinearFactorGraph& factors) final; - - void ThresholdBiasUncertainty(gtsam::Matrix& bias_covariance) const; - - graph_optimizer::NodeUpdaterType type() const final; - - boost::optional<localization_common::Time> SlideWindowNewOldestTime() const final; - - gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time, - const gtsam::NonlinearFactorGraph& graph) const final; - - boost::optional<gtsam::Key> GetKey(graph_optimizer::KeyCreatorFunction key_creator_function, - const localization_common::Time timestamp) const final; - - boost::optional<localization_common::Time> OldestTimestamp() const final; - - boost::optional<localization_common::Time> LatestTimestamp() const final; - - std::shared_ptr<const CombinedNavStateGraphValues> shared_graph_values() const; - - std::shared_ptr<CombinedNavStateGraphValues> shared_graph_values(); - - const CombinedNavStateGraphValues& graph_values() const; - - private: - void RemovePriors(const int key_index, gtsam::NonlinearFactorGraph& factors); - int GenerateKeyIndex(); - bool AddOrSplitImuFactorIfNeeded(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors, - CombinedNavStateGraphValues& graph_values); - bool CreateAndAddLatestImuFactorAndCombinedNavState(const localization_common::Time timestamp, - gtsam::NonlinearFactorGraph& factors, - CombinedNavStateGraphValues& graph_values); - bool CreateAndAddImuFactorAndPredictedCombinedNavState(const localization_common::CombinedNavState& global_N_body, - const gtsam::PreintegratedCombinedMeasurements& pim, - gtsam::NonlinearFactorGraph& factors, - CombinedNavStateGraphValues& graph_values); - bool SplitOldImuFactorAndAddCombinedNavState(const localization_common::Time timestamp, - gtsam::NonlinearFactorGraph& factors, - CombinedNavStateGraphValues& graph_values); - - // Serialization function - friend class boost::serialization::access; - template <class Archive> - void serialize(Archive& ar, const unsigned int file_version) { - ar& BOOST_SERIALIZATION_NVP(params_); - // ar& BOOST_SERIALIZATION_NVP(latest_imu_integrator_); - ar& BOOST_SERIALIZATION_NVP(graph_values_); - ar& BOOST_SERIALIZATION_NVP(key_index_); - ar& BOOST_SERIALIZATION_NVP(global_N_body_start_noise_); - } - - CombinedNavStateNodeUpdaterParams params_; - std::shared_ptr<imu_integration::LatestImuIntegrator> latest_imu_integrator_; - std::shared_ptr<CombinedNavStateGraphValues> graph_values_; - int key_index_; - localization_common::CombinedNavStateNoise global_N_body_start_noise_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_COMBINED_NAV_STATE_NODE_UPDATER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/combined_nav_state_node_updater_params.h b/localization/graph_localizer/include/graph_localizer/combined_nav_state_node_updater_params.h deleted file mode 100644 index 6d0915b362..0000000000 --- a/localization/graph_localizer/include/graph_localizer/combined_nav_state_node_updater_params.h +++ /dev/null @@ -1,62 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#ifndef GRAPH_LOCALIZER_COMBINED_NAV_STATE_NODE_UPDATER_PARAMS_H_ -#define GRAPH_LOCALIZER_COMBINED_NAV_STATE_NODE_UPDATER_PARAMS_H_ - -#include <graph_localizer/combined_nav_state_graph_values_params.h> -#include <localization_common/combined_nav_state.h> - -#include <boost/serialization/serialization.hpp> - -namespace graph_localizer { -struct CombinedNavStateNodeUpdaterParams { - double starting_prior_translation_stddev; - double starting_prior_quaternion_stddev; - double starting_prior_velocity_stddev; - double starting_prior_accel_bias_stddev; - double starting_prior_gyro_bias_stddev; - double huber_k; - localization_common::CombinedNavState global_N_body_start; - bool add_priors; - CombinedNavStateGraphValuesParams graph_values; - bool threshold_bias_uncertainty; - double accel_bias_stddev_threshold; - double gyro_bias_stddev_threshold; - - private: - // Serialization function - friend class boost::serialization::access; - template <class ARCHIVE> - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_NVP(starting_prior_translation_stddev); - ar& BOOST_SERIALIZATION_NVP(starting_prior_quaternion_stddev); - ar& BOOST_SERIALIZATION_NVP(starting_prior_velocity_stddev); - ar& BOOST_SERIALIZATION_NVP(starting_prior_accel_bias_stddev); - ar& BOOST_SERIALIZATION_NVP(starting_prior_gyro_bias_stddev); - ar& BOOST_SERIALIZATION_NVP(huber_k); - ar& BOOST_SERIALIZATION_NVP(global_N_body_start); - ar& BOOST_SERIALIZATION_NVP(add_priors); - ar& BOOST_SERIALIZATION_NVP(graph_values); - ar& BOOST_SERIALIZATION_NVP(threshold_bias_uncertainty); - ar& BOOST_SERIALIZATION_NVP(accel_bias_stddev_threshold); - ar& BOOST_SERIALIZATION_NVP(gyro_bias_stddev_threshold); - } -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_COMBINED_NAV_STATE_NODE_UPDATER_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/depth_odometry_factor_adder.h b/localization/graph_localizer/include/graph_localizer/depth_odometry_factor_adder.h deleted file mode 100644 index bb6d896930..0000000000 --- a/localization/graph_localizer/include/graph_localizer/depth_odometry_factor_adder.h +++ /dev/null @@ -1,43 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_DEPTH_ODOMETRY_FACTOR_ADDER_H_ -#define GRAPH_LOCALIZER_DEPTH_ODOMETRY_FACTOR_ADDER_H_ - -#include <graph_localizer/depth_odometry_factor_adder_params.h> -#include <graph_optimizer/factor_adder.h> -#include <localization_measurements/depth_odometry_measurement.h> - -#include <vector> - -namespace graph_localizer { -class DepthOdometryFactorAdder - : public graph_optimizer::FactorAdder<localization_measurements::DepthOdometryMeasurement, - DepthOdometryFactorAdderParams> { - using Base = - graph_optimizer::FactorAdder<localization_measurements::DepthOdometryMeasurement, DepthOdometryFactorAdderParams>; - - public: - explicit DepthOdometryFactorAdder(const DepthOdometryFactorAdderParams& params); - - std::vector<graph_optimizer::FactorsToAdd> AddFactors( - const localization_measurements::DepthOdometryMeasurement& depth_odometry_measurement) final; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_DEPTH_ODOMETRY_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/factor_params.h b/localization/graph_localizer/include/graph_localizer/factor_params.h deleted file mode 100644 index c432d22de9..0000000000 --- a/localization/graph_localizer/include/graph_localizer/factor_params.h +++ /dev/null @@ -1,42 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#ifndef GRAPH_LOCALIZER_FACTOR_PARAMS_H_ -#define GRAPH_LOCALIZER_FACTOR_PARAMS_H_ - -#include <graph_localizer/depth_odometry_factor_adder_params.h> -#include <graph_localizer/handrail_factor_adder_params.h> -#include <graph_localizer/loc_factor_adder_params.h> -#include <graph_localizer/rotation_factor_adder_params.h> -#include <graph_localizer/projection_factor_adder_params.h> -#include <graph_localizer/smart_projection_factor_adder_params.h> -#include <graph_localizer/standstill_factor_adder_params.h> - -namespace graph_localizer { -struct FactorParams { - DepthOdometryFactorAdderParams depth_odometry_adder; - HandrailFactorAdderParams handrail_adder; - RotationFactorAdderParams rotation_adder; - SmartProjectionFactorAdderParams smart_projection_adder; - StandstillFactorAdderParams standstill_adder; - ProjectionFactorAdderParams projection_adder; - LocFactorAdderParams loc_adder; - LocFactorAdderParams ar_tag_loc_adder; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_FACTOR_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/feature_point_graph_values.h b/localization/graph_localizer/include/graph_localizer/feature_point_graph_values.h deleted file mode 100644 index 86eb6af8a7..0000000000 --- a/localization/graph_localizer/include/graph_localizer/feature_point_graph_values.h +++ /dev/null @@ -1,93 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_FEATURE_POINT_GRAPH_VALUES_H_ -#define GRAPH_LOCALIZER_FEATURE_POINT_GRAPH_VALUES_H_ - -#include <graph_optimizer/graph_values.h> -#include <localization_common/logger.h> -#include <localization_common/time.h> -#include <localization_measurements/feature_point.h> - -#include <gtsam/geometry/Point3.h> -#include <gtsam/inference/Symbol.h> -#include <gtsam/nonlinear/NonlinearFactorGraph.h> -#include <gtsam/nonlinear/Values.h> - -#include <boost/optional.hpp> -#include <boost/serialization/unordered_map.hpp> - -#include <map> -#include <unordered_map> -#include <utility> - -namespace graph_localizer { -namespace sym = gtsam::symbol_shorthand; -// TODO(rsoussan): Make seperate base class for static graph values so don't have to return boost::none -// for so many virtual fcns -class FeaturePointGraphValues : public graph_optimizer::GraphValues { - public: - FeaturePointGraphValues(std::shared_ptr<gtsam::Values> values = std::shared_ptr<gtsam::Values>(new gtsam::Values())); - - // Returns the oldest time that will be in graph values once the window is slid using params - boost::optional<localization_common::Time> SlideWindowNewOldestTime() const final; - - gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time, - const gtsam::NonlinearFactorGraph& graph) const final; - - boost::optional<gtsam::Key> GetKey(graph_optimizer::KeyCreatorFunction key_creator_function, - const localization_common::Time timestamp) const final; - - boost::optional<localization_common::Time> OldestTimestamp() const final; - - boost::optional<localization_common::Time> LatestTimestamp() const final; - - gtsam::KeyVector OldFeatureKeys(const gtsam::NonlinearFactorGraph& factors) const; - - void RemoveOldFeatures(const gtsam::KeyVector& old_feature_keys); - - gtsam::KeyVector FeatureKeys() const; - - int NumFeatures() const; - - bool AddFeature(const localization_measurements::FeatureId id, const gtsam::Point3& feature_point, - const gtsam::Key& key); - - bool HasFeature(const localization_measurements::FeatureId id) const; - - boost::optional<gtsam::Key> FeatureKey(const localization_measurements::FeatureId id) const; - - // TODO(rsoussan): This shouldn't be const, modify when changes are made to projection factor adder - gtsam::Key CreateFeatureKey() const; - - private: - // Serialization function - friend class boost::serialization::access; - template <class ARCHIVE> - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_NVP(feature_id_key_map_); - ar& BOOST_SERIALIZATION_NVP(feature_key_index_); - } - - std::unordered_map<localization_measurements::FeatureId, gtsam::Key> feature_id_key_map_; - // Modified by projection_factor_adder, remove mutable if this changes - mutable std::uint64_t feature_key_index_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_FEATURE_POINT_GRAPH_VALUES_H_ diff --git a/localization/graph_localizer/include/graph_localizer/feature_track.h b/localization/graph_localizer/include/graph_localizer/feature_track.h deleted file mode 100644 index 55f8ca33d7..0000000000 --- a/localization/graph_localizer/include/graph_localizer/feature_track.h +++ /dev/null @@ -1,68 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_FEATURE_TRACK_H_ -#define GRAPH_LOCALIZER_FEATURE_TRACK_H_ - -#include <localization_measurements/feature_point.h> - -#include <map> -#include <set> -#include <vector> - -namespace graph_localizer { -class FeatureTrack { - public: - using Points = std::map<localization_common::Time, localization_measurements::FeaturePoint>; - explicit FeatureTrack(const localization_measurements::FeatureId id); - FeatureTrack() {} - void AddMeasurement(const localization_common::Time timestamp, - const localization_measurements::FeaturePoint& feature_point); - void RemoveOldMeasurements(const localization_common::Time oldest_allowed_timestamp); - bool HasMeasurement(const localization_common::Time timestamp); - const Points& points() const; - const localization_measurements::FeatureId& id() const; - size_t size() const; - bool empty() const; - std::vector<localization_measurements::FeaturePoint> AllowedPoints( - const std::set<localization_common::Time>& allowed_timestamps) const; - std::vector<localization_measurements::FeaturePoint> LatestPointsInWindow(const double duration) const; - std::vector<localization_measurements::FeaturePoint> LatestPoints(const int spacing = 0) const; - bool SpacingFits(const int spacing, const int max_num_points) const; - int MaxSpacing(const int max_num_points) const; - int ClosestSpacing(const int ideal_spacing, const int ideal_max_num_points) const; - boost::optional<localization_measurements::FeaturePoint> LatestPoint() const; - boost::optional<localization_common::Time> PreviousTimestamp() const; - boost::optional<localization_common::Time> LatestTimestamp() const; - boost::optional<localization_common::Time> OldestTimestamp() const; - - private: - // Serialization function - friend class boost::serialization::access; - template <class ARCHIVE> - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_NVP(id_); - ar& BOOST_SERIALIZATION_NVP(points_); - } - - localization_measurements::FeatureId id_; - Points points_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_FEATURE_TRACK_H_ diff --git a/localization/graph_localizer/include/graph_localizer/feature_tracker.h b/localization/graph_localizer/include/graph_localizer/feature_tracker.h deleted file mode 100644 index 44c6af1f53..0000000000 --- a/localization/graph_localizer/include/graph_localizer/feature_tracker.h +++ /dev/null @@ -1,78 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_FEATURE_TRACKER_H_ -#define GRAPH_LOCALIZER_FEATURE_TRACKER_H_ - -#include <graph_localizer/feature_track.h> -#include <graph_localizer/feature_tracker_params.h> -#include <localization_common/time.h> -#include <localization_measurements/feature_point.h> - -#include <gtsam/geometry/Point2.h> - -#include <deque> -#include <map> -#include <set> - -namespace graph_localizer { -using FeatureTrackIdMap = std::map<localization_measurements::FeatureId, std::shared_ptr<FeatureTrack>>; -using FeatureTrackLengthMap = std::multimap<int, std::shared_ptr<FeatureTrack>>; -class FeatureTracker { - public: - explicit FeatureTracker(const FeatureTrackerParams& params = FeatureTrackerParams()); - // Update existing tracks and add new tracks. Remove tracks without - // detections. - void UpdateFeatureTracks(const localization_measurements::FeaturePoints& feature_points); - const FeatureTrackIdMap& feature_tracks() const; - const std::set<localization_common::Time>& smart_factor_timestamp_allow_list() const; - const FeatureTrackLengthMap& feature_tracks_length_ordered() const; - int NumTracksWithAtLeastNPoints(int n) const; - void RemoveUndetectedFeatures(const localization_common::Time& feature_point); - void RemoveOldFeaturePointsAndSlideWindow( - boost::optional<localization_common::Time> oldest_allowed_time = boost::none); - void AddOrUpdateTrack(const localization_measurements::FeaturePoint& feature_point); - void UpdateLengthMap(); - void UpdateAllowList(const localization_common::Time& timestamp); - void SlideAllowList(const localization_common::Time& oldest_allowed_time); - boost::optional<const FeatureTrack&> LongestFeatureTrack() const; - size_t size() const; - bool empty() const; - void Clear(); - boost::optional<localization_common::Time> OldestTimestamp() const; - boost::optional<localization_common::Time> LatestTimestamp() const; - boost::optional<localization_common::Time> PreviousTimestamp() const; - - private: - // Serialization function - friend class boost::serialization::access; - template <class ARCHIVE> - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_NVP(feature_track_id_map_); - ar& BOOST_SERIALIZATION_NVP(feature_track_length_map_); - } - - FeatureTrackIdMap feature_track_id_map_; - FeatureTrackLengthMap feature_track_length_map_; - FeatureTrackerParams params_; - // TODO(rsoussan): Move ths somewhere else? - std::set<localization_common::Time> smart_factor_timestamp_allow_list_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_FEATURE_TRACKER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer.h b/localization/graph_localizer/include/graph_localizer/graph_localizer.h index 39131f104a..a9c58e05b2 100644 --- a/localization/graph_localizer/include/graph_localizer/graph_localizer.h +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer.h @@ -19,168 +19,69 @@ #ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_H_ #define GRAPH_LOCALIZER_GRAPH_LOCALIZER_H_ -#include <graph_localizer/combined_nav_state_node_updater.h> -#include <graph_localizer/combined_nav_state_node_updater_params.h> -#include <graph_localizer/feature_tracker.h> -#include <graph_localizer/feature_point_node_updater.h> -#include <graph_localizer/depth_odometry_factor_adder.h> -#include <graph_localizer/handrail_factor_adder.h> +#include <factor_adders/loc_factor_adder.h> #include <graph_localizer/graph_localizer_params.h> -#include <graph_localizer/graph_localizer_stats.h> -#include <graph_localizer/robust_smart_projection_pose_factor.h> -#include <graph_localizer/loc_factor_adder.h> -#include <graph_localizer/loc_graph_action_completer.h> -#include <graph_localizer/projection_graph_action_completer.h> -#include <graph_localizer/projection_factor_adder.h> -#include <graph_localizer/rotation_factor_adder.h> -#include <graph_localizer/smart_projection_cumulative_factor_adder.h> -#include <graph_localizer/smart_projection_graph_action_completer.h> -#include <graph_localizer/standstill_factor_adder.h> -#include <graph_optimizer/graph_optimizer.h> -#include <imu_integration/latest_imu_integrator.h> -#include <localization_common/combined_nav_state.h> -#include <localization_common/combined_nav_state_covariances.h> -#include <localization_common/time.h> -#include <localization_measurements/depth_odometry_measurement.h> -#include <localization_measurements/fan_speed_mode.h> -#include <localization_measurements/feature_points_measurement.h> -#include <localization_measurements/handrail_points_measurement.h> #include <localization_measurements/matched_projections_measurement.h> - -#include <gtsam/geometry/Cal3_S2.h> -#include <gtsam/geometry/PinholePose.h> -#include <gtsam/inference/Symbol.h> -#include <gtsam/navigation/CombinedImuFactor.h> -#include <gtsam/navigation/ImuBias.h> -#include <gtsam/navigation/NavState.h> -#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> -#include <gtsam/nonlinear/Marginals.h> -#include <gtsam/nonlinear/NonlinearFactorGraph.h> -#include <gtsam/slam/ProjectionFactor.h> -#include <gtsam/slam/SmartFactorParams.h> -#include <gtsam/slam/SmartProjectionPoseFactor.h> +#include <localization_measurements/pose_measurement.h> +#include <node_adders/pose_node_adder.h> +#include <nodes/timestamped_nodes.h> +#include <sliding_window_graph_optimizer/sliding_window_graph_optimizer.h> #include <boost/serialization/serialization.hpp> -#include <map> -#include <string> -#include <utility> -#include <vector> - namespace graph_localizer { -namespace sym = gtsam::symbol_shorthand; -using Calibration = gtsam::Cal3_S2; -using Camera = gtsam::PinholePose<Calibration>; -using RobustSmartFactor = gtsam::RobustSmartProjectionPoseFactor<Calibration>; -using SharedRobustSmartFactor = boost::shared_ptr<RobustSmartFactor>; -using ProjectionFactor = gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3>; - -class GraphLocalizer : public graph_optimizer::GraphOptimizer { +// Siding window graph optimizer that uses matched projections and odomery poses to perform localization. +// Uses the PoseNodeAdder to add relative odometry pose factors between graph nodes. +// Matched projections typically come from matching images to a map of image +// features. +class GraphLocalizer : public sliding_window_graph_optimizer::SlidingWindowGraphOptimizer { public: explicit GraphLocalizer(const GraphLocalizerParams& params); + // For Serialization Only GraphLocalizer() {} - ~GraphLocalizer() = default; - void AddImuMeasurement(const localization_measurements::ImuMeasurement& imu_measurement); - boost::optional<localization_common::CombinedNavState> LatestCombinedNavState() const; - boost::optional<localization_common::CombinedNavState> GetCombinedNavState( - const localization_common::Time time) const; - boost::optional<std::pair<localization_common::CombinedNavState, localization_common::CombinedNavStateCovariances>> - LatestCombinedNavStateAndCovariances() const; - bool AddOpticalFlowMeasurement( - const localization_measurements::FeaturePointsMeasurement& optical_flow_feature_points_measurement); - void CheckForStandstill(); - void AddARTagMeasurement( - const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement); - void AddSparseMappingMeasurement( - const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement); - void AddHandrailMeasurement(const localization_measurements::HandrailPointsMeasurement& handrail_points_measurement); - void AddDepthOdometryMeasurement( - const localization_measurements::DepthOdometryMeasurement& depth_odometry_measurement); - bool DoPostOptimizeActions() final; - const FeatureTrackIdMap& feature_tracks() const { return feature_tracker_->feature_tracks(); } - - boost::optional<std::pair<gtsam::imuBias::ConstantBias, localization_common::Time>> LatestBiases() const; - - boost::optional<localization_common::Time> LatestExtrapolatedPoseTime() const; - - int NumFeatures() const; - - int NumOFFactors(const bool check_valid = true) const; - - int NumProjectionFactors(const bool check_valid = true) const; - - bool standstill() const; - const GraphLocalizerParams& params() const; + // Adds pose measurement to the pose node adder. + void AddPoseMeasurement(const localization_measurements::PoseWithCovarianceMeasurement& pose_measurement); - const GraphLocalizerStats& graph_localizer_stats() const; - - void SetFanSpeedMode(const localization_measurements::FanSpeedMode fan_speed_mode); + // Adds sparse map matched projections measurement to loc factor adder. + void AddSparseMapMatchedProjectionsMeasurement( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement); - const localization_measurements::FanSpeedMode fan_speed_mode() const; + // Adds AR tag matched projections measurement to loc factor adder. + void AddArTagMatchedProjectionsMeasurement( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement); - const CombinedNavStateGraphValues& combined_nav_state_graph_values() const; + // Returns a const reference to pose nodes. + const nodes::TimestampedNodes<gtsam::Pose3>& pose_nodes() const; - const CombinedNavStateNodeUpdater& combined_nav_state_node_updater() const; + // Sets pose covariance interpolater for relative odometry pose node creation + void SetPoseCovarianceInterpolater( + const std::shared_ptr<localization_common::MarginalsPoseCovarianceInterpolater<nodes::CombinedNavStateNodes>>& + pose_covariance_interpolater); private: - void InitializeNodeUpdaters(); - void InitializeFactorAdders(); - void InitializeGraphActionCompleters(); - void DoPostSlideWindowActions(const localization_common::Time oldest_allowed_time, - const boost::optional<gtsam::Marginals>& marginals) final; - - boost::optional<std::pair<localization_common::CombinedNavState, localization_common::CombinedNavStateCovariances>> - LatestCombinedNavStateAndCovariances(const gtsam::Marginals& marginals) const; - - void BufferCumulativeFactors() final; - - void RemoveOldMeasurementsFromCumulativeFactors(const gtsam::KeyVector& old_keys) final; - - bool ValidGraph() const final; - - bool ReadyToAddFactors(const localization_common::Time timestamp) const final; - - bool MeasurementRecentEnough(const localization_common::Time timestamp) const final; - - void PrintFactorDebugInfo() const final; + // bool ValidGraph() const final; // Serialization function friend class boost::serialization::access; template <class Archive> void serialize(Archive& ar, const unsigned int file_version) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(graph_optimizer::GraphOptimizer); - ar& BOOST_SERIALIZATION_NVP(feature_tracker_); - ar& BOOST_SERIALIZATION_NVP(combined_nav_state_node_updater_); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(sliding_window_graph_optimizer::SlidingWindowGraphOptimizer); + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(sparse_map_loc_factor_adder_); + ar& BOOST_SERIALIZATION_NVP(ar_tag_loc_factor_adder_); + ar& BOOST_SERIALIZATION_NVP(pose_node_adder_); } - std::shared_ptr<FeatureTracker> feature_tracker_; - std::shared_ptr<imu_integration::LatestImuIntegrator> latest_imu_integrator_; GraphLocalizerParams params_; - boost::optional<localization_measurements::FeaturePointsMeasurement> last_optical_flow_measurement_; // Factor Adders - std::shared_ptr<LocFactorAdder> ar_tag_loc_factor_adder_; - std::shared_ptr<DepthOdometryFactorAdder> depth_odometry_factor_adder_; - std::shared_ptr<HandrailFactorAdder> handrail_factor_adder_; - std::shared_ptr<LocFactorAdder> loc_factor_adder_; - std::shared_ptr<ProjectionFactorAdder> projection_factor_adder_; - std::shared_ptr<RotationFactorAdder> rotation_factor_adder_; - std::shared_ptr<SmartProjectionCumulativeFactorAdder> smart_projection_cumulative_factor_adder_; - std::shared_ptr<StandstillFactorAdder> standstill_factor_adder_; - - // Node Updaters - std::shared_ptr<CombinedNavStateNodeUpdater> combined_nav_state_node_updater_; - std::shared_ptr<FeaturePointNodeUpdater> feature_point_node_updater_; - - // Graph Action Completers - std::shared_ptr<LocGraphActionCompleter> ar_tag_loc_graph_action_completer_; - std::shared_ptr<LocGraphActionCompleter> loc_graph_action_completer_; - std::shared_ptr<ProjectionGraphActionCompleter> projection_graph_action_completer_; - std::shared_ptr<SmartProjectionGraphActionCompleter> smart_projection_graph_action_completer_; - - boost::optional<bool> standstill_; + std::shared_ptr<factor_adders::LocFactorAdder<node_adders::PoseNodeAdder>> sparse_map_loc_factor_adder_; + std::shared_ptr<factor_adders::LocFactorAdder<node_adders::PoseNodeAdder>> ar_tag_loc_factor_adder_; + + // Node Adders + std::shared_ptr<node_adders::PoseNodeAdder> pose_node_adder_; }; } // namespace graph_localizer diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer_initializer.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_initializer.h deleted file mode 100644 index faaf9b774b..0000000000 --- a/localization/graph_localizer/include/graph_localizer/graph_localizer_initializer.h +++ /dev/null @@ -1,79 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_INITIALIZER_H_ -#define GRAPH_LOCALIZER_GRAPH_LOCALIZER_INITIALIZER_H_ - -#include <camera/camera_params.h> -#include <config_reader/config_reader.h> -#include <graph_localizer/graph_localizer_params.h> -#include <imu_integration/dynamic_imu_filter.h> -#include <localization_measurements/fan_speed_mode.h> -#include <localization_measurements/imu_measurement.h> -#include <localization_measurements/timestamped_pose.h> -#include <msg_conversions/msg_conversions.h> - -#include <gtsam/geometry/Pose3.h> -#include <gtsam/navigation/ImuBias.h> - -#include <string> -#include <vector> - -namespace graph_localizer { -class GraphLocalizerInitializer { - public: - GraphLocalizerInitializer(); - void SetBiases(const gtsam::imuBias::ConstantBias& imu_bias, const bool loaded_from_previous_estimate = false, - const bool save_to_file = false); - void SetStartPose(const localization_measurements::TimestampedPose& timestamped_pose); - void SetFanSpeedMode(const localization_measurements::FanSpeedMode fan_speed_mode); - void RemoveGravityFromBiasIfPossibleAndNecessary(); - bool ReadyToInitialize() const; - void ResetBiasesAndStartPose(); - void ResetBiasesFromFileAndResetStartPose(); - void ResetStartPose(); - void ResetBiases(); - void ResetBiasesFromFile(); - void StartBiasEstimation(); - bool HasBiases() const; - bool HasStartPose() const; - bool HasParams() const; - bool HasFanSpeedMode() const; - bool EstimateBiases() const; - const GraphLocalizerParams& params() const; - void LoadGraphLocalizerParams(config_reader::ConfigReader& config); - bool RemovedGravityFromBiasIfNecessary() const; - void EstimateAndSetImuBiases(const localization_measurements::ImuMeasurement& imu_measurement, - localization_measurements::FanSpeedMode fan_speed_mode); - - private: - void RemoveGravityFromBias(const gtsam::Vector3& global_F_gravity, const gtsam::Pose3& body_T_imu, - const gtsam::Pose3& global_T_body, gtsam::imuBias::ConstantBias& imu_bias); - - bool has_biases_; - bool has_start_pose_; - bool has_params_; - bool has_fan_speed_mode_; - bool estimate_biases_; - bool removed_gravity_from_bias_if_necessary_; - graph_localizer::GraphLocalizerParams params_; - std::unique_ptr<imu_integration::DynamicImuFilter> imu_bias_filter_; - std::vector<localization_measurements::ImuMeasurement> imu_bias_measurements_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_GRAPH_LOCALIZER_INITIALIZER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet.h deleted file mode 100644 index f24fdfc2a0..0000000000 --- a/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet.h +++ /dev/null @@ -1,145 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_NODELET_H_ -#define GRAPH_LOCALIZER_GRAPH_LOCALIZER_NODELET_H_ - -#include <ff_msgs/DepthOdometry.h> -#include <ff_msgs/DepthLandmarks.h> -#include <ff_msgs/Feature2dArray.h> -#include <ff_msgs/FlightMode.h> -#include <ff_msgs/Heartbeat.h> -#include <ff_msgs/ResetMap.h> -#include <ff_msgs/SetEkfInput.h> -#include <ff_msgs/VisualLandmarks.h> -#include <ff_util/ff_nodelet.h> -#include <graph_localizer/graph_localizer_nodelet_params.h> -#include <graph_localizer/graph_localizer_wrapper.h> -#include <localization_common/ros_timer.h> -#include <localization_common/timer.h> - -#include <ros/node_handle.h> -#include <ros/publisher.h> -#include <ros/subscriber.h> -#include <sensor_msgs/Imu.h> -#include <std_srvs/Empty.h> -#include <tf2_ros/transform_broadcaster.h> - -#include <string> -#include <vector> - -namespace graph_localizer { -class GraphLocalizerNodelet : public ff_util::FreeFlyerNodelet { - public: - GraphLocalizerNodelet(); - - private: - void Initialize(ros::NodeHandle* nh) final; - - bool SetMode(ff_msgs::SetEkfInput::Request& req, ff_msgs::SetEkfInput::Response& res); - - void DisableLocalizer(); - - void EnableLocalizer(); - - bool localizer_enabled() const; - - bool ResetMapLocalizer(ff_msgs::ResetMap::Request& req, ff_msgs::ResetMap::Response& res); - - bool ResetBiasesAndLocalizer(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); - - bool ResetBiasesFromFileAndResetLocalizer(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); - - bool ResetBiasesFromFileAndResetLocalizer(); - - bool ResetLocalizer(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); - - void ResetAndEnableLocalizer(); - - void SubscribeAndAdvertise(ros::NodeHandle* nh); - - void InitializeGraph(); - - void PublishLocalizationState(); - - void PublishLocalizationGraph(); - - void PublishSparseMappingPose() const; - - void PublishARTagPose() const; - - void PublishHandrailPose() const; - - void PublishWorldTDockTF(); - - void PublishWorldTHandrailTF(); - - void PublishWorldTBodyTF(); - - void PublishReset() const; - - void PublishGraphMessages(); - - void PublishHeartbeat(); - - void OpticalFlowCallback(const ff_msgs::Feature2dArray::ConstPtr& feature_array_msg); - - void VLVisualLandmarksCallback(const ff_msgs::VisualLandmarks::ConstPtr& visual_landmarks_msg); - - void ARVisualLandmarksCallback(const ff_msgs::VisualLandmarks::ConstPtr& visual_landmarks_msg); - - void DepthOdometryCallback(const ff_msgs::DepthOdometry::ConstPtr& depth_odometry_msg); - - void DepthLandmarksCallback(const ff_msgs::DepthLandmarks::ConstPtr& depth_landmarks_msg); - - void ImuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg); - - void FlightModeCallback(ff_msgs::FlightMode::ConstPtr const& mode); - - void Run(); - - graph_localizer::GraphLocalizerWrapper graph_localizer_wrapper_; - ros::NodeHandle private_nh_; - ros::CallbackQueue private_queue_; - bool localizer_enabled_ = true; - ros::Subscriber imu_sub_, of_sub_, vl_sub_, ar_sub_, dl_sub_, depth_odometry_sub_, flight_mode_sub_; - ros::Publisher state_pub_, graph_pub_, ar_tag_pose_pub_, handrail_pose_pub_, sparse_mapping_pose_pub_, reset_pub_, - heartbeat_pub_; - ros::ServiceServer reset_srv_, bias_srv_, bias_from_file_srv_, input_mode_srv_, reset_map_srv_; - tf2_ros::TransformBroadcaster transform_pub_; - std::string platform_name_; - ff_msgs::Heartbeat heartbeat_; - GraphLocalizerNodeletParams params_; - int last_mode_ = -1; - - ros::Time last_time_tf_dock_; - ros::Time last_time_tf_handrail_; - ros::Time last_heartbeat_time_; - - // Timers - localization_common::RosTimer vl_timer_ = localization_common::RosTimer("VL msg"); - localization_common::RosTimer of_timer_ = localization_common::RosTimer("OF msg"); - localization_common::RosTimer ar_timer_ = localization_common::RosTimer("AR msg"); - localization_common::RosTimer depth_odometry_timer_ = localization_common::RosTimer("Depth odometry msg"); - localization_common::RosTimer depth_timer_ = localization_common::RosTimer("Depth msg"); - localization_common::RosTimer imu_timer_ = localization_common::RosTimer("Imu msg"); - localization_common::Timer callbacks_timer_ = localization_common::Timer("Callbacks"); - localization_common::Timer nodelet_runtime_timer_ = localization_common::Timer("Nodelet Runtime"); -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_GRAPH_LOCALIZER_NODELET_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet_params.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet_params.h deleted file mode 100644 index 5f5f1e4756..0000000000 --- a/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet_params.h +++ /dev/null @@ -1,35 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_NODELET_PARAMS_H_ -#define GRAPH_LOCALIZER_GRAPH_LOCALIZER_NODELET_PARAMS_H_ - -namespace graph_localizer { -struct GraphLocalizerNodeletParams { - int max_imu_buffer_size; - int max_optical_flow_buffer_size; - int max_vl_buffer_size; - int max_ar_buffer_size; - int max_depth_odometry_buffer_size; - int max_dl_buffer_size; - // Used to avoid saving ml/ar poses with too few landmark detections - int loc_adder_min_num_matches; - int ar_tag_loc_adder_min_num_matches; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_GRAPH_LOCALIZER_NODELET_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer_params.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_params.h index 2077cab11c..d079aebfb1 100644 --- a/localization/graph_localizer/include/graph_localizer/graph_localizer_params.h +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer_params.h @@ -15,37 +15,41 @@ * License for the specific language governing permissions and limitations * under the License. */ + #ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_PARAMS_H_ #define GRAPH_LOCALIZER_GRAPH_LOCALIZER_PARAMS_H_ -#include <graph_localizer/calibration_params.h> -#include <graph_localizer/combined_nav_state_node_updater_params.h> -#include <graph_localizer/factor_params.h> -#include <graph_localizer/feature_point_node_updater_params.h> -#include <graph_localizer/feature_tracker_params.h> -#include <graph_localizer/handrail_params.h> -#include <graph_localizer/graph_initializer_params.h> -#include <graph_optimizer/graph_optimizer_params.h> -#include <localization_measurements/fan_speed_mode.h> +#include <factor_adders/loc_factor_adder_params.h> +#include <node_adders/pose_node_adder_params.h> +#include <node_adders/timestamped_node_adder_model_params.h> +#include <optimizers/nonlinear_optimizer.h> +#include <sliding_window_graph_optimizer/sliding_window_graph_optimizer_params.h> -#include <string> +#include <boost/serialization/serialization.hpp> namespace graph_localizer { struct GraphLocalizerParams { - CombinedNavStateNodeUpdaterParams combined_nav_state_node_updater; - CalibrationParams calibration; - FactorParams factor; - FeaturePointNodeUpdaterParams feature_point_node_updater; - FeatureTrackerParams feature_tracker; - graph_optimizer::GraphOptimizerParams graph_optimizer; - GraphInitializerParams graph_initializer; - HandrailParams handrail; - double max_standstill_feature_track_avg_distance_from_mean; - int standstill_min_num_points_per_track; - double huber_k; - bool estimate_world_T_dock_using_loc; - double standstill_feature_track_duration; - localization_measurements::FanSpeedMode initial_fan_speed_mode; + factor_adders::LocFactorAdderParams ar_tag_loc_factor_adder; + factor_adders::LocFactorAdderParams sparse_map_loc_factor_adder; + node_adders::PoseNodeAdderParams pose_node_adder; + node_adders::TimestampedNodeAdderModelParams pose_node_adder_model; + optimizers::NonlinearOptimizerParams nonlinear_optimizer; + sliding_window_graph_optimizer::SlidingWindowGraphOptimizerParams sliding_window_graph_optimizer; + // Max gap between vio measurements. If this is exceeded, graph localizer is reset. + double max_vio_measurement_gap; + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_NVP(ar_tag_loc_factor_adder); + ar& BOOST_SERIALIZATION_NVP(sparse_map_loc_factor_adder); + ar& BOOST_SERIALIZATION_NVP(pose_node_adder); + ar& BOOST_SERIALIZATION_NVP(pose_node_adder_model); + ar& BOOST_SERIALIZATION_NVP(nonlinear_optimizer); + ar& BOOST_SERIALIZATION_NVP(sliding_window_graph_optimizer); + ar& BOOST_SERIALIZATION_NVP(max_vio_measurement_gap); + } }; } // namespace graph_localizer diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer_stats.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_stats.h deleted file mode 100644 index bfa5272de5..0000000000 --- a/localization/graph_localizer/include/graph_localizer/graph_localizer_stats.h +++ /dev/null @@ -1,74 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_STATS_H_ -#define GRAPH_LOCALIZER_GRAPH_LOCALIZER_STATS_H_ - -#include <graph_localizer/combined_nav_state_graph_values.h> -#include <graph_optimizer/graph_stats.h> - -namespace graph_localizer { -class GraphLocalizerStats : public graph_optimizer::GraphStats { - public: - GraphLocalizerStats(); - void SetCombinedNavStateGraphValues( - std::shared_ptr<const CombinedNavStateGraphValues> combined_nav_state_graph_values); - void UpdateErrors(const gtsam::NonlinearFactorGraph& graph_factors) final; - void UpdateStats(const gtsam::NonlinearFactorGraph& graph_factors) final; - - // Graph Stats Averagers - localization_common::Averager num_states_averager_ = localization_common::Averager("Num States"); - localization_common::Averager duration_averager_ = localization_common::Averager("Duration"); - localization_common::Averager num_marginal_factors_averager_ = localization_common::Averager("Num Marginal Factors"); - localization_common::Averager num_factors_averager_ = localization_common::Averager("Num Factors"); - localization_common::Averager num_features_averager_ = localization_common::Averager("Num Features"); - localization_common::Averager num_depth_odometry_rel_pose_factors_averager_ = - localization_common::Averager("Num Depth Odometry Rel Pose Factors"); - localization_common::Averager num_depth_odometry_rel_point_factors_averager_ = - localization_common::Averager("Num Depth Odometry Rel Point Factors"); - localization_common::Averager num_optical_flow_factors_averager_ = - localization_common::Averager("Num Optical Flow Factors"); - localization_common::Averager num_loc_proj_factors_averager_ = localization_common::Averager("Num Loc Proj Factors"); - localization_common::Averager num_loc_pose_factors_averager_ = localization_common::Averager("Num Loc Pose Factors"); - localization_common::Averager num_imu_factors_averager_ = localization_common::Averager("Num Imu Factors"); - localization_common::Averager num_rotation_factors_averager_ = localization_common::Averager("Num Rotation Factors"); - localization_common::Averager num_standstill_between_factors_averager_ = - localization_common::Averager("Num Standstill Between Factors"); - localization_common::Averager num_vel_prior_factors_averager_ = - localization_common::Averager("Num Vel Prior Factors"); - // Factor Error Averagers - localization_common::Averager depth_odom_rel_pose_error_averager_ = - localization_common::Averager("Depth Odom Rel Pose Factor Error"); - localization_common::Averager depth_odom_rel_point_error_averager_ = - localization_common::Averager("Depth Odom Rel Point Factor Error"); - localization_common::Averager of_error_averager_ = localization_common::Averager("OF Factor Error"); - localization_common::Averager loc_proj_error_averager_ = localization_common::Averager("Loc Proj Factor Error"); - localization_common::Averager loc_pose_error_averager_ = localization_common::Averager("Loc Pose Factor Error"); - localization_common::Averager imu_error_averager_ = localization_common::Averager("Imu Factor Error"); - localization_common::Averager rotation_error_averager_ = localization_common::Averager("Rotation Factor Error"); - localization_common::Averager standstill_between_error_averager_ = - localization_common::Averager("Standstill Between Error"); - localization_common::Averager pose_prior_error_averager_ = localization_common::Averager("Pose Prior Error"); - localization_common::Averager velocity_prior_error_averager_ = localization_common::Averager("Velocity Prior Error"); - localization_common::Averager bias_prior_error_averager_ = localization_common::Averager("Bias Prior Error"); - - private: - std::shared_ptr<const CombinedNavStateGraphValues> combined_nav_state_graph_values_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_GRAPH_LOCALIZER_STATS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer_wrapper.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_wrapper.h deleted file mode 100644 index 026a551d22..0000000000 --- a/localization/graph_localizer/include/graph_localizer/graph_localizer_wrapper.h +++ /dev/null @@ -1,149 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_WRAPPER_H_ -#define GRAPH_LOCALIZER_GRAPH_LOCALIZER_WRAPPER_H_ - -#include <ff_msgs/DepthOdometry.h> -#include <ff_msgs/DepthLandmarks.h> -#include <ff_msgs/GraphState.h> -#include <ff_msgs/Feature2dArray.h> -#include <ff_msgs/FlightMode.h> -#include <ff_msgs/LocalizationGraph.h> -#include <ff_msgs/VisualLandmarks.h> -#include <graph_localizer/feature_counts.h> -#include <graph_localizer/graph_localizer.h> -#include <graph_localizer/graph_localizer_initializer.h> -#include <graph_localizer/graph_localizer_stats.h> -#include <graph_localizer/sanity_checker.h> -#include <localization_measurements/fan_speed_mode.h> -#include <localization_measurements/imu_measurement.h> -#include <localization_measurements/matched_projections_measurement.h> -#include <localization_measurements/timestamped_handrail_pose.h> -#include <localization_measurements/timestamped_pose.h> - -#include <geometry_msgs/PoseStamped.h> -#include <sensor_msgs/Imu.h> - -#include <string> -#include <utility> -#include <vector> - -namespace graph_localizer { -// Handles initialization of parameters, biases, and initial pose for graph -// localizer. Provides callbacks that can be used by a ROS or non-ROS system -// (i.e. graph_bag, which does not use a ROS core, vs. graph_localizer_nodelet, -// which is used when running live). -class GraphLocalizerWrapper { - public: - explicit GraphLocalizerWrapper(const std::string& graph_config_path_prefix = ""); - - // Assumes previous bias estimates are available and uses these. - void ResetLocalizer(); - - void ResetBiasesAndLocalizer(); - - void ResetBiasesFromFileAndResetLocalizer(); - - boost::optional<geometry_msgs::PoseStamped> LatestSparseMappingPoseMsg() const; - - boost::optional<geometry_msgs::PoseStamped> LatestARTagPoseMsg() const; - - boost::optional<geometry_msgs::PoseStamped> LatestHandrailPoseMsg() const; - - boost::optional<localization_common::CombinedNavState> LatestCombinedNavState() const; - - boost::optional<ff_msgs::GraphState> LatestLocalizationStateMsg(); - - boost::optional<ff_msgs::LocalizationGraph> LatestLocalizationGraphMsg() const; - - bool Initialized() const; - - void Update(); - - void OpticalFlowCallback(const ff_msgs::Feature2dArray& feature_array_msg); - - void VLVisualLandmarksCallback(const ff_msgs::VisualLandmarks& visual_landmarks_msg); - - void ARVisualLandmarksCallback(const ff_msgs::VisualLandmarks& visual_landmarks_msg); - - void DepthOdometryCallback(const ff_msgs::DepthOdometry& depth_odometry_msg); - - void DepthLandmarksCallback(const ff_msgs::DepthLandmarks& depth_landmarks_msg); - - void ImuCallback(const sensor_msgs::Imu& imu_msg); - - void FlightModeCallback(const ff_msgs::FlightMode& flight_mode); - - boost::optional<const FeatureTrackIdMap&> feature_tracks() const; - - boost::optional<const GraphLocalizer&> graph_localizer() const; - - void MarkWorldTDockForResettingIfNecessary(); - - void MarkWorldTHandrailForResetting(); - - void ResetWorldTDockUsingLoc(const ff_msgs::VisualLandmarks& visual_landmarks_msg); - - void ResetWorldTHandrailIfNecessary(const ff_msgs::DepthLandmarks& depth_landmarks_msg); - - gtsam::Pose3 estimated_world_T_dock() const; - - boost::optional<localization_measurements::TimestampedHandrailPose> estimated_world_T_handrail() const; - - void SaveLocalizationGraphDotFile() const; - - boost::optional<const GraphLocalizerStats&> graph_localizer_stats() const; - - bool publish_localization_graph() const; - - bool save_localization_graph_dot_file() const; - - private: - void InitializeGraph(); - - bool CheckPoseSanity(const gtsam::Pose3& sparse_mapping_pose, const localization_common::Time timestamp) const; - - bool CheckCovarianceSanity() const; - - std::unique_ptr<GraphLocalizer> graph_localizer_; - // TODO(rsoussan): Make graph localizer wrapper params - bool publish_localization_graph_; - bool save_localization_graph_dot_file_; - boost::optional<gtsam::imuBias::ConstantBias> latest_biases_; - GraphLocalizerInitializer graph_localizer_initializer_; - FeatureCounts feature_counts_; - // world_T_body poses using sensor measurements - // TODO(rsoussan): Rename these? world_T_body_sensor_name? - boost::optional<localization_measurements::TimestampedPose> sparse_mapping_pose_; - boost::optional<localization_measurements::TimestampedPose> ar_tag_pose_; - boost::optional<localization_measurements::TimestampedHandrailPose> handrail_pose_; - std::unique_ptr<SanityChecker> sanity_checker_; - double position_cov_log_det_lost_threshold_; - double orientation_cov_log_det_lost_threshold_; - gtsam::Pose3 estimated_world_T_dock_; - boost::optional<localization_measurements::TimestampedHandrailPose> estimated_world_T_handrail_; - bool reset_world_T_dock_; - bool reset_world_T_handrail_; - bool estimate_world_T_dock_using_loc_; - int ar_min_num_landmarks_; - int sparse_mapping_min_num_landmarks_; - localization_measurements::FanSpeedMode fan_speed_mode_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_GRAPH_LOCALIZER_WRAPPER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/handrail_factor_adder.h b/localization/graph_localizer/include/graph_localizer/handrail_factor_adder.h deleted file mode 100644 index cba8a85329..0000000000 --- a/localization/graph_localizer/include/graph_localizer/handrail_factor_adder.h +++ /dev/null @@ -1,50 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_HANDRAIL_FACTOR_ADDER_H_ -#define GRAPH_LOCALIZER_HANDRAIL_FACTOR_ADDER_H_ - -#include <graph_localizer/handrail_factor_adder_params.h> -#include <graph_optimizer/factor_adder.h> -#include <localization_measurements/handrail_points_measurement.h> - -#include <vector> - -namespace graph_localizer { -class HandrailFactorAdder : public graph_optimizer::FactorAdder<localization_measurements::HandrailPointsMeasurement, - HandrailFactorAdderParams> { - using Base = - graph_optimizer::FactorAdder<localization_measurements::HandrailPointsMeasurement, HandrailFactorAdderParams>; - - public: - explicit HandrailFactorAdder(const HandrailFactorAdderParams& params); - - std::vector<graph_optimizer::FactorsToAdd> AddFactors( - const localization_measurements::HandrailPointsMeasurement& handrail_points_measurement) final; - void AddPointToLineOrLineSegmentFactors( - const localization_measurements::HandrailPointsMeasurement& handrail_points_measurement, - std::vector<graph_optimizer::FactorsToAdd>& factors_to_add); - void AddPointToPlaneFactors(const localization_measurements::HandrailPointsMeasurement& handrail_points_measurement, - std::vector<graph_optimizer::FactorsToAdd>& factors_to_add); - void AddPointToHandrailEndpointFactors( - const localization_measurements::HandrailPointsMeasurement& handrail_points_measurement, - std::vector<graph_optimizer::FactorsToAdd>& factors_to_add); -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_HANDRAIL_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/loc_factor_adder.h b/localization/graph_localizer/include/graph_localizer/loc_factor_adder.h deleted file mode 100644 index ce5993c445..0000000000 --- a/localization/graph_localizer/include/graph_localizer/loc_factor_adder.h +++ /dev/null @@ -1,50 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_LOC_FACTOR_ADDER_H_ -#define GRAPH_LOCALIZER_LOC_FACTOR_ADDER_H_ - -#include <graph_localizer/loc_factor_adder_params.h> -#include <graph_optimizer/factor_adder.h> -#include <localization_common/averager.h> -#include <localization_measurements/matched_projections_measurement.h> - -#include <vector> - -namespace graph_localizer { -class LocFactorAdder : public graph_optimizer::FactorAdder<localization_measurements::MatchedProjectionsMeasurement, - LocFactorAdderParams> { - using Base = - graph_optimizer::FactorAdder<localization_measurements::MatchedProjectionsMeasurement, LocFactorAdderParams>; - - public: - LocFactorAdder(const LocFactorAdderParams& params, - const graph_optimizer::GraphActionCompleterType graph_action_completer_type); - - std::vector<graph_optimizer::FactorsToAdd> AddFactors( - const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement) final; - - private: - graph_optimizer::GraphActionCompleterType type() const; - - graph_optimizer::GraphActionCompleterType graph_action_completer_type_; - localization_common::Averager num_landmarks_averager_ = localization_common::Averager("Num Landmarks"); -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_LOC_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/loc_factor_adder_params.h b/localization/graph_localizer/include/graph_localizer/loc_factor_adder_params.h deleted file mode 100644 index 6d3eb124a9..0000000000 --- a/localization/graph_localizer/include/graph_localizer/loc_factor_adder_params.h +++ /dev/null @@ -1,49 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_LOC_FACTOR_ADDER_PARAMS_H_ -#define GRAPH_LOCALIZER_LOC_FACTOR_ADDER_PARAMS_H_ - -#include <graph_optimizer/factor_adder_params.h> - -#include <gtsam/geometry/Pose3.h> -#include <gtsam/geometry/Cal3_S2.h> -#include <gtsam/linear/NoiseModel.h> - -namespace graph_localizer { -struct LocFactorAdderParams : public graph_optimizer::FactorAdderParams { - bool add_pose_priors; - bool add_projections; - double prior_translation_stddev; - double prior_quaternion_stddev; - int max_num_factors; - int min_num_matches; - bool scale_pose_noise_with_num_landmarks; - bool scale_projection_noise_with_num_landmarks; - double pose_noise_scale; - double projection_noise_scale; - double max_inlier_weighted_projection_norm; - bool weight_projections_with_distance; - bool add_prior_if_projections_fail; - gtsam::Pose3 body_T_cam; - boost::shared_ptr<gtsam::Cal3_S2> cam_intrinsics; - gtsam::SharedIsotropic cam_noise; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_LOC_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/loc_graph_action_completer.h b/localization/graph_localizer/include/graph_localizer/loc_graph_action_completer.h deleted file mode 100644 index 6f3c8d67fd..0000000000 --- a/localization/graph_localizer/include/graph_localizer/loc_graph_action_completer.h +++ /dev/null @@ -1,45 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_LOC_GRAPH_ACTION_COMPLETER_H_ -#define GRAPH_LOCALIZER_LOC_GRAPH_ACTION_COMPLETER_H_ - -#include <graph_localizer/combined_nav_state_graph_values.h> -#include <graph_localizer/loc_factor_adder_params.h> -#include <graph_optimizer/graph_action_completer.h> -#include <graph_optimizer/graph_action_completer_type.h> - -namespace graph_localizer { -class LocGraphActionCompleter : public graph_optimizer::GraphActionCompleter { - public: - LocGraphActionCompleter(const LocFactorAdderParams& params, - const graph_optimizer::GraphActionCompleterType graph_action_completer_type, - std::shared_ptr<CombinedNavStateGraphValues> graph_values); - - bool DoAction(graph_optimizer::FactorsToAdd& factors_to_add, gtsam::NonlinearFactorGraph& graph_factors) final; - - graph_optimizer::GraphActionCompleterType type() const final; - - private: - LocFactorAdderParams params_; - graph_optimizer::GraphActionCompleterType graph_action_completer_type_; - std::shared_ptr<CombinedNavStateGraphValues> graph_values_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_LOC_GRAPH_ACTION_COMPLETER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/parameter_reader.h b/localization/graph_localizer/include/graph_localizer/parameter_reader.h deleted file mode 100644 index f47e34bd2d..0000000000 --- a/localization/graph_localizer/include/graph_localizer/parameter_reader.h +++ /dev/null @@ -1,69 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#ifndef GRAPH_LOCALIZER_PARAMETER_READER_H_ -#define GRAPH_LOCALIZER_PARAMETER_READER_H_ - -#include <config_reader/config_reader.h> -#include <graph_localizer/calibration_params.h> -#include <graph_localizer/combined_nav_state_graph_values_params.h> -#include <graph_localizer/combined_nav_state_node_updater_params.h> -#include <graph_localizer/factor_params.h> -#include <graph_localizer/feature_point_node_updater_params.h> -#include <graph_localizer/feature_tracker_params.h> -#include <graph_localizer/graph_initializer_params.h> -#include <graph_localizer/graph_localizer_nodelet_params.h> -#include <graph_localizer/graph_localizer_params.h> -#include <graph_localizer/handrail_params.h> -#include <graph_localizer/depth_odometry_factor_adder_params.h> -#include <graph_localizer/handrail_factor_adder_params.h> -#include <graph_localizer/loc_factor_adder_params.h> -#include <graph_localizer/projection_factor_adder_params.h> -#include <graph_localizer/rotation_factor_adder_params.h> -#include <graph_localizer/sanity_checker_params.h> -#include <graph_localizer/smart_projection_factor_adder_params.h> -#include <graph_localizer/standstill_factor_adder_params.h> - -namespace graph_localizer { -void LoadCalibrationParams(config_reader::ConfigReader& config, CalibrationParams& params); -void LoadFactorParams(config_reader::ConfigReader& config, FactorParams& params); -void LoadDepthOdometryFactorAdderParams(config_reader::ConfigReader& config, DepthOdometryFactorAdderParams& params); -void LoadHandrailFactorAdderParams(config_reader::ConfigReader& config, HandrailFactorAdderParams& params); -void LoadLocFactorAdderParams(config_reader::ConfigReader& config, LocFactorAdderParams& params); -void LoadARTagLocFactorAdderParams(config_reader::ConfigReader& config, LocFactorAdderParams& params); -void LoadProjectionFactorAdderParams(config_reader::ConfigReader& config, ProjectionFactorAdderParams& params); -void LoadRotationFactorAdderParams(config_reader::ConfigReader& config, RotationFactorAdderParams& params); -void LoadSmartProjectionFactorAdderParams(config_reader::ConfigReader& config, - SmartProjectionFactorAdderParams& params); -void LoadStandstillFactorAdderParams(config_reader::ConfigReader& config, StandstillFactorAdderParams& params); -void LoadFeatureTrackerParams(config_reader::ConfigReader& config, FeatureTrackerParams& params); -void LoadImuIntegrationParams(config_reader::ConfigReader& config, GraphInitializerParams& params); -void LoadSanityCheckerParams(config_reader::ConfigReader& config, SanityCheckerParams& params); -// Loads all params except some (biases and start pose) that are -// not loaded from config files -void LoadGraphInitializerParams(config_reader::ConfigReader& config, GraphInitializerParams& params); -void LoadCombinedNavStateGraphValuesParams(config_reader::ConfigReader& config, - CombinedNavStateGraphValuesParams& params); -void LoadCombinedNavStateNodeUpdaterParams(config_reader::ConfigReader& config, - CombinedNavStateNodeUpdaterParams& params); -void LoadFeaturePointNodeUpdaterParams(config_reader::ConfigReader& config, FeaturePointNodeUpdaterParams& params); -void LoadHandrailParams(config_reader::ConfigReader& config, HandrailParams& params); -void LoadGraphLocalizerParams(config_reader::ConfigReader& config, GraphLocalizerParams& params); -void LoadGraphLocalizerNodeletParams(config_reader::ConfigReader& config, GraphLocalizerNodeletParams& params); -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_PARAMETER_READER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/projection_factor_adder.h b/localization/graph_localizer/include/graph_localizer/projection_factor_adder.h deleted file mode 100644 index fe7bd35123..0000000000 --- a/localization/graph_localizer/include/graph_localizer/projection_factor_adder.h +++ /dev/null @@ -1,52 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_PROJECTION_FACTOR_ADDER_H_ -#define GRAPH_LOCALIZER_PROJECTION_FACTOR_ADDER_H_ - -#include <graph_localizer/feature_point_graph_values.h> -#include <graph_localizer/feature_tracker.h> -#include <graph_localizer/projection_factor_adder_params.h> -#include <graph_optimizer/factor_adder.h> -#include <localization_measurements/feature_points_measurement.h> - -#include <gtsam/geometry/triangulation.h> - -#include <vector> - -namespace graph_localizer { -class ProjectionFactorAdder : public graph_optimizer::FactorAdder<localization_measurements::FeaturePointsMeasurement, - ProjectionFactorAdderParams> { - using Base = - graph_optimizer::FactorAdder<localization_measurements::FeaturePointsMeasurement, ProjectionFactorAdderParams>; - - public: - ProjectionFactorAdder(const ProjectionFactorAdderParams& params, - std::shared_ptr<const FeatureTracker> feature_tracker, - std::shared_ptr<const FeaturePointGraphValues> feature_point_graph_values); - - std::vector<graph_optimizer::FactorsToAdd> AddFactors( - const localization_measurements::FeaturePointsMeasurement& feature_points_measurement) final; - - private: - std::shared_ptr<const FeatureTracker> feature_tracker_; - std::shared_ptr<const FeaturePointGraphValues> feature_point_graph_values_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_PROJECTION_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/projection_factor_adder_params.h b/localization/graph_localizer/include/graph_localizer/projection_factor_adder_params.h deleted file mode 100644 index 2c15543913..0000000000 --- a/localization/graph_localizer/include/graph_localizer/projection_factor_adder_params.h +++ /dev/null @@ -1,45 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_PROJECTION_FACTOR_ADDER_PARAMS_H_ -#define GRAPH_LOCALIZER_PROJECTION_FACTOR_ADDER_PARAMS_H_ - -#include <graph_optimizer/factor_adder_params.h> - -#include <gtsam/geometry/Pose3.h> -#include <gtsam/geometry/Cal3_S2.h> -#include <gtsam/linear/NoiseModel.h> - -#include <string> - -namespace graph_localizer { -struct ProjectionFactorAdderParams : public graph_optimizer::FactorAdderParams { - bool enable_EPI; - double landmark_distance_threshold; - double dynamic_outlier_rejection_threshold; - int max_num_features; - int min_num_measurements_for_triangulation; - bool add_point_priors; - double point_prior_translation_stddev; - gtsam::Pose3 body_T_cam; - boost::shared_ptr<gtsam::Cal3_S2> cam_intrinsics; - gtsam::SharedIsotropic cam_noise; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_PROJECTION_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/projection_graph_action_completer.h b/localization/graph_localizer/include/graph_localizer/projection_graph_action_completer.h deleted file mode 100644 index 5ff8cecb1d..0000000000 --- a/localization/graph_localizer/include/graph_localizer/projection_graph_action_completer.h +++ /dev/null @@ -1,50 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_PROJECTION_GRAPH_ACTION_COMPLETER_H_ -#define GRAPH_LOCALIZER_PROJECTION_GRAPH_ACTION_COMPLETER_H_ - -#include <graph_localizer/combined_nav_state_graph_values.h> -#include <graph_localizer/projection_factor_adder_params.h> -#include <graph_localizer/feature_point_graph_values.h> -#include <graph_optimizer/graph_action_completer.h> - -#include <gtsam/geometry/triangulation.h> - -namespace graph_localizer { -class ProjectionGraphActionCompleter : public graph_optimizer::GraphActionCompleter { - public: - ProjectionGraphActionCompleter(const ProjectionFactorAdderParams& params, - std::shared_ptr<const CombinedNavStateGraphValues> graph_values, - std::shared_ptr<FeaturePointGraphValues> feature_point_graph_values); - - bool DoAction(graph_optimizer::FactorsToAdd& factors_to_add, gtsam::NonlinearFactorGraph& graph_factors) final; - - graph_optimizer::GraphActionCompleterType type() const final; - - private: - bool TriangulateNewPoint(graph_optimizer::FactorsToAdd& factors_to_add, gtsam::NonlinearFactorGraph& graph_factors); - - ProjectionFactorAdderParams params_; - std::shared_ptr<const CombinedNavStateGraphValues> graph_values_; - std::shared_ptr<FeaturePointGraphValues> feature_point_graph_values_; - gtsam::TriangulationParameters projection_triangulation_params_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_PROJECTION_GRAPH_ACTION_COMPLETER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/rotation_factor_adder.h b/localization/graph_localizer/include/graph_localizer/rotation_factor_adder.h deleted file mode 100644 index f640fe148a..0000000000 --- a/localization/graph_localizer/include/graph_localizer/rotation_factor_adder.h +++ /dev/null @@ -1,46 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_ROTATION_FACTOR_ADDER_H_ -#define GRAPH_LOCALIZER_ROTATION_FACTOR_ADDER_H_ - -#include <graph_localizer/feature_tracker.h> -#include <graph_localizer/rotation_factor_adder_params.h> -#include <graph_optimizer/factor_adder.h> -#include <localization_measurements/feature_points_measurement.h> - -#include <vector> - -namespace graph_localizer { -class RotationFactorAdder : public graph_optimizer::FactorAdder<localization_measurements::FeaturePointsMeasurement, - RotationFactorAdderParams> { - using Base = - graph_optimizer::FactorAdder<localization_measurements::FeaturePointsMeasurement, RotationFactorAdderParams>; - - public: - RotationFactorAdder(const RotationFactorAdderParams& params, std::shared_ptr<const FeatureTracker> feature_tracker); - - std::vector<graph_optimizer::FactorsToAdd> AddFactors( - const localization_measurements::FeaturePointsMeasurement& measurement) final; - - private: - std::shared_ptr<const FeatureTracker> feature_tracker_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_ROTATION_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/sanity_checker.h b/localization/graph_localizer/include/graph_localizer/sanity_checker.h deleted file mode 100644 index 702029881e..0000000000 --- a/localization/graph_localizer/include/graph_localizer/sanity_checker.h +++ /dev/null @@ -1,41 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_SANITY_CHECKER_H_ -#define GRAPH_LOCALIZER_SANITY_CHECKER_H_ - -#include <graph_localizer/sanity_checker_params.h> -#include <localization_common/combined_nav_state_covariances.h> - -#include <gtsam/geometry/Pose3.h> - -namespace graph_localizer { -class SanityChecker { - public: - explicit SanityChecker(const SanityCheckerParams& params); - bool CheckPoseSanity(const gtsam::Pose3& sparse_mapping_pose, const gtsam::Pose3& localizer_pose); - bool CheckCovarianceSanity(const localization_common::CombinedNavStateCovariances& covariances) const; - void Reset(); - - private: - SanityCheckerParams params_; - int num_consecutive_failures_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_SANITY_CHECKER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/sanity_checker_params.h b/localization/graph_localizer/include/graph_localizer/sanity_checker_params.h deleted file mode 100644 index 3052cb1ba6..0000000000 --- a/localization/graph_localizer/include/graph_localizer/sanity_checker_params.h +++ /dev/null @@ -1,35 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#ifndef GRAPH_LOCALIZER_SANITY_CHECKER_PARAMS_H_ -#define GRAPH_LOCALIZER_SANITY_CHECKER_PARAMS_H_ - -namespace graph_localizer { -struct SanityCheckerParams { - // Check Pose - bool check_pose_difference; - int num_consecutive_pose_difference_failures_until_insane; - double max_sane_position_difference; - // Check Covariances - bool check_position_covariance; - bool check_orientation_covariance; - double position_covariance_threshold; - double orientation_covariance_threshold; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_SANITY_CHECKER_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/smart_projection_cumulative_factor_adder.h b/localization/graph_localizer/include/graph_localizer/smart_projection_cumulative_factor_adder.h deleted file mode 100644 index 1101fdf4e1..0000000000 --- a/localization/graph_localizer/include/graph_localizer/smart_projection_cumulative_factor_adder.h +++ /dev/null @@ -1,65 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_SMART_PROJECTION_CUMULATIVE_FACTOR_ADDER_H_ -#define GRAPH_LOCALIZER_SMART_PROJECTION_CUMULATIVE_FACTOR_ADDER_H_ - -#include <graph_localizer/feature_tracker.h> -#include <graph_localizer/smart_projection_factor_adder_params.h> -#include <graph_optimizer/cumulative_factor_adder.h> - -#include <gtsam/slam/SmartFactorParams.h> - -#include <unordered_map> -#include <vector> - -namespace graph_localizer { -class SmartProjectionCumulativeFactorAdder - : public graph_optimizer::CumulativeFactorAdder<SmartProjectionFactorAdderParams> { - using Base = graph_optimizer::CumulativeFactorAdder<SmartProjectionFactorAdderParams>; - - public: - SmartProjectionCumulativeFactorAdder(const SmartProjectionFactorAdderParams& params, - std::shared_ptr<const FeatureTracker> feature_tracker); - - std::vector<graph_optimizer::FactorsToAdd> AddFactors() final; - void AddFactors( - const FeatureTrackLengthMap& feature_tracks, const int spacing, const double feature_track_min_separation, - graph_optimizer::FactorsToAdd& smart_factors_to_add, - std::unordered_map<localization_measurements::FeatureId, localization_measurements::FeaturePoint>& added_points); - void AddAllowedFactors( - const FeatureTrackLengthMap& feature_tracks, const double feature_track_min_separation, - graph_optimizer::FactorsToAdd& smart_factors_to_add, - std::unordered_map<localization_measurements::FeatureId, localization_measurements::FeaturePoint>& added_points); - - const gtsam::SmartProjectionParams& smart_projection_params() const; - - private: - void AddSmartFactor(const std::vector<localization_measurements::FeaturePoint>& feature_track_points, - graph_optimizer::FactorsToAdd& smart_factors_to_add) const; - - bool TooClose(const std::unordered_map<localization_measurements::FeatureId, localization_measurements::FeaturePoint>& - added_points, - const localization_measurements::FeaturePoint& point, const double feature_track_min_separation) const; - - std::shared_ptr<const FeatureTracker> feature_tracker_; - gtsam::SmartProjectionParams smart_projection_params_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_SMART_PROJECTION_CUMULATIVE_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/smart_projection_factor_adder_params.h b/localization/graph_localizer/include/graph_localizer/smart_projection_factor_adder_params.h deleted file mode 100644 index 5240f70879..0000000000 --- a/localization/graph_localizer/include/graph_localizer/smart_projection_factor_adder_params.h +++ /dev/null @@ -1,55 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_SMART_PROJECTION_FACTOR_ADDER_PARAMS_H_ -#define GRAPH_LOCALIZER_SMART_PROJECTION_FACTOR_ADDER_PARAMS_H_ - -#include <graph_optimizer/factor_adder_params.h> - -#include <gtsam/geometry/Pose3.h> -#include <gtsam/geometry/Cal3_S2.h> -#include <gtsam/linear/NoiseModel.h> - -#include <string> - -namespace graph_localizer { -struct SmartProjectionFactorAdderParams : public graph_optimizer::FactorAdderParams { - double min_avg_distance_from_mean; - bool enable_EPI; - double landmark_distance_threshold; - double dynamic_outlier_rejection_threshold; - double retriangulation_threshold; - bool verbose_cheirality; - bool robust; - int max_num_factors; - int min_num_points; - int max_num_points_per_factor; - int measurement_spacing; - double feature_track_min_separation; - bool rotation_only_fallback; - bool splitting; - bool scale_noise_with_num_points; - double noise_scale; - bool use_allowed_timestamps; - gtsam::Pose3 body_T_cam; - boost::shared_ptr<gtsam::Cal3_S2> cam_intrinsics; - gtsam::SharedIsotropic cam_noise; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_SMART_PROJECTION_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/smart_projection_graph_action_completer.h b/localization/graph_localizer/include/graph_localizer/smart_projection_graph_action_completer.h deleted file mode 100644 index 9e1d89ddc3..0000000000 --- a/localization/graph_localizer/include/graph_localizer/smart_projection_graph_action_completer.h +++ /dev/null @@ -1,50 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_SMART_PROJECTION_GRAPH_ACTION_COMPLETER_H_ -#define GRAPH_LOCALIZER_SMART_PROJECTION_GRAPH_ACTION_COMPLETER_H_ - -#include <graph_localizer/smart_projection_factor_adder_params.h> -#include <graph_optimizer/graph_action_completer.h> -#include <graph_localizer/combined_nav_state_graph_values.h> - -#include <gtsam/slam/SmartFactorParams.h> - -namespace graph_localizer { -class SmartProjectionGraphActionCompleter : public graph_optimizer::GraphActionCompleter { - public: - SmartProjectionGraphActionCompleter(const SmartProjectionFactorAdderParams& params, - std::shared_ptr<const CombinedNavStateGraphValues> graph_values); - - bool DoAction(graph_optimizer::FactorsToAdd& factors_to_add, gtsam::NonlinearFactorGraph& graph_factors) final; - - graph_optimizer::GraphActionCompleterType type() const final; - - const gtsam::SmartProjectionParams& smart_projection_params() const; - - private: - void SplitSmartFactorsIfNeeded(const CombinedNavStateGraphValues& graph_values, - graph_optimizer::FactorsToAdd& factors_to_add); - - SmartProjectionFactorAdderParams params_; - std::shared_ptr<const CombinedNavStateGraphValues> graph_values_; - gtsam::SmartProjectionParams smart_projection_params_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_SMART_PROJECTION_GRAPH_ACTION_COMPLETER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/standstill_factor_adder.h b/localization/graph_localizer/include/graph_localizer/standstill_factor_adder.h deleted file mode 100644 index e230f57201..0000000000 --- a/localization/graph_localizer/include/graph_localizer/standstill_factor_adder.h +++ /dev/null @@ -1,47 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_H_ -#define GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_H_ - -#include <graph_localizer/feature_tracker.h> -#include <graph_localizer/standstill_factor_adder_params.h> -#include <graph_optimizer/factor_adder.h> -#include <localization_measurements/feature_points_measurement.h> - -#include <vector> - -namespace graph_localizer { -class StandstillFactorAdder : public graph_optimizer::FactorAdder<localization_measurements::FeaturePointsMeasurement, - StandstillFactorAdderParams> { - using Base = - graph_optimizer::FactorAdder<localization_measurements::FeaturePointsMeasurement, StandstillFactorAdderParams>; - - public: - explicit StandstillFactorAdder(const StandstillFactorAdderParams& params, - std::shared_ptr<const FeatureTracker> feature_tracker); - - std::vector<graph_optimizer::FactorsToAdd> AddFactors( - const localization_measurements::FeaturePointsMeasurement& feature_points_measurement) final; - - private: - std::shared_ptr<const FeatureTracker> feature_tracker_; -}; -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/test_utilities.h b/localization/graph_localizer/include/graph_localizer/test_utilities.h deleted file mode 100644 index 272da6dad0..0000000000 --- a/localization/graph_localizer/include/graph_localizer/test_utilities.h +++ /dev/null @@ -1,49 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_TEST_UTILITIES_H_ -#define GRAPH_LOCALIZER_TEST_UTILITIES_H_ - -#include <graph_optimizer/graph_optimizer_params.h> -#include <graph_localizer/combined_nav_state_graph_values_params.h> -#include <graph_localizer/depth_odometry_factor_adder_params.h> -#include <graph_localizer/graph_initializer_params.h> -#include <graph_localizer/graph_localizer_params.h> -#include <localization_measurements/depth_odometry_measurement.h> -#include <localization_measurements/plane.h> - -namespace graph_localizer { -localization_measurements::Plane RandomPlane(); - -localization_measurements::DepthOdometryMeasurement DepthOdometryMeasurementFromPose( - const Eigen::Isometry3d& pose, const localization_common::Time source_time, - const localization_common::Time target_time); - -CombinedNavStateGraphValuesParams DefaultCombinedNavStateGraphValuesParams(); - -graph_optimizer::GraphOptimizerParams DefaultGraphOptimizerParams(); - -CombinedNavStateNodeUpdaterParams DefaultCombinedNavStateNodeUpdaterParams(); - -GraphInitializerParams DefaultGraphInitializerParams(); - -GraphLocalizerParams DefaultGraphLocalizerParams(); - -DepthOdometryFactorAdderParams DefaultDepthOdometryFactorAdderParams(); -} // namespace graph_localizer -#endif // GRAPH_LOCALIZER_TEST_UTILITIES_H_ diff --git a/localization/graph_localizer/include/graph_localizer/utilities.h b/localization/graph_localizer/include/graph_localizer/utilities.h deleted file mode 100644 index 67ce5c9337..0000000000 --- a/localization/graph_localizer/include/graph_localizer/utilities.h +++ /dev/null @@ -1,99 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_LOCALIZER_UTILITIES_H_ -#define GRAPH_LOCALIZER_UTILITIES_H_ - -#include <ff_msgs/DepthLandmarks.h> -#include <ff_msgs/GraphState.h> -#include <ff_msgs/LocalizationGraph.h> -#include <ff_msgs/VisualLandmarks.h> -#include <graph_localizer/combined_nav_state_graph_values.h> -#include <graph_localizer/feature_counts.h> -#include <graph_localizer/feature_track.h> -#include <graph_localizer/graph_localizer.h> -#include <graph_localizer/graph_localizer_initializer.h> -#include <graph_localizer/graph_localizer_stats.h> -#include <localization_common/combined_nav_state.h> -#include <localization_common/combined_nav_state_covariances.h> -#include <localization_measurements/feature_point.h> -#include <localization_measurements/imu_measurement.h> -#include <localization_measurements/timestamped_pose.h> - -#include <gtsam/geometry/Pose3.h> -#include <gtsam/linear/NoiseModel.h> - -#include <Eigen/Core> - -#include <boost/optional.hpp> - -#include <geometry_msgs/PoseStamped.h> -#include <std_msgs/Header.h> - -#include <deque> -#include <string> -#include <unordered_set> -#include <vector> - -namespace graph_localizer { -bool ValidPointSet(const int num_points, const double average_distance_from_mean, - const double min_avg_distance_from_mean, const int min_num_points); - -double AverageDistanceFromMean(const std::vector<localization_measurements::FeaturePoint>& points); - -bool ValidVLMsg(const ff_msgs::VisualLandmarks& visual_landmarks_msg, const int min_num_landmarks); - -bool ValidDepthMsg(const ff_msgs::DepthLandmarks& depth_landmarks_msg); - -ff_msgs::GraphState GraphStateMsg(const localization_common::CombinedNavState& combined_nav_state, - const localization_common::CombinedNavStateCovariances& covariances, - const FeatureCounts& detected_feature_counts, const bool estimating_bias, - const double position_log_det_threshold, const double orientation_log_det_threshold, - const bool standstill, const GraphLocalizerStats& graph_stats, - const localization_measurements::FanSpeedMode fan_speed_mode); - -ff_msgs::LocalizationGraph GraphMsg(const GraphLocalizer& graph_localizer); - -geometry_msgs::PoseStamped PoseMsg(const Eigen::Isometry3d& global_T_body, const std_msgs::Header& header); - -geometry_msgs::PoseStamped PoseMsg(const Eigen::Isometry3d& global_T_body, const localization_common::Time time); - -geometry_msgs::PoseStamped PoseMsg(const gtsam::Pose3& global_T_body, const localization_common::Time time); - -geometry_msgs::PoseStamped PoseMsg(const localization_measurements::TimestampedPose& timestamped_pose); - -gtsam::noiseModel::Robust::shared_ptr Robust(const gtsam::SharedNoiseModel& noise, const double huber_k); - -boost::optional<SharedRobustSmartFactor> FixSmartFactorByRemovingIndividualMeasurements( - const SmartProjectionFactorAdderParams& params, const RobustSmartFactor& smart_factor, - const gtsam::SmartProjectionParams& smart_projection_params, const CombinedNavStateGraphValues& graph_values); - -boost::optional<SharedRobustSmartFactor> FixSmartFactorByRemovingMeasurementSequence( - const SmartProjectionFactorAdderParams& params, const RobustSmartFactor& smart_factor, - const gtsam::SmartProjectionParams& smart_projection_params, const CombinedNavStateGraphValues& graph_values); - -SharedRobustSmartFactor RemoveSmartFactorMeasurements(const RobustSmartFactor& smart_factor, - const std::unordered_set<int>& factor_key_indices_to_remove, - const SmartProjectionFactorAdderParams& params, - const gtsam::SmartProjectionParams& smart_projection_params); - -int NumSmartFactors(const gtsam::NonlinearFactorGraph& graph_factors, const gtsam::Values& values, - const bool check_valid); -} // namespace graph_localizer - -#endif // GRAPH_LOCALIZER_UTILITIES_H_ diff --git a/localization/graph_localizer/nodelet_plugins.xml b/localization/graph_localizer/nodelet_plugins.xml deleted file mode 100644 index 5dfc21ed51..0000000000 --- a/localization/graph_localizer/nodelet_plugins.xml +++ /dev/null @@ -1,11 +0,0 @@ -<library path="lib/libgraph_localizer"> - <class name="graph_localizer/GraphLocalizerNodelet" - type="graph_localizer::GraphLocalizerNodelet" - base_class_type="nodelet::Nodelet"> - <description> - This nodelet wraps graph localizer - </description> - </class> -</library> - - diff --git a/localization/graph_localizer/package.xml b/localization/graph_localizer/package.xml index d1d51136ad..b819f20d9b 100644 --- a/localization/graph_localizer/package.xml +++ b/localization/graph_localizer/package.xml @@ -14,29 +14,16 @@ Astrobee Flight Software </maintainer> <buildtool_depend>catkin</buildtool_depend> - <build_depend>nodelet</build_depend> - <build_depend>camera</build_depend> - <build_depend>config_reader</build_depend> - <build_depend>ff_util</build_depend> - <build_depend>ff_msgs</build_depend> - <build_depend>graph_optimizer</build_depend> - <build_depend>imu_integration</build_depend> + <build_depend>factor_adders</build_depend> + <build_depend>graph_factors</build_depend> <build_depend>localization_common</build_depend> <build_depend>localization_measurements</build_depend> - <build_depend>msg_conversions</build_depend> - <build_depend>vision_common</build_depend> - <run_depend>nodelet</run_depend> - <run_depend>camera</run_depend> - <run_depend>config_reader</run_depend> - <run_depend>ff_util</run_depend> - <run_depend>ff_msgs</run_depend> - <run_depend>graph_optimizer</run_depend> - <run_depend>imu_integration</run_depend> + <build_depend>node_adders</build_depend> + <build_depend>sliding_window_graph_optimizer</build_depend> + <run_depend>factor_adders</run_depend> + <run_depend>graph_factors</run_depend> <run_depend>localization_common</run_depend> <run_depend>localization_measurements</run_depend> - <run_depend>msg_conversions</run_depend> - <run_depend>vision_common</run_depend> - <export> - <nodelet plugin="${prefix}/nodelet_plugins.xml" /> - </export> + <run_depend>node_adders</run_depend> + <run_depend>sliding_window_graph_optimizer</run_depend> </package> diff --git a/localization/graph_localizer/readme.md b/localization/graph_localizer/readme.md index 34678428c1..be8456496e 100644 --- a/localization/graph_localizer/readme.md +++ b/localization/graph_localizer/readme.md @@ -1,92 +1,22 @@ -\page graphlocalizer Graph Localizer +\page graphlocalizer Graph Localizer # Package Overview -The GraphLocalizer uses the GraphOptimizer (please first read the documentation in the GraphOptimizer package as this package uses many objects from there) to track the pose, velocity and IMU biases (the set of these three state parameters is referred to as a CombinedNavState) for a robot. It uses a set of measurements (IMU, optical flow, map-based image features, ar tag detections, 3D handrail and wall plane points) to generate factors that constrain its history of CombinedNavStates. -## Code Structure -The GraphLocalizer package contains the GraphLocalizer, GraphLocalizerWrapper, and GraphLocalizerNodelet objects. The GraphLocalizer handles the creation of factors and contains the GraphOptimizer, while the GraphLocalizerWrapper provides an interface that can be used both online with the GraphLocalizerNodelet and offline in a ROS free environement as is done in the GraphBag tool (see GraphBag package). The GraphLocalizerWrapper converts ROS messages to localization_measurement objects and subsequently passes these to the GraphLocalizer so that the GraphLocalizer does not contain any ROS code. Finally, the GraphLocalizerNodelet is a ROS nodelet that subscribes to the required messages and passes these to the GraphLocalizerWrapper. It also publishes required messages and TFs for online usage. +Performs sliding-window graph based optimization using a LocFactorAdder. Uses a PoseNodeAdder for creating pose nodes at required timestamps using relative odometry measurements. # Background -For more information on the theory behind the GraphLocalizer and the factors used, please see our paper (TODO(rsoussan): link paper when publicly available). -# Background (until paper is linked) - The GraphLocaluzer uses the `CombinedImuFactor` (_Forster, Christian, et al. "IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation." Georgia Institute of Technology, 2015._) from gtsam which intelligently integrates imu measurements and provides an error function for relative states (consisting of pose, velocity, and imu biases) that depends on each state. Additionally, the graph localizer uses the `SmartProjectionFactor` (_Carlone, Luca, et al. "Eliminating conditionally independent sets in factor graphs: A unifying perspective based on smart factors." 2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2014._) which is a structureless visual odometry factor that combines feature tracks from different camera poses and uses these measurements to estimate first the 3D feature, then project this feature into each measurement to create the factor. The factor is structureless since it does not optimize for the 3D feature, rather it marginalizes this out using a similar null-space projection as presented in (_Mourikis, Anastasios I., and Stergios I. Roumeliotis. "A multi-state constraint Kalman filter for vision-aided inertial navigation." Proceedings 2007 IEEE International Conference on Robotics and Automation. IEEE, 2007._). Additionally, image projection measurements from map to image space are incorporated into the graph. - - -# Important Classes - -## Graph Localizer -The GraphLocalizer is a GraphOptimizer that contains measurement specific FactorAdders and state parameter specific NodeUpdaters. These are detailed further in the following sections. - -## FactorAdders -### LocFactorAdder -The LocFactorAdder takes map-based image feature measurements and generates either LocProjectionFactors or LocPoseFactors, depeneding on if the LocProjectionFactors suffered cheirality errors or not. - -### ProjectionFactorAdder -The ProjectionFactorAdder creates bundle-adjustment ProjectionFactors for tracked image features. (Not currently used). - -### RotationFactorAdder -The RotationFactorAdder generates a relative rotation using tracked image features in two successive images. (Not currently used). - -### SmartProjectionCumulativeFactorAdder -The SmartProjectionCumulativeFactorAdder generates visual odometry smart factors using image feature tracks. It contains options for the minimum disparity allowed for feature tracks, minimum separation in image space for added feature tracks, whether to use a rotation-only factor if created smart factors suffer from cheirality errors, and more. It can generate factors using maximally spaced measurements in time to include a longer history of measurements while including only a maximum number of total measurements or only include measurements from a given set with a set spacing (toggle these with the use\_allowed\_timestamps option). - -### StandstillFactorAdder -The StandstillFactorAdder creates a zero velocity prior and zero tranform between factor for successive CombinedNavState nodes when standstill is detected. Standstill detection checks for a minimum average disparity for image feature tracks over time. - -## Factors - -### LocPoseFactor -The LocPoseFactor is simply a gtsam::PriorFactor\<gtsam::Pose3\> that enables the differention of a pose prior from a localization map-based image feature factor. - -### LocProjectionFactor -The LocProjectionFactor is almost a direct copy of the gtsam::ProjectionFactor except it does not optimize for the 3D feature point location. - -### PoseRotationFactor -The PoseRotationFactor constrains two gtsam::Pose3 nodes using their relative rotation. - -### PointToHandrailEndpointFactor.h -The PointToHandrailEndpointFactor constrains a gtsam::Pose3 using a handrail endpoint detection in the sensor frame compared with the closest handrail endpoint from a know handrail. - -### PointToLineFactor.h -The PointToLineFactor constrains a gtsam::Pose3 using a point detection in the sensor frame compared with a line in the world frame. - -### PointToLineSegmentFactor.h -The PointToLineSegmentFactor constrains a gtsam::Pose3 using a point detection in the sensor frame compared with a line segment in the world frame. This factor contains a discontinuity in the Jacobian as there is zero error along the line segment axis if the point is between line segment endpoints and non-zero error otherwise. An option to use a SILU (Sigmoid Linear Unit) approximation is provided for this case. - -###PointToPlaneFactor.h -The PointToPlaneFactor constrains a gtsam::Pose3 using a point detection in the sensor frame compared with a plane in the world frame. - -### RobustSmartProjectionFactor -The RobustSmartProjectionFactor adds to the gtsam::SmartProjectionFactor by providing a robust huber kernel. Additionally, it fixes some issues in the SmartProjectionFactor allowing for a rotation-only fallback when using the JacobianSVD option and allows for proper serialization of the factor. - -## NodeUpdaters -### CombinedNavStateNodeUpdater -The CombinedNavStateNodeUpdater is responsible for inserting and removing CombinedNavState nodes for the graph. It links nodes using IMU preintegration factors (gtsam::CombinedIMUFactor). If a node to be created's timestamp is between two existing nodes, the IMU factor linking the two existing nodes is split and the new node is inserted between them. The CombinedNavStateNodeUpdater uses the LatestImuIntegrator object to store and utilize IMU measurements. When sliding the window, old nodes are removed from the graph and prior factors are created for the new oldest node using the node's covariances from the most recent round of optimization if the add_priors option is enabled. - -### FeaturePointNodeUpdater -The FeaturePointNodeUpdater maintains feature point nodes for image features. It removes old point nodes that are no longer being tracked by the graph. Creation of point nodes occurs in this case in the ProjectionGraphActionCompleter which uses each measurement of a feature track to triangulate a new point node. (TODO(rsoussan): Update this when this is changed) - -## Other -### FeatureTracker -The FeatureTracker maintains feature tracks for image based measurements. Feature tracks can be queried using feature ids or by returning the N longest feature tracks. The feature tracker also slides the window for feature tracks to remove old measurements if necessary. - -### Sanity Checker -The SanityChecker validates the current localization pose using a reference pose or by checking that the covariances are within defined bounds. -### GraphLocalizerInitializer -The GraphLocalizerInitializer is responsible for loading GraphLocalizer parameters. It also stores the starting pose for the localizer and estimates initial IMU biases using a set of N consecutive measurements at standstill. - -### GraphLocalizerWrapper -The GraphLocalizerWrapper contains the GraphLocalizer, GraphLocalizerInitializer, and SanityChecker. It passes ROS messages to each of these and also provides interfaces for accessing some GraphLocalizer data, such as messages built using GraphLocalizer values, estimates of the dock and handrail poses wrt the world frame, and more. - -### GraphLocalizerNodelet -The GraphLocalizerNodlet subscribes to ROS messages for online use and publishes GraphLocalizer messages and TFs. - -# Inputs -* `/loc/of/features` -* `/loc/ml/features` -* `/loc/ar/features` -* `/loc/handrail/features` -* `/hw/imu` - -# Outputs -* `graph_loc/graph` -* `graph_loc/state` +For more information on the theory behind the GraphLocalizer and the factors used, please see our paper: +* Ryan Soussan, Varsha Kumar, Brian Coltin, and Trey Smith, "Astroloc: An efficient and robust localizer for a free-flying robot", Int. Conf. on Robotics and Automation (ICRA), 2022. [Link](https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9811919 "Link") + +# Graph Optimization Structure +<p align="center"> +<img src="../doc/images/graph_localizer.png" width="550"> +</p> + +## Factor Adders +* LocFactorAdder +## Graph Factors +* gtsam::BetweenFactor<gtsam::Pose3> +* LocPoseFactor +* LocProjectionFactor +## Node Adders +* PoseNodeAdder diff --git a/localization/graph_localizer/src/combined_nav_state_graph_values.cc b/localization/graph_localizer/src/combined_nav_state_graph_values.cc deleted file mode 100644 index 0b042dfc75..0000000000 --- a/localization/graph_localizer/src/combined_nav_state_graph_values.cc +++ /dev/null @@ -1,426 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/combined_nav_state_graph_values.h> -#include <localization_common/logger.h> - -#include <gtsam/base/Vector.h> -#include <gtsam/geometry/Pose3.h> -#include <gtsam/inference/Symbol.h> -#include <gtsam/navigation/ImuBias.h> -#include <gtsam/navigation/NavState.h> -#include <gtsam/slam/ProjectionFactor.h> - -#include <iomanip> - -namespace graph_localizer { -namespace go = graph_optimizer; -namespace lc = localization_common; -namespace lm = localization_measurements; -CombinedNavStateGraphValues::CombinedNavStateGraphValues(const CombinedNavStateGraphValuesParams& params, - std::shared_ptr<gtsam::Values> values) - : GraphValues(std::move(values)), params_(params) { - LogDebug("CombinedNavStateGraphValues: Window duration: " << params_.ideal_duration); - LogDebug("CombinedNavStateGraphValues: Window min num states: " << params_.min_num_states); -} - -const CombinedNavStateGraphValuesParams& CombinedNavStateGraphValues::params() const { return params_; } - -std::vector<lc::Time> CombinedNavStateGraphValues::Timestamps() const { - std::vector<lc::Time> timestamps; - for (const auto& timestamp_key_index_pair : timestamp_key_index_map_) { - timestamps.emplace_back(timestamp_key_index_pair.first); - } - return timestamps; -} - -boost::optional<lc::CombinedNavState> CombinedNavStateGraphValues::LatestCombinedNavState() const { - if (Empty()) { - LogError("LatestCombinedNavState: No combined nav states available."); - return boost::none; - } - - const lc::Time timestamp = timestamp_key_index_map_.crbegin()->first; - return GetCombinedNavState(timestamp); -} - -boost::optional<lc::CombinedNavState> CombinedNavStateGraphValues::OldestCombinedNavState() const { - if (Empty()) { - LogError("OldestCombinedNavState: No combined nav states available."); - return boost::none; - } - const lc::Time timestamp = timestamp_key_index_map_.cbegin()->first; - return GetCombinedNavState(timestamp); -} - -boost::optional<lc::Time> CombinedNavStateGraphValues::OldestTimestamp() const { - if (Empty()) { - LogError("OldestTimestamp: No states available."); - return boost::none; - } - return timestamp_key_index_map_.cbegin()->first; -} - -boost::optional<lc::Time> CombinedNavStateGraphValues::LatestTimestamp() const { - if (Empty()) { - LogError("LatestTimestamp: No states available."); - return boost::none; - } - return timestamp_key_index_map_.crbegin()->first; -} - -boost::optional<lc::Time> CombinedNavStateGraphValues::ClosestPoseTimestamp(const lc::Time timestamp) const { - if (Empty()) { - LogError("ClosestPoseTimestamp: No states available."); - return boost::none; - } - - const auto lower_and_upper_bound_timestamp = LowerAndUpperBoundTimestamp(timestamp); - if (!lower_and_upper_bound_timestamp.first && !lower_and_upper_bound_timestamp.second) { - LogError("ClosestPoseTimestamp: Failed to get lower or upper bound timestamp."); - return boost::none; - } - - lc::Time closest_timestamp; - if (!lower_and_upper_bound_timestamp.first) { - closest_timestamp = *(lower_and_upper_bound_timestamp.second); - } else if (!lower_and_upper_bound_timestamp.second) { - closest_timestamp = *(lower_and_upper_bound_timestamp.first); - } else { - const lc::Time lower_bound_timestamp = *(lower_and_upper_bound_timestamp.first); - const lc::Time upper_bound_timestamp = *(lower_and_upper_bound_timestamp.second); - const double upper_bound_dt = std::abs(timestamp - upper_bound_timestamp); - const double lower_bound_dt = std::abs(timestamp - lower_bound_timestamp); - closest_timestamp = (upper_bound_dt < lower_bound_dt) ? upper_bound_timestamp : lower_bound_timestamp; - } - - LogDebug("ClosestPoseTimestamp: dt is " << std::abs(timestamp - closest_timestamp)); - return closest_timestamp; -} - -std::pair<boost::optional<lc::Time>, boost::optional<lc::Time>> -CombinedNavStateGraphValues::LowerAndUpperBoundTimestamp(const lc::Time timestamp) const { - if (Empty()) { - LogError("LowerAndUpperBoundTimestamp: No states available."); - return {boost::none, boost::none}; - } - - // lower bound returns first it >= query, call this upper bound - const auto upper_bound_it = timestamp_key_index_map_.lower_bound(timestamp); - if (upper_bound_it == timestamp_key_index_map_.cend()) { - LogDebug("LowerAndUpperBoundTimestamp: No upper bound timestamp exists."); - const lc::Time lower_bound_time = (timestamp_key_index_map_.crbegin())->first; - return {boost::optional<lc::Time>(lower_bound_time), boost::none}; - } else if (upper_bound_it == timestamp_key_index_map_.cbegin()) { - LogDebug("LowerAndUpperBoundTimestamp: No lower bound timestamp exists."); - return {boost::none, boost::optional<lc::Time>(upper_bound_it->first)}; - } - const auto lower_bound_it = std::prev(upper_bound_it); - - return {lower_bound_it->first, upper_bound_it->first}; -} - -boost::optional<gtsam::Key> CombinedNavStateGraphValues::GetKey(go::KeyCreatorFunction key_creator_function, - const localization_common::Time timestamp) const { - if (timestamp_key_index_map_.count(timestamp) == 0) { - LogError("GetKey: No key index found at timestamp."); - return boost::none; - } - - const int key_index = timestamp_key_index_map_.at(timestamp); - - const auto key = key_creator_function(key_index); - if (!values().exists(key)) { - LogError("GetKey: Key not present in values."); - return boost::none; - } - - return key; -} - -bool CombinedNavStateGraphValues::HasKey(const lc::Time timestamp) const { - return (timestamp_key_index_map_.count(timestamp) != 0); -} - -bool CombinedNavStateGraphValues::Empty() const { return timestamp_key_index_map_.empty(); } - -boost::optional<gtsam::Key> CombinedNavStateGraphValues::PoseKey(const lc::Time timestamp) const { - return GetKey(&sym::P, timestamp); -} - -boost::optional<lc::CombinedNavState> CombinedNavStateGraphValues::GetCombinedNavState(const lc::Time timestamp) const { - if (!HasKey(timestamp)) { - LogError("GetCombinedNavState: No CombinedNavState found at timestamp."); - return boost::none; - } - - const auto key_index = KeyIndex(timestamp); - if (!key_index) { - LogError("GetCombinedNavState: Failed to get key index."); - return boost::none; - } - - const auto pose = at<gtsam::Pose3>(sym::P(*key_index)); - if (!pose) { - LogError("GetCombinedNavState: Failed to get pose for key index."); - return boost::none; - } - - const auto velocity = at<gtsam::Velocity3>(sym::V(*key_index)); - if (!velocity) { - LogError("GetCombinedNavState: Failed to get velocity for key index."); - return boost::none; - } - - const auto bias = at<gtsam::imuBias::ConstantBias>(sym::B(*key_index)); - if (!bias) { - LogError("GetCombinedNavState: Failed to get bias for key index."); - return boost::none; - } - - return lc::CombinedNavState{*pose, *velocity, *bias, timestamp}; -} - -double CombinedNavStateGraphValues::Duration() const { - if (Empty()) return 0; - return (*LatestTimestamp() - *OldestTimestamp()); -} - -int CombinedNavStateGraphValues::NumStates() const { return timestamp_key_index_map_.size(); } - -boost::optional<lc::Time> CombinedNavStateGraphValues::Timestamp(const int key_index) const { - for (const auto& timestamp_key_index_pair : timestamp_key_index_map_) { - if (timestamp_key_index_pair.second == key_index) return timestamp_key_index_pair.first; - } - return boost::none; -} - -boost::optional<lc::Time> CombinedNavStateGraphValues::Timestamp( - graph_optimizer::KeyCreatorFunction key_creator_function, const gtsam::Key key) const { - for (const auto& timestamp_key_index_pair : timestamp_key_index_map_) { - if (key_creator_function(timestamp_key_index_pair.second) == key) return timestamp_key_index_pair.first; - } - return boost::none; -} - -boost::optional<int> CombinedNavStateGraphValues::LatestCombinedNavStateKeyIndex() const { - if (Empty()) { - LogError("LatestCombinedNavStateKeyIndex: No combined nav states available."); - return boost::none; - } - return timestamp_key_index_map_.crbegin()->second; -} - -boost::optional<int> CombinedNavStateGraphValues::OldestCombinedNavStateKeyIndex() const { - if (Empty()) { - LogError("OldestCombinedNavStateKeyIndex: No combined nav states available."); - return boost::none; - } - return timestamp_key_index_map_.cbegin()->second; -} - -boost::optional<std::pair<gtsam::imuBias::ConstantBias, lc::Time>> CombinedNavStateGraphValues::LatestBias() const { - if (Empty()) { - LogError("LatestBias: No bias values available."); - return boost::none; - } - - const lc::Time timestamp = timestamp_key_index_map_.crbegin()->first; - const int key_index = timestamp_key_index_map_.crbegin()->second; - - if (!values().exists(sym::B(key_index))) { - LogError("LatestBias: Bias key not present in values."); - return boost::none; - } - - const auto bias = at<gtsam::imuBias::ConstantBias>(sym::B(key_index)); - if (!bias) { - LogError("LatestBias: Failed to get bias at key index."); - return boost::none; - } - - return std::pair<gtsam::imuBias::ConstantBias, lc::Time>{*bias, timestamp}; -} - -boost::optional<lc::Time> CombinedNavStateGraphValues::LowerBoundOrEqualTimestamp(const lc::Time timestamp) const { - const auto lower_and_upper_bound_timestamp = LowerAndUpperBoundTimestamp(timestamp); - if (!lower_and_upper_bound_timestamp.first && !lower_and_upper_bound_timestamp.second) { - LogError("LowerBoundOrEqualTimestamp: Failed to get lower or upper bound timestamps."); - return boost::none; - } - - // Only return upper bound timestamp if it is equal to timestamp - if (lower_and_upper_bound_timestamp.second && *(lower_and_upper_bound_timestamp.second) == timestamp) { - return lower_and_upper_bound_timestamp.second; - } - - return lower_and_upper_bound_timestamp.first; -} - -boost::optional<lc::CombinedNavState> CombinedNavStateGraphValues::LowerBoundOrEqualCombinedNavState( - const lc::Time timestamp) const { - const auto lower_bound_or_equal_timestamp = LowerBoundOrEqualTimestamp(timestamp); - if (!lower_bound_or_equal_timestamp) { - LogDebug("LowerBoundOrEqualCombinedNavState: Failed to get lower bound or equal timestamp."); - return boost::none; - } - - return GetCombinedNavState(*lower_bound_or_equal_timestamp); -} - -boost::optional<lc::Time> CombinedNavStateGraphValues::SlideWindowNewOldestTime() const { - if (Empty()) { - LogDebug("SlideWindowOldestTime: No states in map."); - return boost::none; - } - - if (NumStates() <= params().min_num_states) { - LogDebug("SlideWindowOldestTime: Not enough states to remove."); - return boost::none; - } - - const double total_duration = timestamp_key_index_map_.crbegin()->first - timestamp_key_index_map_.cbegin()->first; - LogDebug("SlideWindowOldestTime: Starting total num states: " << timestamp_key_index_map_.size()); - LogDebug("SlideWindowOldestTime: Starting total duration is " << total_duration); - const lc::Time ideal_oldest_allowed_state = - std::max(0.0, timestamp_key_index_map_.crbegin()->first - params().ideal_duration); - - int num_states_to_be_removed = 0; - // Ensures that new oldest time is consistent with a number of states <= max_num_states - // and >= min_num_states. - // Assumes min_num_states < max_num_states. - for (const auto& timestamp_key_pair : timestamp_key_index_map_) { - ++num_states_to_be_removed; - const int new_num_states = NumStates() - num_states_to_be_removed; - if (new_num_states > params().max_num_states) continue; - const auto& time = timestamp_key_pair.first; - if (new_num_states <= params().min_num_states) return time; - if (time >= ideal_oldest_allowed_state) return time; - } - - // Shouldn't occur - return boost::none; -} - -// Add timestamp and keys to timestamp_key_index_map, and values to values -bool CombinedNavStateGraphValues::AddCombinedNavState(const lc::CombinedNavState& combined_nav_state, - const int key_index) { - if (HasKey(combined_nav_state.timestamp())) { - LogError( - "AddCombinedNavState: Timestamp key index map already " - "contains timestamp."); - return false; - } - timestamp_key_index_map_.emplace(combined_nav_state.timestamp(), key_index); - if (values().exists(sym::P(key_index))) { - LogError("AddCombinedNavState: Pose key already in values."); - return false; - } - if (values().exists(sym::V(key_index))) { - LogError("AddCombinedNavState: Velocity key already in values."); - return false; - } - if (values().exists(sym::B(key_index))) { - LogError("AddCombinedNavState: Bias key already in values."); - return false; - } - - values().insert(sym::P(key_index), combined_nav_state.pose()); - values().insert(sym::V(key_index), combined_nav_state.velocity()); - values().insert(sym::B(key_index), combined_nav_state.bias()); - - LogDebug("AddCombinedNavState: Added key_index " << key_index); - LogDebug("AddCombinedNavState: Added timestamp " << std::setprecision(15) << combined_nav_state.timestamp()); - return true; -} - -boost::optional<int> CombinedNavStateGraphValues::KeyIndex(const lc::Time timestamp) const { - if (!HasKey(timestamp)) { - LogError("KeyIndex: No key found for timestamp."); - return boost::none; - } - - return timestamp_key_index_map_.at(timestamp); -} - -int CombinedNavStateGraphValues::RemoveOldCombinedNavStates(const lc::Time oldest_allowed_time) { - int num_states_removed = 0; - while (timestamp_key_index_map_.begin()->first < oldest_allowed_time) { - RemoveCombinedNavState(timestamp_key_index_map_.begin()->first); - ++num_states_removed; - } - LogDebug("RemoveOldCombinedNavStates: New total num states: " << timestamp_key_index_map_.size()); - const double new_total_duration = - timestamp_key_index_map_.crbegin()->first - timestamp_key_index_map_.cbegin()->first; - LogDebug("RemoveOldCombinedNavStates: New total duration is " << new_total_duration); - LogDebug("RemoveOldCombinedNavStates: Num states removed: " << num_states_removed); - return num_states_removed; -} - -gtsam::KeyVector CombinedNavStateGraphValues::OldKeys(const lc::Time oldest_allowed_time, - const gtsam::NonlinearFactorGraph& graph) const { - gtsam::KeyVector old_keys; - for (const auto& timestamp_key_index_pair : timestamp_key_index_map_) { - if (timestamp_key_index_pair.first >= oldest_allowed_time) break; - const auto& key_index = timestamp_key_index_pair.second; - old_keys.emplace_back(sym::P(key_index)); - old_keys.emplace_back(sym::V(key_index)); - old_keys.emplace_back(sym::B(key_index)); - } - - return old_keys; -} - -// Removes keys from timestamp_key_index_map, values from values -// Assumes for each stamped_key_index there is a Pose, Velocity, and Bias key -bool CombinedNavStateGraphValues::RemoveCombinedNavState(const lc::Time timestamp) { - if (!HasKey(timestamp)) { - LogError( - "RemoveCombinedNavState: Timestamp not found in timestamp " - "key index map."); - return false; - } - const int key_index = timestamp_key_index_map_.at(timestamp); - timestamp_key_index_map_.erase(timestamp); - bool removed_values = true; - - // Remove key/value pairs from values - if (values().exists(sym::P(key_index))) { - values().erase(sym::P(key_index)); - } else { - LogError("RemoveCombinedNavState: Pose key not present in values."); - removed_values = false; - } - if (values().exists(sym::V(key_index))) { - values().erase(sym::V(key_index)); - } else { - LogError("RemoveCombinedNavState: Velocity key not present in values."); - removed_values = false; - } - if (values().exists(sym::B(key_index))) { - values().erase(sym::B(key_index)); - } else { - LogError("RemoveCombinedNavState: Bias key not present in values."); - removed_values = false; - } - - LogDebug("RemoveCombinedNavState: Removed key index " << key_index); - LogDebug("RemoveCombinedNavState: Removed timestamp" << std::setprecision(15) << timestamp); - return removed_values; -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/combined_nav_state_node_updater.cc b/localization/graph_localizer/src/combined_nav_state_node_updater.cc deleted file mode 100644 index 981f8304f8..0000000000 --- a/localization/graph_localizer/src/combined_nav_state_node_updater.cc +++ /dev/null @@ -1,402 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/combined_nav_state_node_updater.h> -#include <graph_localizer/loc_pose_factor.h> -#include <graph_localizer/utilities.h> -#include <imu_integration/utilities.h> - -#include <gtsam/geometry/Pose3.h> -#include <gtsam/inference/Symbol.h> -#include <gtsam/navigation/ImuBias.h> -#include <gtsam/navigation/NavState.h> -#include <gtsam/slam/PriorFactor.h> - -namespace graph_localizer { -namespace go = graph_optimizer; -namespace ii = imu_integration; -namespace lc = localization_common; -namespace sym = gtsam::symbol_shorthand; -CombinedNavStateNodeUpdater::CombinedNavStateNodeUpdater( - const CombinedNavStateNodeUpdaterParams& params, - std::shared_ptr<imu_integration::LatestImuIntegrator> latest_imu_integrator, std::shared_ptr<gtsam::Values> values) - : params_(params), - latest_imu_integrator_(std::move(latest_imu_integrator)), - graph_values_(new CombinedNavStateGraphValues(params.graph_values, std::move(values))), - key_index_(0) { - const gtsam::Vector6 pose_prior_noise_sigmas( - (gtsam::Vector(6) << params_.starting_prior_translation_stddev, params_.starting_prior_translation_stddev, - params_.starting_prior_translation_stddev, params_.starting_prior_quaternion_stddev, - params_.starting_prior_quaternion_stddev, params_.starting_prior_quaternion_stddev) - .finished()); - const gtsam::Vector3 velocity_prior_noise_sigmas((gtsam::Vector(3) << params_.starting_prior_velocity_stddev, - params_.starting_prior_velocity_stddev, - params_.starting_prior_velocity_stddev) - .finished()); - const gtsam::Vector6 bias_prior_noise_sigmas( - (gtsam::Vector(6) << params_.starting_prior_accel_bias_stddev, params_.starting_prior_accel_bias_stddev, - params_.starting_prior_accel_bias_stddev, params_.starting_prior_gyro_bias_stddev, - params_.starting_prior_gyro_bias_stddev, params_.starting_prior_gyro_bias_stddev) - .finished()); - global_N_body_start_noise_.pose_noise = Robust( - gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(pose_prior_noise_sigmas)), params_.huber_k); - global_N_body_start_noise_.velocity_noise = - Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(velocity_prior_noise_sigmas)), - params_.huber_k); - global_N_body_start_noise_.bias_noise = Robust( - gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(bias_prior_noise_sigmas)), params_.huber_k); -} - -void CombinedNavStateNodeUpdater::AddInitialValuesAndPriors(gtsam::NonlinearFactorGraph& factors) { - AddInitialValuesAndPriors(params_.global_N_body_start, global_N_body_start_noise_, factors); -} - -void CombinedNavStateNodeUpdater::AddInitialValuesAndPriors(const lc::CombinedNavState& global_N_body, - const lc::CombinedNavStateNoise& noise, - gtsam::NonlinearFactorGraph& factors) { - const int key_index = GenerateKeyIndex(); - graph_values_->AddCombinedNavState(global_N_body, key_index); - AddPriors(global_N_body, noise, factors); -} - -void CombinedNavStateNodeUpdater::AddPriors(const lc::CombinedNavState& global_N_body, - const lc::CombinedNavStateNoise& noise, - gtsam::NonlinearFactorGraph& factors) { - const auto key_index = graph_values_->KeyIndex(global_N_body.timestamp()); - if (!key_index) { - LogError("AddPriors: Failed to get key index."); - return; - } - // TODO(rsoussan): Ensure symbols not used by other node updaters - gtsam::PriorFactor<gtsam::Pose3> pose_prior_factor(sym::P(*key_index), global_N_body.pose(), noise.pose_noise); - factors.push_back(pose_prior_factor); - gtsam::PriorFactor<gtsam::Velocity3> velocity_prior_factor(sym::V(*key_index), global_N_body.velocity(), - noise.velocity_noise); - factors.push_back(velocity_prior_factor); - gtsam::PriorFactor<gtsam::imuBias::ConstantBias> bias_prior_factor(sym::B(*key_index), global_N_body.bias(), - noise.bias_noise); - factors.push_back(bias_prior_factor); -} - -bool CombinedNavStateNodeUpdater::SlideWindow(const lc::Time oldest_allowed_timestamp, - const boost::optional<gtsam::Marginals>& marginals, - const gtsam::KeyVector& old_keys, const double huber_k, - gtsam::NonlinearFactorGraph& factors) { - graph_values_->RemoveOldCombinedNavStates(oldest_allowed_timestamp); - if (params_.add_priors) { - // Add prior to oldest nav state using covariances from last round of - // optimization - const auto global_N_body_oldest = graph_values_->OldestCombinedNavState(); - if (!global_N_body_oldest) { - LogError("SlideWindow: Failed to get oldest combined nav state."); - return false; - } - - LogDebug("SlideWindow: Oldest state time: " << global_N_body_oldest->timestamp()); - - const auto key_index = graph_values_->OldestCombinedNavStateKeyIndex(); - if (!key_index) { - LogError("SlideWindow: Failed to get oldest combined nav state key index."); - return false; - } - - LogDebug("SlideWindow: key index: " << *key_index); - - // Make sure priors are removed before adding new ones - RemovePriors(*key_index, factors); - if (marginals) { - lc::CombinedNavStateNoise noise; - noise.pose_noise = - Robust(gtsam::noiseModel::Gaussian::Covariance(marginals->marginalCovariance(sym::P(*key_index))), huber_k); - noise.velocity_noise = - Robust(gtsam::noiseModel::Gaussian::Covariance(marginals->marginalCovariance(sym::V(*key_index))), huber_k); - auto bias_covariance = marginals->marginalCovariance(sym::B(*key_index)); - if (params_.threshold_bias_uncertainty) ThresholdBiasUncertainty(bias_covariance); - noise.bias_noise = Robust(gtsam::noiseModel::Gaussian::Covariance(bias_covariance), huber_k); - AddPriors(*global_N_body_oldest, noise, factors); - } else { - // TODO(rsoussan): Add seperate marginal fallback sigmas instead of relying on starting prior sigmas - AddPriors(*global_N_body_oldest, global_N_body_start_noise_, factors); - } - } - - return true; -} - -void CombinedNavStateNodeUpdater::ThresholdBiasUncertainty(gtsam::Matrix& bias_covariance) const { - // Only checking sigmas for now - const auto bias_covariance_sigmas = bias_covariance.diagonal().cwiseSqrt(); - bool valid_sigmas = true; - for (int i = 0; i < 3; ++i) { - if (bias_covariance_sigmas[i] > params_.accel_bias_stddev_threshold) { - valid_sigmas = false; - break; - } - } - if (valid_sigmas) { - for (int i = 3; i < 6; ++i) { - if (bias_covariance_sigmas[i] > params_.gyro_bias_stddev_threshold) { - valid_sigmas = false; - break; - } - } - } - if (valid_sigmas) return; - gtsam::Vector6 new_sigmas; - for (int i = 0; i < 3; ++i) { - new_sigmas[i] = std::min(bias_covariance_sigmas[i], params_.accel_bias_stddev_threshold); - } - for (int i = 3; i < 6; ++i) { - new_sigmas[i] = std::min(bias_covariance_sigmas[i], params_.gyro_bias_stddev_threshold); - } - const gtsam::Vector6 new_variances = new_sigmas.cwiseAbs2(); - bias_covariance = new_variances.asDiagonal(); -} - -go::NodeUpdaterType CombinedNavStateNodeUpdater::type() const { return go::NodeUpdaterType::CombinedNavState; } - -boost::optional<lc::Time> CombinedNavStateNodeUpdater::SlideWindowNewOldestTime() const { - return graph_values_->SlideWindowNewOldestTime(); -} - -gtsam::KeyVector CombinedNavStateNodeUpdater::OldKeys(const lc::Time oldest_allowed_time, - const gtsam::NonlinearFactorGraph& graph) const { - return graph_values_->OldKeys(oldest_allowed_time, graph); -} - -boost::optional<gtsam::Key> CombinedNavStateNodeUpdater::GetKey(go::KeyCreatorFunction key_creator_function, - const lc::Time timestamp) const { - return graph_values_->GetKey(key_creator_function, timestamp); -} - -boost::optional<lc::Time> CombinedNavStateNodeUpdater::OldestTimestamp() const { - return graph_values_->OldestTimestamp(); -} - -boost::optional<lc::Time> CombinedNavStateNodeUpdater::LatestTimestamp() const { - return graph_values_->LatestTimestamp(); -} - -std::shared_ptr<const CombinedNavStateGraphValues> CombinedNavStateNodeUpdater::shared_graph_values() const { - return graph_values_; -} - -std::shared_ptr<CombinedNavStateGraphValues> CombinedNavStateNodeUpdater::shared_graph_values() { - return graph_values_; -} - -const CombinedNavStateGraphValues& CombinedNavStateNodeUpdater::graph_values() const { return *graph_values_; } - -void CombinedNavStateNodeUpdater::RemovePriors(const int key_index, gtsam::NonlinearFactorGraph& factors) { - int removed_factors = 0; - for (auto factor_it = factors.begin(); factor_it != factors.end();) { - bool erase_factor = false; - const auto pose_prior_factor = dynamic_cast<gtsam::PriorFactor<gtsam::Pose3>*>(factor_it->get()); - const auto loc_pose_factor = dynamic_cast<gtsam::LocPoseFactor*>(factor_it->get()); - if (pose_prior_factor && !loc_pose_factor && pose_prior_factor->key() == sym::P(key_index)) { - erase_factor = true; - } - const auto velocity_prior_factor = dynamic_cast<gtsam::PriorFactor<gtsam::Velocity3>*>(factor_it->get()); - if (velocity_prior_factor && velocity_prior_factor->key() == sym::V(key_index)) { - erase_factor = true; - } - const auto bias_prior_factor = dynamic_cast<gtsam::PriorFactor<gtsam::imuBias::ConstantBias>*>(factor_it->get()); - if (bias_prior_factor && bias_prior_factor->key() == sym::B(key_index)) { - erase_factor = true; - } - - if (erase_factor) { - factor_it = factors.erase(factor_it); - ++removed_factors; - } else { - ++factor_it; - continue; - } - } - LogDebug("RemovePriors: Erase " << removed_factors << " factors."); -} - -int CombinedNavStateNodeUpdater::GenerateKeyIndex() { return key_index_++; } - -bool CombinedNavStateNodeUpdater::Update(const lc::Time timestamp, gtsam::NonlinearFactorGraph& factors) { - return AddOrSplitImuFactorIfNeeded(timestamp, factors, *graph_values_); -} - -bool CombinedNavStateNodeUpdater::AddOrSplitImuFactorIfNeeded(const lc::Time timestamp, - gtsam::NonlinearFactorGraph& factors, - CombinedNavStateGraphValues& graph_values) { - if (graph_values.HasKey(timestamp)) { - LogDebug( - "AddOrSplitImuFactorIfNeeded: CombinedNavState exists at " - "timestamp, nothing to do."); - return true; - } - - const auto latest_timestamp = graph_values.LatestTimestamp(); - if (!latest_timestamp) { - LogError("AddOrSplitImuFactorIfNeeded: Failed to get latest timestamp."); - return false; - } - - if (timestamp > *latest_timestamp) { - LogDebug( - "AddOrSplitImuFactorIfNeeded: Creating and adding latest imu " - "factor and nav state."); - return CreateAndAddLatestImuFactorAndCombinedNavState(timestamp, factors, graph_values); - } else { - LogDebug("AddOrSplitImuFactorIfNeeded: Splitting old imu factor."); - return SplitOldImuFactorAndAddCombinedNavState(timestamp, factors, graph_values); - } -} - -bool CombinedNavStateNodeUpdater::CreateAndAddLatestImuFactorAndCombinedNavState( - const lc::Time timestamp, gtsam::NonlinearFactorGraph& factors, CombinedNavStateGraphValues& graph_values) { - if (!latest_imu_integrator_->IntegrateLatestImuMeasurements(timestamp)) { - LogError("CreateAndAddLatestImuFactorAndCombinedNavState: Failed to integrate latest imu measurements."); - return false; - } - - const auto latest_combined_nav_state = graph_values.LatestCombinedNavState(); - if (!latest_combined_nav_state) { - LogError("CreateAndAddLatestImuFactorAndCombinedNavState: Failed to get latest combined nav state."); - return false; - } - if (!CreateAndAddImuFactorAndPredictedCombinedNavState(*latest_combined_nav_state, latest_imu_integrator_->pim(), - factors, graph_values)) { - LogError("CreateAndAddLatestImuFactorAndCombinedNavState: Failed to create and add imu factor."); - return false; - } - - const auto latest_bias = graph_values.LatestBias(); - if (!latest_bias) { - LogError("CreateAndAddLatestImuFactorAndCombinedNavState: Failed to get latest bias."); - return false; - } - - latest_imu_integrator_->ResetPimIntegrationAndSetBias(latest_bias->first); - return true; -} - -bool CombinedNavStateNodeUpdater::CreateAndAddImuFactorAndPredictedCombinedNavState( - const lc::CombinedNavState& global_N_body, const gtsam::PreintegratedCombinedMeasurements& pim, - gtsam::NonlinearFactorGraph& factors, CombinedNavStateGraphValues& graph_values) { - const auto key_index_0 = graph_values.KeyIndex(global_N_body.timestamp()); - if (!key_index_0) { - LogError("CreateAndAddImuFactorAndPredictedCombinedNavState: Failed to get first key index."); - return false; - } - - const lc::CombinedNavState global_N_body_predicted = ii::PimPredict(global_N_body, pim); - const int key_index_1 = GenerateKeyIndex(); - const auto combined_imu_factor = ii::MakeCombinedImuFactor(*key_index_0, key_index_1, pim); - factors.push_back(combined_imu_factor); - graph_values.AddCombinedNavState(global_N_body_predicted, key_index_1); - return true; -} - -bool CombinedNavStateNodeUpdater::SplitOldImuFactorAndAddCombinedNavState(const lc::Time timestamp, - gtsam::NonlinearFactorGraph& factors, - CombinedNavStateGraphValues& graph_values) { - const auto timestamp_bounds = graph_values.LowerAndUpperBoundTimestamp(timestamp); - if (!timestamp_bounds.first || !timestamp_bounds.second) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to get upper and lower bound timestamp."); - return false; - } - - const lc::Time lower_bound_time = *(timestamp_bounds.first); - const lc::Time upper_bound_time = *(timestamp_bounds.second); - - if (timestamp < lower_bound_time || timestamp > upper_bound_time) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Timestamp is not within bounds of existing timestamps."); - return false; - } - - const auto lower_bound_key_index = graph_values.KeyIndex(lower_bound_time); - const auto upper_bound_key_index = graph_values.KeyIndex(upper_bound_time); - if (!lower_bound_key_index || !upper_bound_key_index) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to get lower and upper bound key indices."); - return false; - } - - // get old imu factor, delete it - bool removed_old_imu_factor = false; - for (auto factor_it = factors.begin(); factor_it != factors.end();) { - if (dynamic_cast<gtsam::CombinedImuFactor*>(factor_it->get()) && - graph_values.ContainsCombinedNavStateKey(**factor_it, *lower_bound_key_index) && - graph_values.ContainsCombinedNavStateKey(**factor_it, *upper_bound_key_index)) { - factors.erase(factor_it); - removed_old_imu_factor = true; - break; - } - ++factor_it; - } - if (!removed_old_imu_factor) { - LogError( - "SplitOldImuFactorAndAddCombinedNavState: Failed to remove " - "old imu factor."); - return false; - } - - const auto lower_bound_bias = graph_values.at<gtsam::imuBias::ConstantBias>(sym::B(*lower_bound_key_index)); - if (!lower_bound_bias) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to get lower bound bias."); - return false; - } - - // Add first factor and new nav state at timestamp - auto first_integrated_pim = latest_imu_integrator_->IntegratedPim(*lower_bound_bias, lower_bound_time, timestamp, - latest_imu_integrator_->pim_params()); - if (!first_integrated_pim) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to create first integrated pim."); - return false; - } - - const auto lower_bound_combined_nav_state = graph_values.GetCombinedNavState(lower_bound_time); - if (!lower_bound_combined_nav_state) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to get lower bound combined nav state."); - return false; - } - - if (!CreateAndAddImuFactorAndPredictedCombinedNavState(*lower_bound_combined_nav_state, *first_integrated_pim, - factors, graph_values)) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to create and add imu factor."); - return false; - } - - // Add second factor, use lower_bound_bias as starting bias since that is the - // best estimate available - auto second_integrated_pim = latest_imu_integrator_->IntegratedPim(*lower_bound_bias, timestamp, upper_bound_time, - latest_imu_integrator_->pim_params()); - if (!second_integrated_pim) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to create second integrated pim."); - return false; - } - - // New nav state already added so just get its key index - const auto new_key_index = graph_values.KeyIndex(timestamp); - if (!new_key_index) { - LogError("SplitOldImuFactorAndAddCombinedNavState: Failed to get new key index."); - return false; - } - - const auto combined_imu_factor = - ii::MakeCombinedImuFactor(*new_key_index, *upper_bound_key_index, *second_integrated_pim); - factors.push_back(combined_imu_factor); - return true; -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/depth_odometry_factor_adder.cc b/localization/graph_localizer/src/depth_odometry_factor_adder.cc deleted file mode 100644 index 0a602e80e1..0000000000 --- a/localization/graph_localizer/src/depth_odometry_factor_adder.cc +++ /dev/null @@ -1,99 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/depth_odometry_factor_adder.h> -#include <graph_localizer/point_to_point_between_factor.h> -#include <graph_localizer/utilities.h> -#include <localization_common/logger.h> -#include <localization_common/utilities.h> - -namespace graph_localizer { -namespace go = graph_optimizer; -namespace lc = localization_common; -namespace lm = localization_measurements; -namespace sym = gtsam::symbol_shorthand; -DepthOdometryFactorAdder::DepthOdometryFactorAdder(const DepthOdometryFactorAdderParams& params) - : DepthOdometryFactorAdder::Base(params) {} - -std::vector<go::FactorsToAdd> DepthOdometryFactorAdder::AddFactors( - const lm::DepthOdometryMeasurement& depth_odometry_measurement) { - std::vector<go::FactorsToAdd> factors_to_add_vector; - if (!lc::PoseCovarianceSane(depth_odometry_measurement.odometry.sensor_F_source_T_target.covariance, - params().position_covariance_threshold, params().orientation_covariance_threshold)) { - LogWarning("AddFactors: Sanity check failed - invalid covariance."); - return factors_to_add_vector; - } - - if (params().use_points_between_factor) { - go::FactorsToAdd points_between_factors_to_add; - const go::KeyInfo source_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, - depth_odometry_measurement.odometry.source_time); - const go::KeyInfo target_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, - depth_odometry_measurement.odometry.target_time); - for (int i = 0; i < depth_odometry_measurement.correspondences.source_3d_points.size() && - points_between_factors_to_add.size() < params().max_num_points_between_factors; - ++i) { - const Eigen::Vector3d& sensor_t_point_source = depth_odometry_measurement.correspondences.source_3d_points[i]; - const Eigen::Vector3d& sensor_t_point_target = depth_odometry_measurement.correspondences.target_3d_points[i]; - - const Eigen::Vector3d estimate_error = - sensor_t_point_source - - depth_odometry_measurement.odometry.sensor_F_source_T_target.pose * sensor_t_point_target; - const double estimate_error_norm = estimate_error.norm(); - if (estimate_error_norm > params().point_to_point_error_threshold) continue; - const auto points_between_factor_noise = - Robust(gtsam::noiseModel::Diagonal::Sigmas(estimate_error * params().noise_scale), params().huber_k); - gtsam::PointToPointBetweenFactor::shared_ptr points_between_factor(new gtsam::PointToPointBetweenFactor( - sensor_t_point_source, sensor_t_point_target, params().body_T_sensor, points_between_factor_noise, - source_key_info.UninitializedKey(), target_key_info.UninitializedKey())); - points_between_factors_to_add.push_back({{source_key_info, target_key_info}, points_between_factor}); - } - points_between_factors_to_add.SetTimestamp(depth_odometry_measurement.odometry.target_time); - LogDebug("AddFactors: Added " << points_between_factors_to_add.size() << " points between factors."); - factors_to_add_vector.emplace_back(points_between_factors_to_add); - } else { - go::FactorsToAdd relative_pose_factors_to_add; - relative_pose_factors_to_add.reserve(1); - - const double translation_norm = - depth_odometry_measurement.odometry.sensor_F_source_T_target.pose.translation().norm(); - if (translation_norm > params().pose_translation_norm_threshold) { - LogDebug("AddFactors: Ignoring pose with large translation norm. Norm: " - << translation_norm << ", threshold: " << params().pose_translation_norm_threshold); - return factors_to_add_vector; - } - - const auto relative_pose_noise = - Robust(gtsam::noiseModel::Gaussian::Covariance( - depth_odometry_measurement.odometry.body_F_source_T_target.covariance * params().noise_scale, false), - params().huber_k); - const go::KeyInfo source_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, - depth_odometry_measurement.odometry.source_time); - const go::KeyInfo target_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, - depth_odometry_measurement.odometry.target_time); - gtsam::BetweenFactor<gtsam::Pose3>::shared_ptr relative_pose_factor(new gtsam::BetweenFactor<gtsam::Pose3>( - source_key_info.UninitializedKey(), target_key_info.UninitializedKey(), - lc::GtPose(depth_odometry_measurement.odometry.body_F_source_T_target.pose), relative_pose_noise)); - relative_pose_factors_to_add.push_back({{source_key_info, target_key_info}, relative_pose_factor}); - relative_pose_factors_to_add.SetTimestamp(depth_odometry_measurement.odometry.target_time); - LogDebug("AddFactors: Added 1 relative pose factor."); - factors_to_add_vector.emplace_back(relative_pose_factors_to_add); - } - return factors_to_add_vector; -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/feature_point_graph_values.cc b/localization/graph_localizer/src/feature_point_graph_values.cc deleted file mode 100644 index 471272b520..0000000000 --- a/localization/graph_localizer/src/feature_point_graph_values.cc +++ /dev/null @@ -1,121 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/feature_point_graph_values.h> -#include <localization_common/logger.h> - -#include <gtsam/base/Vector.h> -#include <gtsam/inference/Symbol.h> -#include <gtsam/slam/ProjectionFactor.h> - -#include <iomanip> - -namespace graph_localizer { -namespace go = graph_optimizer; -namespace lc = localization_common; -namespace lm = localization_measurements; -FeaturePointGraphValues::FeaturePointGraphValues(std::shared_ptr<gtsam::Values> values) - : go::GraphValues(std::move(values)), feature_key_index_(0) {} - -boost::optional<gtsam::Key> FeaturePointGraphValues::GetKey(go::KeyCreatorFunction key_creator_function, - const localization_common::Time timestamp) const { - return boost::none; -} - -boost::optional<lc::Time> FeaturePointGraphValues::OldestTimestamp() const { return boost::none; } - -boost::optional<lc::Time> FeaturePointGraphValues::LatestTimestamp() const { return boost::none; } - -boost::optional<lc::Time> FeaturePointGraphValues::SlideWindowNewOldestTime() const { return boost::none; } - -bool FeaturePointGraphValues::HasFeature(const lm::FeatureId id) const { return (feature_id_key_map_.count(id) > 0); } - -boost::optional<gtsam::Key> FeaturePointGraphValues::FeatureKey(const lm::FeatureId id) const { - if (!HasFeature(id)) return boost::none; - return feature_id_key_map_.at(id); -} - -gtsam::Key FeaturePointGraphValues::CreateFeatureKey() const { return sym::F(++feature_key_index_); } - -gtsam::KeyVector FeaturePointGraphValues::FeatureKeys() const { - gtsam::KeyVector feature_keys; - for (const auto& feature_id_key_pair : feature_id_key_map_) { - feature_keys.emplace_back(feature_id_key_pair.second); - } - return feature_keys; -} - -bool FeaturePointGraphValues::AddFeature(const lm::FeatureId id, const gtsam::Point3& feature_point, - const gtsam::Key& key) { - if (HasFeature(id)) { - LogError("AddFeature: Feature already exists."); - return false; - } - - if (values().exists(key)) { - LogError("AddFeature: Key already exists in values."); - } - - feature_id_key_map_.emplace(id, key); - values().insert(key, feature_point); - return true; -} - -int FeaturePointGraphValues::NumFeatures() const { return feature_id_key_map_.size(); } - -gtsam::KeyVector FeaturePointGraphValues::OldKeys(const localization_common::Time oldest_allowed_time, - const gtsam::NonlinearFactorGraph& factors) const { - using ProjectionFactor = gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3>; - gtsam::KeyVector old_features; - for (const auto& feature_id_key_pair : feature_id_key_map_) { - const auto& key = feature_id_key_pair.second; - int num_factors = 0; - for (const auto& factor : factors) { - // Only consider projection factors for min num feature factors - const auto projection_factor = dynamic_cast<const ProjectionFactor*>(factor.get()); - if (!projection_factor) continue; - if (factor->find(key) != factor->end()) { - ++num_factors; - // if (num_factors >= params_.min_num_factors_per_feature) break; - } - } - - // if (num_factors < params_.min_num_factors_per_feature) { - if (num_factors <= 0) { - old_features.emplace_back(key); - } - } - return old_features; -} - -void FeaturePointGraphValues::RemoveOldFeatures(const gtsam::KeyVector& old_keys) { - for (const auto& key : old_keys) { - // TODO(rsoussan): test this - if (gtsam::Symbol(key).chr() != 'F') continue; - values().erase(key); - for (auto feature_id_key_it = feature_id_key_map_.begin(); feature_id_key_it != feature_id_key_map_.end();) { - if (feature_id_key_it->second == key) { - feature_id_key_it = feature_id_key_map_.erase(feature_id_key_it); - } else { - ++feature_id_key_it; - } - } - } -} - -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/feature_track.cc b/localization/graph_localizer/src/feature_track.cc deleted file mode 100644 index 8c7d92dd1c..0000000000 --- a/localization/graph_localizer/src/feature_track.cc +++ /dev/null @@ -1,128 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/feature_track.h> -#include <localization_common/logger.h> - -namespace graph_localizer { -namespace lc = localization_common; -namespace lm = localization_measurements; -FeatureTrack::FeatureTrack(const localization_measurements::FeatureId id) : id_(id) {} - -void FeatureTrack::AddMeasurement(const lc::Time timestamp, const lm::FeaturePoint& feature_point) { - points_.emplace(timestamp, feature_point); -} - -void FeatureTrack::RemoveOldMeasurements(const lc::Time oldest_allowed_timestamp) { - points_.erase(points_.begin(), points_.lower_bound(oldest_allowed_timestamp)); -} - -bool FeatureTrack::HasMeasurement(const lc::Time timestamp) { return (points_.count(timestamp) > 0); } - -const FeatureTrack::Points& FeatureTrack::points() const { return points_; } - -const localization_measurements::FeatureId& FeatureTrack::id() const { return id_; } - -size_t FeatureTrack::size() const { return points_.size(); } - -bool FeatureTrack::empty() const { return points_.empty(); } - -std::vector<lm::FeaturePoint> FeatureTrack::AllowedPoints(const std::set<lc::Time>& allowed_timestamps) const { - std::vector<lm::FeaturePoint> allowed_points; - // Start with oldest points - for (auto point_it = points_.begin(); point_it != points_.end(); ++point_it) { - if (allowed_timestamps.count(point_it->second.timestamp) <= 0) continue; - allowed_points.emplace_back(point_it->second); - } - return allowed_points; -} - -std::vector<lm::FeaturePoint> FeatureTrack::LatestPointsInWindow(const double duration) const { - std::vector<lm::FeaturePoint> latest_points; - const auto latest_timestamp = LatestTimestamp(); - if (!latest_timestamp) return {}; - const lc::Time oldest_allowed_time = *latest_timestamp - duration; - // Start with latest points - for (auto point_it = points_.rbegin(); point_it != points_.rend(); ++point_it) { - if (point_it->second.timestamp < oldest_allowed_time) break; - latest_points.push_back(point_it->second); - } - return latest_points; -} - -std::vector<lm::FeaturePoint> FeatureTrack::LatestPoints(const int spacing) const { - std::vector<lm::FeaturePoint> latest_points; - int i = 0; - // Start with latest points - for (auto point_it = points_.rbegin(); point_it != points_.rend(); ++point_it) { - if (i++ % (spacing + 1) != 0) continue; - latest_points.push_back(point_it->second); - } - return latest_points; -} - -bool FeatureTrack::SpacingFits(const int spacing, const int max_num_points) const { - // Since we include the latest point, the points included for spacing and max_num_points - // is 1 for the latest point plus a point at intervals of spacing + 1 for max_num_points - 1 (excludes latest point). - // 1 0 0 1 0 0 1 0 0 -> here a 1 indicates a point used, the first 1 is the latest point, and the spacing is 2. - // In this case if max_num_points <= 3 this suceeds and otherwise this fails as fewer than 3 points would be included - // with the desired spacing. - return ((spacing + 1) * (max_num_points - 1) + 1) <= static_cast<int>(size()); -} - -int FeatureTrack::MaxSpacing(const int max_num_points) const { - // Avoid divide by zero and other corner cases - if (max_num_points <= 1 || static_cast<int>(size()) <= 0) return 0; - // Derived using equation from SpacingFits - // (spacing + 1 ) * (max_num_points - 1) + 1 = size - // -> spacing = (size - max_num_points)/(max_num_points - 1) - return std::max(0, (static_cast<int>(size()) - max_num_points) / (max_num_points - 1)); -} - -int FeatureTrack::ClosestSpacing(const int ideal_spacing, const int ideal_max_num_points) const { - // Check Ideal Case - if (SpacingFits(ideal_spacing, ideal_max_num_points)) return ideal_spacing; - // Check too few points case - if (static_cast<int>(size()) <= ideal_max_num_points) return 0; - // Derive new optimal spacing for ideal_max_num_points - for (int spacing = ideal_spacing - 1; spacing >= 0; --spacing) { - if (SpacingFits(spacing, ideal_max_num_points)) return spacing; - } - return 0; -} - -boost::optional<lm::FeaturePoint> FeatureTrack::LatestPoint() const { - if (empty()) return boost::none; - return points_.crbegin()->second; -} - -boost::optional<lc::Time> FeatureTrack::PreviousTimestamp() const { - if (size() < 2) return boost::none; - return std::next(points_.crbegin())->first; -} - -boost::optional<lc::Time> FeatureTrack::LatestTimestamp() const { - if (empty()) return boost::none; - return points_.crbegin()->first; -} - -boost::optional<lc::Time> FeatureTrack::OldestTimestamp() const { - if (empty()) return boost::none; - return points_.cbegin()->first; -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/feature_tracker.cc b/localization/graph_localizer/src/feature_tracker.cc deleted file mode 100644 index 01582bc795..0000000000 --- a/localization/graph_localizer/src/feature_tracker.cc +++ /dev/null @@ -1,148 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/feature_tracker.h> -#include <localization_common/logger.h> - -namespace graph_localizer { -namespace lc = localization_common; -namespace lm = localization_measurements; -FeatureTracker::FeatureTracker(const FeatureTrackerParams& params) : params_(params) {} -void FeatureTracker::UpdateFeatureTracks(const lm::FeaturePoints& feature_points) { - if (feature_points.empty()) { - Clear(); - LogDebug("UpdateFeatureTracks: Removed all feature tracks."); - return; - } - - const int starting_num_feature_tracks = size(); - LogDebug("UpdateFeatureTracks: Starting num feature tracks: " << starting_num_feature_tracks); - for (const auto& feature_point : feature_points) { - AddOrUpdateTrack(feature_point); - } - const int post_add_num_feature_tracks = size(); - LogDebug("UpdateFeatureTracks: Added feature tracks: " << post_add_num_feature_tracks - starting_num_feature_tracks); - - // Remove features that weren't detected - const auto feature_points_timestamp = feature_points.front().timestamp; - RemoveUndetectedFeatures(feature_points_timestamp); - UpdateLengthMap(); - UpdateAllowList(feature_points.front().timestamp); - const int removed_num_feature_tracks = post_add_num_feature_tracks - size(); - LogDebug("UpdateFeatureTracks: Removed feature tracks: " << removed_num_feature_tracks); - LogDebug("UpdateFeatureTracks: Final total num feature tracks: " << size()); -} - -void FeatureTracker::UpdateAllowList(const lc::Time& timestamp) { - // Space out optical flow measurements for smart factor adder if necessary - static int measurement_count = 0; - if (measurement_count++ % (params_.smart_projection_adder_measurement_spacing + 1) != 0) return; - smart_factor_timestamp_allow_list_.emplace(timestamp); -} - -void FeatureTracker::SlideAllowList(const lc::Time& oldest_allowed_time) { - smart_factor_timestamp_allow_list_.erase(smart_factor_timestamp_allow_list_.begin(), - smart_factor_timestamp_allow_list_.lower_bound(oldest_allowed_time)); -} - -void FeatureTracker::RemoveOldFeaturePointsAndSlideWindow(boost::optional<lc::Time> oldest_allowed_time) { - const auto latest_time = LatestTimestamp(); - if (!latest_time) return; - const lc::Time window_start = *latest_time - params_.sliding_window_duration; - if (window_start <= 0 && !oldest_allowed_time) return; - oldest_allowed_time = oldest_allowed_time ? std::max(*oldest_allowed_time, window_start) : window_start; - - for (auto feature_track : feature_track_id_map_) { - feature_track.second->RemoveOldMeasurements(*oldest_allowed_time); - } - - SlideAllowList(*oldest_allowed_time); - UpdateLengthMap(); -} - -void FeatureTracker::RemoveUndetectedFeatures(const lc::Time& feature_point_timestamp) { - for (auto feature_it = feature_track_id_map_.cbegin(); feature_it != feature_track_id_map_.cend();) { - if (!feature_it->second->HasMeasurement(feature_point_timestamp)) { - feature_it = feature_track_id_map_.erase(feature_it); - } else { - ++feature_it; - } - } -} - -void FeatureTracker::AddOrUpdateTrack(const lm::FeaturePoint& feature_point) { - if (feature_track_id_map_.count(feature_point.feature_id) == 0) { - feature_track_id_map_[feature_point.feature_id] = std::make_shared<FeatureTrack>(feature_point.feature_id); - } - feature_track_id_map_[feature_point.feature_id]->AddMeasurement(feature_point.timestamp, feature_point); -} - -void FeatureTracker::UpdateLengthMap() { - feature_track_length_map_.clear(); - for (const auto& feature_track : feature_track_id_map_) { - feature_track_length_map_.emplace(feature_track.second->size(), feature_track.second); - } -} - -const FeatureTrackIdMap& FeatureTracker::feature_tracks() const { return feature_track_id_map_; } - -const std::set<lc::Time>& FeatureTracker::smart_factor_timestamp_allow_list() const { - return smart_factor_timestamp_allow_list_; -} - -const FeatureTrackLengthMap& FeatureTracker::feature_tracks_length_ordered() const { return feature_track_length_map_; } - -int FeatureTracker::NumTracksWithAtLeastNPoints(int n) const { - const auto lower_bound_it = feature_track_length_map_.lower_bound(n); - return std::distance(lower_bound_it, feature_track_length_map_.end()); -} - -size_t FeatureTracker::size() const { return feature_track_id_map_.size(); } - -bool FeatureTracker::empty() const { return feature_track_id_map_.empty(); } - -void FeatureTracker::Clear() { - feature_track_id_map_.clear(); - feature_track_length_map_.clear(); - smart_factor_timestamp_allow_list_.clear(); -} - -boost::optional<lc::Time> FeatureTracker::LatestTimestamp() const { - if (empty()) return boost::none; - // Since Feature Tracks without latest timestamp are erased on updates, each track contains the latest timestamp - return feature_track_id_map_.cbegin()->second->LatestTimestamp(); -} - -boost::optional<lc::Time> FeatureTracker::PreviousTimestamp() const { - const auto& longest_feature_track = LongestFeatureTrack(); - if (!longest_feature_track) return boost::none; - // TODO(rsoussan): Need to check this before returning? If boost::none, is this cast correctly when returned here? - return longest_feature_track->PreviousTimestamp(); -} - -boost::optional<localization_common::Time> FeatureTracker::OldestTimestamp() const { - const auto& longest_feature_track = LongestFeatureTrack(); - if (!longest_feature_track) return boost::none; - return longest_feature_track->OldestTimestamp(); -} - -boost::optional<const FeatureTrack&> FeatureTracker::LongestFeatureTrack() const { - if (empty()) return boost::none; - return *(feature_track_length_map_.rbegin()->second.get()); -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/graph_localizer.cc b/localization/graph_localizer/src/graph_localizer.cc index beb916ae5a..7f36a25e6e 100644 --- a/localization/graph_localizer/src/graph_localizer.cc +++ b/localization/graph_localizer/src/graph_localizer.cc @@ -17,506 +17,54 @@ */ #include <graph_localizer/graph_localizer.h> -#include <graph_localizer/loc_projection_factor.h> -#include <graph_localizer/loc_pose_factor.h> -#include <graph_localizer/point_to_handrail_endpoint_factor.h> -#include <graph_localizer/point_to_line_factor.h> -#include <graph_localizer/point_to_line_segment_factor.h> -#include <graph_localizer/point_to_plane_factor.h> -#include <graph_localizer/point_to_point_between_factor.h> -#include <graph_localizer/pose_rotation_factor.h> -#include <graph_localizer/utilities.h> -#include <graph_optimizer/utilities.h> -#include <imu_integration/utilities.h> -#include <localization_common/logger.h> -#include <localization_common/utilities.h> -#include <localization_measurements/measurement_conversions.h> - -#include <gtsam/base/Vector.h> -#include <gtsam/geometry/Pose3.h> -#include <gtsam/geometry/triangulation.h> -#include <gtsam/linear/NoiseModel.h> -#include <gtsam/navigation/ImuBias.h> -#include <gtsam/navigation/NavState.h> -#include <gtsam/nonlinear/LinearContainerFactor.h> -#include <gtsam/slam/PriorFactor.h> - -#include <unistd.h> - -#include <chrono> -#include <unordered_set> namespace graph_localizer { -namespace go = graph_optimizer; -namespace ii = imu_integration; +namespace fa = factor_adders; namespace lc = localization_common; namespace lm = localization_measurements; +namespace na = node_adders; +namespace no = nodes; +namespace op = optimizers; GraphLocalizer::GraphLocalizer(const GraphLocalizerParams& params) - : GraphOptimizer(params.graph_optimizer, std::unique_ptr<GraphLocalizerStats>(new GraphLocalizerStats())), - feature_tracker_(new FeatureTracker(params.feature_tracker)), - latest_imu_integrator_(new ii::LatestImuIntegrator(params.graph_initializer)), + : SlidingWindowGraphOptimizer(params.sliding_window_graph_optimizer, + std::make_unique<op::NonlinearOptimizer>(params.nonlinear_optimizer)), params_(params) { - // TODO(rsoussan): Add this param to imu integrator, set on construction - latest_imu_integrator_->SetFanSpeedMode(params_.initial_fan_speed_mode); - InitializeNodeUpdaters(); - InitializeFactorAdders(); - InitializeGraphActionCompleters(); -} - -void GraphLocalizer::InitializeNodeUpdaters() { - const lc::CombinedNavState global_N_body_start( - params_.graph_initializer.global_T_body_start, params_.graph_initializer.global_V_body_start, - params_.graph_initializer.initial_imu_bias, params_.graph_initializer.start_time); - params_.combined_nav_state_node_updater.global_N_body_start = global_N_body_start; - combined_nav_state_node_updater_.reset( - new CombinedNavStateNodeUpdater(params_.combined_nav_state_node_updater, latest_imu_integrator_, shared_values())); - combined_nav_state_node_updater_->AddInitialValuesAndPriors(graph_factors()); - AddNodeUpdater(combined_nav_state_node_updater_); - // TODO(rsoussan): Clean this up - dynamic_cast<GraphLocalizerStats*>(graph_stats()) - ->SetCombinedNavStateGraphValues(combined_nav_state_node_updater_->shared_graph_values()); - - feature_point_node_updater_.reset(new FeaturePointNodeUpdater(params_.feature_point_node_updater, shared_values())); - AddNodeUpdater(feature_point_node_updater_); -} - -void GraphLocalizer::InitializeFactorAdders() { - ar_tag_loc_factor_adder_.reset( - new LocFactorAdder(params_.factor.ar_tag_loc_adder, go::GraphActionCompleterType::ARTagLocProjectionFactor)); - depth_odometry_factor_adder_.reset(new DepthOdometryFactorAdder(params_.factor.depth_odometry_adder)); - handrail_factor_adder_.reset(new HandrailFactorAdder(params_.factor.handrail_adder)); - loc_factor_adder_.reset( - new LocFactorAdder(params_.factor.loc_adder, go::GraphActionCompleterType::LocProjectionFactor)); - projection_factor_adder_.reset( - new ProjectionFactorAdder(params_.factor.projection_adder, feature_tracker_, - feature_point_node_updater_->shared_feature_point_graph_values())); - rotation_factor_adder_.reset(new RotationFactorAdder(params_.factor.rotation_adder, feature_tracker_)); - smart_projection_cumulative_factor_adder_.reset( - new SmartProjectionCumulativeFactorAdder(params_.factor.smart_projection_adder, feature_tracker_)); - standstill_factor_adder_.reset(new StandstillFactorAdder(params_.factor.standstill_adder, feature_tracker_)); -} - -void GraphLocalizer::InitializeGraphActionCompleters() { - ar_tag_loc_graph_action_completer_.reset( - new LocGraphActionCompleter(params_.factor.ar_tag_loc_adder, go::GraphActionCompleterType::ARTagLocProjectionFactor, - combined_nav_state_node_updater_->shared_graph_values())); - AddGraphActionCompleter(ar_tag_loc_graph_action_completer_); - - loc_graph_action_completer_.reset( - new LocGraphActionCompleter(params_.factor.loc_adder, go::GraphActionCompleterType::LocProjectionFactor, - combined_nav_state_node_updater_->shared_graph_values())); - AddGraphActionCompleter(loc_graph_action_completer_); - projection_graph_action_completer_.reset(new ProjectionGraphActionCompleter( - params_.factor.projection_adder, combined_nav_state_node_updater_->shared_graph_values(), - feature_point_node_updater_->shared_feature_point_graph_values())); - AddGraphActionCompleter(projection_graph_action_completer_); - smart_projection_graph_action_completer_.reset(new SmartProjectionGraphActionCompleter( - params_.factor.smart_projection_adder, combined_nav_state_node_updater_->shared_graph_values())); - AddGraphActionCompleter(smart_projection_graph_action_completer_); -} - -boost::optional<std::pair<lc::CombinedNavState, lc::CombinedNavStateCovariances>> -GraphLocalizer::LatestCombinedNavStateAndCovariances() const { - if (!marginals()) { - LogDebugEveryN(50, "LatestCombinedNavStateAndCovariances: No marginals available."); - return boost::none; - } - const auto state_covariance_pair = LatestCombinedNavStateAndCovariances(*marginals()); - if (!state_covariance_pair) { - LogDebug( - "LatestCombinedNavStateAndCovariances: Failed to get latest combined nav state and " - "covariances."); - return boost::none; - } - - return state_covariance_pair; -} - -boost::optional<std::pair<lc::CombinedNavState, lc::CombinedNavStateCovariances>> -GraphLocalizer::LatestCombinedNavStateAndCovariances(const gtsam::Marginals& marginals) const { - const auto global_N_body_latest = combined_nav_state_node_updater_->graph_values().LatestCombinedNavState(); - if (!global_N_body_latest) { - LogError("LatestCombinedNavStateAndCovariance: Failed to get latest combined nav state."); - return boost::none; - } - const auto latest_combined_nav_state_key_index = - combined_nav_state_node_updater_->graph_values().LatestCombinedNavStateKeyIndex(); - if (!latest_combined_nav_state_key_index) { - LogError("LatestCombinedNavStateAndCovariance: Failed to get latest combined nav state."); - return boost::none; - } - - try { - const auto pose_covariance = marginals.marginalCovariance(sym::P(*latest_combined_nav_state_key_index)); - const auto velocity_covariance = marginals.marginalCovariance(sym::V(*latest_combined_nav_state_key_index)); - const auto bias_covariance = marginals.marginalCovariance(sym::B(*latest_combined_nav_state_key_index)); - const lc::CombinedNavStateCovariances latest_combined_nav_state_covariances(pose_covariance, velocity_covariance, - bias_covariance); - return std::pair<lc::CombinedNavState, lc::CombinedNavStateCovariances>{*global_N_body_latest, - latest_combined_nav_state_covariances}; - } catch (...) { - LogError("LatestCombinedNavStateAndCovariances: Failed to get marginal covariances."); - return boost::none; - } -} - -boost::optional<lc::CombinedNavState> GraphLocalizer::LatestCombinedNavState() const { - const auto global_N_body_latest = combined_nav_state_node_updater_->graph_values().LatestCombinedNavState(); - if (!global_N_body_latest) { - LogError("LatestCombinedNavState: Failed to get latest combined nav state."); - return boost::none; - } - - return global_N_body_latest; -} - -boost::optional<lc::CombinedNavState> GraphLocalizer::GetCombinedNavState(const lc::Time time) const { - const auto lower_bound_or_equal_combined_nav_state = - combined_nav_state_node_updater_->graph_values().LowerBoundOrEqualCombinedNavState(time); - if (!lower_bound_or_equal_combined_nav_state) { - LogDebug("GetCombinedNavState: Failed to get lower bound or equal combined nav state."); - return boost::none; - } - - if (lower_bound_or_equal_combined_nav_state->timestamp() == time) { - return lower_bound_or_equal_combined_nav_state; - } - - // Pim predict from lower bound state rather than closest state so there is no - // need to reverse predict (going backwards in time) using a pim prediction which is not yet supported in gtsam. - auto integrated_pim = latest_imu_integrator_->IntegratedPim(lower_bound_or_equal_combined_nav_state->bias(), - lower_bound_or_equal_combined_nav_state->timestamp(), - time, latest_imu_integrator_->pim_params()); - if (!integrated_pim) { - LogDebug("GetCombinedNavState: Failed to create integrated pim."); - return boost::none; - } - - return ii::PimPredict(*lower_bound_or_equal_combined_nav_state, *integrated_pim); -} - -boost::optional<std::pair<gtsam::imuBias::ConstantBias, lc::Time>> GraphLocalizer::LatestBiases() const { - const auto latest_bias = combined_nav_state_node_updater_->graph_values().LatestBias(); - if (!latest_bias) { - LogError("LatestBiases: Failed to get latest biases."); - return boost::none; - } - return latest_bias; -} - -// Latest extrapolated pose time is limited by latest imu time -boost::optional<lc::Time> GraphLocalizer::LatestExtrapolatedPoseTime() const { - return latest_imu_integrator_->LatestTime(); -} - -void GraphLocalizer::AddImuMeasurement(const lm::ImuMeasurement& imu_measurement) { - latest_imu_integrator_->BufferImuMeasurement(imu_measurement); -} - -bool GraphLocalizer::AddOpticalFlowMeasurement( - const lm::FeaturePointsMeasurement& optical_flow_feature_points_measurement) { - if (!MeasurementRecentEnough(optical_flow_feature_points_measurement.timestamp)) { - LogDebug("AddOpticalFlowMeasurement: Measurement too old - discarding."); - return false; - } - - // TODO(rsoussan): This is a bug in optical flow node, fix there - static lc::Time last_time = optical_flow_feature_points_measurement.timestamp; - if (last_time == optical_flow_feature_points_measurement.timestamp) { - LogDebug("AddOpticalFlowMeasurement: Same timestamp measurement, ignoring."); - last_time = optical_flow_feature_points_measurement.timestamp; - return false; - } - last_time = optical_flow_feature_points_measurement.timestamp; - - LogDebug("AddOpticalFlowMeasurement: Adding optical flow measurement."); - feature_tracker_->UpdateFeatureTracks(optical_flow_feature_points_measurement.feature_points); - - if (optical_flow_feature_points_measurement.feature_points.empty()) { - LogDebug("AddOpticalFlowMeasurement: Empty measurement."); - return false; - } - - // TODO(rsoussan): Enforce that projection and smart factor adders are not both enabled - if (params_.factor.projection_adder.enabled) { - BufferFactors(projection_factor_adder_->AddFactors(optical_flow_feature_points_measurement)); - } - if (params_.factor.rotation_adder.enabled) { - BufferFactors(rotation_factor_adder_->AddFactors(optical_flow_feature_points_measurement)); - } - - CheckForStandstill(); - if (standstill() && params_.factor.standstill_adder.enabled) { - BufferFactors(standstill_factor_adder_->AddFactors(optical_flow_feature_points_measurement)); - } - - return true; -} - -void GraphLocalizer::CheckForStandstill() { - // Check for standstill via low disparity for all feature tracks - double total_average_distance_from_mean = 0; - int num_valid_feature_tracks = 0; - for (const auto& feature_track : feature_tracker_->feature_tracks()) { - const double average_distance_from_mean = - AverageDistanceFromMean(feature_track.second->LatestPointsInWindow(params_.standstill_feature_track_duration)); - // Only consider long enough feature tracks for standstill candidates - if (static_cast<int>(feature_track.second->size()) >= params_.standstill_min_num_points_per_track) { - total_average_distance_from_mean += average_distance_from_mean; - ++num_valid_feature_tracks; - } - } - - double average_distance_from_mean = 0; - if (num_valid_feature_tracks > 0) - average_distance_from_mean = total_average_distance_from_mean / num_valid_feature_tracks; - - standstill_ = (num_valid_feature_tracks >= 5 && - average_distance_from_mean <= params_.max_standstill_feature_track_avg_distance_from_mean); - if (*standstill_) LogDebug("CheckForStandstill: Standstill."); + // Initialize sliding window node adders + pose_node_adder_ = + std::make_shared<na::PoseNodeAdder>(params_.pose_node_adder, params_.pose_node_adder_model, values()); + AddSlidingWindowNodeAdder(pose_node_adder_); + // Initialize factor adders + sparse_map_loc_factor_adder_ = + std::make_shared<fa::LocFactorAdder<na::PoseNodeAdder>>(params_.sparse_map_loc_factor_adder, pose_node_adder_); + AddFactorAdder(sparse_map_loc_factor_adder_); + ar_tag_loc_factor_adder_ = + std::make_shared<fa::LocFactorAdder<na::PoseNodeAdder>>(params_.ar_tag_loc_factor_adder, pose_node_adder_); + AddFactorAdder(ar_tag_loc_factor_adder_); } -void GraphLocalizer::AddARTagMeasurement(const lm::MatchedProjectionsMeasurement& matched_projections_measurement) { - if (!MeasurementRecentEnough(matched_projections_measurement.timestamp)) { - LogDebug("AddARTagMeasurement: Measurement too old - discarding."); - return; - } - - if (params_.factor.ar_tag_loc_adder.enabled && - static_cast<int>(matched_projections_measurement.matched_projections.size()) >= - params_.factor.ar_tag_loc_adder.min_num_matches) { - LogDebug("AddARTagMeasurement: Adding AR tag measurement."); - BufferFactors(ar_tag_loc_factor_adder_->AddFactors(matched_projections_measurement)); - } +void GraphLocalizer::AddPoseMeasurement(const lm::PoseWithCovarianceMeasurement& pose_measurement) { + pose_node_adder_->AddMeasurement(pose_measurement); } -void GraphLocalizer::AddSparseMappingMeasurement( +void GraphLocalizer::AddSparseMapMatchedProjectionsMeasurement( const lm::MatchedProjectionsMeasurement& matched_projections_measurement) { - if (!MeasurementRecentEnough(matched_projections_measurement.timestamp)) { - LogDebug("AddSparseMappingMeasurement: Measurement too old - discarding."); - return; - } - - if (params_.factor.loc_adder.enabled) { - LogDebug("AddSparseMappingMeasurement: Adding sparse mapping measurement."); - BufferFactors(loc_factor_adder_->AddFactors(matched_projections_measurement)); - } -} - -void GraphLocalizer::AddHandrailMeasurement(const lm::HandrailPointsMeasurement& handrail_points_measurement) { - if (!MeasurementRecentEnough(handrail_points_measurement.timestamp)) { - LogDebug("AddHandrailPointsMeasurement: Measurement too old - discarding."); - return; - } - - if (params_.factor.handrail_adder.enabled) { - LogDebug("AddHandrailPointsMeasurement: Adding handrail measurement."); - BufferFactors(handrail_factor_adder_->AddFactors(handrail_points_measurement)); - } -} - -void GraphLocalizer::AddDepthOdometryMeasurement(const lm::DepthOdometryMeasurement& depth_odometry_measurement) { - if (!MeasurementRecentEnough(depth_odometry_measurement.timestamp)) { - LogDebug("AddDepthOdometryMeasurement: Measurement too old - discarding."); - return; - } - - if (params_.factor.depth_odometry_adder.enabled) { - LogDebug("AddDepthOdometryMeasurement: Adding depth odometry measurement."); - BufferFactors(depth_odometry_factor_adder_->AddFactors(depth_odometry_measurement)); - } -} - -void GraphLocalizer::DoPostSlideWindowActions(const localization_common::Time oldest_allowed_time, - const boost::optional<gtsam::Marginals>& marginals) { - feature_tracker_->RemoveOldFeaturePointsAndSlideWindow(oldest_allowed_time); - latest_imu_integrator_->RemoveOldMeasurements(oldest_allowed_time); -} - -void GraphLocalizer::BufferCumulativeFactors() { - // Remove measurements here so they are more likely to fit in sliding window duration when optimized - feature_tracker_->RemoveOldFeaturePointsAndSlideWindow(); - if (params_.factor.smart_projection_adder.enabled) { - BufferFactors(smart_projection_cumulative_factor_adder_->AddFactors()); - } -} - -void GraphLocalizer::RemoveOldMeasurementsFromCumulativeFactors(const gtsam::KeyVector& old_keys) { - for (auto factor_it = graph_factors().begin(); factor_it != graph_factors().end();) { - const auto smart_factor = dynamic_cast<const RobustSmartFactor*>(factor_it->get()); - // Currently the only cumulative factors are smart factors - // TODO(rsoussan): Generalize this - if (!smart_factor) { - ++factor_it; - continue; - } - const auto& factor_keys = (*factor_it)->keys(); - std::unordered_set<int> factor_key_indices_to_remove; - for (int i = 0; i < static_cast<int>(factor_keys.size()); ++i) { - for (const auto& old_key : old_keys) { - if (factor_keys[i] == old_key) { - factor_key_indices_to_remove.emplace(i); - break; - } - } - } - if (factor_key_indices_to_remove.empty()) { - ++factor_it; - continue; - } else { - if (static_cast<int>(factor_keys.size() - factor_key_indices_to_remove.size()) < - params_.factor.smart_projection_adder.min_num_points) { - factor_it = graph_factors().erase(factor_it); - continue; - } else { - const auto new_smart_factor = RemoveSmartFactorMeasurements( - *smart_factor, factor_key_indices_to_remove, params_.factor.smart_projection_adder, - smart_projection_cumulative_factor_adder_->smart_projection_params()); - *factor_it = new_smart_factor; - continue; - } - } - } -} - -bool GraphLocalizer::ValidGraph() const { - // If graph consists of only priors and imu factors, consider it invalid and don't optimize. - // Make sure smart factors are valid before including them. - const int num_valid_non_imu_measurement_factors = - NumOFFactors(true) + go::NumFactors<gtsam::LocPoseFactor>(graph_factors()) + - go::NumFactors<gtsam::LocProjectionFactor<>>(graph_factors()) + - go::NumFactors<gtsam::PointToLineFactor>(graph_factors()) + - go::NumFactors<gtsam::PointToLineSegmentFactor>(graph_factors()) + - go::NumFactors<gtsam::PointToPlaneFactor>(graph_factors()) + - go::NumFactors<gtsam::PointToPointBetweenFactor>(graph_factors()) + - go::NumFactors<gtsam::PointToHandrailEndpointFactor>(graph_factors()) + - go::NumFactors<gtsam::PoseRotationFactor>(graph_factors()) + - go::NumFactors<gtsam::BetweenFactor<gtsam::Pose3>>(graph_factors()); - return num_valid_non_imu_measurement_factors > 0; -} - -bool GraphLocalizer::ReadyToAddFactors(const localization_common::Time timestamp) const { - const auto latest_time = latest_imu_integrator_->LatestTime(); - if (!latest_time) { - LogError("ReadyToAddMeasurement: Failed to get latet imu time."); - return false; - } - - return (timestamp <= *latest_time); + if (params_.sparse_map_loc_factor_adder.enabled) + sparse_map_loc_factor_adder_->AddMeasurement(matched_projections_measurement); } -bool GraphLocalizer::MeasurementRecentEnough(const lc::Time timestamp) const { - if (!GraphOptimizer::MeasurementRecentEnough(timestamp)) return false; - if (!latest_imu_integrator_->OldestTime()) { - LogDebug("MeasurementRecentEnough: Waiting until imu measurements have been received."); - return false; - } - if (timestamp < latest_imu_integrator_->OldestTime()) return false; - return true; -} - -void GraphLocalizer::PrintFactorDebugInfo() const { - for (const auto& factor : graph_factors()) { - const auto smart_factor = dynamic_cast<const RobustSmartFactor*>(factor.get()); - if (smart_factor) { - smart_factor->print(); - if (smart_factor->isValid()) - LogDebug("PrintFactorDebugInfo: SmartFactor valid."); - else - LogDebug("PrintFactorDebugInfo: SmartFactor invalid."); - if (smart_factor->isDegenerate()) LogDebug("PrintFactorDebugInfo: SmartFactor degenerate."); - if (smart_factor->isPointBehindCamera()) LogDebug("PrintFactorDebugInfo: SmartFactor point behind camera."); - if (smart_factor->isOutlier()) LogDebug("PrintFactorDebugInfo: SmartFactor is outlier."); - if (smart_factor->isFarPoint()) LogDebug("PrintFactorDebugInfo: SmartFactor is far point."); - } - const auto imu_factor = dynamic_cast<gtsam::CombinedImuFactor*>(factor.get()); - if (imu_factor) { - LogDebug("PrintFactorDebugInfo: CombinedImuFactor: " << *imu_factor); - LogDebug("PrintFactorDebugInfo: CombinedImuFactor PIM: " << imu_factor->preintegratedMeasurements()); - } - } -} - -void GraphLocalizer::SetFanSpeedMode(const lm::FanSpeedMode fan_speed_mode) { - latest_imu_integrator_->SetFanSpeedMode(fan_speed_mode); -} - -const lm::FanSpeedMode GraphLocalizer::fan_speed_mode() const { return latest_imu_integrator_->fan_speed_mode(); } - -const CombinedNavStateGraphValues& GraphLocalizer::combined_nav_state_graph_values() const { - return combined_nav_state_node_updater_->graph_values(); -} - -const CombinedNavStateNodeUpdater& GraphLocalizer::combined_nav_state_node_updater() const { - return *combined_nav_state_node_updater_; -} - -const GraphLocalizerParams& GraphLocalizer::params() const { return params_; } - -int GraphLocalizer::NumFeatures() const { return feature_point_node_updater_->NumFeatures(); } - -// TODO(rsoussan): fix this call to happen before of factors are removed! -int GraphLocalizer::NumOFFactors(const bool check_valid) const { - if (params_.factor.smart_projection_adder.enabled) - return NumSmartFactors(graph_factors(), combined_nav_state_node_updater_->graph_values().values(), check_valid); - if (params_.factor.projection_adder.enabled) return NumProjectionFactors(check_valid); - return 0; -} - -int GraphLocalizer::NumProjectionFactors(const bool check_valid) const { - int num_factors = 0; - for (const auto& factor : graph_factors()) { - const auto projection_factor = dynamic_cast<const ProjectionFactor*>(factor.get()); - if (projection_factor) { - if (check_valid) { - const auto world_t_point = - feature_point_node_updater_->feature_point_graph_values().at<gtsam::Point3>(projection_factor->key2()); - if (!world_t_point) { - LogError("NumProjectionFactors: Failed to get point."); - continue; - } - const auto world_T_body = - combined_nav_state_node_updater_->graph_values().at<gtsam::Pose3>(projection_factor->key1()); - if (!world_T_body) { - LogError("NumProjectionFactors: Failed to get pose."); - continue; - } - const auto world_T_camera = *world_T_body * params_.calibration.body_T_nav_cam; - const auto camera_t_point = world_T_camera.inverse() * *world_t_point; - if (camera_t_point.z() <= 0) { - LogDebug("NumProjectionFactors: Behind camera."); - continue; - } - ++num_factors; - } else { - ++num_factors; - } - } - } - return num_factors; -} - -const GraphLocalizerStats& GraphLocalizer::graph_localizer_stats() const { - return *(static_cast<const GraphLocalizerStats*>(graph_stats())); -} - -bool GraphLocalizer::standstill() const { - // If uninitialized, return not at standstill - // TODO(rsoussan): Is this the appropriate behavior? - if (!standstill_) return false; - return *standstill_; +void GraphLocalizer::AddArTagMatchedProjectionsMeasurement( + const lm::MatchedProjectionsMeasurement& matched_projections_measurement) { + if (params_.ar_tag_loc_factor_adder.enabled) + ar_tag_loc_factor_adder_->AddMeasurement(matched_projections_measurement); } -bool GraphLocalizer::DoPostOptimizeActions() { - // Update imu integrator bias - const auto latest_bias = combined_nav_state_node_updater_->graph_values().LatestBias(); - if (!latest_bias) { - LogError("Update: Failed to get latest bias."); - return false; - } +const no::TimestampedNodes<gtsam::Pose3>& GraphLocalizer::pose_nodes() const { return pose_node_adder_->nodes(); } - latest_imu_integrator_->ResetPimIntegrationAndSetBias(latest_bias->first); - return true; +void GraphLocalizer::SetPoseCovarianceInterpolater( + const std::shared_ptr<lc::MarginalsPoseCovarianceInterpolater<no::CombinedNavStateNodes>>& + pose_covariance_interpolater) { + pose_node_adder_->node_adder_model().pose_interpolater().params().pose_covariance_interpolater = + pose_covariance_interpolater; } } // namespace graph_localizer diff --git a/localization/graph_localizer/src/graph_localizer_intializer.cc b/localization/graph_localizer/src/graph_localizer_intializer.cc deleted file mode 100644 index 5ccad1d0e9..0000000000 --- a/localization/graph_localizer/src/graph_localizer_intializer.cc +++ /dev/null @@ -1,202 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/graph_localizer_initializer.h> -#include <graph_localizer/parameter_reader.h> -#include <graph_localizer/utilities.h> -#include <localization_common/utilities.h> - -#include <iostream> -#include <string> - -namespace graph_localizer { -namespace lc = localization_common; -namespace lm = localization_measurements; -GraphLocalizerInitializer::GraphLocalizerInitializer() - : has_biases_(false), - has_start_pose_(false), - has_params_(false), - has_fan_speed_mode_(false), - estimate_biases_(false), - removed_gravity_from_bias_if_necessary_(false) {} -void GraphLocalizerInitializer::SetBiases(const gtsam::imuBias::ConstantBias& imu_bias, - const bool loaded_from_previous_estimate, const bool save_to_file) { - params_.graph_initializer.initial_imu_bias = imu_bias; - has_biases_ = true; - estimate_biases_ = false; - // Assumes previous bias is already gravity compensated if neccessary - if (!loaded_from_previous_estimate) RemoveGravityFromBiasIfPossibleAndNecessary(); - if (save_to_file) { - std::ofstream imu_bias_file(params_.graph_initializer.imu_bias_filename); - if (!imu_bias_file.is_open()) { - LogError("SetBiases: Failed to create imu bias output file."); - return; - } - const auto& accel_bias = params_.graph_initializer.initial_imu_bias.accelerometer(); - imu_bias_file << accel_bias.x() << "," << accel_bias.y() << "," << accel_bias.z() << std::endl; - const auto& gyro_bias = params_.graph_initializer.initial_imu_bias.gyroscope(); - imu_bias_file << gyro_bias.x() << "," << gyro_bias.y() << "," << gyro_bias.z() << std::endl; - imu_bias_file.close(); - } -} - -void GraphLocalizerInitializer::SetStartPose(const lm::TimestampedPose& timestamped_pose) { - params_.graph_initializer.start_time = timestamped_pose.time; - params_.graph_initializer.global_T_body_start = timestamped_pose.pose; - // Assumes zero initial velocity - params_.graph_initializer.global_V_body_start = gtsam::Velocity3::Zero(); - has_start_pose_ = true; - RemoveGravityFromBiasIfPossibleAndNecessary(); -} - -void GraphLocalizerInitializer::SetFanSpeedMode(const lm::FanSpeedMode fan_speed_mode) { - params_.initial_fan_speed_mode = fan_speed_mode; - has_fan_speed_mode_ = true; -} - -void GraphLocalizerInitializer::RemoveGravityFromBiasIfPossibleAndNecessary() { - if (RemovedGravityFromBiasIfNecessary() || !HasParams()) return; - if (params_.graph_initializer.gravity.isZero()) { - removed_gravity_from_bias_if_necessary_ = true; - return; - } - if (!HasStartPose() || !HasBiases()) return; - // Biases, start pose and params are available and gravity is non zero, gravity can and should now be removed - // from the initial bias estimates. - LogDebug("RemoveGravityFromBiasIfPossibleAndNecessary: Removing gravity from initial biases."); - RemoveGravityFromBias(params_.graph_initializer.gravity, params_.graph_initializer.body_T_imu, - params_.graph_initializer.global_T_body_start, params_.graph_initializer.initial_imu_bias); - - LogDebug("RemoveGravityFromBiasIfPossibleAndNecessary: New gravity corrected accelerometer bias: " - << params_.graph_initializer.initial_imu_bias.accelerometer().matrix()); - removed_gravity_from_bias_if_necessary_ = true; - return; -} - -void GraphLocalizerInitializer::ResetBiasesAndStartPose() { - ResetBiases(); - ResetStartPose(); -} - -void GraphLocalizerInitializer::ResetBiasesFromFileAndResetStartPose() { - ResetBiasesFromFile(); - ResetStartPose(); -} - -void GraphLocalizerInitializer::ResetStartPose() { has_start_pose_ = false; } - -void GraphLocalizerInitializer::ResetBiases() { - has_biases_ = false; - imu_bias_filter_.reset(new imu_integration::DynamicImuFilter(params_.graph_initializer.filter)); - imu_bias_measurements_.clear(); - StartBiasEstimation(); -} - -void GraphLocalizerInitializer::ResetBiasesFromFile() { - std::ifstream imu_bias_file(params_.graph_initializer.imu_bias_filename); - if (!imu_bias_file.is_open()) { - LogDebug("ResetBiasesFromFile: Failed to read imu bias file."); - return; - } - - gtsam::Vector3 accelerometer_bias; - gtsam::Vector3 gyro_bias; - for (int line_num = 0; line_num < 2; ++line_num) { - std::string line; - if (!std::getline(imu_bias_file, line)) { - LogError("ResetBiasesFromFile: Failed to get line from imu bias file."); - return; - } - std::istringstream line_stream(line); - std::string val; - for (int val_index = 0; val_index < 3; ++val_index) { - if (!std::getline(line_stream, val, ',')) { - LogError("ResetBiasesFromFile: Failed to get value from imu bias file."); - return; - } - if (line_num == 0) { - accelerometer_bias[val_index] = std::stold(val); - } else { - gyro_bias[val_index] = std::stold(val); - } - } - } - SetBiases(gtsam::imuBias::ConstantBias(accelerometer_bias, gyro_bias), true); -} - -void GraphLocalizerInitializer::EstimateAndSetImuBiases( - const localization_measurements::ImuMeasurement& imu_measurement, const lm::FanSpeedMode fan_speed_mode) { - imu_bias_filter_->SetFanSpeedMode(fan_speed_mode); - const auto filtered_imu_measurement = imu_bias_filter_->AddMeasurement(imu_measurement); - if (filtered_imu_measurement) { - imu_bias_measurements_.emplace_back(*filtered_imu_measurement); - } - if (static_cast<int>(imu_bias_measurements_.size()) < params_.graph_initializer.num_bias_estimation_measurements) - return; - - Eigen::Vector3d sum_of_acceleration_measurements = Eigen::Vector3d::Zero(); - Eigen::Vector3d sum_of_angular_velocity_measurements = Eigen::Vector3d::Zero(); - for (const auto& imu_measurement : imu_bias_measurements_) { - sum_of_acceleration_measurements += imu_measurement.acceleration; - sum_of_angular_velocity_measurements += imu_measurement.angular_velocity; - } - - LogDebug( - "Number of imu measurements per bias estimate: " << params_.graph_initializer.num_bias_estimation_measurements); - const Eigen::Vector3d accelerometer_bias = sum_of_acceleration_measurements / imu_bias_measurements_.size(); - const Eigen::Vector3d gyro_bias = sum_of_angular_velocity_measurements / imu_bias_measurements_.size(); - LogDebug("Accelerometer bias: " << std::endl << accelerometer_bias.matrix()); - LogDebug("Gyro bias: " << std::endl << gyro_bias.matrix()); - - gtsam::imuBias::ConstantBias biases(accelerometer_bias, gyro_bias); - SetBiases(biases, false, true); - imu_bias_filter_.reset(new imu_integration::DynamicImuFilter(params_.graph_initializer.filter, fan_speed_mode)); - imu_bias_measurements_.clear(); -} - -void GraphLocalizerInitializer::RemoveGravityFromBias(const gtsam::Vector3& global_F_gravity, - const gtsam::Pose3& body_T_imu, const gtsam::Pose3& global_T_body, - gtsam::imuBias::ConstantBias& imu_bias) { - const gtsam::Vector3 gravity_corrected_accelerometer_bias = lc::RemoveGravityFromAccelerometerMeasurement( - global_F_gravity, body_T_imu, global_T_body, imu_bias.accelerometer()); - imu_bias = gtsam::imuBias::ConstantBias(gravity_corrected_accelerometer_bias, imu_bias.gyroscope()); -} - -void GraphLocalizerInitializer::LoadGraphLocalizerParams(config_reader::ConfigReader& config) { - graph_localizer::LoadGraphLocalizerParams(config, params_); - has_params_ = true; -} - -bool GraphLocalizerInitializer::ReadyToInitialize() const { - return HasBiases() && HasStartPose() && HasParams() && HasFanSpeedMode() && RemovedGravityFromBiasIfNecessary(); -} - -void GraphLocalizerInitializer::StartBiasEstimation() { estimate_biases_ = true; } - -bool GraphLocalizerInitializer::HasBiases() const { return has_biases_; } -bool GraphLocalizerInitializer::HasStartPose() const { return has_start_pose_; } -bool GraphLocalizerInitializer::HasParams() const { return has_params_; } -bool GraphLocalizerInitializer::HasFanSpeedMode() const { return has_fan_speed_mode_; } -bool GraphLocalizerInitializer::EstimateBiases() const { return estimate_biases_; } -bool GraphLocalizerInitializer::RemovedGravityFromBiasIfNecessary() const { - return removed_gravity_from_bias_if_necessary_; -} - -const GraphLocalizerParams& GraphLocalizerInitializer::params() const { return params_; } - -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/graph_localizer_nodelet.cc b/localization/graph_localizer/src/graph_localizer_nodelet.cc deleted file mode 100644 index bfd2b199b2..0000000000 --- a/localization/graph_localizer/src/graph_localizer_nodelet.cc +++ /dev/null @@ -1,351 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <ff_msgs/GraphState.h> -#include <ff_msgs/LocalizationGraph.h> -#include <ff_common/ff_names.h> -#include <graph_localizer/graph_localizer_nodelet.h> -#include <graph_localizer/parameter_reader.h> -#include <graph_localizer/utilities.h> -#include <localization_common/logger.h> -#include <localization_common/utilities.h> -#include <msg_conversions/msg_conversions.h> - -#include <std_msgs/Empty.h> - -namespace graph_localizer { -namespace lc = localization_common; -namespace mc = msg_conversions; - -GraphLocalizerNodelet::GraphLocalizerNodelet() : ff_util::FreeFlyerNodelet(NODE_GRAPH_LOC, true) { - private_nh_.setCallbackQueue(&private_queue_); - heartbeat_.node = GetName(); - heartbeat_.nodelet_manager = ros::this_node::getName(); - - config_reader::ConfigReader config; - lc::LoadGraphLocalizerConfig(config); - if (!config.ReadFiles()) { - LogFatal("Failed to read config files."); - } - LoadGraphLocalizerNodeletParams(config, params_); - last_heartbeat_time_ = ros::Time::now(); -} - -void GraphLocalizerNodelet::Initialize(ros::NodeHandle* nh) { - // Setup the platform name - platform_name_ = GetPlatform(); - platform_name_ = (platform_name_.empty() ? "" : platform_name_ + "/"); - - ff_common::InitFreeFlyerApplication(getMyArgv()); - SubscribeAndAdvertise(nh); - Run(); -} - -void GraphLocalizerNodelet::SubscribeAndAdvertise(ros::NodeHandle* nh) { - state_pub_ = nh->advertise<ff_msgs::GraphState>(TOPIC_GRAPH_LOC_STATE, 10); - sparse_mapping_pose_pub_ = nh->advertise<geometry_msgs::PoseStamped>(TOPIC_SPARSE_MAPPING_POSE, 10); - ar_tag_pose_pub_ = nh->advertise<geometry_msgs::PoseStamped>(TOPIC_AR_TAG_POSE, 10); - graph_pub_ = nh->advertise<ff_msgs::LocalizationGraph>(TOPIC_GRAPH_LOC, 10); - reset_pub_ = nh->advertise<std_msgs::Empty>(TOPIC_GNC_EKF_RESET, 10); - heartbeat_pub_ = nh->advertise<ff_msgs::Heartbeat>(TOPIC_HEARTBEAT, 5, true); - - imu_sub_ = private_nh_.subscribe(TOPIC_HARDWARE_IMU, params_.max_imu_buffer_size, &GraphLocalizerNodelet::ImuCallback, - this, ros::TransportHints().tcpNoDelay()); - ar_sub_ = - private_nh_.subscribe(TOPIC_LOCALIZATION_AR_FEATURES, params_.max_ar_buffer_size, - &GraphLocalizerNodelet::ARVisualLandmarksCallback, this, ros::TransportHints().tcpNoDelay()); - dl_sub_ = - private_nh_.subscribe(TOPIC_LOCALIZATION_HR_FEATURES, params_.max_dl_buffer_size, - &GraphLocalizerNodelet::DepthLandmarksCallback, this, ros::TransportHints().tcpNoDelay()); - depth_odometry_sub_ = - private_nh_.subscribe(TOPIC_LOCALIZATION_DEPTH_ODOM, params_.max_depth_odometry_buffer_size, - &GraphLocalizerNodelet::DepthOdometryCallback, this, ros::TransportHints().tcpNoDelay()); - of_sub_ = - private_nh_.subscribe(TOPIC_LOCALIZATION_OF_FEATURES, params_.max_optical_flow_buffer_size, - &GraphLocalizerNodelet::OpticalFlowCallback, this, ros::TransportHints().tcpNoDelay()); - vl_sub_ = - private_nh_.subscribe(TOPIC_LOCALIZATION_ML_FEATURES, params_.max_vl_buffer_size, - &GraphLocalizerNodelet::VLVisualLandmarksCallback, this, ros::TransportHints().tcpNoDelay()); - flight_mode_sub_ = - private_nh_.subscribe(TOPIC_MOBILITY_FLIGHT_MODE, 10, &GraphLocalizerNodelet::FlightModeCallback, this); - bias_srv_ = - private_nh_.advertiseService(SERVICE_GNC_EKF_INIT_BIAS, &GraphLocalizerNodelet::ResetBiasesAndLocalizer, this); - bias_from_file_srv_ = private_nh_.advertiseService( - SERVICE_GNC_EKF_INIT_BIAS_FROM_FILE, &GraphLocalizerNodelet::ResetBiasesFromFileAndResetLocalizer, this); - reset_map_srv_ = - private_nh_.advertiseService(SERVICE_LOCALIZATION_RESET_MAP_LOC, &GraphLocalizerNodelet::ResetMapLocalizer, this); - reset_srv_ = private_nh_.advertiseService(SERVICE_GNC_EKF_RESET, &GraphLocalizerNodelet::ResetLocalizer, this); - input_mode_srv_ = private_nh_.advertiseService(SERVICE_GNC_EKF_SET_INPUT, &GraphLocalizerNodelet::SetMode, this); -} - -bool GraphLocalizerNodelet::SetMode(ff_msgs::SetEkfInput::Request& req, ff_msgs::SetEkfInput::Response& res) { - const auto input_mode = req.mode; - if (input_mode == ff_msgs::SetEkfInputRequest::MODE_NONE) { - LogInfo("Received Mode None request, turning off localizer."); - DisableLocalizer(); - } else if (last_mode_ == ff_msgs::SetEkfInputRequest::MODE_NONE) { - LogInfo( - "Received Mode request that is not None and current mode is " - "None, resetting localizer."); - ResetAndEnableLocalizer(); - } - - // Might need to resestimate world_T_dock on ar mode switch - if (input_mode == ff_msgs::SetEkfInputRequest::MODE_AR_TAGS && - last_mode_ != ff_msgs::SetEkfInputRequest::MODE_AR_TAGS) { - LogInfo("SetMode: Switching to AR_TAG mode."); - graph_localizer_wrapper_.MarkWorldTDockForResettingIfNecessary(); - } else if (input_mode == ff_msgs::SetEkfInputRequest::MODE_HANDRAIL && - last_mode_ != ff_msgs::SetEkfInputRequest::MODE_HANDRAIL) { - LogInfo("SetMode: Switching to Handrail mode."); - graph_localizer_wrapper_.MarkWorldTHandrailForResetting(); - } - - last_mode_ = input_mode; - return true; -} - -void GraphLocalizerNodelet::DisableLocalizer() { localizer_enabled_ = false; } - -void GraphLocalizerNodelet::EnableLocalizer() { localizer_enabled_ = true; } - -bool GraphLocalizerNodelet::localizer_enabled() const { return localizer_enabled_; } - -bool GraphLocalizerNodelet::ResetBiasesAndLocalizer(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { - DisableLocalizer(); - graph_localizer_wrapper_.ResetBiasesAndLocalizer(); - PublishReset(); - EnableLocalizer(); - return true; -} - -bool GraphLocalizerNodelet::ResetMapLocalizer(ff_msgs::ResetMap::Request& req, ff_msgs::ResetMap::Response& res) { - // Reset localizer while loading previous biases when map is reset to prevent possible initial - // map jump from affecting estimated IMU biases and velocity estimation. - // Clear vl messages to prevent old localization results from being used by localizer after reset - // TODO(rsoussan): Better way to clear buffer? - vl_sub_ = - private_nh_.subscribe(TOPIC_LOCALIZATION_ML_FEATURES, params_.max_vl_buffer_size, - &GraphLocalizerNodelet::VLVisualLandmarksCallback, this, ros::TransportHints().tcpNoDelay()); - return ResetBiasesFromFileAndResetLocalizer(); -} - -bool GraphLocalizerNodelet::ResetBiasesFromFileAndResetLocalizer(std_srvs::Empty::Request& req, - std_srvs::Empty::Response& res) { - return ResetBiasesFromFileAndResetLocalizer(); -} - -bool GraphLocalizerNodelet::ResetBiasesFromFileAndResetLocalizer() { - DisableLocalizer(); - graph_localizer_wrapper_.ResetBiasesFromFileAndResetLocalizer(); - PublishReset(); - EnableLocalizer(); - return true; -} - -bool GraphLocalizerNodelet::ResetLocalizer(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { - ResetAndEnableLocalizer(); - return true; -} - -void GraphLocalizerNodelet::ResetAndEnableLocalizer() { - DisableLocalizer(); - graph_localizer_wrapper_.ResetLocalizer(); - PublishReset(); - EnableLocalizer(); -} - -void GraphLocalizerNodelet::OpticalFlowCallback(const ff_msgs::Feature2dArray::ConstPtr& feature_array_msg) { - of_timer_.HeaderDiff(feature_array_msg->header); - of_timer_.VlogEveryN(100, 2); - - if (!localizer_enabled()) return; - graph_localizer_wrapper_.OpticalFlowCallback(*feature_array_msg); -} - -void GraphLocalizerNodelet::VLVisualLandmarksCallback(const ff_msgs::VisualLandmarks::ConstPtr& visual_landmarks_msg) { - vl_timer_.HeaderDiff(visual_landmarks_msg->header); - vl_timer_.VlogEveryN(100, 2); - - if (!localizer_enabled()) return; - graph_localizer_wrapper_.VLVisualLandmarksCallback(*visual_landmarks_msg); - if (ValidVLMsg(*visual_landmarks_msg, params_.loc_adder_min_num_matches)) PublishSparseMappingPose(); -} - -void GraphLocalizerNodelet::ARVisualLandmarksCallback(const ff_msgs::VisualLandmarks::ConstPtr& visual_landmarks_msg) { - ar_timer_.HeaderDiff(visual_landmarks_msg->header); - ar_timer_.VlogEveryN(100, 2); - - if (!localizer_enabled()) return; - graph_localizer_wrapper_.ARVisualLandmarksCallback(*visual_landmarks_msg); - PublishWorldTDockTF(); - if (ValidVLMsg(*visual_landmarks_msg, params_.ar_tag_loc_adder_min_num_matches)) PublishARTagPose(); -} - -void GraphLocalizerNodelet::DepthOdometryCallback(const ff_msgs::DepthOdometry::ConstPtr& depth_odometry_msg) { - depth_odometry_timer_.HeaderDiff(depth_odometry_msg->header); - depth_odometry_timer_.VlogEveryN(100, 2); - - if (!localizer_enabled()) return; - graph_localizer_wrapper_.DepthOdometryCallback(*depth_odometry_msg); -} - -void GraphLocalizerNodelet::DepthLandmarksCallback(const ff_msgs::DepthLandmarks::ConstPtr& depth_landmarks_msg) { - depth_timer_.HeaderDiff(depth_landmarks_msg->header); - depth_timer_.VlogEveryN(100, 2); - - if (!localizer_enabled()) return; - graph_localizer_wrapper_.DepthLandmarksCallback(*depth_landmarks_msg); - PublishWorldTHandrailTF(); - if (ValidDepthMsg(*depth_landmarks_msg)) PublishHandrailPose(); -} - -void GraphLocalizerNodelet::ImuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg) { - imu_timer_.HeaderDiff(imu_msg->header); - imu_timer_.VlogEveryN(100, 2); - - if (!localizer_enabled()) return; - graph_localizer_wrapper_.ImuCallback(*imu_msg); -} - -void GraphLocalizerNodelet::FlightModeCallback(ff_msgs::FlightMode::ConstPtr const& mode) { - graph_localizer_wrapper_.FlightModeCallback(*mode); -} - -void GraphLocalizerNodelet::PublishLocalizationState() { - auto latest_localization_state_msg = graph_localizer_wrapper_.LatestLocalizationStateMsg(); - if (!latest_localization_state_msg) { - LogDebugEveryN(100, "PublishLocalizationState: Failed to get latest localization state msg."); - return; - } - latest_localization_state_msg->callbacks_time = callbacks_timer_.last_value(); - latest_localization_state_msg->nodelet_runtime = nodelet_runtime_timer_.last_value(); - state_pub_.publish(*latest_localization_state_msg); -} - -void GraphLocalizerNodelet::PublishLocalizationGraph() { - const auto latest_localization_graph_msg = graph_localizer_wrapper_.LatestLocalizationGraphMsg(); - if (!latest_localization_graph_msg) { - LogDebugEveryN(100, "PublishLocalizationGraph: Failed to get latest localization graph msg."); - return; - } - graph_pub_.publish(*latest_localization_graph_msg); -} - -void GraphLocalizerNodelet::PublishSparseMappingPose() const { - const auto latest_sparse_mapping_pose_msg = graph_localizer_wrapper_.LatestSparseMappingPoseMsg(); - if (!latest_sparse_mapping_pose_msg) { - LogWarning("PublishSparseMappingPose: Failed to get latest sparse mapping pose msg."); - return; - } - sparse_mapping_pose_pub_.publish(*latest_sparse_mapping_pose_msg); -} - -void GraphLocalizerNodelet::PublishARTagPose() const { - const auto latest_ar_tag_pose_msg = graph_localizer_wrapper_.LatestARTagPoseMsg(); - if (!latest_ar_tag_pose_msg) { - LogDebug("PublishARTagPose: Failed to get latest ar tag pose msg."); - return; - } - ar_tag_pose_pub_.publish(*latest_ar_tag_pose_msg); -} - -void GraphLocalizerNodelet::PublishHandrailPose() const { - const auto latest_handrail_pose_msg = graph_localizer_wrapper_.LatestHandrailPoseMsg(); - if (!latest_handrail_pose_msg) { - LogWarning("PublishHandrailPose: Failed to get latest handrail pose msg."); - return; - } - handrail_pose_pub_.publish(*latest_handrail_pose_msg); -} - -void GraphLocalizerNodelet::PublishWorldTBodyTF() { - const auto latest_combined_nav_state = graph_localizer_wrapper_.LatestCombinedNavState(); - if (!latest_combined_nav_state) { - LogErrorEveryN(100, "PublishWorldTBodyTF: Failed to get world_T_body."); - return; - } - - const auto world_T_body_tf = lc::PoseToTF(latest_combined_nav_state->pose(), "world", "body", - latest_combined_nav_state->timestamp(), platform_name_); - transform_pub_.sendTransform(world_T_body_tf); -} - -void GraphLocalizerNodelet::PublishWorldTDockTF() { - const auto world_T_dock = graph_localizer_wrapper_.estimated_world_T_dock(); - const auto world_T_dock_tf = - lc::PoseToTF(world_T_dock, "world", "dock/body", lc::TimeFromRosTime(ros::Time::now()), platform_name_); - // If the rate is higher than the sim time, prevent repeated timestamps - if (world_T_dock_tf.header.stamp == last_time_tf_dock_) return; - last_time_tf_dock_ = world_T_dock_tf.header.stamp; - transform_pub_.sendTransform(world_T_dock_tf); -} - -void GraphLocalizerNodelet::PublishWorldTHandrailTF() { - const auto world_T_handrail = graph_localizer_wrapper_.estimated_world_T_handrail(); - if (!world_T_handrail) return; - const auto world_T_handrail_tf = lc::PoseToTF(world_T_handrail->pose, "world", "handrail/body", - lc::TimeFromRosTime(ros::Time::now()), platform_name_); - // If the rate is higher than the sim time, prevent repeated timestamps - if (world_T_handrail_tf.header.stamp == last_time_tf_handrail_) return; - last_time_tf_handrail_ = world_T_handrail_tf.header.stamp; - transform_pub_.sendTransform(world_T_handrail_tf); -} - -void GraphLocalizerNodelet::PublishReset() const { - std_msgs::Empty msg; - reset_pub_.publish(msg); -} - -void GraphLocalizerNodelet::PublishHeartbeat() { - heartbeat_.header.stamp = ros::Time::now(); - if ((heartbeat_.header.stamp - last_heartbeat_time_).toSec() < 1.0) return; - heartbeat_pub_.publish(heartbeat_); - last_heartbeat_time_ = heartbeat_.header.stamp; -} - -void GraphLocalizerNodelet::PublishGraphMessages() { - if (!localizer_enabled()) return; - - // Publish loc information here since graph updates occur on optical flow updates - PublishLocalizationState(); - if (graph_localizer_wrapper_.publish_localization_graph()) PublishLocalizationGraph(); - if (graph_localizer_wrapper_.save_localization_graph_dot_file()) - graph_localizer_wrapper_.SaveLocalizationGraphDotFile(); -} - -void GraphLocalizerNodelet::Run() { - ros::Rate rate(100); - // Load Biases from file by default - // Biases reestimated if a intialize bias service call is received - ResetBiasesFromFileAndResetLocalizer(); - while (ros::ok()) { - nodelet_runtime_timer_.Start(); - callbacks_timer_.Start(); - private_queue_.callAvailable(); - callbacks_timer_.Stop(); - graph_localizer_wrapper_.Update(); - nodelet_runtime_timer_.Stop(); - PublishGraphMessages(); - PublishHeartbeat(); - rate.sleep(); - } -} -} // namespace graph_localizer - -PLUGINLIB_EXPORT_CLASS(graph_localizer::GraphLocalizerNodelet, nodelet::Nodelet); diff --git a/localization/graph_localizer/src/graph_localizer_stats.cc b/localization/graph_localizer/src/graph_localizer_stats.cc deleted file mode 100644 index 3a4b50dedf..0000000000 --- a/localization/graph_localizer/src/graph_localizer_stats.cc +++ /dev/null @@ -1,175 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#include <graph_localizer/graph_localizer_stats.h> -#include <graph_localizer/point_to_point_between_factor.h> -#include <graph_localizer/loc_projection_factor.h> -#include <graph_localizer/loc_pose_factor.h> -#include <graph_localizer/pose_rotation_factor.h> -#include <graph_localizer/robust_smart_projection_pose_factor.h> -#include <graph_localizer/utilities.h> -#include <graph_optimizer/utilities.h> - -#include <gtsam/geometry/Cal3_S2.h> -#include <gtsam/geometry/PinholePose.h> -#include <gtsam/nonlinear/LinearContainerFactor.h> -#include <gtsam/slam/BetweenFactor.h> -#include <gtsam/slam/ProjectionFactor.h> -#include <gtsam/slam/PriorFactor.h> - -namespace graph_localizer { -namespace go = graph_optimizer; -GraphLocalizerStats::GraphLocalizerStats() { - AddStatsAverager(num_states_averager_); - AddStatsAverager(duration_averager_); - AddStatsAverager(num_marginal_factors_averager_); - AddStatsAverager(num_factors_averager_); - // AddStatsAverager(num_features_averager_); - AddStatsAverager(num_depth_odometry_rel_pose_factors_averager_); - AddStatsAverager(num_depth_odometry_rel_point_factors_averager_); - AddStatsAverager(num_optical_flow_factors_averager_); - AddStatsAverager(num_loc_pose_factors_averager_); - AddStatsAverager(num_loc_proj_factors_averager_); - AddStatsAverager(num_imu_factors_averager_); - AddStatsAverager(num_rotation_factors_averager_); - AddStatsAverager(num_standstill_between_factors_averager_); - AddStatsAverager(num_vel_prior_factors_averager_); - - AddErrorAverager(depth_odom_rel_pose_error_averager_); - AddErrorAverager(depth_odom_rel_point_error_averager_); - AddErrorAverager(of_error_averager_); - AddErrorAverager(loc_proj_error_averager_); - AddErrorAverager(loc_pose_error_averager_); - AddErrorAverager(imu_error_averager_); - AddErrorAverager(rotation_error_averager_); - AddErrorAverager(standstill_between_error_averager_); - AddErrorAverager(pose_prior_error_averager_); - AddErrorAverager(velocity_prior_error_averager_); - AddErrorAverager(bias_prior_error_averager_); -} - -void GraphLocalizerStats::SetCombinedNavStateGraphValues( - std::shared_ptr<const CombinedNavStateGraphValues> combined_nav_state_graph_values) { - combined_nav_state_graph_values_ = std::move(combined_nav_state_graph_values); -} - -void GraphLocalizerStats::UpdateErrors(const gtsam::NonlinearFactorGraph& graph_factors) { - using Calibration = gtsam::Cal3_S2; - using Camera = gtsam::PinholePose<Calibration>; - using RobustSmartFactor = gtsam::RobustSmartProjectionPoseFactor<Calibration>; - using ProjectionFactor = gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3>; - - double total_error = 0; - double depth_odom_rel_pose_factor_error = 0; - double depth_odom_rel_point_factor_error = 0; - double optical_flow_factor_error = 0; - double loc_proj_error = 0; - double loc_pose_error = 0; - double imu_factor_error = 0; - double rotation_factor_error = 0; - double standstill_between_factor_error = 0; - double pose_prior_error = 0; - double velocity_prior_error = 0; - double bias_prior_error = 0; - for (const auto& factor : graph_factors) { - const double error = factor->error(combined_nav_state_graph_values_->values()); - total_error += error; - // TODO(rsoussan): Differentiate between this and standstill between factor - const auto depth_odom_rel_pose_factor = dynamic_cast<const gtsam::BetweenFactor<gtsam::Pose3>*>(factor.get()); - if (depth_odom_rel_pose_factor) { - depth_odom_rel_pose_factor_error += error; - } - const auto depth_odom_rel_point_factor = dynamic_cast<const gtsam::PointToPointBetweenFactor*>(factor.get()); - if (depth_odom_rel_point_factor) { - depth_odom_rel_point_factor_error += error; - } - const auto smart_factor = dynamic_cast<const RobustSmartFactor*>(factor.get()); - if (smart_factor) { - optical_flow_factor_error += error; - } - const auto projection_factor = dynamic_cast<const ProjectionFactor*>(factor.get()); - if (projection_factor) { - optical_flow_factor_error += error; - } - const auto imu_factor = dynamic_cast<gtsam::CombinedImuFactor*>(factor.get()); - if (imu_factor) { - imu_factor_error += error; - } - const auto loc_factor = dynamic_cast<gtsam::LocProjectionFactor<>*>(factor.get()); - if (loc_factor) { - loc_proj_error += error; - } - const auto loc_pose_factor = dynamic_cast<gtsam::LocPoseFactor*>(factor.get()); - if (loc_pose_factor) { - loc_pose_error += error; - } - const auto rotation_factor = dynamic_cast<gtsam::PoseRotationFactor*>(factor.get()); - if (rotation_factor) { - rotation_factor_error += error; - } - const auto standstill_between_factor = dynamic_cast<gtsam::BetweenFactor<gtsam::Pose3>*>(factor.get()); - if (standstill_between_factor) { - standstill_between_factor_error += error; - } - - // Prior Factors - const auto pose_prior_factor = dynamic_cast<gtsam::PriorFactor<gtsam::Pose3>*>(factor.get()); - if (pose_prior_factor && !loc_pose_factor) { - pose_prior_error += error; - } - const auto velocity_prior_factor = dynamic_cast<gtsam::PriorFactor<gtsam::Velocity3>*>(factor.get()); - if (velocity_prior_factor) { - velocity_prior_error += error; - } - const auto bias_prior_factor = dynamic_cast<gtsam::PriorFactor<gtsam::imuBias::ConstantBias>*>(factor.get()); - if (bias_prior_factor) { - bias_prior_error += error; - } - } - total_error_averager_.Update(total_error); - depth_odom_rel_pose_error_averager_.Update(depth_odom_rel_pose_factor_error); - depth_odom_rel_point_error_averager_.Update(depth_odom_rel_point_factor_error); - of_error_averager_.Update(optical_flow_factor_error); - loc_proj_error_averager_.Update(loc_proj_error); - loc_pose_error_averager_.Update(loc_pose_error); - imu_error_averager_.Update(imu_factor_error); - rotation_error_averager_.Update(rotation_factor_error); - standstill_between_error_averager_.Update(standstill_between_factor_error); - pose_prior_error_averager_.Update(pose_prior_error); - velocity_prior_error_averager_.Update(velocity_prior_error); - bias_prior_error_averager_.Update(bias_prior_error); -} - -void GraphLocalizerStats::UpdateStats(const gtsam::NonlinearFactorGraph& graph_factors) { - num_states_averager_.Update(combined_nav_state_graph_values_->NumStates()); - duration_averager_.Update(combined_nav_state_graph_values_->Duration()); - num_marginal_factors_averager_.Update(go::NumFactors<gtsam::LinearContainerFactor>(graph_factors)); - num_factors_averager_.Update(graph_factors.size()); - num_optical_flow_factors_averager_.Update( - NumSmartFactors(graph_factors, combined_nav_state_graph_values_->values(), true)); - num_depth_odometry_rel_pose_factors_averager_.Update( - go::NumFactors<gtsam::BetweenFactor<gtsam::Pose3>>(graph_factors)); - num_depth_odometry_rel_point_factors_averager_.Update( - go::NumFactors<gtsam::PointToPointBetweenFactor>(graph_factors)); - num_loc_pose_factors_averager_.Update(go::NumFactors<gtsam::LocPoseFactor>(graph_factors)); - num_loc_proj_factors_averager_.Update(go::NumFactors<gtsam::LocProjectionFactor<>>(graph_factors)); - num_imu_factors_averager_.Update(go::NumFactors<gtsam::CombinedImuFactor>(graph_factors)); - num_rotation_factors_averager_.Update(go::NumFactors<gtsam::PoseRotationFactor>(graph_factors)); - num_standstill_between_factors_averager_.Update(go::NumFactors<gtsam::BetweenFactor<gtsam::Pose3>>(graph_factors)); - num_vel_prior_factors_averager_.Update(go::NumFactors<gtsam::PriorFactor<gtsam::Velocity3>>(graph_factors)); -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/graph_localizer_wrapper.cc b/localization/graph_localizer/src/graph_localizer_wrapper.cc deleted file mode 100644 index d2be170da8..0000000000 --- a/localization/graph_localizer/src/graph_localizer_wrapper.cc +++ /dev/null @@ -1,414 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <config_reader/config_reader.h> -#include <graph_localizer/graph_localizer_wrapper.h> -#include <graph_localizer/parameter_reader.h> -#include <graph_localizer/utilities.h> -#include <imu_integration/utilities.h> -#include <localization_common/logger.h> -#include <localization_common/utilities.h> -#include <localization_measurements/measurement_conversions.h> -#include <msg_conversions/msg_conversions.h> - -#include <Eigen/Core> - -namespace graph_localizer { -namespace ii = imu_integration; -namespace lc = localization_common; -namespace lm = localization_measurements; -namespace mc = msg_conversions; - -GraphLocalizerWrapper::GraphLocalizerWrapper(const std::string& graph_config_path_prefix) - : reset_world_T_dock_(false), reset_world_T_handrail_(false), fan_speed_mode_(lm::FanSpeedMode::kNominal) { - config_reader::ConfigReader config; - lc::LoadGraphLocalizerConfig(config, graph_config_path_prefix); - config.AddFile("transforms.config"); - config.AddFile("cameras.config"); - config.AddFile("geometry.config"); - config.AddFile("localization/handrail_detect.config"); - - if (!config.ReadFiles()) { - LogFatal("Failed to read config files."); - } - - if (!config.GetBool("publish_localization_graph", &publish_localization_graph_)) { - LogFatal("Failed to load publish_localization_graph."); - } - - if (!config.GetBool("save_localization_graph_dot_file", &save_localization_graph_dot_file_)) { - LogFatal("Failed to load save_localization_graph_dot_file."); - } - - position_cov_log_det_lost_threshold_ = mc::LoadDouble(config, "position_cov_log_det_lost_threshold"); - orientation_cov_log_det_lost_threshold_ = mc::LoadDouble(config, "orientation_cov_log_det_lost_threshold"); - - graph_localizer_initializer_.LoadGraphLocalizerParams(config); - SanityCheckerParams sanity_checker_params; - LoadSanityCheckerParams(config, sanity_checker_params); - sanity_checker_.reset(new SanityChecker(sanity_checker_params)); - // Initialize with config. Optionally update during localization - // TODO(rsoussan): Make graph localizer wrapper config file and load fcn in parameter reader - estimated_world_T_dock_ = lc::LoadTransform(config, "world_dock_transform"); - estimate_world_T_dock_using_loc_ = mc::LoadBool(config, "estimate_world_T_dock_using_loc"); - sparse_mapping_min_num_landmarks_ = mc::LoadInt(config, "loc_adder_min_num_matches"); - ar_min_num_landmarks_ = mc::LoadInt(config, "ar_tag_loc_adder_min_num_matches"); -} - -bool GraphLocalizerWrapper::Initialized() const { return (graph_localizer_.get() != nullptr); } - -void GraphLocalizerWrapper::Update() { - if (graph_localizer_) { - graph_localizer_->Update(); - // Sanity check covariances after updates - if (!CheckCovarianceSanity()) { - LogError("OpticalFlowCallback: Covariance sanity check failed, resetting localizer."); - ResetLocalizer(); - return; - } - } -} - -void GraphLocalizerWrapper::OpticalFlowCallback(const ff_msgs::Feature2dArray& feature_array_msg) { - feature_counts_.of = feature_array_msg.feature_array.size(); - if (graph_localizer_) { - graph_localizer_->AddOpticalFlowMeasurement(lm::MakeFeaturePointsMeasurement(feature_array_msg)); - } -} - -void GraphLocalizerWrapper::ResetLocalizer() { - LogInfo("ResetLocalizer: Resetting localizer."); - graph_localizer_initializer_.ResetStartPose(); - if (!latest_biases_) { - LogError( - "ResetLocalizer: Trying to reset localizer when no biases " - "are available."); - return; - } - - // TODO(rsoussan): compare current time with latest bias timestamp and print - // warning if it is too old - graph_localizer_initializer_.SetBiases(*latest_biases_, true); - graph_localizer_.reset(); - sanity_checker_->Reset(); -} - -void GraphLocalizerWrapper::ResetBiasesAndLocalizer() { - LogInfo("ResetBiasAndLocalizer: Resetting biases and localizer."); - graph_localizer_initializer_.ResetBiasesAndStartPose(); - graph_localizer_.reset(); - sanity_checker_->Reset(); -} - -void GraphLocalizerWrapper::ResetBiasesFromFileAndResetLocalizer() { - LogInfo("ResetBiasAndLocalizer: Resetting biases from file and resetting localizer."); - graph_localizer_initializer_.ResetBiasesFromFileAndResetStartPose(); - if (graph_localizer_initializer_.HasBiases()) - latest_biases_ = graph_localizer_initializer_.params().graph_initializer.initial_imu_bias; - graph_localizer_.reset(); - sanity_checker_->Reset(); -} - -void GraphLocalizerWrapper::VLVisualLandmarksCallback(const ff_msgs::VisualLandmarks& visual_landmarks_msg) { - feature_counts_.vl = visual_landmarks_msg.landmarks.size(); - if (!ValidVLMsg(visual_landmarks_msg, sparse_mapping_min_num_landmarks_)) return; - if (graph_localizer_) { - graph_localizer_->AddSparseMappingMeasurement(lm::MakeMatchedProjectionsMeasurement(visual_landmarks_msg)); - } - - const gtsam::Pose3 sparse_mapping_global_T_body = lc::PoseFromMsgWithExtrinsics( - visual_landmarks_msg.pose, graph_localizer_initializer_.params().calibration.body_T_nav_cam.inverse()); - const lc::Time timestamp = lc::TimeFromHeader(visual_landmarks_msg.header); - sparse_mapping_pose_ = lm::TimestampedPose(sparse_mapping_global_T_body, timestamp); - - // Sanity Check - if (graph_localizer_ && !CheckPoseSanity(sparse_mapping_global_T_body, timestamp)) { - LogError("VLVisualLandmarksCallback: Sanity check failed, resetting localizer."); - ResetLocalizer(); - return; - } - - if (!graph_localizer_) { - // Set or update initial pose if a new one is available before the localizer - // has started running. - graph_localizer_initializer_.SetStartPose(*sparse_mapping_pose_); - // Set fan speed mode as well in case this hasn't been set yet - // TODO(rsoussan): Do this in a cleaner way - graph_localizer_initializer_.SetFanSpeedMode(fan_speed_mode_); - } -} - -bool GraphLocalizerWrapper::CheckPoseSanity(const gtsam::Pose3& sparse_mapping_pose, const lc::Time timestamp) const { - if (!graph_localizer_) return true; - const auto latest_extrapolated_pose_time = graph_localizer_->LatestExtrapolatedPoseTime(); - if (!latest_extrapolated_pose_time) { - LogDebug("CheckPoseSanity: Failed to get latest extrapolated pose time."); - return true; - } - if (timestamp > *latest_extrapolated_pose_time) { - LogDebug("CheckPoseSanity: Timestamp occurs after latest extrapolated pose time"); - return true; - } - const auto combined_nav_state = graph_localizer_->GetCombinedNavState(timestamp); - if (!combined_nav_state) { - LogDebugEveryN(50, "CheckPoseSanity: Failed to get combined nav state."); - return true; - } - return sanity_checker_->CheckPoseSanity(sparse_mapping_pose, combined_nav_state->pose()); -} - -bool GraphLocalizerWrapper::CheckCovarianceSanity() const { - if (!graph_localizer_) return true; - const auto combined_nav_state_and_covariances = graph_localizer_->LatestCombinedNavStateAndCovariances(); - if (!combined_nav_state_and_covariances) { - LogDebugEveryN(50, "CheckCovarianceSanity: No combined nav state and covariances available."); - return true; - } - - return sanity_checker_->CheckCovarianceSanity(combined_nav_state_and_covariances->second); -} - -void GraphLocalizerWrapper::ARVisualLandmarksCallback(const ff_msgs::VisualLandmarks& visual_landmarks_msg) { - feature_counts_.ar = visual_landmarks_msg.landmarks.size(); - if (!ValidVLMsg(visual_landmarks_msg, ar_min_num_landmarks_)) return; - if (graph_localizer_) { - if (reset_world_T_dock_) { - ResetWorldTDockUsingLoc(visual_landmarks_msg); - reset_world_T_dock_ = false; - } - const auto frame_changed_ar_measurements = lm::FrameChangeMatchedProjectionsMeasurement( - lm::MakeMatchedProjectionsMeasurement(visual_landmarks_msg), estimated_world_T_dock_); - graph_localizer_->AddARTagMeasurement(frame_changed_ar_measurements); - ar_tag_pose_ = lm::TimestampedPose(frame_changed_ar_measurements.global_T_cam * - graph_localizer_initializer_.params().calibration.body_T_dock_cam.inverse(), - frame_changed_ar_measurements.timestamp); - } -} - -void GraphLocalizerWrapper::DepthOdometryCallback(const ff_msgs::DepthOdometry& depth_odometry_msg) { - if (graph_localizer_) { - const auto depth_odometry_measurement = lm::MakeDepthOdometryMeasurement(depth_odometry_msg); - graph_localizer_->AddDepthOdometryMeasurement(depth_odometry_measurement); - } -} - -void GraphLocalizerWrapper::DepthLandmarksCallback(const ff_msgs::DepthLandmarks& depth_landmarks_msg) { - feature_counts_.depth = depth_landmarks_msg.landmarks.size(); - if (!ValidDepthMsg(depth_landmarks_msg)) return; - if (graph_localizer_) { - ResetWorldTHandrailIfNecessary(depth_landmarks_msg); - if (!estimated_world_T_handrail_) { - LogWarning("DepthLandmarksCallback: No estimated world_T_handrail pose available."); - return; - } - const auto handrail_points_measurement = - lm::MakeHandrailPointsMeasurement(depth_landmarks_msg, *estimated_world_T_handrail_); - graph_localizer_->AddHandrailMeasurement(handrail_points_measurement); - // TODO(rsoussan): Don't update a pose with endpoints with a new measurement without endpoints? - if (estimated_world_T_handrail_) { - const auto handrail_T_perch_cam = lc::PoseFromMsg(depth_landmarks_msg.sensor_T_handrail).inverse(); - // 0 value is default, 1 means endpoints seen, 2 means none seen - // TODO(rsoussan): Change this once this is changed in handrail node - const bool accurate_z_position = depth_landmarks_msg.end_seen == 1; - handrail_pose_ = - lm::TimestampedHandrailPose(estimated_world_T_handrail_->pose * handrail_T_perch_cam * - graph_localizer_initializer_.params().calibration.body_T_perch_cam.inverse(), - handrail_points_measurement.timestamp, accurate_z_position, - graph_localizer_initializer_.params().handrail.length, - graph_localizer_initializer_.params().handrail.distance_to_wall); - } - } -} - -void GraphLocalizerWrapper::ImuCallback(const sensor_msgs::Imu& imu_msg) { - if (graph_localizer_) { - graph_localizer_->AddImuMeasurement(lm::ImuMeasurement(imu_msg)); - const auto latest_biases = graph_localizer_->LatestBiases(); - if (!latest_biases) { - LogError("ImuCallback: Failed to get latest biases."); - } else { - latest_biases_ = latest_biases->first; - } - } else if (graph_localizer_initializer_.EstimateBiases()) { - graph_localizer_initializer_.EstimateAndSetImuBiases(lm::ImuMeasurement(imu_msg), fan_speed_mode_); - if (graph_localizer_initializer_.HasBiases()) - latest_biases_ = graph_localizer_initializer_.params().graph_initializer.initial_imu_bias; - } - - if (!graph_localizer_ && graph_localizer_initializer_.ReadyToInitialize()) { - InitializeGraph(); - LogDebug("ImuCallback: Initialized Graph."); - } -} - -void GraphLocalizerWrapper::FlightModeCallback(const ff_msgs::FlightMode& flight_mode) { - fan_speed_mode_ = lm::ConvertFanSpeedMode(flight_mode.speed); - if (graph_localizer_) graph_localizer_->SetFanSpeedMode(fan_speed_mode_); - graph_localizer_initializer_.SetFanSpeedMode(fan_speed_mode_); -} - -void GraphLocalizerWrapper::InitializeGraph() { - if (!graph_localizer_initializer_.ReadyToInitialize()) { - LogDebug("InitializeGraph: Trying to initialize graph when not ready."); - return; - } - - graph_localizer_.reset(new graph_localizer::GraphLocalizer(graph_localizer_initializer_.params())); -} - -boost::optional<const FeatureTrackIdMap&> GraphLocalizerWrapper::feature_tracks() const { - if (!graph_localizer_) return boost::none; - return graph_localizer_->feature_tracks(); -} - -boost::optional<const GraphLocalizer&> GraphLocalizerWrapper::graph_localizer() const { - if (!graph_localizer_) return boost::none; - return *graph_localizer_; -} - -void GraphLocalizerWrapper::MarkWorldTDockForResettingIfNecessary() { - if (estimate_world_T_dock_using_loc_) reset_world_T_dock_ = true; -} - -void GraphLocalizerWrapper::MarkWorldTHandrailForResetting() { reset_world_T_handrail_ = true; } - -void GraphLocalizerWrapper::ResetWorldTDockUsingLoc(const ff_msgs::VisualLandmarks& visual_landmarks_msg) { - const auto latest_combined_nav_state = LatestCombinedNavState(); - if (!latest_combined_nav_state) { - LogError("ResetWorldTDockIfNecessary: Failed to get latest combined nav state."); - return; - } - // TODO(rsoussan): Extrapolate latest world_T_body loc estimate with imu data? - const gtsam::Pose3 dock_T_body = lc::PoseFromMsgWithExtrinsics( - visual_landmarks_msg.pose, graph_localizer_initializer_.params().calibration.body_T_dock_cam.inverse()); - estimated_world_T_dock_ = latest_combined_nav_state->pose() * dock_T_body.inverse(); -} - -void GraphLocalizerWrapper::ResetWorldTHandrailIfNecessary(const ff_msgs::DepthLandmarks& depth_landmarks_msg) { - const bool accurate_z_position = depth_landmarks_msg.end_seen == 1; - // Update old handrail estimate with new one if an accurate z position is now avaible and wasn't previously - const bool update_with_new_z_position = - accurate_z_position && estimated_world_T_handrail_ && !estimated_world_T_handrail_->accurate_z_position; - if (!reset_world_T_handrail_ && !update_with_new_z_position) return; - - const auto latest_combined_nav_state = LatestCombinedNavState(); - if (!latest_combined_nav_state) { - LogError("ResetWorldTDockIfNecessary: Failed to get latest combined nav state."); - return; - } - // TODO(rsoussan): Extrapolate latest world_T_body loc estimate with imu data? - const auto handrail_T_perch_cam = lc::PoseFromMsg(depth_landmarks_msg.sensor_T_handrail).inverse(); - const gtsam::Pose3 handrail_T_body = - handrail_T_perch_cam * graph_localizer_initializer_.params().calibration.body_T_perch_cam.inverse(); - estimated_world_T_handrail_ = lm::TimestampedHandrailPose( - latest_combined_nav_state->pose() * handrail_T_body.inverse(), latest_combined_nav_state->timestamp(), - accurate_z_position, graph_localizer_initializer_.params().handrail.length, - graph_localizer_initializer_.params().handrail.distance_to_wall); - reset_world_T_handrail_ = false; -} - -gtsam::Pose3 GraphLocalizerWrapper::estimated_world_T_dock() const { return estimated_world_T_dock_; } - -boost::optional<lm::TimestampedHandrailPose> GraphLocalizerWrapper::estimated_world_T_handrail() const { - return estimated_world_T_handrail_; -} - -boost::optional<geometry_msgs::PoseStamped> GraphLocalizerWrapper::LatestSparseMappingPoseMsg() const { - if (!sparse_mapping_pose_) { - LogWarningEveryN(50, "LatestSparseMappingPoseMsg: Failed to get latest sparse mapping pose msg."); - return boost::none; - } - - return PoseMsg(*sparse_mapping_pose_); -} - -boost::optional<geometry_msgs::PoseStamped> GraphLocalizerWrapper::LatestARTagPoseMsg() const { - if (!ar_tag_pose_) { - LogWarningEveryN(50, "LatestARTagPoseMsg: Failed to get latest ar tag pose msg."); - return boost::none; - } - - return PoseMsg(*ar_tag_pose_); -} - -boost::optional<geometry_msgs::PoseStamped> GraphLocalizerWrapper::LatestHandrailPoseMsg() const { - if (!handrail_pose_) { - LogWarningEveryN(50, "LatestHandrailPoseMsg: Failed to get latest handrail pose msg."); - return boost::none; - } - - return PoseMsg(*handrail_pose_); -} - -boost::optional<lc::CombinedNavState> GraphLocalizerWrapper::LatestCombinedNavState() const { - if (!graph_localizer_) { - LogWarningEveryN(50, "LatestCombinedNavState: Graph localizater not initialized yet."); - return boost::none; - } - const auto latest_combined_nav_state = graph_localizer_->LatestCombinedNavState(); - if (!latest_combined_nav_state) { - LogWarningEveryN(50, "LatestCombinedNavState: No combined nav state available."); - return boost::none; - } - return latest_combined_nav_state; -} - -boost::optional<ff_msgs::GraphState> GraphLocalizerWrapper::LatestLocalizationStateMsg() { - if (!graph_localizer_) { - LogDebugEveryN(50, "LatestLocalizationMsg: Graph localizater not initialized yet."); - return boost::none; - } - const auto combined_nav_state_and_covariances = graph_localizer_->LatestCombinedNavStateAndCovariances(); - if (!combined_nav_state_and_covariances) { - LogDebugEveryN(50, "LatestLocalizationMsg: No combined nav state and covariances available."); - return boost::none; - } - const auto graph_state_msg = - GraphStateMsg(combined_nav_state_and_covariances->first, combined_nav_state_and_covariances->second, - feature_counts_, graph_localizer_initializer_.EstimateBiases(), position_cov_log_det_lost_threshold_, - orientation_cov_log_det_lost_threshold_, graph_localizer_->standstill(), - graph_localizer_->graph_localizer_stats(), graph_localizer_->fan_speed_mode()); - feature_counts_.Reset(); - return graph_state_msg; -} - -boost::optional<ff_msgs::LocalizationGraph> GraphLocalizerWrapper::LatestLocalizationGraphMsg() const { - if (!graph_localizer_) { - LogWarningEveryN(50, "LatestGraphMsg: Graph localizater not initialized yet."); - return boost::none; - } - return GraphMsg(*graph_localizer_); -} - -void GraphLocalizerWrapper::SaveLocalizationGraphDotFile() const { - if (graph_localizer_) graph_localizer_->SaveGraphDotFile(); -} - -boost::optional<const GraphLocalizerStats&> GraphLocalizerWrapper::graph_localizer_stats() const { - if (!graph_localizer_) { - LogDebug("GraphStats: Failed to get graph stats."); - return boost::none; - } - return graph_localizer_->graph_localizer_stats(); -} - -bool GraphLocalizerWrapper::publish_localization_graph() const { return publish_localization_graph_; } - -bool GraphLocalizerWrapper::save_localization_graph_dot_file() const { return save_localization_graph_dot_file_; } - -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/handrail_factor_adder.cc b/localization/graph_localizer/src/handrail_factor_adder.cc deleted file mode 100644 index 88f6b881a4..0000000000 --- a/localization/graph_localizer/src/handrail_factor_adder.cc +++ /dev/null @@ -1,154 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/handrail_factor_adder.h> -#include <graph_localizer/point_to_handrail_endpoint_factor.h> -#include <graph_localizer/point_to_line_factor.h> -#include <graph_localizer/point_to_line_segment_factor.h> -#include <graph_localizer/point_to_plane_factor.h> -#include <graph_localizer/utilities.h> -#include <localization_common/logger.h> - -#include <gtsam/inference/Symbol.h> - -namespace graph_localizer { -namespace go = graph_optimizer; -namespace lm = localization_measurements; -namespace sym = gtsam::symbol_shorthand; -HandrailFactorAdder::HandrailFactorAdder(const HandrailFactorAdderParams& params) : HandrailFactorAdder::Base(params) {} - -void HandrailFactorAdder::AddPointToLineOrLineSegmentFactors( - const lm::HandrailPointsMeasurement& handrail_points_measurement, std::vector<go::FactorsToAdd>& factors_to_add) { - const int num_line_measurements = static_cast<int>(handrail_points_measurement.sensor_t_line_points.size()); - if (num_line_measurements < params().min_num_line_matches) { - LogDebug("AddPointToLineOrLineSegmentFactors: Not enough handrail line measurements."); - return; - } - go::FactorsToAdd point_to_line_or_line_segment_factors_to_add; - point_to_line_or_line_segment_factors_to_add.reserve(num_line_measurements); - point_to_line_or_line_segment_factors_to_add.SetTimestamp(handrail_points_measurement.timestamp); - const gtsam::Vector2 point_to_line_noise_sigmas( - (gtsam::Vector(2) << params().point_to_line_stddev, params().point_to_line_stddev).finished()); - const auto point_to_line_noise = - Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(point_to_line_noise_sigmas)), - params().huber_k); - const gtsam::Vector3 point_to_line_segment_noise_sigmas( - (gtsam::Vector(3) << params().point_to_line_stddev, params().point_to_line_stddev, params().point_to_line_stddev) - .finished()); - const auto point_to_line_segment_noise = - Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(point_to_line_segment_noise_sigmas)), - params().huber_k); - const go::KeyInfo key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, handrail_points_measurement.timestamp); - for (const auto& sensor_t_line_point : handrail_points_measurement.sensor_t_line_points) { - if (handrail_points_measurement.world_T_handrail.accurate_z_position) { - gtsam::PointToLineSegmentFactor::shared_ptr point_to_line_segment_factor(new gtsam::PointToLineSegmentFactor( - sensor_t_line_point, handrail_points_measurement.world_T_handrail.pose, params().body_T_perch_cam, - handrail_points_measurement.world_T_handrail.length, point_to_line_segment_noise, key_info.UninitializedKey())); - point_to_line_or_line_segment_factors_to_add.push_back({{key_info}, point_to_line_segment_factor}); - } else { - gtsam::PointToLineFactor::shared_ptr point_to_line_factor( - new gtsam::PointToLineFactor(sensor_t_line_point, handrail_points_measurement.world_T_handrail.pose, - params().body_T_perch_cam, point_to_line_noise, key_info.UninitializedKey())); - point_to_line_or_line_segment_factors_to_add.push_back({{key_info}, point_to_line_factor}); - } - } - if (handrail_points_measurement.world_T_handrail.accurate_z_position) { - LogDebug("AddPointToLineOrLineSegmentFactors: Added " << point_to_line_or_line_segment_factors_to_add.size() - << " point to line segment factors."); - } else { - LogDebug("AddPointToLineOrLineSegmentFactors: Added " << point_to_line_or_line_segment_factors_to_add.size() - << " point to line factors."); - } - factors_to_add.emplace_back(point_to_line_or_line_segment_factors_to_add); -} - -void HandrailFactorAdder::AddPointToPlaneFactors(const lm::HandrailPointsMeasurement& handrail_points_measurement, - std::vector<go::FactorsToAdd>& factors_to_add) { - const int num_plane_measurements = static_cast<int>(handrail_points_measurement.sensor_t_plane_points.size()); - if (num_plane_measurements < params().min_num_plane_matches) { - LogDebug("AddPointToPlaneFactors: Not enough handrail plane measurements."); - return; - } - - go::FactorsToAdd point_to_plane_factors_to_add; - point_to_plane_factors_to_add.reserve(num_plane_measurements); - point_to_plane_factors_to_add.SetTimestamp(handrail_points_measurement.timestamp); - const gtsam::Vector1 point_to_plane_noise_sigmas((gtsam::Vector(1) << params().point_to_plane_stddev).finished()); - const auto point_to_plane_noise = - Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(point_to_plane_noise_sigmas)), - params().huber_k); - const go::KeyInfo key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, handrail_points_measurement.timestamp); - for (const auto& sensor_t_plane_point : handrail_points_measurement.sensor_t_plane_points) { - gtsam::PointToPlaneFactor::shared_ptr point_to_plane_factor( - new gtsam::PointToPlaneFactor(sensor_t_plane_point, handrail_points_measurement.world_T_handrail_plane, - params().body_T_perch_cam, point_to_plane_noise, key_info.UninitializedKey())); - point_to_plane_factors_to_add.push_back({{key_info}, point_to_plane_factor}); - } - LogDebug("AddPointToPlaneFactors: Added " << point_to_plane_factors_to_add.size() << " point to plane factors."); - factors_to_add.emplace_back(point_to_plane_factors_to_add); -} - -void HandrailFactorAdder::AddPointToHandrailEndpointFactors( - const lm::HandrailPointsMeasurement& handrail_points_measurement, std::vector<go::FactorsToAdd>& factors_to_add) { - const int num_handrail_endpoint_measurements = - static_cast<int>(handrail_points_measurement.sensor_t_line_endpoints.size()); - if (num_handrail_endpoint_measurements == 0) { - LogDebug("AddPointToHandrailEndpointFactors: Not enough handrail endpoint measurements."); - return; - } - - go::FactorsToAdd point_to_handrail_endpoint_factors_to_add; - point_to_handrail_endpoint_factors_to_add.reserve(num_handrail_endpoint_measurements); - point_to_handrail_endpoint_factors_to_add.SetTimestamp(handrail_points_measurement.timestamp); - const gtsam::Vector3 point_to_handrail_endpoint_noise_sigmas( - (gtsam::Vector(3) << params().point_to_line_stddev, params().point_to_line_stddev, params().point_to_line_stddev) - .finished()); - const auto point_to_handrail_endpoint_noise = Robust( - gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(point_to_handrail_endpoint_noise_sigmas)), - params().huber_k); - const go::KeyInfo key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, handrail_points_measurement.timestamp); - for (const auto& sensor_t_handrail_endpoint : handrail_points_measurement.sensor_t_line_endpoints) { - gtsam::PointToHandrailEndpointFactor::shared_ptr point_to_handrail_endpoint_factor( - new gtsam::PointToHandrailEndpointFactor( - sensor_t_handrail_endpoint, handrail_points_measurement.world_t_handrail_endpoints->first, - handrail_points_measurement.world_t_handrail_endpoints->second, params().body_T_perch_cam, - point_to_handrail_endpoint_noise, key_info.UninitializedKey())); - point_to_handrail_endpoint_factors_to_add.push_back({{key_info}, point_to_handrail_endpoint_factor}); - } - LogDebug("AddPointToHandrailEndpointFactors: Added " << point_to_handrail_endpoint_factors_to_add.size() - << " point to handrail endpoint factors."); - factors_to_add.emplace_back(point_to_handrail_endpoint_factors_to_add); -} - -std::vector<go::FactorsToAdd> HandrailFactorAdder::AddFactors( - const lm::HandrailPointsMeasurement& handrail_points_measurement) { - if (handrail_points_measurement.sensor_t_line_points.empty() && - handrail_points_measurement.sensor_t_plane_points.empty()) { - LogDebug("AddFactors: Empty measurement."); - return {}; - } - - std::vector<go::FactorsToAdd> factors_to_add; - AddPointToLineOrLineSegmentFactors(handrail_points_measurement, factors_to_add); - AddPointToPlaneFactors(handrail_points_measurement, factors_to_add); - if (handrail_points_measurement.world_t_handrail_endpoints) { - AddPointToHandrailEndpointFactors(handrail_points_measurement, factors_to_add); - } - return factors_to_add; -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/loc_factor_adder.cc b/localization/graph_localizer/src/loc_factor_adder.cc deleted file mode 100644 index 0c2f34a574..0000000000 --- a/localization/graph_localizer/src/loc_factor_adder.cc +++ /dev/null @@ -1,109 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/loc_factor_adder.h> -#include <graph_localizer/loc_pose_factor.h> -#include <graph_localizer/loc_projection_factor.h> -#include <graph_localizer/utilities.h> -#include <localization_common/logger.h> - -#include <gtsam/base/Vector.h> - -namespace graph_localizer { -namespace go = graph_optimizer; -namespace lm = localization_measurements; -namespace sym = gtsam::symbol_shorthand; -LocFactorAdder::LocFactorAdder(const LocFactorAdderParams& params, - const go::GraphActionCompleterType graph_action_completer_type) - : LocFactorAdder::Base(params), graph_action_completer_type_(graph_action_completer_type) {} - -std::vector<go::FactorsToAdd> LocFactorAdder::AddFactors( - const lm::MatchedProjectionsMeasurement& matched_projections_measurement) { - if (matched_projections_measurement.matched_projections.empty()) { - LogDebug("AddFactors: Empty measurement."); - return {}; - } - - if (static_cast<int>(matched_projections_measurement.matched_projections.size()) < params().min_num_matches) { - LogDebug("AddFactors: Not enough matches in projection measurement."); - return {}; - } - - const int num_landmarks = matched_projections_measurement.matched_projections.size(); - num_landmarks_averager_.Update(num_landmarks); - std::vector<go::FactorsToAdd> factors_to_add; - if (params().add_pose_priors) { - double noise_scale = params().pose_noise_scale; - if (params().scale_pose_noise_with_num_landmarks) { - noise_scale *= std::pow((num_landmarks_averager_.average() / static_cast<double>(num_landmarks)), 2); - } - - go::FactorsToAdd pose_factors_to_add; - pose_factors_to_add.reserve(1); - const gtsam::Vector6 pose_prior_noise_sigmas((gtsam::Vector(6) << params().prior_translation_stddev, - params().prior_translation_stddev, params().prior_translation_stddev, - params().prior_quaternion_stddev, params().prior_quaternion_stddev, - params().prior_quaternion_stddev) - .finished()); - const auto pose_noise = Robust( - gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(noise_scale * pose_prior_noise_sigmas)), - params().huber_k); - - const go::KeyInfo key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, - matched_projections_measurement.timestamp); - gtsam::LocPoseFactor::shared_ptr pose_prior_factor(new gtsam::LocPoseFactor( - key_info.UninitializedKey(), matched_projections_measurement.global_T_cam * params().body_T_cam.inverse(), - pose_noise)); - pose_factors_to_add.push_back({{key_info}, pose_prior_factor}); - pose_factors_to_add.SetTimestamp(matched_projections_measurement.timestamp); - LogDebug("AddFactors: Added 1 loc pose prior factor."); - factors_to_add.emplace_back(pose_factors_to_add); - } - if (params().add_projections) { - double noise_scale = params().projection_noise_scale; - if (params().scale_projection_noise_with_num_landmarks) { - noise_scale *= std::pow((num_landmarks_averager_.average() / static_cast<double>(num_landmarks)), 2); - } - - int num_loc_projection_factors = 0; - go::FactorsToAdd projection_factors_to_add(type()); - projection_factors_to_add.reserve(matched_projections_measurement.matched_projections.size()); - for (const auto& matched_projection : matched_projections_measurement.matched_projections) { - const go::KeyInfo key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, - matched_projections_measurement.timestamp); - // TODO(rsoussan): Pass sigma insted of already constructed isotropic noise - const gtsam::SharedIsotropic scaled_noise( - gtsam::noiseModel::Isotropic::Sigma(2, noise_scale * params().cam_noise->sigma())); - gtsam::LocProjectionFactor<>::shared_ptr loc_projection_factor(new gtsam::LocProjectionFactor<>( - matched_projection.image_point, matched_projection.map_point, Robust(scaled_noise, params().huber_k), - key_info.UninitializedKey(), params().cam_intrinsics, params().body_T_cam)); - // Set world_T_cam estimate in case need to use it as a fallback - loc_projection_factor->setWorldTCam(matched_projections_measurement.global_T_cam); - projection_factors_to_add.push_back({{key_info}, loc_projection_factor}); - ++num_loc_projection_factors; - if (num_loc_projection_factors >= params().max_num_factors) break; - } - projection_factors_to_add.SetTimestamp(matched_projections_measurement.timestamp); - LogDebug("AddFactors: Added " << num_loc_projection_factors << " loc projection factors."); - factors_to_add.emplace_back(projection_factors_to_add); - } - return factors_to_add; -} - -go::GraphActionCompleterType LocFactorAdder::type() const { return graph_action_completer_type_; } -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/loc_graph_action_completer.cc b/localization/graph_localizer/src/loc_graph_action_completer.cc deleted file mode 100644 index 1cc20e3394..0000000000 --- a/localization/graph_localizer/src/loc_graph_action_completer.cc +++ /dev/null @@ -1,106 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/loc_graph_action_completer.h> -#include <graph_localizer/loc_pose_factor.h> -#include <graph_localizer/loc_projection_factor.h> -#include <graph_localizer/utilities.h> -#include <localization_common/logger.h> - -#include <gtsam/base/Vector.h> - -namespace graph_localizer { -namespace go = graph_optimizer; -namespace lm = localization_measurements; -namespace sym = gtsam::symbol_shorthand; -LocGraphActionCompleter::LocGraphActionCompleter(const LocFactorAdderParams& params, - const go::GraphActionCompleterType graph_action_completer_type, - std::shared_ptr<CombinedNavStateGraphValues> graph_values) - : params_(params), - graph_action_completer_type_(graph_action_completer_type), - graph_values_(std::move(graph_values)) {} - -go::GraphActionCompleterType LocGraphActionCompleter::type() const { return graph_action_completer_type_; } - -bool LocGraphActionCompleter::DoAction(go::FactorsToAdd& factors_to_add, gtsam::NonlinearFactorGraph& graph_factors) { - auto& factors = factors_to_add.Get(); - boost::optional<gtsam::Key> pose_key; - boost::optional<gtsam::Pose3> world_T_cam; - for (auto factor_it = factors.begin(); factor_it != factors.end();) { - auto& factor = factor_it->factor; - auto projection_factor = dynamic_cast<gtsam::LocProjectionFactor<>*>(factor.get()); - if (!projection_factor) { - LogError("MapProjectionNoiseScaling: Failed to cast to projection factor."); - return false; - } - - // Save pose and pose key in case need to make loc prior - pose_key = projection_factor->key(); - world_T_cam = projection_factor->world_T_cam(); - - const auto world_T_body = graph_values_->at<gtsam::Pose3>(projection_factor->key()); - if (!world_T_body) { - LogError("MapProjectionNoiseScaling: Failed to get pose."); - return false; - } - const auto error = (projection_factor->evaluateError(*world_T_body)).norm(); - const auto cheirality_error = projection_factor->cheiralityError(*world_T_body); - if (cheirality_error) { - factor_it = factors.erase(factor_it); - } else if (error > params_.max_inlier_weighted_projection_norm) { - factor_it = factors.erase(factor_it); - } else { - if (params_.weight_projections_with_distance) { - const gtsam::Point3 nav_cam_t_landmark = - (*world_T_body * *(projection_factor->body_P_sensor())).inverse() * projection_factor->landmark_point(); - const gtsam::SharedIsotropic scaled_noise( - gtsam::noiseModel::Isotropic::Sigma(2, params_.projection_noise_scale * 1.0 / nav_cam_t_landmark.z())); - // Don't use robust cost here to more effectively correct a drift occurance - gtsam::LocProjectionFactor<>::shared_ptr loc_projection_factor(new gtsam::LocProjectionFactor<>( - projection_factor->measured(), projection_factor->landmark_point(), scaled_noise, projection_factor->key(), - projection_factor->calibration(), *(projection_factor->body_P_sensor()))); - factor_it->factor = loc_projection_factor; - } - ++factor_it; - } - } - // All factors have been removed due to errors, use loc pose prior instead - if (factors.empty() && params_.add_prior_if_projections_fail) { - if (!pose_key || !world_T_cam) { - LogError("MapProjectionNoiseScaling: Failed to get pose key and world_T_cam"); - return false; - } - const gtsam::Vector6 pose_prior_noise_sigmas((gtsam::Vector(6) << params_.prior_translation_stddev, - params_.prior_translation_stddev, params_.prior_translation_stddev, - params_.prior_quaternion_stddev, params_.prior_quaternion_stddev, - params_.prior_quaternion_stddev) - .finished()); - // TODO(rsoussan): enable scaling with num landmarks - const int noise_scale = 1; - const auto pose_noise = Robust( - gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(noise_scale * pose_prior_noise_sigmas)), - params_.huber_k); - gtsam::LocPoseFactor::shared_ptr pose_prior_factor( - new gtsam::LocPoseFactor(*pose_key, *world_T_cam * params_.body_T_cam.inverse(), pose_noise)); - factors_to_add.push_back(go::FactorToAdd( - {go::KeyInfo(&sym::P, go::NodeUpdaterType::CombinedNavState, factors_to_add.timestamp())}, pose_prior_factor)); - } - return true; -} - -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/parameter_reader.cc b/localization/graph_localizer/src/parameter_reader.cc deleted file mode 100644 index a780f1f1c0..0000000000 --- a/localization/graph_localizer/src/parameter_reader.cc +++ /dev/null @@ -1,273 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/parameter_reader.h> -#include <graph_localizer/utilities.h> -#include <graph_optimizer/parameter_reader.h> -#include <imu_integration/utilities.h> -#include <localization_common/utilities.h> -#include <msg_conversions/msg_conversions.h> - -namespace graph_localizer { -namespace go = graph_optimizer; -namespace ii = imu_integration; -namespace lc = localization_common; -namespace mc = msg_conversions; - -void LoadCalibrationParams(config_reader::ConfigReader& config, CalibrationParams& params) { - params.body_T_dock_cam = lc::LoadTransform(config, "dock_cam_transform"); - params.body_T_nav_cam = lc::LoadTransform(config, "nav_cam_transform"); - params.body_T_perch_cam = lc::LoadTransform(config, "perch_cam_transform"); - params.world_T_dock = lc::LoadTransform(config, "world_dock_transform"); - params.nav_cam_intrinsics.reset(new gtsam::Cal3_S2(lc::LoadCameraIntrinsics(config, "nav_cam"))); - params.dock_cam_intrinsics.reset(new gtsam::Cal3_S2(lc::LoadCameraIntrinsics(config, "dock_cam"))); -} - -void LoadFactorParams(config_reader::ConfigReader& config, FactorParams& params) { - LoadHandrailFactorAdderParams(config, params.handrail_adder); - LoadDepthOdometryFactorAdderParams(config, params.depth_odometry_adder); - LoadLocFactorAdderParams(config, params.loc_adder); - LoadARTagLocFactorAdderParams(config, params.ar_tag_loc_adder); - LoadRotationFactorAdderParams(config, params.rotation_adder); - LoadProjectionFactorAdderParams(config, params.projection_adder); - LoadSmartProjectionFactorAdderParams(config, params.smart_projection_adder); - LoadStandstillFactorAdderParams(config, params.standstill_adder); -} - -void LoadHandrailFactorAdderParams(config_reader::ConfigReader& config, HandrailFactorAdderParams& params) { - params.enabled = mc::LoadBool(config, "handrail_adder_enabled"); - params.huber_k = mc::LoadDouble(config, "huber_k"); - params.min_num_line_matches = mc::LoadDouble(config, "handrail_adder_min_num_line_matches"); - params.min_num_plane_matches = mc::LoadDouble(config, "handrail_adder_min_num_plane_matches"); - params.point_to_line_stddev = mc::LoadDouble(config, "handrail_adder_point_to_line_stddev"); - params.point_to_plane_stddev = mc::LoadDouble(config, "handrail_adder_point_to_plane_stddev"); - params.body_T_perch_cam = lc::LoadTransform(config, "perch_cam_transform"); - params.use_silu_for_point_to_line_segment_factor = - mc::LoadBool(config, "handrail_adder_use_silu_for_point_to_line_segment_factor"); -} - -void LoadARTagLocFactorAdderParams(config_reader::ConfigReader& config, LocFactorAdderParams& params) { - params.add_pose_priors = mc::LoadBool(config, "ar_tag_loc_adder_add_pose_priors"); - params.add_projections = mc::LoadBool(config, "ar_tag_loc_adder_add_projections"); - params.enabled = params.add_pose_priors || params.add_projections ? true : false; - params.huber_k = mc::LoadDouble(config, "huber_k"); - params.min_num_matches = mc::LoadInt(config, "ar_tag_loc_adder_min_num_matches"); - params.max_num_factors = mc::LoadInt(config, "ar_tag_loc_adder_max_num_factors"); - params.prior_translation_stddev = mc::LoadDouble(config, "ar_tag_loc_adder_prior_translation_stddev"); - params.prior_quaternion_stddev = mc::LoadDouble(config, "ar_tag_loc_adder_prior_quaternion_stddev"); - params.scale_pose_noise_with_num_landmarks = - mc::LoadBool(config, "ar_tag_loc_adder_scale_pose_noise_with_num_landmarks"); - params.scale_projection_noise_with_num_landmarks = - mc::LoadBool(config, "ar_tag_loc_adder_scale_projection_noise_with_num_landmarks"); - params.pose_noise_scale = mc::LoadDouble(config, "ar_tag_loc_adder_pose_noise_scale"); - params.projection_noise_scale = mc::LoadDouble(config, "ar_tag_loc_adder_projection_noise_scale"); - params.max_inlier_weighted_projection_norm = - mc::LoadDouble(config, "ar_tag_loc_adder_max_inlier_weighted_projection_norm"); - params.weight_projections_with_distance = mc::LoadBool(config, "ar_tag_loc_adder_weight_projections_with_distance"); - params.add_prior_if_projections_fail = mc::LoadBool(config, "ar_tag_loc_adder_add_prior_if_projections_fail"); - params.body_T_cam = lc::LoadTransform(config, "dock_cam_transform"); - params.cam_intrinsics.reset(new gtsam::Cal3_S2(lc::LoadCameraIntrinsics(config, "dock_cam"))); - params.cam_noise = gtsam::noiseModel::Isotropic::Sigma(2, mc::LoadDouble(config, "loc_dock_cam_noise_stddev")); -} - -void LoadDepthOdometryFactorAdderParams(config_reader::ConfigReader& config, DepthOdometryFactorAdderParams& params) { - params.enabled = mc::LoadBool(config, "depth_odometry_adder_enabled"); - params.huber_k = mc::LoadDouble(config, "huber_k"); - params.noise_scale = mc::LoadDouble(config, "depth_odometry_adder_noise_scale"); - params.use_points_between_factor = mc::LoadBool(config, "depth_odometry_adder_use_points_between_factor"); - params.position_covariance_threshold = mc::LoadDouble(config, "depth_odometry_adder_position_covariance_threshold"); - params.orientation_covariance_threshold = - mc::LoadDouble(config, "depth_odometry_adder_orientation_covariance_threshold"); - params.body_T_sensor = lc::LoadTransform(config, "haz_cam_transform"); - params.point_to_point_error_threshold = mc::LoadDouble(config, "depth_odometry_adder_point_to_point_error_threshold"); - params.pose_translation_norm_threshold = - mc::LoadDouble(config, "depth_odometry_adder_pose_translation_norm_threshold"); - params.max_num_points_between_factors = mc::LoadDouble(config, "depth_odometry_adder_max_num_points_between_factors"); -} - -void LoadLocFactorAdderParams(config_reader::ConfigReader& config, LocFactorAdderParams& params) { - params.add_pose_priors = mc::LoadBool(config, "loc_adder_add_pose_priors"); - params.add_projections = mc::LoadBool(config, "loc_adder_add_projections"); - params.enabled = params.add_pose_priors || params.add_projections ? true : false; - params.huber_k = mc::LoadDouble(config, "huber_k"); - params.min_num_matches = mc::LoadInt(config, "loc_adder_min_num_matches"); - params.max_num_factors = mc::LoadInt(config, "loc_adder_max_num_factors"); - params.prior_translation_stddev = mc::LoadDouble(config, "loc_adder_prior_translation_stddev"); - params.prior_quaternion_stddev = mc::LoadDouble(config, "loc_adder_prior_quaternion_stddev"); - params.scale_pose_noise_with_num_landmarks = mc::LoadBool(config, "loc_adder_scale_pose_noise_with_num_landmarks"); - params.scale_projection_noise_with_num_landmarks = - mc::LoadBool(config, "loc_adder_scale_projection_noise_with_num_landmarks"); - params.pose_noise_scale = mc::LoadDouble(config, "loc_adder_pose_noise_scale"); - params.projection_noise_scale = mc::LoadDouble(config, "loc_adder_projection_noise_scale"); - params.max_inlier_weighted_projection_norm = mc::LoadDouble(config, "loc_adder_max_inlier_weighted_projection_norm"); - params.weight_projections_with_distance = mc::LoadBool(config, "loc_adder_weight_projections_with_distance"); - params.add_prior_if_projections_fail = mc::LoadBool(config, "loc_adder_add_prior_if_projections_fail"); - params.body_T_cam = lc::LoadTransform(config, "nav_cam_transform"); - params.cam_intrinsics.reset(new gtsam::Cal3_S2(lc::LoadCameraIntrinsics(config, "nav_cam"))); - params.cam_noise = gtsam::noiseModel::Isotropic::Sigma(2, mc::LoadDouble(config, "loc_nav_cam_noise_stddev")); -} - -void LoadRotationFactorAdderParams(config_reader::ConfigReader& config, RotationFactorAdderParams& params) { - params.enabled = mc::LoadBool(config, "rotation_adder_enabled"); - params.huber_k = mc::LoadDouble(config, "huber_k"); - params.min_avg_disparity = mc::LoadDouble(config, "rotation_adder_min_avg_disparity"); - params.rotation_stddev = mc::LoadDouble(config, "rotation_adder_rotation_stddev"); - params.max_percent_outliers = mc::LoadDouble(config, "rotation_adder_max_percent_outliers"); - params.body_T_nav_cam = lc::LoadTransform(config, "nav_cam_transform"); - params.nav_cam_intrinsics = lc::LoadCameraIntrinsics(config, "nav_cam"); -} - -void LoadProjectionFactorAdderParams(config_reader::ConfigReader& config, ProjectionFactorAdderParams& params) { - params.enabled = mc::LoadBool(config, "projection_adder_enabled"); - params.huber_k = mc::LoadDouble(config, "huber_k"); - params.enable_EPI = mc::LoadBool(config, "projection_adder_enable_EPI"); - params.landmark_distance_threshold = mc::LoadDouble(config, "projection_adder_landmark_distance_threshold"); - params.dynamic_outlier_rejection_threshold = - mc::LoadDouble(config, "projection_adder_dynamic_outlier_rejection_threshold"); - params.max_num_features = mc::LoadInt(config, "projection_adder_max_num_features"); - params.min_num_measurements_for_triangulation = - mc::LoadInt(config, "projection_adder_min_num_measurements_for_triangulation"); - params.add_point_priors = mc::LoadBool(config, "projection_adder_add_point_priors"); - params.point_prior_translation_stddev = mc::LoadDouble(config, "projection_adder_point_prior_translation_stddev"); - params.body_T_cam = lc::LoadTransform(config, "nav_cam_transform"); - params.cam_intrinsics.reset(new gtsam::Cal3_S2(lc::LoadCameraIntrinsics(config, "nav_cam"))); - params.cam_noise = - gtsam::noiseModel::Isotropic::Sigma(2, mc::LoadDouble(config, "optical_flow_nav_cam_noise_stddev")); -} - -void LoadSmartProjectionFactorAdderParams(config_reader::ConfigReader& config, - SmartProjectionFactorAdderParams& params) { - params.enabled = mc::LoadBool(config, "smart_projection_adder_enabled"); - params.huber_k = mc::LoadDouble(config, "huber_k"); - params.min_avg_distance_from_mean = mc::LoadDouble(config, "smart_projection_adder_min_avg_distance_from_mean"); - params.enable_EPI = mc::LoadBool(config, "smart_projection_adder_enable_EPI"); - params.landmark_distance_threshold = mc::LoadDouble(config, "smart_projection_adder_landmark_distance_threshold"); - params.dynamic_outlier_rejection_threshold = - mc::LoadDouble(config, "smart_projection_adder_dynamic_outlier_rejection_threshold"); - params.retriangulation_threshold = mc::LoadDouble(config, "smart_projection_adder_retriangulation_threshold"); - params.verbose_cheirality = mc::LoadBool(config, "smart_projection_adder_verbose_cheirality"); - params.robust = mc::LoadBool(config, "smart_projection_adder_robust"); - params.max_num_factors = mc::LoadInt(config, "smart_projection_adder_max_num_factors"); - params.min_num_points = mc::LoadInt(config, "smart_projection_adder_min_num_points"); - params.max_num_points_per_factor = mc::LoadInt(config, "smart_projection_adder_max_num_points_per_factor"); - params.measurement_spacing = mc::LoadInt(config, "smart_projection_adder_measurement_spacing"); - params.feature_track_min_separation = mc::LoadDouble(config, "smart_projection_adder_feature_track_min_separation"); - params.rotation_only_fallback = mc::LoadBool(config, "smart_projection_adder_rotation_only_fallback"); - params.splitting = mc::LoadBool(config, "smart_projection_adder_splitting"); - params.scale_noise_with_num_points = mc::LoadBool(config, "smart_projection_adder_scale_noise_with_num_points"); - params.noise_scale = mc::LoadDouble(config, "smart_projection_adder_noise_scale"); - params.use_allowed_timestamps = mc::LoadBool(config, "smart_projection_adder_use_allowed_timestamps"); - params.body_T_cam = lc::LoadTransform(config, "nav_cam_transform"); - params.cam_intrinsics.reset(new gtsam::Cal3_S2(lc::LoadCameraIntrinsics(config, "nav_cam"))); - params.cam_noise = - gtsam::noiseModel::Isotropic::Sigma(2, mc::LoadDouble(config, "optical_flow_nav_cam_noise_stddev")); -} - -void LoadStandstillFactorAdderParams(config_reader::ConfigReader& config, StandstillFactorAdderParams& params) { - params.add_velocity_prior = mc::LoadBool(config, "standstill_adder_add_velocity_prior"); - params.add_pose_between_factor = mc::LoadBool(config, "standstill_adder_add_pose_between_factor"); - params.enabled = params.add_velocity_prior || params.add_pose_between_factor; - params.prior_velocity_stddev = mc::LoadDouble(config, "standstill_adder_prior_velocity_stddev"); - params.pose_between_factor_translation_stddev = - mc::LoadDouble(config, "standstill_adder_pose_between_factor_translation_stddev"); - params.pose_between_factor_rotation_stddev = - mc::LoadDouble(config, "standstill_adder_pose_between_factor_rotation_stddev"); - params.huber_k = mc::LoadDouble(config, "huber_k"); -} - -void LoadFeatureTrackerParams(config_reader::ConfigReader& config, FeatureTrackerParams& params) { - params.sliding_window_duration = mc::LoadDouble(config, "feature_tracker_sliding_window_duration"); - params.smart_projection_adder_measurement_spacing = mc::LoadInt(config, "smart_projection_adder_measurement_spacing"); -} - -void LoadSanityCheckerParams(config_reader::ConfigReader& config, SanityCheckerParams& params) { - params.num_consecutive_pose_difference_failures_until_insane = - mc::LoadInt(config, "num_consecutive_pose_difference_failures_until_insane"); - params.max_sane_position_difference = mc::LoadDouble(config, "max_sane_position_difference"); - params.check_pose_difference = mc::LoadBool(config, "check_pose_difference"); - params.check_position_covariance = mc::LoadBool(config, "check_position_covariance"); - params.check_orientation_covariance = mc::LoadBool(config, "check_orientation_covariance"); - params.position_covariance_threshold = mc::LoadDouble(config, "position_covariance_threshold"); - params.orientation_covariance_threshold = mc::LoadDouble(config, "orientation_covariance_threshold"); -} - -void LoadGraphInitializerParams(config_reader::ConfigReader& config, GraphInitializerParams& params) { - ii::LoadImuIntegratorParams(config, params); - params.imu_bias_filename = mc::LoadString(config, "imu_bias_file"); - params.num_bias_estimation_measurements = mc::LoadInt(config, "num_bias_estimation_measurements"); -} - -void LoadCombinedNavStateNodeUpdaterParams(config_reader::ConfigReader& config, - CombinedNavStateNodeUpdaterParams& params) { - params.starting_prior_translation_stddev = mc::LoadDouble(config, "starting_prior_translation_stddev"); - params.starting_prior_quaternion_stddev = mc::LoadDouble(config, "starting_prior_quaternion_stddev"); - params.starting_prior_velocity_stddev = mc::LoadDouble(config, "starting_prior_velocity_stddev"); - params.starting_prior_accel_bias_stddev = mc::LoadDouble(config, "starting_prior_accel_bias_stddev"); - params.starting_prior_gyro_bias_stddev = mc::LoadDouble(config, "starting_prior_gyro_bias_stddev"); - params.huber_k = mc::LoadDouble(config, "huber_k"); - params.add_priors = mc::LoadBool(config, "add_priors"); - params.threshold_bias_uncertainty = mc::LoadBool(config, "threshold_bias_uncertainty"); - params.accel_bias_stddev_threshold = mc::LoadDouble(config, "accel_bias_stddev_threshold"); - params.gyro_bias_stddev_threshold = mc::LoadDouble(config, "gyro_bias_stddev_threshold"); - LoadCombinedNavStateGraphValuesParams(config, params.graph_values); -} - -void LoadCombinedNavStateGraphValuesParams(config_reader::ConfigReader& config, - CombinedNavStateGraphValuesParams& params) { - params.ideal_duration = mc::LoadDouble(config, "ideal_duration"); - params.min_num_states = mc::LoadInt(config, "min_num_states"); - params.max_num_states = mc::LoadInt(config, "max_num_states"); -} - -void LoadFeaturePointNodeUpdaterParams(config_reader::ConfigReader& config, FeaturePointNodeUpdaterParams& params) { - params.huber_k = mc::LoadDouble(config, "huber_k"); -} - -void LoadHandrailParams(config_reader::ConfigReader& config, HandrailParams& params) { - params.length = mc::LoadDouble(config, "handrail_length"); - params.distance_to_wall = mc::LoadDouble(config, "handrail_wall_min_gap"); -} - -void LoadGraphLocalizerParams(config_reader::ConfigReader& config, GraphLocalizerParams& params) { - LoadCalibrationParams(config, params.calibration); - LoadCombinedNavStateNodeUpdaterParams(config, params.combined_nav_state_node_updater); - LoadGraphInitializerParams(config, params.graph_initializer); - LoadFactorParams(config, params.factor); - LoadFeaturePointNodeUpdaterParams(config, params.feature_point_node_updater); - LoadFeatureTrackerParams(config, params.feature_tracker); - LoadHandrailParams(config, params.handrail); - go::LoadGraphOptimizerParams(config, params.graph_optimizer); - params.huber_k = mc::LoadDouble(config, "huber_k"); - params.max_standstill_feature_track_avg_distance_from_mean = - mc::LoadDouble(config, "max_standstill_feature_track_avg_distance_from_mean"); - params.standstill_min_num_points_per_track = mc::LoadInt(config, "standstill_min_num_points_per_track"); - params.standstill_feature_track_duration = mc::LoadDouble(config, "standstill_feature_track_duration"); - params.estimate_world_T_dock_using_loc = mc::LoadBool(config, "estimate_world_T_dock_using_loc"); -} - -void LoadGraphLocalizerNodeletParams(config_reader::ConfigReader& config, GraphLocalizerNodeletParams& params) { - params.loc_adder_min_num_matches = mc::LoadInt(config, "loc_adder_min_num_matches"); - params.ar_tag_loc_adder_min_num_matches = mc::LoadInt(config, "ar_tag_loc_adder_min_num_matches"); - params.max_imu_buffer_size = mc::LoadInt(config, "max_imu_buffer_size"); - params.max_optical_flow_buffer_size = mc::LoadInt(config, "max_optical_flow_buffer_size"); - params.max_vl_buffer_size = mc::LoadInt(config, "max_vl_buffer_size"); - params.max_ar_buffer_size = mc::LoadInt(config, "max_ar_buffer_size"); - params.max_depth_odometry_buffer_size = mc::LoadInt(config, "max_depth_odometry_buffer_size"); - params.max_dl_buffer_size = mc::LoadInt(config, "max_dl_buffer_size"); -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/projection_factor_adder.cc b/localization/graph_localizer/src/projection_factor_adder.cc deleted file mode 100644 index 7b47f25eeb..0000000000 --- a/localization/graph_localizer/src/projection_factor_adder.cc +++ /dev/null @@ -1,92 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/projection_factor_adder.h> -#include <graph_localizer/utilities.h> -#include <localization_common/logger.h> - -#include <gtsam/base/Vector.h> -#include <gtsam/slam/ProjectionFactor.h> - -namespace graph_localizer { -namespace go = graph_optimizer; -namespace lm = localization_measurements; -namespace sym = gtsam::symbol_shorthand; -ProjectionFactorAdder::ProjectionFactorAdder(const ProjectionFactorAdderParams& params, - std::shared_ptr<const FeatureTracker> feature_tracker, - std::shared_ptr<const FeaturePointGraphValues> feature_point_graph_values) - : ProjectionFactorAdder::Base(params), - feature_tracker_(feature_tracker), - feature_point_graph_values_(std::move(feature_point_graph_values)) {} - -std::vector<go::FactorsToAdd> ProjectionFactorAdder::AddFactors( - const lm::FeaturePointsMeasurement& feature_points_measurement) { - std::vector<go::FactorsToAdd> factors_to_add_vec; - // Add projection factors for new measurements of already existing features - go::FactorsToAdd projection_factors_to_add; - for (const auto& feature_point : feature_points_measurement.feature_points) { - if (feature_point_graph_values_->HasFeature(feature_point.feature_id)) { - const go::KeyInfo pose_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, feature_point.timestamp); - const go::KeyInfo static_point_key_info(&sym::F, go::NodeUpdaterType::FeaturePoint, feature_point.feature_id); - const auto point_key = feature_point_graph_values_->FeatureKey(feature_point.feature_id); - if (!point_key) { - LogError("AddFactors: Failed to get point key."); - continue; - } - const auto projection_factor = boost::make_shared<ProjectionFactor>( - feature_point.image_point, Robust(params().cam_noise, params().huber_k), pose_key_info.UninitializedKey(), - *point_key, params().cam_intrinsics, params().body_T_cam); - projection_factors_to_add.push_back({{pose_key_info, static_point_key_info}, projection_factor}); - } - } - if (!projection_factors_to_add.empty()) { - projection_factors_to_add.SetTimestamp(feature_points_measurement.timestamp); - factors_to_add_vec.emplace_back(projection_factors_to_add); - LogDebug("AddFactors: Added " << projection_factors_to_add.size() << " projection factors for existing features."); - } - - // Add new feature tracks and measurements if possible - int new_features = 0; - for (const auto& feature_track_pair : feature_tracker_->feature_tracks()) { - const auto& feature_track = *(feature_track_pair.second); - if (static_cast<int>(feature_track.size()) >= params().min_num_measurements_for_triangulation && - !feature_point_graph_values_->HasFeature(feature_track.id()) && - (new_features + feature_point_graph_values_->NumFeatures()) < params().max_num_features) { - // Create new factors to add for each feature track so the graph action can act on only that - // feature track to triangulate a new point - go::FactorsToAdd projection_factors_with_new_point_to_add(go::GraphActionCompleterType::ProjectionFactor); - const auto point_key = feature_point_graph_values_->CreateFeatureKey(); - for (const auto& feature_point_pair : feature_track.points()) { - const auto& feature_point = feature_point_pair.second; - const go::KeyInfo pose_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, feature_point.timestamp); - const go::KeyInfo static_point_key_info(&sym::F, go::NodeUpdaterType::FeaturePoint, feature_point.feature_id); - const auto projection_factor = boost::make_shared<ProjectionFactor>( - feature_point.image_point, Robust(params().cam_noise, params().huber_k), pose_key_info.UninitializedKey(), - point_key, params().cam_intrinsics, params().body_T_cam); - projection_factors_with_new_point_to_add.push_back({{pose_key_info, static_point_key_info}, projection_factor}); - } - projection_factors_with_new_point_to_add.SetTimestamp(feature_points_measurement.timestamp); - factors_to_add_vec.emplace_back(projection_factors_with_new_point_to_add); - ++new_features; - } - } - if (new_features > 0) LogDebug("AddFactors: Added " << new_features << " new features."); - if (factors_to_add_vec.empty()) return {}; - return factors_to_add_vec; -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/projection_graph_action_completer.cc b/localization/graph_localizer/src/projection_graph_action_completer.cc deleted file mode 100644 index 20e5885fad..0000000000 --- a/localization/graph_localizer/src/projection_graph_action_completer.cc +++ /dev/null @@ -1,102 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/projection_graph_action_completer.h> -#include <graph_localizer/utilities.h> -#include <localization_common/logger.h> - -#include <gtsam/base/Vector.h> -#include <gtsam/slam/ProjectionFactor.h> - -namespace graph_localizer { -namespace go = graph_optimizer; -namespace sym = gtsam::symbol_shorthand; -ProjectionGraphActionCompleter::ProjectionGraphActionCompleter( - const ProjectionFactorAdderParams& params, std::shared_ptr<const CombinedNavStateGraphValues> graph_values, - std::shared_ptr<FeaturePointGraphValues> feature_point_graph_values) - : params_(params), graph_values_(graph_values), feature_point_graph_values_(std::move(feature_point_graph_values)) { - projection_triangulation_params_.rankTolerance = 1e-9; - // TODO(rsoussan): why is Base necessary for these? - projection_triangulation_params_.enableEPI = params_.enable_EPI; - projection_triangulation_params_.landmarkDistanceThreshold = params_.landmark_distance_threshold; - projection_triangulation_params_.dynamicOutlierRejectionThreshold = params_.dynamic_outlier_rejection_threshold; -} - -go::GraphActionCompleterType ProjectionGraphActionCompleter::type() const { - return go::GraphActionCompleterType::ProjectionFactor; -} - -bool ProjectionGraphActionCompleter::DoAction(go::FactorsToAdd& factors_to_add, - gtsam::NonlinearFactorGraph& graph_factors) { - return TriangulateNewPoint(factors_to_add, graph_factors); -} - -bool ProjectionGraphActionCompleter::TriangulateNewPoint(go::FactorsToAdd& factors_to_add, - gtsam::NonlinearFactorGraph& graph_factors) { - gtsam::CameraSet<Camera> camera_set; - Camera::MeasurementVector measurements; - gtsam::Key point_key; - for (const auto& factor_to_add : factors_to_add.Get()) { - const auto& factor = factor_to_add.factor; - const auto projection_factor = dynamic_cast<ProjectionFactor*>(factor.get()); - if (!projection_factor) { - LogError("TriangulateNewPoint: Failed to cast to projection factor."); - return false; - } - const auto world_T_body = graph_values_->at<gtsam::Pose3>(projection_factor->key1()); - if (!world_T_body) { - LogError("TriangulateNewPoint: Failed to get pose."); - return false; - } - - const gtsam::Pose3 world_T_camera = *world_T_body * params_.body_T_cam; - camera_set.emplace_back(world_T_camera, params_.cam_intrinsics); - measurements.emplace_back(projection_factor->measured()); - point_key = projection_factor->key2(); - } - gtsam::TriangulationResult world_t_triangulated_point; - // TODO(rsoussan): Gtsam shouldn't be throwing exceptions for this, but needed if enable_epi enabled. - // Is there a build setting that prevents cheirality from being thrown in this case? - try { - world_t_triangulated_point = gtsam::triangulateSafe(camera_set, measurements, projection_triangulation_params_); - } catch (...) { - LogDebug("TriangulateNewPoint: Exception occurred during triangulation"); - return false; - } - - if (!world_t_triangulated_point.valid()) { - LogDebug("TriangulateNewPoint: Failed to triangulate point"); - return false; - } - // TODO(rsoussan): clean this up - const auto feature_id = factors_to_add.Get().front().key_infos[1].id(); - feature_point_graph_values_->AddFeature(feature_id, *world_t_triangulated_point, point_key); - if (params_.add_point_priors) { - const gtsam::Vector3 point_prior_noise_sigmas((gtsam::Vector(3) << params_.point_prior_translation_stddev, - params_.point_prior_translation_stddev, - params_.point_prior_translation_stddev) - .finished()); - const auto point_noise = - Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(point_prior_noise_sigmas)), - params_.huber_k); - gtsam::PriorFactor<gtsam::Point3> point_prior_factor(point_key, *world_t_triangulated_point, point_noise); - graph_factors.push_back(point_prior_factor); - } - return true; -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/rotation_factor_adder.cc b/localization/graph_localizer/src/rotation_factor_adder.cc deleted file mode 100644 index 381a393ed7..0000000000 --- a/localization/graph_localizer/src/rotation_factor_adder.cc +++ /dev/null @@ -1,105 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/pose_rotation_factor.h> -#include <graph_localizer/rotation_factor_adder.h> -#include <graph_localizer/utilities.h> -#include <localization_common/logger.h> - -#include <gtsam/inference/Symbol.h> - -#include <opencv2/calib3d.hpp> -#include <opencv2/core/eigen.hpp> -#include <opencv2/core/types.hpp> - -namespace graph_localizer { -namespace go = graph_optimizer; -namespace lm = localization_measurements; -namespace sym = gtsam::symbol_shorthand; -RotationFactorAdder::RotationFactorAdder(const RotationFactorAdderParams& params, - std::shared_ptr<const FeatureTracker> feature_tracker) - : RotationFactorAdder::Base(params), feature_tracker_(feature_tracker) {} - -std::vector<go::FactorsToAdd> RotationFactorAdder::AddFactors(const lm::FeaturePointsMeasurement& measurement) { - std::vector<cv::Point2d> points_1; - std::vector<cv::Point2d> points_2; - double total_disparity = 0; - for (const auto& feature_track_pair : feature_tracker_->feature_tracks()) { - const auto& feature_track = *(feature_track_pair.second); - if (feature_track.size() < 2) continue; - // Get points for most recent and second to most recent images - const auto& point_1 = std::next(feature_track.points().crbegin())->second.image_point; - const auto& point_2 = feature_track.points().crbegin()->second.image_point; - points_1.emplace_back(cv::Point2d(point_1.x(), point_1.y())); - points_2.emplace_back(cv::Point2d(point_2.x(), point_2.y())); - total_disparity += (point_1 - point_2).norm(); - } - - if (points_1.size() < 5) { - LogDebug("AddFactors: Not enough corresponding points found."); - return {}; - } - const double average_disparity = total_disparity / points_1.size(); - if (average_disparity < params().min_avg_disparity) { - LogDebug("AddFactors: Disparity too low."); - return {}; - } - - cv::Mat intrinsics; - cv::eigen2cv(params().nav_cam_intrinsics.K(), intrinsics); - cv::Mat outlier_mask; - const auto essential_matrix = - cv::findEssentialMat(points_1, points_2, intrinsics, cv::RANSAC, 0.999, 1e-3, outlier_mask); - int num_inliers_essential_matrix = 0; - for (int i = 0; i < outlier_mask.rows; ++i) { - if (outlier_mask.at<uint8_t>(i, 0) == 1) ++num_inliers_essential_matrix; - } - cv::Mat cv_cam_2_R_cam_1; - cv::Mat cv_translation; - const int num_inliers_pose_calculation = - cv::recoverPose(essential_matrix, points_1, points_2, intrinsics, cv_cam_2_R_cam_1, cv_translation, outlier_mask); - // Only consider outliers from recoverPose calculation, ignore already pruned outliers from findEssentialMat - const double percent_outliers = - static_cast<double>(num_inliers_essential_matrix - num_inliers_pose_calculation) / num_inliers_essential_matrix; - if (percent_outliers > params().max_percent_outliers) { - LogDebug("AddFactors: Too many outliers, discarding result."); - return {}; - } - Eigen::Matrix3d eigen_cam_2_R_cam_1; - cv::cv2eigen(cv_cam_2_R_cam_1, eigen_cam_2_R_cam_1); - const gtsam::Rot3& body_R_cam = params().body_T_nav_cam.rotation(); - // Put measurement in body frame since factor expects this - const gtsam::Rot3 cam_1_R_cam_2(eigen_cam_2_R_cam_1.transpose()); - const gtsam::Rot3 body_1_R_body_2 = body_R_cam * cam_1_R_cam_2 * body_R_cam.inverse(); - // Create Rotation Factor - const go::KeyInfo pose_1_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, - *(feature_tracker_->PreviousTimestamp())); - const go::KeyInfo pose_2_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, measurement.timestamp); - const gtsam::Vector3 rotation_noise_sigmas( - (gtsam::Vector(3) << params().rotation_stddev, params().rotation_stddev, params().rotation_stddev).finished()); - const auto rotation_noise = Robust( - gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(rotation_noise_sigmas)), params().huber_k); - const auto rotation_factor = boost::make_shared<gtsam::PoseRotationFactor>( - body_1_R_body_2, rotation_noise, pose_1_key_info.UninitializedKey(), pose_2_key_info.UninitializedKey()); - go::FactorsToAdd rotation_factors_to_add; - rotation_factors_to_add.push_back({{pose_1_key_info, pose_2_key_info}, rotation_factor}); - rotation_factors_to_add.SetTimestamp(pose_2_key_info.timestamp()); - LogDebug("AddFactors: Added a rotation factor."); - return {rotation_factors_to_add}; -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/sanity_checker.cc b/localization/graph_localizer/src/sanity_checker.cc deleted file mode 100644 index b06b6ea9ad..0000000000 --- a/localization/graph_localizer/src/sanity_checker.cc +++ /dev/null @@ -1,47 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/sanity_checker.h> -#include <localization_common/combined_nav_state_covariances.h> -#include <localization_common/utilities.h> - -namespace graph_localizer { -namespace lc = localization_common; -SanityChecker::SanityChecker(const SanityCheckerParams& params) : params_(params), num_consecutive_failures_(0) {} - -bool SanityChecker::CheckPoseSanity(const gtsam::Pose3& sparse_mapping_pose, const gtsam::Pose3& localizer_pose) { - if (!params_.check_pose_difference) return true; - - const gtsam::Vector3 difference_vector = sparse_mapping_pose.translation() - localizer_pose.translation(); - const double euclidean_distance = difference_vector.norm(); - if (euclidean_distance > params_.max_sane_position_difference) - ++num_consecutive_failures_; - else - num_consecutive_failures_ = 0; - return (num_consecutive_failures_ < params_.num_consecutive_pose_difference_failures_until_insane); -} - -bool SanityChecker::CheckCovarianceSanity(const lc::CombinedNavStateCovariances& covariances) const { - return lc::PoseCovarianceSane(covariances.pose_covariance(), params_.position_covariance_threshold, - params_.orientation_covariance_threshold, params_.check_position_covariance, - params_.check_orientation_covariance); -} - -void SanityChecker::Reset() { num_consecutive_failures_ = 0; } - -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/serialization.cc b/localization/graph_localizer/src/serialization.cc deleted file mode 100644 index 61f51a77c6..0000000000 --- a/localization/graph_localizer/src/serialization.cc +++ /dev/null @@ -1,101 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/serialization.h> - -#include <gtsam/base/serialization.h> - -#include <boost/serialization/serialization.hpp> - -// Value types used in GraphValues -#include <gtsam/base/Vector.h> -#include <gtsam/geometry/Cal3_S2.h> -#include <gtsam/geometry/Point3.h> -#include <gtsam/geometry/Pose3.h> -#include <gtsam/inference/Symbol.h> -#include <gtsam/navigation/ImuBias.h> -#include <gtsam/navigation/NavState.h> - -// Factors used in NonlinearFactorGraph -#include <graph_localizer/loc_pose_factor.h> -#include <graph_localizer/loc_projection_factor.h> -#include <graph_localizer/point_to_handrail_endpoint_factor.h> -#include <graph_localizer/point_to_line_factor.h> -#include <graph_localizer/point_to_line_segment_factor.h> -#include <graph_localizer/point_to_plane_factor.h> -#include <graph_localizer/pose_rotation_factor.h> -#include <graph_localizer/robust_smart_projection_pose_factor.h> -#include <gtsam/nonlinear/LinearContainerFactor.h> -#include <gtsam/nonlinear/NonlinearFactor.h> -#include <gtsam/slam/BetweenFactor.h> -#include <gtsam/slam/PriorFactor.h> -#include <gtsam/slam/ProjectionFactor.h> -#include <gtsam/slam/SmartProjectionPoseFactor.h> - -// Export all classes derived from Value so they can -// be properly serialized using base class (value) pointers in GraphValues -GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); -GTSAM_VALUE_EXPORT(gtsam::imuBias::ConstantBias); -GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<gtsam::Cal3_S2>); -GTSAM_VALUE_EXPORT(gtsam::Pose3); -GTSAM_VALUE_EXPORT(gtsam::Vector3); - -// Export all factors and noise models used in factor graph so -// these can be serialized using their base class pointers in -// the gtsam::NonlinearFactorGraph -using LocProjectionFactor = gtsam::LocProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2>; -BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor<gtsam::Pose3>, "gtsam::PriorFactorPose3"); -BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor<gtsam::Vector3>, "gtsam::PriorFactorVector3"); -BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor<gtsam::imuBias::ConstantBias>, "gtsam::PriorFactorConstantBias"); -BOOST_CLASS_EXPORT_GUID(LocProjectionFactor, "gtsam::LocProjectionFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::LocPoseFactor, "gtsam::LocPoseFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::PointToHandrailEndpointFactor, "gtsam::PointToHandrailEndpointFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::PointToLineFactor, "gtsam::PointToLineFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::PointToLineSegmentFactor, "gtsam::PointToLineSegmentFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::PointToPlaneFactor, "gtsam::PointToPlaneFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::PoseRotationFactor, "gtsam::PoseRotationFactor"); -using ProjectionFactor = gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3>; -BOOST_CLASS_EXPORT_GUID(ProjectionFactor, "gtsam::GenericProjectionFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::RobustSmartProjectionPoseFactor<gtsam::Cal3_S2>, - "gtsam::RobustSmartProjectionPoseFactorCal3_S2"); -BOOST_CLASS_EXPORT_GUID(gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2>, "gtsam::SmartProjectionPoseFactorCal3_S2"); -BOOST_CLASS_EXPORT_GUID(gtsam::CombinedImuFactor, "gtsam::CombinedImuFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::LinearContainerFactor, "gtsam::LinearContainerFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::BetweenFactor<gtsam::Pose3>, "gtsam::BetweenFactorPose3"); - -// Add all possible noise models used by factors -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust"); - -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base, "gtsam_noiseModel_mEstimator_Base"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null, "gtsam_noiseModel_mEstimator_Null"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair, "gtsam_noiseModel_mEstimator_Fair"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey"); - -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); - -namespace graph_localizer { - -std::string SerializeBinary(const GraphLocalizer& graph_localizer) { return gtsam::serializeBinary(graph_localizer); } -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/smart_projection_cumulative_factor_adder.cc b/localization/graph_localizer/src/smart_projection_cumulative_factor_adder.cc deleted file mode 100644 index 8ab9fae35e..0000000000 --- a/localization/graph_localizer/src/smart_projection_cumulative_factor_adder.cc +++ /dev/null @@ -1,145 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/smart_projection_cumulative_factor_adder.h> -#include <graph_localizer/utilities.h> -#include <graph_optimizer/utilities.h> -#include <localization_common/logger.h> - -#include <gtsam/base/Vector.h> -#include <gtsam/slam/SmartProjectionPoseFactor.h> - -namespace graph_localizer { -namespace go = graph_optimizer; -namespace lm = localization_measurements; -namespace sym = gtsam::symbol_shorthand; -SmartProjectionCumulativeFactorAdder::SmartProjectionCumulativeFactorAdder( - const SmartProjectionFactorAdderParams& params, std::shared_ptr<const FeatureTracker> feature_tracker) - : SmartProjectionCumulativeFactorAdder::Base(params), feature_tracker_(feature_tracker) { - smart_projection_params_.verboseCheirality = params.verbose_cheirality; - smart_projection_params_.setRankTolerance(1e-9); - smart_projection_params_.setLandmarkDistanceThreshold(params.landmark_distance_threshold); - smart_projection_params_.setDynamicOutlierRejectionThreshold(params.dynamic_outlier_rejection_threshold); - smart_projection_params_.setRetriangulationThreshold(params.retriangulation_threshold); - if (params.rotation_only_fallback) smart_projection_params_.setDegeneracyMode(gtsam::DegeneracyMode::HANDLE_INFINITY); - smart_projection_params_.setEnableEPI(params.enable_EPI); -} - -void SmartProjectionCumulativeFactorAdder::AddFactors( - const FeatureTrackLengthMap& feature_tracks, const int spacing, const double feature_track_min_separation, - go::FactorsToAdd& smart_factors_to_add, std::unordered_map<lm::FeatureId, lm::FeaturePoint>& added_points) { - // Iterate in reverse order so longer feature tracks are prioritized - for (auto feature_track_it = feature_tracks.crbegin(); feature_track_it != feature_tracks.crend(); - ++feature_track_it) { - if (static_cast<int>(smart_factors_to_add.size()) >= params().max_num_factors) break; - const auto& feature_track = *(feature_track_it->second); - const auto points = feature_track.LatestPoints(spacing); - // Skip already added tracks - if (added_points.count(points.front().feature_id) > 0) continue; - const double average_distance_from_mean = AverageDistanceFromMean(points); - if (ValidPointSet(points.size(), average_distance_from_mean, params().min_avg_distance_from_mean, - params().min_num_points) && - !TooClose(added_points, points.front(), feature_track_min_separation)) { - AddSmartFactor(points, smart_factors_to_add); - // Use latest point - added_points.emplace(points.front().feature_id, points.front()); - } - } -} - -std::vector<go::FactorsToAdd> SmartProjectionCumulativeFactorAdder::AddFactors() { - // Add smart factor for each valid feature track - go::FactorsToAdd smart_factors_to_add(go::GraphActionCompleterType::SmartFactor); - if (params().use_allowed_timestamps) { - for (const auto& feature_track : feature_tracker_->feature_tracks()) { - const auto points = feature_track.second->AllowedPoints(feature_tracker_->smart_factor_timestamp_allow_list()); - const double average_distance_from_mean = AverageDistanceFromMean(points); - if (ValidPointSet(points.size(), average_distance_from_mean, params().min_avg_distance_from_mean, - params().min_num_points) && - static_cast<int>(smart_factors_to_add.size()) < params().max_num_factors) { - AddSmartFactor(points, smart_factors_to_add); - } - } - } else { - const auto& feature_tracks = feature_tracker_->feature_tracks_length_ordered(); - const auto& longest_feature_track = feature_tracker_->LongestFeatureTrack(); - if (!longest_feature_track) { - LogDebug("AddFactors: Failed to get longest feature track."); - return {}; - } - const int spacing = longest_feature_track->MaxSpacing(params().max_num_points_per_factor); - - std::unordered_map<lm::FeatureId, lm::FeaturePoint> added_points; - AddFactors(feature_tracks, spacing, params().feature_track_min_separation, smart_factors_to_add, added_points); - if (static_cast<int>(smart_factors_to_add.size()) < params().max_num_factors) { - // Zero min separation so any valid feature track is added as a fallback to try to add up to max_num_factors - AddFactors(feature_tracks, spacing, 0, smart_factors_to_add, added_points); - } - } - if (smart_factors_to_add.empty()) return {}; - const auto latest_timestamp = feature_tracker_->LatestTimestamp(); - if (!latest_timestamp) { - LogError("AddFactors: Failed to get latest timestamp."); - return {}; - } - smart_factors_to_add.SetTimestamp(*latest_timestamp); - LogDebug("AddFactors: Added " << smart_factors_to_add.size() << " smart factors."); - return {smart_factors_to_add}; -} - -void SmartProjectionCumulativeFactorAdder::AddSmartFactor(const std::vector<lm::FeaturePoint>& feature_track_points, - go::FactorsToAdd& smart_factors_to_add) const { - SharedRobustSmartFactor smart_factor; - const int num_feature_track_points = feature_track_points.size(); - const double noise_scale = - params().scale_noise_with_num_points ? params().noise_scale * num_feature_track_points : params().noise_scale; - const auto noise = gtsam::noiseModel::Isotropic::Sigma(2, noise_scale * params().cam_noise->sigma()); - smart_factor = - boost::make_shared<RobustSmartFactor>(noise, params().cam_intrinsics, params().body_T_cam, smart_projection_params_, - params().rotation_only_fallback, params().robust, params().huber_k); - - go::KeyInfos key_infos; - key_infos.reserve(feature_track_points.size()); - // Gtsam requires unique key indices for each key, even though these will be replaced later - int uninitialized_key_index = 0; - for (int i = 0; i < static_cast<int>(feature_track_points.size()); ++i) { - const auto& feature_point = feature_track_points[i]; - if (i >= params().max_num_points_per_factor) break; - const go::KeyInfo key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, feature_point.timestamp); - key_infos.emplace_back(key_info); - smart_factor->add(Camera::Measurement(feature_point.image_point), key_info.MakeKey(uninitialized_key_index++)); - } - smart_factors_to_add.push_back({key_infos, smart_factor}); -} - -bool SmartProjectionCumulativeFactorAdder::TooClose( - const std::unordered_map<lm::FeatureId, lm::FeaturePoint>& added_points, const lm::FeaturePoint& point, - const double feature_track_min_separation) const { - for (const auto& added_point_pair : added_points) { - const auto& added_point = added_point_pair.second; - if (((added_point.image_point - point.image_point).norm()) < feature_track_min_separation) { - return true; - } - } - return false; -} - -const gtsam::SmartProjectionParams& SmartProjectionCumulativeFactorAdder::smart_projection_params() const { - return smart_projection_params_; -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/smart_projection_graph_action_completer.cc b/localization/graph_localizer/src/smart_projection_graph_action_completer.cc deleted file mode 100644 index d6be435eb5..0000000000 --- a/localization/graph_localizer/src/smart_projection_graph_action_completer.cc +++ /dev/null @@ -1,82 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/smart_projection_graph_action_completer.h> -#include <graph_localizer/utilities.h> -#include <graph_optimizer/utilities.h> -#include <localization_common/logger.h> - -#include <gtsam/base/Vector.h> -#include <gtsam/slam/SmartProjectionPoseFactor.h> - -namespace graph_localizer { -namespace go = graph_optimizer; -namespace lm = localization_measurements; -namespace sym = gtsam::symbol_shorthand; -SmartProjectionGraphActionCompleter::SmartProjectionGraphActionCompleter( - const SmartProjectionFactorAdderParams& params, std::shared_ptr<const CombinedNavStateGraphValues> graph_values) - : params_(params), graph_values_(std::move(graph_values)) { - smart_projection_params_.verboseCheirality = params_.verbose_cheirality; - smart_projection_params_.setRankTolerance(1e-9); - smart_projection_params_.setLandmarkDistanceThreshold(params_.landmark_distance_threshold); - smart_projection_params_.setDynamicOutlierRejectionThreshold(params_.dynamic_outlier_rejection_threshold); - smart_projection_params_.setRetriangulationThreshold(params_.retriangulation_threshold); - smart_projection_params_.setEnableEPI(params_.enable_EPI); -} - -go::GraphActionCompleterType SmartProjectionGraphActionCompleter::type() const { - return go::GraphActionCompleterType::SmartFactor; -} - -bool SmartProjectionGraphActionCompleter::DoAction(go::FactorsToAdd& factors_to_add, - gtsam::NonlinearFactorGraph& graph_factors) { - go::DeleteFactors<RobustSmartFactor>(graph_factors); - if (params_.splitting) SplitSmartFactorsIfNeeded(*graph_values_, factors_to_add); - return true; -} - -// TODO(rsoussan): Clean this function up (duplicate code), address other todo's -void SmartProjectionGraphActionCompleter::SplitSmartFactorsIfNeeded(const CombinedNavStateGraphValues& graph_values, - go::FactorsToAdd& factors_to_add) { - for (auto& factor_to_add : factors_to_add.Get()) { - auto smart_factor = dynamic_cast<RobustSmartFactor*>(factor_to_add.factor.get()); - if (!smart_factor) continue; - // Can't remove measurements if there are only 2 or fewer - if (smart_factor->measured().size() <= 2) continue; - const auto point = smart_factor->triangulateSafe(smart_factor->cameras(graph_values.values())); - if (point.valid()) continue; - { - const auto fixed_smart_factor = - FixSmartFactorByRemovingIndividualMeasurements(params_, *smart_factor, smart_projection_params_, graph_values); - if (fixed_smart_factor) { - factor_to_add.factor = *fixed_smart_factor; - continue; - } - } - { - const auto fixed_smart_factor = - FixSmartFactorByRemovingMeasurementSequence(params_, *smart_factor, smart_projection_params_, graph_values); - if (fixed_smart_factor) { - factor_to_add.factor = *fixed_smart_factor; - continue; - } - } - LogDebug("SplitSmartFactorsIfNeeded: Failed to fix smart factor"); - } -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/standstill_factor_adder.cc b/localization/graph_localizer/src/standstill_factor_adder.cc deleted file mode 100644 index e8636bfc9f..0000000000 --- a/localization/graph_localizer/src/standstill_factor_adder.cc +++ /dev/null @@ -1,83 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/standstill_factor_adder.h> -#include <graph_localizer/utilities.h> -#include <localization_common/logger.h> - -#include <gtsam/inference/Symbol.h> -#include <gtsam/navigation/NavState.h> -#include <gtsam/nonlinear/PriorFactor.h> - -namespace graph_localizer { -namespace go = graph_optimizer; -namespace lm = localization_measurements; -namespace sym = gtsam::symbol_shorthand; -StandstillFactorAdder::StandstillFactorAdder(const StandstillFactorAdderParams& params, - std::shared_ptr<const FeatureTracker> feature_tracker) - : StandstillFactorAdder::Base(params), feature_tracker_(feature_tracker) {} - -std::vector<go::FactorsToAdd> StandstillFactorAdder::AddFactors( - const lm::FeaturePointsMeasurement& feature_points_measurement) { - std::vector<go::FactorsToAdd> factors_to_add; - if (params().add_velocity_prior) { - go::FactorsToAdd standstill_prior_factors_to_add; - const gtsam::Vector3 velocity_prior_noise_sigmas((gtsam::Vector(3) << params().prior_velocity_stddev, - params().prior_velocity_stddev, params().prior_velocity_stddev) - .finished()); - const auto velocity_noise = - Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(velocity_prior_noise_sigmas)), - params().huber_k); - - const go::KeyInfo velocity_key_info(&sym::V, go::NodeUpdaterType::CombinedNavState, - feature_points_measurement.timestamp); - gtsam::PriorFactor<gtsam::Velocity3>::shared_ptr velocity_prior_factor(new gtsam::PriorFactor<gtsam::Velocity3>( - velocity_key_info.UninitializedKey(), gtsam::Velocity3::Zero(), velocity_noise)); - standstill_prior_factors_to_add.push_back({{velocity_key_info}, velocity_prior_factor}); - standstill_prior_factors_to_add.SetTimestamp(feature_points_measurement.timestamp); - LogDebug("AddFactors: Added " << standstill_prior_factors_to_add.size() << " standstill velocity prior factors."); - factors_to_add.emplace_back(standstill_prior_factors_to_add); - } - if (params().add_pose_between_factor) { - const auto previous_timestamp = feature_tracker_->PreviousTimestamp(); - if (previous_timestamp) { - go::FactorsToAdd pose_between_factors_to_add; - const gtsam::Vector6 pose_between_noise_sigmas( - (gtsam::Vector(6) << params().pose_between_factor_rotation_stddev, params().pose_between_factor_rotation_stddev, - params().pose_between_factor_rotation_stddev, params().pose_between_factor_translation_stddev, - params().pose_between_factor_translation_stddev, params().pose_between_factor_translation_stddev) - .finished()); - const auto pose_between_noise = - Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(pose_between_noise_sigmas)), - params().huber_k); - const go::KeyInfo previous_between_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, *previous_timestamp); - const go::KeyInfo current_between_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, - feature_points_measurement.timestamp); - gtsam::BetweenFactor<gtsam::Pose3>::shared_ptr pose_between_factor(new gtsam::BetweenFactor<gtsam::Pose3>( - previous_between_key_info.UninitializedKey(), current_between_key_info.UninitializedKey(), - gtsam::Pose3::identity(), pose_between_noise)); - pose_between_factors_to_add.push_back( - {{previous_between_key_info, current_between_key_info}, pose_between_factor}); - pose_between_factors_to_add.SetTimestamp(feature_points_measurement.timestamp); - LogDebug("AddFactors: Added " << pose_between_factors_to_add.size() << " standstill pose between factors."); - factors_to_add.emplace_back(pose_between_factors_to_add); - } - } - return factors_to_add; -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/test_utilities.cc b/localization/graph_localizer/src/test_utilities.cc deleted file mode 100644 index fb5e5a9e04..0000000000 --- a/localization/graph_localizer/src/test_utilities.cc +++ /dev/null @@ -1,130 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#include <graph_localizer/test_utilities.h> -#include <localization_common/test_utilities.h> - -#include <gtsam/geometry/Point3.h> - -namespace graph_localizer { -namespace lc = localization_common; -namespace lm = localization_measurements; -namespace go = graph_optimizer; - -localization_measurements::Plane RandomPlane() { - gtsam::Point3 point = lc::RandomPoint3d(); - gtsam::Vector3 normal = lc::RandomVector3d().normalized(); - return localization_measurements::Plane(point, normal); -} - -lm::DepthOdometryMeasurement DepthOdometryMeasurementFromPose(const Eigen::Isometry3d& pose, const lc::Time source_time, - const lc::Time target_time) { - lm::Odometry odometry; - odometry.source_time = source_time; - odometry.target_time = target_time; - odometry.sensor_F_source_T_target.pose = pose; - odometry.sensor_F_source_T_target.covariance = Eigen::Matrix<double, 6, 6>::Identity(); - odometry.body_F_source_T_target.pose = pose; - odometry.body_F_source_T_target.covariance = Eigen::Matrix<double, 6, 6>::Identity(); - std::vector<Eigen::Vector3d> empty_points; - lm::DepthCorrespondences correspondences(empty_points, empty_points); - return lm::DepthOdometryMeasurement(odometry, correspondences, target_time); -} - -CombinedNavStateGraphValuesParams DefaultCombinedNavStateGraphValuesParams() { - CombinedNavStateGraphValuesParams params; - params.ideal_duration = 3; - params.min_num_states = 3; - params.max_num_states = 20; - return params; -} - -go::GraphOptimizerParams DefaultGraphOptimizerParams() { - go::GraphOptimizerParams params; - params.verbose = false; - params.fatal_failures = false; - params.log_on_destruction = false; - params.print_factor_info = false; - params.use_ceres_params = false; - params.max_iterations = 4; - params.marginals_factorization = "qr"; - params.add_marginal_factors = false; - params.huber_k = 1.345; - params.log_rate = 100; - return params; -} - -CombinedNavStateNodeUpdaterParams DefaultCombinedNavStateNodeUpdaterParams() { - CombinedNavStateNodeUpdaterParams params; - params.starting_prior_translation_stddev = 0.02; - params.starting_prior_quaternion_stddev = 0.01; - params.starting_prior_velocity_stddev = 0.01; - params.starting_prior_accel_bias_stddev = 0.001; - params.starting_prior_gyro_bias_stddev = 0.001; - params.huber_k = 1.345; - params.global_N_body_start = - lc::CombinedNavState(gtsam::Pose3::identity(), gtsam::Velocity3::Zero(), gtsam::imuBias::ConstantBias(), 0.0); - params.add_priors = true; - params.graph_values = DefaultCombinedNavStateGraphValuesParams(); - params.threshold_bias_uncertainty = false; - return params; -} - -GraphInitializerParams DefaultGraphInitializerParams() { - GraphInitializerParams params; - params.global_T_body_start = gtsam::Pose3::identity(); - params.global_V_body_start = gtsam::Velocity3::Zero(); - params.num_bias_estimation_measurements = 1; - params.start_time = 0; - params.initial_imu_bias = gtsam::imuBias::ConstantBias(); - params.gravity = gtsam::Vector3::Zero(); - params.body_T_imu = gtsam::Pose3::identity(); - params.filter = imu_integration::ImuFilterParams(); - params.gyro_sigma = 0.001; - params.accel_sigma = 0.001; - params.accel_bias_sigma = 0.001; - params.gyro_bias_sigma = 0.001; - params.integration_variance = 0.001; - params.bias_acc_omega_int = 0.001; - return params; -} - -GraphLocalizerParams DefaultGraphLocalizerParams() { - GraphLocalizerParams params; - params.combined_nav_state_node_updater = DefaultCombinedNavStateNodeUpdaterParams(); - params.graph_optimizer = DefaultGraphOptimizerParams(); - params.graph_initializer = DefaultGraphInitializerParams(); - params.huber_k = 1.345; - return params; -} - -DepthOdometryFactorAdderParams DefaultDepthOdometryFactorAdderParams() { - DepthOdometryFactorAdderParams params; - params.enabled = true; - params.huber_k = 1.345; - params.noise_scale = 1.0; - params.use_points_between_factor = false; - params.position_covariance_threshold = 100; - params.orientation_covariance_threshold = 100; - params.point_to_point_error_threshold = 100.0; - params.pose_translation_norm_threshold = 100.0; - params.max_num_points_between_factors = 100; - params.body_T_sensor = gtsam::Pose3::identity(); - params.enabled = true; - return params; -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/src/utilities.cc b/localization/graph_localizer/src/utilities.cc deleted file mode 100644 index 2d3135c827..0000000000 --- a/localization/graph_localizer/src/utilities.cc +++ /dev/null @@ -1,277 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/feature_track.h> -#include <graph_localizer/serialization.h> -#include <graph_localizer/utilities.h> -#include <imu_integration/utilities.h> -#include <localization_common/logger.h> -#include <localization_common/utilities.h> -#include <msg_conversions/msg_conversions.h> - -#include <cstdlib> -#include <string> - -namespace graph_localizer { -namespace go = graph_optimizer; -namespace ii = imu_integration; -namespace lc = localization_common; -namespace lm = localization_measurements; -namespace mc = msg_conversions; - -bool ValidPointSet(const int num_points, const double average_distance_from_mean, - const double min_avg_distance_from_mean, const int min_num_points) { - if (num_points < min_num_points) return false; - return (average_distance_from_mean >= min_avg_distance_from_mean); -} - -double AverageDistanceFromMean(const std::vector<lm::FeaturePoint>& points) { - // Calculate mean point and avg distance from mean - Eigen::Vector2d sum_of_points = Eigen::Vector2d::Zero(); - for (const auto& point : points) { - sum_of_points += point.image_point; - } - const Eigen::Vector2d mean_point = sum_of_points / points.size(); - - double sum_of_distances_from_mean = 0; - for (const auto& point : points) { - const Eigen::Vector2d mean_centered_point = point.image_point - mean_point; - sum_of_distances_from_mean += mean_centered_point.norm(); - } - const double average_distance_from_mean = sum_of_distances_from_mean / points.size(); - return average_distance_from_mean; -} - -bool ValidVLMsg(const ff_msgs::VisualLandmarks& visual_landmarks_msg, const int min_num_landmarks) { - return (static_cast<int>(visual_landmarks_msg.landmarks.size()) >= min_num_landmarks); -} - -bool ValidDepthMsg(const ff_msgs::DepthLandmarks& depth_landmarks_msg) { - return (static_cast<int>(depth_landmarks_msg.landmarks.size()) >= 0); -} - -ff_msgs::GraphState GraphStateMsg(const lc::CombinedNavState& combined_nav_state, - const lc::CombinedNavStateCovariances& covariances, - const FeatureCounts& detected_feature_counts, const bool estimating_bias, - const double position_log_det_threshold, const double orientation_log_det_threshold, - const bool standstill, const GraphLocalizerStats& graph_stats, - const lm::FanSpeedMode fan_speed_mode) { - ff_msgs::GraphState loc_msg; - - // Set Header Frames - loc_msg.header.frame_id = "world"; - loc_msg.child_frame_id = "body"; - - // Set CombinedNavState - lc::CombinedNavStateToMsg(combined_nav_state, loc_msg); - - // Set Variances - lc::CombinedNavStateCovariancesToMsg(covariances, loc_msg); - - // Set Confidence - // Controller can't handle a confidence other than 0. TODO(rsoussan): switch back when this is fixed. - loc_msg.confidence = 0; // covariances.PoseConfidence(position_log_det_threshold, orientation_log_det_threshold); - - // Set Graph Feature Counts/Information - loc_msg.num_detected_of_features = detected_feature_counts.of; - loc_msg.num_detected_ml_features = detected_feature_counts.vl; - loc_msg.num_detected_ar_features = detected_feature_counts.ar; - loc_msg.estimating_bias = estimating_bias; - - // Set Graph Stats - loc_msg.iterations = graph_stats.iterations_averager_.last_value(); - loc_msg.optimization_time = graph_stats.optimization_timer_.last_value(); - loc_msg.update_time = graph_stats.update_timer_.last_value(); - loc_msg.num_factors = graph_stats.num_factors_averager_.last_value(); - loc_msg.num_of_factors = graph_stats.num_optical_flow_factors_averager_.last_value(); - loc_msg.num_ml_projection_factors = graph_stats.num_loc_proj_factors_averager_.last_value(); - loc_msg.num_ml_pose_factors = graph_stats.num_loc_pose_factors_averager_.last_value(); - loc_msg.num_states = graph_stats.num_states_averager_.last_value(); - // Other - loc_msg.standstill = standstill; - loc_msg.fan_speed_mode = static_cast<uint8_t>(fan_speed_mode); - return loc_msg; -} - -ff_msgs::LocalizationGraph GraphMsg(const GraphLocalizer& graph_localizer) { - ff_msgs::LocalizationGraph graph_msg; - - // Set Header Frames - graph_msg.header.frame_id = "world"; - graph_msg.child_frame_id = "body"; - - // TODO(rsoussan): set correct time - lc::TimeToHeader(5, graph_msg.header); - - graph_msg.serialized_graph = SerializeBinary(graph_localizer); - return graph_msg; -} - -geometry_msgs::PoseStamped PoseMsg(const Eigen::Isometry3d& global_T_body, const std_msgs::Header& header) { - geometry_msgs::PoseStamped pose_msg; - pose_msg.header = header; - mc::EigenPoseToMsg(global_T_body, pose_msg.pose); - return pose_msg; -} - -geometry_msgs::PoseStamped PoseMsg(const Eigen::Isometry3d& global_T_body, const lc::Time time) { - std_msgs::Header header; - lc::TimeToHeader(time, header); - header.frame_id = "world"; - return PoseMsg(global_T_body, header); -} - -geometry_msgs::PoseStamped PoseMsg(const gtsam::Pose3& global_T_body, const lc::Time time) { - return PoseMsg(lc::EigenPose(global_T_body), time); -} - -geometry_msgs::PoseStamped PoseMsg(const lm::TimestampedPose& timestamped_pose) { - return PoseMsg(timestamped_pose.pose, timestamped_pose.time); -} - -gtsam::noiseModel::Robust::shared_ptr Robust(const gtsam::SharedNoiseModel& noise, const double huber_k) { - return gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Huber::Create(huber_k), noise); -} - -boost::optional<SharedRobustSmartFactor> FixSmartFactorByRemovingIndividualMeasurements( - const SmartProjectionFactorAdderParams& params, const RobustSmartFactor& smart_factor, - const gtsam::SmartProjectionParams& smart_projection_params, const CombinedNavStateGraphValues& graph_values) { - // TODO(rsoussan): Make this more efficient by enabled removal of measurements and keys in smart factor - const auto original_measurements = smart_factor.measured(); - const auto original_keys = smart_factor.keys(); - int measurement_index_to_remove; - // Start with latest measurement - for (measurement_index_to_remove = original_measurements.size() - 1; measurement_index_to_remove >= 0; - --measurement_index_to_remove) { - gtsam::PinholePose<gtsam::Cal3_S2>::MeasurementVector measurements_to_add; - gtsam::KeyVector keys_to_add; - for (int i = 0; i < static_cast<int>(original_measurements.size()); ++i) { - if (i == measurement_index_to_remove) continue; - measurements_to_add.emplace_back(original_measurements[i]); - keys_to_add.emplace_back(original_keys[i]); - } - auto new_smart_factor = boost::make_shared<RobustSmartFactor>( - params.cam_noise, params.cam_intrinsics, params.body_T_cam, smart_projection_params, - params.rotation_only_fallback, params.robust, params.huber_k); - new_smart_factor->add(measurements_to_add, keys_to_add); - const auto new_point = new_smart_factor->triangulateSafe(new_smart_factor->cameras(graph_values.values())); - if (new_point.valid()) { - LogDebug("FixSmartFactorByRemovingIndividualMeasurements: Fixed by removing measurement " - << measurement_index_to_remove << ", num original measurements: " << original_measurements.size()); - return new_smart_factor; - } - } - return boost::none; -} - -boost::optional<SharedRobustSmartFactor> FixSmartFactorByRemovingMeasurementSequence( - const SmartProjectionFactorAdderParams& params, const RobustSmartFactor& smart_factor, - const gtsam::SmartProjectionParams& smart_projection_params, const CombinedNavStateGraphValues& graph_values) { - constexpr int min_num_measurements = 2; - // TODO(rsoussan): Make this more efficient by enabled removal of measurements and keys in smart factor - const auto original_measurements = smart_factor.measured(); - const auto original_keys = smart_factor.keys(); - int num_measurements_to_add = original_measurements.size() - 1; - // Try to remove min number of most recent measurements - while (num_measurements_to_add >= min_num_measurements) { - gtsam::PinholePose<gtsam::Cal3_S2>::MeasurementVector measurements_to_add; - gtsam::KeyVector keys_to_add; - for (int i = 0; i < num_measurements_to_add; ++i) { - measurements_to_add.emplace_back(original_measurements[i]); - keys_to_add.emplace_back(original_keys[i]); - } - auto new_smart_factor = boost::make_shared<RobustSmartFactor>( - params.cam_noise, params.cam_intrinsics, params.body_T_cam, smart_projection_params, - params.rotation_only_fallback, params.robust, params.huber_k); - new_smart_factor->add(measurements_to_add, keys_to_add); - const auto new_point = new_smart_factor->triangulateSafe(new_smart_factor->cameras(graph_values.values())); - if (new_point.valid()) { - LogDebug( - "FixSmartFactorByRemovingMeasurementSequence: Fixed smart factor by removing most recent " - "measurements. Original " - "measurement size: " - << original_measurements.size() << ", new size: " << num_measurements_to_add); - return new_smart_factor; - } else { - --num_measurements_to_add; - } - } - if (num_measurements_to_add < min_num_measurements) { - num_measurements_to_add = original_measurements.size() - 1; - // Try to remove min number of oldest measurements - while (num_measurements_to_add >= min_num_measurements) { - gtsam::PinholePose<gtsam::Cal3_S2>::MeasurementVector measurements_to_add; - gtsam::KeyVector keys_to_add; - for (int i = num_measurements_to_add; - i >= static_cast<int>(original_measurements.size()) - num_measurements_to_add; --i) { - measurements_to_add.emplace_back(original_measurements[i]); - keys_to_add.emplace_back(original_keys[i]); - } - auto new_smart_factor = boost::make_shared<RobustSmartFactor>( - params.cam_noise, params.cam_intrinsics, params.body_T_cam, smart_projection_params, - params.rotation_only_fallback, params.robust, params.huber_k); - new_smart_factor->add(measurements_to_add, keys_to_add); - const auto new_point = new_smart_factor->triangulateSafe(new_smart_factor->cameras(graph_values.values())); - if (new_point.valid()) { - LogDebug( - "FixSmartFactorByRemovingMeasurementSequence: Fixed smart factor by removing oldest measurements. " - "Original " - "measurement size: " - << original_measurements.size() << ", new size: " << num_measurements_to_add); - return new_smart_factor; - } else { - --num_measurements_to_add; - } - } - } - // Failed to fix smart factor - return boost::none; - // TODO(rsoussan): delete factor if fail to find acceptable new one? - // TODO(rsoussan): attempt to make a second factor with remaining measuremnts!!! -} - -SharedRobustSmartFactor RemoveSmartFactorMeasurements(const RobustSmartFactor& smart_factor, - const std::unordered_set<int>& factor_key_indices_to_remove, - const SmartProjectionFactorAdderParams& params, - const gtsam::SmartProjectionParams& smart_projection_params) { - auto new_smart_factor = boost::make_shared<RobustSmartFactor>( - params.cam_noise, params.cam_intrinsics, params.body_T_cam, smart_projection_params, params.rotation_only_fallback, - params.robust, params.huber_k); - for (int i = 0; i < static_cast<int>(smart_factor.keys().size()); ++i) { - if (factor_key_indices_to_remove.count(i) == 0) - new_smart_factor->add(smart_factor.measured()[i], smart_factor.keys()[i]); - } - return new_smart_factor; -} - -int NumSmartFactors(const gtsam::NonlinearFactorGraph& graph_factors, const gtsam::Values& values, - const bool check_valid) { - int num_of_factors = 0; - for (const auto& factor : graph_factors) { - const auto smart_factor = dynamic_cast<const RobustSmartFactor*>(factor.get()); - if (smart_factor) { - if (check_valid) { - if (smart_factor->valid(values)) ++num_of_factors; - } else { - ++num_of_factors; - } - } - } - return num_of_factors; -} -} // namespace graph_localizer diff --git a/localization/graph_localizer/test/test_combined_nav_state_node_updater.cc b/localization/graph_localizer/test/test_combined_nav_state_node_updater.cc deleted file mode 100644 index 24081ea581..0000000000 --- a/localization/graph_localizer/test/test_combined_nav_state_node_updater.cc +++ /dev/null @@ -1,405 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/graph_localizer.h> -#include <graph_localizer/graph_localizer_params.h> -#include <graph_localizer/test_utilities.h> -#include <localization_common/logger.h> -#include <localization_common/test_utilities.h> -#include <localization_common/utilities.h> -#include <localization_measurements/imu_measurement.h> - -#include <gtsam/inference/Symbol.h> - -#include <gtest/gtest.h> - -namespace gl = graph_localizer; -namespace lc = localization_common; -namespace lm = localization_measurements; -namespace sym = gtsam::symbol_shorthand; - -TEST(CombinedNavStateNodeUpdaterTester, ConstantVelocity) { - auto params = gl::DefaultGraphLocalizerParams(); - // Use depth odometry factor adder since it can add relative pose factors - params.factor.depth_odometry_adder = gl::DefaultDepthOdometryFactorAdderParams(); - constexpr double kInitialVelocity = 0.1; - params.graph_initializer.global_V_body_start = Eigen::Vector3d(kInitialVelocity, 0, 0); - gl::GraphLocalizer graph_localizer(params); - constexpr int kNumIterations = 100; - constexpr double kTimeDiff = 0.1; - lc::Time time = 0.0; - const Eigen::Vector3d relative_translation = kTimeDiff * params.graph_initializer.global_V_body_start; - Eigen::Isometry3d current_pose = lc::EigenPose(params.graph_initializer.global_T_body_start); - // Add initial zero acceleration value so the imu integrator has more than one measurement when the subsequent - // measurement is added - const lm::ImuMeasurement initial_zero_imu_measurement(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), time); - graph_localizer.AddImuMeasurement(initial_zero_imu_measurement); - for (int i = 0; i < kNumIterations; ++i) { - time += kTimeDiff; - const lm::ImuMeasurement zero_imu_measurement(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), time); - graph_localizer.AddImuMeasurement(zero_imu_measurement); - const Eigen::Isometry3d relative_pose = lc::Isometry3d(relative_translation, Eigen::Matrix3d::Identity()); - current_pose = current_pose * relative_pose; - const lc::Time source_time = time - kTimeDiff; - const lc::Time target_time = time; - const lm::DepthOdometryMeasurement constant_velocity_measurement = - gl::DepthOdometryMeasurementFromPose(relative_pose, source_time, target_time); - graph_localizer.AddDepthOdometryMeasurement(constant_velocity_measurement); - graph_localizer.Update(); - const auto latest_combined_nav_state = graph_localizer.LatestCombinedNavState(); - ASSERT_TRUE(latest_combined_nav_state != boost::none); - EXPECT_NEAR(latest_combined_nav_state->timestamp(), time, 1e-6); - EXPECT_MATRIX_NEAR(latest_combined_nav_state->pose(), current_pose, 1e-5); - } -} - -TEST(CombinedNavStateNodeUpdaterTester, ConstantAcceleration) { - auto params = gl::DefaultGraphLocalizerParams(); - // Use depth odometry factor adder since it can add relative pose factors - params.factor.depth_odometry_adder = gl::DefaultDepthOdometryFactorAdderParams(); - gl::GraphLocalizer graph_localizer(params); - constexpr int kNumIterations = 100; - constexpr double kTimeDiff = 0.1; - lc::Time time = 0.0; - Eigen::Isometry3d current_pose = lc::EigenPose(params.graph_initializer.global_T_body_start); - const Eigen::Vector3d acceleration(0.01, 0.02, 0.03); - Eigen::Vector3d velocity = params.graph_initializer.global_V_body_start; - // Add initial zero acceleration value so the imu integrator has more than one measurement when the subsequent - // measurement is added - const lm::ImuMeasurement zero_imu_measurement(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), time); - graph_localizer.AddImuMeasurement(zero_imu_measurement); - for (int i = 0; i < kNumIterations; ++i) { - time += kTimeDiff; - const lm::ImuMeasurement imu_measurement(acceleration, Eigen::Vector3d::Zero(), time); - graph_localizer.AddImuMeasurement(imu_measurement); - const Eigen::Vector3d relative_translation = velocity * kTimeDiff + 0.5 * acceleration * kTimeDiff * kTimeDiff; - velocity += acceleration * kTimeDiff; - const Eigen::Isometry3d relative_pose = lc::Isometry3d(relative_translation, Eigen::Matrix3d::Identity()); - current_pose = current_pose * relative_pose; - const lc::Time source_time = time - kTimeDiff; - const lc::Time target_time = time; - const lm::DepthOdometryMeasurement constant_acceleration_measurement = - gl::DepthOdometryMeasurementFromPose(relative_pose, source_time, target_time); - graph_localizer.AddDepthOdometryMeasurement(constant_acceleration_measurement); - graph_localizer.Update(); - const auto latest_combined_nav_state = graph_localizer.LatestCombinedNavState(); - ASSERT_TRUE(latest_combined_nav_state != boost::none); - EXPECT_NEAR(latest_combined_nav_state->timestamp(), time, 1e-6); - EXPECT_MATRIX_NEAR(latest_combined_nav_state->pose(), current_pose, 1e-5); - } -} - -TEST(CombinedNavStateNodeUpdaterTester, ConstantAccelerationNonZeroBias) { - const Eigen::Vector3d acceleration(0.01, 0.02, 0.03); - const Eigen::Vector3d angular_velocity(0.04, 0.05, 0.06); - const Eigen::Vector3d acceleration_bias = 0.5 * acceleration; - const Eigen::Vector3d angular_velocity_bias = angular_velocity; - const Eigen::Vector3d bias_corrected_acceleration = acceleration - acceleration_bias; - const Eigen::Vector3d bias_corrected_angular_velocity = angular_velocity - angular_velocity_bias; - auto params = gl::DefaultGraphLocalizerParams(); - params.graph_initializer.initial_imu_bias = gtsam::imuBias::ConstantBias(acceleration_bias, angular_velocity_bias); - // Use depth odometry factor adder since it can add relative pose factors - params.factor.depth_odometry_adder = gl::DefaultDepthOdometryFactorAdderParams(); - gl::GraphLocalizer graph_localizer(params); - constexpr int kNumIterations = 100; - constexpr double kTimeDiff = 0.1; - lc::Time time = 0.0; - Eigen::Isometry3d current_pose = lc::EigenPose(params.graph_initializer.global_T_body_start); - Eigen::Vector3d velocity = params.graph_initializer.global_V_body_start; - // Add initial zero imu value so the imu integrator has more than one measurement when the subsequent - // measurement is added - const lm::ImuMeasurement zero_imu_measurement(acceleration_bias, angular_velocity_bias, time); - graph_localizer.AddImuMeasurement(zero_imu_measurement); - for (int i = 0; i < kNumIterations; ++i) { - time += kTimeDiff; - const lm::ImuMeasurement imu_measurement(acceleration, angular_velocity, time); - graph_localizer.AddImuMeasurement(imu_measurement); - const Eigen::Vector3d relative_translation = - velocity * kTimeDiff + 0.5 * bias_corrected_acceleration * kTimeDiff * kTimeDiff; - velocity += bias_corrected_acceleration * kTimeDiff; - const Eigen::Isometry3d relative_pose = lc::Isometry3d(relative_translation, Eigen::Matrix3d::Identity()); - current_pose = current_pose * relative_pose; - const lc::Time source_time = time - kTimeDiff; - const lc::Time target_time = time; - const lm::DepthOdometryMeasurement constant_acceleration_measurement = - gl::DepthOdometryMeasurementFromPose(relative_pose, source_time, target_time); - graph_localizer.AddDepthOdometryMeasurement(constant_acceleration_measurement); - graph_localizer.Update(); - const auto latest_combined_nav_state = graph_localizer.LatestCombinedNavState(); - ASSERT_TRUE(latest_combined_nav_state != boost::none); - EXPECT_NEAR(latest_combined_nav_state->timestamp(), time, 1e-6); - EXPECT_MATRIX_NEAR(latest_combined_nav_state->pose(), current_pose, 1e-5); - } -} - -TEST(CombinedNavStateNodeUpdaterTester, ConstantAccelerationConstantAngularVelocityNonZeroBias) { - const Eigen::Vector3d acceleration(0.01, 0.02, 0.03); - const Eigen::Vector3d angular_velocity(0.04, 0.05, 0.06); - const Eigen::Vector3d acceleration_bias = 0.5 * acceleration; - const Eigen::Vector3d angular_velocity_bias = 0.5 * angular_velocity; - const Eigen::Vector3d bias_corrected_acceleration = acceleration - acceleration_bias; - const Eigen::Vector3d bias_corrected_angular_velocity = angular_velocity - angular_velocity_bias; - auto params = gl::DefaultGraphLocalizerParams(); - params.graph_initializer.initial_imu_bias = gtsam::imuBias::ConstantBias(acceleration_bias, angular_velocity_bias); - // Use depth odometry factor adder since it can add relative pose factors - params.factor.depth_odometry_adder = gl::DefaultDepthOdometryFactorAdderParams(); - gl::GraphLocalizer graph_localizer(params); - constexpr int kNumIterations = 100; - constexpr double kTimeDiff = 0.1; - lc::Time time = 0.0; - Eigen::Isometry3d current_pose = lc::EigenPose(params.graph_initializer.global_T_body_start); - Eigen::Vector3d velocity = params.graph_initializer.global_V_body_start; - // Add initial zero imu value so the imu integrator has more than one measurement when the subsequent - // measurement is added - const lm::ImuMeasurement zero_imu_measurement(acceleration_bias, angular_velocity_bias, time); - graph_localizer.AddImuMeasurement(zero_imu_measurement); - for (int i = 0; i < kNumIterations; ++i) { - time += kTimeDiff; - const lm::ImuMeasurement imu_measurement(acceleration, angular_velocity, time); - graph_localizer.AddImuMeasurement(imu_measurement); - const Eigen::Matrix3d relative_orientation = - (gtsam::Rot3::Expmap(bias_corrected_angular_velocity * kTimeDiff)).matrix(); - const Eigen::Vector3d relative_translation = - velocity * kTimeDiff + 0.5 * bias_corrected_acceleration * kTimeDiff * kTimeDiff; - velocity += bias_corrected_acceleration * kTimeDiff; - // Put velocity in new body frame after integrating accelerations - velocity = relative_orientation.transpose() * velocity; - const Eigen::Isometry3d relative_pose = lc::Isometry3d(relative_translation, relative_orientation); - current_pose = current_pose * relative_pose; - const lc::Time source_time = time - kTimeDiff; - const lc::Time target_time = time; - const lm::DepthOdometryMeasurement constant_acceleration_measurement = - gl::DepthOdometryMeasurementFromPose(relative_pose, source_time, target_time); - graph_localizer.AddDepthOdometryMeasurement(constant_acceleration_measurement); - graph_localizer.Update(); - const auto latest_combined_nav_state = graph_localizer.LatestCombinedNavState(); - ASSERT_TRUE(latest_combined_nav_state != boost::none); - EXPECT_NEAR(latest_combined_nav_state->timestamp(), time, 1e-6); - EXPECT_MATRIX_NEAR(latest_combined_nav_state->pose(), current_pose, 1e-5); - } -} - -TEST(CombinedNavStateNodeUpdaterTester, SlidingWindow) { - auto params = gl::DefaultGraphLocalizerParams(); - params.combined_nav_state_node_updater.graph_values.max_num_states = 1000000; - // Add 1e-5 to avoid float comparison issues when sliding window. - // In practice, latest_timestamp - ideal_duration shouldn't be identical to a node timestamp - // and these float comparisons aren't an issue. Only an issue in tests. - const double ideal_duration = 3.0; - params.combined_nav_state_node_updater.graph_values.ideal_duration = ideal_duration + 1e-5; - // Use depth odometry factor adder since it can add relative pose factors - params.factor.depth_odometry_adder = gl::DefaultDepthOdometryFactorAdderParams(); - constexpr double kInitialVelocity = 0.1; - params.graph_initializer.global_V_body_start = Eigen::Vector3d(kInitialVelocity, 0, 0); - gl::GraphLocalizer graph_localizer(params); - constexpr int kNumIterations = 100; - constexpr double kTimeDiff = 0.1; - // Add 1 for initial state - const int max_num_states_in_sliding_window = ideal_duration / kTimeDiff + 1; - lc::Time time = 0.0; - const Eigen::Vector3d relative_translation = kTimeDiff * params.graph_initializer.global_V_body_start; - Eigen::Isometry3d current_pose = lc::EigenPose(params.graph_initializer.global_T_body_start); - // Add initial zero acceleration value so the imu integrator has more than one measurement when the subsequent - // measurement is added - const lm::ImuMeasurement initial_zero_imu_measurement(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), time); - graph_localizer.AddImuMeasurement(initial_zero_imu_measurement); - lc::Time last_time = 0; - for (int i = 0; i < kNumIterations; ++i) { - last_time = time; - time += kTimeDiff; - const lm::ImuMeasurement zero_imu_measurement(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), time); - graph_localizer.AddImuMeasurement(zero_imu_measurement); - const Eigen::Isometry3d relative_pose = lc::Isometry3d(relative_translation, Eigen::Matrix3d::Identity()); - current_pose = current_pose * relative_pose; - const lc::Time source_time = last_time; - const lc::Time target_time = time; - const lm::DepthOdometryMeasurement constant_velocity_measurement = - gl::DepthOdometryMeasurementFromPose(relative_pose, source_time, target_time); - graph_localizer.AddDepthOdometryMeasurement(constant_velocity_measurement); - graph_localizer.Update(); - const auto latest_combined_nav_state = graph_localizer.LatestCombinedNavState(); - ASSERT_TRUE(latest_combined_nav_state != boost::none); - EXPECT_NEAR(latest_combined_nav_state->timestamp(), time, 1e-6); - EXPECT_MATRIX_NEAR(latest_combined_nav_state->pose(), current_pose, 1e-5); - // Check num states, ensure window is sliding properly - // i + 2 since graph is initialized with a starting state and i = 0 also adds a state - const int total_states_added = i + 2; - EXPECT_EQ(graph_localizer.combined_nav_state_graph_values().NumStates(), - std::min(total_states_added, max_num_states_in_sliding_window)); - const auto& combined_nav_state_node_updater = graph_localizer.combined_nav_state_node_updater(); - const lc::Time expected_oldest_timestamp = std::max(0.0, time - ideal_duration); - // Check latest and oldest timestamps - { - const auto oldest_timestamp = combined_nav_state_node_updater.OldestTimestamp(); - ASSERT_TRUE(oldest_timestamp != boost::none); - EXPECT_NEAR(*oldest_timestamp, expected_oldest_timestamp, 1e-6); - const auto latest_timestamp = combined_nav_state_node_updater.LatestTimestamp(); - ASSERT_TRUE(latest_timestamp != boost::none); - EXPECT_NEAR(*latest_timestamp, time, 1e-6); - } - // Check node timestamps - { - const auto timestamps = graph_localizer.combined_nav_state_graph_values().Timestamps(); - lc::Time expected_timestamp = expected_oldest_timestamp; - for (const auto timestamp : timestamps) { - EXPECT_NEAR(timestamp, expected_timestamp, 1e-6); - expected_timestamp += kTimeDiff; - } - } - // Check corect factors are in graph - { - // i + 1 factors for each relative pose factor and imu factor and 3 prior factors for pose, velocity, and bias - const int num_relative_factors = std::min(30, i + 1); - const int num_prior_factors = 3; - // Max 30 relative pose and imu factors due to sliding graph size - EXPECT_EQ(graph_localizer.num_factors(), 2 * num_relative_factors + num_prior_factors); - const auto imu_factors = graph_localizer.Factors<gtsam::CombinedImuFactor>(); - EXPECT_EQ(imu_factors.size(), num_relative_factors); - const auto rel_pose_factors = graph_localizer.Factors<gtsam::BetweenFactor<gtsam::Pose3>>(); - EXPECT_EQ(rel_pose_factors.size(), num_relative_factors); - // Check priors - const auto oldest_key_index = graph_localizer.combined_nav_state_graph_values().OldestCombinedNavStateKeyIndex(); - ASSERT_TRUE(oldest_key_index != boost::none); - const auto pose_prior_factors = graph_localizer.Factors<gtsam::PriorFactor<gtsam::Pose3>>(); - ASSERT_EQ(pose_prior_factors.size(), 1); - EXPECT_EQ(sym::P(*oldest_key_index), pose_prior_factors[0]->keys()[0]); - const auto velocity_prior_factors = graph_localizer.Factors<gtsam::PriorFactor<gtsam::Velocity3>>(); - ASSERT_EQ(velocity_prior_factors.size(), 1); - EXPECT_EQ(sym::V(*oldest_key_index), velocity_prior_factors[0]->keys()[0]); - const auto imu_bias_prior_factors = graph_localizer.Factors<gtsam::PriorFactor<gtsam::imuBias::ConstantBias>>(); - ASSERT_EQ(imu_bias_prior_factors.size(), 1); - EXPECT_EQ(sym::B(*oldest_key_index), imu_bias_prior_factors[0]->keys()[0]); - } - } -} - -TEST(CombinedNavStateNodeUpdaterTester, SplitExistingFactor) { - auto params = gl::DefaultGraphLocalizerParams(); - params.combined_nav_state_node_updater.graph_values.max_num_states = 1000000; - constexpr double kInitialVelocity = 0.1; - params.graph_initializer.global_V_body_start = Eigen::Vector3d(kInitialVelocity, 0, 0); - // Use depth odometry factor adder since it can add relative pose factors - params.factor.depth_odometry_adder = gl::DefaultDepthOdometryFactorAdderParams(); - gl::GraphLocalizer graph_localizer(params); - const auto& combined_nav_state_node_updater = graph_localizer.combined_nav_state_node_updater(); - constexpr double kTimeDiff = 0.1; - const lc::Time oldest_time = 0.0; - Eigen::Isometry3d initial_pose = lc::EigenPose(params.graph_initializer.global_T_body_start); - // Add initial zero acceleration value so the imu integrator has more than one measurement when the subsequent - // measurement is added - const lm::ImuMeasurement initial_zero_imu_measurement(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), oldest_time); - graph_localizer.AddImuMeasurement(initial_zero_imu_measurement); - const lc::Time latest_time = kTimeDiff; - const Eigen::Vector3d latest_relative_translation = latest_time * params.graph_initializer.global_V_body_start; - const Eigen::Isometry3d latest_relative_pose = - lc::Isometry3d(latest_relative_translation, Eigen::Matrix3d::Identity()); - const Eigen::Isometry3d latest_pose = initial_pose * latest_relative_pose; - // Add latest measurement - { - const lm::ImuMeasurement zero_imu_measurement(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), latest_time); - graph_localizer.AddImuMeasurement(zero_imu_measurement); - const lc::Time source_time = oldest_time; - const lc::Time target_time = latest_time; - const lm::DepthOdometryMeasurement constant_velocity_measurement = - gl::DepthOdometryMeasurementFromPose(latest_relative_pose, source_time, target_time); - graph_localizer.AddDepthOdometryMeasurement(constant_velocity_measurement); - graph_localizer.Update(); - EXPECT_EQ(graph_localizer.combined_nav_state_graph_values().NumStates(), 2); - const auto oldest_timestamp = combined_nav_state_node_updater.OldestTimestamp(); - ASSERT_TRUE(oldest_timestamp != boost::none); - EXPECT_NEAR(*oldest_timestamp, oldest_time, 1e-6); - const auto latest_timestamp = combined_nav_state_node_updater.LatestTimestamp(); - ASSERT_TRUE(latest_timestamp != boost::none); - EXPECT_NEAR(*latest_timestamp, latest_time, 1e-6); - const auto imu_factors = graph_localizer.Factors<gtsam::CombinedImuFactor>(); - ASSERT_EQ(imu_factors.size(), 1); - const auto pose_key_1 = imu_factors[0]->key1(); - const auto key_1_time = graph_localizer.combined_nav_state_graph_values().Timestamp(sym::P, pose_key_1); - ASSERT_TRUE(key_1_time != boost::none); - EXPECT_NEAR(*key_1_time, oldest_time, 1e-6); - const auto pose_key_2 = imu_factors[0]->key3(); - const auto key_2_time = graph_localizer.combined_nav_state_graph_values().Timestamp(sym::P, pose_key_2); - ASSERT_TRUE(key_2_time != boost::none); - EXPECT_NEAR(*key_2_time, latest_time, 1e-6); - const auto latest_combined_nav_state = graph_localizer.LatestCombinedNavState(); - ASSERT_TRUE(latest_combined_nav_state != boost::none); - EXPECT_NEAR(latest_combined_nav_state->timestamp(), latest_time, 1e-6); - EXPECT_MATRIX_NEAR(latest_combined_nav_state->pose(), latest_pose, 1e-5); - } - // Add measurement between initial and latest times - { - const lc::Time middle_time = kTimeDiff / 2.0; - const Eigen::Vector3d middle_relative_translation = middle_time * params.graph_initializer.global_V_body_start; - const Eigen::Isometry3d middle_relative_pose = - lc::Isometry3d(middle_relative_translation, Eigen::Matrix3d::Identity()); - const Eigen::Isometry3d middle_pose = initial_pose * middle_relative_pose; - const lc::Time source_time = oldest_time; - const lc::Time target_time = middle_time; - const lm::DepthOdometryMeasurement constant_velocity_measurement = - gl::DepthOdometryMeasurementFromPose(middle_relative_pose, source_time, target_time); - graph_localizer.AddDepthOdometryMeasurement(constant_velocity_measurement); - graph_localizer.Update(); - EXPECT_EQ(graph_localizer.combined_nav_state_graph_values().NumStates(), 3); - const auto oldest_timestamp = combined_nav_state_node_updater.OldestTimestamp(); - ASSERT_TRUE(oldest_timestamp != boost::none); - EXPECT_NEAR(*oldest_timestamp, oldest_time, 1e-6); - const auto latest_timestamp = combined_nav_state_node_updater.LatestTimestamp(); - ASSERT_TRUE(latest_timestamp != boost::none); - EXPECT_NEAR(*latest_timestamp, latest_time, 1e-6); - const auto imu_factors = graph_localizer.Factors<gtsam::CombinedImuFactor>(); - ASSERT_EQ(imu_factors.size(), 2); - // Check middle imu factor - { - const auto& middle_imu_factor = imu_factors[0]; - const auto pose_key_1 = middle_imu_factor->key1(); - const auto key_1_time = graph_localizer.combined_nav_state_graph_values().Timestamp(sym::P, pose_key_1); - ASSERT_TRUE(key_1_time != boost::none); - EXPECT_NEAR(*key_1_time, oldest_time, 1e-6); - const auto pose_key_2 = middle_imu_factor->key3(); - const auto key_2_time = graph_localizer.combined_nav_state_graph_values().Timestamp(sym::P, pose_key_2); - ASSERT_TRUE(key_2_time != boost::none); - EXPECT_NEAR(*key_2_time, middle_time, 1e-6); - const auto middle_combined_nav_state = graph_localizer.GetCombinedNavState(middle_time); - ASSERT_TRUE(middle_combined_nav_state != boost::none); - EXPECT_NEAR(middle_combined_nav_state->timestamp(), middle_time, 1e-6); - EXPECT_MATRIX_NEAR(middle_combined_nav_state->pose(), middle_pose, 1e-5); - } - // Check latest imu factor - { - const auto& latest_imu_factor = imu_factors[1]; - const auto pose_key_1 = latest_imu_factor->key1(); - const auto key_1_time = graph_localizer.combined_nav_state_graph_values().Timestamp(sym::P, pose_key_1); - ASSERT_TRUE(key_1_time != boost::none); - EXPECT_NEAR(*key_1_time, middle_time, 1e-6); - const auto pose_key_2 = latest_imu_factor->key3(); - const auto key_2_time = graph_localizer.combined_nav_state_graph_values().Timestamp(sym::P, pose_key_2); - ASSERT_TRUE(key_2_time != boost::none); - EXPECT_NEAR(*key_2_time, latest_time, 1e-6); - const auto latest_combined_nav_state = graph_localizer.GetCombinedNavState(latest_time); - ASSERT_TRUE(latest_combined_nav_state != boost::none); - EXPECT_NEAR(latest_combined_nav_state->timestamp(), latest_time, 1e-6); - EXPECT_MATRIX_NEAR(latest_combined_nav_state->pose(), latest_pose, 1e-5); - } - } -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/localization/graph_localizer/test/test_depth_odometry_factor_adder.cc b/localization/graph_localizer/test/test_depth_odometry_factor_adder.cc deleted file mode 100644 index 1d29cde52c..0000000000 --- a/localization/graph_localizer/test/test_depth_odometry_factor_adder.cc +++ /dev/null @@ -1,238 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/depth_odometry_factor_adder.h> -#include <graph_localizer/graph_localizer.h> -#include <graph_localizer/point_to_point_between_factor.h> -#include <graph_localizer/test_utilities.h> -#include <localization_common/logger.h> -#include <localization_common/test_utilities.h> -#include <localization_common/utilities.h> -#include <localization_measurements/depth_odometry_measurement.h> -#include <localization_measurements/imu_measurement.h> - -#include <gtsam/base/numericalDerivative.h> -#include <gtsam/inference/Symbol.h> -#include <gtsam/linear/NoiseModel.h> -#include <gtsam/slam/BetweenFactor.h> - -#include <gtest/gtest.h> - -namespace gl = graph_localizer; -namespace go = graph_optimizer; -namespace lc = localization_common; -namespace lm = localization_measurements; -namespace sym = gtsam::symbol_shorthand; - -void AddInlierAndOutlierPoints(const int num_inliers, const int num_outliers, - const double point_to_point_error_threshold, lm::DepthOdometryMeasurement& measurement) { - for (int i = 0; i < num_inliers; ++i) { - const Eigen::Vector3d source_point = lc::RandomPoint3d(); - const Eigen::Vector3d target_point = measurement.odometry.sensor_F_source_T_target.pose.inverse() * source_point; - measurement.correspondences.source_3d_points.emplace_back(source_point); - measurement.correspondences.target_3d_points.emplace_back(target_point); - } - for (int i = 0; i < num_outliers; ++i) { - const Eigen::Vector3d source_point = lc::RandomPoint3d(); - const Eigen::Vector3d noise(1.1 * point_to_point_error_threshold, 0, 0); - const Eigen::Vector3d target_point = - measurement.odometry.sensor_F_source_T_target.pose.inverse() * (source_point + noise); - measurement.correspondences.source_3d_points.emplace_back(source_point); - measurement.correspondences.target_3d_points.emplace_back(target_point); - } -} - -TEST(DepthOdometryFactorAdderTester, ValidPose) { - const auto params = gl::DefaultDepthOdometryFactorAdderParams(); - gl::DepthOdometryFactorAdder factor_adder(params); - const lc::Time source_timestamp = 0; - const lc::Time target_timestamp = 0.1; - const Eigen::Isometry3d relative_pose = Eigen::Isometry3d::Identity(); - const auto measurement = gl::DepthOdometryMeasurementFromPose(relative_pose, source_timestamp, target_timestamp); - const auto factors_to_add_vec = factor_adder.AddFactors(measurement); - ASSERT_EQ(factors_to_add_vec.size(), 1); - EXPECT_EQ(factors_to_add_vec[0].timestamp(), target_timestamp); - ASSERT_EQ(factors_to_add_vec[0].Get().size(), 1); - const auto& factor_to_add = factors_to_add_vec[0].Get()[0]; - // Check Key Info - { - const auto& key_infos = factor_to_add.key_infos; - ASSERT_EQ(key_infos.size(), 2); - // Key Info Source - { - const auto& key_info = key_infos[0]; - EXPECT_FALSE(key_info.is_static()); - EXPECT_NEAR(key_info.timestamp(), source_timestamp, 1e-6); - EXPECT_EQ(key_info.UninitializedKey(), sym::P(0)); - EXPECT_EQ(key_info.node_updater_type(), go::NodeUpdaterType::CombinedNavState); - } - // Key Info Target - { - const auto& key_info = key_infos[1]; - EXPECT_FALSE(key_info.is_static()); - EXPECT_NEAR(key_info.timestamp(), target_timestamp, 1e-6); - EXPECT_EQ(key_info.UninitializedKey(), sym::P(0)); - EXPECT_EQ(key_info.node_updater_type(), go::NodeUpdaterType::CombinedNavState); - } - } - // Check Factors - { - const auto factor = dynamic_cast<const gtsam::BetweenFactor<gtsam::Pose3>*>(factor_to_add.factor.get()); - ASSERT_TRUE(factor); - const auto& pose = factor->measured(); - EXPECT_MATRIX_NEAR(pose, relative_pose, 1e-6); - const auto& keys = factor->keys(); - const auto& key_info = factor_to_add.key_infos[0]; - EXPECT_EQ(keys[0], key_info.UninitializedKey()); - EXPECT_EQ(keys[1], key_info.UninitializedKey()); - } -} - -TEST(DepthOdometryFactorAdderTester, InvalidPose) { - const auto params = gl::DefaultDepthOdometryFactorAdderParams(); - gl::DepthOdometryFactorAdder factor_adder(params); - const lc::Time source_timestamp = 0; - const lc::Time target_timestamp = 0.1; - Eigen::Isometry3d relative_pose = Eigen::Isometry3d::Identity(); - // Ensure translation is above threshold - relative_pose.translation() += Eigen::Vector3d(1.1, 0, 0) * params.pose_translation_norm_threshold; - const auto measurement = gl::DepthOdometryMeasurementFromPose(relative_pose, source_timestamp, target_timestamp); - const auto factors_to_add_vec = factor_adder.AddFactors(measurement); - ASSERT_EQ(factors_to_add_vec.size(), 0); -} - -TEST(DepthOdometryFactorAdderTester, Points) { - auto params = gl::DefaultDepthOdometryFactorAdderParams(); - for (int i = 0; i < 50; ++i) { - params.use_points_between_factor = true; - gl::DepthOdometryFactorAdder factor_adder(params); - const lc::Time source_timestamp = 0; - const lc::Time target_timestamp = 0.1; - const Eigen::Isometry3d relative_pose = lc::RandomIsometry3d(); - auto measurement = gl::DepthOdometryMeasurementFromPose(relative_pose, source_timestamp, target_timestamp); - const int num_inliers = 2; - const int num_outliers = 2; - AddInlierAndOutlierPoints(num_inliers, num_outliers, params.point_to_point_error_threshold, measurement); - ASSERT_EQ(measurement.correspondences.source_3d_points.size(), num_inliers + num_outliers); - const auto factors_to_add_vec = factor_adder.AddFactors(measurement); - ASSERT_EQ(factors_to_add_vec.size(), 1); - EXPECT_EQ(factors_to_add_vec[0].timestamp(), target_timestamp); - ASSERT_EQ(factors_to_add_vec[0].Get().size(), num_inliers); - // Factor 0 - { - const auto& factor_to_add = factors_to_add_vec[0].Get()[0]; - // Check Key Info - { - const auto& key_infos = factor_to_add.key_infos; - ASSERT_EQ(key_infos.size(), 2); - // Key Info Source - { - const auto& key_info = key_infos[0]; - EXPECT_FALSE(key_info.is_static()); - EXPECT_NEAR(key_info.timestamp(), source_timestamp, 1e-6); - EXPECT_EQ(key_info.UninitializedKey(), sym::P(0)); - EXPECT_EQ(key_info.node_updater_type(), go::NodeUpdaterType::CombinedNavState); - } - // Key Info Target - { - const auto& key_info = key_infos[1]; - EXPECT_FALSE(key_info.is_static()); - EXPECT_NEAR(key_info.timestamp(), target_timestamp, 1e-6); - EXPECT_EQ(key_info.UninitializedKey(), sym::P(0)); - EXPECT_EQ(key_info.node_updater_type(), go::NodeUpdaterType::CombinedNavState); - } - } - // Check Factor - { - const auto factor = dynamic_cast<const gtsam::PointToPointBetweenFactor*>(factor_to_add.factor.get()); - ASSERT_TRUE(factor); - const auto& source_point = factor->sensor_t_point_source(); - EXPECT_MATRIX_NEAR(source_point, measurement.correspondences.source_3d_points[0], 1e-6); - const auto& target_point = factor->sensor_t_point_target(); - EXPECT_MATRIX_NEAR(target_point, measurement.correspondences.target_3d_points[0], 1e-6); - const auto& body_T_sensor = factor->body_T_sensor(); - EXPECT_MATRIX_NEAR(body_T_sensor, params.body_T_sensor, 1e-6); - const auto& keys = factor->keys(); - const auto& key_info = factor_to_add.key_infos[0]; - EXPECT_EQ(keys[0], key_info.UninitializedKey()); - EXPECT_EQ(keys[1], key_info.UninitializedKey()); - } - } - // Factor 1 - { - const auto& factor_to_add = factors_to_add_vec[0].Get()[1]; - const auto factor = dynamic_cast<const gtsam::PointToPointBetweenFactor*>(factor_to_add.factor.get()); - ASSERT_TRUE(factor); - const auto& source_point = factor->sensor_t_point_source(); - EXPECT_MATRIX_NEAR(source_point, measurement.correspondences.source_3d_points[1], 1e-6); - const auto& target_point = factor->sensor_t_point_target(); - EXPECT_MATRIX_NEAR(target_point, measurement.correspondences.target_3d_points[1], 1e-6); - const auto& body_T_sensor = factor->body_T_sensor(); - EXPECT_MATRIX_NEAR(body_T_sensor, params.body_T_sensor, 1e-6); - const auto& keys = factor->keys(); - const auto& key_info = factor_to_add.key_infos[0]; - EXPECT_EQ(keys[0], key_info.UninitializedKey()); - EXPECT_EQ(keys[1], key_info.UninitializedKey()); - } - } -} - -TEST(DepthOdometryFactorAdderTester, ConstantVelocityPoints) { - auto params = gl::DefaultGraphLocalizerParams(); - params.factor.depth_odometry_adder = gl::DefaultDepthOdometryFactorAdderParams(); - params.factor.depth_odometry_adder.use_points_between_factor = true; - params.factor.depth_odometry_adder.body_T_sensor = lc::RandomPose(); - constexpr double kInitialVelocity = 0.1; - params.graph_initializer.global_V_body_start = Eigen::Vector3d(kInitialVelocity, 0, 0); - gl::GraphLocalizer graph_localizer(params); - constexpr int kNumIterations = 100; - constexpr double kTimeDiff = 0.1; - lc::Time time = 0.0; - const Eigen::Vector3d relative_translation = kTimeDiff * params.graph_initializer.global_V_body_start; - Eigen::Isometry3d current_pose = lc::EigenPose(params.graph_initializer.global_T_body_start); - // Add initial zero acceleration value so the imu integrator has more than one measurement when the subsequent - // measurement is added - const lm::ImuMeasurement initial_zero_imu_measurement(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), time); - graph_localizer.AddImuMeasurement(initial_zero_imu_measurement); - for (int i = 0; i < kNumIterations; ++i) { - time += kTimeDiff; - const lm::ImuMeasurement zero_imu_measurement(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), time); - graph_localizer.AddImuMeasurement(zero_imu_measurement); - const Eigen::Isometry3d relative_pose = lc::Isometry3d(relative_translation, Eigen::Matrix3d::Identity()); - current_pose = current_pose * relative_pose; - const lc::Time source_time = time - kTimeDiff; - const lc::Time target_time = time; - lm::DepthOdometryMeasurement constant_velocity_measurement = gl::DepthOdometryMeasurementFromPose( - lc::FrameChangeRelativePose(relative_pose, lc::EigenPose(params.factor.depth_odometry_adder.body_T_sensor)), - source_time, target_time); - const int num_inliers = 20; - AddInlierAndOutlierPoints(num_inliers, 0, 0, constant_velocity_measurement); - graph_localizer.AddDepthOdometryMeasurement(constant_velocity_measurement); - graph_localizer.Update(); - const auto latest_combined_nav_state = graph_localizer.LatestCombinedNavState(); - ASSERT_TRUE(latest_combined_nav_state != boost::none); - EXPECT_NEAR(latest_combined_nav_state->timestamp(), time, 1e-6); - EXPECT_MATRIX_NEAR(latest_combined_nav_state->pose(), current_pose, 1e-5); - } -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/localization/graph_localizer/test/test_rotation_factor_adder.cc b/localization/graph_localizer/test/test_rotation_factor_adder.cc deleted file mode 100644 index 236615cd8b..0000000000 --- a/localization/graph_localizer/test/test_rotation_factor_adder.cc +++ /dev/null @@ -1,159 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_localizer/pose_rotation_factor.h> -#include <graph_localizer/rotation_factor_adder.h> -#include <localization_common/logger.h> -#include <localization_measurements/feature_point.h> - -#include <gtsam/geometry/PinholePose.h> -#include <gtsam/inference/Symbol.h> -#include <gtsam/linear/NoiseModel.h> -#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> -#include <gtsam/nonlinear/NonlinearFactorGraph.h> -#include <gtsam/nonlinear/PriorFactor.h> -#include <gtsam/nonlinear/Values.h> -#include <gtsam/slam/PoseTranslationPrior.h> - -#include <boost/make_shared.hpp> - -#include <gtest/gtest.h> - -namespace gl = graph_localizer; -namespace lm = localization_measurements; -namespace sym = gtsam::symbol_shorthand; -class RotationFactorAdderTester : public ::testing::Test { - protected: - virtual void SetUp() { - gl::FeatureTrackerParams feature_tracker_params; - feature_tracker_params.sliding_window_duration = 2.0; - feature_tracker_.reset(new gl::FeatureTracker(feature_tracker_params)); - rotation_factor_adder_params_.enabled = true; - rotation_factor_adder_params_.huber_k = 1.345; - rotation_factor_adder_params_.min_avg_disparity = 0.2; - rotation_factor_adder_params_.rotation_stddev = 0.1; - rotation_factor_adder_params_.max_percent_outliers = 0.5; - Eigen::Matrix<double, 4, 4> body_T_nav_cam; - body_T_nav_cam << 0, 0, 1, 0.1177, 1, 0, 0, -0.0422, 0, 1, 0, -0.0826, 0, 0, 0, 1; - rotation_factor_adder_params_.body_T_nav_cam = gtsam::Pose3(body_T_nav_cam.matrix()); - rotation_factor_adder_params_.nav_cam_intrinsics = gtsam::Cal3_S2(608.807, 607.614, 0, 0, 0); - rotation_factor_adder_.reset(new gl::RotationFactorAdder(rotation_factor_adder_params_, feature_tracker_)); - } - - void AddMeasurementsForTwoFrames(const gtsam::Pose3& world_T_cam_1, const gtsam::Pose3& world_T_cam_2, - const int num_points, const localization_common::Time time_diff = 1.0) { - const gtsam::PinholePose<gtsam::Cal3_S2> cam_1( - world_T_cam_1, boost::make_shared<gtsam::Cal3_S2>(rotation_factor_adder_params_.nav_cam_intrinsics)); - const gtsam::PinholePose<gtsam::Cal3_S2> cam_2( - world_T_cam_2, boost::make_shared<gtsam::Cal3_S2>(rotation_factor_adder_params_.nav_cam_intrinsics)); - const double x_min = -1.0; - const double x_max = 1.0; - const double y_min = -1.0; - const double y_max = 1.0; - const double z_min = 1.0; - const double z_max = 10.0; - std::random_device rd; - std::mt19937 gen(rd()); - std::uniform_real_distribution<> x_dis(x_min, x_max); - std::uniform_real_distribution<> y_dis(y_min, y_max); - std::uniform_real_distribution<> z_dis(z_min, z_max); - lm::FeaturePoints feature_points_1; - lm::FeaturePoints feature_points_2; - for (int i = 0; i < num_points; ++i) { - const double x = x_dis(gen); - const double y = y_dis(gen); - const double z = z_dis(gen); - const gtsam::Point3 cam_1_t_point(x, y, z); - const gtsam::Point3 world_t_point = world_T_cam_1 * cam_1_t_point; - const auto result_1 = cam_1.projectSafe(world_t_point); - const auto result_2 = cam_2.projectSafe(world_t_point); - if (!result_1.second || !result_2.second) { - continue; - } - feature_points_1.emplace_back(result_1.first.x(), result_1.first.y(), 1, i, 0); - feature_points_2.emplace_back(result_2.first.x(), result_2.first.y(), 2, i, 0.1); - } - feature_tracker_->UpdateFeatureTracks(feature_points_1); - feature_tracker_->UpdateFeatureTracks(feature_points_2); - ASSERT_EQ(feature_points_1.size(), feature_points_2.size()); - ASSERT_GT(feature_points_1.size(), 5); - } - - gl::RotationFactorAdderParams rotation_factor_adder_params_; - std::shared_ptr<gl::FeatureTracker> feature_tracker_; - std::unique_ptr<gl::RotationFactorAdder> rotation_factor_adder_; -}; - -TEST_F(RotationFactorAdderTester, CorrectRotation) { - const gtsam::Pose3 world_T_cam_1; - const gtsam::Point3 cam_1_t_cam_2(0.1, 0.1, 0.1); - const gtsam::Rot3 cam_1_R_cam_2(gtsam::Rot3::RzRyRx(3.0 * M_PI / 180.0, 2.0 * M_PI / 180.0, 1.0 * M_PI / 180.0)); - const gtsam::Pose3 cam_1_T_cam_2(cam_1_R_cam_2, cam_1_t_cam_2); - const gtsam::Pose3 world_T_cam_2 = world_T_cam_1 * cam_1_T_cam_2; - AddMeasurementsForTwoFrames(world_T_cam_1, world_T_cam_2, 30); - const auto factors_to_add_vec = rotation_factor_adder_->AddFactors(lm::FeaturePointsMeasurement()); - const auto rotation_factor = - dynamic_cast<gtsam::PoseRotationFactor*>(factors_to_add_vec.front().Get().front().factor.get()); - ASSERT_TRUE(rotation_factor); - const auto rotation = rotation_factor->rotation(); - const auto world_T_body_1 = world_T_cam_1 * rotation_factor_adder_params_.body_T_nav_cam.inverse(); - const auto world_T_body_2 = world_T_cam_2 * rotation_factor_adder_params_.body_T_nav_cam.inverse(); - const auto body_1_R_body_2 = (world_T_body_1.inverse() * world_T_body_2).rotation(); - ASSERT_TRUE(rotation.equals(body_1_R_body_2, 1e-6)); -} - -TEST_F(RotationFactorAdderTester, CorrectOptimization) { - const gtsam::Pose3 world_T_cam_1; - const gtsam::Point3 cam_1_t_cam_2(0.1, 0.1, 0.1); - const gtsam::Rot3 cam_1_R_cam_2(gtsam::Rot3::RzRyRx(3.0 * M_PI / 180.0, 2.0 * M_PI / 180.0, 1.0 * M_PI / 180.0)); - const gtsam::Pose3 cam_1_T_cam_2(cam_1_R_cam_2, cam_1_t_cam_2); - const gtsam::Pose3 world_T_cam_2 = world_T_cam_1 * cam_1_T_cam_2; - AddMeasurementsForTwoFrames(world_T_cam_1, world_T_cam_2, 30); - const auto factors_to_add_vec = rotation_factor_adder_->AddFactors(lm::FeaturePointsMeasurement()); - auto rotation_factor = - dynamic_cast<gtsam::PoseRotationFactor*>(factors_to_add_vec.front().Get().front().factor.get()); - ASSERT_TRUE(rotation_factor); - const gtsam::Pose3 world_T_body_1 = world_T_cam_1 * rotation_factor_adder_params_.body_T_nav_cam.inverse(); - const gtsam::Pose3 world_T_body_2 = world_T_cam_2 * rotation_factor_adder_params_.body_T_nav_cam.inverse(); - gtsam::Values values_; - values_.insert(sym::P(1), world_T_body_1); - // Use cam_1_T_cam_2 as a pertubation and make sure the optimizer corrects this - const gtsam::Pose3 world_T_perturbed_body_2 = world_T_body_2 * cam_1_T_cam_2; - values_.insert(sym::P(2), world_T_perturbed_body_2); - gtsam::NonlinearFactorGraph graph_; - const auto noise_1 = gtsam::noiseModel::Unit::Create(6); - const gtsam::PriorFactor<gtsam::Pose3> world_T_body_1_prior(sym::P(1), world_T_body_1, noise_1); - graph_.push_back(world_T_body_1_prior); - const auto noise_2 = gtsam::noiseModel::Unit::Create(3); - gtsam::PoseTranslationPrior<gtsam::Pose3> pose_translation_prior(sym::P(2), world_T_body_2.translation(), noise_2); - graph_.push_back(pose_translation_prior); - rotation_factor->keys() = gtsam::KeyVector({sym::P(1), sym::P(2)}); - graph_.push_back(*rotation_factor); - gtsam::LevenbergMarquardtOptimizer optimizer(graph_, values_); - const auto optimized_values = optimizer.optimize(); - const auto optimized_world_T_body_1 = optimized_values.at<gtsam::Pose3>(sym::P(1)); - const auto optimized_world_T_body_2 = optimized_values.at<gtsam::Pose3>(sym::P(2)); - ASSERT_TRUE(optimized_world_T_body_1.equals(world_T_body_1, 1e-6)); - ASSERT_TRUE(optimized_world_T_body_2.equals(world_T_body_2, 1e-6)); -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/localization/graph_optimizer/CMakeLists.txt b/localization/graph_optimizer/CMakeLists.txt index 604d1dc2ad..2af998da04 100644 --- a/localization/graph_optimizer/CMakeLists.txt +++ b/localization/graph_optimizer/CMakeLists.txt @@ -23,12 +23,15 @@ add_compile_options(-std=c++14) ## Find catkin macros and libraries find_package(catkin2 REQUIRED COMPONENTS - config_reader + factor_adders localization_common - localization_measurements - msg_conversions + node_adders + optimizers ) +# Find GTSAM +find_package(GTSAM REQUIRED) + # System dependencies are found with CMake's conventions find_package(Eigen3 REQUIRED) @@ -36,7 +39,7 @@ find_package(Eigen3 REQUIRED) catkin_package( LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} - CATKIN_DEPENDS config_reader localization_common localization_measurements msg_conversions + CATKIN_DEPENDS factor_adders localization_common node_adders optimizers ) ########### @@ -54,14 +57,21 @@ include_directories( # Declare C++ libraries add_library(${PROJECT_NAME} src/graph_optimizer.cc - src/graph_values.cc - src/utilities.cc - src/graph_stats.cc - src/parameter_reader.cc ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_graph_optimizer + test/test_graph_optimizer.test + test/test_graph_optimizer.cc + ) + target_link_libraries(test_graph_optimizer + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) +endif() + ############# ## Install ## ############# @@ -78,4 +88,4 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE -) \ No newline at end of file +) diff --git a/localization/graph_optimizer/include/graph_optimizer/factor_to_add.h b/localization/graph_optimizer/include/graph_optimizer/factor_to_add.h deleted file mode 100644 index d522ade8ee..0000000000 --- a/localization/graph_optimizer/include/graph_optimizer/factor_to_add.h +++ /dev/null @@ -1,70 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_OPTIMIZER_FACTOR_TO_ADD_H_ -#define GRAPH_OPTIMIZER_FACTOR_TO_ADD_H_ - -#include <graph_optimizer/graph_action_completer_type.h> -#include <graph_optimizer/key_info.h> -#include <localization_common/time.h> - -#include <gtsam/nonlinear/NonlinearFactor.h> - -#include <vector> - -namespace graph_optimizer { -struct FactorToAdd { - FactorToAdd(const KeyInfos& key_infos, boost::shared_ptr<gtsam::NonlinearFactor> factor) - : factor(factor), key_infos(key_infos) {} - - boost::shared_ptr<gtsam::NonlinearFactor> factor; - KeyInfos key_infos; -}; - -class FactorsToAdd { - public: - FactorsToAdd(const localization_common::Time timestamp, const std::vector<FactorToAdd>& factors_to_add, - const GraphActionCompleterType graph_action_completer_type = GraphActionCompleterType::None) - : timestamp_(timestamp), - factors_to_add_(factors_to_add), - graph_action_completer_type_(graph_action_completer_type) {} - - explicit FactorsToAdd(const GraphActionCompleterType graph_action_completer_type = GraphActionCompleterType::None) - : graph_action_completer_type_(graph_action_completer_type) {} - - void reserve(const int size) { factors_to_add_.reserve(size); } - size_t size() const { return factors_to_add_.size(); } - bool empty() const { return factors_to_add_.empty(); } - void push_back(FactorToAdd&& factor_to_add) { factors_to_add_.emplace_back(std::move(factor_to_add)); } // NOLINT - void push_back(const FactorToAdd& factor_to_add) { factors_to_add_.push_back(factor_to_add); } - void SetTimestamp(const localization_common::Time timestamp) { timestamp_ = timestamp; } - - localization_common::Time timestamp() const { return timestamp_; } - const std::vector<FactorToAdd>& Get() const { return factors_to_add_; } - std::vector<FactorToAdd>& Get() { return factors_to_add_; } - GraphActionCompleterType graph_action_completer_type() const { return graph_action_completer_type_; } - - private: - // Timestamp used to sort factors when adding to graph. - localization_common::Time timestamp_; - std::vector<FactorToAdd> factors_to_add_; - GraphActionCompleterType graph_action_completer_type_; -}; -} // namespace graph_optimizer - -#endif // GRAPH_OPTIMIZER_FACTOR_TO_ADD_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/graph_action_completer.h b/localization/graph_optimizer/include/graph_optimizer/graph_action_completer.h deleted file mode 100644 index 9fe1d65fff..0000000000 --- a/localization/graph_optimizer/include/graph_optimizer/graph_action_completer.h +++ /dev/null @@ -1,36 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_OPTIMIZER_GRAPH_ACTION_COMPLETER_H_ -#define GRAPH_OPTIMIZER_GRAPH_ACTION_COMPLETER_H_ - -#include <graph_optimizer/graph_action_completer_type.h> -#include <graph_optimizer/factor_to_add.h> - -#include <gtsam/nonlinear/NonlinearFactorGraph.h> - -namespace graph_optimizer { -class GraphActionCompleter { - public: - virtual ~GraphActionCompleter() {} - virtual bool DoAction(FactorsToAdd& factors_to_add, gtsam::NonlinearFactorGraph& graph_factors) = 0; - virtual GraphActionCompleterType type() const = 0; -}; -} // namespace graph_optimizer - -#endif // GRAPH_OPTIMIZER_GRAPH_ACTION_COMPLETER_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/graph_optimizer.h b/localization/graph_optimizer/include/graph_optimizer/graph_optimizer.h index 812d49ad86..f29a92c346 100644 --- a/localization/graph_optimizer/include/graph_optimizer/graph_optimizer.h +++ b/localization/graph_optimizer/include/graph_optimizer/graph_optimizer.h @@ -19,157 +19,190 @@ #ifndef GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_H_ #define GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_H_ -#include <graph_optimizer/factor_to_add.h> -#include <graph_optimizer/graph_action_completer.h> +#include <factor_adders/factor_adder.h> #include <graph_optimizer/graph_optimizer_params.h> -#include <graph_optimizer/graph_stats.h> -#include <graph_optimizer/key_info.h> -#include <graph_optimizer/node_updater.h> +#include <optimizers/optimizer.h> +#include <node_adders/node_adder.h> +#include <nodes/values.h> +#include <localization_common/stats_logger.h> #include <localization_common/time.h> -#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> -#include <gtsam/nonlinear/Marginals.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <boost/serialization/serialization.hpp> -#include <map> #include <memory> #include <string> #include <utility> #include <vector> namespace graph_optimizer { +// Time-based factor graph optimizer that uses FactorAdders to generate and add factors for a given time range to the +// graph and NodeAdders to provide corresponding nodes for these factors at required timestamps. Generally FactorAdders +// are measurement-based and generate factors for a certain type of measurement at given timestamps. NodeAdders are also +// typically measurement-based and create linked nodes using relative factors that span the graph. As an example, a +// Visual-Inertial Odometry (VIO) graph may contain an image-based FactorAdder that generates visual-odometry factors +// using image measurements. A VIO graph will solve for Pose/Velocity/IMU Bias (PVB) values at different timestamps, so +// it will also contain a PVB NodeAdder that creates timestamped PVB nodes for the visual-odometry factors and links +// these nodes using IMU measurements. See the FactorAdder and NodeAdder packages for more information and different +// types of FactorAdders and NodeAdders. Maintains a set of values used for optimization. All NodeAdders added to the +// GraphOptimizer should be constructed using the GraphOptimizer's values (accessable with the values() member +// function). Acts as a base class for the SlidingWindowGraphOptimizer. class GraphOptimizer { public: - GraphOptimizer(const GraphOptimizerParams& params, - std::unique_ptr<GraphStats> graph_stats = std::unique_ptr<GraphStats>(new GraphStats())); - // Default constructor/destructor for serialization only + // Construct GraphOptimizer with provided optimizer. + GraphOptimizer(const GraphOptimizerParams& params, std::unique_ptr<optimizers::Optimizer> optimizer); + + // Default constructor for serialization only GraphOptimizer() {} - ~GraphOptimizer(); - - // Registers graph action completer. Called after adding buffered factors and updating nodes - void AddGraphActionCompleter(std::shared_ptr<GraphActionCompleter> graph_action_completer); - // Registers node updater. Called when adding buffered factors - void AddNodeUpdater(std::shared_ptr<NodeUpdater> node_updater); - // Adds factors to buffered list which is sorted by time - void BufferFactors(const std::vector<FactorsToAdd>& factors_to_add_vec); - // Adds buffered factors and optimizes graph. Calls DoPostOptimizeActions afterwards - bool Update(); - // Checks whether a measurement is too old to be inserted into graph - virtual bool MeasurementRecentEnough(const localization_common::Time timestamp) const; - const gtsam::NonlinearFactorGraph& graph_factors() const; - gtsam::NonlinearFactorGraph& graph_factors(); - template <typename FactorType> - const std::vector<boost::shared_ptr<const FactorType>> Factors() const; - const int num_factors() const; - void SaveGraphDotFile(const std::string& output_path = "graph.dot") const; - const GraphOptimizerParams& params() const; - void LogOnDestruction(const bool log_on_destruction); - const GraphStats* const graph_stats() const; - GraphStats* graph_stats(); - const boost::optional<gtsam::Marginals>& marginals() const; - std::shared_ptr<gtsam::Values> shared_values(); - const gtsam::Values& values() const; - private: - gtsam::NonlinearFactorGraph MarginalFactors(const gtsam::NonlinearFactorGraph& old_factors, - const gtsam::KeyVector& old_keys, - const gtsam::GaussianFactorGraph::Eliminate& eliminate_function) const; + virtual ~GraphOptimizer() = default; - // Removes Keys and Values outside of sliding window. - // Removes any factors depending on removed values - // Optionally adds marginalized factors encapsulating linearized error of removed factors - // Optionally adds priors using marginalized covariances for new oldest states - bool SlideWindow(const boost::optional<gtsam::Marginals>& marginals, - const localization_common::Time last_latest_time); + // Adds node adder used for graph optimization. + // Initializes the nodes and priors for the node adder. + void AddNodeAdder(std::shared_ptr<node_adders::NodeAdder> node_adder); - boost::optional<localization_common::Time> SlideWindowNewOldestTime() const; + // Adds factor adder used for graph optimization. + void AddFactorAdder(std::shared_ptr<factor_adders::FactorAdder> factor_adder); - gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time) const; + // Adds factors for each factor in valid time range to the graph. + // Since factor adders are linked to the node adder they use, this also adds required + // nodes for these factors. + // Return the number of factors added by the factor adders, not including the relative factors added by the node + // adders. + // TODO(rsoussan): Return both? + int AddFactors(const localization_common::Time start_time, const localization_common::Time end_time); - std::pair<gtsam::KeyVector, gtsam::NonlinearFactorGraph> OldKeysAndFactors( - const localization_common::Time oldest_allowed_time); + // Wrapper for AddFactors passing a pair of timestamps. + int AddFactors(const std::pair<localization_common::Time, localization_common::Time>& start_and_end_time); - // Called after SlideWindow - virtual void DoPostSlideWindowActions(const localization_common::Time oldest_allowed_time, - const boost::optional<gtsam::Marginals>& marginals); + // Performs optimization on the factor graph. + bool Optimize(); - // void UpdatePointPriors(const gtsam::Marginals& marginals); + // Calculates the covariance matrix for the provided node's key. + // Requires a successful round of optimization to have been performed. + boost::optional<gtsam::Matrix> Covariance(const gtsam::Key& key) const; - // Creates factors for cumulative factor adders and adds them to buffered list - virtual void BufferCumulativeFactors(); + // Calculates the covariance matrix wrt two nodes for the provided keys. + // Requires a successful round of optimization to have been performed. + boost::optional<gtsam::Matrix> Covariance(const gtsam::Key& key_a, const gtsam::Key& key_b) const; - // Removes old measurements from cumulative factors that do not fit in sliding window so cumulative factors are still - // valid after the SlideWindow call - virtual void RemoveOldMeasurementsFromCumulativeFactors(const gtsam::KeyVector& old_keys); + // Returns set of factors currently in the graph. + const gtsam::NonlinearFactorGraph& factors() const; - // Optional validity check for graph before optimizing - virtual bool ValidGraph() const; + // Returns set of factors currently in the graph. + gtsam::NonlinearFactorGraph& factors(); - // Adds buffered factors to graph_factors for future optimization - // Only adds factors that pass ReadyToAddFactors(timestamp) check - int AddBufferedFactors(); + // Returns set of factors of FactorType currently in the graph. + template <typename FactorType> + std::vector<boost::shared_ptr<const FactorType>> Factors() const; + + // Returns number of factors currently in the graph. + int num_factors() const; + + // Returns number of factors of provided FactorType currently in the graph. + template <typename FactorType> + int NumFactors() const; - // Calls Update for each registered NodeUpdater to create required nodes while inserting new factors - bool UpdateNodes(const KeyInfo& key_info); + // Returns number of values currently in the graph. + int num_values() const; - // Calls DoAction for each registered GraphActionCompleter after inserting new factors - bool DoGraphAction(FactorsToAdd& factors_to_add); + // Graph optimizer params. + const GraphOptimizerParams& params() const; - boost::optional<gtsam::Key> GetKey(KeyCreatorFunction key_creator_function, - const localization_common::Time timestamp) const; + // Returns a shared pointer to the values used by the graph optimizer. + // All node adders added to the graph optimizer should be constructed + // with these values. + std::shared_ptr<nodes::Values> values(); - // Updates keys in factors after new nodes are created/updated as required by registered NodeUpdater - bool Rekey(FactorToAdd& factor_to_add); + // Returns a const reference to the gtsam Values used within the Values object. + const gtsam::Values& gtsam_values() const; - // Checks whether the graph is ready for insertion of a factor with timestamp - virtual bool ReadyToAddFactors(const localization_common::Time timestamp) const; + // Returns a reference to the stats logger + localization_common::StatsLogger& stats_logger(); - boost::optional<localization_common::Time> OldestTimestamp() const; + // Returns a const reference to the optimization timer + const localization_common::Timer& optimization_timer() const; - boost::optional<localization_common::Time> LatestTimestamp() const; + // Returns a const reference to the optimization_iterations averager + const localization_common::Averager& optimization_iterations_averager() const; - // Removes buffered factors that are too old for insertion into graph - void RemoveOldBufferedFactors(const localization_common::Time oldest_allowed_timestamp); + // Sum of factor errors for each factor in the graph + double TotalGraphError() const; - virtual void PrintFactorDebugInfo() const; + // Optional validity check for graph before optimizing. + // If this fails, no optimization is performed. + // Default behavior always returns true. + virtual bool ValidGraph() const; - // Called after optimizing graph - virtual bool DoPostOptimizeActions(); + // Prints factor graph information and logs stats. + virtual void Print() const; + + // Saves the graph to a dot file, can be used to generate visual representation + // of the graph. + void SaveGraphDotFile(const std::string& output_path = "graph.dot") const; + + // Returns marginals if they have been calculated. + boost::optional<const gtsam::Marginals&> marginals() const; + + private: + // Add averagers and timers for logging. + void AddAveragersAndTimers(); + + // Returns a reference to the gtsam Values used within the Values object. + gtsam::Values& gtsam_values(); // Serialization function friend class boost::serialization::access; template <class Archive> void serialize(Archive& ar, const unsigned int file_version) { - ar& BOOST_SERIALIZATION_NVP(graph_); + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(optimizer_); + ar& BOOST_SERIALIZATION_NVP(factors_); ar& BOOST_SERIALIZATION_NVP(values_); + ar& BOOST_SERIALIZATION_NVP(factor_adders_); + ar& BOOST_SERIALIZATION_NVP(node_adders_); + ar& BOOST_SERIALIZATION_NVP(stats_logger_); + ar& BOOST_SERIALIZATION_NVP(optimization_timer_); + ar& BOOST_SERIALIZATION_NVP(optimization_timer_); + ar& BOOST_SERIALIZATION_NVP(optimization_iterations_averager_); + ar& BOOST_SERIALIZATION_NVP(total_error_averager_); } - std::unique_ptr<GraphStats> graph_stats_; - std::shared_ptr<gtsam::Values> values_; - bool log_on_destruction_; GraphOptimizerParams params_; - gtsam::LevenbergMarquardtParams levenberg_marquardt_params_; - gtsam::NonlinearFactorGraph graph_; - boost::optional<gtsam::Marginals> marginals_; - std::multimap<localization_common::Time, FactorsToAdd> buffered_factors_to_add_; - - std::vector<std::shared_ptr<NodeUpdater>> node_updaters_; - std::vector<std::shared_ptr<GraphActionCompleter>> graph_action_completers_; - gtsam::Marginals::Factorization marginals_factorization_; - boost::optional<localization_common::Time> last_latest_time_; + std::unique_ptr<optimizers::Optimizer> optimizer_; + gtsam::NonlinearFactorGraph factors_; + std::shared_ptr<nodes::Values> values_; + std::vector<std::shared_ptr<factor_adders::FactorAdder>> factor_adders_; + std::vector<std::shared_ptr<node_adders::NodeAdder>> node_adders_; + localization_common::StatsLogger stats_logger_; + + // Logging + localization_common::Timer optimization_timer_ = localization_common::Timer("Optimization"); + localization_common::Averager optimization_iterations_averager_ = + localization_common::Averager("Optimization Iterations"); + localization_common::Averager total_error_averager_ = localization_common::Averager("Total Factor Error"); }; // Implementation template <typename FactorType> -const std::vector<boost::shared_ptr<const FactorType>> GraphOptimizer::Factors() const { - typename std::vector<boost::shared_ptr<const FactorType>> factors; - for (const auto& factor : graph_factors()) { +std::vector<boost::shared_ptr<const FactorType>> GraphOptimizer::Factors() const { + std::vector<boost::shared_ptr<const FactorType>> factors_vector; + for (const auto& factor : factors()) { + const auto casted_factor = boost::dynamic_pointer_cast<const FactorType>(factor); + if (casted_factor) factors_vector.emplace_back(casted_factor); + } + return factors_vector; +} + +template <typename FactorType> +int GraphOptimizer::NumFactors() const { + int num_factors = 0; + for (const auto& factor : factors()) { const auto casted_factor = boost::dynamic_pointer_cast<const FactorType>(factor); - if (casted_factor) factors.emplace_back(casted_factor); + if (casted_factor) ++num_factors; } - return factors; + return num_factors; } } // namespace graph_optimizer diff --git a/localization/graph_optimizer/include/graph_optimizer/graph_optimizer_params.h b/localization/graph_optimizer/include/graph_optimizer/graph_optimizer_params.h index b6ce961a38..61bca72125 100644 --- a/localization/graph_optimizer/include/graph_optimizer/graph_optimizer_params.h +++ b/localization/graph_optimizer/include/graph_optimizer/graph_optimizer_params.h @@ -18,20 +18,14 @@ #ifndef GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_PARAMS_H_ #define GRAPH_OPTIMIZER_GRAPH_OPTIMIZER_PARAMS_H_ -#include <string> - namespace graph_optimizer { struct GraphOptimizerParams { - bool verbose; - bool fatal_failures; - bool log_on_destruction; - bool print_factor_info; - bool use_ceres_params; - int max_iterations; - std::string marginals_factorization; - bool add_marginal_factors; + // Huber k used for all factors. double huber_k; - int log_rate; + // Log statistics after destruction. + bool log_stats_on_destruction; + // Log factor graph info and stats after optimization. + bool print_after_optimization; }; } // namespace graph_optimizer diff --git a/localization/graph_optimizer/include/graph_optimizer/graph_values.h b/localization/graph_optimizer/include/graph_optimizer/graph_values.h deleted file mode 100644 index d65895fb23..0000000000 --- a/localization/graph_optimizer/include/graph_optimizer/graph_values.h +++ /dev/null @@ -1,83 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_OPTIMIZER_GRAPH_VALUES_H_ -#define GRAPH_OPTIMIZER_GRAPH_VALUES_H_ - -#include <graph_optimizer/key_info.h> -#include <localization_common/logger.h> -#include <localization_common/time.h> -#include <localization_measurements/feature_point.h> - -#include <gtsam/inference/Symbol.h> -#include <gtsam/nonlinear/NonlinearFactorGraph.h> -#include <gtsam/nonlinear/Values.h> - -#include <boost/optional.hpp> -#include <boost/serialization/unordered_map.hpp> - -#include <map> -#include <unordered_map> -#include <utility> - -namespace graph_optimizer { -namespace sym = gtsam::symbol_shorthand; -class GraphValues { - public: - GraphValues(std::shared_ptr<gtsam::Values> values = std::shared_ptr<gtsam::Values>(new gtsam::Values())); - - // Returns the oldest time that will be in graph values once the window is slid using params - virtual boost::optional<localization_common::Time> SlideWindowNewOldestTime() const = 0; - - virtual gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time, - const gtsam::NonlinearFactorGraph& graph) const = 0; - - virtual boost::optional<gtsam::Key> GetKey(KeyCreatorFunction key_creator_function, - const localization_common::Time timestamp) const = 0; - - virtual boost::optional<localization_common::Time> OldestTimestamp() const = 0; - - virtual boost::optional<localization_common::Time> LatestTimestamp() const = 0; - - template <typename ValueType> - boost::optional<ValueType> at(const gtsam::Key& key) const { - if (!values_->exists(key)) { - LogError("at: Key not present in values."); - return boost::none; - } - - return values_->at<ValueType>(key); - } - - const gtsam::Values& values() const; - - gtsam::Values& values(); - - private: - // Serialization function - friend class boost::serialization::access; - template <class ARCHIVE> - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_NVP(values_); - } - - std::shared_ptr<gtsam::Values> values_; -}; -} // namespace graph_optimizer - -#endif // GRAPH_OPTIMIZER_GRAPH_VALUES_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/key_info.h b/localization/graph_optimizer/include/graph_optimizer/key_info.h deleted file mode 100644 index e7e967da82..0000000000 --- a/localization/graph_optimizer/include/graph_optimizer/key_info.h +++ /dev/null @@ -1,71 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_OPTIMIZER_KEY_INFO_H_ -#define GRAPH_OPTIMIZER_KEY_INFO_H_ - -#include <graph_optimizer/node_updater_type.h> -#include <localization_common/time.h> - -#include <gtsam/inference/Key.h> - -#include <functional> -#include <vector> - -namespace graph_optimizer { - -using KeyCreatorFunction = std::function<gtsam::Key(std::uint64_t)>; -class KeyInfo { - public: - KeyInfo(KeyCreatorFunction key_creator_function, const NodeUpdaterType node_updater_type, - const localization_common::Time timestamp) - : key_creator_function_(key_creator_function), - node_updater_type_(node_updater_type), - timestamp_(timestamp), - static_(false) {} - // Use for static keys - // TODO(rsoussan): Clean up this interface, use inheritance instead? - KeyInfo(KeyCreatorFunction key_creator_function, const NodeUpdaterType node_updater_type, const int id) - : key_creator_function_(key_creator_function), - node_updater_type_(node_updater_type), - timestamp_(0), - id_(id), - static_(true) {} - gtsam::Key UninitializedKey() const { return key_creator_function_(0); } - gtsam::Key MakeKey(const std::uint64_t key_index) const { return key_creator_function_(key_index); } - static gtsam::Key UninitializedKey(KeyCreatorFunction key_creator_function) { return key_creator_function(0); } - KeyCreatorFunction key_creator_function() const { return key_creator_function_; } - localization_common::Time timestamp() const { return timestamp_; } - bool is_static() const { return static_; } - // TODO(rsoussan): This is only available for static keys, clean this up - int id() const { return id_; } - NodeUpdaterType node_updater_type() const { return node_updater_type_; } - - private: - KeyCreatorFunction key_creator_function_; - NodeUpdaterType node_updater_type_; - localization_common::Time timestamp_; - int id_; - // Static (not changing with time) key - bool static_; -}; - -using KeyInfos = std::vector<KeyInfo>; -} // namespace graph_optimizer - -#endif // GRAPH_OPTIMIZER_KEY_INFO_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/node_updater.h b/localization/graph_optimizer/include/graph_optimizer/node_updater.h deleted file mode 100644 index 4932cd8853..0000000000 --- a/localization/graph_optimizer/include/graph_optimizer/node_updater.h +++ /dev/null @@ -1,58 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_OPTIMIZER_NODE_UPDATER_H_ -#define GRAPH_OPTIMIZER_NODE_UPDATER_H_ - -#include <graph_optimizer/key_info.h> -#include <graph_optimizer/node_updater_type.h> -#include <localization_common/time.h> - -#include <gtsam/nonlinear/Marginals.h> -#include <gtsam/nonlinear/NonlinearFactorGraph.h> - -namespace graph_optimizer { -class NodeUpdater { - public: - virtual ~NodeUpdater() {} - - virtual bool Update(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) = 0; - - virtual bool SlideWindow(const localization_common::Time oldest_allowed_timestamp, - const boost::optional<gtsam::Marginals>& marginals, const gtsam::KeyVector& old_keys, - const double huber_k, gtsam::NonlinearFactorGraph& factors) = 0; - - // Returns the oldest time that will be in graph values once the window is slid using params - virtual boost::optional<localization_common::Time> SlideWindowNewOldestTime() const = 0; - - // TODO(rsoussan): consolidate these with graph values base? - virtual gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time, - const gtsam::NonlinearFactorGraph& graph) const = 0; - - virtual boost::optional<gtsam::Key> GetKey(KeyCreatorFunction key_creator_function, - const localization_common::Time timestamp) const = 0; - - virtual boost::optional<localization_common::Time> OldestTimestamp() const = 0; - - virtual boost::optional<localization_common::Time> LatestTimestamp() const = 0; - - virtual NodeUpdaterType type() const = 0; -}; -} // namespace graph_optimizer - -#endif // GRAPH_OPTIMIZER_NODE_UPDATER_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/node_updater_with_priors.h b/localization/graph_optimizer/include/graph_optimizer/node_updater_with_priors.h deleted file mode 100644 index 769b246376..0000000000 --- a/localization/graph_optimizer/include/graph_optimizer/node_updater_with_priors.h +++ /dev/null @@ -1,39 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_OPTIMIZER_NODE_UPDATER_WITH_PRIORS_H_ -#define GRAPH_OPTIMIZER_NODE_UPDATER_WITH_PRIORS_H_ - -#include <graph_optimizer/node_updater.h> - -#include <gtsam/nonlinear/NonlinearFactorGraph.h> - -namespace graph_optimizer { -template <typename NodeType, typename NoiseType> -class NodeUpdaterWithPriors : public NodeUpdater { - public: - virtual ~NodeUpdaterWithPriors() {} - - virtual void AddInitialValuesAndPriors(const NodeType& node, const NoiseType& noise, - gtsam::NonlinearFactorGraph& graph) = 0; - - virtual void AddPriors(const NodeType& node, const NoiseType& noise, gtsam::NonlinearFactorGraph& factors) = 0; -}; -} // namespace graph_optimizer - -#endif // GRAPH_OPTIMIZER_NODE_UPDATER_WITH_PRIORS_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/utilities.h b/localization/graph_optimizer/include/graph_optimizer/utilities.h deleted file mode 100644 index 8617cb4305..0000000000 --- a/localization/graph_optimizer/include/graph_optimizer/utilities.h +++ /dev/null @@ -1,56 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef GRAPH_OPTIMIZER_UTILITIES_H_ -#define GRAPH_OPTIMIZER_UTILITIES_H_ - -#include <localization_common/logger.h> - -#include <gtsam/nonlinear/NonlinearFactorGraph.h> - -namespace graph_optimizer { -gtsam::NonlinearFactorGraph RemoveOldFactors(const gtsam::KeyVector& old_keys, gtsam::NonlinearFactorGraph& graph); - -template <typename FactorType> -void DeleteFactors(gtsam::NonlinearFactorGraph& graph) { - int num_removed_factors = 0; - for (auto factor_it = graph.begin(); factor_it != graph.end();) { - if (dynamic_cast<FactorType*>(factor_it->get())) { - factor_it = graph.erase(factor_it); - ++num_removed_factors; - continue; - } - ++factor_it; - } - LogDebug("DeleteFactors: Num removed factors: " << num_removed_factors); -} - -template <typename FactorType> -int NumFactors(const gtsam::NonlinearFactorGraph& graph) { - int num_factors = 0; - for (const auto& factor : graph) { - if (dynamic_cast<const FactorType*>(factor.get())) { - ++num_factors; - } - } - return num_factors; -} - -} // namespace graph_optimizer - -#endif // GRAPH_OPTIMIZER_UTILITIES_H_ diff --git a/localization/graph_optimizer/package.xml b/localization/graph_optimizer/package.xml index 10b279e9d5..846f534031 100644 --- a/localization/graph_optimizer/package.xml +++ b/localization/graph_optimizer/package.xml @@ -14,16 +14,12 @@ Astrobee Flight Software </maintainer> <buildtool_depend>catkin</buildtool_depend> - <build_depend>roscpp</build_depend> - <build_depend>nodelet</build_depend> - <build_depend>config_reader</build_depend> + <build_depend>factor_adders</build_depend> <build_depend>localization_common</build_depend> - <build_depend>localization_measurements</build_depend> - <build_depend>msg_conversions</build_depend> - <run_depend>roscpp</run_depend> - <run_depend>nodelet</run_depend> - <run_depend>config_reader</run_depend> + <build_depend>node_adders</build_depend> + <build_depend>optimizers</build_depend> + <run_depend>factor_adders</run_depend> <run_depend>localization_common</run_depend> - <run_depend>localization_measurements</run_depend> - <run_depend>msg_conversions</run_depend> + <run_depend>node_adders</run_depend> + <run_depend>optimizers</run_depend> </package> diff --git a/localization/graph_optimizer/readme.md b/localization/graph_optimizer/readme.md index 18a8175d21..9a39039b61 100644 --- a/localization/graph_optimizer/readme.md +++ b/localization/graph_optimizer/readme.md @@ -1,59 +1,10 @@ \page graphoptimizer Graph Optimizer # Package Overview -The GraphOptimizer class provides a sliding window optimizer using the GTSAM library that slides in time. Several objects are utilitized to track state parameters (nodes) and error functions (factors) as described below: NodeUpdater, FactorAdder, GraphValues, and GraphActionCompleter. The GraphOptimizer contains a set of NodeUpdaters which handle node insertion and deletion for each state parameter type. Each of these NodeUpdaters contains a GraphValues object for storing a type of state parameters. +The graph optimizer contains factor and node adders and uses these to generate factors and nodes for optimization. During an optimize call, it queries each factor adder and adds new factors and nodes to an internal factor graph before calling the respective optimizer. See the factor_adder and node_adder packages for more information on each of these classes. <p align="center"> -<img src="./doc/images/graph_optimizer.png" width="330"> +<img src="../doc/images/graph_optimizer.png" width="550"> </p> -FactorAdders push factors to the GraphOptimzer where they are buffered until they are able to be added to the graph. -<p align="center"> -<img src="./doc/images/measurement_adding.png" width="520"> -</p> - -If the factors are too old they are discarded. Otherwise, if the factors are more recent than the state parameter nodes in the graph they depend on, the factors remain buffered until this is no longer the case. This can occur, for example, if a factor is created using a sensor measurement that arrives at a higher frequency than the sensor data required to link subsequent state parameter nodes. For more information on linking state parameter nodes, see the NodeUpdater section below.<br/> -The GraphOptimizer uses the latest and oldest timestamps of nodes stored in its NodeUpdaters to maintiain a sliding window of the correct duration. - - - -# Background -The GraphOptimizer uses the GTSAM library (_Dellaert, Frank. Factor graphs and GTSAM: A hands-on introduction. Georgia Institute of Technology, 2012._) to optimize a nonlinear weighted least-squares problem where the weights are the measurement uncertainties and the least-squares errors are measurement errors (factors). - -# Important Classes -## GraphOptimizer - The GraphOptimizer maintains sets of NodeUpdaters and GraphActionCompleters. It adds factors provided by FactorAdders and CumulativeFactorAdders and handles optimization of the graph. When factors are added to the graph, the NodeUpdaters responsible for creating new nodes are called and the factors are updated with Keys for the newly created nodes. The GraphOptimizer also maintains the required sliding window duration by calling each of its NodeUpdaters with the new desired window size. Marginal factors can optionally be added for factors removed during a sliding window operation. Additional functions are provided that allow for functions to be called after the window is slid and after each Update() call which includes the nonlinear optimization of the graph. The Update() call is illustrated below: - - <p align="center"> - <img src="./doc/images/update.png" width="200"> - </p> - -## FactorAdder -FactorAdders are responsible for outputting factors given a certain measurement type. The output factors from a FactorAdder should be added to the GraphOptimizer using its BufferFactors() member function. - -## CumulativeFactorAdder -CumulativeFactorAdders provide an alternative interface for adding factors to a GraphOptimizer than FactorAdders. Whereas FactorAdders add factors given a single measurement, CumulativeFactorAdders add factors for a set of measurements. CumulativeFactorAdder creation is triggered at the beginning of each GraphOptimizer Update() call so that factors are created once the maximum number of measurements have been accumulated. An example of a use for CumulativeFactorAdders is for SmartFactors (_Carlone, Luca, et al. "Eliminating conditionally independent sets in factor graphs: A unifying perspective based on smart factors." 2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2014._). These create a set of factors using a set of measurements depending on a state parameter and subsequently marginalize out the state parameter from this factor set, creating a single resulting factor containing the information of the factor set in the process. Since this can only occur once all the measurements are provided, the normal FactorAdder interface that creates factors given a single measurement would not suffice for this type of factor. - -## GraphValues -GraphValues offer an interface on top of the gtsam::Values class that adds several helper functions for accessing values. Additionally, OldestTimestamp() and LatestTimestamp() functions are provided so the values can be used for sliding window optimization. - -## NodeUpdater -NodeUpdaters are responsible for adding and removing state parameters when new factors are added or the graph optimizer window slides. For time varying state parameters, new nodes should be created when a new factor is added whose timestamp is not contained in the current set of state parameter nodes that it depends on. Typically this occurs when a factor is more recent than previously added factors or when an out of order measurement arrives and a state parameter needs to be added between previously existing state parameters. The NodeUpdater maintains its own copy of GraphValues for the state parameter being tracked and updates this with newly added state parameters. Additionally, the NodeUpdater is responsible for adding required relative factors for new state parameter nodes. - -<p align="center"> -<img src="./doc/images/factor_updating.png" width="500"> -</p> - - For a new state parameter node that is more recent than any previous nodes, this simply requires connecting the new node with the previous latest node using some type of factor that provides a relative constraint. Typically these are in the form of a motion model or using sensor measurements such as an IMU when dealing with a robot body pose as the state parameter. Additionally, NodeUpdaters are responsible for providing a SlideWindow() member function that handles removing old state parameter nodes. Each unique state parameter set tracked in a GraphOptimizer should have its own NodeUpdater to handle insertion and deletion of its nodes. NodeUpdaters must be added to the GraphOptimizer object using its AddNodeUpdater() function. - -## GraphActionCompleter -The GraphActionCompleter provides the option of completing a graph action operation after a set of factors from a FactorAdder that were buffered have been added to the GraphOptimizer. A graph action is any operation modifying the newly added factors or other factors in the graph. Since factors from a FactorAdder are not immediately added to the graph optimizer (see Overivew section for more details), this enables checks on factors once required state parameter nodes are created. For example, checks can see if a factor measurement is an outlier and remove the factor. GraphActionCompleters should be pushed to the GraphOptimizer object using the AddGraphActionCompleter() function. - -# Delayed Insertion of Factor and Key Objects -As factors are first buffered in the GraphOptimizer before being added (see Overview section for more details), not all required information for factors are available when they are created in FactorAdders. For example, GTSAM requires Keys for the state parameter nodes that factors depend on. Since these may not have been created yet, the FactorToAdd and KeyInfo classes are used to allow for the delayed insertion of factors. - -## FactorToAdd -The FactorToAdd object contains a set of factors and KeyInfo objects that contain the required information for subsequent key and node creation in the GraphOptimizer. -## KeyInfo -KeyInfo objects contain the required information to create a gtsam::Key. They take a KeyCreatorFunction (typically gtsam::symbol_shorthand::X, where X should be unique for each state parameter type), the NodeUpdaterType so the graph optimizer knows which NodeUpdater to use for node creation, and an optional timestamp. KeyInfos can be static or dynamic, depending on if they vary in time or not. diff --git a/localization/graph_optimizer/src/graph_optimizer.cc b/localization/graph_optimizer/src/graph_optimizer.cc index 2dade79768..b2ff8a1db2 100644 --- a/localization/graph_optimizer/src/graph_optimizer.cc +++ b/localization/graph_optimizer/src/graph_optimizer.cc @@ -17,476 +17,124 @@ */ #include <graph_optimizer/graph_optimizer.h> -#include <graph_optimizer/utilities.h> +#include <optimizers/isam2_optimizer.h> +#include <optimizers/nonlinear_optimizer.h> #include <localization_common/logger.h> #include <localization_common/utilities.h> -#include <gtsam/nonlinear/LinearContainerFactor.h> - -#include <unistd.h> - -#include <chrono> -#include <unordered_set> - -namespace { -// TODO(rsoussan): Is this necessary? Just use DFATAL and compile with debug? -// Avoid having to compile with DEBUG to toggle between fatal and non-fatal failures -void log(const bool fatal_failure, const std::string& description) { - if (fatal_failure) { - LogFatal(description); - } else { - LogError(description); - } -} -} // namespace - namespace graph_optimizer { +namespace fa = factor_adders; namespace lc = localization_common; +namespace na = node_adders; +namespace no = nodes; +namespace op = optimizers; -GraphOptimizer::GraphOptimizer(const GraphOptimizerParams& params, std::unique_ptr<GraphStats> graph_stats) - : graph_stats_(std::move(graph_stats)), values_(new gtsam::Values()), params_(params) { - // Initialize lm params - if (params_.verbose) { - levenberg_marquardt_params_.verbosityLM = gtsam::LevenbergMarquardtParams::VerbosityLM::TRYDELTA; - levenberg_marquardt_params_.verbosity = gtsam::NonlinearOptimizerParams::Verbosity::LINEAR; - } - if (params_.use_ceres_params) { - gtsam::LevenbergMarquardtParams::SetCeresDefaults(&levenberg_marquardt_params_); - } - - levenberg_marquardt_params_.maxIterations = params_.max_iterations; - - if (params_.marginals_factorization == "qr") { - marginals_factorization_ = gtsam::Marginals::Factorization::QR; - } else if (params_.marginals_factorization == "cholesky") { - marginals_factorization_ = gtsam::Marginals::Factorization::CHOLESKY; - } else { - LogError("GraphOptimizer: No marginals factorization entered, defaulting to qr."); - marginals_factorization_ = gtsam::Marginals::Factorization::QR; - } +GraphOptimizer::GraphOptimizer(const GraphOptimizerParams& params, std::unique_ptr<optimizers::Optimizer> optimizer) + : params_(params), + optimizer_(std::move(optimizer)), + values_(new no::Values()), + stats_logger_(params_.log_stats_on_destruction) { + AddAveragersAndTimers(); } -GraphOptimizer::~GraphOptimizer() { - if (params_.log_on_destruction) graph_stats_->Log(); +void GraphOptimizer::AddNodeAdder(std::shared_ptr<na::NodeAdder> node_adder) { + node_adder->AddInitialNodesAndPriors(factors_); + node_adders_.emplace_back(std::move(node_adder)); } -void GraphOptimizer::AddGraphActionCompleter(std::shared_ptr<GraphActionCompleter> graph_action_completer) { - graph_action_completers_.emplace_back(std::move(graph_action_completer)); +void GraphOptimizer::AddFactorAdder(std::shared_ptr<fa::FactorAdder> factor_adder) { + factor_adders_.emplace_back(std::move(factor_adder)); } -void GraphOptimizer::AddNodeUpdater(std::shared_ptr<NodeUpdater> node_updater) { - node_updaters_.emplace_back(std::move(node_updater)); -} - -// Adapted from gtsam::BatchFixedLagSmoother -gtsam::NonlinearFactorGraph GraphOptimizer::MarginalFactors( - const gtsam::NonlinearFactorGraph& old_factors, const gtsam::KeyVector& old_keys, - const gtsam::GaussianFactorGraph::Eliminate& eliminate_function) const { - // Old keys not present in old factors. This shouldn't occur. - if (old_keys.size() == 0) { - LogDebug("MarginalFactors: No old keys provided."); - return old_factors; - } - - // Linearize Graph - const auto linearized_graph = old_factors.linearize(*values_); - const auto linear_marginal_factors = - *(linearized_graph->eliminatePartialMultifrontal(old_keys, eliminate_function).second); - return gtsam::LinearContainerFactor::ConvertLinearGraph(linear_marginal_factors, *values_); -} - -boost::optional<lc::Time> GraphOptimizer::SlideWindowNewOldestTime() const { - boost::optional<lc::Time> new_oldest_time; - for (const auto& node_updater : node_updaters_) { - const auto node_new_oldest_time = node_updater->SlideWindowNewOldestTime(); - if (node_new_oldest_time) { - new_oldest_time = new_oldest_time ? std::min(*node_new_oldest_time, *new_oldest_time) : *node_new_oldest_time; - } - } - - return new_oldest_time; -} - -gtsam::KeyVector GraphOptimizer::OldKeys(const localization_common::Time oldest_allowed_time) const { - gtsam::KeyVector all_old_keys; - for (const auto& node_updater : node_updaters_) { - const auto old_keys = node_updater->OldKeys(oldest_allowed_time, graph_); - all_old_keys.insert(all_old_keys.end(), old_keys.begin(), old_keys.end()); +int GraphOptimizer::AddFactors(const lc::Time start_time, const lc::Time end_time) { + int num_added_factors = 0; + for (const auto& factor_adder : factor_adders_) { + num_added_factors += factor_adder->AddFactors(start_time, end_time, factors_); } - return all_old_keys; + return num_added_factors; } -std::pair<gtsam::KeyVector, gtsam::NonlinearFactorGraph> GraphOptimizer::OldKeysAndFactors( - const lc::Time oldest_allowed_time) { - const auto old_keys = OldKeys(oldest_allowed_time); - // Since cumlative factors have many keys and shouldn't be marginalized, need to remove old measurements depending on - // old keys before marginalizing and sliding window - RemoveOldMeasurementsFromCumulativeFactors(old_keys); - const auto old_factors = RemoveOldFactors(old_keys, graph_); - return std::make_pair(old_keys, old_factors); +int GraphOptimizer::AddFactors(const std::pair<lc::Time, lc::Time>& start_and_end_time) { + return AddFactors(start_and_end_time.first, start_and_end_time.second); } -bool GraphOptimizer::SlideWindow(const boost::optional<gtsam::Marginals>& marginals, const lc::Time last_latest_time) { - const auto ideal_new_oldest_time = SlideWindowNewOldestTime(); - if (!ideal_new_oldest_time) { - LogDebug("SlideWindow: No states removed. "); - return true; +bool GraphOptimizer::Optimize() { + if (!ValidGraph()) { + LogError("Optimize: Invalid graph, not optimizing."); + return false; } - // Ensure that new oldest time isn't more recent than last latest time - // since then priors couldn't be added for the new oldest state - if (last_latest_time < *ideal_new_oldest_time) - LogError("SlideWindow: Ideal oldest time is more recent than last latest time."); - const auto new_oldest_time = std::min(last_latest_time, *ideal_new_oldest_time); - const auto old_keys_and_factors = OldKeysAndFactors(new_oldest_time); - if (params_.add_marginal_factors) { - const auto marginal_factors = - MarginalFactors(old_keys_and_factors.second, old_keys_and_factors.first, gtsam::EliminateQR); - for (const auto& marginal_factor : marginal_factors) { - graph_.push_back(marginal_factor); - } - } + // Optimize + LogDebug("Optimize: Optimizing."); + optimization_timer_.Start(); + const bool successful_optimization = optimizer_->Optimize(factors_, gtsam_values()); + optimization_timer_.Stop(); - for (auto& node_updater : node_updaters_) - node_updater->SlideWindow(new_oldest_time, marginals, old_keys_and_factors.first, params_.huber_k, graph_); + optimization_iterations_averager_.Update(optimizer_->iterations()); + total_error_averager_.Update(TotalGraphError()); - RemoveOldBufferedFactors(new_oldest_time); - DoPostSlideWindowActions(new_oldest_time, marginals); - return true; + if (params_.print_after_optimization) Print(); + return successful_optimization; } -void GraphOptimizer::DoPostSlideWindowActions(const localization_common::Time oldest_allowed_time, - const boost::optional<gtsam::Marginals>& marginals) {} - -void GraphOptimizer::BufferCumulativeFactors() {} - -void GraphOptimizer::RemoveOldMeasurementsFromCumulativeFactors(const gtsam::KeyVector& old_keys) {} - -bool GraphOptimizer::ValidGraph() const { return true; } - -void GraphOptimizer::BufferFactors(const std::vector<FactorsToAdd>& factors_to_add_vec) { - for (const auto& factors_to_add : factors_to_add_vec) - buffered_factors_to_add_.emplace(factors_to_add.timestamp(), factors_to_add); +boost::optional<gtsam::Matrix> GraphOptimizer::Covariance(const gtsam::Key& key) const { + return optimizer_->Covariance(key); } -void GraphOptimizer::RemoveOldBufferedFactors(const lc::Time oldest_allowed_timestamp) { - for (auto factors_to_add_it = buffered_factors_to_add_.begin(); - factors_to_add_it != buffered_factors_to_add_.end();) { - auto& factors_to_add = factors_to_add_it->second.Get(); - for (auto factor_to_add_it = factors_to_add.begin(); factor_to_add_it != factors_to_add.end();) { - bool removed_factor = false; - for (const auto& key_info : factor_to_add_it->key_infos) { - // Ignore static keys - if (key_info.is_static()) continue; - if (key_info.timestamp() < oldest_allowed_timestamp) { - LogDebug("RemoveOldBufferedFactors: Removing old factor from buffered factors."); - factor_to_add_it = factors_to_add.erase(factor_to_add_it); - removed_factor = true; - break; - } - } - if (!removed_factor) ++factor_to_add_it; - } - if (factors_to_add_it->second.Get().empty()) { - LogDebug("RemoveOldBufferedFactors: Removing old factors from buffered factors."); - factors_to_add_it = buffered_factors_to_add_.erase(factors_to_add_it); - } else { - ++factors_to_add_it; - } - } +boost::optional<gtsam::Matrix> GraphOptimizer::Covariance(const gtsam::Key& key_a, const gtsam::Key& key_b) const { + return optimizer_->Covariance(key_a, key_b); } -int GraphOptimizer::AddBufferedFactors() { - LogDebug("AddBufferedfactors: Adding buffered factors."); - LogDebug("AddBufferedFactors: Num buffered factors to add: " << buffered_factors_to_add_.size()); - - int num_added_factors = 0; - for (auto factors_to_add_it = buffered_factors_to_add_.begin(); - factors_to_add_it != buffered_factors_to_add_.end() && ReadyToAddFactors(factors_to_add_it->first);) { - auto& factors_to_add = factors_to_add_it->second; - for (auto factor_to_add_it = factors_to_add.Get().begin(); factor_to_add_it != factors_to_add.Get().end();) { - auto& factor_to_add = *factor_to_add_it; - bool valid_factor = true; - for (const auto& key_info : factor_to_add.key_infos) { - if (!UpdateNodes(key_info)) { - LogError("AddBufferedFactors: Failed to update nodes."); - valid_factor = false; - break; - } - } +const gtsam::NonlinearFactorGraph& GraphOptimizer::factors() const { return factors_; } - if (valid_factor && !Rekey(factor_to_add)) { - LogError("AddBufferedMeasurements: Failed to rekey factor to add."); - valid_factor = false; - } +gtsam::NonlinearFactorGraph& GraphOptimizer::factors() { return factors_; } - // Remove invalid factors leading to errors - factor_to_add_it = valid_factor ? ++factor_to_add_it : factors_to_add.Get().erase(factor_to_add_it); - } +int GraphOptimizer::num_factors() const { return factors_.size(); } - if (factors_to_add.empty()) { - LogDebug("AddBufferedFactors: Factors to add empty."); - factors_to_add_it = buffered_factors_to_add_.erase(factors_to_add_it); - continue; - } +int GraphOptimizer::num_values() const { return values_->size(); } - if (!DoGraphAction(factors_to_add)) { - LogDebug("AddBufferedFactors: Failed to complete graph action."); - factors_to_add_it = buffered_factors_to_add_.erase(factors_to_add_it); - continue; - } +const GraphOptimizerParams& GraphOptimizer::params() const { return params_; } - for (auto& factor_to_add : factors_to_add.Get()) { - graph_.push_back(factor_to_add.factor); - ++num_added_factors; - } - factors_to_add_it = buffered_factors_to_add_.erase(factors_to_add_it); - } +std::shared_ptr<nodes::Values> GraphOptimizer::values() { return values_; } - LogDebug("AddBufferedFactors: Added " << num_added_factors << " factors."); - return num_added_factors; -} +const gtsam::Values& GraphOptimizer::gtsam_values() const { return values_->gtsam_values(); } -bool GraphOptimizer::UpdateNodes(const KeyInfo& key_info) { - // Do nothing for static nodes - if (key_info.is_static()) return true; - for (auto& node_updater : node_updaters_) { - if (node_updater->type() == key_info.node_updater_type()) return node_updater->Update(key_info.timestamp(), graph_); - } - LogError("UpdateNodes: No node updater found for key info."); - return false; -} +gtsam::Values& GraphOptimizer::gtsam_values() { return values_->gtsam_values(); } -bool GraphOptimizer::DoGraphAction(FactorsToAdd& factors_to_add) { - if (factors_to_add.graph_action_completer_type() == GraphActionCompleterType::None) return true; - for (auto& graph_action_completer : graph_action_completers_) { - if (graph_action_completer->type() == factors_to_add.graph_action_completer_type()) - return graph_action_completer->DoAction(factors_to_add, graph_); - } +lc::StatsLogger& GraphOptimizer::stats_logger() { return stats_logger_; } - LogError("DoGraphAction: No graph action completer found for factors to add."); - return false; -} +const lc::Timer& GraphOptimizer::optimization_timer() const { return optimization_timer_; } -boost::optional<gtsam::Key> GraphOptimizer::GetKey(KeyCreatorFunction key_creator_function, - const localization_common::Time timestamp) const { - // This avoids a bug in the compiler that produces a false warning that key may be uninitialized - boost::optional<gtsam::Key> key = boost::make_optional(false, gtsam::Key()); - for (const auto& node_updater : node_updaters_) { - const auto node_key = node_updater->GetKey(key_creator_function, timestamp); - if (node_key) { - if (!key) { - key = node_key; - } else { - LogError("GetKey: Found key in multiple node updators."); - return boost::none; - } - } - } - return key; +const localization_common::Averager& GraphOptimizer::optimization_iterations_averager() const { + return optimization_iterations_averager_; } -bool GraphOptimizer::Rekey(FactorToAdd& factor_to_add) { - gtsam::KeyVector new_keys; - const auto& old_keys = factor_to_add.factor->keys(); - for (int i = 0; i < static_cast<int>(factor_to_add.key_infos.size()); ++i) { - const auto& key_info = factor_to_add.key_infos[i]; - if (key_info.is_static()) { - // Don't change static keys. Assumes static key currently in factor is correct - new_keys.emplace_back(old_keys[i]); - } else { - const auto new_key = GetKey(key_info.key_creator_function(), key_info.timestamp()); - if (!new_key) { - LogError("ReKey: Failed to find new key for timestamp."); - return false; - } - new_keys.emplace_back(*new_key); - } +double GraphOptimizer::TotalGraphError() const { + double total_error = 0; + for (const auto& factor : factors_) { + const double error = factor->error(gtsam_values()); + total_error += error; } - factor_to_add.factor->keys() = new_keys; - return true; + return total_error; } -bool GraphOptimizer::ReadyToAddFactors(const localization_common::Time timestamp) const { return true; } - -boost::optional<lc::Time> GraphOptimizer::OldestTimestamp() const { - boost::optional<lc::Time> oldest_timestamp; - for (const auto& node_updater : node_updaters_) { - const auto node_oldest_timestamp = node_updater->OldestTimestamp(); - if (node_oldest_timestamp) { - oldest_timestamp = - oldest_timestamp ? std::min(*oldest_timestamp, *node_oldest_timestamp) : *node_oldest_timestamp; - } - } - return oldest_timestamp; -} +bool GraphOptimizer::ValidGraph() const { return true; } -boost::optional<lc::Time> GraphOptimizer::LatestTimestamp() const { - boost::optional<lc::Time> latest_timestamp; - for (const auto& node_updater : node_updaters_) { - const auto node_latest_timestamp = node_updater->LatestTimestamp(); - if (node_latest_timestamp) { - latest_timestamp = - latest_timestamp ? std::max(*latest_timestamp, *node_latest_timestamp) : *node_latest_timestamp; - } - } - return latest_timestamp; +void GraphOptimizer::Print() const { + factors_.print(); + stats_logger_.Log(); } -bool GraphOptimizer::MeasurementRecentEnough(const lc::Time timestamp) const { - const auto oldest_timestamp = OldestTimestamp(); - if (!oldest_timestamp) { - LogError("MeasurementRecentEnough: Failed to get oldest timestamp."); - return false; - } - if (timestamp < *oldest_timestamp) return false; - return true; -} - -void GraphOptimizer::PrintFactorDebugInfo() const {} - -const GraphOptimizerParams& GraphOptimizer::params() const { return params_; } - -const gtsam::NonlinearFactorGraph& GraphOptimizer::graph_factors() const { return graph_; } - -gtsam::NonlinearFactorGraph& GraphOptimizer::graph_factors() { return graph_; } - -const int GraphOptimizer::num_factors() const { return graph_.size(); } - -const boost::optional<gtsam::Marginals>& GraphOptimizer::marginals() const { return marginals_; } - -std::shared_ptr<gtsam::Values> GraphOptimizer::shared_values() { return values_; } - -const gtsam::Values& GraphOptimizer::values() const { return *values_; } - void GraphOptimizer::SaveGraphDotFile(const std::string& output_path) const { std::ofstream of(output_path.c_str()); - graph_.saveGraph(of, *values_); + factors_.saveGraph(of, gtsam_values()); } -const GraphStats* const GraphOptimizer::graph_stats() const { return graph_stats_.get(); } - -GraphStats* GraphOptimizer::graph_stats() { return graph_stats_.get(); } - -void GraphOptimizer::LogOnDestruction(const bool log_on_destruction) { - params_.log_on_destruction = log_on_destruction; -} - -bool GraphOptimizer::DoPostOptimizeActions() { return true; } - -bool GraphOptimizer::Update() { - LogDebug("Update: Updating."); - graph_stats_->update_timer_.Start(); +boost::optional<const gtsam::Marginals&> GraphOptimizer::marginals() const { return optimizer_->marginals(); } - graph_stats_->add_buffered_factors_timer_.Start(); - BufferCumulativeFactors(); - const int num_added_factors = AddBufferedFactors(); - graph_stats_->add_buffered_factors_timer_.Stop(); - if (num_added_factors <= 0) { - LogDebug("Update: No factors added."); - return false; - } - - // Only get marginals and slide window if optimization has already occured - // TODO(rsoussan): Make cleaner way to check for this - if (last_latest_time_) { - graph_stats_->marginals_timer_.Start(); - // Calculate marginals for covariances - try { - marginals_ = gtsam::Marginals(graph_, *values_, marginals_factorization_); - } catch (gtsam::IndeterminantLinearSystemException) { - log(params_.fatal_failures, "Update: Indeterminant linear system error during computation of marginals."); - marginals_ = boost::none; - } catch (const std::exception& exception) { - log(params_.fatal_failures, "Update: Computing marginals failed. " + std::string(exception.what())); - marginals_ = boost::none; - } catch (...) { - log(params_.fatal_failures, "Update: Computing marginals failed."); - marginals_ = boost::none; - } - graph_stats_->marginals_timer_.Stop(); - - graph_stats_->slide_window_timer_.Start(); - if (!SlideWindow(marginals_, *last_latest_time_)) { - LogError("Update: Failed to slide window."); - return false; - } - graph_stats_->slide_window_timer_.Stop(); - } - - // TODO(rsoussan): Is ordering required? if so clean these calls open and unify with marginalization - // TODO(rsoussan): Remove this now that marginalization occurs before optimization? - if (params_.add_marginal_factors) { - // Add graph ordering to place keys that will be marginalized in first group - const auto new_oldest_time = SlideWindowNewOldestTime(); - if (new_oldest_time) { - const auto old_keys = OldKeys(*new_oldest_time); - const auto ordering = gtsam::Ordering::ColamdConstrainedFirst(graph_, old_keys); - levenberg_marquardt_params_.setOrdering(ordering); - } else { - levenberg_marquardt_params_.orderingType = gtsam::Ordering::COLAMD; - } - } - - if (!ValidGraph()) { - LogError("Update: Invalid graph, not optimizing."); - return false; - } - - // Optimize - gtsam::LevenbergMarquardtOptimizer optimizer(graph_, *values_, levenberg_marquardt_params_); - - graph_stats_->optimization_timer_.Start(); - // TODO(rsoussan): Indicate if failure occurs in state msg, perhaps using confidence value in msg - try { - *values_ = optimizer.optimize(); - } catch (gtsam::IndeterminantLinearSystemException) { - log(params_.fatal_failures, "Update: Graph optimization failed, indeterminant linear system, keeping old values."); - } catch (gtsam::InvalidNoiseModel) { - log(params_.fatal_failures, "Update: Graph optimization failed, invalid noise model, keeping old values."); - } catch (gtsam::InvalidMatrixBlock) { - log(params_.fatal_failures, "Update: Graph optimization failed, invalid matrix block, keeping old values."); - } catch (gtsam::InvalidDenseElimination) { - log(params_.fatal_failures, "Update: Graph optimization failed, invalid dense elimination, keeping old values."); - } catch (...) { - log(params_.fatal_failures, "Update: Graph optimization failed, keeping old values."); - } - graph_stats_->optimization_timer_.Stop(); - - // Calculate marginals after the first optimization iteration so covariances - // can be used for first loc msg - // TODO(rsoussan): Clean this up - if (!last_latest_time_) { - graph_stats_->marginals_timer_.Start(); - // Calculate marginals for covariances - try { - marginals_ = gtsam::Marginals(graph_, *values_, marginals_factorization_); - } catch (gtsam::IndeterminantLinearSystemException) { - log(params_.fatal_failures, "Update: Indeterminant linear system error during computation of marginals."); - marginals_ = boost::none; - } catch (...) { - log(params_.fatal_failures, "Update: Computing marginals failed."); - marginals_ = boost::none; - } - graph_stats_->marginals_timer_.Stop(); - } - - last_latest_time_ = LatestTimestamp(); - - graph_stats_->log_stats_timer_.Start(); - graph_stats_->iterations_averager_.Update(optimizer.iterations()); - graph_stats_->UpdateStats(graph_); - graph_stats_->log_stats_timer_.Stop(); - graph_stats_->log_error_timer_.Start(); - graph_stats_->UpdateErrors(graph_); - graph_stats_->log_error_timer_.Stop(); - - if (params_.print_factor_info) PrintFactorDebugInfo(); - if (!DoPostOptimizeActions()) { - LogError("Update: Failed to complete post optimize actions."); - return false; - } - graph_stats_->update_timer_.Stop(); - return true; +void GraphOptimizer::AddAveragersAndTimers() { + stats_logger_.AddTimer(optimization_timer_); + stats_logger_.AddAverager(optimization_iterations_averager_); + stats_logger_.AddAverager(total_error_averager_); } } // namespace graph_optimizer diff --git a/localization/graph_optimizer/src/graph_stats.cc b/localization/graph_optimizer/src/graph_stats.cc deleted file mode 100644 index 5f569c8a1e..0000000000 --- a/localization/graph_optimizer/src/graph_stats.cc +++ /dev/null @@ -1,67 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#include <graph_optimizer/graph_stats.h> -#include <graph_optimizer/utilities.h> - -#include <gtsam/nonlinear/LinearContainerFactor.h> - -namespace graph_optimizer { -GraphStats::GraphStats() { - timers_.emplace_back(optimization_timer_); - timers_.emplace_back(update_timer_); - timers_.emplace_back(marginals_timer_); - timers_.emplace_back(slide_window_timer_); - timers_.emplace_back(add_buffered_factors_timer_); - timers_.emplace_back(log_error_timer_); - timers_.emplace_back(log_stats_timer_); - - AddStatsAverager(iterations_averager_); - AddErrorAverager(total_error_averager_); -} - -void GraphStats::AddStatsAverager(localization_common::Averager& stats_averager) { - stats_averagers_.emplace_back(stats_averager); -} - -void GraphStats::AddErrorAverager(localization_common::Averager& error_averager) { - error_averagers_.emplace_back(error_averager); -} - -void GraphStats::UpdateErrors(const gtsam::NonlinearFactorGraph& graph_factors) {} - -void GraphStats::UpdateStats(const gtsam::NonlinearFactorGraph& graph_factors) {} - -void GraphStats::Log() const { - Log(timers_); - Log(stats_averagers_); - Log(error_averagers_); -} - -void GraphStats::LogToFile(std::ofstream& ofstream) const { - LogToFile(timers_, ofstream); - LogToFile(stats_averagers_, ofstream); - LogToFile(error_averagers_, ofstream); -} - -void GraphStats::LogToCsv(std::ofstream& ofstream) const { - ofstream << "name,avg,min,max,stddev" << std::endl; - LogToCsv(timers_, ofstream); - LogToCsv(stats_averagers_, ofstream); - LogToCsv(error_averagers_, ofstream); -} -} // namespace graph_optimizer diff --git a/localization/graph_optimizer/src/parameter_reader.cc b/localization/graph_optimizer/src/parameter_reader.cc deleted file mode 100644 index a5204566f9..0000000000 --- a/localization/graph_optimizer/src/parameter_reader.cc +++ /dev/null @@ -1,37 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_optimizer/parameter_reader.h> -#include <msg_conversions/msg_conversions.h> - -namespace graph_optimizer { -namespace mc = msg_conversions; - -void LoadGraphOptimizerParams(config_reader::ConfigReader& config, GraphOptimizerParams& params) { - params.verbose = mc::LoadBool(config, "verbose"); - params.fatal_failures = mc::LoadBool(config, "fatal_failures"); - params.log_on_destruction = mc::LoadBool(config, "log_on_destruction"); - params.print_factor_info = mc::LoadBool(config, "print_factor_info"); - params.use_ceres_params = mc::LoadBool(config, "use_ceres_params"); - params.max_iterations = mc::LoadInt(config, "max_iterations"); - params.marginals_factorization = mc::LoadString(config, "marginals_factorization"); - params.add_marginal_factors = mc::LoadBool(config, "add_marginal_factors"); - params.huber_k = mc::LoadDouble(config, "huber_k"); - params.log_rate = mc::LoadInt(config, "log_rate"); -} -} // namespace graph_optimizer diff --git a/localization/graph_optimizer/src/utilities.cc b/localization/graph_optimizer/src/utilities.cc deleted file mode 100644 index db7eb6c301..0000000000 --- a/localization/graph_optimizer/src/utilities.cc +++ /dev/null @@ -1,44 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <graph_optimizer/utilities.h> - -namespace graph_optimizer { -gtsam::NonlinearFactorGraph RemoveOldFactors(const gtsam::KeyVector& old_keys, gtsam::NonlinearFactorGraph& graph) { - gtsam::NonlinearFactorGraph removed_factors; - if (old_keys.empty()) return removed_factors; - - for (auto factor_it = graph.begin(); factor_it != graph.end();) { - bool found_key = false; - for (const auto& key : old_keys) { - if ((*factor_it)->find(key) != (*factor_it)->end()) { - found_key = true; - break; - } - } - if (found_key) { - removed_factors.push_back(*factor_it); - factor_it = graph.erase(factor_it); - } else { - ++factor_it; - } - } - - return removed_factors; -} -} // namespace graph_optimizer diff --git a/localization/graph_optimizer/test/test_graph_optimizer.cc b/localization/graph_optimizer/test/test_graph_optimizer.cc new file mode 100644 index 0000000000..7615523b35 --- /dev/null +++ b/localization/graph_optimizer/test/test_graph_optimizer.cc @@ -0,0 +1,191 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <factor_adders/factor_adder.h> +#include <graph_optimizer/graph_optimizer.h> +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <node_adders/node_adder.h> +#include <nodes/values.h> +#include <optimizers/optimizer.h> + +#include <gtest/gtest.h> + +namespace fa = factor_adders; +namespace go = graph_optimizer; +namespace lc = localization_common; +namespace na = node_adders; +namespace no = nodes; +namespace op = optimizers; + +// Test node adder that just returns keys that should be used. +// Key values are calculated using the integer timestamps passed. +// TODO(rsoussan): Unify this with VO factor_adder test +class SimpleNodeAdder : public na::NodeAdder { + public: + explicit SimpleNodeAdder(std::shared_ptr<no::Values> values) : values_(values) {} + + // Add prior factor and node + void AddInitialNodesAndPriors(gtsam::NonlinearFactorGraph& factors) final { + AddNode(0, factors); + const auto keys = Keys(0); + const gtsam::Vector6 noise_sigmas((gtsam::Vector(6) << 0.1, 0.1, 0.1, 0.2, 0.2, 0.2).finished()); + const auto noise = + lc::Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(noise_sigmas)), huber_k); + gtsam::PriorFactor<gtsam::Pose3>::shared_ptr prior_factor( + new gtsam::PriorFactor<gtsam::Pose3>(keys[0], gtsam::Pose3::identity(), noise)); + factors.push_back(prior_factor); + } + + bool AddNode(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final { + values_->Add(gtsam::Pose3::identity()); + return true; + } + + bool CanAddNode(const localization_common::Time timestamp) const final { return true; } + + // Assumes integer timestamps that perfectly cast to ints. + // First key is pose key. + gtsam::KeyVector Keys(const localization_common::Time timestamp) const final { + gtsam::KeyVector keys; + // Offset by 1 since node keys start at 1 + keys.emplace_back(gtsam::Key(static_cast<int>(timestamp + 1))); + return keys; + } + + private: + std::shared_ptr<no::Values> values_; + const double huber_k = 1.345; +}; + +class SimpleFactorAdder : public fa::FactorAdder { + public: + SimpleFactorAdder(const fa::FactorAdderParams& params, std::shared_ptr<SimpleNodeAdder> node_adder) + : fa::FactorAdder(params), node_adder_(node_adder) {} + int AddFactors(const localization_common::Time oldest_allowed_time, + const localization_common::Time newest_allowed_time, gtsam::NonlinearFactorGraph& factors) final { + node_adder_->AddNode(oldest_allowed_time, factors); + const auto keys = node_adder_->Keys(oldest_allowed_time); + const gtsam::Vector6 noise_sigmas((gtsam::Vector(6) << 0.1, 0.1, 0.1, 0.2, 0.2, 0.2).finished()); + const auto noise = + lc::Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(noise_sigmas)), params_.huber_k); + gtsam::PriorFactor<gtsam::Pose3>::shared_ptr prior_factor( + new gtsam::PriorFactor<gtsam::Pose3>(keys[0], gtsam::Pose3::identity(), noise)); + factors.push_back(prior_factor); + return 1; + } + + std::shared_ptr<SimpleNodeAdder> node_adder_; +}; + +class SimpleOptimizer : public op::Optimizer { + public: + explicit SimpleOptimizer(const op::OptimizerParams& params) : op::Optimizer(params) {} + + bool Optimize(const gtsam::NonlinearFactorGraph& factors, gtsam::Values& values) final { return true; } + + boost::optional<gtsam::Matrix> Covariance(const gtsam::Key& key) const final { + return gtsam::Matrix(gtsam::Matrix6::Identity()); + } + + int iterations() const final { return 0; } +}; + +class GraphOptimizerTest : public ::testing::Test { + public: + GraphOptimizerTest() {} + + void SetUp() final {} + + void Initialize(const go::GraphOptimizerParams& params) { + std::unique_ptr<SimpleOptimizer> optimizer(new SimpleOptimizer(DefaultOptimizerParams())); + graph_optimizer_.reset(new go::GraphOptimizer(params, std::move(optimizer))); + node_adder_.reset(new SimpleNodeAdder(graph_optimizer_->values())); + factor_adder_.reset(new SimpleFactorAdder(DefaultFactorAdderParams(), node_adder_)); + graph_optimizer_->AddNodeAdder(node_adder_); + graph_optimizer_->AddFactorAdder(factor_adder_); + } + + go::GraphOptimizerParams DefaultParams() { + go::GraphOptimizerParams params; + params.huber_k = 1.345; + params.log_stats_on_destruction = false; + params.print_after_optimization = false; + return params; + } + + fa::FactorAdderParams DefaultFactorAdderParams() { + fa::FactorAdderParams params; + params.enabled = true; + params.huber_k = 1.345; + return params; + } + + op::OptimizerParams DefaultOptimizerParams() { + op::OptimizerParams params; + params.marginals_factorization = "qr"; + return params; + } + + std::unique_ptr<go::GraphOptimizer> graph_optimizer_; + std::shared_ptr<SimpleFactorAdder> factor_adder_; + std::shared_ptr<SimpleNodeAdder> node_adder_; +}; + +TEST_F(GraphOptimizerTest, AddFactors) { + auto params = DefaultParams(); + Initialize(params); + // Node and factor should be added for initial node adder node and prior + EXPECT_EQ(graph_optimizer_->num_factors(), 1); + EXPECT_EQ(graph_optimizer_->num_values(), 1); + // Add first factors + EXPECT_EQ(graph_optimizer_->AddFactors(0, 1), 1); + EXPECT_EQ(graph_optimizer_->factors().size(), 2); + EXPECT_EQ(graph_optimizer_->num_factors(), 2); + EXPECT_EQ(graph_optimizer_->num_values(), 2); + EXPECT_TRUE(graph_optimizer_->Optimize()); + // Add second factors + EXPECT_EQ(graph_optimizer_->AddFactors(1, 2), 1); + EXPECT_EQ(graph_optimizer_->factors().size(), 3); + EXPECT_EQ(graph_optimizer_->num_factors(), 3); + EXPECT_EQ(graph_optimizer_->num_values(), 3); + EXPECT_TRUE(graph_optimizer_->Optimize()); +} + +TEST_F(GraphOptimizerTest, Covariance) { + auto params = DefaultParams(); + Initialize(params); + EXPECT_EQ(graph_optimizer_->num_factors(), 1); + // Add first factors + EXPECT_EQ(graph_optimizer_->AddFactors(0, 1), 1); + EXPECT_EQ(graph_optimizer_->factors().size(), 2); + EXPECT_EQ(graph_optimizer_->num_factors(), 2); + EXPECT_EQ(graph_optimizer_->num_values(), 2); + EXPECT_TRUE(graph_optimizer_->Optimize()); + const auto keys = node_adder_->Keys(0); + const auto covariance = graph_optimizer_->Covariance(keys[0]); + EXPECT_TRUE(covariance != boost::none); + EXPECT_MATRIX_NEAR(gtsam::Matrix(*covariance), gtsam::Matrix6::Identity(), 1e-6); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/graph_optimizer/test/test_graph_optimizer.test b/localization/graph_optimizer/test/test_graph_optimizer.test new file mode 100644 index 0000000000..94268240d4 --- /dev/null +++ b/localization/graph_optimizer/test/test_graph_optimizer.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="graph_optimizer" type="test_graph_optimizer" test-name="test_graph_optimizer" /> +</launch> diff --git a/localization/graph_vio/CMakeLists.txt b/localization/graph_vio/CMakeLists.txt new file mode 100644 index 0000000000..400726e509 --- /dev/null +++ b/localization/graph_vio/CMakeLists.txt @@ -0,0 +1,97 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(graph_vio) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +# Find GTSAM +find_package(GTSAM REQUIRED) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + factor_adders + graph_factors + imu_integration + localization_common + localization_measurements + node_adders + sliding_window_graph_optimizer + vision_common +) + +# Find OpenCV +LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") +find_package(OpenCV4WithXFeatures REQUIRED) + +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} + CATKIN_DEPENDS + factor_adders + graph_factors + imu_integration + localization_common + localization_measurements + node_adders + sliding_window_graph_optimizer + vision_common +) + +########### +## Build ## +########### +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +file(GLOB cc_files + "src/*.cc" +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + ${cc_files} +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/graph_vio/include/graph_vio/graph_vio.h b/localization/graph_vio/include/graph_vio/graph_vio.h new file mode 100644 index 0000000000..6b06bb5466 --- /dev/null +++ b/localization/graph_vio/include/graph_vio/graph_vio.h @@ -0,0 +1,114 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_VIO_GRAPH_VIO_H_ +#define GRAPH_VIO_GRAPH_VIO_H_ + +#include <factor_adders/depth_odometry_factor_adder.h> +#include <factor_adders/standstill_factor_adder.h> +#include <factor_adders/vo_smart_projection_factor_adder.h> +#include <graph_vio/graph_vio_params.h> +#include <localization_common/marginals_pose_covariance_interpolater.h> +#include <localization_measurements/depth_odometry_measurement.h> +#include <localization_measurements/fan_speed_mode.h> +#include <localization_measurements/feature_points_measurement.h> +#include <localization_measurements/imu_measurement.h> +#include <localization_measurements/standstill_measurement.h> +#include <node_adders/combined_nav_state_node_adder.h> +#include <nodes/combined_nav_state_nodes.h> +#include <sliding_window_graph_optimizer/sliding_window_graph_optimizer.h> +#include <vision_common/feature_tracker.h> + +#include <boost/serialization/serialization.hpp> + +namespace graph_vio { +// Siding window graph optimizer that uses IMU and optical flow feature point measurements +// with VO smart projection factors to perform VIO. +// Uses the CombinedNavStateNodeAdder to add relative IMU factors between graph nodes. +// Also adds standstill pose and velocity factors when standstill is detected +// using the history of feature point measurements. +class GraphVIO : public sliding_window_graph_optimizer::SlidingWindowGraphOptimizer { + public: + explicit GraphVIO(const GraphVIOParams& params); + + // For Serialization Only + GraphVIO() {} + + // Adds imu measurement to combined nav state node adder. + void AddImuMeasurement(const localization_measurements::ImuMeasurement& imu_measurement); + + // Sets the fan speed mode in the combined nav state node model's IMU integrator + void SetFanSpeedMode(const localization_measurements::FanSpeedMode& fan_speed_mode); + + // Adds feature points measurement to vo smart projection factor adder. + void AddFeaturePointsMeasurement( + const localization_measurements::FeaturePointsMeasurement& feature_points_measurement); + + // Adds depth odometry measurement for depth odometry relative pose factor adder. + void AddDepthOdometryMeasurement( + const localization_measurements::DepthOdometryMeasurement& depth_odometry_measurement); + + // Returns a const reference to combined nav state nodes. + const nodes::CombinedNavStateNodes& combined_nav_state_nodes() const; + + // Returns whether standstill is detected or not. + bool standstill() const; + + // Const accesor to feature tracker used for smart factor creation + const vision_common::SpacedFeatureTracker& feature_tracker() const; + + // Construct pose covariance interpolater using latest marginals and nodes + std::shared_ptr<localization_common::MarginalsPoseCovarianceInterpolater<nodes::CombinedNavStateNodes>> + MarginalsPoseCovarianceInterpolater(); + + private: + // Uses the latest feature track points to detect standstill. + bool CheckForStandstill(const localization_common::Time oldest_allowed_time); + + // bool ValidGraph() const final; + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(sliding_window_graph_optimizer::SlidingWindowGraphOptimizer); + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(vo_smart_projection_factor_adder_); + ar& BOOST_SERIALIZATION_NVP(standstill_factor_adder_); + ar& BOOST_SERIALIZATION_NVP(depth_odometry_factor_adder_); + ar& BOOST_SERIALIZATION_NVP(combined_nav_state_node_adder_); + } + + GraphVIOParams params_; + std::shared_ptr<node_adders::CombinedNavStateNodeAdder> combined_nav_state_node_adder_; + bool standstill_; + + // Factor Adders + std::shared_ptr<factor_adders::DepthOdometryFactorAdder<node_adders::CombinedNavStateNodeAdder>> + depth_odometry_factor_adder_; + std::shared_ptr<factor_adders::VoSmartProjectionFactorAdder<node_adders::CombinedNavStateNodeAdder>> + vo_smart_projection_factor_adder_; + std::shared_ptr<factor_adders::StandstillFactorAdder<node_adders::CombinedNavStateNodeAdder>> + standstill_factor_adder_; + + // Node Adders + // std::shared_ptr<node_adders::CombinedNavStateNodeAdder> combined_nav_state_node_adder_; +}; +} // namespace graph_vio + +#endif // GRAPH_VIO_GRAPH_VIO_H_ diff --git a/localization/graph_vio/include/graph_vio/graph_vio_params.h b/localization/graph_vio/include/graph_vio/graph_vio_params.h new file mode 100644 index 0000000000..d95ae9e453 --- /dev/null +++ b/localization/graph_vio/include/graph_vio/graph_vio_params.h @@ -0,0 +1,59 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef GRAPH_VIO_GRAPH_VIO_PARAMS_H_ +#define GRAPH_VIO_GRAPH_VIO_PARAMS_H_ + +#include <factor_adders/depth_odometry_factor_adder_params.h> +#include <factor_adders/standstill_factor_adder_params.h> +#include <factor_adders/vo_smart_projection_factor_adder_params.h> +#include <node_adders/combined_nav_state_node_adder.h> +#include <node_adders/combined_nav_state_node_adder_model_params.h> +#include <optimizers/nonlinear_optimizer.h> +#include <sliding_window_graph_optimizer/sliding_window_graph_optimizer_params.h> +#include <vision_common/standstill_params.h> + +#include <boost/serialization/serialization.hpp> + +namespace graph_vio { +struct GraphVIOParams { + factor_adders::DepthOdometryFactorAdderParams depth_odometry_factor_adder; + factor_adders::StandstillFactorAdderParams standstill_factor_adder; + factor_adders::VoSmartProjectionFactorAdderParams vo_smart_projection_factor_adder; + node_adders::CombinedNavStateNodeAdder::Params combined_nav_state_node_adder; + node_adders::CombinedNavStateNodeAdderModelParams combined_nav_state_node_adder_model; + optimizers::NonlinearOptimizerParams nonlinear_optimizer; + sliding_window_graph_optimizer::SlidingWindowGraphOptimizerParams sliding_window_graph_optimizer; + vision_common::StandstillParams standstill; + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_NVP(depth_odometry_factor_adder); + ar& BOOST_SERIALIZATION_NVP(standstill_factor_adder); + ar& BOOST_SERIALIZATION_NVP(vo_smart_projection_factor_adder); + ar& BOOST_SERIALIZATION_NVP(combined_nav_state_node_adder); + ar& BOOST_SERIALIZATION_NVP(nonlinear_optimizer); + ar& BOOST_SERIALIZATION_NVP(sliding_window_graph_optimizer); + ar& BOOST_SERIALIZATION_NVP(standstill); + } +}; +} // namespace graph_vio + +#endif // GRAPH_VIO_GRAPH_VIO_PARAMS_H_ diff --git a/localization/graph_vio/package.xml b/localization/graph_vio/package.xml new file mode 100644 index 0000000000..786b39650a --- /dev/null +++ b/localization/graph_vio/package.xml @@ -0,0 +1,36 @@ +<package> + <name>graph_vio</name> + <version>1.0.0</version> + <description> + The graph VIO package + </description> + <license> + Apache License, Version 2.0 + </license> + <author email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </author> + <maintainer email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </maintainer> + <buildtool_depend>catkin</buildtool_depend> + <build_depend>factor_adders</build_depend> + <build_depend>graph_factors</build_depend> + <build_depend>imu_integration</build_depend> + <build_depend>localization_common</build_depend> + <build_depend>localization_measurements</build_depend> + <build_depend>node_adders</build_depend> + <build_depend>sliding_window_graph_optimizer</build_depend> + <build_depend>vision_common</build_depend> + <run_depend>factor_adders</run_depend> + <run_depend>graph_factors</run_depend> + <run_depend>imu_integration</run_depend> + <run_depend>localization_common</run_depend> + <run_depend>localization_measurements</run_depend> + <run_depend>node_adders</run_depend> + <run_depend>sliding_window_graph_optimizer</run_depend> + <run_depend>vision_common</run_depend> + <export> + <nodelet plugin="${prefix}/nodelet_plugins.xml" /> + </export> +</package> diff --git a/localization/graph_vio/readme.md b/localization/graph_vio/readme.md new file mode 100644 index 0000000000..d525e31fb0 --- /dev/null +++ b/localization/graph_vio/readme.md @@ -0,0 +1,25 @@ +\page graphvio Graph VIO + +# Package Overview +Performs sliding-window graph based visual-intertial odometry (VIO) using a VOSmartProjectionFactorAdder and StandstillFactorAdder. Uses a CombinedNavStateNodeAdder for creating combined nav state nodes (containing a pose, velocity, and IMU bias) at required timestamps using IMU measurements. Optionally adds depth image correspondences as point-to-point between factors. + +# Background +For more information on the theory behind the GraphVIO and the factors used, please see our paper: +* Ryan Soussan, Varsha Kumar, Brian Coltin, and Trey Smith, "Astroloc: An efficient and robust localizer for a free-flying robot", Int. Conf. on Robotics and Automation (ICRA), 2022. [Link](https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9811919 "Link") + +# Graph Optimization Structure +<p align="center"> +<img src="../doc/images/graph_vio.png" width="550"> +</p> + +## Factor Adders +* DepthOdometryFactorAdder +* VoSmartProjectionFactorAdder +* StandstillFactorAdder +## Graph Factors +* Point BetweenFactors (for depth odometry correspondences) +* CombinedIMUFactor +* RobustSmartProjectionFactor +* Standstill Factors (zero velocity prior and identity relative transform between factors) +## Node Adders +* CombinedNavStateNodeAdder diff --git a/localization/graph_vio/src/graph_vio.cc b/localization/graph_vio/src/graph_vio.cc new file mode 100644 index 0000000000..6fdf359d02 --- /dev/null +++ b/localization/graph_vio/src/graph_vio.cc @@ -0,0 +1,97 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <graph_vio/graph_vio.h> +#include <vision_common/utilities.h> + +namespace graph_vio { +namespace fa = factor_adders; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace na = node_adders; +namespace no = nodes; +namespace op = optimizers; +namespace vc = vision_common; + +GraphVIO::GraphVIO(const GraphVIOParams& params) + : SlidingWindowGraphOptimizer(params.sliding_window_graph_optimizer, + std::make_unique<op::NonlinearOptimizer>(params.nonlinear_optimizer)), + params_(params), + standstill_(false) { + // Initialize sliding window node adders + combined_nav_state_node_adder_ = std::make_shared<na::CombinedNavStateNodeAdder>( + params_.combined_nav_state_node_adder, params_.combined_nav_state_node_adder_model, values()); + AddSlidingWindowNodeAdder(combined_nav_state_node_adder_); + // Initialize factor adders + vo_smart_projection_factor_adder_ = std::make_shared<fa::VoSmartProjectionFactorAdder<na::CombinedNavStateNodeAdder>>( + params_.vo_smart_projection_factor_adder, combined_nav_state_node_adder_); + AddFactorAdder(vo_smart_projection_factor_adder_); + standstill_factor_adder_ = std::make_shared<fa::StandstillFactorAdder<na::CombinedNavStateNodeAdder>>( + params_.standstill_factor_adder, combined_nav_state_node_adder_); + AddFactorAdder(standstill_factor_adder_); + depth_odometry_factor_adder_ = std::make_shared<fa::DepthOdometryFactorAdder<na::CombinedNavStateNodeAdder>>( + params_.depth_odometry_factor_adder, combined_nav_state_node_adder_); + AddFactorAdder(depth_odometry_factor_adder_); +} + +void GraphVIO::AddImuMeasurement(const lm::ImuMeasurement& imu_measurement) { + combined_nav_state_node_adder_->AddMeasurement(imu_measurement); +} + +void GraphVIO::SetFanSpeedMode(const localization_measurements::FanSpeedMode& fan_speed_mode) { + combined_nav_state_node_adder_->node_adder_model().SetFanSpeedMode(fan_speed_mode); +} + +void GraphVIO::AddFeaturePointsMeasurement(const lm::FeaturePointsMeasurement& feature_points_measurement) { + if (params_.vo_smart_projection_factor_adder.enabled) + vo_smart_projection_factor_adder_->AddMeasurement(feature_points_measurement); + + // Check for standstill and optionally add standstill measurement if detected. + const auto& feature_tracks = vo_smart_projection_factor_adder_->feature_tracker().feature_tracks(); + standstill_ = vc::Standstill(feature_tracks, params_.standstill); + if (standstill_ && params_.standstill_factor_adder.enabled) { + const lc::Time latest_timestamp = feature_points_measurement.timestamp; + const auto previous_timestamp = feature_tracks.cbegin()->second.SecondLatestTimestamp(); + standstill_factor_adder_->AddMeasurement(lm::StandstillMeasurement(latest_timestamp, *previous_timestamp)); + } +} + +void GraphVIO::AddDepthOdometryMeasurement( + const localization_measurements::DepthOdometryMeasurement& depth_odometry_measurement) { + // Don't add depth odom measurements when at standstill to avoid + // introducing pose and velocity noise + if (params_.depth_odometry_factor_adder.enabled && !standstill_) + depth_odometry_factor_adder_->AddMeasurement(depth_odometry_measurement); +} + +const no::CombinedNavStateNodes& GraphVIO::combined_nav_state_nodes() const { + return combined_nav_state_node_adder_->nodes(); +} + +bool GraphVIO::standstill() const { return standstill_; } + +const vc::SpacedFeatureTracker& GraphVIO::feature_tracker() const { + return vo_smart_projection_factor_adder_->feature_tracker(); +} + +std::shared_ptr<lc::MarginalsPoseCovarianceInterpolater<no::CombinedNavStateNodes>> +GraphVIO::MarginalsPoseCovarianceInterpolater() { + return std::make_shared<lc::MarginalsPoseCovarianceInterpolater<no::CombinedNavStateNodes>>( + combined_nav_state_node_adder_->nodes_ptr(), marginals()); +} +} // namespace graph_vio diff --git a/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc b/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc index a86ef9b329..38e399878c 100644 --- a/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc +++ b/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc @@ -74,11 +74,11 @@ void GroundTruthLocalizerNodelet::PoseCallback(geometry_msgs::PoseStamped::Const const lc::Time timestamp = lc::TimeFromHeader(pose->header); PublishLocState(timestamp); heartbeat_.header.stamp = ros::Time::now(); - // Publish heartbeat for graph localizer and imu augmentor since flight software expects this + // Publish heartbeat for graph localizer and pose extrapolator since flight software expects this // and this runs in place of them heartbeat_.node = NODE_GRAPH_LOC; heartbeat_pub_.publish(heartbeat_); - heartbeat_.node = NODE_IMU_AUG; + heartbeat_.node = NODE_POSE_EXTR; heartbeat_pub_.publish(heartbeat_); } diff --git a/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_wrapper.h b/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_wrapper.h deleted file mode 100644 index 420f7e8da0..0000000000 --- a/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_wrapper.h +++ /dev/null @@ -1,77 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#ifndef IMU_AUGMENTOR_IMU_AUGMENTOR_WRAPPER_H_ -#define IMU_AUGMENTOR_IMU_AUGMENTOR_WRAPPER_H_ - -#include <ff_msgs/EkfState.h> -#include <ff_msgs/FlightMode.h> -#include <ff_msgs/GraphState.h> -#include <imu_augmentor/imu_augmentor.h> -#include <localization_common/combined_nav_state.h> -#include <localization_common/combined_nav_state_covariances.h> -#include <localization_common/rate_timer.h> -#include <localization_measurements/imu_measurement.h> - -#include <geometry_msgs/PoseWithCovarianceStamped.h> -#include <sensor_msgs/Imu.h> - -#include <memory> -#include <string> -#include <utility> - -namespace imu_augmentor { -class ImuAugmentorWrapper { - public: - explicit ImuAugmentorWrapper(const std::string& graph_config_path_prefix = ""); - - explicit ImuAugmentorWrapper(const ImuAugmentorParams& params); - - void LocalizationStateCallback(const ff_msgs::GraphState& loc_msg); - - void ImuCallback(const sensor_msgs::Imu& imu_msg); - - void FlightModeCallback(const ff_msgs::FlightMode& flight_mode); - - boost::optional<std::pair<localization_common::CombinedNavState, localization_common::CombinedNavStateCovariances>> - LatestImuAugmentedCombinedNavStateAndCovariances(); - - boost::optional<ff_msgs::EkfState> LatestImuAugmentedLocalizationMsg(); - - private: - void Initialize(const ImuAugmentorParams& params); - - bool LatestImuAugmentedCombinedNavStateAndCovariances( - localization_common::CombinedNavState& latest_imu_augmented_combined_nav_state, - localization_common::CombinedNavStateCovariances& latest_imu_augmented_covariances); - - bool LatestImuMeasurement(localization_measurements::ImuMeasurement& latest_imu_measurement); - - bool standstill() const; - - std::unique_ptr<ImuAugmentor> imu_augmentor_; - boost::optional<localization_common::CombinedNavState> latest_combined_nav_state_; - boost::optional<localization_common::CombinedNavState> latest_imu_augmented_combined_nav_state_; - boost::optional<localization_common::CombinedNavStateCovariances> latest_covariances_; - boost::optional<ff_msgs::GraphState> latest_loc_msg_; - std::unique_ptr<gtsam::TangentPreintegration> preintegration_helper_; - ImuAugmentorParams params_; - boost::optional<bool> standstill_; - localization_common::RateTimer loc_state_timer_ = localization_common::RateTimer("Loc State Msg"); -}; -} // namespace imu_augmentor -#endif // IMU_AUGMENTOR_IMU_AUGMENTOR_WRAPPER_H_ diff --git a/localization/imu_augmentor/nodelet_plugins.xml b/localization/imu_augmentor/nodelet_plugins.xml deleted file mode 100644 index 8839d4225b..0000000000 --- a/localization/imu_augmentor/nodelet_plugins.xml +++ /dev/null @@ -1,11 +0,0 @@ -<library path="lib/libimu_augmentor"> - <class name="imu_augmentor/ImuAugmentorNodelet" - type="imu_augmentor::ImuAugmentorNodelet" - base_class_type="nodelet::Nodelet"> - <description> - This nodelet wraps the imu_augmentor - </description> - </class> -</library> - - diff --git a/localization/imu_augmentor/readme.md b/localization/imu_augmentor/readme.md deleted file mode 100644 index 085c0cd436..0000000000 --- a/localization/imu_augmentor/readme.md +++ /dev/null @@ -1,16 +0,0 @@ -\page imuaugmentor Imu Augmentor - -# Package Overview -The imu augmentor extrapolates a localization estimate with imu data. This is useful in order to "catch up" the latest localization estimate to current time, for use with a planner or other module. The imu augmentor uses an ImuIntegrator object to maintain a history of imu measurements and integrate up to the most recent imu measurement. Imu biases are reset on receival of localization estimates to more accurately integrate measurements. - -## Ros Node -The ros node subscribes to the localization estimate and publishes an updated imu augmented localization estimate. It also fills in the latest acceleration and angular velocity. The augmentor also publishes the latest pose and twist as respective messages. - -# Inputs -* `/hw/imu` -* `graph_loc/state` - -# Outputs -* `gnc/ekf` -* `loc/pose` -* `loc/twist` diff --git a/localization/imu_augmentor/src/imu_augmentor.cc b/localization/imu_augmentor/src/imu_augmentor.cc deleted file mode 100644 index 49fce03489..0000000000 --- a/localization/imu_augmentor/src/imu_augmentor.cc +++ /dev/null @@ -1,55 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <imu_augmentor/imu_augmentor.h> -#include <imu_integration/utilities.h> -#include <localization_common/logger.h> - -namespace imu_augmentor { -namespace ii = imu_integration; -namespace lc = localization_common; -namespace lm = localization_measurements; -ImuAugmentor::ImuAugmentor(const ImuAugmentorParams& params) : ii::ImuIntegrator(params) {} - -void ImuAugmentor::PimPredict(const lc::CombinedNavState& latest_combined_nav_state, - lc::CombinedNavState& latest_imu_augmented_combined_nav_state) { - if (Empty()) return; - // Start with least upper bound measurement - // Don't add measurements with same timestamp as start_time - // since these would have a dt of 0 (wrt the pim start time) and cause errors for the pim - auto measurement_it = measurements().upper_bound(latest_imu_augmented_combined_nav_state.timestamp()); - if (measurement_it == measurements().cend()) return; - // Always use the biases from the lastest combined nav state - auto pim = ii::Pim(latest_combined_nav_state.bias(), pim_params()); - int num_measurements_added = 0; - // Create new pim each time since pim uses beginning orientation and velocity for - // gravity integration and initial velocity integration. - for (; measurement_it != measurements().cend(); ++measurement_it) { - pim.resetIntegrationAndSetBias(latest_combined_nav_state.bias()); - auto time = latest_imu_augmented_combined_nav_state.timestamp(); - ii::AddMeasurement(measurement_it->second, time, pim); - latest_imu_augmented_combined_nav_state = ii::PimPredict(latest_imu_augmented_combined_nav_state, pim); - ++num_measurements_added; - } - - // Only remove measurements up to latest combined nav state so that when a new nav state is received IMU data is still - // available for extrapolation - RemoveOldMeasurements(latest_combined_nav_state.timestamp()); - LogDebug("PimPredict: Added " << num_measurements_added << " measurements."); -} -} // namespace imu_augmentor diff --git a/localization/imu_augmentor/src/imu_augmentor_wrapper.cc b/localization/imu_augmentor/src/imu_augmentor_wrapper.cc deleted file mode 100644 index 7373e4035a..0000000000 --- a/localization/imu_augmentor/src/imu_augmentor_wrapper.cc +++ /dev/null @@ -1,181 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <config_reader/config_reader.h> -#include <imu_augmentor/imu_augmentor_wrapper.h> -#include <imu_integration/utilities.h> -#include <localization_common/logger.h> -#include <localization_common/utilities.h> -#include <localization_measurements/imu_measurement.h> -#include <localization_measurements/measurement_conversions.h> -#include <msg_conversions/msg_conversions.h> - -namespace imu_augmentor { -namespace ii = imu_integration; -namespace lc = localization_common; -namespace lm = localization_measurements; -namespace mc = msg_conversions; - -ImuAugmentorWrapper::ImuAugmentorWrapper(const std::string& graph_config_path_prefix) { - config_reader::ConfigReader config; - config.AddFile("transforms.config"); - config.AddFile("geometry.config"); - lc::LoadGraphLocalizerConfig(config, graph_config_path_prefix); - - if (!config.ReadFiles()) { - LogFatal("Failed to read config files."); - } - - ImuAugmentorParams params; - ii::LoadImuIntegratorParams(config, params); - params.standstill_enabled = mc::LoadBool(config, "imu_augmentor_standstill"); - Initialize(params); -} - -ImuAugmentorWrapper::ImuAugmentorWrapper(const ImuAugmentorParams& params) { Initialize(params); } - -void ImuAugmentorWrapper::Initialize(const ImuAugmentorParams& params) { - params_ = params; - imu_augmentor_.reset(new ImuAugmentor(params_)); - - // Preintegration_helper_ is only being used to frame change and remove centrifugal acceleration, so body_T_imu is the - // only parameter needed. - boost::shared_ptr<gtsam::PreintegrationParams> preintegration_params(new gtsam::PreintegrationParams()); - preintegration_params->setBodyPSensor(params_.body_T_imu); - preintegration_helper_.reset(new gtsam::TangentPreintegration(preintegration_params, gtsam::imuBias::ConstantBias())); -} - -void ImuAugmentorWrapper::LocalizationStateCallback(const ff_msgs::GraphState& loc_msg) { - loc_state_timer_.RecordAndVlogEveryN(10, 2); - const auto loc_state_elapsed_time = loc_state_timer_.LastValue(); - if (loc_state_elapsed_time && *loc_state_elapsed_time >= 2) { - LogError("LocalizationStateCallback: More than 2 seconds elapsed between loc state msgs."); - } - - latest_combined_nav_state_ = lc::CombinedNavStateFromMsg(loc_msg); - latest_covariances_ = lc::CombinedNavStateCovariancesFromMsg(loc_msg); - // Reset imu augmented state so IMU data is added starting with the most recent combined nav state's biases - latest_imu_augmented_combined_nav_state_ = latest_combined_nav_state_; - latest_loc_msg_ = loc_msg; - standstill_ = loc_msg.standstill; -} - -bool ImuAugmentorWrapper::standstill() const { - if (!params_.standstill_enabled) return false; - // If uninitialized, return not at standstill - // TODO(rsoussan): Is this the appropriate behavior? - if (!standstill_) return false; - return *standstill_; -} - -void ImuAugmentorWrapper::ImuCallback(const sensor_msgs::Imu& imu_msg) { - imu_augmentor_->BufferImuMeasurement(lm::ImuMeasurement(imu_msg)); -} - -void ImuAugmentorWrapper::FlightModeCallback(const ff_msgs::FlightMode& flight_mode) { - imu_augmentor_->SetFanSpeedMode(lm::ConvertFanSpeedMode(flight_mode.speed)); -} - -boost::optional<std::pair<lc::CombinedNavState, lc::CombinedNavStateCovariances>> -ImuAugmentorWrapper::LatestImuAugmentedCombinedNavStateAndCovariances() { - if (!latest_combined_nav_state_ || !latest_covariances_ || !imu_augmentor_ || - !latest_imu_augmented_combined_nav_state_) { - LogError( - "LatestImuAugmentedCombinedNavStateAndCovariances: Not enough information available to create desired data."); - return boost::none; - } - - if (standstill()) { - LogDebugEveryN(100, "LatestImuAugmentedCombinedNavStateAndCovariances: Standstill."); - const auto latest_timestamp = imu_augmentor_->LatestTime(); - if (!latest_timestamp) { - LogError("LatestImuAugmentedCombinedNavStateAndCovariances: Failed to get latest timestamp."); - return boost::none; - } - const lc::CombinedNavState latest_timestamp_combined_nav_state( - latest_combined_nav_state_->nav_state(), latest_combined_nav_state_->bias(), *latest_timestamp); - return std::pair<lc::CombinedNavState, lc::CombinedNavStateCovariances>{latest_timestamp_combined_nav_state, - *latest_covariances_}; - } - - imu_augmentor_->PimPredict(*latest_combined_nav_state_, *latest_imu_augmented_combined_nav_state_); - if (!latest_imu_augmented_combined_nav_state_) { - LogError("LatestImuAugmentedCombinedNavSTateAndCovariances: Failed to pim predict combined nav state."); - return boost::none; - } - // TODO(rsoussan): propogate uncertainties from imu augmentor - return std::pair<lc::CombinedNavState, lc::CombinedNavStateCovariances>{*latest_imu_augmented_combined_nav_state_, - *latest_covariances_}; -} - -boost::optional<ff_msgs::EkfState> ImuAugmentorWrapper::LatestImuAugmentedLocalizationMsg() { - if (!latest_loc_msg_) { - LogDebugEveryN(50, "LatestImuAugmentedLocalizationMsg: No latest loc msg available."); - return boost::none; - } - - const auto latest_imu_augmented_combined_nav_state_and_covariances = - LatestImuAugmentedCombinedNavStateAndCovariances(); - if (!latest_imu_augmented_combined_nav_state_and_covariances) { - LogError("LatestImuAugmentedLocalizationMsg: Failed to get latest imu augmented nav state and covariances."); - return boost::none; - } - - // Get feature counts and other info from latest_loc_msg - ff_msgs::EkfState latest_imu_augmented_loc_msg; - latest_imu_augmented_loc_msg.header = latest_loc_msg_->header; - latest_imu_augmented_loc_msg.child_frame_id = latest_loc_msg_->child_frame_id; - latest_imu_augmented_loc_msg.confidence = latest_loc_msg_->confidence; - // Prevent overflow of uin8_t - latest_imu_augmented_loc_msg.of_count = - latest_loc_msg_->num_of_factors <= 255 ? latest_loc_msg_->num_of_factors : 255; - latest_imu_augmented_loc_msg.ml_count = - latest_loc_msg_->num_ml_projection_factors <= 255 ? latest_loc_msg_->num_ml_projection_factors : 255; - latest_imu_augmented_loc_msg.estimating_bias = latest_loc_msg_->estimating_bias; - - // Update nav state and covariances with latest imu measurements - lc::CombinedNavStateToMsg(latest_imu_augmented_combined_nav_state_and_covariances->first, - latest_imu_augmented_loc_msg); - lc::CombinedNavStateCovariancesToMsg(latest_imu_augmented_combined_nav_state_and_covariances->second, - latest_imu_augmented_loc_msg); - - // Add latest bias corrected acceleration and angular velocity to loc msg - const auto latest_imu_measurement = imu_augmentor_->LatestMeasurement(); - if (!latest_imu_measurement) { - LogError("LatestImuAugmentedLocalizationMsg: Failed to get latest measurement."); - return boost::none; - } - const auto& latest_bias = latest_imu_augmented_combined_nav_state_and_covariances->first.bias(); - auto latest_bias_corrected_acceleration = latest_bias.correctAccelerometer(latest_imu_measurement->acceleration); - const auto latest_bias_corrected_angular_velocity = - latest_bias.correctGyroscope(latest_imu_measurement->angular_velocity); - // Correct for gravity if needed - if (!params_.gravity.isZero()) { - const gtsam::Pose3& global_T_body_latest = latest_imu_augmented_combined_nav_state_and_covariances->first.pose(); - latest_bias_corrected_acceleration = lc::RemoveGravityFromAccelerometerMeasurement( - params_.gravity, params_.body_T_imu, global_T_body_latest, latest_bias_corrected_acceleration); - } - // Frame change measurements to body frame, correct for centripetal accel - const auto corrected_measurements = preintegration_helper_->correctMeasurementsBySensorPose( - latest_bias_corrected_acceleration, latest_bias_corrected_angular_velocity); - - mc::VectorToMsg(corrected_measurements.first, latest_imu_augmented_loc_msg.accel); - mc::VectorToMsg(corrected_measurements.second, latest_imu_augmented_loc_msg.omega); - return latest_imu_augmented_loc_msg; -} -} // namespace imu_augmentor diff --git a/localization/imu_augmentor/test/test_imu_augmentor.cc b/localization/imu_augmentor/test/test_imu_augmentor.cc deleted file mode 100644 index 251f49f259..0000000000 --- a/localization/imu_augmentor/test/test_imu_augmentor.cc +++ /dev/null @@ -1,203 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include "test_utilities.h" // NOLINT -#include <imu_augmentor/imu_augmentor.h> -#include <localization_common/logger.h> -#include <localization_common/test_utilities.h> -#include <localization_common/time.h> - -#include <gtest/gtest.h> - -namespace ia = imu_augmentor; -namespace lc = localization_common; -namespace lm = localization_measurements; - -class ConstantAccelerationTest : public ::testing::Test { - public: - // Start at time increment so first IMU measurement is after starting combined nav state time - ConstantAccelerationTest() - : acceleration_i_(0.1), - acceleration_(acceleration_i_, acceleration_i_, acceleration_i_), - time_increment_(1.0 / 125.0), - start_time_(time_increment_), - num_measurements_(20) { - const ia::ImuAugmentorParams params = ia::DefaultImuAugmentorParams(); - imu_augmentor_.reset(new ia::ImuAugmentor(params)); - } - - void SetUp() final { - const std::vector<lm::ImuMeasurement> imu_measurements = - ia::ConstantAccelerationMeasurements(acceleration_, num_measurements_, start_time_, time_increment_); - for (const auto& imu_measurement : imu_measurements) { - imu_augmentor_->BufferImuMeasurement(imu_measurement); - } - } - - ia::ImuAugmentor& imu_augmentor() { return *imu_augmentor_; } - - double acceleration_i() { return acceleration_i_; } - - const Eigen::Vector3d& acceleration() { return acceleration_; } - - double time_increment() { return time_increment_; } - - double start_time() { return start_time_; } - - int num_measurements() { return num_measurements_; } - - private: - std::unique_ptr<ia::ImuAugmentor> imu_augmentor_; - const double acceleration_i_; - const Eigen::Vector3d acceleration_; - const double time_increment_; - const lc::Time start_time_; - const int num_measurements_; -}; - -class ConstantAngularVelocityTest : public ::testing::Test { - public: - // Start at time increment so first IMU measurement is after starting combined nav state time - ConstantAngularVelocityTest() - : angular_velocity_i_(0.1), - angular_velocity_(angular_velocity_i_, angular_velocity_i_, angular_velocity_i_), - time_increment_(1.0 / 125.0), - start_time_(time_increment_), - num_measurements_(20) { - const ia::ImuAugmentorParams params = ia::DefaultImuAugmentorParams(); - imu_augmentor_.reset(new ia::ImuAugmentor(params)); - } - - void SetUp() final { - imu_measurements_ = - ia::ConstantAngularVelocityMeasurements(angular_velocity_, num_measurements_, start_time_, time_increment_); - for (const auto& imu_measurement : imu_measurements_) { - imu_augmentor_->BufferImuMeasurement(imu_measurement); - } - } - - ia::ImuAugmentor& imu_augmentor() { return *imu_augmentor_; } - - double angular_velocity_i() { return angular_velocity_i_; } - - const Eigen::Vector3d& angular_velocity() { return angular_velocity_; } - - double time_increment() { return time_increment_; } - - double start_time() { return start_time_; } - - int num_measurements() { return num_measurements_; } - - const std::vector<lm::ImuMeasurement>& imu_measurements() { return imu_measurements_; } - - private: - std::unique_ptr<ia::ImuAugmentor> imu_augmentor_; - const double angular_velocity_i_; - const Eigen::Vector3d angular_velocity_; - const double time_increment_; - const lc::Time start_time_; - const int num_measurements_; - std::vector<lm::ImuMeasurement> imu_measurements_; -}; - -TEST_F(ConstantAccelerationTest, AddAllMeasurements) { - const lc::CombinedNavState initial_state(gtsam::Pose3::identity(), gtsam::Velocity3::Zero(), - gtsam::imuBias::ConstantBias(), 0); - lc::CombinedNavState imu_augmented_state = initial_state; - imu_augmentor().PimPredict(initial_state, imu_augmented_state); - - EXPECT_NEAR(imu_augmented_state.timestamp(), num_measurements() * time_increment(), 1e-6); - const double expected_velocity_i = acceleration_i() * num_measurements() * time_increment(); - const gtsam::Vector3 expected_velocity(expected_velocity_i, expected_velocity_i, expected_velocity_i); - EXPECT_MATRIX_NEAR(imu_augmented_state.velocity(), expected_velocity, 1e-6); - // x = 1/2*a*t^2 - const double expected_position_i = acceleration_i() * 0.5 * std::pow(num_measurements() * time_increment(), 2); - const gtsam::Vector3 expected_position(expected_position_i, expected_position_i, expected_position_i); - EXPECT_MATRIX_NEAR(imu_augmented_state.pose().translation(), expected_position, 1e-6); -} - -TEST_F(ConstantAccelerationTest, AddHalfOfMeasurements) { - const lc::CombinedNavState initial_state(gtsam::Pose3::identity(), gtsam::Velocity3::Zero(), - gtsam::imuBias::ConstantBias(), 0); - const lc::Time imu_augmented_state_start_time = num_measurements() / 2 * time_increment(); - lc::CombinedNavState imu_augmented_state(gtsam::Pose3::identity(), gtsam::Velocity3::Zero(), - gtsam::imuBias::ConstantBias(), imu_augmented_state_start_time); - imu_augmentor().PimPredict(initial_state, imu_augmented_state); - - EXPECT_NEAR(imu_augmented_state.timestamp(), num_measurements() * time_increment(), 1e-6); - const double expected_velocity_i = acceleration_i() * num_measurements() / 2 * time_increment(); - const gtsam::Vector3 expected_velocity(expected_velocity_i, expected_velocity_i, expected_velocity_i); - EXPECT_MATRIX_NEAR(imu_augmented_state.velocity(), expected_velocity, 1e-6); - // x = 1/2*a*t^2 - const double expected_position_i = acceleration_i() * 0.5 * std::pow(num_measurements() / 2 * time_increment(), 2); - const gtsam::Vector3 expected_position(expected_position_i, expected_position_i, expected_position_i); - EXPECT_MATRIX_NEAR(imu_augmented_state.pose().translation(), expected_position, 1e-6); -} - -TEST_F(ConstantAccelerationTest, AddAllMeasurementsWithAccelBias) { - const lc::CombinedNavState initial_state(gtsam::Pose3::identity(), gtsam::Velocity3::Zero(), - gtsam::imuBias::ConstantBias(acceleration(), gtsam::Vector3::Zero()), 0); - lc::CombinedNavState imu_augmented_state = initial_state; - imu_augmentor().PimPredict(initial_state, imu_augmented_state); - - EXPECT_NEAR(imu_augmented_state.timestamp(), num_measurements() * time_increment(), 1e-6); - EXPECT_MATRIX_NEAR(imu_augmented_state.velocity(), gtsam::Vector3::Zero(), 1e-6); - EXPECT_MATRIX_NEAR(imu_augmented_state.pose().translation(), gtsam::Vector3::Zero(), 1e-6); -} - -TEST_F(ConstantAngularVelocityTest, AddAllMeasurements) { - const lc::CombinedNavState initial_state(gtsam::Pose3::identity(), gtsam::Velocity3::Zero(), - gtsam::imuBias::ConstantBias(), 0); - lc::CombinedNavState imu_augmented_state = initial_state; - imu_augmentor().PimPredict(initial_state, imu_augmented_state); - - EXPECT_NEAR(imu_augmented_state.timestamp(), num_measurements() * time_increment(), 1e-6); - gtsam::Rot3 expected_orientation = - ia::IntegrateAngularVelocities(imu_measurements(), gtsam::Rot3::identity(), initial_state.timestamp()); - EXPECT_MATRIX_NEAR(imu_augmented_state.pose().rotation(), expected_orientation, 1e-6); -} - -TEST_F(ConstantAngularVelocityTest, AddHalfOfMeasurements) { - const lc::CombinedNavState initial_state(gtsam::Pose3::identity(), gtsam::Velocity3::Zero(), - gtsam::imuBias::ConstantBias(), 0); - const lc::Time imu_augmented_state_start_time = num_measurements() / 2 * time_increment(); - lc::CombinedNavState imu_augmented_state(gtsam::Pose3::identity(), gtsam::Velocity3::Zero(), - gtsam::imuBias::ConstantBias(), imu_augmented_state_start_time); - imu_augmentor().PimPredict(initial_state, imu_augmented_state); - - EXPECT_NEAR(imu_augmented_state.timestamp(), num_measurements() * time_increment(), 1e-6); - gtsam::Rot3 expected_orientation = - ia::IntegrateAngularVelocities(imu_measurements(), gtsam::Rot3::identity(), imu_augmented_state_start_time); - EXPECT_MATRIX_NEAR(imu_augmented_state.pose().rotation(), expected_orientation, 1e-6); -} - -TEST_F(ConstantAngularVelocityTest, AddAllMeasurementsWithAccelBias) { - const lc::CombinedNavState initial_state(gtsam::Pose3::identity(), gtsam::Velocity3::Zero(), - gtsam::imuBias::ConstantBias(gtsam::Vector3::Zero(), angular_velocity()), 0); - lc::CombinedNavState imu_augmented_state = initial_state; - imu_augmentor().PimPredict(initial_state, imu_augmented_state); - - EXPECT_NEAR(imu_augmented_state.timestamp(), num_measurements() * time_increment(), 1e-6); - EXPECT_MATRIX_NEAR(imu_augmented_state.pose().rotation(), gtsam::Rot3::identity(), 1e-6); -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/localization/imu_integration/CMakeLists.txt b/localization/imu_integration/CMakeLists.txt index 1a7b581447..88d2de82c9 100644 --- a/localization/imu_integration/CMakeLists.txt +++ b/localization/imu_integration/CMakeLists.txt @@ -55,12 +55,23 @@ add_library(${PROJECT_NAME} src/identity_filter.cc src/imu_filter.cc src/imu_integrator.cc - src/latest_imu_integrator.cc + src/parameter_reader.cc + src/test_utilities.cc src/utilities.cc ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_imu_integrator + test/test_imu_integrator.test + test/test_imu_integrator.cc + ) + target_link_libraries(test_imu_integrator + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) +endif() ############# ## Install ## diff --git a/localization/imu_integration/include/imu_integration/imu_integrator.h b/localization/imu_integration/include/imu_integration/imu_integrator.h index 5194f5663f..5f57793219 100644 --- a/localization/imu_integration/include/imu_integration/imu_integrator.h +++ b/localization/imu_integration/include/imu_integration/imu_integrator.h @@ -24,6 +24,7 @@ #include <localization_common/combined_nav_state.h> #include <localization_common/time.h> +#include <localization_common/timestamped_set.h> #include <localization_measurements/fan_speed_mode.h> #include <localization_measurements/imu_measurement.h> @@ -36,51 +37,40 @@ namespace imu_integration { // Integrates imu measurements and propagates uncertainties. // Maintains a window of measurements so that any interval of measurements in // that window can be integrated into a pim. -class ImuIntegrator { +class ImuIntegrator : public localization_common::TimestampedSet<localization_measurements::ImuMeasurement> { public: explicit ImuIntegrator(const ImuIntegratorParams& params = ImuIntegratorParams()); - // Buffers imu measurement so they can be integrated when needed. - // Delayed integration useful so imu integation does not advance - // past latest sensor measurement timestamps. - void BufferImuMeasurement(const localization_measurements::ImuMeasurement& imu_measurement); - - // Returns last integrated measurement timestamp. - boost::optional<localization_common::Time> IntegrateImuMeasurements( - const localization_common::Time start_time, const localization_common::Time end_time, - gtsam::PreintegratedCombinedMeasurements& pim) const; + // Buffers an imu measurement. + void AddImuMeasurement(const localization_measurements::ImuMeasurement& imu_measurement); + // Creates an integrated GTSAM Pim using the provided bias and imu measurements + // between the provided start and end times. boost::optional<gtsam::PreintegratedCombinedMeasurements> IntegratedPim( const gtsam::imuBias::ConstantBias& bias, const localization_common::Time start_time, - const localization_common::Time end_time, - boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> params) const; - - void RemoveOldMeasurements(const localization_common::Time new_start_time); - - boost::optional<localization_common::Time> OldestTime() const; - - boost::optional<localization_common::Time> LatestTime() const; - - boost::optional<localization_measurements::ImuMeasurement> LatestMeasurement() const; + const localization_common::Time end_time) const; - bool Empty() const; + // Extrapolates the provided combined nav state using imu measurements up to the provided + // end time. + boost::optional<localization_common::CombinedNavState> Extrapolate( + const localization_common::CombinedNavState& combined_nav_state, const localization_common::Time end_time) const; - int Size() const; + // Extrapolates using all IMU measurements more recent than the combined nav state + boost::optional<localization_common::CombinedNavState> ExtrapolateLatest( + const localization_common::CombinedNavState& combined_nav_state) const; void SetFanSpeedMode(const localization_measurements::FanSpeedMode fan_speed_mode); localization_measurements::FanSpeedMode fan_speed_mode() const; - boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> pim_params() const; - - bool WithinBounds(const localization_common::Time timestamp); - - const std::map<localization_common::Time, localization_measurements::ImuMeasurement>& measurements() const; - private: + // Returns last integrated measurement timestamp. + boost::optional<localization_common::Time> IntegrateImuMeasurements( + const localization_common::Time start_time, const localization_common::Time end_time, + gtsam::PreintegratedCombinedMeasurements& pim) const; + ImuIntegratorParams params_; boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> pim_params_; - std::map<localization_common::Time, localization_measurements::ImuMeasurement> measurements_; std::unique_ptr<DynamicImuFilter> imu_filter_; }; } // namespace imu_integration diff --git a/localization/imu_integration/include/imu_integration/latest_imu_integrator.h b/localization/imu_integration/include/imu_integration/latest_imu_integrator.h deleted file mode 100644 index 9881b990db..0000000000 --- a/localization/imu_integration/include/imu_integration/latest_imu_integrator.h +++ /dev/null @@ -1,48 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef IMU_INTEGRATION_LATEST_IMU_INTEGRATOR_H_ -#define IMU_INTEGRATION_LATEST_IMU_INTEGRATOR_H_ - -#include <imu_integration/imu_integrator.h> -#include <imu_integration/latest_imu_integrator_params.h> - -#include <memory> - -namespace imu_integration { -// Adds functionality to imu integrator to integrate latest measurements, meaning it keeps track of -// last integrated measurement and only integrates measurements more recent than that measurement. -class LatestImuIntegrator : public ImuIntegrator { - public: - explicit LatestImuIntegrator(const LatestImuIntegratorParams& params = LatestImuIntegratorParams()); - - const gtsam::PreintegratedCombinedMeasurements& pim() const; - - void ResetPimIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& bias); - - // Integrates all imu measurements that have not been added up to end_time. - bool IntegrateLatestImuMeasurements(const localization_common::Time end_time); - - private: - LatestImuIntegratorParams params_; - std::unique_ptr<gtsam::PreintegratedCombinedMeasurements> pim_; - localization_common::Time last_added_imu_measurement_time_; -}; -} // namespace imu_integration - -#endif // IMU_INTEGRATION_LATEST_IMU_INTEGRATOR_H_ diff --git a/localization/imu_integration/include/imu_integration/latest_imu_integrator_params.h b/localization/imu_integration/include/imu_integration/parameter_reader.h similarity index 62% rename from localization/imu_integration/include/imu_integration/latest_imu_integrator_params.h rename to localization/imu_integration/include/imu_integration/parameter_reader.h index 7ba5e4630b..bc5ebdd112 100644 --- a/localization/imu_integration/include/imu_integration/latest_imu_integrator_params.h +++ b/localization/imu_integration/include/imu_integration/parameter_reader.h @@ -15,18 +15,23 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef IMU_INTEGRATION_LATEST_IMU_INTEGRATOR_PARAMS_H_ -#define IMU_INTEGRATION_LATEST_IMU_INTEGRATOR_PARAMS_H_ +#ifndef IMU_INTEGRATION_PARAMETER_READER_H_ +#define IMU_INTEGRATION_PARAMETER_READER_H_ + +#include <config_reader/config_reader.h> +#include <imu_integration/filter.h> #include <imu_integration/imu_integrator_params.h> +#include <imu_integration/imu_filter_params.h> -#include <gtsam/navigation/ImuBias.h> +#include <string> namespace imu_integration { -struct LatestImuIntegratorParams : public ImuIntegratorParams { - double start_time; - gtsam::imuBias::ConstantBias initial_imu_bias; -}; +void LoadImuIntegratorParams(config_reader::ConfigReader& config, ImuIntegratorParams& params); + +void LoadImuFilterParams(config_reader::ConfigReader& config, ImuFilterParams& params); + +std::unique_ptr<Filter> LoadFilter(const std::string& filter_type); } // namespace imu_integration -#endif // IMU_INTEGRATION_LATEST_IMU_INTEGRATOR_PARAMS_H_ +#endif // IMU_INTEGRATION_PARAMETER_READER_H_ diff --git a/localization/imu_integration/include/imu_integration/test_utilities.h b/localization/imu_integration/include/imu_integration/test_utilities.h new file mode 100644 index 0000000000..7d06b6435f --- /dev/null +++ b/localization/imu_integration/include/imu_integration/test_utilities.h @@ -0,0 +1,45 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef IMU_INTEGRATION_TEST_UTILITIES_H_ +#define IMU_INTEGRATION_TEST_UTILITIES_H_ + +#include <imu_integration/imu_integrator.h> +#include <imu_integration/imu_integrator_params.h> +#include <localization_common/combined_nav_state.h> +#include <localization_measurements/imu_measurement.h> + +#include <vector> + +namespace imu_integration { +ImuIntegratorParams DefaultImuIntegratorParams(); + +std::vector<localization_measurements::ImuMeasurement> ConstantMeasurements(const Eigen::Vector3d& acceleration, + const Eigen::Vector3d& angular_velocity, + const int num_measurements, + const localization_common::Time start_time, + const double time_increment); + +gtsam::Rot3 IntegrateAngularVelocities(const std::vector<localization_measurements::ImuMeasurement>& imu_measurements, + const gtsam::Rot3& starting_orientation, + const localization_common::Time starting_time); + +sensor_msgs::Imu ImuMsg(const localization_measurements::ImuMeasurement& imu_measurement); +} // namespace imu_integration + +#endif // IMU_INTEGRATION_TEST_UTILITIES_H_ diff --git a/localization/imu_integration/include/imu_integration/utilities.h b/localization/imu_integration/include/imu_integration/utilities.h index e837b5669e..4490c3d1b6 100644 --- a/localization/imu_integration/include/imu_integration/utilities.h +++ b/localization/imu_integration/include/imu_integration/utilities.h @@ -19,19 +19,15 @@ #ifndef IMU_INTEGRATION_UTILITIES_H_ #define IMU_INTEGRATION_UTILITIES_H_ -#include <config_reader/config_reader.h> -#include <imu_integration/imu_integrator_params.h> #include <imu_integration/filter.h> #include <localization_common/combined_nav_state.h> #include <localization_measurements/imu_measurement.h> #include <gtsam/inference/Symbol.h> -#include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/navigation/ImuBias.h> #include <Eigen/Core> -#include <string> #include <vector> namespace imu_integration { @@ -50,15 +46,6 @@ void AddMeasurement(const localization_measurements::ImuMeasurement& imu_measure localization_common::CombinedNavState PimPredict(const localization_common::CombinedNavState& combined_nav_state, const gtsam::PreintegratedCombinedMeasurements& pim); - -gtsam::CombinedImuFactor::shared_ptr MakeCombinedImuFactor(const int key_index_0, const int key_index_1, - const gtsam::PreintegratedCombinedMeasurements& pim); - -void LoadImuIntegratorParams(config_reader::ConfigReader& config, ImuIntegratorParams& params); - -void LoadImuFilterParams(config_reader::ConfigReader& config, ImuFilterParams& params); - -std::unique_ptr<Filter> LoadFilter(const std::string& filter_type); } // namespace imu_integration #endif // IMU_INTEGRATION_UTILITIES_H_ diff --git a/localization/imu_integration/src/dynamic_imu_filter.cc b/localization/imu_integration/src/dynamic_imu_filter.cc index e858411824..e2c88cf510 100644 --- a/localization/imu_integration/src/dynamic_imu_filter.cc +++ b/localization/imu_integration/src/dynamic_imu_filter.cc @@ -16,6 +16,7 @@ * under the License. */ #include <imu_integration/dynamic_imu_filter.h> +#include <imu_integration/parameter_reader.h> #include <imu_integration/utilities.h> #include <localization_common/logger.h> @@ -35,7 +36,6 @@ boost::optional<lm::ImuMeasurement> DynamicImuFilter::AddMeasurement(const lm::I const double filtered_angular_velocity_z = angular_velocity_z_filter_->AddValue(imu_measurement.angular_velocity.z()); // Use original timestamp - // TODO(rsoussan): incorporate phase delay into timestamp? auto filtered_imu_measurement = imu_measurement; filtered_imu_measurement.acceleration << filtered_acceleration_x, filtered_acceleration_y, filtered_acceleration_z; filtered_imu_measurement.angular_velocity << filtered_angular_velocity_x, filtered_angular_velocity_y, diff --git a/localization/imu_integration/src/imu_filter.cc b/localization/imu_integration/src/imu_filter.cc index 05a85bc520..ec19bec26e 100644 --- a/localization/imu_integration/src/imu_filter.cc +++ b/localization/imu_integration/src/imu_filter.cc @@ -16,6 +16,7 @@ * under the License. */ #include <imu_integration/imu_filter.h> +#include <imu_integration/parameter_reader.h> #include <imu_integration/utilities.h> #include <localization_common/logger.h> @@ -40,7 +41,6 @@ boost::optional<lm::ImuMeasurement> ImuFilter::AddMeasurement(const lm::ImuMeasu const double filtered_angular_velocity_z = angular_velocity_z_filter_->AddValue(imu_measurement.angular_velocity.z()); // Use original timestamp - // TODO(rsoussan): incorporate phase delay into timestamp? auto filtered_imu_measurement = imu_measurement; filtered_imu_measurement.acceleration << filtered_acceleration_x, filtered_acceleration_y, filtered_acceleration_z; filtered_imu_measurement.angular_velocity << filtered_angular_velocity_x, filtered_angular_velocity_y, diff --git a/localization/imu_integration/src/imu_integrator.cc b/localization/imu_integration/src/imu_integrator.cc index 8dc4a5c37f..cbafff7fa2 100644 --- a/localization/imu_integration/src/imu_integrator.cc +++ b/localization/imu_integration/src/imu_integrator.cc @@ -41,25 +41,24 @@ ImuIntegrator::ImuIntegrator(const ImuIntegratorParams& params) : params_(params pim_params_->setBodyPSensor(params_.body_T_imu); } -void ImuIntegrator::BufferImuMeasurement(const lm::ImuMeasurement& imu_measurement) { +void ImuIntegrator::AddImuMeasurement(const lm::ImuMeasurement& imu_measurement) { const auto filtered_imu_measurement = imu_filter_->AddMeasurement(imu_measurement); if (filtered_imu_measurement) { - // TODO(rsoussan): Prevent measurements_ from growing too large, add optional window size - measurements_.emplace(filtered_imu_measurement->timestamp, *filtered_imu_measurement); + Add(filtered_imu_measurement->timestamp, *filtered_imu_measurement); } } boost::optional<lc::Time> ImuIntegrator::IntegrateImuMeasurements(const lc::Time start_time, const lc::Time end_time, gtsam::PreintegratedCombinedMeasurements& pim) const { - if (measurements_.size() < 2) { + if (size() < 2) { LogError("IntegrateImuMeasurements: Less than 2 measurements available."); return boost::none; } - if (end_time < measurements_.cbegin()->first) { - LogDebug("IntegrateImuMeasurements: End time occurs before first measurement."); + if (end_time < *OldestTimestamp()) { + LogError("IntegrateImuMeasurements: End time occurs before first measurement."); return boost::none; } - if (end_time > measurements_.crbegin()->first) { + if (end_time > *LatestTimestamp()) { LogError("IntegrateImuMeasurements: End time occurs after last measurement."); return boost::none; } @@ -71,10 +70,10 @@ boost::optional<lc::Time> ImuIntegrator::IntegrateImuMeasurements(const lc::Time // Start with least upper bound measurement // Don't add measurements with same timestamp as start_time // since these would have a dt of 0 (wrt the pim start time) and cause errors for the pim - auto measurement_it = measurements_.upper_bound(start_time); + auto measurement_it = set().upper_bound(start_time); lc::Time last_added_imu_measurement_time = start_time; int num_measurements_added = 0; - for (; measurement_it != measurements_.cend() && measurement_it->first <= end_time; ++measurement_it) { + for (; measurement_it != set().cend() && measurement_it->first <= end_time; ++measurement_it) { AddMeasurement(measurement_it->second, last_added_imu_measurement_time, pim); ++num_measurements_added; } @@ -100,92 +99,52 @@ boost::optional<lc::Time> ImuIntegrator::IntegrateImuMeasurements(const lc::Time LogDebug( "IntegrateImuMeasurements: Total Num Imu Measurements after " "integrating: " - << measurements_.size()); + << size()); return last_added_imu_measurement_time; } -void ImuIntegrator::RemoveOldMeasurements(const lc::Time new_start_time) { - const auto lower_bound_it = measurements_.lower_bound(new_start_time); - // Every element is too old - if (lower_bound_it == measurements_.cend()) { - measurements_.clear(); - return; - } - // No elements are too old - if (lower_bound_it == measurements_.cbegin()) return; - - // Keep one before new_start_time so measurements before lower_bound_it can be interpolated if necessary - const auto new_oldest_measurement_it = std::prev(lower_bound_it); - measurements_.erase(measurements_.begin(), new_oldest_measurement_it); -} - boost::optional<gtsam::PreintegratedCombinedMeasurements> ImuIntegrator::IntegratedPim( - const gtsam::imuBias::ConstantBias& bias, const lc::Time start_time, const lc::Time end_time, - boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> params) const { - auto pim = Pim(bias, params); + const gtsam::imuBias::ConstantBias& bias, const lc::Time start_time, const lc::Time end_time) const { + auto pim = Pim(bias, pim_params_); const auto last_integrated_measurement_time = IntegrateImuMeasurements(start_time, end_time, pim); if (!last_integrated_measurement_time) { - LogDebug("IntegratedPim: Failed to integrate imu measurments."); + LogError("IntegratedPim: Failed to integrate imu measurments."); return boost::none; } return pim; } -void ImuIntegrator::SetFanSpeedMode(const lm::FanSpeedMode fan_speed_mode) { - imu_filter_->SetFanSpeedMode(fan_speed_mode); -} - -lm::FanSpeedMode ImuIntegrator::fan_speed_mode() const { return imu_filter_->fan_speed_mode(); } - -boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> ImuIntegrator::pim_params() const { - // Make a copy so internal params aren't exposed. Gtsam expects a point to a - // non const type, so can't pass a pointer to const. - boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> copied_params( - new gtsam::PreintegratedCombinedMeasurements::Params(*pim_params_)); - return copied_params; -} - -boost::optional<lc::Time> ImuIntegrator::OldestTime() const { - if (Empty()) { - LogError("OldestTime: No measurements available."); +boost::optional<lc::CombinedNavState> ImuIntegrator::Extrapolate(const lc::CombinedNavState& combined_nav_state, + const lc::Time end_time) const { + const auto pim = IntegratedPim(combined_nav_state.bias(), combined_nav_state.timestamp(), end_time); + if (!pim) { + LogError("Extrapolate: Failed to get pim."); return boost::none; } - return measurements_.cbegin()->first; + + return PimPredict(combined_nav_state, *pim); } -boost::optional<lc::Time> ImuIntegrator::LatestTime() const { - if (Empty()) { - LogError("LatestTime: No measurements available."); +boost::optional<localization_common::CombinedNavState> ImuIntegrator::ExtrapolateLatest( + const localization_common::CombinedNavState& combined_nav_state) const { + const auto latest = Latest(); + if (!latest) { + LogError("ExtrapolateLatest: Failed to get latest measurement."); return boost::none; } - return measurements_.crbegin()->first; -} -boost::optional<lm::ImuMeasurement> ImuIntegrator::LatestMeasurement() const { - if (Empty()) { - LogError("LatestTime: No measurements available."); + const auto extrapolated_combined_nav_state = Extrapolate(combined_nav_state, latest->timestamp); + if (!extrapolated_combined_nav_state) { + LogError("ExtrapolateLatest: Failed to extrapolate combined nav state."); return boost::none; } - return measurements_.crbegin()->second; -} - -bool ImuIntegrator::Empty() const { return measurements_.empty(); } -int ImuIntegrator::Size() const { return measurements_.size(); } - -bool ImuIntegrator::WithinBounds(const localization_common::Time timestamp) { - const auto oldest_time = OldestTime(); - const auto latest_time = LatestTime(); - if (!oldest_time || !latest_time) { - LogError("WithinBounds: Failed to get time bounds."); - return false; - } - return (timestamp >= *oldest_time && timestamp <= *latest_time); + return *extrapolated_combined_nav_state; } -const std::map<localization_common::Time, localization_measurements::ImuMeasurement>& ImuIntegrator::measurements() - const { - return measurements_; +void ImuIntegrator::SetFanSpeedMode(const lm::FanSpeedMode fan_speed_mode) { + imu_filter_->SetFanSpeedMode(fan_speed_mode); } +lm::FanSpeedMode ImuIntegrator::fan_speed_mode() const { return imu_filter_->fan_speed_mode(); } } // namespace imu_integration diff --git a/localization/imu_integration/src/latest_imu_integrator.cc b/localization/imu_integration/src/latest_imu_integrator.cc deleted file mode 100644 index 4450f9af53..0000000000 --- a/localization/imu_integration/src/latest_imu_integrator.cc +++ /dev/null @@ -1,60 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <imu_integration/latest_imu_integrator.h> -#include <localization_common/logger.h> - -namespace imu_integration { -namespace lc = localization_common; -namespace lm = localization_measurements; -LatestImuIntegrator::LatestImuIntegrator(const LatestImuIntegratorParams& params) - : ImuIntegrator(params), params_(params), last_added_imu_measurement_time_(0) { - pim_.reset(new gtsam::PreintegratedCombinedMeasurements(pim_params())); - ResetPimIntegrationAndSetBias(params_.initial_imu_bias); -} - -const gtsam::PreintegratedCombinedMeasurements& LatestImuIntegrator::pim() const { return *pim_; } - -void LatestImuIntegrator::ResetPimIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& bias) { - pim_->resetIntegrationAndSetBias(bias); -} - -bool LatestImuIntegrator::IntegrateLatestImuMeasurements(const lc::Time end_time) { - if (Size() < 2) { - LogError( - "IntegrateLatestImuMeasurements: Less than 2 measurements " - "available."); - return false; - } - - if (last_added_imu_measurement_time_ == 0) { - LogDebug("IntegrateLatestImuMeasurements: Adding first imu measurement."); - last_added_imu_measurement_time_ = params_.start_time; - } - - const auto last_added_imu_measurement_time = - IntegrateImuMeasurements(last_added_imu_measurement_time_, end_time, *pim_); - if (!last_added_imu_measurement_time) { - LogError("IntegrateLatestImuMeasurements: Failed to integrate measurements."); - return false; - } - - last_added_imu_measurement_time_ = *last_added_imu_measurement_time; - return true; -} -} // namespace imu_integration diff --git a/localization/imu_integration/src/parameter_reader.cc b/localization/imu_integration/src/parameter_reader.cc new file mode 100644 index 0000000000..86f192ebf0 --- /dev/null +++ b/localization/imu_integration/src/parameter_reader.cc @@ -0,0 +1,109 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <imu_integration/parameter_reader.h> +#include <imu_integration/butterO1.h> +#include <imu_integration/butterO3.h> +#include <imu_integration/butterO5.h> +#include <imu_integration/butterO7.h> +#include <imu_integration/butterO10.h> +#include <imu_integration/identity_filter.h> +#include <localization_common/logger.h> +#include <localization_common/utilities.h> +#include <msg_conversions/msg_conversions.h> + +namespace imu_integration { +namespace lc = localization_common; +namespace mc = msg_conversions; +void LoadImuIntegratorParams(config_reader::ConfigReader& config, ImuIntegratorParams& params) { + params.gravity = lc::LoadVector3(config, "world_gravity_vector"); + const bool ignore_gravity = mc::LoadBool(config, "ignore_gravity"); + if (ignore_gravity) params.gravity = gtsam::Vector3::Zero(); + params.body_T_imu = lc::LoadTransform(config, "imu_transform"); + LoadImuFilterParams(config, params.filter); + params.gyro_sigma = mc::LoadDouble(config, "gyro_sigma"); + params.accel_sigma = mc::LoadDouble(config, "accel_sigma"); + params.accel_bias_sigma = mc::LoadDouble(config, "accel_bias_sigma"); + params.gyro_bias_sigma = mc::LoadDouble(config, "gyro_bias_sigma"); + params.integration_variance = mc::LoadDouble(config, "integration_variance"); + params.bias_acc_omega_int = mc::LoadDouble(config, "bias_acc_omega_int"); +} + +void LoadImuFilterParams(config_reader::ConfigReader& config, ImuFilterParams& params) { + params.quiet_accel = mc::LoadString(config, "imu_filter_quiet_accel"); + params.quiet_ang_vel = mc::LoadString(config, "imu_filter_quiet_ang_vel"); + params.nominal_accel = mc::LoadString(config, "imu_filter_nominal_accel"); + params.nominal_ang_vel = mc::LoadString(config, "imu_filter_nominal_ang_vel"); + params.aggressive_accel = mc::LoadString(config, "imu_filter_aggressive_accel"); + params.aggressive_ang_vel = mc::LoadString(config, "imu_filter_aggressive_ang_vel"); +} + +std::unique_ptr<Filter> LoadFilter(const std::string& filter_type) { + // 1st Order + if (filter_type == "ButterO1S62_5Lp3N29_16") { + return std::unique_ptr<Filter>(new ButterO1S62_5Lp3N29_16()); + } else if (filter_type == "ButterO1S62_5Lp3N20_83") { + return std::unique_ptr<Filter>(new ButterO1S62_5Lp3N20_83()); + } else if (filter_type == "ButterO1S62_5Lp3N15_83") { + return std::unique_ptr<Filter>(new ButterO1S62_5Lp3N15_83()); + } else if (filter_type == "ButterO1S125Lp3N33_33") { // 125Hz 1st Order + return std::unique_ptr<Filter>(new ButterO1S125Lp3N33_33()); + } else if (filter_type == "ButterO1S125Lp3N41_66") { + return std::unique_ptr<Filter>(new ButterO1S125Lp3N41_66()); + } else if (filter_type == "ButterO1S125Lp3N46_66") { + return std::unique_ptr<Filter>(new ButterO1S125Lp3N46_66()); + } else if (filter_type == "ButterO3S62_5Lp3N29_16") { // 3rd Order + return std::unique_ptr<Filter>(new ButterO3S62_5Lp3N29_16()); + } else if (filter_type == "ButterO3S62_5Lp3N20_83") { + return std::unique_ptr<Filter>(new ButterO3S62_5Lp3N20_83()); + } else if (filter_type == "ButterO3S62_5Lp3N15_83") { + return std::unique_ptr<Filter>(new ButterO3S62_5Lp3N15_83()); + } else if (filter_type == "ButterO3S125Lp3N33_33") { // 125Hz 3rd Order + return std::unique_ptr<Filter>(new ButterO3S125Lp3N33_33()); + } else if (filter_type == "ButterO3S125Lp3N41_66") { + return std::unique_ptr<Filter>(new ButterO3S125Lp3N41_66()); + } else if (filter_type == "ButterO3S125Lp3N46_66") { + return std::unique_ptr<Filter>(new ButterO3S125Lp3N46_66()); + } else if (filter_type == "ButterO5S62_5Lp3N29_16") { // 5th Order + return std::unique_ptr<Filter>(new ButterO5S62_5Lp3N29_16()); + } else if (filter_type == "ButterO5S62_5Lp3N20_83") { + return std::unique_ptr<Filter>(new ButterO5S62_5Lp3N20_83()); + } else if (filter_type == "ButterO5S62_5Lp3N15_83") { + return std::unique_ptr<Filter>(new ButterO5S62_5Lp3N15_83()); + } else if (filter_type == "ButterO5S125Lp3N41_66") { // 125Hz + return std::unique_ptr<Filter>(new ButterO5S125Lp3N41_66()); + } else if (filter_type == "ButterO5S125Lp3N46_66") { + return std::unique_ptr<Filter>(new ButterO5S125Lp3N46_66()); + } else if (filter_type == "ButterO5S125Lp3N33_33") { + return std::unique_ptr<Filter>(new ButterO5S125Lp3N33_33()); + } else if (filter_type == "ButterO7S62_5Lp3N20_83") { // 7th Order + return std::unique_ptr<Filter>(new ButterO7S62_5Lp3N20_83()); + } else if (filter_type == "ButterO10S62_5Lp3N20_83") { // 10th Order + return std::unique_ptr<Filter>(new ButterO10S62_5Lp3N20_83()); + } else if (filter_type == "ButterO5S62_5Lp1N29_16") { // Lower pass band + return std::unique_ptr<Filter>(new ButterO5S62_5Lp1N29_16()); + } else if (filter_type == "ButterO5S62_5Lp0_5N29_16") { + return std::unique_ptr<Filter>(new ButterO5S62_5Lp0_5N29_16()); + } else if (filter_type == "none") { + return std::unique_ptr<Filter>(new IdentityFilter()); + } else { + LogFatal("LoadFilter: Invalid filter selection."); + return std::unique_ptr<Filter>(new IdentityFilter()); + } +} +} // namespace imu_integration diff --git a/localization/imu_integration/src/test_utilities.cc b/localization/imu_integration/src/test_utilities.cc new file mode 100644 index 0000000000..2dbb62b846 --- /dev/null +++ b/localization/imu_integration/src/test_utilities.cc @@ -0,0 +1,79 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <imu_integration/test_utilities.h> +#include <localization_common/logger.h> +#include <localization_common/utilities.h> +#include <msg_conversions/msg_conversions.h> + +namespace imu_integration { +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace mc = msg_conversions; + +ImuIntegratorParams DefaultImuIntegratorParams() { + ImuIntegratorParams params; + params.gravity = gtsam::Vector3::Zero(); + params.body_T_imu = gtsam::Pose3::identity(); + // Filer params are already default none + params.filter = imu_integration::ImuFilterParams(); + params.gyro_sigma = 0.1; + params.accel_sigma = 0.1; + params.accel_bias_sigma = 0.1; + params.gyro_bias_sigma = 0.1; + params.integration_variance = 0.1; + params.bias_acc_omega_int = 0.1; + return params; +} + +std::vector<lm::ImuMeasurement> ConstantMeasurements(const Eigen::Vector3d& acceleration, + const Eigen::Vector3d& angular_velocity, + const int num_measurements, const lc::Time start_time, + const double time_increment) { + std::vector<lm::ImuMeasurement> imu_measurements; + for (int i = 0; i < num_measurements; ++i) { + const lc::Time time = start_time + i * time_increment; + imu_measurements.emplace_back(lm::ImuMeasurement(acceleration, angular_velocity, time)); + } + return imu_measurements; +} + +gtsam::Rot3 IntegrateAngularVelocities(const std::vector<localization_measurements::ImuMeasurement>& imu_measurements, + const gtsam::Rot3& starting_orientation, + const localization_common::Time starting_time) { + gtsam::Rot3 integrated_orientation = starting_orientation; + lc::Time integrated_time = starting_time; + for (const auto& imu_measurement : imu_measurements) { + if (imu_measurement.timestamp <= integrated_time) continue; + const double dt = imu_measurement.timestamp - integrated_time; + integrated_time = imu_measurement.timestamp; + // TODO(rsoussan): subtract ang vel bias first!! add this as param!! + const gtsam::Rot3 orientation_update = gtsam::Rot3::Expmap(imu_measurement.angular_velocity * dt); + integrated_orientation = integrated_orientation * orientation_update; + } + return integrated_orientation; +} + +sensor_msgs::Imu ImuMsg(const localization_measurements::ImuMeasurement& imu_measurement) { + sensor_msgs::Imu imu_msg; + msg_conversions::VectorToMsg(imu_measurement.acceleration, imu_msg.linear_acceleration); + msg_conversions::VectorToMsg(imu_measurement.angular_velocity, imu_msg.angular_velocity); + lc::TimeToHeader(imu_measurement.timestamp, imu_msg.header); + return imu_msg; +} +} // namespace imu_integration diff --git a/localization/imu_integration/src/utilities.cc b/localization/imu_integration/src/utilities.cc index a510c411ce..a502b3cda0 100644 --- a/localization/imu_integration/src/utilities.cc +++ b/localization/imu_integration/src/utilities.cc @@ -17,20 +17,12 @@ */ #include <imu_integration/utilities.h> -#include <imu_integration/butterO1.h> -#include <imu_integration/butterO3.h> -#include <imu_integration/butterO5.h> -#include <imu_integration/butterO7.h> -#include <imu_integration/butterO10.h> -#include <imu_integration/identity_filter.h> #include <localization_common/logger.h> #include <localization_common/utilities.h> -#include <msg_conversions/msg_conversions.h> namespace imu_integration { namespace lc = localization_common; namespace lm = localization_measurements; -namespace mc = msg_conversions; boost::optional<lm::ImuMeasurement> Interpolate(const lm::ImuMeasurement& imu_measurement_a, const lm::ImuMeasurement& imu_measurement_b, const lc::Time timestamp) { @@ -77,88 +69,4 @@ lc::CombinedNavState PimPredict(const lc::CombinedNavState& combined_nav_state, const lc::Time timestamp = combined_nav_state.timestamp() + pim.deltaTij(); return lc::CombinedNavState(predicted_nav_state, pim.biasHat(), timestamp); } - -gtsam::CombinedImuFactor::shared_ptr MakeCombinedImuFactor(const int key_index_0, const int key_index_1, - const gtsam::PreintegratedCombinedMeasurements& pim) { - return gtsam::CombinedImuFactor::shared_ptr( - new gtsam::CombinedImuFactor(sym::P(key_index_0), sym::V(key_index_0), sym::P(key_index_1), sym::V(key_index_1), - sym::B(key_index_0), sym::B(key_index_1), pim)); -} - -void LoadImuIntegratorParams(config_reader::ConfigReader& config, ImuIntegratorParams& params) { - params.gravity = lc::LoadVector3(config, "world_gravity_vector"); - const bool ignore_gravity = mc::LoadBool(config, "ignore_gravity"); - if (ignore_gravity) params.gravity = gtsam::Vector3::Zero(); - params.body_T_imu = lc::LoadTransform(config, "imu_transform"); - LoadImuFilterParams(config, params.filter); - params.gyro_sigma = mc::LoadDouble(config, "gyro_sigma"); - params.accel_sigma = mc::LoadDouble(config, "accel_sigma"); - params.accel_bias_sigma = mc::LoadDouble(config, "accel_bias_sigma"); - params.gyro_bias_sigma = mc::LoadDouble(config, "gyro_bias_sigma"); - params.integration_variance = mc::LoadDouble(config, "integration_variance"); - params.bias_acc_omega_int = mc::LoadDouble(config, "bias_acc_omega_int"); -} - -void LoadImuFilterParams(config_reader::ConfigReader& config, ImuFilterParams& params) { - params.quiet_accel = mc::LoadString(config, "imu_filter_quiet_accel"); - params.quiet_ang_vel = mc::LoadString(config, "imu_filter_quiet_ang_vel"); - params.nominal_accel = mc::LoadString(config, "imu_filter_nominal_accel"); - params.nominal_ang_vel = mc::LoadString(config, "imu_filter_nominal_ang_vel"); - params.aggressive_accel = mc::LoadString(config, "imu_filter_aggressive_accel"); - params.aggressive_ang_vel = mc::LoadString(config, "imu_filter_aggressive_ang_vel"); -} - -std::unique_ptr<Filter> LoadFilter(const std::string& filter_type) { - // 1st Order - if (filter_type == "ButterO1S62_5Lp3N29_16") { - return std::unique_ptr<Filter>(new ButterO1S62_5Lp3N29_16()); - } else if (filter_type == "ButterO1S62_5Lp3N20_83") { - return std::unique_ptr<Filter>(new ButterO1S62_5Lp3N20_83()); - } else if (filter_type == "ButterO1S62_5Lp3N15_83") { - return std::unique_ptr<Filter>(new ButterO1S62_5Lp3N15_83()); - } else if (filter_type == "ButterO1S125Lp3N33_33") { // 125Hz 1st Order - return std::unique_ptr<Filter>(new ButterO1S125Lp3N33_33()); - } else if (filter_type == "ButterO1S125Lp3N41_66") { - return std::unique_ptr<Filter>(new ButterO1S125Lp3N41_66()); - } else if (filter_type == "ButterO1S125Lp3N46_66") { - return std::unique_ptr<Filter>(new ButterO1S125Lp3N46_66()); - } else if (filter_type == "ButterO3S62_5Lp3N29_16") { // 3rd Order - return std::unique_ptr<Filter>(new ButterO3S62_5Lp3N29_16()); - } else if (filter_type == "ButterO3S62_5Lp3N20_83") { - return std::unique_ptr<Filter>(new ButterO3S62_5Lp3N20_83()); - } else if (filter_type == "ButterO3S62_5Lp3N15_83") { - return std::unique_ptr<Filter>(new ButterO3S62_5Lp3N15_83()); - } else if (filter_type == "ButterO3S125Lp3N33_33") { // 125Hz 3rd Order - return std::unique_ptr<Filter>(new ButterO3S125Lp3N33_33()); - } else if (filter_type == "ButterO3S125Lp3N41_66") { - return std::unique_ptr<Filter>(new ButterO3S125Lp3N41_66()); - } else if (filter_type == "ButterO3S125Lp3N46_66") { - return std::unique_ptr<Filter>(new ButterO3S125Lp3N46_66()); - } else if (filter_type == "ButterO5S62_5Lp3N29_16") { // 5th Order - return std::unique_ptr<Filter>(new ButterO5S62_5Lp3N29_16()); - } else if (filter_type == "ButterO5S62_5Lp3N20_83") { - return std::unique_ptr<Filter>(new ButterO5S62_5Lp3N20_83()); - } else if (filter_type == "ButterO5S62_5Lp3N15_83") { - return std::unique_ptr<Filter>(new ButterO5S62_5Lp3N15_83()); - } else if (filter_type == "ButterO5S125Lp3N41_66") { // 125Hz - return std::unique_ptr<Filter>(new ButterO5S125Lp3N41_66()); - } else if (filter_type == "ButterO5S125Lp3N46_66") { - return std::unique_ptr<Filter>(new ButterO5S125Lp3N46_66()); - } else if (filter_type == "ButterO5S125Lp3N33_33") { - return std::unique_ptr<Filter>(new ButterO5S125Lp3N33_33()); - } else if (filter_type == "ButterO7S62_5Lp3N20_83") { // 7th Order - return std::unique_ptr<Filter>(new ButterO7S62_5Lp3N20_83()); - } else if (filter_type == "ButterO10S62_5Lp3N20_83") { // 10th Order - return std::unique_ptr<Filter>(new ButterO10S62_5Lp3N20_83()); - } else if (filter_type == "ButterO5S62_5Lp1N29_16") { // Lower pass band - return std::unique_ptr<Filter>(new ButterO5S62_5Lp1N29_16()); - } else if (filter_type == "ButterO5S62_5Lp0_5N29_16") { - return std::unique_ptr<Filter>(new ButterO5S62_5Lp0_5N29_16()); - } else if (filter_type == "none") { - return std::unique_ptr<Filter>(new IdentityFilter()); - } else { - LogFatal("LoadFilter: Invalid filter selection."); - return std::unique_ptr<Filter>(new IdentityFilter()); - } -} } // namespace imu_integration diff --git a/localization/imu_integration/test/test_imu_integrator.cc b/localization/imu_integration/test/test_imu_integrator.cc new file mode 100644 index 0000000000..7480fdfd78 --- /dev/null +++ b/localization/imu_integration/test/test_imu_integrator.cc @@ -0,0 +1,250 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <imu_integration/imu_integrator.h> +#include <imu_integration/test_utilities.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <localization_measurements/imu_measurement.h> + +#include <gtest/gtest.h> + +namespace ii = imu_integration; +namespace lc = localization_common; +namespace lm = localization_measurements; + +namespace { +// Defaults to random start pose and velocity and zero IMU/IMU bias values +struct TestParams { + Eigen::Vector3d acceleration = Eigen::Vector3d::Zero(); + Eigen::Vector3d angular_velocity = Eigen::Vector3d::Zero(); + Eigen::Vector3d accelerometer_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d gyroscope_bias = Eigen::Vector3d::Zero(); + gtsam::Pose3 body_T_sensor = gtsam::Pose3::identity(); + Eigen::Vector3d gravity = Eigen::Vector3d::Zero(); + gtsam::Pose3 initial_pose = lc::RandomPose(); + Eigen::Vector3d initial_velocity = lc::RandomVector3d(); + double integration_start_time = 0; +}; + +Eigen::Vector3d AccelerationOnlyIntegratedPosition(const TestParams& params, const double duration) { + return params.initial_pose.translation() + params.initial_velocity * duration + + params.initial_pose.rotation() * (params.acceleration - params.accelerometer_bias) * 0.5 * + std::pow(duration, 2); +} + +Eigen::Vector3d AccelerationOnlyIntegratedVelocity(const TestParams& params, const double duration) { + return params.initial_velocity + + params.initial_pose.rotation() * (params.acceleration - params.accelerometer_bias) * duration; +} +} // namespace + +class ConstantIMUTest : public ::testing::Test { + public: + // Start at time increment so first IMU measurement is after starting combined nav state time + ConstantIMUTest() : time_increment_(1.0 / 125.0), start_time_(time_increment_), num_measurements_(20) { + const ii::ImuIntegratorParams params = ii::DefaultImuIntegratorParams(); + imu_integrator_.reset(new ii::ImuIntegrator(params)); + } + + void SetUp() final {} + + void SetAndAddMeasurements(const Eigen::Vector3d& acceleration, const Eigen::Vector3d& angular_velocity) { + acceleration_ = acceleration; + angular_velocity_ = angular_velocity; + imu_measurements_ = + ii::ConstantMeasurements(acceleration_, angular_velocity_, num_measurements_, start_time_, time_increment_); + for (const auto& imu_measurement : imu_measurements_) { + imu_integrator_->AddImuMeasurement(imu_measurement); + } + } + + void SetAndAddMeasurements(const double acceleration = 0, const double angular_velocity = 0) { + SetAndAddMeasurements(Eigen::Vector3d(acceleration, acceleration, acceleration), + Eigen::Vector3d(angular_velocity, angular_velocity, angular_velocity)); + } + + void TestAccelerationOnly(const TestParams& params) { + SetAndAddMeasurements(params.acceleration, params.angular_velocity); + const lc::CombinedNavState initial_state( + params.initial_pose, params.initial_velocity, + gtsam::imuBias::ConstantBias(params.accelerometer_bias, params.gyroscope_bias), params.integration_start_time); + const auto imu_augmented_state = imu_integrator().ExtrapolateLatest(initial_state); + ASSERT_TRUE(imu_augmented_state); + const double duration = Duration() - params.integration_start_time; + EXPECT_NEAR(imu_augmented_state->timestamp(), Duration(), 1e-6); + const Eigen::Vector3d expected_velocity = AccelerationOnlyIntegratedVelocity(params, duration); + EXPECT_MATRIX_NEAR(imu_augmented_state->velocity(), expected_velocity, 1e-6); + const Eigen::Vector3d expected_position = AccelerationOnlyIntegratedPosition(params, duration); + EXPECT_MATRIX_NEAR(imu_augmented_state->pose().translation(), expected_position, 1e-6); + EXPECT_MATRIX_NEAR(imu_augmented_state->bias().accelerometer(), initial_state.bias().accelerometer(), 1e-6); + EXPECT_MATRIX_NEAR(imu_augmented_state->bias().gyroscope(), initial_state.bias().gyroscope(), 1e-6); + } + + void Test(const TestParams& params) { + SetAndAddMeasurements(params.acceleration, params.angular_velocity); + const lc::CombinedNavState initial_state( + params.initial_pose, params.initial_velocity, + gtsam::imuBias::ConstantBias(params.accelerometer_bias, params.gyroscope_bias), params.integration_start_time); + + const auto imu_augmented_state = imu_integrator().ExtrapolateLatest(initial_state); + ASSERT_TRUE(imu_augmented_state); + + const Eigen::Vector3d corrected_angular_velocity = + params.body_T_sensor.rotation() * (params.angular_velocity - params.gyroscope_bias); + const Eigen::Vector3d centrifugal_acceleration = + corrected_angular_velocity.cross(corrected_angular_velocity.cross(params.body_T_sensor.translation())); + const Eigen::Vector3d corrected_acceleration = + params.body_T_sensor.rotation() * (params.acceleration - params.accelerometer_bias) - centrifugal_acceleration; + Eigen::Vector3d velocity = params.initial_velocity; + gtsam::Pose3 pose = params.initial_pose; + for (const auto& imu_measurement : imu_measurements()) { + if (imu_measurement.timestamp <= params.integration_start_time) continue; + // Catch float equality + if (std::abs(imu_measurement.timestamp - params.integration_start_time) < 1e-6) continue; + const Eigen::Matrix3d relative_orientation = + (gtsam::Rot3::Expmap(corrected_angular_velocity * time_increment())).matrix(); + const Eigen::Vector3d relative_velocity = + pose.rotation() * (corrected_acceleration * time_increment()) + params.gravity * time_increment(); + // Convert velocity to body frame + const Eigen::Vector3d relative_translation = + pose.rotation().inverse() * (velocity * time_increment()) + + + pose.rotation().inverse() * (0.5 * params.gravity * time_increment() * time_increment()) + + 0.5 * corrected_acceleration * time_increment() * time_increment(); + velocity += relative_velocity; + const Eigen::Isometry3d relative_pose = lc::Isometry3d(relative_translation, relative_orientation); + pose = pose * lc::GtPose(relative_pose); + } + EXPECT_MATRIX_NEAR(imu_augmented_state->velocity(), velocity, 1e-6); + EXPECT_MATRIX_NEAR(imu_augmented_state->pose(), pose, 1e-6); + } + + ii::ImuIntegrator& imu_integrator() { return *imu_integrator_; } + + void SetImuIntegrator(const ii::ImuIntegratorParams& params) { imu_integrator_.reset(new ii::ImuIntegrator(params)); } + + const Eigen::Vector3d& acceleration() const { return acceleration_; } + + const Eigen::Vector3d& angular_velocity() const { return angular_velocity_; } + + const std::vector<lm::ImuMeasurement>& imu_measurements() const { return imu_measurements_; } + + double time_increment() const { return time_increment_; } + + double Duration() const { return num_measurements_ * time_increment_; } + + double start_time() const { return start_time_; } + + int num_measurements() const { return num_measurements_; } + + private: + std::unique_ptr<ii::ImuIntegrator> imu_integrator_; + Eigen::Vector3d acceleration_; + Eigen::Vector3d angular_velocity_; + std::vector<lm::ImuMeasurement> imu_measurements_; + const double time_increment_; + const lc::Time start_time_; + const int num_measurements_; +}; + +TEST_F(ConstantIMUTest, ConstVelocity) { + TestParams params; + TestAccelerationOnly(params); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstVelocityNonZeroAccBias) { + TestParams params; + params.accelerometer_bias = lc::RandomVector3d(); + TestAccelerationOnly(params); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAcc) { + TestParams params; + params.acceleration = lc::RandomVector3d(); + TestAccelerationOnly(params); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAccHalfMeasurements) { + TestParams params; + params.acceleration = lc::RandomVector3d(); + params.integration_start_time = Duration() / 2; + TestAccelerationOnly(params); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAccNonZeroAccBias) { + TestParams params; + params.acceleration = lc::RandomVector3d(); + params.accelerometer_bias = lc::RandomVector3d(); + TestAccelerationOnly(params); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAngularVelocity) { + TestParams params; + params.angular_velocity = lc::RandomVector3d(); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAngularVelocityNonZeroAngBias) { + TestParams params; + params.angular_velocity = lc::RandomVector3d(); + params.gyroscope_bias = lc::RandomVector3d(); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAccelerationAngularVelocity) { + TestParams params; + params.acceleration = lc::RandomVector3d(); + params.angular_velocity = lc::RandomVector3d(); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAccelerationAngularVelocityGravity) { + TestParams params; + params.acceleration = lc::RandomVector3d(); + params.angular_velocity = lc::RandomVector3d(); + params.gravity = lc::RandomVector3d(); + auto integrator_params = ii::DefaultImuIntegratorParams(); + integrator_params.gravity = params.gravity; + SetImuIntegrator(integrator_params); + Test(params); +} + +TEST_F(ConstantIMUTest, ConstAccelerationAngularVelocityGravitySensorOffset) { + TestParams params; + params.acceleration = lc::RandomVector3d(); + params.angular_velocity = lc::RandomVector3d(); + params.gravity = lc::RandomVector3d(); + params.body_T_sensor = lc::RandomPose(); + auto integrator_params = ii::DefaultImuIntegratorParams(); + integrator_params.gravity = params.gravity; + integrator_params.body_T_imu = params.body_T_sensor; + SetImuIntegrator(integrator_params); + Test(params); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/imu_integration/test/test_imu_integrator.test b/localization/imu_integration/test/test_imu_integrator.test new file mode 100644 index 0000000000..1f067076eb --- /dev/null +++ b/localization/imu_integration/test/test_imu_integrator.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="imu_integration" type="test_imu_integrator" test-name="test_imu_integrator" /> +</launch> diff --git a/localization/localization_common/CMakeLists.txt b/localization/localization_common/CMakeLists.txt index 4e7ce13c76..1b7acd8457 100644 --- a/localization/localization_common/CMakeLists.txt +++ b/localization/localization_common/CMakeLists.txt @@ -23,6 +23,7 @@ add_compile_options(-std=c++14) ## Find catkin macros and libraries find_package(catkin2 REQUIRED COMPONENTS + astrobee camera config_reader msg_conversions @@ -42,6 +43,7 @@ catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} ${GTSAM_LIBRARIES} gtsam CATKIN_DEPENDS + astrobee camera config_reader msg_conversions @@ -65,9 +67,13 @@ add_library(${PROJECT_NAME} src/averager.cc src/combined_nav_state.cc src/combined_nav_state_covariances.cc + src/distance_scaled_pose_covariance_interpolater.cc + src/pose_covariance_interpolater.cc src/pose_interpolater.cc + src/pose_with_covariance_interpolater.cc src/rate_timer.cc src/ros_timer.cc + src/stats_logger.cc src/time.cc src/timer.cc src/test_utilities.cc @@ -97,6 +103,14 @@ if(CATKIN_ENABLE_TESTING) ${PROJECT_NAME} ) + add_rostest_gtest(test_pose_with_covariance_interpolater + test/test_pose_with_covariance_interpolater.test + test/test_pose_with_covariance_interpolater.cc + ) + target_link_libraries(test_pose_with_covariance_interpolater + ${PROJECT_NAME} + ) + add_rostest_gtest(test_timestamped_set test/test_timestamped_set.test test/test_timestamped_set.cc diff --git a/localization/localization_common/include/localization_common/distance_scaled_pose_covariance_interpolater.h b/localization/localization_common/include/localization_common/distance_scaled_pose_covariance_interpolater.h new file mode 100644 index 0000000000..c470565ec1 --- /dev/null +++ b/localization/localization_common/include/localization_common/distance_scaled_pose_covariance_interpolater.h @@ -0,0 +1,44 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_DISTANCE_SCALED_POSE_COVARIANCE_INTERPOLATER_H_ +#define LOCALIZATION_COMMON_DISTANCE_SCALED_POSE_COVARIANCE_INTERPOLATER_H_ + +#include <localization_common/pose_covariance_interpolater.h> + +namespace localization_common { +struct DistanceScaledPoseCovarianceInterpolaterParams { + double min_covariance = 1e-4; + double translation_scale = 1; + double orientation_scale = 0.01; +}; + +class DistanceScaledPoseCovarianceInterpolater : public PoseCovarianceInterpolater { + public: + DistanceScaledPoseCovarianceInterpolater( + const DistanceScaledPoseCovarianceInterpolaterParams& params = DistanceScaledPoseCovarianceInterpolaterParams()); + + PoseCovariance Interpolate(const PoseWithCovariance& a, const PoseWithCovariance& b, const Time& timestamp_a, + const Time& timestamp_b) final; + + private: + DistanceScaledPoseCovarianceInterpolaterParams params_; +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_DISTANCE_SCALED_POSE_COVARIANCE_INTERPOLATER_H_ diff --git a/localization/localization_common/include/localization_common/marginals_pose_covariance_interpolater.h b/localization/localization_common/include/localization_common/marginals_pose_covariance_interpolater.h new file mode 100644 index 0000000000..d79e08113b --- /dev/null +++ b/localization/localization_common/include/localization_common/marginals_pose_covariance_interpolater.h @@ -0,0 +1,74 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_MARGINALS_POSE_COVARIANCE_INTERPOLATER_H_ +#define LOCALIZATION_COMMON_MARGINALS_POSE_COVARIANCE_INTERPOLATER_H_ + +#include <localization_common/pose_covariance_interpolater.h> +#include <localization_common/utilities.h> + +#include <boost/optional.hpp> +#include <gtsam/nonlinear/Marginals.h> + +namespace localization_common { +template <typename NodesT> +class MarginalsPoseCovarianceInterpolater : public PoseCovarianceInterpolater { + public: + MarginalsPoseCovarianceInterpolater(const std::shared_ptr<const NodesT>& nodes, + const boost::optional<const gtsam::Marginals&>& marginals = boost::none); + PoseCovariance Interpolate(const PoseWithCovariance& a, const PoseWithCovariance& b, const Time& timestamp_a, + const Time& timestamp_b) final; + + private: + boost::optional<const gtsam::Marginals&> marginals_; + std::shared_ptr<const NodesT> nodes_; +}; + +// Implementation +template <typename NodesT> +MarginalsPoseCovarianceInterpolater<NodesT>::MarginalsPoseCovarianceInterpolater( + const std::shared_ptr<const NodesT>& nodes, const boost::optional<const gtsam::Marginals&>& marginals) + : nodes_(nodes), marginals_(marginals) {} + +template <typename NodesT> +PoseCovariance MarginalsPoseCovarianceInterpolater<NodesT>::Interpolate(const PoseWithCovariance& a, + const PoseWithCovariance& b, + const Time& timestamp_a, + const Time& timestamp_b) { + boost::optional<PoseCovariance> covariance_a_b; + // Set covariance_a_b if available + if (marginals_ && nodes_) { + const auto closest_t_a = nodes_->ClosestTimestamp(timestamp_a); + const auto closest_t_b = nodes_->ClosestTimestamp(timestamp_b); + if (!closest_t_a || !closest_t_b) { + LogError("Interpolate: Failed to get closest timestamps."); + } else { + // TODO(rsoussan): Add fallback covariance/relative covariance if gap too large + if (std::abs(*closest_t_a - timestamp_a) > 3.0 || std::abs(*closest_t_b - timestamp_b) > 3.0) { + LogWarning("Interpolate: Timestamps far from closest timestamps available."); + } + const auto keys_a = nodes_->Keys(*closest_t_a); + const auto keys_b = nodes_->Keys(*closest_t_b); + covariance_a_b = marginals_->jointMarginalCovariance({keys_a[0], keys_b[0]}).fullMatrix(); + } + } + return RelativePoseCovariance(a, b, covariance_a_b); +} +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_MARGINALS_POSE_COVARIANCE_INTERPOLATER_H_ diff --git a/localization/localization_common/include/localization_common/measurement_buffer.h b/localization/localization_common/include/localization_common/measurement_buffer.h deleted file mode 100644 index fb1a43b430..0000000000 --- a/localization/localization_common/include/localization_common/measurement_buffer.h +++ /dev/null @@ -1,97 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef LOCALIZATION_COMMON_MEASUREMENT_BUFFER_H_ -#define LOCALIZATION_COMMON_MEASUREMENT_BUFFER_H_ - -#include <localization_common/time.h> - -#include <boost/optional.hpp> - -#include <map> - -namespace localization_common { -template <typename MeasurementType> -class MeasurementBuffer { - public: - void Add(const Time time, const MeasurementType& measurement) { measurements_.emplace(time, measurement); } - - boost::optional<const MeasurementType&> Get(const localization_common::Time time) const { - const auto measurement_it = measurements_.find(time); - if (measurement_it == measurements_.end()) return boost::none; - return measurement_it->second; - } - - boost::optional<const MeasurementType&> GetLatest() const { - if (measurements_.empty()) return boost::none; - return measurements_.crbegin()->second; - } - - boost::optional<const MeasurementType&> GetNearby(const localization_common::Time time, - const double tolerance) const { - const auto upper_bound_it = measurements_.lower_bound(time); - if (upper_bound_it == measurements_.begin()) { - if (ValidTime(upper_bound_it->first, time, tolerance)) { - return upper_bound_it->second; - } else { - return boost::none; - } - } - if (upper_bound_it == measurements_.end()) { - const auto last_measurement_it = measurements_.rbegin(); - if (ValidTime(last_measurement_it->first, time, tolerance)) { - return last_measurement_it->second; - } else { - return boost::none; - } - } - - const auto lower_bound_it = std::prev(upper_bound_it); - const double lower_bound_time_diff = std::abs(lower_bound_it->first - time); - const double upper_bound_time_diff = std::abs(upper_bound_it->first - time); - const auto closest_measurement_it = - lower_bound_time_diff <= upper_bound_time_diff ? lower_bound_it : upper_bound_it; - if (ValidTime(closest_measurement_it->first, time, tolerance)) { - return closest_measurement_it->second; - } - return boost::none; - } - - void EraseUpTo(const Time oldest_allowed_time) { - const auto measurement_it = measurements_.lower_bound(oldest_allowed_time); - measurements_.erase(measurements_.begin(), measurement_it); - } - - void EraseUpToAndIncluding(const Time oldest_allowed_time) { - const auto measurement_it = measurements_.upper_bound(oldest_allowed_time); - measurements_.erase(measurements_.begin(), measurement_it); - } - - const std::map<Time, MeasurementType>& measurements() const { return measurements_; } - - size_t size() const { return measurements_.size(); } - - private: - static bool ValidTime(const localization_common::Time time_a, const localization_common::Time time_b, - const double tolerance) { - return std::abs(time_a - time_b) <= tolerance; - } - std::map<Time, MeasurementType> measurements_; -}; -} // namespace localization_common -#endif // LOCALIZATION_COMMON_MEASUREMENT_BUFFER_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/cumulative_factor_adder.h b/localization/localization_common/include/localization_common/pose_covariance_interpolater.h similarity index 55% rename from localization/graph_optimizer/include/graph_optimizer/cumulative_factor_adder.h rename to localization/localization_common/include/localization_common/pose_covariance_interpolater.h index 5599a5803c..1431c79aa3 100644 --- a/localization/graph_optimizer/include/graph_optimizer/cumulative_factor_adder.h +++ b/localization/localization_common/include/localization_common/pose_covariance_interpolater.h @@ -16,28 +16,19 @@ * under the License. */ -#ifndef GRAPH_OPTIMIZER_CUMULATIVE_FACTOR_ADDER_H_ -#define GRAPH_OPTIMIZER_CUMULATIVE_FACTOR_ADDER_H_ +#ifndef LOCALIZATION_COMMON_POSE_COVARIANCE_INTERPOLATER_H_ +#define LOCALIZATION_COMMON_POSE_COVARIANCE_INTERPOLATER_H_ -#include <graph_optimizer/factor_to_add.h> +#include <localization_common/pose_with_covariance.h> -#include <vector> - -namespace graph_optimizer { -template <typename PARAMS> -class CumulativeFactorAdder { +namespace localization_common { +class PoseCovarianceInterpolater { public: - explicit CumulativeFactorAdder(const PARAMS& params) : params_(params) {} - - virtual ~CumulativeFactorAdder() {} - - virtual std::vector<FactorsToAdd> AddFactors() = 0; - - const PARAMS& params() const { return params_; } - - private: - PARAMS params_; + PoseCovarianceInterpolater() = default; + virtual ~PoseCovarianceInterpolater() = default; + virtual PoseCovariance Interpolate(const PoseWithCovariance& a, const PoseWithCovariance& b, const Time& timestamp_a, + const Time& timestamp_b); }; -} // namespace graph_optimizer +} // namespace localization_common -#endif // GRAPH_OPTIMIZER_CUMULATIVE_FACTOR_ADDER_H_ +#endif // LOCALIZATION_COMMON_POSE_COVARIANCE_INTERPOLATER_H_ diff --git a/localization/localization_common/include/localization_common/pose_interpolater.h b/localization/localization_common/include/localization_common/pose_interpolater.h index 1c7da81e29..242fbc02f9 100644 --- a/localization/localization_common/include/localization_common/pose_interpolater.h +++ b/localization/localization_common/include/localization_common/pose_interpolater.h @@ -19,26 +19,19 @@ #ifndef LOCALIZATION_COMMON_POSE_INTERPOLATER_H_ #define LOCALIZATION_COMMON_POSE_INTERPOLATER_H_ -#include <ff_common/eigen_vectors.h> -#include <localization_common/time.h> -#include <localization_common/timestamped_set.h> +#include <localization_common/timestamped_interpolater.h> #include <Eigen/Geometry> -#include <boost/optional.hpp> - -#include <vector> - namespace localization_common { -class PoseInterpolater : public TimestampedSet<Eigen::Isometry3d> { - public: - PoseInterpolater(const std::vector<Time>& timestamps, const std::vector<Eigen::Isometry3d>& poses); +using PoseInterpolater = TimestampedInterpolater<Eigen::Isometry3d>; - boost::optional<Eigen::Isometry3d> Interpolate(const Time timestamp) const; +template <> +Eigen::Isometry3d PoseInterpolater::Interpolate(const Eigen::Isometry3d& a, const Eigen::Isometry3d& b, + const double alpha) const; - private: - TimestampedSet timestamped_poses_; -}; +template <> +Eigen::Isometry3d PoseInterpolater::Relative(const Eigen::Isometry3d& a, const Eigen::Isometry3d& b) const; } // namespace localization_common #endif // LOCALIZATION_COMMON_POSE_INTERPOLATER_H_ diff --git a/localization/localization_common/include/localization_common/pose_with_covariance.h b/localization/localization_common/include/localization_common/pose_with_covariance.h index 177e9d20af..4e70146d53 100644 --- a/localization/localization_common/include/localization_common/pose_with_covariance.h +++ b/localization/localization_common/include/localization_common/pose_with_covariance.h @@ -19,6 +19,8 @@ #ifndef LOCALIZATION_COMMON_POSE_WITH_COVARIANCE_H_ #define LOCALIZATION_COMMON_POSE_WITH_COVARIANCE_H_ +#include <localization_common/timestamped_set.h> + #include <Eigen/Geometry> namespace localization_common { @@ -26,10 +28,16 @@ using PoseCovariance = Eigen::Matrix<double, 6, 6>; struct PoseWithCovariance { PoseWithCovariance(const Eigen::Isometry3d& pose, const PoseCovariance& covariance) : pose(pose), covariance(covariance) {} + PoseWithCovariance(const Eigen::Isometry3d& pose, const PoseCovariance& covariance, + const TimestampedSet<PoseCovariance>& correlation_covariances) + : pose(pose), covariance(covariance), correlation_covariances(correlation_covariances) {} + PoseWithCovariance() {} Eigen::Isometry3d pose; PoseCovariance covariance; + // Optional correlated covariances wrt other timestamped poses + TimestampedSet<PoseCovariance> correlation_covariances; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/localization/localization_common/include/localization_common/pose_with_covariance_interpolater.h b/localization/localization_common/include/localization_common/pose_with_covariance_interpolater.h new file mode 100644 index 0000000000..89465d1d4f --- /dev/null +++ b/localization/localization_common/include/localization_common/pose_with_covariance_interpolater.h @@ -0,0 +1,46 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_POSE_WITH_COVARIANCE_INTERPOLATER_H_ +#define LOCALIZATION_COMMON_POSE_WITH_COVARIANCE_INTERPOLATER_H_ + +#include <localization_common/pose_covariance_interpolater.h> +#include <localization_common/pose_with_covariance.h> +#include <localization_common/timestamped_interpolater.h> + +namespace localization_common { +struct PoseWithCovarianceInterpolaterParams { + PoseWithCovarianceInterpolaterParams(const std::shared_ptr<PoseCovarianceInterpolater>& pose_covariance_interpolater = + std::make_shared<PoseCovarianceInterpolater>()) + : pose_covariance_interpolater(pose_covariance_interpolater) {} + std::shared_ptr<PoseCovarianceInterpolater> pose_covariance_interpolater; +}; + +using PoseWithCovarianceInterpolater = + TimestampedInterpolater<PoseWithCovariance, PoseWithCovarianceInterpolaterParams>; + +template <> +PoseWithCovariance PoseWithCovarianceInterpolater::Interpolate(const PoseWithCovariance& a, const PoseWithCovariance& b, + const double alpha) const; + +template <> +boost::optional<PoseWithCovariance> PoseWithCovarianceInterpolater::Relative(const Time timestamp_a, + const Time timestamp_b) const; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_POSE_WITH_COVARIANCE_INTERPOLATER_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/graph_stats.h b/localization/localization_common/include/localization_common/stats_logger.h similarity index 52% rename from localization/graph_optimizer/include/graph_optimizer/graph_stats.h rename to localization/localization_common/include/localization_common/stats_logger.h index a22bc6d5f0..904b4adf5a 100644 --- a/localization/graph_optimizer/include/graph_optimizer/graph_stats.h +++ b/localization/localization_common/include/localization_common/stats_logger.h @@ -15,45 +15,41 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef GRAPH_OPTIMIZER_GRAPH_STATS_H_ -#define GRAPH_OPTIMIZER_GRAPH_STATS_H_ +#ifndef LOCALIZATION_COMMON_STATS_LOGGER_H_ +#define LOCALIZATION_COMMON_STATS_LOGGER_H_ #include <localization_common/averager.h> #include <localization_common/timer.h> -#include <gtsam/nonlinear/NonlinearFactorGraph.h> - #include <vector> -namespace graph_optimizer { -class GraphStats { +namespace localization_common { +// Logs various statistics and runtimes to a desired output (command line, CSV file, text file) using averager and timer +// objects. Uses references to provided averagers and timers to log min/max/mean/stddev values and runtimes +// respectively. +class StatsLogger { public: - GraphStats(); - void AddStatsAverager(localization_common::Averager& stats_averager); - void AddErrorAverager(localization_common::Averager& error_averager); - virtual void UpdateErrors(const gtsam::NonlinearFactorGraph& graph_factors); - virtual void UpdateStats(const gtsam::NonlinearFactorGraph& graph_factors); + explicit StatsLogger(const bool log_on_destruction = true); + + ~StatsLogger(); + + // Add averager for logging + void AddAverager(localization_common::Averager& averager); + + // Add timer for logging + void AddTimer(localization_common::Timer& timer); + + // Log averages and times to command line. void Log() const; + + // Log averages and times to a text file. void LogToFile(std::ofstream& ofstream) const; + + // Log averages and times to a CSV file. void LogToCsv(std::ofstream& ofstream) const; std::vector<std::reference_wrapper<localization_common::Timer>> timers_; - std::vector<std::reference_wrapper<localization_common::Averager>> stats_averagers_; - std::vector<std::reference_wrapper<localization_common::Averager>> error_averagers_; - - // Timers - localization_common::Timer optimization_timer_ = localization_common::Timer("Optimization"); - localization_common::Timer update_timer_ = localization_common::Timer("Update"); - localization_common::Timer marginals_timer_ = localization_common::Timer("Marginals"); - localization_common::Timer slide_window_timer_ = localization_common::Timer("Slide Window"); - localization_common::Timer add_buffered_factors_timer_ = localization_common::Timer("Add Buffered Factors"); - localization_common::Timer log_error_timer_ = localization_common::Timer("Log Error"); - localization_common::Timer log_stats_timer_ = localization_common::Timer("Log Stats"); - - // Graph Stats Averagers - localization_common::Averager iterations_averager_ = localization_common::Averager("Iterations"); - // Factor Error Averagers - localization_common::Averager total_error_averager_ = localization_common::Averager("Total Factor Error"); + std::vector<std::reference_wrapper<localization_common::Averager>> averagers_; private: template <typename Logger> @@ -70,7 +66,9 @@ class GraphStats { void Log(const std::vector<std::reference_wrapper<Logger>>& loggers) const { for (const auto& logger : loggers) logger.get().Log(); } + + bool log_on_destruction_; }; -} // namespace graph_optimizer +} // namespace localization_common -#endif // GRAPH_OPTIMIZER_GRAPH_STATS_H_ +#endif // LOCALIZATION_COMMON_STATS_LOGGER_H_ diff --git a/localization/localization_common/include/localization_common/test_utilities.h b/localization/localization_common/include/localization_common/test_utilities.h index ff84c6c5ee..116f3719dd 100644 --- a/localization/localization_common/include/localization_common/test_utilities.h +++ b/localization/localization_common/include/localization_common/test_utilities.h @@ -19,6 +19,11 @@ #ifndef LOCALIZATION_COMMON_TEST_UTILITIES_H_ #define LOCALIZATION_COMMON_TEST_UTILITIES_H_ +#include <localization_common/combined_nav_state.h> +#include <localization_common/pose_with_covariance.h> +#include <localization_common/time.h> + +#include <Eigen/Geometry> #include <gtsam/geometry/Pose3.h> #include <gtest/gtest.h> @@ -49,6 +54,8 @@ double RandomGaussianDouble(const double mean, const double stddev); bool RandomBool(); +Time RandomTime(); + // Returns white noise with set stddev double Noise(const double stddev); @@ -60,18 +67,29 @@ Eigen::Vector3d RandomVector3d(); Eigen::Vector3d RandomPoint3d(); +Eigen::Vector3d RandomVelocity(); + Eigen::Vector2d RandomVector2d(); Eigen::Vector2d RandomPoint2d(); +template <int D> +Eigen::Matrix<double, D, D> RandomCovariance(); + // Translation ranges from [-100, 100] // Rotation spans all possible rotations gtsam::Pose3 RandomPose(); +PoseCovariance RandomPoseCovariance(); + +PoseWithCovariance RandomPoseWithCovariance(); + Eigen::Isometry3d RandomIsometry3d(); Eigen::Affine3d RandomAffine3d(); +CombinedNavState RandomCombinedNavState(); + // Focal lengths and principal points selected from [0.1, 1000] Eigen::Matrix3d RandomIntrinsics(); @@ -112,6 +130,13 @@ Eigen::Matrix<double, Dim, 1> RandomVector() { return RandomDouble() * Eigen::Matrix<double, Dim, 1>::Random(); } +template <int D> +Eigen::Matrix<double, D, D> RandomCovariance() { + const Eigen::Matrix<double, D, D> random_matrix = Eigen::Matrix<double, D, D>::Random(); + // Random covariance is a random symmetric matrix + return random_matrix.transpose() * random_matrix; +} + template <int N> Eigen::Matrix<double, N, 1> AddNoiseToVector(const Eigen::Matrix<double, N, 1>& vector, const double noise_stddev) { Eigen::Matrix<double, N, 1> noisy_vector = vector; diff --git a/localization/localization_common/include/localization_common/timestamped_interpolater.h b/localization/localization_common/include/localization_common/timestamped_interpolater.h new file mode 100644 index 0000000000..d1c55a705c --- /dev/null +++ b/localization/localization_common/include/localization_common/timestamped_interpolater.h @@ -0,0 +1,137 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_COMMON_TIMESTAMPED_INTERPOLATER_H_ +#define LOCALIZATION_COMMON_TIMESTAMPED_INTERPOLATER_H_ + +#include <ff_common/eigen_vectors.h> +#include <localization_common/time.h> +#include <localization_common/timestamped_set.h> +#include <gtsam/base/Matrix.h> + +#include <Eigen/Geometry> + +#include <boost/optional.hpp> + +#include <vector> + +namespace localization_common { +// Empty default interpolater params +struct DefaultInterpolationParams {}; + +// Provides interpolation functions that extends a timestamp set for interpolatable objects. +// Optionally stores params to help with interpolation. +template <typename T, class ParamsT = DefaultInterpolationParams> +class TimestampedInterpolater : public TimestampedSet<T> { + public: + explicit TimestampedInterpolater(const boost::optional<int> max_size = boost::none); + TimestampedInterpolater(const std::vector<Time>& timestamps, const std::vector<T>& objects, + const boost::optional<int> max_size = boost::none); + + // Returns the interpolated object (or exact object if timestamps match) at the provided timestamp. + boost::optional<T> Interpolate(const Time timestamp) const; + + // Interpolates two objects given an alpha [0,1], where alpha = 0 should return object a, + // alpha = 1 should return object b, and otherwise should return the appropriate combination + // of object a and b. + // This needs to be specialized + T Interpolate(const T& a, const T& b, const double alpha) const; + + // Returns the relative object given timestamps a and b. + boost::optional<T> Relative(const Time timestamp_a, const Time timestamp_b) const; + + // Computes a relative object given objects a and b. + // This needs to be specialized. + T Relative(const T& a, const T& b) const; + + // Accessor to params. + ParamsT& params(); + + // Const accessor to params. + const ParamsT& params() const; + + mutable boost::optional<gtsam::Matrix> covariance_a_b; + + private: + TimestampedSet<T> timestamped_objects_; + ParamsT params_; +}; + +// Implementation +template <typename T, class ParamsT> +TimestampedInterpolater<T, ParamsT>::TimestampedInterpolater(const boost::optional<int> max_size) + : TimestampedSet<T>(max_size) {} + +template <typename T, class ParamsT> +TimestampedInterpolater<T, ParamsT>::TimestampedInterpolater(const std::vector<Time>& timestamps, + const std::vector<T>& objects, + const boost::optional<int> max_size) + : TimestampedSet<T>(timestamps, objects, max_size) {} + +template <typename T, class ParamsT> +boost::optional<T> TimestampedInterpolater<T, ParamsT>::Interpolate(const Time timestamp) const { + const auto lower_and_upper_bound = TimestampedSet<T>::LowerAndUpperBound(timestamp); + // Check if equal timestamp exists + if (lower_and_upper_bound.second && lower_and_upper_bound.second->timestamp == timestamp) + return lower_and_upper_bound.second->value; + if (!lower_and_upper_bound.first || !lower_and_upper_bound.second) { + LogDebug("Interpolate: Failed to get lower and upper bound timestamps for query timestamp " << timestamp << "."); + return boost::none; + } + + const Time lower_bound_time = lower_and_upper_bound.first->timestamp; + const Time upper_bound_time = lower_and_upper_bound.second->timestamp; + const double alpha = (timestamp - lower_bound_time) / (upper_bound_time - lower_bound_time); + return Interpolate(lower_and_upper_bound.first->value, lower_and_upper_bound.second->value, alpha); +} + +template <typename T, class ParamsT> +T TimestampedInterpolater<T, ParamsT>::Interpolate(const T& a, const T& b, const double alpha) const { + static_assert(sizeof(T) == std::size_t(-1), "This needs to be specialized by template class."); +} + +template <typename T, class ParamsT> +boost::optional<T> TimestampedInterpolater<T, ParamsT>::Relative(const Time timestamp_a, const Time timestamp_b) const { + const auto a = Interpolate(timestamp_a); + const auto b = Interpolate(timestamp_b); + if (!a || !b) { + LogDebug("Relative: Failed to get interpolated objects for query timestamps " << timestamp_a << " and " + << timestamp_b << "."); + return boost::none; + } + + return Relative(*a, *b); +} + +template <typename T, class ParamsT> +T TimestampedInterpolater<T, ParamsT>::Relative(const T& a, const T& b) const { + static_assert(sizeof(T) == std::size_t(-1), "This needs to be specialized by template class."); +} + +template <typename T, class ParamsT> +ParamsT& TimestampedInterpolater<T, ParamsT>::params() { + return params_; +} + +template <typename T, class ParamsT> +const ParamsT& TimestampedInterpolater<T, ParamsT>::params() const { + return params_; +} +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_TIMESTAMPED_INTERPOLATER_H_ diff --git a/localization/localization_common/include/localization_common/timestamped_set.h b/localization/localization_common/include/localization_common/timestamped_set.h index 4bf5847706..a84eb38f08 100644 --- a/localization/localization_common/include/localization_common/timestamped_set.h +++ b/localization/localization_common/include/localization_common/timestamped_set.h @@ -26,73 +26,156 @@ #include <boost/serialization/serialization.hpp> #include <boost/serialization/unordered_map.hpp> +#include <algorithm> #include <map> #include <utility> #include <vector> namespace localization_common { +// Helper wrapper class that returns a value and timestamp. template <typename T> struct TimestampedValue { TimestampedValue(const Time timestamp, const T& value) : timestamp(timestamp), value(value) {} explicit TimestampedValue(const std::pair<const Time, T>& pair) : timestamp(pair.first), value(pair.second) {} Time timestamp; + // TODO(rsoussan): Store this as const ref? Make it optional to be ref or not? T value; }; +// Stores a set of timesetamped values and provides many functions to +// access and interact with the values using timestamps. +// Optionally enforce a size limit, which when exceeded will remove the first half of the elements in the set. template <typename T> class TimestampedSet { public: - TimestampedSet(); + explicit TimestampedSet(const boost::optional<int> max_size = boost::none); + ~TimestampedSet() = default; - // Assumes values indices have corresponding timestamps in timestamps vectors and each timestamp is unique. - TimestampedSet(const std::vector<Time>& timestamps, const std::vector<T>& values); + // Assumes values have corresponding timestamps at the same index and each timestamp is unique. + TimestampedSet(const std::vector<Time>& timestamps, const std::vector<T>& values, + const boost::optional<int> max_size = boost::none); + // Adds a value at the corresponding timestamp. + // Returns whether the value was successfully added. bool Add(const Time timestamp, const T& value); + // Removes a value at the provided timestamp if it exists. + // Returns whether the value was sucessfully removed. bool Remove(const Time timestamp); + // Get the timestamped value at the provied timestamp. + // Returns boost::none if no value exists. boost::optional<TimestampedValue<T>> Get(const Time timestamp) const; + // Returns the number of timestamped values in the set. size_t size() const; + // Returns whether the set is empty. bool empty() const; + // Clears all values and timestamps from the set. + void Clear(); + + // Returns the oldest timestamped value in the set. + // Returns boost::none if the set is empty. boost::optional<TimestampedValue<T>> Oldest() const; + // Returns the oldest timestamp in the set. + // Returns boost::none if the set is empty. + boost::optional<Time> OldestTimestamp() const; + + // Returns the latest timestamped value in the set. + // Returns boost::none if the set is empty. boost::optional<TimestampedValue<T>> Latest() const; - // Return lower and upper bounds. Equal values are set as upper bound only. + // Returns the latest timestamp in the set. + // Returns boost::none if the set is empty. + boost::optional<Time> LatestTimestamp() const; + + // Returns whether oldest_timestamp <= timestamp <= latest timestamp for the set. + bool WithinBounds(const Time timestamp) const; + + // Returns the lower and upper bound timestamped values in the set corresponding to the + // provided timestamp. If the set has a value at the provided timestamp, + // both the lower and upper bound will be set to this value. std::pair<boost::optional<TimestampedValue<T>>, boost::optional<TimestampedValue<T>>> LowerAndUpperBound( const Time timestamp) const; + // Returns the closest timestamped value in the set to the provided timestamp + // or boost::none if the set is empty. boost::optional<TimestampedValue<T>> Closest(const Time timestamp) const; + // Returns the lower bound or equal timestamped value in the set corresponding to the + // provided timestamp or boost::none if the set is empty. boost::optional<TimestampedValue<T>> LowerBoundOrEqual(const Time timestamp) const; + // Returns a vector containing the ordered (oldest to latest) timestamps contained + // in the set. std::vector<Time> Timestamps() const; + // Returns the total duration of time (latest_time - oldest_time) contained in the set. double Duration() const; + // Returns whether the set contains the provided timestamp. bool Contains(const Time timestamp) const; + // Returns the latest values not older than the provided oldest allowed timestamp. + // Orders values from older to later values. + std::vector<TimestampedValue<T>> LatestValues(const Time oldest_allowed_timestamp) const; + + // Returns timestamped values in the set older than the provided oldest allowed timestamp. + // Values are ordered from oldest to latest. std::vector<TimestampedValue<T>> OldValues(const Time oldest_allowed_timestamp) const; + // Returns values occuring at the provided timestamps. + // Orders values from older to later values. + template <typename TimestampSetType> + std::vector<TimestampedValue<T>> DownsampledValues(const TimestampSetType& allowed_timestamps) const; + + // Removes values older than the provided oldest_allowed_timestamp. + // Returns the number of removed values. int RemoveOldValues(const Time oldest_allowed_timestamp); + // Finds the lower bound element for timestamp and removes all values below this. + // Keeps the lower bound, unlike RemoveOldValues which only keeps a lower bound + // equal to the provided timestamp. + int RemoveBelowLowerBoundValues(const Time timestamp); + + // Removes and returns the oldest value if it exists. + boost::optional<TimestampedValue<T>> RemoveOldest(); + + // Returns a const reference to the internal map containing timestamps and corresponding values. + const std::map<Time, T>& set() const; + + // Returns a reference to the internal map containing timestamps and corresponding values. + std::map<Time, T>& set(); + + // Returns an end iterator for the internal map containing timestamps and corresponding values. + typename std::map<Time, T>::const_iterator cend() const; + + // Returns iterators to values in range of oldest and latest allowed timestamps. + // The second iterator is one-past the latest allowed element to allow for iterating + // using: for(auto it = pair.first; it != pair.second; ++it) + std::pair<typename std::map<Time, T>::const_iterator, typename std::map<Time, T>::const_iterator> InRangeValues( + const Time oldest_allowed_timestamp, const Time latest_allowed_timestamp); + private: friend class boost::serialization::access; template <class ARCHIVE> void serialize(ARCHIVE& ar, const unsigned int /*version*/); std::map<Time, T> timestamp_value_map_; + boost::optional<int> max_size_; }; // Implementation template <typename T> -TimestampedSet<T>::TimestampedSet() {} +TimestampedSet<T>::TimestampedSet(const boost::optional<int> max_size) : max_size_(max_size) {} template <typename T> -TimestampedSet<T>::TimestampedSet(const std::vector<Time>& timestamps, const std::vector<T>& values) { +TimestampedSet<T>::TimestampedSet(const std::vector<Time>& timestamps, const std::vector<T>& values, + const boost::optional<int> max_size) + : max_size_(max_size) { for (int i = 0; i < values.size(); ++i) { Add(timestamps[i], values[i]); } @@ -102,6 +185,12 @@ template <typename T> bool TimestampedSet<T>::Add(const Time timestamp, const T& value) { if (Contains(timestamp)) return false; timestamp_value_map_.emplace(timestamp, value); + // Optionally shrink elements to half of max size if max size exceeded. Removes first half of set. + if (max_size_ && size() > *max_size_) { + auto end_it = timestamp_value_map_.begin(); + std::advance(end_it, *max_size_ / 2); + timestamp_value_map_.erase(timestamp_value_map_.begin(), end_it); + } return true; } @@ -128,6 +217,11 @@ bool TimestampedSet<T>::empty() const { return timestamp_value_map_.empty(); } +template <typename T> +void TimestampedSet<T>::Clear() { + timestamp_value_map_.clear(); +} + template <typename T> boost::optional<TimestampedValue<T>> TimestampedSet<T>::Oldest() const { if (empty()) { @@ -137,6 +231,15 @@ boost::optional<TimestampedValue<T>> TimestampedSet<T>::Oldest() const { return TimestampedValue<T>(*timestamp_value_map_.cbegin()); } +template <typename T> +boost::optional<Time> TimestampedSet<T>::OldestTimestamp() const { + if (empty()) { + LogDebug("OldestTimestamp: No timestamps available."); + return boost::none; + } + return timestamp_value_map_.cbegin()->first; +} + template <typename T> boost::optional<TimestampedValue<T>> TimestampedSet<T>::Latest() const { if (empty()) { @@ -146,6 +249,26 @@ boost::optional<TimestampedValue<T>> TimestampedSet<T>::Latest() const { return TimestampedValue<T>(*timestamp_value_map_.crbegin()); } +template <typename T> +boost::optional<Time> TimestampedSet<T>::LatestTimestamp() const { + if (empty()) { + LogDebug("LatestTimestamp: No values available."); + return boost::none; + } + return timestamp_value_map_.crbegin()->first; +} + +template <typename T> +bool TimestampedSet<T>::WithinBounds(const Time time) const { + const auto oldest = Oldest(); + const auto latest = Latest(); + if (!oldest || !latest) { + LogError("WithinBounds: Failed to get time bounds."); + return false; + } + return (time >= oldest->timestamp && time <= latest->timestamp); +} + template <typename T> std::pair<boost::optional<TimestampedValue<T>>, boost::optional<TimestampedValue<T>>> TimestampedSet<T>::LowerAndUpperBound(const Time timestamp) const { @@ -159,6 +282,10 @@ TimestampedSet<T>::LowerAndUpperBound(const Time timestamp) const { if (upper_bound_it == timestamp_value_map_.cend()) { LogDebug("LowerAndUpperBoundTimestamps: No upper bound timestamp exists."); return {boost::optional<TimestampedValue<T>>(*(timestamp_value_map_.crbegin())), boost::none}; + } else if (upper_bound_it->first == timestamp) { + // Handle equality, set lower and upper bound to same elements + return {boost::optional<TimestampedValue<T>>(*upper_bound_it), + boost::optional<TimestampedValue<T>>(*upper_bound_it)}; } else if (upper_bound_it == timestamp_value_map_.cbegin()) { LogDebug("LowerAndUpperBoundTimestamps: No lower bound timestamp exists."); return {boost::none, boost::optional<TimestampedValue<T>>(*upper_bound_it)}; @@ -196,7 +323,7 @@ std::vector<Time> TimestampedSet<T>::Timestamps() const { template <typename T> double TimestampedSet<T>::Duration() const { if (empty()) return 0; - return (Latest()->timestamp - Oldest()->timestamp); + return (*LatestTimestamp() - *OldestTimestamp()); } template <typename T> @@ -230,33 +357,91 @@ bool TimestampedSet<T>::Contains(const Time timestamp) const { return timestamp_value_map_.count(timestamp) > 0; } +template <typename T> +std::vector<TimestampedValue<T>> TimestampedSet<T>::LatestValues(const Time oldest_allowed_timestamp) const { + std::vector<TimestampedValue<T>> latest_values; + std::transform(timestamp_value_map_.lower_bound(oldest_allowed_timestamp), timestamp_value_map_.end(), + std::back_inserter(latest_values), + [](const std::pair<Time, T>& value) { return TimestampedValue<T>(value); }); + return latest_values; +} + template <typename T> std::vector<TimestampedValue<T>> TimestampedSet<T>::OldValues(const Time oldest_allowed_timestamp) const { std::vector<TimestampedValue<T>> old_values; - for (const auto& timestamped_value : timestamp_value_map_) { - if (timestamped_value.first >= oldest_allowed_timestamp) break; - old_values.emplace_back(timestamped_value); - } - + std::transform(timestamp_value_map_.begin(), timestamp_value_map_.lower_bound(oldest_allowed_timestamp), + std::back_inserter(old_values), + [](const std::pair<Time, T>& value) { return TimestampedValue<T>(value); }); return old_values; } template <typename T> -int TimestampedSet<T>::RemoveOldValues(const Time oldest_allowed_timestamp) { - int num_removed_values = 0; - for (auto it = timestamp_value_map_.begin(); it != timestamp_value_map_.end();) { - if (it->first >= oldest_allowed_timestamp) break; - it = timestamp_value_map_.erase(it); - ++num_removed_values; +template <typename TimestampSetType> +std::vector<TimestampedValue<T>> TimestampedSet<T>::DownsampledValues( + const TimestampSetType& allowed_timestamps) const { + std::vector<TimestampedValue<T>> downsampled_values; + for (const auto& pair : timestamp_value_map_) { + if (allowed_timestamps.count(pair.first) > 0) { + downsampled_values.emplace_back(TimestampedValue<T>(pair)); + } } + return downsampled_values; +} + +template <typename T> +int TimestampedSet<T>::RemoveOldValues(const Time oldest_allowed_timestamp) { + const int initial_num_values = size(); + timestamp_value_map_.erase(timestamp_value_map_.begin(), timestamp_value_map_.lower_bound(oldest_allowed_timestamp)); + return initial_num_values - size(); +} + +template <typename T> +int TimestampedSet<T>::RemoveBelowLowerBoundValues(const Time timestamp) { + const int initial_num_values = size(); + const auto upper_bound = timestamp_value_map_.lower_bound(timestamp); + const auto lower_bound = + upper_bound != timestamp_value_map_.cbegin() ? std::prev(upper_bound) : timestamp_value_map_.cbegin(); + timestamp_value_map_.erase(timestamp_value_map_.begin(), lower_bound); + return initial_num_values - size(); +} + +template <typename T> +boost::optional<TimestampedValue<T>> TimestampedSet<T>::RemoveOldest() { + const auto oldest = Oldest(); + if (oldest) Remove(oldest->timestamp); + return oldest; +} + +template <typename T> +const std::map<Time, T>& TimestampedSet<T>::set() const { + return timestamp_value_map_; +} - return num_removed_values; +template <typename T> +std::map<Time, T>& TimestampedSet<T>::set() { + return timestamp_value_map_; +} + +template <typename T> +typename std::map<Time, T>::const_iterator TimestampedSet<T>::cend() const { + return timestamp_value_map_.cend(); +} + +template <typename T> +std::pair<typename std::map<Time, T>::const_iterator, typename std::map<Time, T>::const_iterator> +TimestampedSet<T>::InRangeValues(const Time oldest_allowed_timestamp, const Time latest_allowed_timestamp) { + auto upper_bound = timestamp_value_map_.upper_bound(latest_allowed_timestamp); + auto lower_bound = timestamp_value_map_.lower_bound(oldest_allowed_timestamp); + // No values less than latest allowed time + if (upper_bound == timestamp_value_map_.cbegin()) return {cend(), cend()}; + return std::make_pair(timestamp_value_map_.lower_bound(oldest_allowed_timestamp), upper_bound); } template <typename T> template <class ARCHIVE> void TimestampedSet<T>::serialize(ARCHIVE& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_NVP(timestamp_value_map_); + ar& BOOST_SERIALIZATION_NVP(max_size_); } } // namespace localization_common diff --git a/localization/localization_common/include/localization_common/utilities.h b/localization/localization_common/include/localization_common/utilities.h index 56002894cb..bf1c08aa89 100644 --- a/localization/localization_common/include/localization_common/utilities.h +++ b/localization/localization_common/include/localization_common/utilities.h @@ -20,7 +20,8 @@ #define LOCALIZATION_COMMON_UTILITIES_H_ #include <config_reader/config_reader.h> -#include <ff_msgs/GraphState.h> +#include <ff_msgs/CombinedNavState.h> +#include <ff_msgs/GraphVIOState.h> #include <ff_msgs/VisualLandmarks.h> #include <localization_common/combined_nav_state.h> #include <localization_common/combined_nav_state_covariances.h> @@ -30,6 +31,8 @@ #include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Pose3.h> +#include <gtsam/linear/NoiseModel.h> +#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <Eigen/Core> @@ -43,11 +46,17 @@ #include <vector> namespace localization_common { -gtsam::Pose3 LoadTransform(config_reader::ConfigReader& config, const std::string& transform_config_name); +gtsam::Pose3 LoadTransform(config_reader::ConfigReader& config, const std::string& transform_config_name, + const std::string& prefix = ""); -gtsam::Vector3 LoadVector3(config_reader::ConfigReader& config, const std::string& config_name); +gtsam::Vector3 LoadVector3(config_reader::ConfigReader& config, const std::string& config_name, + const std::string& prefix = ""); -gtsam::Cal3_S2 LoadCameraIntrinsics(config_reader::ConfigReader& config, const std::string& intrinsics_config_name); +Eigen::Matrix3d LoadCameraIntrinsicsMatrix(config_reader::ConfigReader& config, + const std::string& intrinsics_config_name, const std::string& prefix = ""); + +gtsam::Cal3_S2 LoadCameraIntrinsics(config_reader::ConfigReader& config, const std::string& intrinsics_config_name, + const std::string& prefix = ""); gtsam::Pose3 GtPose(const Eigen::Isometry3d& eigen_pose); @@ -61,8 +70,9 @@ gtsam::Pose3 PoseFromMsgWithExtrinsics(const geometry_msgs::Pose& pose, const gt // Load either iss or granite graph localizer config depending on environment variable for ASTROBEE_WORLD void LoadGraphLocalizerConfig(config_reader::ConfigReader& config, const std::string& path_prefix = ""); -void SetEnvironmentConfigs(const std::string& astrobee_configs_path, const std::string& world, - const std::string& robot_config_file); +void LoadGraphVIOConfig(config_reader::ConfigReader& config, const std::string& path_prefix = ""); + +void SetEnvironmentConfigs(const std::string& world = "iss", const std::string& robot_config_file = "bumble.config"); ros::Time RosTimeFromHeader(const std_msgs::Header& header); @@ -88,9 +98,11 @@ geometry_msgs::TransformStamped PoseToTF(const gtsam::Pose3& pose, const std::st const std::string& child_frame, const Time timestamp, const std::string& platform_name = ""); -CombinedNavState CombinedNavStateFromMsg(const ff_msgs::GraphState& loc_msg); +CombinedNavState CombinedNavStateFromMsg(const ff_msgs::CombinedNavState& msg); + +CombinedNavStateCovariances CombinedNavStateCovariancesFromMsg(const ff_msgs::CombinedNavState& msg); -CombinedNavStateCovariances CombinedNavStateCovariancesFromMsg(const ff_msgs::GraphState& loc_msg); +TimestampedSet<PoseCovariance> CorrelationCovariancesFromMsg(const ff_msgs::CombinedNavState& msg); // Returns gravity corrected accelerometer measurement gtsam::Vector3 RemoveGravityFromAccelerometerMeasurement(const gtsam::Vector3& global_F_gravity, @@ -98,11 +110,17 @@ gtsam::Vector3 RemoveGravityFromAccelerometerMeasurement(const gtsam::Vector3& g const gtsam::Pose3& global_T_body, const gtsam::Vector3& uncorrected_accelerometer_measurement); +ff_msgs::CombinedNavState CombinedNavStateToMsg(const CombinedNavState& combined_nav_state, + const PoseCovariance& pose_covariance, + const Eigen::Matrix3d& velocity_covariance, + const Eigen::Matrix<double, 6, 6>& imu_bias_covariance, + const TimestampedSet<PoseCovariance>& correlation_covariances = {}); + template <class LocMsgType> -void CombinedNavStateToMsg(const CombinedNavState& combined_nav_state, LocMsgType& loc_msg); +void CombinedNavStateToLocMsg(const CombinedNavState& combined_nav_state, LocMsgType& loc_msg); template <class LocMsgType> -void CombinedNavStateCovariancesToMsg(const CombinedNavStateCovariances& covariances, LocMsgType& loc_msg); +void CombinedNavStateCovariancesToLocMsg(const CombinedNavStateCovariances& covariances, LocMsgType& loc_msg); template <typename MatrixType> double LogDeterminant(const MatrixType& matrix); @@ -125,6 +143,11 @@ Eigen::Vector3d CylindricalToCartesian(const Eigen::Vector3d& cylindrical_coordi // Uses Euler Angles in intrinsic ypr representation in degrees Eigen::Matrix3d RotationFromEulerAngles(const double yaw, const double pitch, const double roll); +// Returns the relative covariance between two pose with covariances. +// Optionally uses the correlation covariance matrix covariance_a_b for a more accurate estimate if provided. +PoseCovariance RelativePoseCovariance(const PoseWithCovariance& a, const PoseWithCovariance& b, + const boost::optional<PoseCovariance> covariance_a_b = boost::none); + Eigen::Isometry3d FrameChangeRelativePose(const Eigen::Isometry3d& a_F_relative_pose, const Eigen::Isometry3d& b_T_a); Eigen::Matrix<double, 6, 6> FrameChangeRelativeCovariance( @@ -152,9 +175,23 @@ std::pair<std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>> TransformP Eigen::Isometry3d Interpolate(const Eigen::Isometry3d& lower_bound_pose, const Eigen::Isometry3d& upper_bound_pose, const double alpha); +PoseWithCovariance Interpolate(const PoseWithCovariance& lower_bound_pose, const PoseWithCovariance& upper_bound_pose, + const double alpha); + +gtsam::noiseModel::Robust::shared_ptr Robust(const gtsam::SharedNoiseModel& noise, const double huber_k); + +template <typename FactorType> +void DeleteFactors(gtsam::NonlinearFactorGraph& graph); + +template <typename FactorType> +int NumFactors(const gtsam::NonlinearFactorGraph& graph); + +template <typename FactorType> +std::vector<const FactorType*> Factors(const gtsam::NonlinearFactorGraph& graph); + // Implementations template <class LocMsgType> -void CombinedNavStateToMsg(const CombinedNavState& combined_nav_state, LocMsgType& loc_msg) { +void CombinedNavStateToLocMsg(const CombinedNavState& combined_nav_state, LocMsgType& loc_msg) { PoseToMsg(combined_nav_state.pose(), loc_msg.pose); msg_conversions::VectorToMsg(combined_nav_state.velocity(), loc_msg.velocity); msg_conversions::VectorToMsg(combined_nav_state.bias().accelerometer(), loc_msg.accel_bias); @@ -163,7 +200,7 @@ void CombinedNavStateToMsg(const CombinedNavState& combined_nav_state, LocMsgTyp } template <class LocMsgType> -void CombinedNavStateCovariancesToMsg(const CombinedNavStateCovariances& covariances, LocMsgType& loc_msg) { +void CombinedNavStateCovariancesToLocMsg(const CombinedNavStateCovariances& covariances, LocMsgType& loc_msg) { // Orientation (0-2) msg_conversions::VariancesToCovDiag(covariances.orientation_variances(), &loc_msg.cov_diag[0]); @@ -228,6 +265,42 @@ std::vector<T> Rotate(const std::vector<T>& a_F_a_T_elements, const Eigen::Matri } return b_F_a_T_elements; } + +template <typename FactorType> +void DeleteFactors(gtsam::NonlinearFactorGraph& graph) { + int num_removed_factors = 0; + for (auto factor_it = graph.begin(); factor_it != graph.end();) { + if (dynamic_cast<FactorType*>(factor_it->get())) { + factor_it = graph.erase(factor_it); + ++num_removed_factors; + continue; + } + ++factor_it; + } +} + +template <typename FactorType> +int NumFactors(const gtsam::NonlinearFactorGraph& graph) { + int num_factors = 0; + for (const auto& factor : graph) { + if (dynamic_cast<const FactorType*>(factor.get())) { + ++num_factors; + } + } + return num_factors; +} + +template <typename FactorType> +std::vector<const FactorType*> Factors(const gtsam::NonlinearFactorGraph& graph) { + std::vector<const FactorType*> factors; + for (const auto& factor : graph) { + const FactorType* casted_factor = dynamic_cast<const FactorType*>(factor.get()); + if (casted_factor) { + factors.emplace_back(casted_factor); + } + } + return factors; +} } // namespace localization_common #endif // LOCALIZATION_COMMON_UTILITIES_H_ diff --git a/localization/localization_common/package.xml b/localization/localization_common/package.xml index 2663824daa..9bdd9a695c 100644 --- a/localization/localization_common/package.xml +++ b/localization/localization_common/package.xml @@ -14,10 +14,12 @@ Astrobee Flight Software </maintainer> <buildtool_depend>catkin</buildtool_depend> + <build_depend>astrobee</build_depend> <build_depend>camera</build_depend> <build_depend>config_reader</build_depend> <build_depend>msg_conversions</build_depend> <build_depend>ff_msgs</build_depend> + <run_depend>astrobee</run_depend> <run_depend>camera</run_depend> <run_depend>config_reader</run_depend> <run_depend>msg_conversions</run_depend> diff --git a/localization/localization_common/scripts/localization_common/utilities.py b/localization/localization_common/scripts/localization_common/utilities.py index 17708cc26f..12c1c0a251 100644 --- a/localization/localization_common/scripts/localization_common/utilities.py +++ b/localization/localization_common/scripts/localization_common/utilities.py @@ -1,3 +1,4 @@ +#!/usr/bin/python3 # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # diff --git a/localization/localization_common/src/distance_scaled_pose_covariance_interpolater.cc b/localization/localization_common/src/distance_scaled_pose_covariance_interpolater.cc new file mode 100644 index 0000000000..2ee8b4890f --- /dev/null +++ b/localization/localization_common/src/distance_scaled_pose_covariance_interpolater.cc @@ -0,0 +1,46 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/distance_scaled_pose_covariance_interpolater.h> + +namespace localization_common { +DistanceScaledPoseCovarianceInterpolater::DistanceScaledPoseCovarianceInterpolater( + const DistanceScaledPoseCovarianceInterpolaterParams& params) + : params_(params) {} + +PoseCovariance DistanceScaledPoseCovarianceInterpolater::Interpolate(const PoseWithCovariance& a, + const PoseWithCovariance& b, + const Time& timestamp_a, const Time& timestamp_b) { + const Eigen::Isometry3d relative_pose = a.pose.inverse() * b.pose; + const double translation_norm = relative_pose.translation().norm(); + const double orientation_angle = Eigen::AngleAxisd(relative_pose.linear()).angle(); + Eigen::Matrix<double, 6, 6> relative_covariance = Eigen::Matrix<double, 6, 6>::Zero(); + // Set translation part of covariance using translation norm + relative_covariance.block<3, 3>(3, 3) = + (relative_pose.translation().cwiseAbs() * params_.translation_scale).asDiagonal(); + // Set orientation part of covariance using orientation angle + relative_covariance.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() * orientation_angle * params_.orientation_scale; + // Make sure no component of the covariance matrix is too small + for (int i = 0; i < 6; ++i) { + if (relative_covariance(i, i) < params_.min_covariance) { + relative_covariance(i, i) = params_.min_covariance; + } + } + return relative_covariance; +} +} // namespace localization_common diff --git a/localization/localization_common/src/pose_covariance_interpolater.cc b/localization/localization_common/src/pose_covariance_interpolater.cc new file mode 100644 index 0000000000..4f0878e05e --- /dev/null +++ b/localization/localization_common/src/pose_covariance_interpolater.cc @@ -0,0 +1,30 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/pose_covariance_interpolater.h> +#include <localization_common/utilities.h> + +namespace localization_common { +PoseCovariance PoseCovarianceInterpolater::Interpolate(const PoseWithCovariance& a, const PoseWithCovariance& b, + const Time& timestamp_a, const Time& timestamp_b) { + // Doesn't use correlation covariance, so not the best estimation. + // If for example a and b are odometry poses, the larger they are, the larger the relative covariance estimation is + // without using the correlation covariance. See MarginalsPoseCovarianceInterpolater for a better estimater. + return RelativePoseCovariance(a, b); +} +} // namespace localization_common diff --git a/localization/localization_common/src/pose_interpolater.cc b/localization/localization_common/src/pose_interpolater.cc index 05c7b2ce32..1ed7086200 100644 --- a/localization/localization_common/src/pose_interpolater.cc +++ b/localization/localization_common/src/pose_interpolater.cc @@ -20,23 +20,14 @@ #include <localization_common/utilities.h> namespace localization_common { -PoseInterpolater::PoseInterpolater(const std::vector<Time>& timestamps, const std::vector<Eigen::Isometry3d>& poses) - : TimestampedSet<Eigen::Isometry3d>(timestamps, poses) {} - -boost::optional<Eigen::Isometry3d> PoseInterpolater::Interpolate(const Time timestamp) const { - const auto lower_and_upper_bound = LowerAndUpperBound(timestamp); - // Check if equal timestamp exists - if (lower_and_upper_bound.second && lower_and_upper_bound.second->timestamp == timestamp) - return lower_and_upper_bound.second->value; - if (!lower_and_upper_bound.first || !lower_and_upper_bound.second) { - LogDebug("Interpolate: Failed to get lower and upper bound timestamps for query timestamp " << timestamp << "."); - return boost::none; - } +template <> +Eigen::Isometry3d PoseInterpolater::Interpolate(const Eigen::Isometry3d& a, const Eigen::Isometry3d& b, + const double alpha) const { + return localization_common::Interpolate(a, b, alpha); +} - const Time lower_bound_time = lower_and_upper_bound.first->timestamp; - const Time upper_bound_time = lower_and_upper_bound.second->timestamp; - const double alpha = (timestamp - lower_bound_time) / (upper_bound_time - lower_bound_time); - return localization_common::Interpolate(lower_and_upper_bound.first->value, lower_and_upper_bound.second->value, - alpha); +template <> +Eigen::Isometry3d PoseInterpolater::Relative(const Eigen::Isometry3d& a, const Eigen::Isometry3d& b) const { + return a.inverse() * b; } } // namespace localization_common diff --git a/localization/localization_common/src/pose_with_covariance_interpolater.cc b/localization/localization_common/src/pose_with_covariance_interpolater.cc new file mode 100644 index 0000000000..b84f304aee --- /dev/null +++ b/localization/localization_common/src/pose_with_covariance_interpolater.cc @@ -0,0 +1,43 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/pose_with_covariance_interpolater.h> +#include <localization_common/utilities.h> + +namespace localization_common { +template <> +PoseWithCovariance PoseWithCovarianceInterpolater::Interpolate(const PoseWithCovariance& a, const PoseWithCovariance& b, + const double alpha) const { + return localization_common::Interpolate(a, b, alpha); +} + +template <> +boost::optional<PoseWithCovariance> PoseWithCovarianceInterpolater::Relative(const Time timestamp_a, + const Time timestamp_b) const { + const auto a = Interpolate(timestamp_a); + const auto b = Interpolate(timestamp_b); + if (!a || !b) { + LogDebug("Relative: Failed to get interpolated objects for query timestamps " << timestamp_a << " and " + << timestamp_b << "."); + return boost::none; + } + const Eigen::Isometry3d relative_pose = a->pose.inverse() * b->pose; + const auto relative_covariance = params().pose_covariance_interpolater->Interpolate(*a, *b, timestamp_a, timestamp_b); + return PoseWithCovariance(relative_pose, relative_covariance); +} +} // namespace localization_common diff --git a/localization/localization_common/src/stats_logger.cc b/localization/localization_common/src/stats_logger.cc new file mode 100644 index 0000000000..85baf9b435 --- /dev/null +++ b/localization/localization_common/src/stats_logger.cc @@ -0,0 +1,46 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#include <localization_common/stats_logger.h> + +namespace localization_common { +StatsLogger::StatsLogger(const bool log_on_destruction) : log_on_destruction_(log_on_destruction) {} + +StatsLogger::~StatsLogger() { + if (log_on_destruction_) Log(); +} + +void StatsLogger::AddAverager(localization_common::Averager& averager) { averagers_.emplace_back(averager); } + +void StatsLogger::AddTimer(localization_common::Timer& timer) { timers_.emplace_back(timer); } + +void StatsLogger::Log() const { + Log(timers_); + Log(averagers_); +} + +void StatsLogger::LogToFile(std::ofstream& ofstream) const { + LogToFile(timers_, ofstream); + LogToFile(averagers_, ofstream); +} + +void StatsLogger::LogToCsv(std::ofstream& ofstream) const { + ofstream << "name,avg,min,max,stddev" << std::endl; + LogToCsv(timers_, ofstream); + LogToCsv(averagers_, ofstream); +} +} // namespace localization_common diff --git a/localization/localization_common/src/test_utilities.cc b/localization/localization_common/src/test_utilities.cc index 6650e5f98b..99c22738d0 100644 --- a/localization/localization_common/src/test_utilities.cc +++ b/localization/localization_common/src/test_utilities.cc @@ -48,9 +48,13 @@ bool RandomBool() { return RandomDouble(0, 1) < 0.5; } double Noise(const double stddev) { return RandomGaussianDouble(0.0, stddev); } +Time RandomTime() { return RandomPositiveDouble(); } + Eigen::Vector3d RandomVector3d() { return RandomVector<3>(); } -Eigen::Vector3d RandomPoint3d() { return RandomVector<3>(); } +Eigen::Vector3d RandomPoint3d() { return RandomVector3d(); } + +Eigen::Vector3d RandomVelocity() { return RandomVector3d(); } Eigen::Vector2d RandomVector2d() { return RandomVector<2>(); } @@ -64,6 +68,10 @@ gtsam::Pose3 RandomPose() { return gtsam::Pose3(rot, trans); } +PoseCovariance RandomPoseCovariance() { return RandomCovariance<6>(); } + +PoseWithCovariance RandomPoseWithCovariance() { return PoseWithCovariance(RandomIsometry3d(), RandomPoseCovariance()); } + Eigen::Isometry3d RandomIsometry3d() { const gtsam::Pose3 random_pose = RandomPose(); return EigenPose(random_pose); @@ -78,6 +86,11 @@ Eigen::Affine3d RandomAffine3d() { return random_affine3d; } +CombinedNavState RandomCombinedNavState() { + return CombinedNavState(RandomPose(), RandomVelocity(), + gtsam::imuBias::ConstantBias(RandomVector3d(), RandomVector3d()), RandomTime()); +} + Eigen::Matrix3d RandomIntrinsics() { Eigen::Matrix3d intrinsics = Eigen::Matrix3d::Identity(); const double f_x = RandomGaussianDouble(500, 20); diff --git a/localization/localization_common/src/utilities.cc b/localization/localization_common/src/utilities.cc index 0051793b03..22a66cd65a 100644 --- a/localization/localization_common/src/utilities.cc +++ b/localization/localization_common/src/utilities.cc @@ -17,13 +17,13 @@ */ #include <camera/camera_params.h> -#include <ff_msgs/GraphState.h> #include <localization_common/logger.h> #include <localization_common/utilities.h> #include <msg_conversions/msg_conversions.h> #include <geometry_msgs/Point.h> #include <geometry_msgs/Quaternion.h> +#include <ros/package.h> #include <cstdlib> #include <string> @@ -31,20 +31,28 @@ namespace localization_common { namespace mc = msg_conversions; -gtsam::Pose3 LoadTransform(config_reader::ConfigReader& config, const std::string& transform_config_name) { - const auto body_T_sensor = mc::LoadEigenTransform(config, transform_config_name); +gtsam::Pose3 LoadTransform(config_reader::ConfigReader& config, const std::string& transform_config_name, + const std::string& prefix) { + const auto body_T_sensor = mc::LoadEigenTransform(config, transform_config_name, prefix); return GtPose(body_T_sensor); } -gtsam::Vector3 LoadVector3(config_reader::ConfigReader& config, const std::string& config_name) { +gtsam::Vector3 LoadVector3(config_reader::ConfigReader& config, const std::string& config_name, + const std::string& prefix) { Eigen::Vector3d vec; - mc::config_read_vector(&config, config_name.c_str(), &vec); + mc::config_read_vector(&config, (prefix + config_name).c_str(), &vec); return vec; } -gtsam::Cal3_S2 LoadCameraIntrinsics(config_reader::ConfigReader& config, const std::string& intrinsics_config_name) { - const camera::CameraParameters cam_params(&config, intrinsics_config_name.c_str()); - const auto intrinsics = cam_params.GetIntrinsicMatrix<camera::UNDISTORTED_C>(); +Eigen::Matrix3d LoadCameraIntrinsicsMatrix(config_reader::ConfigReader& config, + const std::string& intrinsics_config_name, const std::string& prefix) { + const camera::CameraParameters cam_params(&config, (prefix + intrinsics_config_name).c_str()); + return cam_params.GetIntrinsicMatrix<camera::UNDISTORTED_C>(); +} + +gtsam::Cal3_S2 LoadCameraIntrinsics(config_reader::ConfigReader& config, const std::string& intrinsics_config_name, + const std::string& prefix) { + const auto intrinsics = LoadCameraIntrinsicsMatrix(config, intrinsics_config_name, prefix); // Assumes zero skew return gtsam::Cal3_S2(intrinsics(0, 0), intrinsics(1, 1), 0, intrinsics(0, 2), intrinsics(1, 2)); } @@ -64,15 +72,35 @@ gtsam::Pose3 PoseFromMsgWithExtrinsics(const geometry_msgs::Pose& pose, const gt } void LoadGraphLocalizerConfig(config_reader::ConfigReader& config, const std::string& path_prefix) { - config.AddFile((path_prefix + "graph_localizer.config").c_str()); - LogDebug("LoadGraphLocalizerconfig: Loaded graph localizer config."); + config.AddFile((path_prefix + "localization/graph_localizer.config").c_str()); + config.AddFile("transforms.config"); + config.AddFile("cameras.config"); + config.AddFile("geometry.config"); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + LogDebug("LoadGraphLocalizerConfig: Loaded graph localizer config."); +} + +void LoadGraphVIOConfig(config_reader::ConfigReader& config, const std::string& path_prefix) { + 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("transforms.config"); + config.AddFile("cameras.config"); + config.AddFile("geometry.config"); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + LogDebug("LoadGraphVIOConfig: Loaded graph VIO config."); } -void SetEnvironmentConfigs(const std::string& astrobee_configs_path, const std::string& world, - const std::string& robot_config_file) { +void SetEnvironmentConfigs(const std::string& world, const std::string& robot_config_file) { + const std::string astrobee_configs_path = ros::package::getPath("astrobee"); + const std::string full_robot_config_file = "config/robots/" + robot_config_file; setenv("ASTROBEE_RESOURCE_DIR", (astrobee_configs_path + "/resources").c_str(), true); setenv("ASTROBEE_CONFIG_DIR", (astrobee_configs_path + "/config").c_str(), true); - setenv("ASTROBEE_ROBOT", (astrobee_configs_path + "/" + robot_config_file).c_str(), true); + setenv("ASTROBEE_ROBOT", (astrobee_configs_path + "/" + full_robot_config_file).c_str(), true); setenv("ASTROBEE_WORLD", world.c_str(), true); } @@ -116,23 +144,29 @@ geometry_msgs::TransformStamped PoseToTF(const Eigen::Isometry3d& pose, const st return transform; } -CombinedNavState CombinedNavStateFromMsg(const ff_msgs::GraphState& loc_msg) { - const auto pose = PoseFromMsg(loc_msg.pose); - const auto velocity = mc::VectorFromMsg<gtsam::Velocity3, geometry_msgs::Vector3>(loc_msg.velocity); - const auto accel_bias = mc::VectorFromMsg<gtsam::Vector3, geometry_msgs::Vector3>(loc_msg.accel_bias); - const auto gyro_bias = mc::VectorFromMsg<gtsam::Vector3, geometry_msgs::Vector3>(loc_msg.gyro_bias); - const auto timestamp = TimeFromHeader(loc_msg.header); +CombinedNavState CombinedNavStateFromMsg(const ff_msgs::CombinedNavState& msg) { + const auto pose = PoseFromMsg(msg.pose.pose.pose); + const auto velocity = mc::VectorFromMsg<gtsam::Velocity3, geometry_msgs::Vector3>(msg.velocity.velocity); + const auto accel_bias = mc::VectorFromMsg<gtsam::Vector3, geometry_msgs::Vector3>(msg.imu_bias.accelerometer_bias); + const auto gyro_bias = mc::VectorFromMsg<gtsam::Vector3, geometry_msgs::Vector3>(msg.imu_bias.gyroscope_bias); + const auto timestamp = TimeFromHeader(msg.header); return CombinedNavState(pose, velocity, gtsam::imuBias::ConstantBias(accel_bias, gyro_bias), timestamp); } -CombinedNavStateCovariances CombinedNavStateCovariancesFromMsg(const ff_msgs::GraphState& loc_msg) { - const Eigen::Vector3d orientation_variances = mc::CovDiagToVariances(&loc_msg.cov_diag[0]); - const Eigen::Vector3d gyro_bias_variances = mc::CovDiagToVariances(&loc_msg.cov_diag[3]); - const Eigen::Vector3d velocity_variances = mc::CovDiagToVariances(&loc_msg.cov_diag[6]); - const Eigen::Vector3d accelerometer_bias_variances = mc::CovDiagToVariances(&loc_msg.cov_diag[9]); - const Eigen::Vector3d position_variances = mc::CovDiagToVariances(&loc_msg.cov_diag[12]); - return CombinedNavStateCovariances(position_variances, orientation_variances, velocity_variances, - accelerometer_bias_variances, gyro_bias_variances); +CombinedNavStateCovariances CombinedNavStateCovariancesFromMsg(const ff_msgs::CombinedNavState& msg) { + const auto pose_covariance = mc::EigenCovarianceFromMsg<6>(msg.pose.pose.covariance); + const auto velocity_covariance = mc::EigenCovarianceFromMsg<3>(msg.velocity.covariance); + const auto imu_bias_covariance = mc::EigenCovarianceFromMsg<6>(msg.imu_bias.covariance); + return CombinedNavStateCovariances(pose_covariance, velocity_covariance, imu_bias_covariance); +} + +TimestampedSet<PoseCovariance> CorrelationCovariancesFromMsg(const ff_msgs::CombinedNavState& msg) { + TimestampedSet<PoseCovariance> correlation_covariances; + for (const auto& correlation_covariance : msg.correlation_covariances) { + correlation_covariances.Add(TimeFromRosTime(correlation_covariance.time), + mc::EigenCovarianceFromMsg<6>(correlation_covariance.covariance)); + } + return correlation_covariances; } gtsam::Vector3 RemoveGravityFromAccelerometerMeasurement(const gtsam::Vector3& global_F_gravity, @@ -145,6 +179,34 @@ gtsam::Vector3 RemoveGravityFromAccelerometerMeasurement(const gtsam::Vector3& g return (uncorrected_accelerometer_measurement + imu_F_gravity); } +ff_msgs::CombinedNavState CombinedNavStateToMsg(const CombinedNavState& combined_nav_state, + const PoseCovariance& pose_covariance, + const Eigen::Matrix3d& velocity_covariance, + const Eigen::Matrix<double, 6, 6>& imu_bias_covariance, + const TimestampedSet<PoseCovariance>& correlation_covariances) { + ff_msgs::CombinedNavState msg; + TimeToHeader(combined_nav_state.timestamp(), msg.header); + // Write CombinedNavState + PoseToMsg(combined_nav_state.pose(), msg.pose.pose.pose); + mc::VectorToMsg(combined_nav_state.velocity(), msg.velocity.velocity); + mc::VectorToMsg(combined_nav_state.bias().accelerometer(), msg.imu_bias.accelerometer_bias); + mc::VectorToMsg(combined_nav_state.bias().gyroscope(), msg.imu_bias.gyroscope_bias); + + // Write covariances + mc::EigenCovarianceToMsg(pose_covariance, msg.pose.pose.covariance); + mc::EigenCovarianceToMsg(velocity_covariance, msg.velocity.covariance); + mc::EigenCovarianceToMsg(imu_bias_covariance, msg.imu_bias.covariance); + + // Write correlation pose covariances if available + for (const auto& correlation_covariance : correlation_covariances.set()) { + ff_msgs::PoseCovarianceStamped covariance_msg; + mc::EigenCovarianceToMsg(correlation_covariance.second, covariance_msg.covariance); + TimeToMsg(correlation_covariance.first, covariance_msg.time); + msg.correlation_covariances.push_back(covariance_msg); + } + return msg; +} + double Deg2Rad(const double degrees) { return M_PI / 180.0 * degrees; } double Rad2Deg(const double radians) { return 180.0 / M_PI * radians; } @@ -168,6 +230,31 @@ Eigen::Matrix3d RotationFromEulerAngles(const double yaw_degrees, const double p return rotation; } +PoseCovariance RelativePoseCovariance(const PoseWithCovariance& a, const PoseWithCovariance& b, + const boost::optional<PoseCovariance> covariance_a_b) { + // Calculate covariance + // See https://gtsam.org/2021/02/23/uncertainties-part3.html + // Adjoints + // Uses convention w_T_a and w_T_b for poses, s.t. adj_w_a is the adjoint for the + // w_T_a pose. + const gtsam::Pose3 w_T_a = GtPose(a.pose); + const gtsam::Pose3 w_T_b = GtPose(b.pose); + const gtsam::Matrix6 adj_w_a = w_T_a.AdjointMap(); + const gtsam::Matrix6 adj_b_w = w_T_b.inverse().AdjointMap(); + // Helper terms + const gtsam::Matrix6 h = adj_b_w * adj_w_a; + const gtsam::Matrix6 h_t = h.transpose(); + // Compute covariance without correlation terms + gtsam::Matrix6 relative_covariance = h * a.covariance * h_t + b.covariance; + // Add correlation terms if they exist + // Assumes correlation_covariances stores covariances a_b rather than b_a + // const auto covariance_a_b = b.correlation_covariances.Get(*(a.correlation_covariances.LatestTimestamp())); + if (covariance_a_b) { + relative_covariance = relative_covariance - h * (*covariance_a_b) - (*covariance_a_b).transpose() * h_t; + } + return relative_covariance; +} + Eigen::Isometry3d FrameChangeRelativePose(const Eigen::Isometry3d& a_F_relative_pose, const Eigen::Isometry3d& b_T_a) { // Consider for example a sensor odometry measurement, sensor_time_0_T_sensor_time_1. // To find the movement of the robot body given static body_T_sensor extrinsics, @@ -177,9 +264,8 @@ Eigen::Isometry3d FrameChangeRelativePose(const Eigen::Isometry3d& a_F_relative_ Eigen::Matrix<double, 6, 6> FrameChangeRelativeCovariance( const Eigen::Matrix<double, 6, 6>& a_F_relative_pose_covariance, const Eigen::Isometry3d& b_T_a) { - // TODO(rsoussan): This might be right for translation component (frame change is equivalent to single rotation), - // but might be wrong for rotation component - return gtsam::TransformCovariance<gtsam::Pose3>(GtPose(b_T_a))(a_F_relative_pose_covariance); + const gtsam::Pose3 b_R_a(gtsam::Rot3(b_T_a.linear()), Eigen::Vector3d::Zero()); + return gtsam::TransformCovariance<gtsam::Pose3>(b_R_a)(a_F_relative_pose_covariance); } PoseWithCovariance FrameChangeRelativePoseWithCovariance(const PoseWithCovariance& a_F_relative_pose_with_covariance, @@ -232,4 +318,20 @@ Eigen::Isometry3d Interpolate(const Eigen::Isometry3d& lower_bound_pose, const E Eigen::Quaterniond(lower_bound_pose.linear()).slerp(alpha, Eigen::Quaterniond(upper_bound_pose.linear())); return Isometry3d(translation, rotation); } + +PoseWithCovariance Interpolate(const PoseWithCovariance& lower_bound_pose, const PoseWithCovariance& upper_bound_pose, + const double alpha) { + const Eigen::Isometry3d interpolated_pose = Interpolate(lower_bound_pose.pose, upper_bound_pose.pose, alpha); + // TODO(rsoussan): Implement this properly + const PoseCovariance& interpolated_covariance = + alpha > 0.5 ? upper_bound_pose.covariance : lower_bound_pose.covariance; + const auto& correlation_covariances = + alpha > 0.5 ? upper_bound_pose.correlation_covariances : lower_bound_pose.correlation_covariances; + + return PoseWithCovariance(interpolated_pose, interpolated_covariance, correlation_covariances); +} + +gtsam::noiseModel::Robust::shared_ptr Robust(const gtsam::SharedNoiseModel& noise, const double huber_k) { + return gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Huber::Create(huber_k), noise); +} } // namespace localization_common diff --git a/localization/localization_common/test/test_pose_interpolater.cc b/localization/localization_common/test/test_pose_interpolater.cc index 9aaaea6f71..d6f76fc5e2 100644 --- a/localization/localization_common/test/test_pose_interpolater.cc +++ b/localization/localization_common/test/test_pose_interpolater.cc @@ -59,6 +59,41 @@ TEST(PoseInterpolaterTester, Interpolate) { } } +TEST(PoseInterpolaterTester, Relative) { + std::vector<Eigen::Isometry3d> poses; + std::vector<lc::Time> timestamps; + for (int i = 0; i < 10; ++i) { + poses.emplace_back(lc::RandomIsometry3d()); + timestamps.emplace_back(i); + } + lc::PoseInterpolater interpolater(timestamps, poses); + + // Too low + { + const auto relative_pose = interpolater.Relative(-1, 5); + EXPECT_TRUE(relative_pose == boost::none); + } + // Too high + { + const auto relative_pose = interpolater.Relative(5, 11); + EXPECT_TRUE(relative_pose == boost::none); + } + // Valid + { + const auto relative_pose = interpolater.Relative(2.2, 3.3); + ASSERT_TRUE(relative_pose != boost::none); + const auto expected_relative_pose = + lc::Interpolate(poses[2], poses[3], 0.2).inverse() * lc::Interpolate(poses[3], poses[4], 0.3); + EXPECT_MATRIX_NEAR((*relative_pose), expected_relative_pose, 1e-6); + } + // Exact pose/timestamp + { + const auto relative_pose = interpolater.Relative(7, 8); + ASSERT_TRUE(relative_pose != boost::none); + EXPECT_MATRIX_NEAR((*relative_pose), poses[7].inverse() * poses[8], 1e-6); + } +} + // Run all the tests that were declared with TEST() int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/localization/localization_common/test/test_pose_with_covariance_interpolater.cc b/localization/localization_common/test/test_pose_with_covariance_interpolater.cc new file mode 100644 index 0000000000..92acc1a66e --- /dev/null +++ b/localization/localization_common/test/test_pose_with_covariance_interpolater.cc @@ -0,0 +1,101 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/logger.h> +#include <localization_common/pose_with_covariance_interpolater.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> + +#include <gtest/gtest.h> + +namespace lc = localization_common; + +TEST(PoseWithCovarianceInterpolaterTester, Interpolate) { + std::vector<lc::PoseWithCovariance> poses; + std::vector<lc::Time> timestamps; + for (int i = 0; i < 10; ++i) { + poses.emplace_back(lc::PoseWithCovariance(lc::RandomIsometry3d(), lc::PoseCovariance::Identity())); + timestamps.emplace_back(i); + } + lc::PoseWithCovarianceInterpolater interpolater(timestamps, poses); + + // Too low + { + const auto interpolated_pose = interpolater.Interpolate(-1); + EXPECT_TRUE(interpolated_pose == boost::none); + } + // Too high + { + const auto interpolated_pose = interpolater.Interpolate(11); + EXPECT_TRUE(interpolated_pose == boost::none); + } + // Valid + { + const auto interpolated_pose = interpolater.Interpolate(3.3); + ASSERT_TRUE(interpolated_pose != boost::none); + const auto expected_interpolated_pose = lc::Interpolate(poses[3], poses[4], 0.3); + EXPECT_MATRIX_NEAR(interpolated_pose->pose, expected_interpolated_pose.pose, 1e-6); + } + // Exact pose/timestamp + { + const auto interpolated_pose = interpolater.Interpolate(8); + ASSERT_TRUE(interpolated_pose != boost::none); + EXPECT_MATRIX_NEAR(interpolated_pose->pose, poses[8].pose, 1e-6); + } +} + +TEST(PoseWithCovarianceInterpolaterTester, Relative) { + std::vector<lc::PoseWithCovariance> poses; + std::vector<lc::Time> timestamps; + for (int i = 0; i < 10; ++i) { + poses.emplace_back(lc::PoseWithCovariance(lc::RandomIsometry3d(), lc::PoseCovariance::Identity())); + timestamps.emplace_back(i); + } + lc::PoseWithCovarianceInterpolater interpolater(timestamps, poses); + + // Too low + { + const auto relative_pose = interpolater.Relative(-1, 5); + EXPECT_TRUE(relative_pose == boost::none); + } + // Too high + { + const auto relative_pose = interpolater.Relative(5, 11); + EXPECT_TRUE(relative_pose == boost::none); + } + // Valid + { + const auto relative_pose = interpolater.Relative(2.2, 3.3); + ASSERT_TRUE(relative_pose != boost::none); + const auto expected_relative_pose = + lc::Interpolate(poses[2], poses[3], 0.2).pose.inverse() * lc::Interpolate(poses[3], poses[4], 0.3).pose; + EXPECT_MATRIX_NEAR(relative_pose->pose, expected_relative_pose, 1e-6); + } + // Exact pose/timestamp + { + const auto relative_pose = interpolater.Relative(7, 8); + ASSERT_TRUE(relative_pose != boost::none); + EXPECT_MATRIX_NEAR(relative_pose->pose, poses[7].pose.inverse() * poses[8].pose, 1e-6); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/localization_common/test/test_pose_with_covariance_interpolater.test b/localization/localization_common/test/test_pose_with_covariance_interpolater.test new file mode 100644 index 0000000000..94fe581976 --- /dev/null +++ b/localization/localization_common/test/test_pose_with_covariance_interpolater.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="localization_common" type="test_pose_with_covariance_interpolater" test-name="test_pose_with_covariance_interpolater" /> +</launch> diff --git a/localization/localization_common/test/test_timestamped_set.cc b/localization/localization_common/test/test_timestamped_set.cc index 3b65736f2d..5026f4a2b5 100644 --- a/localization/localization_common/test/test_timestamped_set.cc +++ b/localization/localization_common/test/test_timestamped_set.cc @@ -114,12 +114,13 @@ TEST(TimestampedSetTester, AddRemoveContainsEmptySize) { } } -TEST(TimestampedSetTester, OldestLatest) { +TEST(TimestampedSetTester, OldestLatestWithinBounds) { lc::TimestampedSet<double> timestamped_set; // No elements { EXPECT_TRUE(timestamped_set.Oldest() == boost::none); EXPECT_TRUE(timestamped_set.Latest() == boost::none); + EXPECT_FALSE(timestamped_set.WithinBounds(123.1)); } const double value_1 = 101.0; const localization_common::Time timestamp_1 = 1.0; @@ -134,6 +135,9 @@ TEST(TimestampedSetTester, OldestLatest) { ASSERT_TRUE(latest_value != boost::none); EXPECT_EQ(latest_value->value, value_1); EXPECT_EQ(latest_value->timestamp, timestamp_1); + EXPECT_FALSE(timestamped_set.WithinBounds(11.01)); + EXPECT_FALSE(timestamped_set.WithinBounds(30009)); + EXPECT_FALSE(timestamped_set.WithinBounds(value_1)); } // 2 elements const double value_2 = 100.3; @@ -148,6 +152,11 @@ TEST(TimestampedSetTester, OldestLatest) { ASSERT_TRUE(latest_value != boost::none); EXPECT_EQ(latest_value->value, value_2); EXPECT_EQ(latest_value->timestamp, timestamp_2); + EXPECT_FALSE(timestamped_set.WithinBounds(timestamp_1 - 1)); + EXPECT_FALSE(timestamped_set.WithinBounds(timestamp_2 + 3.5)); + EXPECT_TRUE(timestamped_set.WithinBounds(2.2)); + EXPECT_TRUE(timestamped_set.WithinBounds(timestamp_1)); + EXPECT_TRUE(timestamped_set.WithinBounds(timestamp_2)); } // 3 elements @@ -163,6 +172,8 @@ TEST(TimestampedSetTester, OldestLatest) { ASSERT_TRUE(latest_value != boost::none); EXPECT_EQ(latest_value->value, value_3); EXPECT_EQ(latest_value->timestamp, timestamp_3); + EXPECT_FALSE(timestamped_set.WithinBounds(timestamp_3 + 100)); + EXPECT_TRUE(timestamped_set.WithinBounds(15.1)); } ASSERT_TRUE(timestamped_set.Remove(timestamp_1)); @@ -196,6 +207,27 @@ TEST(TimestampedSetTester, OldestLatest) { } } +TEST(TimestampedSetTester, Clear) { + lc::TimestampedSet<double> timestamped_set; + EXPECT_EQ(timestamped_set.size(), 0); + EXPECT_TRUE(timestamped_set.empty()); + // Add element 1 + const double value_1 = 100.3; + const localization_common::Time timestamp_1 = 1.0; + EXPECT_TRUE(timestamped_set.Add(timestamp_1, value_1)); + EXPECT_EQ(timestamped_set.size(), 1); + EXPECT_FALSE(timestamped_set.empty()); + // Add element 2 + const double value_2 = 100.3; + const localization_common::Time timestamp_2 = 3.3; + EXPECT_TRUE(timestamped_set.Add(timestamp_2, value_2)); + EXPECT_EQ(timestamped_set.size(), 2); + // Clear + timestamped_set.Clear(); + EXPECT_EQ(timestamped_set.size(), 0); + EXPECT_TRUE(timestamped_set.empty()); +} + TEST(TimestampedSetTester, LowerAndUpperBounds) { lc::TimestampedSet<double> timestamped_set; // No elements @@ -227,8 +259,10 @@ TEST(TimestampedSetTester, LowerAndUpperBounds) { // 1 element equal { const auto lower_and_upper_bound_values = timestamped_set.LowerAndUpperBound(timestamp_1); - EXPECT_TRUE(lower_and_upper_bound_values.first == boost::none); + ASSERT_TRUE(lower_and_upper_bound_values.first != boost::none); ASSERT_TRUE(lower_and_upper_bound_values.second != boost::none); + EXPECT_EQ(lower_and_upper_bound_values.first->value, value_1); + EXPECT_EQ(lower_and_upper_bound_values.first->timestamp, timestamp_1); EXPECT_EQ(lower_and_upper_bound_values.second->value, value_1); EXPECT_EQ(lower_and_upper_bound_values.second->timestamp, timestamp_1); } @@ -256,8 +290,10 @@ TEST(TimestampedSetTester, LowerAndUpperBounds) { // 2 elements equal lower { const auto lower_and_upper_bound_values = timestamped_set.LowerAndUpperBound(timestamp_2); - EXPECT_TRUE(lower_and_upper_bound_values.first == boost::none); + ASSERT_TRUE(lower_and_upper_bound_values.first != boost::none); ASSERT_TRUE(lower_and_upper_bound_values.second != boost::none); + EXPECT_EQ(lower_and_upper_bound_values.first->value, value_2); + EXPECT_EQ(lower_and_upper_bound_values.first->timestamp, timestamp_2); EXPECT_EQ(lower_and_upper_bound_values.second->value, value_2); EXPECT_EQ(lower_and_upper_bound_values.second->timestamp, timestamp_2); } @@ -265,9 +301,9 @@ TEST(TimestampedSetTester, LowerAndUpperBounds) { { const auto lower_and_upper_bound_values = timestamped_set.LowerAndUpperBound(timestamp_1); ASSERT_TRUE(lower_and_upper_bound_values.first != boost::none); - EXPECT_EQ(lower_and_upper_bound_values.first->value, value_2); - EXPECT_EQ(lower_and_upper_bound_values.first->timestamp, timestamp_2); ASSERT_TRUE(lower_and_upper_bound_values.second != boost::none); + EXPECT_EQ(lower_and_upper_bound_values.first->value, value_1); + EXPECT_EQ(lower_and_upper_bound_values.first->timestamp, timestamp_1); EXPECT_EQ(lower_and_upper_bound_values.second->value, value_1); EXPECT_EQ(lower_and_upper_bound_values.second->timestamp, timestamp_1); } @@ -382,6 +418,62 @@ TEST(TimestampedSetTester, Closest) { EXPECT_EQ(equal_value->timestamp, timestamp_2); } +TEST(TimestampedSetTester, LatestValues) { + lc::TimestampedSet<double> timestamped_set; + const double t0 = 0; + const double v0 = lc::RandomDouble(); + const double t1 = 1.001; + const double v1 = lc::RandomDouble(); + const double t2 = 2.100; + const double v2 = lc::RandomDouble(); + const double t3 = 3.0222; + const double v3 = lc::RandomDouble(); + ASSERT_TRUE(timestamped_set.Add(t0, v0)); + ASSERT_TRUE(timestamped_set.Add(t1, v1)); + ASSERT_TRUE(timestamped_set.Add(t2, v2)); + ASSERT_TRUE(timestamped_set.Add(t3, v3)); + { + const auto latest_values = timestamped_set.LatestValues(2134); + EXPECT_EQ(latest_values.size(), 0); + } + { + const auto latest_values = timestamped_set.LatestValues((t2 + t3) / 2.0); + ASSERT_EQ(latest_values.size(), 1); + EXPECT_EQ(latest_values[0].value, v3); + EXPECT_EQ(latest_values[0].timestamp, t3); + } + { + const auto latest_values = timestamped_set.LatestValues(t2); + ASSERT_EQ(latest_values.size(), 2); + EXPECT_EQ(latest_values[0].value, v2); + EXPECT_EQ(latest_values[0].timestamp, t2); + EXPECT_EQ(latest_values[1].value, v3); + EXPECT_EQ(latest_values[1].timestamp, t3); + } + { + const auto latest_values = timestamped_set.LatestValues(0.1); + ASSERT_EQ(latest_values.size(), 3); + EXPECT_EQ(latest_values[0].value, v1); + EXPECT_EQ(latest_values[0].timestamp, t1); + EXPECT_EQ(latest_values[1].value, v2); + EXPECT_EQ(latest_values[1].timestamp, t2); + EXPECT_EQ(latest_values[2].value, v3); + EXPECT_EQ(latest_values[2].timestamp, t3); + } + { + const auto latest_values = timestamped_set.LatestValues(0); + ASSERT_EQ(latest_values.size(), 4); + EXPECT_EQ(latest_values[0].value, v0); + EXPECT_EQ(latest_values[0].timestamp, t0); + EXPECT_EQ(latest_values[1].value, v1); + EXPECT_EQ(latest_values[1].timestamp, t1); + EXPECT_EQ(latest_values[2].value, v2); + EXPECT_EQ(latest_values[2].timestamp, t2); + EXPECT_EQ(latest_values[3].value, v3); + EXPECT_EQ(latest_values[3].timestamp, t3); + } +} + TEST(TimestampedSetTester, OldValues) { lc::TimestampedSet<double> timestamped_set; const double t0 = 0; @@ -428,6 +520,52 @@ TEST(TimestampedSetTester, OldValues) { } } +TEST(TimestampedSetTester, DownsampledValues) { + lc::TimestampedSet<double> timestamped_set; + const double t0 = 0; + const double v0 = lc::RandomDouble(); + const double t1 = 1.001; + const double v1 = lc::RandomDouble(); + const double t2 = 2.100; + const double v2 = lc::RandomDouble(); + const double t3 = 3.0222; + const double v3 = lc::RandomDouble(); + ASSERT_TRUE(timestamped_set.Add(t0, v0)); + ASSERT_TRUE(timestamped_set.Add(t1, v1)); + ASSERT_TRUE(timestamped_set.Add(t2, v2)); + ASSERT_TRUE(timestamped_set.Add(t3, v3)); + { + std::set<lc::Time> allowed_timestamps; + const auto downsampled_values = timestamped_set.DownsampledValues(allowed_timestamps); + EXPECT_EQ(downsampled_values.size(), 0); + } + { + std::set<lc::Time> allowed_timestamps; + allowed_timestamps.emplace((t1 + t2) / 2.0); + const auto downsampled_values = timestamped_set.DownsampledValues(allowed_timestamps); + EXPECT_EQ(downsampled_values.size(), 0); + } + { + std::set<lc::Time> allowed_timestamps; + allowed_timestamps.emplace(t1); + const auto downsampled_values = timestamped_set.DownsampledValues(allowed_timestamps); + EXPECT_EQ(downsampled_values.size(), 1); + EXPECT_EQ(downsampled_values[0].timestamp, t1); + EXPECT_EQ(downsampled_values[0].value, v1); + } + { + std::set<lc::Time> allowed_timestamps; + allowed_timestamps.emplace(t0); + allowed_timestamps.emplace(t3); + const auto downsampled_values = timestamped_set.DownsampledValues(allowed_timestamps); + EXPECT_EQ(downsampled_values.size(), 2); + EXPECT_EQ(downsampled_values[0].timestamp, t0); + EXPECT_EQ(downsampled_values[0].value, v0); + EXPECT_EQ(downsampled_values[1].timestamp, t3); + EXPECT_EQ(downsampled_values[1].value, v3); + } +} + TEST(TimestampedSetTester, RemoveOldValues) { { lc::TimestampedSet<double> timestamped_set; @@ -532,6 +670,117 @@ TEST(TimestampedSetTester, RemoveOldValues) { } } +TEST(TimestampedSetTester, RemoveBelowLowerBoundValues) { + { + lc::TimestampedSet<double> timestamped_set; + const double t0 = 0; + const double v0 = lc::RandomDouble(); + const double t1 = 1.001; + const double v1 = lc::RandomDouble(); + const double t2 = 2.100; + const double v2 = lc::RandomDouble(); + const double t3 = 3.0222; + const double v3 = lc::RandomDouble(); + ASSERT_TRUE(timestamped_set.Add(t0, v0)); + ASSERT_TRUE(timestamped_set.Add(t1, v1)); + ASSERT_TRUE(timestamped_set.Add(t2, v2)); + ASSERT_TRUE(timestamped_set.Add(t3, v3)); + { + const int num_values_removed = timestamped_set.RemoveBelowLowerBoundValues(0); + EXPECT_EQ(num_values_removed, 0); + EXPECT_EQ(timestamped_set.size(), 4); + } + { + const int num_values_removed = timestamped_set.RemoveBelowLowerBoundValues(-1.1); + EXPECT_EQ(num_values_removed, 0); + EXPECT_EQ(timestamped_set.size(), 4); + } + } + { + lc::TimestampedSet<double> timestamped_set; + const double t0 = 0; + const double v0 = lc::RandomDouble(); + const double t1 = 1.001; + const double v1 = lc::RandomDouble(); + const double t2 = 2.100; + const double v2 = lc::RandomDouble(); + const double t3 = 3.0222; + const double v3 = lc::RandomDouble(); + ASSERT_TRUE(timestamped_set.Add(t0, v0)); + ASSERT_TRUE(timestamped_set.Add(t1, v1)); + ASSERT_TRUE(timestamped_set.Add(t2, v2)); + ASSERT_TRUE(timestamped_set.Add(t3, v3)); + const int num_values_removed = timestamped_set.RemoveBelowLowerBoundValues(0.1); + EXPECT_EQ(num_values_removed, 0); + EXPECT_EQ(timestamped_set.size(), 4); + } + { + lc::TimestampedSet<double> timestamped_set; + const double t0 = 0; + const double v0 = lc::RandomDouble(); + const double t1 = 1.001; + const double v1 = lc::RandomDouble(); + const double t2 = 2.100; + const double v2 = lc::RandomDouble(); + const double t3 = 3.0222; + const double v3 = lc::RandomDouble(); + ASSERT_TRUE(timestamped_set.Add(t0, v0)); + ASSERT_TRUE(timestamped_set.Add(t1, v1)); + ASSERT_TRUE(timestamped_set.Add(t2, v2)); + ASSERT_TRUE(timestamped_set.Add(t3, v3)); + const int num_values_removed = timestamped_set.RemoveBelowLowerBoundValues(1.334); + EXPECT_EQ(num_values_removed, 1); + EXPECT_EQ(timestamped_set.size(), 3); + const auto timestamps = timestamped_set.Timestamps(); + EXPECT_EQ(timestamps[0], t1); + EXPECT_EQ(timestamps[1], t2); + EXPECT_EQ(timestamps[2], t3); + } + + { + lc::TimestampedSet<double> timestamped_set; + const double t0 = 0; + const double v0 = lc::RandomDouble(); + const double t1 = 1.001; + const double v1 = lc::RandomDouble(); + const double t2 = 2.100; + const double v2 = lc::RandomDouble(); + const double t3 = 3.0222; + const double v3 = lc::RandomDouble(); + ASSERT_TRUE(timestamped_set.Add(t0, v0)); + ASSERT_TRUE(timestamped_set.Add(t1, v1)); + ASSERT_TRUE(timestamped_set.Add(t2, v2)); + ASSERT_TRUE(timestamped_set.Add(t3, v3)); + const int num_values_removed = timestamped_set.RemoveBelowLowerBoundValues(2.78); + EXPECT_EQ(num_values_removed, 2); + EXPECT_EQ(timestamped_set.size(), 2); + const auto timestamps = timestamped_set.Timestamps(); + EXPECT_EQ(timestamps[0], t2); + EXPECT_EQ(timestamps[1], t3); + } + + { + lc::TimestampedSet<double> timestamped_set; + const double t0 = 0; + const double v0 = lc::RandomDouble(); + const double t1 = 1.001; + const double v1 = lc::RandomDouble(); + const double t2 = 2.100; + const double v2 = lc::RandomDouble(); + const double t3 = 3.0222; + const double v3 = lc::RandomDouble(); + ASSERT_TRUE(timestamped_set.Add(t0, v0)); + ASSERT_TRUE(timestamped_set.Add(t1, v1)); + ASSERT_TRUE(timestamped_set.Add(t2, v2)); + ASSERT_TRUE(timestamped_set.Add(t3, v3)); + const int num_values_removed = timestamped_set.RemoveBelowLowerBoundValues(1923.78); + EXPECT_EQ(num_values_removed, 3); + EXPECT_EQ(timestamped_set.size(), 1); + const auto timestamps = timestamped_set.Timestamps(); + EXPECT_EQ(timestamps[0], t3); + } +} + TEST(TimestampedSetTester, Duration) { lc::TimestampedSet<double> timestamped_set; EXPECT_EQ(timestamped_set.Duration(), 0); @@ -566,6 +815,134 @@ TEST(TimestampedSetTester, Timestamps) { } } +TEST(TimestampedSetTester, InRangeValues) { + lc::TimestampedSet<double> timestamped_set; + // No elements + { + const auto range = timestamped_set.InRangeValues(0.0, 1.0); + EXPECT_TRUE(range.first == timestamped_set.cend()); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } + // 1 element + const double value_1 = -77.0; + const localization_common::Time timestamp_1 = 37.0; + ASSERT_TRUE(timestamped_set.Add(timestamp_1, value_1)); + // 1 element too low + { + const auto range = timestamped_set.InRangeValues(timestamp_1 - 2.0, timestamp_1 - 1.0); + EXPECT_TRUE(range.first == timestamped_set.cend()); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } + // 1 element too high + { + const auto range = timestamped_set.InRangeValues(timestamp_1 + 1.0, timestamp_1 + 2.0); + EXPECT_TRUE(range.first == timestamped_set.cend()); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } + // 1 element in range + { + const auto range = timestamped_set.InRangeValues(timestamp_1 - 1.0, timestamp_1 + 2.0); + EXPECT_TRUE(range.first->first == timestamp_1); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } + + // 1 element in range equal low + { + const auto range = timestamped_set.InRangeValues(timestamp_1, timestamp_1 + 2.0); + EXPECT_TRUE(range.first->first == timestamp_1); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } + // 1 element in range equal high + { + const auto range = timestamped_set.InRangeValues(timestamp_1 - 1.0, timestamp_1); + EXPECT_TRUE(range.first->first == timestamp_1); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } + + // 2 elements + const double value_2 = 512; + const localization_common::Time timestamp_2 = 2.33; + ASSERT_TRUE(timestamped_set.Add(timestamp_2, value_2)); + // 2 element too low + { + const auto range = timestamped_set.InRangeValues(timestamp_2 - 2.0, timestamp_2 - 1.0); + EXPECT_TRUE(range.first == timestamped_set.cend()); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } + // 2 element too high + { + const auto range = timestamped_set.InRangeValues(timestamp_1 + 1.0, timestamp_1 + 2.0); + EXPECT_TRUE(range.first == timestamped_set.cend()); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } + // 2 element in range + { + const auto range = timestamped_set.InRangeValues(timestamp_2 - 1.0, timestamp_1 + 2.0); + EXPECT_TRUE(range.first->first == timestamp_2); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } + // 2 element lower in range + { + const auto range = timestamped_set.InRangeValues(timestamp_2 - 1.0, timestamp_2 + 1.0); + EXPECT_TRUE(range.first->first == timestamp_2); + EXPECT_TRUE(std::prev(range.second)->first == timestamp_2); + } + + // 2 element upper in range + { + const auto range = timestamped_set.InRangeValues(timestamp_1 - 1.0, timestamp_1 + 2.0); + EXPECT_TRUE(range.first->first == timestamp_1); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } + // 2 element upper in range equal high + { + const auto range = timestamped_set.InRangeValues(timestamp_1 - 1.0, timestamp_1); + EXPECT_TRUE(range.first->first == timestamp_1); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } + // 3 elements + const double value_3 = 700; + const localization_common::Time timestamp_3 = 4.43; + ASSERT_TRUE(timestamped_set.Add(timestamp_3, value_3)); + // 3 element too low + { + const auto range = timestamped_set.InRangeValues(timestamp_2 - 2.0, timestamp_2 - 1.0); + EXPECT_TRUE(range.first == timestamped_set.cend()); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } + // 3 element too high + { + const auto range = timestamped_set.InRangeValues(timestamp_1 + 1.0, timestamp_1 + 2.0); + EXPECT_TRUE(range.first == timestamped_set.cend()); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } + // 3 element in range + { + const auto range = timestamped_set.InRangeValues(timestamp_2 - 1.0, timestamp_1 + 2.0); + EXPECT_TRUE(range.first->first == timestamp_2); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } + // 3 element 2 lower in range + { + const auto range = timestamped_set.InRangeValues(timestamp_2 - 1.0, timestamp_3 + 1.0); + EXPECT_TRUE(range.first->first == timestamp_2); + EXPECT_TRUE(std::prev(range.second)->first == timestamp_3); + } + + // 3 element 2 upper in range + { + const auto range = timestamped_set.InRangeValues(timestamp_3 - 1.0, timestamp_1 + 2.0); + EXPECT_TRUE(range.first->first == timestamp_3); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } + // 3 element 2 upper in range equal high + { + const auto range = timestamped_set.InRangeValues(timestamp_3 - 1.0, timestamp_1); + EXPECT_TRUE(range.first->first == timestamp_3); + EXPECT_TRUE(range.second == timestamped_set.cend()); + } +} + TEST(TimestampedSetTester, Serialization) { const lc::TimestampedSet<double> set; const auto serialized_set = gtsam::serializeBinary(set); diff --git a/localization/localization_common/test/test_utilities.cc b/localization/localization_common/test/test_utilities.cc index 493db4e258..722de7df60 100644 --- a/localization/localization_common/test/test_utilities.cc +++ b/localization/localization_common/test/test_utilities.cc @@ -139,6 +139,46 @@ TEST(UtilitiesTester, InterpolatePoses) { } } +TEST(UtilitiesTester, InterpolatePosesWithCovariance) { + const auto t1 = lc::RandomVector3d(); + const auto t2 = lc::RandomVector3d(); + const auto rot1 = lc::RotationFromEulerAngles(0, 0, 0); + const auto rot2 = lc::RotationFromEulerAngles(180, 0, 0); + const auto covariance = lc::PoseCovariance::Identity(); + const auto pose1 = lc::PoseWithCovariance(lc::Isometry3d(t1, rot1), covariance); + const auto pose2 = lc::PoseWithCovariance(lc::Isometry3d(t2, rot2), covariance); + + // 0 alpha + { + const auto interpolated_pose = lc::Interpolate(pose1, pose2, 0); + EXPECT_MATRIX_NEAR(interpolated_pose.pose, pose1.pose, 1e-6); + } + + // 1 alpha + { + const auto interpolated_pose = lc::Interpolate(pose1, pose2, 1); + EXPECT_MATRIX_NEAR(interpolated_pose.pose, pose2.pose, 1e-6); + } + + // 0.5 alpha + { + const auto interpolated_pose = lc::Interpolate(pose1, pose2, 0.5); + const auto expected_translation = (t1 + t2) * 0.5; + const auto expected_rotation = lc::RotationFromEulerAngles(90, 0, 0); + const auto expected_pose = lc::Isometry3d(expected_translation, expected_rotation); + EXPECT_MATRIX_NEAR(interpolated_pose.pose, expected_pose, 1e-6); + } + + // 0.3 alpha + { + const auto interpolated_pose = lc::Interpolate(pose1, pose2, 0.3); + const auto expected_translation = t1 * 0.7 + t2 * 0.3; + const auto expected_rotation = lc::RotationFromEulerAngles(180 * 0.3, 0, 0); + const auto expected_pose = lc::Isometry3d(expected_translation, expected_rotation); + EXPECT_MATRIX_NEAR(interpolated_pose.pose, expected_pose, 1e-6); + } +} + // Run all the tests that were declared with TEST() int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/localization/localization_measurements/CMakeLists.txt b/localization/localization_measurements/CMakeLists.txt index 0c097e80a1..b92efee6de 100644 --- a/localization/localization_measurements/CMakeLists.txt +++ b/localization/localization_measurements/CMakeLists.txt @@ -29,6 +29,7 @@ find_package(catkin2 REQUIRED COMPONENTS localization_common msg_conversions point_cloud_common + vision_common ) find_package(PCL REQUIRED COMPONENTS common features filters octree search) @@ -44,7 +45,7 @@ find_package(Eigen3 REQUIRED) catkin_package( LIBRARIES ${PROJECT_NAME} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ${pcl_conversions_INCLUDE_DIRS} INCLUDE_DIRS include ${PCL_INCLUDE_DIRS} - CATKIN_DEPENDS config_reader cv_bridge ff_msgs localization_common msg_conversions point_cloud_common + CATKIN_DEPENDS config_reader cv_bridge ff_msgs localization_common msg_conversions point_cloud_common vision_common ) ########### diff --git a/localization/localization_measurements/include/localization_measurements/feature_points_measurement.h b/localization/localization_measurements/include/localization_measurements/feature_points_measurement.h index d93d407c7e..7e354dac27 100644 --- a/localization/localization_measurements/include/localization_measurements/feature_points_measurement.h +++ b/localization/localization_measurements/include/localization_measurements/feature_points_measurement.h @@ -20,14 +20,14 @@ #define LOCALIZATION_MEASUREMENTS_FEATURE_POINTS_MEASUREMENT_H_ #include <localization_common/time.h> -#include <localization_measurements/feature_point.h> #include <localization_measurements/measurement.h> +#include <vision_common/feature_point.h> #include <vector> namespace localization_measurements { struct FeaturePointsMeasurement : public Measurement { - FeaturePoints feature_points; + vision_common::FeaturePoints feature_points; }; } // namespace localization_measurements diff --git a/localization/localization_measurements/include/localization_measurements/imu_measurement.h b/localization/localization_measurements/include/localization_measurements/imu_measurement.h index 60075a803e..1b802f8cdb 100644 --- a/localization/localization_measurements/include/localization_measurements/imu_measurement.h +++ b/localization/localization_measurements/include/localization_measurements/imu_measurement.h @@ -42,6 +42,8 @@ struct ImuMeasurement : public Measurement { const localization_common::Time timestamp) : Measurement(timestamp), acceleration(acceleration), angular_velocity(angular_velocity) {} + ImuMeasurement() = default; + Eigen::Vector3d acceleration; Eigen::Vector3d angular_velocity; }; diff --git a/localization/localization_measurements/include/localization_measurements/matched_projections_measurement.h b/localization/localization_measurements/include/localization_measurements/matched_projections_measurement.h index 38f2bc675b..c0809172c0 100644 --- a/localization/localization_measurements/include/localization_measurements/matched_projections_measurement.h +++ b/localization/localization_measurements/include/localization_measurements/matched_projections_measurement.h @@ -28,7 +28,7 @@ namespace localization_measurements { struct MatchedProjectionsMeasurement : public Measurement { MatchedProjections matched_projections; - // TODO(soussan): put this somewhere else? + // TODO(soussan): Make this optional? Move somewhere else? gtsam::Pose3 global_T_cam; }; } // namespace localization_measurements diff --git a/localization/graph_localizer/include/graph_localizer/calibration_params.h b/localization/localization_measurements/include/localization_measurements/pose_measurement.h similarity index 59% rename from localization/graph_localizer/include/graph_localizer/calibration_params.h rename to localization/localization_measurements/include/localization_measurements/pose_measurement.h index 9705e16c94..aea24c9760 100644 --- a/localization/graph_localizer/include/graph_localizer/calibration_params.h +++ b/localization/localization_measurements/include/localization_measurements/pose_measurement.h @@ -15,21 +15,21 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef GRAPH_LOCALIZER_CALIBRATION_PARAMS_H_ -#define GRAPH_LOCALIZER_CALIBRATION_PARAMS_H_ -#include <gtsam/geometry/Cal3_S2.h> +#ifndef LOCALIZATION_MEASUREMENTS_POSE_MEASUREMENT_H_ +#define LOCALIZATION_MEASUREMENTS_POSE_MEASUREMENT_H_ + +#include <localization_common/time.h> +#include <localization_measurements/measurement.h> + #include <gtsam/geometry/Pose3.h> -namespace graph_localizer { -struct CalibrationParams { - gtsam::Pose3 body_T_dock_cam; - gtsam::Pose3 body_T_nav_cam; - gtsam::Pose3 body_T_perch_cam; - gtsam::Pose3 world_T_dock; - boost::shared_ptr<gtsam::Cal3_S2> nav_cam_intrinsics; - boost::shared_ptr<gtsam::Cal3_S2> dock_cam_intrinsics; +namespace localization_measurements { +struct PoseMeasurement : public Measurement { + PoseMeasurement(const gtsam::Pose3& pose, const localization_common::Time timestamp) + : Measurement(timestamp), pose(pose) {} + gtsam::Pose3 pose; }; -} // namespace graph_localizer +} // namespace localization_measurements -#endif // GRAPH_LOCALIZER_CALIBRATION_PARAMS_H_ +#endif // LOCALIZATION_MEASUREMENTS_POSE_MEASUREMENT_H_ diff --git a/localization/localization_measurements/include/localization_measurements/pose_with_covariance_measurement.h b/localization/localization_measurements/include/localization_measurements/pose_with_covariance_measurement.h new file mode 100644 index 0000000000..65f89394b1 --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/pose_with_covariance_measurement.h @@ -0,0 +1,57 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_MEASUREMENTS_POSE_WITH_COVARIANCE_MEASUREMENT_H_ +#define LOCALIZATION_MEASUREMENTS_POSE_WITH_COVARIANCE_MEASUREMENT_H_ + +#include <localization_common/pose_with_covariance.h> +#include <localization_common/time.h> +#include <localization_common/timestamped_set.h> +#include <localization_measurements/measurement.h> + +#include <gtsam/geometry/Pose3.h> + +namespace localization_measurements { +using PoseCovariance = Eigen::Matrix<double, 6, 6>; +struct PoseWithCovarianceMeasurement : public Measurement { + PoseWithCovarianceMeasurement(const gtsam::Pose3& pose, const PoseCovariance& covariance, + const localization_common::Time timestamp, + const localization_common::TimestampedSet<PoseCovariance>& correlation_covariances = {}) + : Measurement(timestamp), pose(pose), covariance(covariance), correlation_covariances(correlation_covariances) {} + + PoseWithCovarianceMeasurement(const localization_common::PoseWithCovariance& pose_with_covariance, + const localization_common::Time timestamp) + : Measurement(timestamp), + pose(localization_common::GtPose(pose_with_covariance.pose)), + covariance(pose_with_covariance.covariance), + correlation_covariances(pose_with_covariance.correlation_covariances) {} + + localization_common::PoseWithCovariance PoseWithCovariance() const { + return localization_common::PoseWithCovariance(localization_common::EigenPose(pose), covariance, + correlation_covariances); + } + + PoseWithCovarianceMeasurement() = default; + + gtsam::Pose3 pose; + PoseCovariance covariance; + localization_common::TimestampedSet<PoseCovariance> correlation_covariances; +}; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_POSE_WITH_COVARIANCE_MEASUREMENT_H_ diff --git a/localization/localization_measurements/include/localization_measurements/relative_pose_with_covariance_measurement.h b/localization/localization_measurements/include/localization_measurements/relative_pose_with_covariance_measurement.h new file mode 100644 index 0000000000..03fec5fdc6 --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/relative_pose_with_covariance_measurement.h @@ -0,0 +1,45 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_MEASUREMENTS_RELATIVE_POSE_WITH_COVARIANCE_MEASUREMENT_H_ +#define LOCALIZATION_MEASUREMENTS_RELATIVE_POSE_WITH_COVARIANCE_MEASUREMENT_H_ + +#include <localization_common/time.h> +#include <localization_measurements/measurement.h> + +#include <gtsam/geometry/Pose3.h> + +namespace localization_measurements { +using PoseCovariance = Eigen::Matrix<double, 6, 6>; +struct RelativePoseWithCovarianceMeasurement : public Measurement { + RelativePoseWithCovarianceMeasurement(const gtsam::Pose3& relative_pose, const PoseCovariance& covariance, + const localization_common::Time timestamp_a, + const localization_common::Time timestamp_b) + : Measurement(timestamp_b), + relative_pose(relative_pose), + covariance(covariance), + timestamp_a(timestamp_a), + timestamp_b(timestamp_b) {} + gtsam::Pose3 relative_pose; + PoseCovariance covariance; + localization_common::Time timestamp_a; + localization_common::Time timestamp_b; +}; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_RELATIVE_POSE_WITH_COVARIANCE_MEASUREMENT_H_ diff --git a/localization/localization_measurements/include/localization_measurements/standstill_measurement.h b/localization/localization_measurements/include/localization_measurements/standstill_measurement.h new file mode 100644 index 0000000000..a8caa4d639 --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/standstill_measurement.h @@ -0,0 +1,35 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_MEASUREMENTS_STANDSTILL_MEASUREMENT_H_ +#define LOCALIZATION_MEASUREMENTS_STANDSTILL_MEASUREMENT_H_ + +#include <localization_common/time.h> +#include <localization_measurements/depth_correspondences.h> +#include <localization_measurements/measurement.h> +#include <localization_measurements/odometry.h> + +namespace localization_measurements { +struct StandstillMeasurement : public Measurement { + StandstillMeasurement(const localization_common::Time timestamp, const localization_common::Time previous_timestamp) + : Measurement(timestamp), previous_timestamp(previous_timestamp) {} + localization_common::Time previous_timestamp; +}; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_STANDSTILL_MEASUREMENT_H_ diff --git a/localization/localization_measurements/package.xml b/localization/localization_measurements/package.xml index d45553426b..4ce605fcf1 100644 --- a/localization/localization_measurements/package.xml +++ b/localization/localization_measurements/package.xml @@ -21,6 +21,7 @@ <build_depend>msg_conversions</build_depend> <build_depend>pcl_conversions</build_depend> <build_depend>point_cloud_common</build_depend> + <build_depend>vision_common</build_depend> <run_depend>config_reader</run_depend> <run_depend>cv_bridge</run_depend> <run_depend>ff_msgs</run_depend> @@ -28,4 +29,5 @@ <run_depend>msg_conversions</run_depend> <run_depend>pcl_conversions</run_depend> <run_depend>point_cloud_common</run_depend> + <run_depend>vision_common</run_depend> </package> diff --git a/localization/localization_measurements/src/measurement_conversions.cc b/localization/localization_measurements/src/measurement_conversions.cc index 106bd8ae94..d4aa45f52d 100644 --- a/localization/localization_measurements/src/measurement_conversions.cc +++ b/localization/localization_measurements/src/measurement_conversions.cc @@ -119,7 +119,7 @@ FeaturePointsMeasurement MakeFeaturePointsMeasurement(const ff_msgs::Feature2dAr for (const auto& feature : optical_flow_feature_points.feature_array) { feature_points_measurement.feature_points.emplace_back( - FeaturePoint(feature.x, feature.y, image_id, feature.id, timestamp)); + vision_common::FeaturePoint(feature.x, feature.y, image_id, feature.id, timestamp)); } return feature_points_measurement; diff --git a/localization/node_adders/CMakeLists.txt b/localization/node_adders/CMakeLists.txt new file mode 100644 index 0000000000..6999477c29 --- /dev/null +++ b/localization/node_adders/CMakeLists.txt @@ -0,0 +1,103 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(node_adders) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + imu_integration + localization_common + nodes +) + +# Find GTSAM +find_package(GTSAM REQUIRED) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +# include ff_nodelet to get ff_util header files since these aren't exposed elsewhere +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS imu_integration localization_common nodes +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${GLOG_INCLUDE_DIRS} +) + +file(GLOB cc_files + "src/*.cc" +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + ${cc_files} +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_pose_node_adder + test/test_pose_node_adder.test + test/test_pose_node_adder.cc + test/test_utilities.cc + ) + target_link_libraries(test_pose_node_adder + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_combined_nav_state_node_adder_model + test/test_combined_nav_state_node_adder_model.test + test/test_combined_nav_state_node_adder_model.cc + test/test_utilities.cc + ) + target_link_libraries(test_combined_nav_state_node_adder_model + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/node_adders/include/node_adders/between_factor_node_adder_model.h b/localization/node_adders/include/node_adders/between_factor_node_adder_model.h new file mode 100644 index 0000000000..bc7801236d --- /dev/null +++ b/localization/node_adders/include/node_adders/between_factor_node_adder_model.h @@ -0,0 +1,173 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef NODE_ADDERS_BETWEEN_FACTOR_NODE_ADDER_MODEL_H_ +#define NODE_ADDERS_BETWEEN_FACTOR_NODE_ADDER_MODEL_H_ + +#include <node_adders/timestamped_node_adder_model.h> +#include <node_adders/measurement_based_timestamped_node_adder_model.h> +#include <node_adders/utilities.h> +#include <nodes/timestamped_nodes.h> + +#include <gtsam/inference/Key.h> +#include <gtsam/geometry/Pose3.h> +#include <gtsam/nonlinear/NonlinearFactorGraph.h> +#include <gtsam/slam/BetweenFactor.h> + +#include <utility> +#include <vector> + +namespace node_adders { +// Node adder model that adds GTSAM between factors as relative factors and GTSAM prior factors as priors +// for a given node type. +template <typename NodeType, typename NodeAdderModelType> +class BetweenFactorNodeAdderModel : public NodeAdderModelType { + public: + using NodesType = nodes::TimestampedNodes<NodeType>; + using Base = NodeAdderModelType; + explicit BetweenFactorNodeAdderModel(const TimestampedNodeAdderModelParams& params) : Base(params) {} + virtual ~BetweenFactorNodeAdderModel() = default; + // Adds prior factors for a given node using provided noise models. + void AddPriors(const NodeType& node, const std::vector<gtsam::SharedNoiseModel>& noise_models, + const localization_common::Time timestamp, const NodesType& nodes, + gtsam::NonlinearFactorGraph& factors) const final; + + // Adds nodes for provided timestamps and connects them with between factors. + bool AddNodesAndRelativeFactors(const localization_common::Time timestamp_a, + const localization_common::Time timestamp_b, NodesType& nodes, + gtsam::NonlinearFactorGraph& factors) const final; + + // Connects nodes at given timestamps with between factors. + bool AddRelativeFactors(const localization_common::Time timestamp_a, const localization_common::Time timestamp_b, + const NodesType& nodes, gtsam::NonlinearFactorGraph& factors) const final; + + bool RemoveRelativeFactors(const localization_common::Time timestamp_a, const localization_common::Time timestamp_b, + const NodesType& nodes, gtsam::NonlinearFactorGraph& factors) const final; + + private: + // Connects nodes at given keys with between factors. + bool AddRelativeFactors(const gtsam::KeyVector& keys_a, const localization_common::Time timestamp_a, + const gtsam::KeyVector& keys_b, const localization_common::Time timestamp_b, + gtsam::NonlinearFactorGraph& factors) const; + + // Adds a node at the given timestamp. + // Needs to be implemented by the child class + virtual gtsam::KeyVector AddNode(const localization_common::Time timestamp, NodesType& nodes) const = 0; + + // Creates a node with noise at timestamp_b relative to timestamp_a. + // Needs to be implemented by the child class + virtual boost::optional<std::pair<NodeType, gtsam::SharedNoiseModel>> RelativeNodeAndNoise( + const localization_common::Time timestamp_a, const localization_common::Time timestamp_b) const = 0; + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; + +// Implementation +template <typename NodeType, typename NodeAdderModelType> +void BetweenFactorNodeAdderModel<NodeType, NodeAdderModelType>::AddPriors( + const NodeType& node, const std::vector<gtsam::SharedNoiseModel>& noise_models, + const localization_common::Time timestamp, const NodesType& nodes, gtsam::NonlinearFactorGraph& factors) const { + const auto keys = nodes.Keys(timestamp); + if (keys.empty()) { + LogError("AddPriors: Failed to get keys."); + return; + } + // TODO(rsoussan): Ensure symbols not used by other node adders + gtsam::PriorFactor<NodeType> prior_factor(keys[0], node, noise_models[0]); + factors.push_back(prior_factor); +} + +template <typename NodeType, typename NodeAdderModelType> +bool BetweenFactorNodeAdderModel<NodeType, NodeAdderModelType>::AddNodesAndRelativeFactors( + const localization_common::Time timestamp_a, const localization_common::Time timestamp_b, NodesType& nodes, + gtsam::NonlinearFactorGraph& factors) const { + if (!nodes.Contains(timestamp_b)) { + const auto keys = AddNode(timestamp_b, nodes); + if (keys.empty()) { + LogError("AddNodesAndRelativeFactors: Failed to add node."); + return false; + } + } + + if (!AddRelativeFactors(timestamp_a, timestamp_b, nodes, factors)) { + LogError("AddNodesAndRelativeFactors: Failed to add relative factor."); + return false; + } + return true; +} + +template <typename NodeType, typename NodeAdderModelType> +bool BetweenFactorNodeAdderModel<NodeType, NodeAdderModelType>::AddRelativeFactors( + const localization_common::Time timestamp_a, const localization_common::Time timestamp_b, const NodesType& nodes, + gtsam::NonlinearFactorGraph& factors) const { + const auto keys_a = nodes.Keys(timestamp_a); + if (keys_a.empty()) { + LogError("AddRelativeFactors: Failed to get keys a."); + return false; + } + const auto keys_b = nodes.Keys(timestamp_b); + if (keys_b.empty()) { + LogError("AddRelativeFactors: Failed to get keys b."); + return false; + } + if (!AddRelativeFactors(keys_a, timestamp_a, keys_b, timestamp_b, factors)) { + LogError("AddRelativeFactor: Failed to add relative factor."); + return false; + } + return true; +} + +template <typename NodeType, typename NodeAdderModelType> +bool BetweenFactorNodeAdderModel<NodeType, NodeAdderModelType>::AddRelativeFactors( + const gtsam::KeyVector& keys_a, const localization_common::Time timestamp_a, const gtsam::KeyVector& keys_b, + const localization_common::Time timestamp_b, gtsam::NonlinearFactorGraph& factors) const { + const auto relative_node_and_noise = RelativeNodeAndNoise(timestamp_a, timestamp_b); + if (!relative_node_and_noise) { + LogError("AddRelativeFactor: Failed to add relative node and noise."); + return false; + } + + typename gtsam::BetweenFactor<NodeType>::shared_ptr relative_factor(new gtsam::BetweenFactor<NodeType>( + keys_a[0], keys_b[0], relative_node_and_noise->first, relative_node_and_noise->second)); + factors.push_back(relative_factor); + return true; +} + +template <typename NodeType, typename NodeAdderModelType> +bool BetweenFactorNodeAdderModel<NodeType, NodeAdderModelType>::RemoveRelativeFactors( + const localization_common::Time timestamp_a, const localization_common::Time timestamp_b, const NodesType& nodes, + gtsam::NonlinearFactorGraph& factors) const { + return RemoveRelativeFactor<gtsam::BetweenFactor<NodeType>>(timestamp_a, timestamp_b, nodes, factors); +} + +// Specialization helpers +template <typename MeasurementType, typename NodeType> +using BetweenFactorMeasurementBasedTimestampedNodeAdderModel = BetweenFactorNodeAdderModel< + NodeType, MeasurementBasedTimestampedNodeAdderModel<MeasurementType, NodeType, nodes::TimestampedNodes<NodeType>>>; + +template <typename NodeType> +using BetweenFactorTimestampedNodeAdderModel = + BetweenFactorNodeAdderModel<NodeType, TimestampedNodeAdderModel<NodeType, nodes::TimestampedNodes<NodeType>>>; +} // namespace node_adders + +#endif // NODE_ADDERS_BETWEEN_FACTOR_NODE_ADDER_MODEL_H_ diff --git a/localization/node_adders/include/node_adders/combined_nav_state_node_adder.h b/localization/node_adders/include/node_adders/combined_nav_state_node_adder.h new file mode 100644 index 0000000000..7b040da50b --- /dev/null +++ b/localization/node_adders/include/node_adders/combined_nav_state_node_adder.h @@ -0,0 +1,33 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef NODE_ADDERS_COMBINED_NAV_STATE_NODE_ADDER_H_ +#define NODE_ADDERS_COMBINED_NAV_STATE_NODE_ADDER_H_ + +#include <localization_measurements/imu_measurement.h> +#include <node_adders/combined_nav_state_node_adder_model.h> +#include <node_adders/measurement_based_timestamped_node_adder.h> +#include <nodes/combined_nav_state_nodes.h> + +namespace node_adders { +using CombinedNavStateNodeAdder = + MeasurementBasedTimestampedNodeAdder<localization_measurements::ImuMeasurement, localization_common::CombinedNavState, + nodes::CombinedNavStateNodes, CombinedNavStateNodeAdderModel>; +} // namespace node_adders + +#endif // NODE_ADDERS_COMBINED_NAV_STATE_NODE_ADDER_H_ diff --git a/localization/node_adders/include/node_adders/combined_nav_state_node_adder_model.h b/localization/node_adders/include/node_adders/combined_nav_state_node_adder_model.h new file mode 100644 index 0000000000..9d6e8b9ac2 --- /dev/null +++ b/localization/node_adders/include/node_adders/combined_nav_state_node_adder_model.h @@ -0,0 +1,82 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef NODE_ADDERS_COMBINED_NAV_STATE_NODE_ADDER_MODEL_H_ +#define NODE_ADDERS_COMBINED_NAV_STATE_NODE_ADDER_MODEL_H_ + +#include <imu_integration/imu_integrator.h> +#include <localization_common/combined_nav_state.h> +#include <localization_measurements/imu_measurement.h> +#include <node_adders/combined_nav_state_node_adder_model_params.h> +#include <node_adders/measurement_based_timestamped_node_adder_model.h> +#include <nodes/combined_nav_state_nodes.h> + +#include <gtsam/nonlinear/NonlinearFactorGraph.h> + +#include <utility> +#include <vector> + +namespace node_adders { +class CombinedNavStateNodeAdderModel + : public MeasurementBasedTimestampedNodeAdderModel<localization_measurements::ImuMeasurement, + localization_common::CombinedNavState, + nodes::CombinedNavStateNodes> { + public: + using NodeType = localization_common::CombinedNavState; + using NodesType = nodes::CombinedNavStateNodes; + using MeasurementType = localization_measurements::ImuMeasurement; + using Params = CombinedNavStateNodeAdderModelParams; + using Base = MeasurementBasedTimestampedNodeAdderModel<MeasurementType, NodeType, NodesType>; + explicit CombinedNavStateNodeAdderModel(const Params& params); + void AddPriors(const NodeType& node, const std::vector<gtsam::SharedNoiseModel>& noise_models, + const localization_common::Time timestamp, const NodesType& nodes, + gtsam::NonlinearFactorGraph& factors) const final; + bool AddNodesAndRelativeFactors(const localization_common::Time timestamp_a, + const localization_common::Time timestamp_b, NodesType& nodes, + gtsam::NonlinearFactorGraph& factors) const final; + bool AddRelativeFactors(const localization_common::Time timestamp_a, const localization_common::Time timestamp_b, + const NodesType& nodes, gtsam::NonlinearFactorGraph& factors) const final; + + void AddMeasurement(const localization_measurements::ImuMeasurement& measurement) final; + void RemoveMeasurements(const localization_common::Time oldest_allowed_time) final; + + bool CanAddNode(const localization_common::Time timestamp) const final; + + bool RemoveRelativeFactors(const localization_common::Time timestamp_a, const localization_common::Time timestamp_b, + const NodesType& nodes, gtsam::NonlinearFactorGraph& factors) const final; + + void SetFanSpeedMode(const localization_measurements::FanSpeedMode& fan_speed_mode); + + private: + bool AddRelativeFactors(const gtsam::KeyVector& keys_a, const localization_common::Time timestamp_a, + const gtsam::KeyVector& keys_b, const localization_common::Time timestamp_b, + gtsam::NonlinearFactorGraph& factors) const; + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(imu_integrator_); + } + + imu_integration::ImuIntegrator imu_integrator_; +}; +} // namespace node_adders + +#endif // NODE_ADDERS_COMBINED_NAV_STATE_NODE_ADDER_MODEL_H_ diff --git a/localization/node_adders/include/node_adders/combined_nav_state_node_adder_model_params.h b/localization/node_adders/include/node_adders/combined_nav_state_node_adder_model_params.h new file mode 100644 index 0000000000..7553d32c99 --- /dev/null +++ b/localization/node_adders/include/node_adders/combined_nav_state_node_adder_model_params.h @@ -0,0 +1,42 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef NODE_ADDERS_COMBINED_NAV_STATE_NODE_ADDER_MODEL_PARAMS_H_ +#define NODE_ADDERS_COMBINED_NAV_STATE_NODE_ADDER_MODEL_PARAMS_H_ + +#include <node_adders/timestamped_node_adder_model_params.h> + +#include <imu_integration/imu_integrator_params.h> + +#include <boost/serialization/serialization.hpp> + +namespace node_adders { +struct CombinedNavStateNodeAdderModelParams : public TimestampedNodeAdderModelParams { + imu_integration::ImuIntegratorParams imu_integrator; + + private: + // Serialization function + friend class boost::serialization::access; + template <class ARCHIVE> + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TimestampedNodeAdderModelParams); + ar& BOOST_SERIALIZATION_NVP(imu_integrator); + } +}; +} // namespace node_adders + +#endif // NODE_ADDERS_COMBINED_NAV_STATE_NODE_ADDER_MODEL_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/feature_point_node_updater.h b/localization/node_adders/include/node_adders/feature_point_node_updater.h_old similarity index 61% rename from localization/graph_localizer/include/graph_localizer/feature_point_node_updater.h rename to localization/node_adders/include/node_adders/feature_point_node_updater.h_old index 4928d6f8a5..00a333972c 100644 --- a/localization/graph_localizer/include/graph_localizer/feature_point_node_updater.h +++ b/localization/node_adders/include/node_adders/feature_point_node_updater.h_old @@ -16,36 +16,37 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_FEATURE_POINT_NODE_UPDATER_H_ -#define GRAPH_LOCALIZER_FEATURE_POINT_NODE_UPDATER_H_ +#ifndef NODE_ADDERS_FEATURE_POINT_NODE_ADDER_H_ +#define NODE_ADDERS_FEATURE_POINT_NODE_ADDER_H_ -#include <graph_optimizer/node_updater_with_priors.h> -#include <graph_localizer/feature_point_graph_values.h> -#include <graph_localizer/feature_point_node_updater_params.h> +#include <graph_optimizer/node_adder_with_priors.h> +#include <graph_values/feature_point_graph_values.h> +#include <node_adders/feature_point_node_adder_params.h> #include <localization_common/feature_point_3d.h> -namespace graph_localizer { -class FeaturePointNodeUpdater - : public graph_optimizer::NodeUpdaterWithPriors<localization_common::FeaturePoint3d, +namespace node_adders { +class FeaturePointNodeAdder + : public graph_optimizer::NodeAdderWithPriors<localization_common::FeaturePoint3d, localization_common::FeaturePoint3dNoise> { public: - FeaturePointNodeUpdater(const FeaturePointNodeUpdaterParams& params, std::shared_ptr<gtsam::Values> values); + FeaturePointNodeAdder(const FeaturePointNodeAdderParams& params, std::shared_ptr<gtsam::Values> values); void AddInitialValuesAndPriors(const localization_common::FeaturePoint3d& global_N_body, const localization_common::FeaturePoint3dNoise& noise, - gtsam::NonlinearFactorGraph& factors) final; + const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final; void AddPriors(const localization_common::FeaturePoint3d& global_N_body, - const localization_common::FeaturePoint3dNoise& noise, gtsam::NonlinearFactorGraph& factors) final; + const localization_common::FeaturePoint3dNoise& noise, const localization_common::Time timestamp, + gtsam::NonlinearFactorGraph& factors); - bool Update(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final; + bool Adder(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final; bool SlideWindow(const localization_common::Time oldest_allowed_timestamp, const boost::optional<gtsam::Marginals>& marginals, const gtsam::KeyVector& old_keys, const double huber_k, gtsam::NonlinearFactorGraph& factors) final; - void UpdatePointPriors(const gtsam::Marginals& marginals, gtsam::NonlinearFactorGraph& factors); + void AdderPointPriors(const gtsam::Marginals& marginals, gtsam::NonlinearFactorGraph& factors); - graph_optimizer::NodeUpdaterType type() const final; + graph_optimizer::NodeAdderType type() const final; boost::optional<localization_common::Time> SlideWindowNewOldestTime() const final; @@ -61,14 +62,14 @@ class FeaturePointNodeUpdater int NumFeatures() const; - std::shared_ptr<FeaturePointGraphValues> shared_feature_point_graph_values(); + std::shared_ptr<graph_values::FeaturePointGraphValues> shared_feature_point_graph_values(); - const FeaturePointGraphValues& feature_point_graph_values() const; + const graph_values::FeaturePointGraphValues& feature_point_graph_values() const; private: - FeaturePointNodeUpdaterParams params_; - std::shared_ptr<FeaturePointGraphValues> feature_point_graph_values_; + FeaturePointNodeAdderParams params_; + std::shared_ptr<graph_values::FeaturePointGraphValues> feature_point_graph_values_; }; -} // namespace graph_localizer +} // namespace node_adders -#endif // GRAPH_LOCALIZER_FEATURE_POINT_NODE_UPDATER_H_ +#endif // NODE_ADDERS_FEATURE_POINT_NODE_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/feature_point_node_updater_params.h b/localization/node_adders/include/node_adders/feature_point_node_updater_params.h_old similarity index 74% rename from localization/graph_localizer/include/graph_localizer/feature_point_node_updater_params.h rename to localization/node_adders/include/node_adders/feature_point_node_updater_params.h_old index 1aca866e0d..dd68c55879 100644 --- a/localization/graph_localizer/include/graph_localizer/feature_point_node_updater_params.h +++ b/localization/node_adders/include/node_adders/feature_point_node_updater_params.h_old @@ -15,15 +15,15 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef GRAPH_LOCALIZER_FEATURE_POINT_NODE_UPDATER_PARAMS_H_ -#define GRAPH_LOCALIZER_FEATURE_POINT_NODE_UPDATER_PARAMS_H_ +#ifndef NODE_ADDERS_FEATURE_POINT_NODE_ADDER_PARAMS_H_ +#define NODE_ADDERS_FEATURE_POINT_NODE_ADDER_PARAMS_H_ #include <localization_common/combined_nav_state.h> -namespace graph_localizer { -struct FeaturePointNodeUpdaterParams { +namespace node_adders { +struct FeaturePointNodeAdderParams { double huber_k; }; -} // namespace graph_localizer +} // namespace node_adders -#endif // GRAPH_LOCALIZER_FEATURE_POINT_NODE_UPDATER_PARAMS_H_ +#endif // NODE_ADDERS_FEATURE_POINT_NODE_ADDER_PARAMS_H_ diff --git a/localization/node_adders/include/node_adders/measurement_based_timestamped_node_adder.h b/localization/node_adders/include/node_adders/measurement_based_timestamped_node_adder.h new file mode 100644 index 0000000000..d83cc3b8bc --- /dev/null +++ b/localization/node_adders/include/node_adders/measurement_based_timestamped_node_adder.h @@ -0,0 +1,119 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef NODE_ADDERS_MEASUREMENT_BASED_TIMESTAMPED_NODE_ADDER_H_ +#define NODE_ADDERS_MEASUREMENT_BASED_TIMESTAMPED_NODE_ADDER_H_ + +#include <node_adders/timestamped_node_adder.h> +#include <node_adders/timestamped_node_adder_params.h> + +namespace node_adders { + +// Timestamped node adder that uses provided measurements to create nodes. +template <typename MeasurementType, typename NodeType, typename TimestampedNodesType, + typename MeasurementBasedTimestampedNodeAdderModelType> +class MeasurementBasedTimestampedNodeAdder + : public TimestampedNodeAdder<NodeType, TimestampedNodesType, MeasurementBasedTimestampedNodeAdderModelType> { + using Base = TimestampedNodeAdder<NodeType, TimestampedNodesType, MeasurementBasedTimestampedNodeAdderModelType>; + + public: + using Params = TimestampedNodeAdderParams<NodeType>; + // Construct using nodes. Creates timestamped nodes interally. + MeasurementBasedTimestampedNodeAdder( + const Params& params, const typename MeasurementBasedTimestampedNodeAdderModelType::Params& node_adder_model_params, + std::shared_ptr<nodes::Values> values); + + // Construct using already constructed timestamped nodes. + MeasurementBasedTimestampedNodeAdder( + const Params& params, const typename MeasurementBasedTimestampedNodeAdderModelType::Params& node_adder_model_params, + std::shared_ptr<TimestampedNodesType> timestamped_nodes); + + void AddMeasurement(const MeasurementType& measurement); + + // Removes measurements using oldest_allowed_time. Depending on the measurement type, + // the lower bound <= oldest_allowed_time may be kept so it can be used to create + // relative factors in the future. + void RemoveMeasurements(const localization_common::Time oldest_allowed_time); + + // Slides window and removes old measurements + bool SlideWindow(const localization_common::Time oldest_allowed_timestamp, + const boost::optional<const gtsam::Marginals&>& marginals, const gtsam::KeyVector& old_keys, + const double huber_k, gtsam::NonlinearFactorGraph& factors) final; + + private: + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; + +// Implementation +template <typename MeasurementType, typename NodeType, typename TimestampedNodesType, + typename MeasurementBasedTimestampedNodeAdderModelType> +MeasurementBasedTimestampedNodeAdder<MeasurementType, NodeType, TimestampedNodesType, + MeasurementBasedTimestampedNodeAdderModelType>:: + MeasurementBasedTimestampedNodeAdder( + const Params& params, const typename MeasurementBasedTimestampedNodeAdderModelType::Params& node_adder_model_params, + std::shared_ptr<nodes::Values> values) + : Base(params, node_adder_model_params, std::move(values)) {} + +template <typename MeasurementType, typename NodeType, typename TimestampedNodesType, + typename MeasurementBasedTimestampedNodeAdderModelType> +MeasurementBasedTimestampedNodeAdder<MeasurementType, NodeType, TimestampedNodesType, + MeasurementBasedTimestampedNodeAdderModelType>:: + MeasurementBasedTimestampedNodeAdder( + const Params& params, const typename MeasurementBasedTimestampedNodeAdderModelType::Params& node_adder_model_params, + std::shared_ptr<TimestampedNodesType> timestamped_nodes) + : Base(params, node_adder_model_params, std::move(timestamped_nodes)) {} + +template <typename MeasurementType, typename NodeType, typename TimestampedNodesType, + typename MeasurementBasedTimestampedNodeAdderModelType> +void MeasurementBasedTimestampedNodeAdder< + MeasurementType, NodeType, TimestampedNodesType, + MeasurementBasedTimestampedNodeAdderModelType>::AddMeasurement(const MeasurementType& measurement) { + Base::node_adder_model().AddMeasurement(measurement); +} + +template <typename MeasurementType, typename NodeType, typename TimestampedNodesType, + typename MeasurementBasedTimestampedNodeAdderModelType> +void MeasurementBasedTimestampedNodeAdder<MeasurementType, NodeType, TimestampedNodesType, + MeasurementBasedTimestampedNodeAdderModelType>:: + RemoveMeasurements(const localization_common::Time oldest_allowed_time) { + Base::node_adder_model().RemoveMeasurements(oldest_allowed_time); +} + +template <typename MeasurementType, typename NodeType, typename TimestampedNodesType, + typename MeasurementBasedTimestampedNodeAdderModelType> +bool MeasurementBasedTimestampedNodeAdder< + MeasurementType, NodeType, TimestampedNodesType, + MeasurementBasedTimestampedNodeAdderModelType>::SlideWindow(const localization_common::Time oldest_allowed_timestamp, + const boost::optional<const gtsam::Marginals&>& marginals, + const gtsam::KeyVector& old_keys, const double huber_k, + gtsam::NonlinearFactorGraph& factors) { + if (!Base::SlideWindow(oldest_allowed_timestamp, marginals, old_keys, huber_k, factors)) { + LogError("Failed to slide window."); + return false; + } + RemoveMeasurements(oldest_allowed_timestamp); + return true; +} +} // namespace node_adders + +#endif // NODE_ADDERS_MEASUREMENT_BASED_TIMESTAMPED_NODE_ADDER_H_ diff --git a/localization/node_adders/include/node_adders/measurement_based_timestamped_node_adder_model.h b/localization/node_adders/include/node_adders/measurement_based_timestamped_node_adder_model.h new file mode 100644 index 0000000000..74fdca8005 --- /dev/null +++ b/localization/node_adders/include/node_adders/measurement_based_timestamped_node_adder_model.h @@ -0,0 +1,42 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef NODE_ADDERS_MEASUREMENT_BASED_TIMESTAMPED_NODE_ADDER_MODEL_H_ +#define NODE_ADDERS_MEASUREMENT_BASED_TIMESTAMPED_NODE_ADDER_MODEL_H_ + +#include <node_adders/timestamped_node_adder_model.h> + +#include <gtsam/inference/Key.h> +#include <gtsam/nonlinear/NonlinearFactorGraph.h> + +#include <vector> + +namespace node_adders { +// Node adder model that uses provided measurements to generate nodes and relative factors. +template <typename MeasurementType, typename NodeType, typename NodesType> +class MeasurementBasedTimestampedNodeAdderModel : public TimestampedNodeAdderModel<NodeType, NodesType> { + public: + explicit MeasurementBasedTimestampedNodeAdderModel(const TimestampedNodeAdderModelParams& params) + : TimestampedNodeAdderModel<NodeType, NodesType>(params) {} + virtual ~MeasurementBasedTimestampedNodeAdderModel() = default; + virtual void AddMeasurement(const MeasurementType& measurement) = 0; + virtual void RemoveMeasurements(const localization_common::Time oldest_allowed_time) = 0; +}; +} // namespace node_adders + +#endif // NODE_ADDERS_MEASUREMENT_BASED_TIMESTAMPED_NODE_ADDER_MODEL_H_ diff --git a/localization/node_adders/include/node_adders/node_adder.h b/localization/node_adders/include/node_adders/node_adder.h new file mode 100644 index 0000000000..1f75c790d7 --- /dev/null +++ b/localization/node_adders/include/node_adders/node_adder.h @@ -0,0 +1,49 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef NODE_ADDERS_NODE_ADDER_H_ +#define NODE_ADDERS_NODE_ADDER_H_ + +#include <localization_common/time.h> + +#include <gtsam/nonlinear/Marginals.h> + +namespace node_adders { +// Base class for adding nodes to a graph. A different node adder should be used +// for each node type added to the graph. Nodes can consist of multiple types that are added in unison, +// for example a pose and velocity. See the nodes package for more details. +// Assumes nodes require an initial values and prior factors. +class NodeAdder { + public: + virtual ~NodeAdder() {} + + // Adds initial nodes and priors to constrain them during optimization. + virtual void AddInitialNodesAndPriors(gtsam::NonlinearFactorGraph& graph) = 0; + + // Adds a node using the provided timestamp if possible. + virtual bool AddNode(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) = 0; + + // Returns whether a node can be added at timestamp or not. + virtual bool CanAddNode(const localization_common::Time timestamp) const = 0; + + // Returns keys for a node at the timestamp. + virtual gtsam::KeyVector Keys(const localization_common::Time timestamp) const = 0; +}; +} // namespace node_adders + +#endif // NODE_ADDERS_NODE_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/rotation_factor_adder_params.h b/localization/node_adders/include/node_adders/pose_node_adder.h similarity index 53% rename from localization/graph_localizer/include/graph_localizer/rotation_factor_adder_params.h rename to localization/node_adders/include/node_adders/pose_node_adder.h index 2b0abfd317..90340ce5ba 100644 --- a/localization/graph_localizer/include/graph_localizer/rotation_factor_adder_params.h +++ b/localization/node_adders/include/node_adders/pose_node_adder.h @@ -16,22 +16,21 @@ * under the License. */ -#ifndef GRAPH_LOCALIZER_ROTATION_FACTOR_ADDER_PARAMS_H_ -#define GRAPH_LOCALIZER_ROTATION_FACTOR_ADDER_PARAMS_H_ +#ifndef NODE_ADDERS_POSE_NODE_ADDER_H_ +#define NODE_ADDERS_POSE_NODE_ADDER_H_ -#include <graph_optimizer/factor_adder_params.h> +#include <localization_measurements/pose_with_covariance_measurement.h> +#include <nodes/timestamped_nodes.h> +#include <node_adders/pose_node_adder_model.h> +#include <node_adders/measurement_based_timestamped_node_adder.h> -#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Pose3.h> -namespace graph_localizer { -struct RotationFactorAdderParams : public graph_optimizer::FactorAdderParams { - double min_avg_disparity; - double rotation_stddev; - double max_percent_outliers; - gtsam::Pose3 body_T_nav_cam; - gtsam::Cal3_S2 nav_cam_intrinsics; -}; -} // namespace graph_localizer +namespace node_adders { +// Timestamp-based node adder using timestamped poses with covariances. +using PoseNodeAdder = + MeasurementBasedTimestampedNodeAdder<localization_measurements::PoseWithCovarianceMeasurement, gtsam::Pose3, + nodes::TimestampedNodes<gtsam::Pose3>, PoseNodeAdderModel>; +} // namespace node_adders -#endif // GRAPH_LOCALIZER_ROTATION_FACTOR_ADDER_PARAMS_H_ +#endif // NODE_ADDERS_POSE_NODE_ADDER_H_ diff --git a/localization/node_adders/include/node_adders/pose_node_adder_model.h b/localization/node_adders/include/node_adders/pose_node_adder_model.h new file mode 100644 index 0000000000..c211f1062a --- /dev/null +++ b/localization/node_adders/include/node_adders/pose_node_adder_model.h @@ -0,0 +1,77 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef NODE_ADDERS_POSE_NODE_ADDER_MODEL_H_ +#define NODE_ADDERS_POSE_NODE_ADDER_MODEL_H_ + +#include <localization_common/marginals_pose_covariance_interpolater.h> +#include <localization_common/pose_with_covariance_interpolater.h> +#include <localization_measurements/pose_with_covariance_measurement.h> +#include <node_adders/between_factor_node_adder_model.h> +#include <nodes/combined_nav_state_nodes.h> +#include <nodes/timestamped_nodes.h> + +#include <gtsam/inference/Key.h> +#include <gtsam/geometry/Pose3.h> +#include <gtsam/nonlinear/Marginals.h> + +#include <utility> + +class GraphVIO; + +namespace node_adders { +class PoseNodeAdderModel : public BetweenFactorMeasurementBasedTimestampedNodeAdderModel< + localization_measurements::PoseWithCovarianceMeasurement, gtsam::Pose3> { + using Base = + BetweenFactorMeasurementBasedTimestampedNodeAdderModel<localization_measurements::PoseWithCovarianceMeasurement, + gtsam::Pose3>; + using NodesType = nodes::TimestampedNodes<gtsam::Pose3>; + + public: + using Params = TimestampedNodeAdderModelParams; + + explicit PoseNodeAdderModel(const Params& params) : Base(params) {} + + gtsam::KeyVector AddNode(const localization_common::Time timestamp, NodesType& nodes) const final; + boost::optional<std::pair<gtsam::Pose3, gtsam::SharedNoiseModel>> RelativeNodeAndNoise( + const localization_common::Time timestamp_a, const localization_common::Time timestamp_b) const final; + void AddMeasurement(const localization_measurements::PoseWithCovarianceMeasurement& measurement); + // Keeps lower_bound measurement <= oldest_allowed_time to use for interpolation if needed. + void RemoveMeasurements(const localization_common::Time oldest_allowed_time); + bool CanAddNode(const localization_common::Time timestamp) const final; + // Sets pose covariance interpolater for relative odometry pose node creation + void SetPoseCovarianceInterpolater( + const std::shared_ptr<localization_common::MarginalsPoseCovarianceInterpolater<nodes::CombinedNavStateNodes>>& + pose_covariance_interpolater); + // Accessor to pose interpolater + localization_common::PoseWithCovarianceInterpolater& pose_interpolater(); + + private: + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(pose_interpolater_); + } + + localization_common::PoseWithCovarianceInterpolater pose_interpolater_; +}; +} // namespace node_adders + +#endif // NODE_ADDERS_POSE_NODE_ADDER_MODEL_H_ diff --git a/localization/node_adders/include/node_adders/pose_node_adder_params.h b/localization/node_adders/include/node_adders/pose_node_adder_params.h new file mode 100644 index 0000000000..8950d5a59f --- /dev/null +++ b/localization/node_adders/include/node_adders/pose_node_adder_params.h @@ -0,0 +1,60 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef NODE_ADDERS_POSE_NODE_ADDER_PARAMS_H_ +#define NODE_ADDERS_POSE_NODE_ADDER_PARAMS_H_ + +#include <localization_common/time.h> +#include <localization_common/utilities.h> +#include <localization_measurements/pose_with_covariance_measurement.h> +#include <node_adders/timestamped_node_adder_params.h> +#include <node_adders/utilities.h> + +#include <gtsam/geometry/Pose3.h> + +#include <boost/serialization/serialization.hpp> + +namespace node_adders { +struct PoseNodeAdderParams : public TimestampedNodeAdderParams<gtsam::Pose3> { + void SetStartNoiseModels() { + const gtsam::Vector6 pose_prior_noise_sigmas((gtsam::Vector(6) << starting_prior_translation_stddev, + starting_prior_translation_stddev, starting_prior_translation_stddev, + starting_prior_quaternion_stddev, starting_prior_quaternion_stddev, + starting_prior_quaternion_stddev) + .finished()); + const gtsam::SharedNoiseModel pose_noise_model = localization_common::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(pose_prior_noise_sigmas)), huber_k); + start_noise_models.emplace_back(pose_noise_model); + } + + using Base = TimestampedNodeAdderParams<gtsam::Pose3>; + double starting_prior_translation_stddev; + double starting_prior_quaternion_stddev; + + private: + // Serialization function + friend class boost::serialization::access; + template <class ARCHIVE> + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(starting_prior_translation_stddev); + ar& BOOST_SERIALIZATION_NVP(starting_prior_quaternion_stddev); + } +}; +} // namespace node_adders + +#endif // NODE_ADDERS_POSE_NODE_ADDER_PARAMS_H_ diff --git a/localization/node_adders/include/node_adders/sliding_window_node_adder.h b/localization/node_adders/include/node_adders/sliding_window_node_adder.h new file mode 100644 index 0000000000..d5399388f5 --- /dev/null +++ b/localization/node_adders/include/node_adders/sliding_window_node_adder.h @@ -0,0 +1,59 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef NODE_ADDERS_SLIDING_WINDOW_NODE_ADDER_H_ +#define NODE_ADDERS_SLIDING_WINDOW_NODE_ADDER_H_ + +#include <localization_common/time.h> +#include <node_adders/node_adder.h> + +#include <gtsam/nonlinear/Marginals.h> +#include <gtsam/nonlinear/NonlinearFactorGraph.h> + +namespace node_adders { +// Base class node adder that maintains a sliding window of nodes. +class SlidingWindowNodeAdder : public NodeAdder { + public: + virtual ~SlidingWindowNodeAdder() {} + + // Slides the window, removes nodes older than oldest allowed time. + // Adds priors to the oldest remaining nodes using their marginalized covariances + // and removes old priors containing any key in old keys if param use_priors is true. + // Note: Old factor removal (other than starting priors) is handled in the graph optimizer. + // The oldest allowed timestamp is also determing by the graph optimizer based on + // the ideal oldest allowed timestamps of each node adder used in the graph. + virtual bool SlideWindow(const localization_common::Time oldest_allowed_timestamp, + const boost::optional<const gtsam::Marginals&>& marginals, const gtsam::KeyVector& old_keys, + const double huber_k, gtsam::NonlinearFactorGraph& factors) = 0; + + // Returns the oldest node time that should remain after SlideWindow is called. + virtual boost::optional<localization_common::Time> SlideWindowNewStartTime() const = 0; + + // Returns old node keys older than the oldest_allowed_time. + virtual gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time, + const gtsam::NonlinearFactorGraph& graph) const = 0; + + // Starting (oldest) timestamp of nodes in the node adder + virtual boost::optional<localization_common::Time> StartTime() const = 0; + + // End (latest) timestamp of nodes in the node adder + virtual boost::optional<localization_common::Time> EndTime() const = 0; +}; +} // namespace node_adders + +#endif // NODE_ADDERS_SLIDING_WINDOW_NODE_ADDER_H_ diff --git a/localization/node_adders/include/node_adders/timestamped_node_adder.h b/localization/node_adders/include/node_adders/timestamped_node_adder.h new file mode 100644 index 0000000000..c713ad4844 --- /dev/null +++ b/localization/node_adders/include/node_adders/timestamped_node_adder.h @@ -0,0 +1,374 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef NODE_ADDERS_TIMESTAMPED_NODE_ADDER_H_ +#define NODE_ADDERS_TIMESTAMPED_NODE_ADDER_H_ + +#include <localization_common/utilities.h> +#include <node_adders/sliding_window_node_adder.h> +#include <node_adders/timestamped_node_adder_model.h> +#include <node_adders/timestamped_node_adder_params.h> +#include <nodes/timestamped_nodes.h> + +#include <algorithm> +#include <vector> + +namespace node_adders { + +// Sliding window node adder using timestamp-indexed nodes. +// Generates functions that adds nodes, relative factors, splits old factors, and so on. +// Uses the provided node adder model to accomplish these. +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +class TimestampedNodeAdder : public SlidingWindowNodeAdder { + using Base = SlidingWindowNodeAdder; + + public: + // Construct using nodes. Creates timestamped nodes interally. + TimestampedNodeAdder(const TimestampedNodeAdderParams<NodeType>& params, + const typename NodeAdderModelType::Params& node_adder_model_params, + std::shared_ptr<nodes::Values> values); + + // Construct using already constructed timestamped nodes. + TimestampedNodeAdder(const TimestampedNodeAdderParams<NodeType>& params, + const typename NodeAdderModelType::Params& node_adder_model_params, + std::shared_ptr<TimestampedNodesType> timestamped_nodes); + + // For serialization only + TimestampedNodeAdder() = default; + + virtual ~TimestampedNodeAdder() = default; + + void AddInitialNodesAndPriors(gtsam::NonlinearFactorGraph& factors) final; + + // Adds initial nodes and priors using provided noise values and timestamp. + void AddInitialNodesAndPriors(const NodeType& initial_node, const std::vector<gtsam::SharedNoiseModel>& initial_noise, + const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors); + + bool AddNode(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final; + + // Slides the window, removes nodes older than oldest allowed time. + // Adds priors to the oldest remaining nodes using their marginalized covariances + // and removes old priors containing any key in old keys if param use_priors is true. + // Note: Old factor removal (other than starting priors) is handled in the graph optimizer. + // The oldest allowed timestamp is also determing by the graph optimizer based on + // the ideal oldest allowed timestamps of each node adder used in the graph. + bool SlideWindow(const localization_common::Time oldest_allowed_timestamp, + const boost::optional<const gtsam::Marginals&>& marginals, const gtsam::KeyVector& old_keys, + const double huber_k, gtsam::NonlinearFactorGraph& factors) override; + + // Returns the oldest node time that should remain after SlideWindow is called. + // Calculated using params to ensure the set min/max node limits are enforced + // and the ideal duration is enforced otherwise. + // Returns boost::none if no nodes exist or too few nodes exist. + // If the min/max limits are enforced and the total duration of nodes is still less than + // the ideal duration, the new oldest time is set to 0. + // Used by the graph optimizer to compute the new oldest time for all node adders + // in the graph. + boost::optional<localization_common::Time> SlideWindowNewStartTime() const final; + + gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time, + const gtsam::NonlinearFactorGraph& graph) const final; + + boost::optional<localization_common::Time> StartTime() const final; + + boost::optional<localization_common::Time> EndTime() const final; + + bool CanAddNode(const localization_common::Time timestamp) const final; + + gtsam::KeyVector Keys(const localization_common::Time timestamp) const final; + + const TimestampedNodesType& nodes() const { return *nodes_; } + + std::shared_ptr<const TimestampedNodesType> nodes_ptr() { return nodes_; } + + NodeAdderModelType& node_adder_model() { return node_adder_model_; } + + private: + void RemovePriors(const gtsam::KeyVector& old_keys, gtsam::NonlinearFactorGraph& factors); + bool AddNewNodesAndRelativeFactors(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors); + bool SplitOldRelativeFactor(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors); + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(nodes_); + ar& BOOST_SERIALIZATION_NVP(node_adder_model_); + ar& BOOST_SERIALIZATION_NVP(params_); + } + + TimestampedNodeAdderParams<NodeType> params_; + NodeAdderModelType node_adder_model_; + std::shared_ptr<TimestampedNodesType> nodes_; +}; + +// Implementation +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +TimestampedNodeAdder<NodeType, TimestampedNodesType, NodeAdderModelType>::TimestampedNodeAdder( + const TimestampedNodeAdderParams<NodeType>& params, + const typename NodeAdderModelType::Params& node_adder_model_params, std::shared_ptr<nodes::Values> values) + : params_(params), + nodes_(std::make_shared<TimestampedNodesType>(values)), + node_adder_model_(node_adder_model_params) {} + +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +TimestampedNodeAdder<NodeType, TimestampedNodesType, NodeAdderModelType>::TimestampedNodeAdder( + const TimestampedNodeAdderParams<NodeType>& params, + const typename NodeAdderModelType::Params& node_adder_model_params, + std::shared_ptr<TimestampedNodesType> timestamped_nodes) + : params_(params), nodes_(timestamped_nodes), node_adder_model_(node_adder_model_params) {} + +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +void TimestampedNodeAdder<NodeType, TimestampedNodesType, NodeAdderModelType>::AddInitialNodesAndPriors( + gtsam::NonlinearFactorGraph& factors) { + AddInitialNodesAndPriors(params_.start_node, params_.start_noise_models, params_.starting_time, factors); +} + +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +void TimestampedNodeAdder<NodeType, TimestampedNodesType, NodeAdderModelType>::AddInitialNodesAndPriors( + const NodeType& initial_node, const std::vector<gtsam::SharedNoiseModel>& initial_noise, + const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) { + nodes_->Add(timestamp, initial_node); + node_adder_model_.AddPriors(initial_node, initial_noise, timestamp, *nodes_, factors); + // Store initial node as measurement so subsequent measurements can be computed relative to this +} + +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +bool TimestampedNodeAdder<NodeType, TimestampedNodesType, NodeAdderModelType>::SlideWindow( + const localization_common::Time oldest_allowed_timestamp, const boost::optional<const gtsam::Marginals&>& marginals, + const gtsam::KeyVector& old_keys, const double huber_k, gtsam::NonlinearFactorGraph& factors) { + if (oldest_allowed_timestamp <= *(StartTime())) { + LogDebug("SlideWindow: Oldest allowed timestamp older than current start time, nothing to do."); + return true; + } + nodes_->RemoveOldNodes(oldest_allowed_timestamp); + if (params_.add_priors) { + // Add prior to oldest pose using covariances from last round of + // optimization + const auto oldest_node = nodes_->OldestNode(); + const auto start_time = StartTime(); + if (!oldest_node || !start_time) { + LogError("SlideWindow: Failed to get oldest node and timestamp."); + return false; + } + + // Make sure priors are removed before adding new ones + RemovePriors(old_keys, factors); + if (marginals) { + const auto keys = nodes_->Keys(*start_time); + if (keys.empty()) { + LogError("SlideWindow: Failed to get oldest keys."); + return false; + } + + std::vector<gtsam::SharedNoiseModel> prior_noise_models; + for (const auto& key : keys) { + // If covariance doesn't exist yet (can happen if the new start node hasn't been optimized yet) + // revert to the initial start noise for the prior. + try { + const auto prior_noise = localization_common::Robust( + gtsam::noiseModel::Gaussian::Covariance(marginals->marginalCovariance(key)), huber_k); + prior_noise_models.emplace_back(prior_noise); + } catch (...) { + prior_noise_models = params_.start_noise_models; + break; + } + } + node_adder_model_.AddPriors(*oldest_node, prior_noise_models, *start_time, *nodes_, factors); + } else { + // TODO(rsoussan): Add seperate marginal fallback sigmas instead of relying on starting prior noise + node_adder_model_.AddPriors(*oldest_node, params_.start_noise_models, *start_time, *nodes_, factors); + } + } + + return true; +} + +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +boost::optional<localization_common::Time> +TimestampedNodeAdder<NodeType, TimestampedNodesType, NodeAdderModelType>::SlideWindowNewStartTime() const { + if (nodes_->empty()) { + LogDebug("SlideWindowNewStartTime: No states in map."); + return boost::none; + } + + const int size = nodes_->size(); + if (size <= params_.min_num_states) { + LogDebug("SlideWindowNewStartTime: Not enough states to remove."); + return boost::none; + } + + const double total_duration = nodes_->Duration(); + LogDebug("SlideWindowNewStartTime: Starting total num states: " << nodes_->size()); + LogDebug("SlideWindowNewStartTime: Starting total duration is " << total_duration); + const localization_common::Time ideal_oldest_allowed_state = std::max(0.0, *(EndTime()) - params_.ideal_duration); + + // Find oldest timestamp that first prioritizes that the graph has at least min_num_states. + // Second priority, find the optimal oldest timestamp that additionally ensures the graph does not have more than + // max_num_states. Last priortity, find the optimial oldest timestamp that is also closest to the ideal oldest + // timestamp (and thus ensures the graph is <= the ideal duration). + int num_states_to_be_removed = 0; + for (const auto& timestamp : nodes_->Timestamps()) { + const int new_num_states = size - num_states_to_be_removed++; + // First priority + if (new_num_states <= params_.min_num_states) return timestamp; + // Second priority + if (new_num_states > params_.max_num_states) continue; + // Last priority + if (timestamp >= ideal_oldest_allowed_state) return timestamp; + } + + // If the constraints aren't able to be satisfied, an error was made + // when setting the graph duration or state limit params + LogError("SlideWindowNewStartTime: Invalid sliding window params set, max num states: " + << params_.max_num_states << ", min num states: " << params_.min_num_states + << ", ideal duration: " << params_.ideal_duration); + return boost::none; +} + +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +gtsam::KeyVector TimestampedNodeAdder<NodeType, TimestampedNodesType, NodeAdderModelType>::OldKeys( + const localization_common::Time oldest_allowed_time, const gtsam::NonlinearFactorGraph& graph) const { + return nodes_->OldKeys(oldest_allowed_time); +} + +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +boost::optional<localization_common::Time> +TimestampedNodeAdder<NodeType, TimestampedNodesType, NodeAdderModelType>::StartTime() const { + return nodes_->OldestTimestamp(); +} + +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +boost::optional<localization_common::Time> +TimestampedNodeAdder<NodeType, TimestampedNodesType, NodeAdderModelType>::EndTime() const { + return nodes_->LatestTimestamp(); +} + +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +bool TimestampedNodeAdder<NodeType, TimestampedNodesType, NodeAdderModelType>::CanAddNode( + const localization_common::Time timestamp) const { + return node_adder_model_.CanAddNode(timestamp); +} + +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +gtsam::KeyVector TimestampedNodeAdder<NodeType, TimestampedNodesType, NodeAdderModelType>::Keys( + const localization_common::Time timestamp) const { + return nodes_->Keys(timestamp); +} + +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +void TimestampedNodeAdder<NodeType, TimestampedNodesType, NodeAdderModelType>::RemovePriors( + const gtsam::KeyVector& old_keys, gtsam::NonlinearFactorGraph& factors) { + int removed_factors = 0; + for (auto factor_it = factors.begin(); factor_it != factors.end();) { + bool erase_factor = false; + const auto prior_factor = dynamic_cast<gtsam::PriorFactor<NodeType>*>(factor_it->get()); + if (prior_factor) { + // Erase factor if it contains an old key + for (const auto& old_key : old_keys) { + if (prior_factor->key() == old_key) { + erase_factor = true; + factor_it = factors.erase(factor_it); + ++removed_factors; + } + } + if (!erase_factor) { + ++factor_it; + } + } else { + ++factor_it; + } + } + LogDebug("RemovePriors: Erase " << removed_factors << " factors."); +} + +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +bool TimestampedNodeAdder<NodeType, TimestampedNodesType, NodeAdderModelType>::AddNode( + const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) { + if (nodes_->Contains(timestamp)) { + LogDebug( + "Adder: Node exists at " + "timestamp, nothing to do."); + return true; + } + + const auto end_time = EndTime(); + if (!end_time) { + LogError("Adder: Failed to get end timestamp."); + return false; + } + if (timestamp > *end_time) { + LogDebug("Adder: Adding new nodes and relative factors."); + return AddNewNodesAndRelativeFactors(timestamp, factors); + } else { + LogDebug("Adder: Splitting old relative factor."); + return SplitOldRelativeFactor(timestamp, factors); + } +} + +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +bool TimestampedNodeAdder<NodeType, TimestampedNodesType, NodeAdderModelType>::AddNewNodesAndRelativeFactors( + const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) { + const auto timestamp_a = EndTime(); + if (!timestamp_a) { + LogError("AddNewNodesAndRelativeFactor: Failed to get end timestamp."); + return false; + } + return node_adder_model_.AddNodesAndRelativeFactors(*timestamp_a, timestamp, *nodes_, factors); +} + +template <typename NodeType, typename TimestampedNodesType, typename NodeAdderModelType> +bool TimestampedNodeAdder<NodeType, TimestampedNodesType, NodeAdderModelType>::SplitOldRelativeFactor( + const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) { + const auto timestamp_bounds = nodes_->LowerAndUpperBoundTimestamps(timestamp); + // TODO(rsoussan): print upper and lower bound! print timestamp! + if (!timestamp_bounds.first || !timestamp_bounds.second) { + LogError("SplitOldRelativeFactor: Failed to get upper and lower bound timestamp."); + return false; + } + + const localization_common::Time lower_bound_time = *(timestamp_bounds.first); + const localization_common::Time upper_bound_time = *(timestamp_bounds.second); + + if (timestamp < lower_bound_time || timestamp > upper_bound_time) { + LogError("SplitOldRelativeFactor: Timestamp is not within bounds of existing timestamps."); + return false; + } + + const bool removed_old_factors = + node_adder_model_.RemoveRelativeFactors(lower_bound_time, upper_bound_time, *nodes_, factors); + if (!removed_old_factors) { + LogError( + "SplitOldRelativeFactor: Failed to remove " + "old factors."); + return false; + } + if (!node_adder_model_.AddNodesAndRelativeFactors(lower_bound_time, timestamp, *nodes_, factors)) { + LogError("SplitOldRelativeFactor: Failed to add first relative node and factor."); + return false; + } + if (!node_adder_model_.AddRelativeFactors(timestamp, upper_bound_time, *nodes_, factors)) { + LogError("SplitOldRelativeFactor: Failed to add second relative factor."); + return false; + } + return true; +} +} // namespace node_adders + +#endif // NODE_ADDERS_TIMESTAMPED_NODE_ADDER_H_ diff --git a/localization/node_adders/include/node_adders/timestamped_node_adder_model.h b/localization/node_adders/include/node_adders/timestamped_node_adder_model.h new file mode 100644 index 0000000000..0f15961ae9 --- /dev/null +++ b/localization/node_adders/include/node_adders/timestamped_node_adder_model.h @@ -0,0 +1,67 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef NODE_ADDERS_TIMESTAMPED_NODE_ADDER_MODEL_H_ +#define NODE_ADDERS_TIMESTAMPED_NODE_ADDER_MODEL_H_ + +#include <node_adders/timestamped_node_adder_model_params.h> + +#include <gtsam/inference/Key.h> +#include <gtsam/nonlinear/NonlinearFactorGraph.h> + +#include <vector> + +namespace node_adders { +// Handles adding priors and relative factors for a given node type. +// Generates these factors for provided timestamps. +// Required by TimestampedNodeAdder. +template <typename NodeType, typename NodesType> +class TimestampedNodeAdderModel { + public: + explicit TimestampedNodeAdderModel(const TimestampedNodeAdderModelParams& params) : params_(params) {} + virtual ~TimestampedNodeAdderModel() = default; + // Adds prior factors for a given node using provided noise models. + virtual void AddPriors(const NodeType& node, const std::vector<gtsam::SharedNoiseModel>& noise_models, + const localization_common::Time timestamp, const NodesType& nodes, + gtsam::NonlinearFactorGraph& factors) const = 0; + + // Adds nodes for timestamp_b and connect it to the node at timestamp_a with relative factors. + virtual bool AddNodesAndRelativeFactors(const localization_common::Time timestamp_a, + const localization_common::Time timestamp_b, NodesType& nodes, + gtsam::NonlinearFactorGraph& factors) const = 0; + + // Adds relative factors between nodes at timestamp_a and timestamp_b. + virtual bool AddRelativeFactors(const localization_common::Time timestamp_a, + const localization_common::Time timestamp_b, const NodesType& nodes, + gtsam::NonlinearFactorGraph& factors) const = 0; + + // Returns whether a node can be added at timestamp or not. + virtual bool CanAddNode(const localization_common::Time timestamp) const = 0; + + // Remove relative adder model factors between nodes + virtual bool RemoveRelativeFactors(const localization_common::Time timestamp_a, + const localization_common::Time timestamp_b, const NodesType& nodes, + gtsam::NonlinearFactorGraph& factors) const = 0; + + protected: + // TODO(rsoussan): Add constructor to set these, template on params? + TimestampedNodeAdderModelParams params_; +}; +} // namespace node_adders + +#endif // NODE_ADDERS_TIMESTAMPED_NODE_ADDER_MODEL_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/factor_adder.h b/localization/node_adders/include/node_adders/timestamped_node_adder_model_params.h similarity index 55% rename from localization/graph_optimizer/include/graph_optimizer/factor_adder.h rename to localization/node_adders/include/node_adders/timestamped_node_adder_model_params.h index e8530dac37..a1146a3c08 100644 --- a/localization/graph_optimizer/include/graph_optimizer/factor_adder.h +++ b/localization/node_adders/include/node_adders/timestamped_node_adder_model_params.h @@ -15,29 +15,24 @@ * License for the specific language governing permissions and limitations * under the License. */ +#ifndef NODE_ADDERS_TIMESTAMPED_NODE_ADDER_MODEL_PARAMS_H_ +#define NODE_ADDERS_TIMESTAMPED_NODE_ADDER_MODEL_PARAMS_H_ -#ifndef GRAPH_OPTIMIZER_FACTOR_ADDER_H_ -#define GRAPH_OPTIMIZER_FACTOR_ADDER_H_ +#include <boost/serialization/serialization.hpp> -#include <graph_optimizer/factor_to_add.h> - -#include <vector> - -namespace graph_optimizer { -template <typename MEASUREMENT, typename PARAMS> -class FactorAdder { - public: - explicit FactorAdder(const PARAMS& params) : params_(params) {} - - virtual ~FactorAdder() {} - - virtual std::vector<FactorsToAdd> AddFactors(const MEASUREMENT& measurement) = 0; - - const PARAMS& params() const { return params_; } +namespace node_adders { +struct TimestampedNodeAdderModelParams { + double huber_k; private: - PARAMS params_; + // Serialization function + friend class boost::serialization::access; + template <class ARCHIVE> + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + // TODO(rsoussan): Why does this lead to a compile error in test? + // ar& BOOST_SERIALIZATION_NVP(huber_k); + } }; -} // namespace graph_optimizer +} // namespace node_adders -#endif // GRAPH_OPTIMIZER_FACTOR_ADDER_H_ +#endif // NODE_ADDERS_TIMESTAMPED_NODE_ADDER_MODEL_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/combined_nav_state_graph_values_params.h b/localization/node_adders/include/node_adders/timestamped_node_adder_params.h similarity index 61% rename from localization/graph_localizer/include/graph_localizer/combined_nav_state_graph_values_params.h rename to localization/node_adders/include/node_adders/timestamped_node_adder_params.h index 67b91e8ec4..9acc55b3b3 100644 --- a/localization/graph_localizer/include/graph_localizer/combined_nav_state_graph_values_params.h +++ b/localization/node_adders/include/node_adders/timestamped_node_adder_params.h @@ -15,17 +15,29 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef GRAPH_LOCALIZER_COMBINED_NAV_STATE_GRAPH_VALUES_PARAMS_H_ -#define GRAPH_LOCALIZER_COMBINED_NAV_STATE_GRAPH_VALUES_PARAMS_H_ +#ifndef NODE_ADDERS_TIMESTAMPED_NODE_ADDER_PARAMS_H_ +#define NODE_ADDERS_TIMESTAMPED_NODE_ADDER_PARAMS_H_ -#include <gtsam/geometry/Pose3.h> +#include <localization_common/time.h> #include <boost/serialization/serialization.hpp> -namespace graph_localizer { -struct CombinedNavStateGraphValuesParams { +#include <gtsam/geometry/Pose3.h> +#include <gtsam/linear/NoiseModel.h> + +#include <vector> + +namespace node_adders { +template <typename NodeType> +struct TimestampedNodeAdderParams { + NodeType start_node; + std::vector<gtsam::SharedNoiseModel> start_noise_models; + double huber_k; + bool add_priors; + localization_common::Time starting_time; // Only kept if there are at least min_num_states and not more than max_num_states double ideal_duration; + // TODO(rsoussan): Rename states to nodes? int min_num_states; int max_num_states; @@ -34,11 +46,16 @@ struct CombinedNavStateGraphValuesParams { friend class boost::serialization::access; template <class ARCHIVE> void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(start_node); + ar& BOOST_SERIALIZATION_NVP(start_noise_models); + ar& BOOST_SERIALIZATION_NVP(huber_k); + ar& BOOST_SERIALIZATION_NVP(add_priors); + ar& BOOST_SERIALIZATION_NVP(starting_time); ar& BOOST_SERIALIZATION_NVP(ideal_duration); ar& BOOST_SERIALIZATION_NVP(min_num_states); ar& BOOST_SERIALIZATION_NVP(max_num_states); } }; -} // namespace graph_localizer +} // namespace node_adders -#endif // GRAPH_LOCALIZER_COMBINED_NAV_STATE_GRAPH_VALUES_PARAMS_H_ +#endif // NODE_ADDERS_TIMESTAMPED_NODE_ADDER_PARAMS_H_ diff --git a/localization/node_adders/include/node_adders/utilities.h b/localization/node_adders/include/node_adders/utilities.h new file mode 100644 index 0000000000..1a3db8b8ef --- /dev/null +++ b/localization/node_adders/include/node_adders/utilities.h @@ -0,0 +1,81 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef NODE_ADDERS_UTILITIES_H_ +#define NODE_ADDERS_UTILITIES_H_ + +#include <localization_common/logger.h> +#include <localization_common/time.h> + +#include <gtsam/inference/Key.h> +#include <gtsam/nonlinear/NonlinearFactorGraph.h> + +namespace node_adders { +// Removes the FactorType factor in factors containing each of the provided keys if it exists +template <typename FactorType> +bool RemoveFactor(const gtsam::KeyVector& keys, gtsam::NonlinearFactorGraph& factors); + +// Removes the FactorType factor in factors containing each of the keys for nodes at timestamp a and b if it exists. +template <typename FactorType, typename NodesType> +bool RemoveRelativeFactor(const localization_common::Time timestamp_a, const localization_common::Time timestamp_b, + const NodesType& nodes, gtsam::NonlinearFactorGraph& factors); + +// Returns the covariance from a robust, gaussian shared noise model. +gtsam::Matrix Covariance(const gtsam::SharedNoiseModel& robust_gaussian_noise); + +// Implementation +template <typename FactorType> +bool RemoveFactor(const gtsam::KeyVector& keys, gtsam::NonlinearFactorGraph& factors) { + for (auto factor_it = factors.begin(); factor_it != factors.end(); ++factor_it) { + if (!dynamic_cast<FactorType*>(factor_it->get())) continue; + bool contains_keys = true; + for (const auto& key : keys) { + if ((*factor_it)->find(key) == std::end((*factor_it)->keys())) { + contains_keys = false; + continue; + } + } + if (contains_keys) { + factors.erase(factor_it); + return true; + } + } + return false; +} + +template <typename FactorType, typename NodesType> +bool RemoveRelativeFactor(const localization_common::Time timestamp_a, const localization_common::Time timestamp_b, + const NodesType& nodes, gtsam::NonlinearFactorGraph& factors) { + const auto keys_a = nodes.Keys(timestamp_a); + if (keys_a.empty()) { + LogError("RemoveRelativeFactor: Failed to get keys for timestamp_a."); + return false; + } + + const auto keys_b = nodes.Keys(timestamp_b); + if (keys_b.empty()) { + LogError("RemoveRelativeFactor: Failed to get keys for timestamp_b."); + return false; + } + + gtsam::KeyVector combined_keys = keys_a; + combined_keys.insert(combined_keys.cend(), keys_b.cbegin(), keys_b.cend()); + return RemoveFactor<FactorType>(combined_keys, factors); +} +} // namespace node_adders + +#endif // NODE_ADDERS_UTILITIES_H_ diff --git a/localization/node_adders/package.xml b/localization/node_adders/package.xml new file mode 100644 index 0000000000..ecfd80aefe --- /dev/null +++ b/localization/node_adders/package.xml @@ -0,0 +1,23 @@ +<package> + <name>node_adders</name> + <version>1.0.0</version> + <description> + The node adders package + </description> + <license> + Apache License, Version 2.0 + </license> + <author email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </author> + <maintainer email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </maintainer> + <buildtool_depend>catkin</buildtool_depend> + <build_depend>imu_integration</build_depend> + <build_depend>localization_common</build_depend> + <build_depend>nodes</build_depend> + <run_depend>imu_integration</run_depend> + <run_depend>localization_common</run_depend> + <run_depend>nodes</run_depend> +</package> diff --git a/localization/node_adders/readme.md b/localization/node_adders/readme.md new file mode 100644 index 0000000000..bccc15a514 --- /dev/null +++ b/localization/node_adders/readme.md @@ -0,0 +1,37 @@ +\page nodeadder Node Adders + +# Package Overview +The node adders package provides various node adders that add nodes given a timestamp to a graph along with connecting relative factors. The SlidingWindowNodeAdder is used with a SlidingWindowGraphOptimizer and provides functions for removing old nodes and relative factors. Implementations of the sliding window node adder are provided such as the timestamped node adder that uses a node adder model to create relative factors and the measurement based node adder which is a timestamped node adder that uses provided measurements to create relative factors and nodes. See the pose node adder/adder model and combined nav state node adder/adder model for examples of these. The BetweenFactorAdderModel creates GTSAM between factors as relative factors and GTSAM prior factors for priors for a single gtsam type. Use this when possible, and see the PoseNodeAdderModel as an example. + +## NodeAdder +Base class for other node adders, interface for adding/accessing nodes and keys given their timestamps and adding initial values and priors for the first nodes. + +## SlidingWindowNodeAdder +Base class node adder that extends the NodeAdder with functions that enable sliding the window, which consists of removed nodes older than a desired time. The SlideWindowNewStartTime function provides the desired start time for the window for the node adder, but the window size is ultimately decided by the SlidingWindowGraphOptimizer using each desired start time for each sliding window node adder in the graph. + +## TimestampedNodeAdder +SlidingWindowNodeAdder that adds nodes given a provided node adder model. Provides functions for adding new relative factors and splitting old ones when out of order nodes are received. + +## TimestampedNodeAdderModel +Helper base class for TimestampedNodeAdder that adds and removes relative factors and priors given relative timestamps. + +## MeasurementBasedTimestampedNodeAdder +Base class that extends the TimestampedNodeAdder to enable adding a measurement from which nodes will be generated. Measurements are forwarded to the MeasurementBasedTimestampedNodeAdderModel. + +## MeasurementBasedTimestampedNodeAdderModel +Virtual class that extends the TimestampedNodeAdderModel with Add/Remove measurement functions. Measurements are used to generate relative factors and priors. + +## BetweenFactorNodeAdderModel +NodeAdderModel that creates GTSAM between factors as relative factors and GTSAM prior factors as priors. Requires some function specializations for adding nodes and measurements. See PoseNodeAdderModel for an example. + +## CombinedNavStateNodeAdder +MeasurementBasedTimestampedNodeAdder that uses IMU measurements as a measurement and creates CombinedNavStateNodes. Uses the CombinedNavStateNodeAdderModel to generate relative IMU factors given the IMU measurements. + +## CombinedNavStateNodeAdderModel +MeasurementBasedTimestampedNodeAdderModel that generates relative IMU factors given IMU measurements. + +## PoseNodeAdder +MeasurementBasedTimestampedNodeAdder that uses timestamped pose with covariance objects as a measurement and creates gtsam::Pose3 timestamped nodes. + +## PoseNodeAdderModel +BetweenFactorNodeAdderModel for gtsam::Pose3 poses. Uses timestamped pose with covariance objects as a measurement. diff --git a/localization/node_adders/src/combined_nav_state_node_adder_model.cc b/localization/node_adders/src/combined_nav_state_node_adder_model.cc new file mode 100644 index 0000000000..530ac3aa3e --- /dev/null +++ b/localization/node_adders/src/combined_nav_state_node_adder_model.cc @@ -0,0 +1,155 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <imu_integration/utilities.h> +#include <node_adders/combined_nav_state_node_adder_model.h> +#include <node_adders/utilities.h> + +#include <gtsam/geometry/Pose3.h> +#include <gtsam/slam/BetweenFactor.h> + +namespace node_adders { +namespace no = nodes; +namespace ii = imu_integration; +namespace lc = localization_common; +namespace lm = localization_measurements; +CombinedNavStateNodeAdderModel::CombinedNavStateNodeAdderModel(const CombinedNavStateNodeAdderModelParams& params) + : Base(params), imu_integrator_(params.imu_integrator) {} + +void CombinedNavStateNodeAdderModel::AddPriors(const lc::CombinedNavState& node, + const std::vector<gtsam::SharedNoiseModel>& noise_models, + const lc::Time timestamp, const no::CombinedNavStateNodes& nodes, + gtsam::NonlinearFactorGraph& factors) const { + const auto keys = nodes.Keys(timestamp); + if (keys.empty()) { + LogError("AddPriors: Failed to get keys."); + return; + } + + const auto& pose_key = keys[0]; + const auto& velocity_key = keys[1]; + const auto& bias_key = keys[2]; + + const auto& pose_noise = noise_models[0]; + const auto& velocity_noise = noise_models[1]; + const auto& bias_noise = noise_models[2]; + + gtsam::PriorFactor<gtsam::Pose3> pose_prior_factor(pose_key, node.pose(), pose_noise); + factors.push_back(pose_prior_factor); + gtsam::PriorFactor<gtsam::Velocity3> velocity_prior_factor(velocity_key, node.velocity(), velocity_noise); + factors.push_back(velocity_prior_factor); + gtsam::PriorFactor<gtsam::imuBias::ConstantBias> bias_prior_factor(bias_key, node.bias(), bias_noise); + factors.push_back(bias_prior_factor); +} + +bool CombinedNavStateNodeAdderModel::AddNodesAndRelativeFactors(const lc::Time timestamp_a, const lc::Time timestamp_b, + no::CombinedNavStateNodes& nodes, + gtsam::NonlinearFactorGraph& factors) const { + if (!nodes.Contains(timestamp_b)) { + const auto node_a = nodes.Node(timestamp_a); + if (!node_a) { + LogError("AddNodesAndRelativeFactors: Failed to get node a."); + return false; + } + + const auto node_b = imu_integrator_.Extrapolate(*node_a, timestamp_b); + if (!node_b) { + LogError("AddNodesAndRelativeFactors: Failed to get node b by extrapolating node a."); + return false; + } + + const auto keys = nodes.Add(timestamp_b, *node_b); + if (keys.empty()) { + LogError("AddNodesAndRelativeFactors: Failed to add node b."); + return false; + } + } + + if (!AddRelativeFactors(timestamp_a, timestamp_b, nodes, factors)) { + LogError("AddNodesAndRelativeFactors: Failed to add relative factor."); + return false; + } + return true; +} + +bool CombinedNavStateNodeAdderModel::AddRelativeFactors(const lc::Time timestamp_a, const lc::Time timestamp_b, + const no::CombinedNavStateNodes& nodes, + gtsam::NonlinearFactorGraph& factors) const { + const auto keys_a = nodes.Keys(timestamp_a); + if (keys_a.empty()) { + LogError("AddRelativeFactors: Failed to get keys a."); + return false; + } + const auto keys_b = nodes.Keys(timestamp_b); + if (keys_b.empty()) { + LogError("AddRelativeFactors: Failed to get keys b."); + return false; + } + + const auto& pose_key_a = keys_a[0]; + const auto& velocity_key_a = keys_a[1]; + const auto& bias_key_a = keys_a[2]; + + const auto& pose_key_b = keys_b[0]; + const auto& velocity_key_b = keys_b[1]; + const auto& bias_key_b = keys_b[2]; + + const auto node_a = nodes.Node(timestamp_a); + const auto node_b = nodes.Node(timestamp_b); + if (!node_a || !node_b) { + LogError("AddRelativeFactors: Failed to get nodes a and b."); + return false; + } + + auto pim = imu_integrator_.IntegratedPim(node_a->bias(), timestamp_a, timestamp_b); + if (!pim) { + LogError("AddRelativeFactors: Failed to create pim."); + return false; + } + + const gtsam::CombinedImuFactor::shared_ptr combined_imu_factor( + new gtsam::CombinedImuFactor(pose_key_a, velocity_key_a, pose_key_b, velocity_key_b, bias_key_a, bias_key_b, *pim)); + factors.push_back(combined_imu_factor); + return true; +} + +void CombinedNavStateNodeAdderModel::AddMeasurement(const lm::ImuMeasurement& measurement) { + imu_integrator_.AddImuMeasurement(measurement); +} + +void CombinedNavStateNodeAdderModel::RemoveMeasurements(const lc::Time oldest_allowed_time) { + // Don't need lower bound since imu integration doesn't use measurements + // at same timestamp as start time for integration + imu_integrator_.RemoveOldValues(oldest_allowed_time); +} + +bool CombinedNavStateNodeAdderModel::CanAddNode(const localization_common::Time timestamp) const { + return imu_integrator_.WithinBounds(timestamp); +} + +bool CombinedNavStateNodeAdderModel::RemoveRelativeFactors(const localization_common::Time timestamp_a, + const localization_common::Time timestamp_b, + const NodesType& nodes, + gtsam::NonlinearFactorGraph& factors) const { + return RemoveRelativeFactor<gtsam::CombinedImuFactor, NodesType>(timestamp_a, timestamp_b, nodes, factors); +} + +void CombinedNavStateNodeAdderModel::SetFanSpeedMode(const localization_measurements::FanSpeedMode& fan_speed_mode) { + imu_integrator_.SetFanSpeedMode(fan_speed_mode); +} +} // namespace node_adders diff --git a/localization/graph_localizer/src/feature_point_node_updater.cc b/localization/node_adders/src/feature_point_node_updater.cc_old similarity index 57% rename from localization/graph_localizer/src/feature_point_node_updater.cc rename to localization/node_adders/src/feature_point_node_updater.cc_old index 3bb355776b..807c49e61c 100644 --- a/localization/graph_localizer/src/feature_point_node_updater.cc +++ b/localization/node_adders/src/feature_point_node_updater.cc_old @@ -16,45 +16,46 @@ * under the License. */ -#include <graph_localizer/feature_point_node_updater.h> -#include <graph_localizer/utilities.h> +#include <graph_optimizer/utilities.h> +#include <node_adders/feature_point_node_adder.h> #include <gtsam/inference/Symbol.h> #include <gtsam/slam/PriorFactor.h> -namespace graph_localizer { +namespace node_adders { namespace go = graph_optimizer; +namespace gv = graph_values; namespace lc = localization_common; namespace sym = gtsam::symbol_shorthand; -// TODO(rsoussan): Make new node updater base class that doesn't have functions left empty here? -FeaturePointNodeUpdater::FeaturePointNodeUpdater(const FeaturePointNodeUpdaterParams& params, +// TODO(rsoussan): Make new node adder base class that doesn't have functions left empty here? +FeaturePointNodeAdder::FeaturePointNodeAdder(const FeaturePointNodeAdderParams& params, std::shared_ptr<gtsam::Values> values) - : params_(params), feature_point_graph_values_(new FeaturePointGraphValues(std::move(values))) {} + : params_(params), feature_point_graph_values_(new gv::FeaturePointGraphValues(std::move(values))) {} -void FeaturePointNodeUpdater::AddInitialValuesAndPriors(const lc::FeaturePoint3d& global_t_point, - const lc::FeaturePoint3dNoise& noise, +void FeaturePointNodeAdder::AddInitialValuesAndPriors(const lc::FeaturePoint3d& global_t_point, + const lc::FeaturePoint3dNoise& noise, const lc::Time timestamp, gtsam::NonlinearFactorGraph& factors) {} -void FeaturePointNodeUpdater::AddPriors(const lc::FeaturePoint3d& global_t_point, const lc::FeaturePoint3dNoise& noise, - gtsam::NonlinearFactorGraph& factors) {} +void FeaturePointNodeAdder::AddPriors(const lc::FeaturePoint3d& global_t_point, const lc::FeaturePoint3dNoise& noise, + const lc::Time timestamp, gtsam::NonlinearFactorGraph& factors) {} -bool FeaturePointNodeUpdater::SlideWindow(const lc::Time oldest_allowed_timestamp, +bool FeaturePointNodeAdder::SlideWindow(const lc::Time oldest_allowed_timestamp, const boost::optional<gtsam::Marginals>& marginals, const gtsam::KeyVector& old_keys, const double huber_k, gtsam::NonlinearFactorGraph& factors) { feature_point_graph_values_->RemoveOldFeatures(old_keys); - if (marginals) UpdatePointPriors(*marginals, factors); + if (marginals) AdderPointPriors(*marginals, factors); return true; } -void FeaturePointNodeUpdater::UpdatePointPriors(const gtsam::Marginals& marginals, +void FeaturePointNodeAdder::AdderPointPriors(const gtsam::Marginals& marginals, gtsam::NonlinearFactorGraph& factors) { const auto feature_keys = feature_point_graph_values_->FeatureKeys(); for (const auto& feature_key : feature_keys) { const auto world_t_point = feature_point_graph_values_->at<gtsam::Point3>(feature_key); if (!world_t_point) { - LogError("UpdatePointPriors: Failed to get world_t_point."); + LogError("AdderPointPriors: Failed to get world_t_point."); continue; } for (auto factor_it = factors.begin(); factor_it != factors.end();) { @@ -62,9 +63,9 @@ void FeaturePointNodeUpdater::UpdatePointPriors(const gtsam::Marginals& marginal if (point_prior_factor && (point_prior_factor->key() == feature_key)) { // Erase old prior factor_it = factors.erase(factor_it); - // Add updated one - const auto point_prior_noise = - Robust(gtsam::noiseModel::Gaussian::Covariance(marginals.marginalCovariance(feature_key)), params_.huber_k); + // Add adderd one + const auto point_prior_noise = go::Robust( + gtsam::noiseModel::Gaussian::Covariance(marginals.marginalCovariance(feature_key)), params_.huber_k); const gtsam::PriorFactor<gtsam::Point3> point_prior_factor(feature_key, *world_t_point, point_prior_noise); factors.push_back(point_prior_factor); // Only one point prior per feature @@ -76,39 +77,39 @@ void FeaturePointNodeUpdater::UpdatePointPriors(const gtsam::Marginals& marginal } } -go::NodeUpdaterType FeaturePointNodeUpdater::type() const { return go::NodeUpdaterType::FeaturePoint; } +go::NodeAdderType FeaturePointNodeAdder::type() const { return go::NodeAdderType::FeaturePoint; } -bool FeaturePointNodeUpdater::Update(const lc::Time timestamp, gtsam::NonlinearFactorGraph& factors) { return true; } +bool FeaturePointNodeAdder::Adder(const lc::Time timestamp, gtsam::NonlinearFactorGraph& factors) { return true; } -boost::optional<lc::Time> FeaturePointNodeUpdater::SlideWindowNewOldestTime() const { +boost::optional<lc::Time> FeaturePointNodeAdder::SlideWindowNewOldestTime() const { return feature_point_graph_values_->SlideWindowNewOldestTime(); } -gtsam::KeyVector FeaturePointNodeUpdater::OldKeys(const localization_common::Time oldest_allowed_time, +gtsam::KeyVector FeaturePointNodeAdder::OldKeys(const localization_common::Time oldest_allowed_time, const gtsam::NonlinearFactorGraph& graph) const { return feature_point_graph_values_->OldKeys(oldest_allowed_time, graph); } -boost::optional<gtsam::Key> FeaturePointNodeUpdater::GetKey(go::KeyCreatorFunction key_creator_function, +boost::optional<gtsam::Key> FeaturePointNodeAdder::GetKey(go::KeyCreatorFunction key_creator_function, const localization_common::Time timestamp) const { return feature_point_graph_values_->GetKey(key_creator_function, timestamp); } -boost::optional<localization_common::Time> FeaturePointNodeUpdater::OldestTimestamp() const { +boost::optional<localization_common::Time> FeaturePointNodeAdder::OldestTimestamp() const { return feature_point_graph_values_->OldestTimestamp(); } -boost::optional<localization_common::Time> FeaturePointNodeUpdater::LatestTimestamp() const { +boost::optional<localization_common::Time> FeaturePointNodeAdder::LatestTimestamp() const { return feature_point_graph_values_->LatestTimestamp(); } -int FeaturePointNodeUpdater::NumFeatures() const { return feature_point_graph_values_->NumFeatures(); } +int FeaturePointNodeAdder::NumFeatures() const { return feature_point_graph_values_->NumFeatures(); } -std::shared_ptr<FeaturePointGraphValues> FeaturePointNodeUpdater::shared_feature_point_graph_values() { +std::shared_ptr<gv::FeaturePointGraphValues> FeaturePointNodeAdder::shared_feature_point_graph_values() { return feature_point_graph_values_; } -const FeaturePointGraphValues& FeaturePointNodeUpdater::feature_point_graph_values() const { +const gv::FeaturePointGraphValues& FeaturePointNodeAdder::feature_point_graph_values() const { return *feature_point_graph_values_; } -} // namespace graph_localizer +} // namespace node_adders diff --git a/localization/node_adders/src/pose_node_adder_model.cc b/localization/node_adders/src/pose_node_adder_model.cc new file mode 100644 index 0000000000..e1ef84c954 --- /dev/null +++ b/localization/node_adders/src/pose_node_adder_model.cc @@ -0,0 +1,78 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/utilities.h> +#include <localization_measurements/timestamped_pose.h> +#include <node_adders/pose_node_adder_model.h> + +namespace node_adders { +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace no = nodes; + +gtsam::KeyVector PoseNodeAdderModel::AddNode(const lc::Time timestamp, NodesType& nodes) const { + const auto lower_bound_or_equal_node = nodes.LowerBoundOrEqual(timestamp); + if (!lower_bound_or_equal_node) { + LogError("AddNode: Failed to get lower bound or equal node."); + return gtsam::KeyVector(); + } + const auto relative_pose = pose_interpolater_.Relative(lower_bound_or_equal_node->timestamp, timestamp); + if (!relative_pose) { + LogError("RelativeNodeAndNoise: Failed to get relative estimate."); + return gtsam::KeyVector(); + } + + const gtsam::Pose3 extrapolated_pose = lower_bound_or_equal_node->value * lc::GtPose(relative_pose->pose); + return nodes.Add(timestamp, extrapolated_pose); +} + +boost::optional<std::pair<gtsam::Pose3, gtsam::SharedNoiseModel>> PoseNodeAdderModel::RelativeNodeAndNoise( + const lc::Time timestamp_a, const lc::Time timestamp_b) const { + const auto relative_pose = pose_interpolater_.Relative(timestamp_a, timestamp_b); + if (!relative_pose) { + LogError("RelativeNodeAndNoise: Failed to get relative estimate."); + return boost::none; + } + const auto relative_pose_noise = + lc::Robust(gtsam::noiseModel::Gaussian::Covariance(relative_pose->covariance, false), params_.huber_k); + return std::pair<gtsam::Pose3, gtsam::SharedNoiseModel>(localization_common::GtPose(relative_pose->pose), + relative_pose_noise); +} + +void PoseNodeAdderModel::AddMeasurement(const lm::PoseWithCovarianceMeasurement& measurement) { + pose_interpolater_.Add(measurement.timestamp, measurement.PoseWithCovariance()); +} + +void PoseNodeAdderModel::RemoveMeasurements(const lc::Time oldest_allowed_time) { + // Keep lower bound so future measurements can be interpolated using it. + pose_interpolater_.RemoveBelowLowerBoundValues(oldest_allowed_time); +} + +bool PoseNodeAdderModel::CanAddNode(const localization_common::Time timestamp) const { + return pose_interpolater_.WithinBounds(timestamp); +} + +void PoseNodeAdderModel::SetPoseCovarianceInterpolater( + const std::shared_ptr<lc::MarginalsPoseCovarianceInterpolater<no::CombinedNavStateNodes>>& + pose_covariance_interpolater) { + pose_interpolater_.params().pose_covariance_interpolater = pose_covariance_interpolater; +} + +lc::PoseWithCovarianceInterpolater& PoseNodeAdderModel::pose_interpolater() { return pose_interpolater_; } + +} // namespace node_adders diff --git a/localization/graph_optimizer/include/graph_optimizer/node_updater_type.h b/localization/node_adders/src/utilities.cc similarity index 69% rename from localization/graph_optimizer/include/graph_optimizer/node_updater_type.h rename to localization/node_adders/src/utilities.cc index 17e8c88365..6c1a154602 100644 --- a/localization/graph_optimizer/include/graph_optimizer/node_updater_type.h +++ b/localization/node_adders/src/utilities.cc @@ -16,12 +16,12 @@ * under the License. */ -#ifndef GRAPH_OPTIMIZER_NODE_UPDATER_TYPE_H_ -#define GRAPH_OPTIMIZER_NODE_UPDATER_TYPE_H_ +#include <node_adders/utilities.h> -namespace graph_optimizer { -// TODO(rsoussan): Generalize this better -enum class NodeUpdaterType { CombinedNavState, FeaturePoint }; -} // namespace graph_optimizer - -#endif // GRAPH_OPTIMIZER_NODE_UPDATER_TYPE_H_ +namespace node_adders { +gtsam::Matrix Covariance(const gtsam::SharedNoiseModel& robust_gaussian_noise) { + return dynamic_cast<gtsam::noiseModel::Gaussian*>( + dynamic_cast<gtsam::noiseModel::Robust*>(robust_gaussian_noise.get())->noise().get()) + ->covariance(); +} +} // namespace node_adders diff --git a/localization/node_adders/test/test_combined_nav_state_node_adder_model.cc b/localization/node_adders/test/test_combined_nav_state_node_adder_model.cc new file mode 100644 index 0000000000..c5395e321e --- /dev/null +++ b/localization/node_adders/test/test_combined_nav_state_node_adder_model.cc @@ -0,0 +1,340 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "test_utilities.h" // NOLINT +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <localization_measurements/imu_measurement.h> +#include <node_adders/combined_nav_state_node_adder_model.h> +#include <node_adders/utilities.h> + +#include <gtest/gtest.h> + +namespace ii = imu_integration; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace no = nodes; +namespace na = node_adders; + +class CombinedNavStateNodeAdderModelTest : public ::testing::Test { + public: + CombinedNavStateNodeAdderModelTest() + : params_(na::DefaultCombinedNavStateNodeAdderModelParams()), + model_(params_), + nodes_(std::make_shared<no::Values>()), + imu_integrator_(params_.imu_integrator) {} + void SetUp() final { + const Eigen::Vector3d acceleration(0.01, 0.02, 0.03); + const Eigen::Vector3d angular_velocity(0.04, 0.05, 0.06); + const Eigen::Vector3d acceleration_bias = 0.5 * acceleration; + const Eigen::Vector3d angular_velocity_bias = 0.5 * angular_velocity; + const Eigen::Vector3d bias_corrected_acceleration = acceleration - acceleration_bias; + const Eigen::Vector3d bias_corrected_angular_velocity = angular_velocity - angular_velocity_bias; + constexpr int kNumIterations = 10; + constexpr double kTimeDiff = 1; + lc::Time time = 0.0; + for (int i = 0; i < kNumIterations; ++i) { + const lm::ImuMeasurement imu_measurement(acceleration, angular_velocity, time); + measurements_.emplace_back(imu_measurement); + timestamps_.emplace_back(time); + time += kTimeDiff; + } + } + + void AddMeasurements() { + for (const auto& measurement : measurements_) { + model_.AddMeasurement(measurement); + imu_integrator_.AddImuMeasurement(measurement); + } + } + + std::vector<gtsam::SharedNoiseModel> Noise() { + constexpr double kTranslationStddev = 0.1; + constexpr double kQuaternionStddev = 0.2; + const gtsam::Vector6 pose_prior_noise_sigmas((gtsam::Vector(6) << kTranslationStddev, kTranslationStddev, + kTranslationStddev, kQuaternionStddev, kQuaternionStddev, + kQuaternionStddev) + .finished()); + constexpr double kVelocityStddev = 0.3; + const gtsam::Vector3 velocity_prior_noise_sigmas( + (gtsam::Vector(3) << kVelocityStddev, kVelocityStddev, kVelocityStddev).finished()); + constexpr double kAccelBiasStddev = 0.4; + constexpr double kGyroBiasStddev = 0.5; + const gtsam::Vector6 bias_prior_noise_sigmas((gtsam::Vector(6) << kAccelBiasStddev, kAccelBiasStddev, + kAccelBiasStddev, kGyroBiasStddev, kGyroBiasStddev, kGyroBiasStddev) + .finished()); + const auto pose_noise = lc::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(pose_prior_noise_sigmas)), params_.huber_k); + const auto velocity_noise = + lc::Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(velocity_prior_noise_sigmas)), + params_.huber_k); + const auto bias_noise = lc::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(bias_prior_noise_sigmas)), params_.huber_k); + std::vector<gtsam::SharedNoiseModel> noise; + noise.emplace_back(pose_noise); + noise.emplace_back(velocity_noise); + noise.emplace_back(bias_noise); + return noise; + } + + const lm::ImuMeasurement& measurement(const int index) { return measurements_[index]; } + lc::Time time(const int index) { return timestamps_[index]; } + + std::vector<lm::ImuMeasurement> measurements_; + std::vector<lc::Time> timestamps_; + na::CombinedNavStateNodeAdderModelParams params_; + na::CombinedNavStateNodeAdderModel model_; + no::CombinedNavStateNodes nodes_; + gtsam::NonlinearFactorGraph factors_; + ii::ImuIntegrator imu_integrator_; +}; + +TEST_F(CombinedNavStateNodeAdderModelTest, AddRemoveCanAddNode) { + EXPECT_FALSE(model_.CanAddNode(time(0))); + // Add 1st measurement + model_.AddMeasurement(measurement(time(0))); + EXPECT_TRUE(model_.CanAddNode(time(0))); + EXPECT_FALSE(model_.CanAddNode(time(0) - 0.1)); + EXPECT_FALSE(model_.CanAddNode(time(1))); + EXPECT_FALSE(model_.CanAddNode(time(5))); + // Add 2nd measurement + model_.AddMeasurement(measurement(1)); + EXPECT_TRUE(model_.CanAddNode(time(0))); + EXPECT_FALSE(model_.CanAddNode(time(0) - 0.1)); + EXPECT_TRUE(model_.CanAddNode((time(0) + time(1)) / 2.0)); + EXPECT_TRUE(model_.CanAddNode(time(1))); + EXPECT_FALSE(model_.CanAddNode(time(2))); + // Add 3rd measurement + model_.AddMeasurement(measurement(2)); + EXPECT_TRUE(model_.CanAddNode(time(0))); + EXPECT_FALSE(model_.CanAddNode(time(0) - 0.1)); + EXPECT_TRUE(model_.CanAddNode((time(0) + time(1)) / 2.0)); + EXPECT_TRUE(model_.CanAddNode(time(2))); + // Remove up to 2nd measurement + model_.RemoveMeasurements(time(1)); + EXPECT_FALSE(model_.CanAddNode(time(0))); + EXPECT_FALSE(model_.CanAddNode((time(0) + time(1)) / 2.0)); + EXPECT_TRUE(model_.CanAddNode(time(1))); + EXPECT_TRUE(model_.CanAddNode(time(2))); +} + +TEST_F(CombinedNavStateNodeAdderModelTest, AddPriors) { + const auto node = lc::RandomCombinedNavState(); + const auto keys = nodes_.Add(node.timestamp(), node); + ASSERT_FALSE(keys.empty()); + const auto noise = Noise(); + model_.AddPriors(node, noise, node.timestamp(), nodes_, factors_); + ASSERT_EQ(factors_.size(), 3); + // Check pose prior + const auto pose_priors = lc::Factors<gtsam::PriorFactor<gtsam::Pose3>>(factors_); + EXPECT_EQ(pose_priors.size(), 1); + EXPECT_MATRIX_NEAR(pose_priors[0]->prior(), node.pose(), 1e-6); + EXPECT_MATRIX_NEAR(na::Covariance(pose_priors[0]->noiseModel()), na::Covariance(noise[0]), 1e-6); + // Check velocity prior + const auto velocity_priors = lc::Factors<gtsam::PriorFactor<gtsam::Velocity3>>(factors_); + EXPECT_EQ(velocity_priors.size(), 1); + EXPECT_MATRIX_NEAR(velocity_priors[0]->prior(), node.velocity(), 1e-6); + EXPECT_MATRIX_NEAR(na::Covariance(velocity_priors[0]->noiseModel()), na::Covariance(noise[1]), 1e-6); + // Check Bias prior + const auto bias_priors = lc::Factors<gtsam::PriorFactor<gtsam::imuBias::ConstantBias>>(factors_); + EXPECT_EQ(bias_priors.size(), 1); + EXPECT_MATRIX_NEAR(bias_priors[0]->prior().accelerometer(), node.bias().accelerometer(), 1e-6); + EXPECT_MATRIX_NEAR(bias_priors[0]->prior().gyroscope(), node.bias().gyroscope(), 1e-6); + EXPECT_MATRIX_NEAR(na::Covariance(bias_priors[0]->noiseModel()), na::Covariance(noise[2]), 1e-6); +} + +TEST_F(CombinedNavStateNodeAdderModelTest, AddRelativeFactors) { + const auto timestamp_a = timestamps_[2]; + const lc::CombinedNavState node_a( + lc::CombinedNavState(lc::RandomPose(), lc::RandomVelocity(), + gtsam::imuBias::ConstantBias(lc::RandomVector3d(), lc::RandomVector3d()), timestamp_a)); + const auto timestamp_b = timestamps_[5]; + const lc::CombinedNavState node_b( + lc::CombinedNavState(lc::RandomPose(), lc::RandomVelocity(), + gtsam::imuBias::ConstantBias(lc::RandomVector3d(), lc::RandomVector3d()), timestamp_b)); + + const auto keys_a = nodes_.Add(node_a.timestamp(), node_a); + ASSERT_FALSE(keys_a.empty()); + const auto keys_b = nodes_.Add(node_b.timestamp(), node_b); + ASSERT_FALSE(keys_b.empty()); + + AddMeasurements(); + + // Add relative factor + ASSERT_FALSE(model_.AddRelativeFactors(timestamps_[0], timestamp_b, nodes_, factors_)); + ASSERT_FALSE(model_.AddRelativeFactors(timestamp_a, timestamps_[3], nodes_, factors_)); + ASSERT_TRUE(model_.AddRelativeFactors(timestamp_a, timestamp_b, nodes_, factors_)); + ASSERT_EQ(factors_.size(), 1); + const auto imu_factors = lc::Factors<gtsam::CombinedImuFactor>(factors_); + ASSERT_EQ(imu_factors.size(), 1); + // Check keys + // pose_a + EXPECT_EQ(imu_factors[0]->key1(), keys_a[0]); + // vel_a + EXPECT_EQ(imu_factors[0]->key2(), keys_a[1]); + // pose_b + EXPECT_EQ(imu_factors[0]->key3(), keys_b[0]); + // vel_b + EXPECT_EQ(imu_factors[0]->key4(), keys_b[1]); + // bias_a + EXPECT_EQ(imu_factors[0]->key5(), keys_a[2]); + // bias_b + EXPECT_EQ(imu_factors[0]->key6(), keys_b[2]); + + // Check PIM + const auto pim = imu_integrator_.IntegratedPim(node_a.bias(), timestamp_a, timestamp_b); + ASSERT_TRUE(pim != boost::none); + EXPECT_TRUE(pim->equals(imu_factors[0]->preintegratedMeasurements())); +} + +TEST_F(CombinedNavStateNodeAdderModelTest, AddRelativeFactorsInBetweenTimes) { + const auto timestamp_a = timestamps_[2]; + const lc::CombinedNavState node_a( + lc::CombinedNavState(lc::RandomPose(), lc::RandomVelocity(), + gtsam::imuBias::ConstantBias(lc::RandomVector3d(), lc::RandomVector3d()), timestamp_a)); + const auto timestamp_b = (timestamps_[5] + timestamps_[6]) / 2.0; + const lc::CombinedNavState node_b( + lc::CombinedNavState(lc::RandomPose(), lc::RandomVelocity(), + gtsam::imuBias::ConstantBias(lc::RandomVector3d(), lc::RandomVector3d()), timestamp_b)); + + const auto keys_a = nodes_.Add(node_a.timestamp(), node_a); + ASSERT_FALSE(keys_a.empty()); + const auto keys_b = nodes_.Add(node_b.timestamp(), node_b); + ASSERT_FALSE(keys_b.empty()); + + AddMeasurements(); + + // Add relative factor + ASSERT_FALSE(model_.AddRelativeFactors(timestamps_[0], timestamp_b, nodes_, factors_)); + ASSERT_FALSE(model_.AddRelativeFactors(timestamp_a, timestamps_[3], nodes_, factors_)); + ASSERT_TRUE(model_.AddRelativeFactors(timestamp_a, timestamp_b, nodes_, factors_)); + ASSERT_EQ(factors_.size(), 1); + const auto imu_factors = lc::Factors<gtsam::CombinedImuFactor>(factors_); + ASSERT_EQ(imu_factors.size(), 1); + // Check keys + // pose_a + EXPECT_EQ(imu_factors[0]->key1(), keys_a[0]); + // vel_a + EXPECT_EQ(imu_factors[0]->key2(), keys_a[1]); + // pose_b + EXPECT_EQ(imu_factors[0]->key3(), keys_b[0]); + // vel_b + EXPECT_EQ(imu_factors[0]->key4(), keys_b[1]); + // bias_a + EXPECT_EQ(imu_factors[0]->key5(), keys_a[2]); + // bias_b + EXPECT_EQ(imu_factors[0]->key6(), keys_b[2]); + + // Check PIM + const auto pim = imu_integrator_.IntegratedPim(node_a.bias(), timestamp_a, timestamp_b); + ASSERT_TRUE(pim != boost::none); + EXPECT_TRUE(pim->equals(imu_factors[0]->preintegratedMeasurements())); +} + +TEST_F(CombinedNavStateNodeAdderModelTest, RemoveRelativeFactors) { + AddMeasurements(); + const auto timestamp_a = timestamps_[3]; + const auto timestamp_b = timestamps_[7]; + const auto timestamp_c = timestamps_[8]; + // Add first nodes and relative factor + const lc::CombinedNavState node_a( + lc::CombinedNavState(lc::RandomPose(), lc::RandomVelocity(), + gtsam::imuBias::ConstantBias(lc::RandomVector3d(), lc::RandomVector3d()), timestamp_a)); + const lc::CombinedNavState node_b( + lc::CombinedNavState(lc::RandomPose(), lc::RandomVelocity(), + gtsam::imuBias::ConstantBias(lc::RandomVector3d(), lc::RandomVector3d()), timestamp_b)); + + const auto keys_a = nodes_.Add(node_a.timestamp(), node_a); + ASSERT_FALSE(keys_a.empty()); + const auto keys_b = nodes_.Add(node_b.timestamp(), node_b); + ASSERT_FALSE(keys_b.empty()); + ASSERT_TRUE(model_.AddRelativeFactors(timestamp_a, timestamp_b, nodes_, factors_)); + // Add second nodes and relative factor + const lc::CombinedNavState node_c( + lc::CombinedNavState(lc::RandomPose(), lc::RandomVelocity(), + gtsam::imuBias::ConstantBias(lc::RandomVector3d(), lc::RandomVector3d()), timestamp_c)); + const auto keys_c = nodes_.Add(node_c.timestamp(), node_c); + ASSERT_FALSE(keys_c.empty()); + ASSERT_TRUE(model_.AddRelativeFactors(timestamp_b, timestamp_c, nodes_, factors_)); + + ASSERT_EQ(factors_.size(), 2); + ASSERT_FALSE(model_.RemoveRelativeFactors(timestamp_a - 1.0, timestamp_b, nodes_, factors_)); + ASSERT_FALSE(model_.RemoveRelativeFactors(timestamp_a, timestamp_c, nodes_, factors_)); + ASSERT_TRUE(model_.RemoveRelativeFactors(timestamp_a, timestamp_b, nodes_, factors_)); + // Check the remaining factor is the second relative factor + const auto imu_factors = lc::Factors<gtsam::CombinedImuFactor>(factors_); + ASSERT_EQ(imu_factors.size(), 1); + // Check keys + // pose_b + EXPECT_EQ(imu_factors[0]->key1(), keys_b[0]); + // vel_b + EXPECT_EQ(imu_factors[0]->key2(), keys_b[1]); + // pose_c + EXPECT_EQ(imu_factors[0]->key3(), keys_c[0]); + // vel_c + EXPECT_EQ(imu_factors[0]->key4(), keys_c[1]); + // bias_b + EXPECT_EQ(imu_factors[0]->key5(), keys_b[2]); + // bias_c + EXPECT_EQ(imu_factors[0]->key6(), keys_c[2]); + // Remove last factor + ASSERT_FALSE(model_.RemoveRelativeFactors(timestamp_a, timestamp_b, nodes_, factors_)); + ASSERT_TRUE(model_.RemoveRelativeFactors(timestamp_b, timestamp_c, nodes_, factors_)); + ASSERT_TRUE(factors_.empty()); +} + +TEST_F(CombinedNavStateNodeAdderModelTest, AddNodeAndRelativeFactors) { + AddMeasurements(); + const auto timestamp_a = timestamps_[1]; + const auto timestamp_b = timestamps_[4]; + // Should fail since no node at timestamp_a exists yet + EXPECT_FALSE(model_.AddNodesAndRelativeFactors(timestamp_a, timestamp_b, nodes_, factors_)); + const lc::CombinedNavState node_a( + lc::CombinedNavState(lc::RandomPose(), lc::RandomVelocity(), + gtsam::imuBias::ConstantBias(lc::RandomVector3d(), lc::RandomVector3d()), timestamp_a)); + const auto keys_a = nodes_.Add(node_a.timestamp(), node_a); + ASSERT_FALSE(keys_a.empty()); + EXPECT_TRUE(model_.AddNodesAndRelativeFactors(timestamp_a, timestamp_b, nodes_, factors_)); + EXPECT_EQ(factors_.size(), 1); + EXPECT_TRUE(nodes_.Contains(timestamp_a)); + EXPECT_TRUE(nodes_.Contains(timestamp_b)); + const auto imu_factors = lc::Factors<gtsam::CombinedImuFactor>(factors_); + ASSERT_EQ(imu_factors.size(), 1); + // Check keys + const auto keys_b = nodes_.Keys(timestamp_b); + ASSERT_FALSE(keys_b.empty()); + // pose_a + EXPECT_EQ(imu_factors[0]->key1(), keys_a[0]); + // vel_a + EXPECT_EQ(imu_factors[0]->key2(), keys_a[1]); + // pose_b + EXPECT_EQ(imu_factors[0]->key3(), keys_b[0]); + // vel_b + EXPECT_EQ(imu_factors[0]->key4(), keys_b[1]); + // bias_a + EXPECT_EQ(imu_factors[0]->key5(), keys_a[2]); + // bias_b + EXPECT_EQ(imu_factors[0]->key6(), keys_b[2]); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/node_adders/test/test_combined_nav_state_node_adder_model.test b/localization/node_adders/test/test_combined_nav_state_node_adder_model.test new file mode 100644 index 0000000000..e00de57668 --- /dev/null +++ b/localization/node_adders/test/test_combined_nav_state_node_adder_model.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="node_adders" type="test_combined_nav_state_node_adder_model" test-name="test_combined_nav_state_node_adder_model" /> +</launch> diff --git a/localization/node_adders/test/test_pose_node_adder.cc b/localization/node_adders/test/test_pose_node_adder.cc new file mode 100644 index 0000000000..ef156b87ff --- /dev/null +++ b/localization/node_adders/test/test_pose_node_adder.cc @@ -0,0 +1,778 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "test_utilities.h" // NOLINT +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <node_adders/pose_node_adder.h> +#include <node_adders/utilities.h> + +#include <gtest/gtest.h> + +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace na = node_adders; +namespace no = nodes; + +class PoseNodeAdderTest : public ::testing::Test { + public: + PoseNodeAdderTest() : time_increment_(1.0), start_time_(0.0), num_measurements_(20) { + params_ = na::DefaultPoseNodeAdderParams(); + node_adder_model_params_.huber_k = 1.345; + } + + // Create random pose with covariance measurements + void SetUp() final { + for (int i = 0; i < num_measurements_; ++i) { + const lc::Time time = start_time_ + time_increment_ * i; + const lm::PoseWithCovarianceMeasurement pose_measurement(lc::RandomPose(), lc::RandomPoseCovariance(), time); + pose_measurements_.emplace_back(pose_measurement); + timestamps_.emplace_back(time); + } + } + + // Add pose measurements to node adder + void AddMeasurements() { + for (const auto& measurement : pose_measurements_) { + pose_node_adder_->AddMeasurement(measurement); + } + } + + // Randomly initialize with random start time + void RandomlyInitialize() { RandomlyInitialize(lc::RandomDouble()); } + + // Randomly initialize with set start time + void RandomlyInitialize(const lc::Time starting_time) { + params_.start_node = lc::RandomPose(); + params_.starting_time = starting_time; + pose_node_adder_.reset(new na::PoseNodeAdder(params_, node_adder_model_params_, std::make_shared<no::Values>())); + pose_node_adder_->AddInitialNodesAndPriors(factors_); + } + + // Initialize with default params + void DefaultInitialize() { + pose_node_adder_.reset(new na::PoseNodeAdder(params_, node_adder_model_params_, std::make_shared<no::Values>())); + pose_node_adder_->AddInitialNodesAndPriors(factors_); + } + + // Initialize at identity pose and 0 start time + void ZeroInitialize() { + params_.start_node = gtsam::Pose3::identity(); + params_.starting_time = 0.0; + pose_node_adder_.reset(new na::PoseNodeAdder(params_, node_adder_model_params_, std::make_shared<no::Values>())); + pose_node_adder_->AddInitialNodesAndPriors(factors_); + } + + // Access the measurement pose at the provided measurement index. + Eigen::Isometry3d measurement_pose(const int index) { return lc::EigenPose(pose_measurements_[index].pose); } + + // Access the measurement pose covariance at the provided measurement index. + lc::PoseCovariance measurement_covariance(const int index) { return pose_measurements_[index].covariance; } + + // Access the timestamp at the provided measurement index. + lc::Time measurement_timestamp(const int index) { return timestamps_[index]; } + + // Check that the pose in the node adder at the provided timestamp matches the provided pose. + void EXPECT_SAME_NODE_POSE(const lc::Time timestamp, const Eigen::Isometry3d& pose) { + const auto& nodes = pose_node_adder_->nodes(); + EXPECT_TRUE(nodes.Contains(timestamp)); + const auto node = nodes.Node(timestamp); + ASSERT_TRUE(node != boost::none); + EXPECT_MATRIX_NEAR(node->matrix(), pose, 1e-6); + } + + // Check that the pose in the node adder at the provided timestamp matches the provided pose. + void EXPECT_SAME_NODE_POSE(const lc::Time timestamp, const gtsam::Pose3& pose) { + EXPECT_SAME_NODE_POSE(timestamp, lc::EigenPose(pose)); + } + + // Check that the pose in the node adder at the provided measurement index matches the computed pose using the same + // index. The expect pose uses the starting pose with the relative measurement pose at that timestamp. + void EXPECT_SAME_NODE_POSE(const int index) { + const gtsam::Pose3 relative_pose = lc::GtPose(measurement_pose(0).inverse() * measurement_pose(index)); + const gtsam::Pose3 expected_pose = params_.start_node * relative_pose; + EXPECT_SAME_NODE_POSE(measurement_timestamp(index), expected_pose); + } + + // Check that the pose in the node adder at the interpolated timestamp using the provided indices and alpha value + // matches the computed interpolated pose using the same values. + void EXPECT_SAME_NODE_POSE_INTERPOLATED(const int measurement_index_a, const int measurement_index_b, + const double alpha) { + const auto interpolated_pose = InterpolatedPose(measurement_index_a, measurement_index_b, alpha); + const auto relative_pose = lc::GtPose(measurement_pose(0).inverse() * interpolated_pose); + const auto expected_pose = params_.start_node * relative_pose; + const lc::Time timestamp_a_b = + measurement_timestamp(measurement_index_a) * alpha + measurement_timestamp(measurement_index_b) * (1.0 - alpha); + EXPECT_SAME_NODE_POSE(timestamp_a_b, expected_pose); + } + + // Check that the prior factor pose at the provided factor index matches the provided pose. + void EXPECT_SAME_PRIOR_FACTOR_POSE(const int factor_index, const Eigen::Isometry3d& pose) { + const auto pose_prior_factor = dynamic_cast<gtsam::PriorFactor<gtsam::Pose3>*>(factors_[factor_index].get()); + ASSERT_TRUE(pose_prior_factor); + EXPECT_MATRIX_NEAR(pose_prior_factor->prior(), pose, 1e-6); + } + + // Check that the prior factor pose at the provided factor index matches the provided pose. + void EXPECT_SAME_PRIOR_FACTOR_POSE(const int factor_index, const gtsam::Pose3& pose) { + return EXPECT_SAME_PRIOR_FACTOR_POSE(factor_index, lc::EigenPose(pose)); + } + + // Check that the relative pose between measurements at index a and b matches the between factor pose at the provided + // index. + void EXPECT_SAME_BETWEEN_FACTOR_POSE(const int factor_index, const int measurement_index_a, + const int measurement_index_b) { + const auto& pose_a = measurement_pose(measurement_index_a); + const auto& pose_b = measurement_pose(measurement_index_b); + const auto relative_pose = pose_a.inverse() * pose_b; + EXPECT_SAME_BETWEEN_FACTOR_POSE(factor_index, relative_pose); + } + + // Check that the relative pose between measurements at the provided index and the previous index to that matches the + // between factor pose at the provided index. + void EXPECT_SAME_BETWEEN_FACTOR_POSE(const int factor_index, const int measurement_index) { + EXPECT_SAME_BETWEEN_FACTOR_POSE(factor_index, measurement_index - 1, measurement_index); + } + + // Check that the relative pose between measurements at the provided index and the previous index to that matches the + // between factor pose at the same index. + void EXPECT_SAME_BETWEEN_FACTOR_POSE(const int index) { + const auto& pose_a = measurement_pose(index - 1); + const auto& pose_b = measurement_pose(index); + const auto relative_pose = pose_a.inverse() * pose_b; + EXPECT_SAME_BETWEEN_FACTOR_POSE(index, relative_pose); + } + + // Check that the provided pose matches the between factor pose at the provided index. + void EXPECT_SAME_BETWEEN_FACTOR_POSE(const int factor_index, const Eigen::Isometry3d& pose) { + const auto pose_between_factor = dynamic_cast<gtsam::BetweenFactor<gtsam::Pose3>*>(factors_[factor_index].get()); + ASSERT_TRUE(pose_between_factor); + EXPECT_MATRIX_NEAR(pose_between_factor->measured(), pose, 1e-6); + } + + // Check that the provided pose matches the between factor pose at the provided index. + // Assumes the first factor index is the prior factor and the subsequent indices are between factors. + void EXPECT_SAME_BETWEEN_FACTOR_POSE(const int factor_index, const gtsam::Pose3& pose) { + EXPECT_SAME_BETWEEN_FACTOR_POSE(factor_index, lc::EigenPose(pose)); + } + + Eigen::Isometry3d InterpolatedPose(const int index_a, const int index_b, const double alpha) { + return lc::Interpolate(measurement_pose(index_a), measurement_pose(index_b), alpha); + } + + // Check that the relative pose in the between factor at the interpolated timestamp using the provided indices and + // alpha value matches the computed interpolated pose using the same values. + void EXPECT_SAME_BETWEEN_FACTOR_POSE_INTERPOLATED(const int measurement_index_a, const int measurement_index_b, + const double alpha) { + const auto interpolated_pose = InterpolatedPose(measurement_index_a, measurement_index_b, alpha); + const auto& pose_a = measurement_pose(measurement_index_a); + const auto relative_pose = pose_a.inverse() * interpolated_pose; + EXPECT_SAME_BETWEEN_FACTOR_POSE(measurement_index_b, relative_pose); + } + + // Check that the relative pose in the between factor at the interpolated timestamp using the provided indices and + // alpha value matches the computed interpolated pose using the same values. Here rather than using the portion from + // the pose at measurement_index_a to the interpolated timestamp, the portion from the interpolated timestamp to the + // pose at measurement_index_b is used. + void EXPECT_SAME_SECOND_BETWEEN_FACTOR_POSE_INTERPOLATED(const int measurement_index_a, const int measurement_index_b, + const double alpha) { + const auto pose_interpolated = InterpolatedPose(measurement_index_a, measurement_index_b, alpha); + const Eigen::Isometry3d relative_pose = pose_interpolated.inverse() * measurement_pose(measurement_index_b); + EXPECT_SAME_BETWEEN_FACTOR_POSE(measurement_index_b + 1, relative_pose); + } + + // Check that the factor noise model matches the provided covariance. + template <typename FactorPtrType> + void EXPECT_SAME_NOISE(const FactorPtrType factor, const lc::PoseCovariance& covariance) { + EXPECT_MATRIX_NEAR(na::Covariance(factor->noiseModel()), covariance, 1e-6); + } + + // Check that the factor noise model matches the provided noise model. + template <typename FactorPtrType> + void EXPECT_SAME_NOISE(const FactorPtrType factor, const gtsam::SharedNoiseModel noise) { + EXPECT_SAME_NOISE(factor, na::Covariance(noise)); + } + + // Check that the between factor noise at the provided index matches the provided covariance. + void EXPECT_SAME_BETWEEN_FACTOR_NOISE(const int factor_index, const lc::PoseCovariance& covariance) { + // Handle special case where need prior noise (remove this once covariance computation is fixed) + /*if (factor_index == -1) { + EXPECT_SAME_NOISE(dynamic_cast<gtsam::PriorFactor<gtsam::Pose3>*>(factors_[0].get()), covariance); + } else {*/ + const auto pose_between_factor = dynamic_cast<gtsam::BetweenFactor<gtsam::Pose3>*>(factors_[factor_index].get()); + EXPECT_SAME_NOISE(pose_between_factor, covariance); + //} + } + + // Check that the between factor noise at the provided index matches the provided covariance. + void EXPECT_SAME_BETWEEN_FACTOR_NOISE(const int factor_index) { + // TODO(rsoussan): Update this with interpolation/relative computation for cov. + // EXPECT_SAME_BETWEEN_FACTOR_NOISE(factor_index, measurement_covariance(factor_index)); + } + + // Check that the between factor noise at factor index_b matches the computed interpolated covariance at measurement + // indices a and b. + void EXPECT_SAME_BETWEEN_FACTOR_NOISE_INTERPOLATED(const int measurement_index_a, const int measurement_index_b, + const double alpha) { + // TODO(rsoussan): Update this with interpolation/relative computation for cov. + // EXPECT_SAME_BETWEEN_FACTOR_NOISE(measurement_index_b, measurement_covariance(measurement_index_a)); + } + + // Check that the between factor noise at factor index_b + 1 matches the second half of the computed interpolated + // covariance at measurement indices a and b. + void EXPECT_SAME_SECOND_BETWEEN_FACTOR_NOISE_INTERPOLATED(const int index_a, const int index_b, const double alpha) { + // TODO(rsoussan): Update this with interpolation/relative computation for cov. + // EXPECT_SAME_BETWEEN_FACTOR_NOISE(index_b + 1, measurement_covariance(index_b)); + } + + // Check that the factor noise at the factor index matches the provided noise. + void EXPECT_SAME_PRIOR_NOISE(const int factor_index, const gtsam::SharedNoiseModel& noise) { + const auto pose_prior_factor = dynamic_cast<gtsam::PriorFactor<gtsam::Pose3>*>(factors_[factor_index].get()); + EXPECT_SAME_NOISE(pose_prior_factor, noise); + } + + // Check that the relative pose, noise, and node pose at the provided factor index + // matches the expected pose and noise at the same measurement index. + void EXPECT_SAME_BETWEEN_FACTOR(const int index) { + EXPECT_SAME_NODE_POSE(index); + EXPECT_SAME_BETWEEN_FACTOR_POSE(index); + EXPECT_SAME_BETWEEN_FACTOR_NOISE(index); + } + + // Check that the relative pose, noise, and node pose at factor index b + // match computed interpolated relative pose, noise, and node pose using measurement + // indices a and b. + void EXPECT_SAME_BETWEEN_FACTOR_INTERPOLATED(const int index_a, const int index_b, const double alpha) { + EXPECT_SAME_NODE_POSE_INTERPOLATED(index_a, index_b, alpha); + EXPECT_SAME_BETWEEN_FACTOR_POSE_INTERPOLATED(index_a, index_b, alpha); + EXPECT_SAME_BETWEEN_FACTOR_NOISE_INTERPOLATED(index_a, index_b, alpha); + } + + // Check that the relative pose, noise, and node pose at factor index b + // match the second half of the computed interpolated relative pose, noise, and node pose using measurement + // indices a and b. + void EXPECT_SAME_SECOND_BETWEEN_FACTOR_INTERPOLATED(const int index_a, const int index_b, const double alpha) { + EXPECT_SAME_NODE_POSE(index_b); + EXPECT_SAME_SECOND_BETWEEN_FACTOR_NOISE_INTERPOLATED(index_a, index_b, alpha); + EXPECT_SAME_SECOND_BETWEEN_FACTOR_POSE_INTERPOLATED(index_a, index_b, alpha); + } + + std::unique_ptr<na::PoseNodeAdder> pose_node_adder_; + na::PoseNodeAdderParams params_; + na::TimestampedNodeAdderModelParams node_adder_model_params_; + std::vector<lm::PoseWithCovarianceMeasurement> pose_measurements_; + std::vector<lc::Time> timestamps_; + gtsam::NonlinearFactorGraph factors_; + + private: + const double time_increment_; + const lc::Time start_time_; + const int num_measurements_; +}; + +// Test CanAddNode() with add and removing measurements. +TEST_F(PoseNodeAdderTest, AddRemoveCanAddNode) { + DefaultInitialize(); + EXPECT_FALSE(pose_node_adder_->CanAddNode(10.1)); + constexpr double epsilon = 0.1; + // With no measurements added, no nodes should be able to be added. + EXPECT_FALSE(pose_node_adder_->CanAddNode(params_.starting_time)); + EXPECT_FALSE(pose_node_adder_->CanAddNode(params_.starting_time + epsilon)); + EXPECT_FALSE(pose_node_adder_->CanAddNode(params_.starting_time - epsilon)); + // Add measurement 0, only a node at that timestamp should be able to be added. + pose_node_adder_->AddMeasurement(pose_measurements_[0]); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[0])); + EXPECT_FALSE(pose_node_adder_->CanAddNode(timestamps_[0] + epsilon)); + EXPECT_FALSE(pose_node_adder_->CanAddNode(timestamps_[0] - epsilon)); + // Add measurement 1, nodes a times [t_0, t_1] should be able to be added. + pose_node_adder_->AddMeasurement(pose_measurements_[1]); + EXPECT_FALSE(pose_node_adder_->CanAddNode(timestamps_[0] - epsilon)); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[0])); + EXPECT_TRUE(pose_node_adder_->CanAddNode((timestamps_[0] + timestamps_[1]) / 2.0)); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[1])); + EXPECT_FALSE(pose_node_adder_->CanAddNode(timestamps_[1] + epsilon)); + // Add measurement 2, nodes a times [t_0, t_2] should be able to be added. + pose_node_adder_->AddMeasurement(pose_measurements_[2]); + EXPECT_FALSE(pose_node_adder_->CanAddNode(timestamps_[0] - epsilon)); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[0])); + EXPECT_TRUE(pose_node_adder_->CanAddNode((timestamps_[0] + timestamps_[1]) / 2.0)); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[1])); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[1] + epsilon)); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[2])); + EXPECT_FALSE(pose_node_adder_->CanAddNode(timestamps_[2] + epsilon)); + // Add measurement 3, nodes a times [t_0, t_3] should be able to be added. + pose_node_adder_->AddMeasurement(pose_measurements_[3]); + EXPECT_FALSE(pose_node_adder_->CanAddNode(timestamps_[0] - epsilon)); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[0])); + EXPECT_TRUE(pose_node_adder_->CanAddNode((timestamps_[0] + timestamps_[1]) / 2.0)); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[1])); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[1] + epsilon)); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[2])); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[2] + epsilon)); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[3])); + EXPECT_FALSE(pose_node_adder_->CanAddNode(timestamps_[3] + epsilon)); + + // Remove measurement 0, but keep 1 since it's the lower bound and + // may be used for interpolation + pose_node_adder_->RemoveMeasurements(timestamps_[1] + epsilon); + EXPECT_FALSE(pose_node_adder_->CanAddNode(params_.starting_time)); + EXPECT_FALSE(pose_node_adder_->CanAddNode(timestamps_[0])); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[1])); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[2])); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[2] + epsilon)); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[3])); + EXPECT_FALSE(pose_node_adder_->CanAddNode(timestamps_[3] + epsilon)); +} + +// Test initialization at a random time and pose. +TEST_F(PoseNodeAdderTest, InitializeWithRandomTime) { + const auto pose = lc::RandomPose(); + const auto time = lc::RandomDouble(); + pose_node_adder_.reset(new na::PoseNodeAdder(params_, node_adder_model_params_, std::make_shared<no::Values>())); + pose_node_adder_->AddInitialNodesAndPriors(pose, params_.start_noise_models, time, factors_); + const auto& nodes = pose_node_adder_->nodes(); + // There should only be one start node and one factor (prior factor on start node) + EXPECT_EQ(nodes.size(), 1); + EXPECT_EQ(factors_.size(), 1); + EXPECT_SAME_NODE_POSE(time, pose); + EXPECT_SAME_PRIOR_FACTOR_POSE(0, pose); + EXPECT_SAME_PRIOR_NOISE(0, params_.start_noise_models[0]); +} + +// Test between factor and node creation after adding measurements and using +// random initialization. +TEST_F(PoseNodeAdderTest, AddNodesAfterRandomInitialization) { + RandomlyInitialize(0.0); + const auto& nodes = pose_node_adder_->nodes(); + // Starting node and prior factor should exist after initialization. + // Values should be the same as starting node/time/noise in provided params. + // Graph structure: + // P_0 -> N_0 + EXPECT_EQ(nodes.size(), 1); + EXPECT_EQ(factors_.size(), 1); + EXPECT_SAME_NODE_POSE(params_.starting_time, params_.start_node); + EXPECT_SAME_PRIOR_FACTOR_POSE(0, params_.start_node); + EXPECT_SAME_PRIOR_NOISE(0, params_.start_noise_models[0]); + + AddMeasurements(); + // Add Node at timestamp 1. + // Graph structure: + // P_0 -> N_0 <-B_0_1-> N_1 + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[1], factors_)); + EXPECT_EQ(nodes.size(), 2); + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_NODE_POSE(params_.starting_time, params_.start_node); + EXPECT_SAME_PRIOR_FACTOR_POSE(0, params_.start_node); + EXPECT_SAME_PRIOR_NOISE(0, params_.start_noise_models[0]); + EXPECT_SAME_BETWEEN_FACTOR(1); + // Add Node at timestamp 2. + // Graph structure: + // P_0 -> N_0 <-B_0_1-> N_1 <-B_1_2-> N_2 + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[2], factors_)); + EXPECT_EQ(nodes.size(), 3); + EXPECT_EQ(factors_.size(), 3); + EXPECT_SAME_NODE_POSE(params_.starting_time, params_.start_node); + EXPECT_SAME_PRIOR_FACTOR_POSE(0, params_.start_node); + EXPECT_SAME_PRIOR_NOISE(0, params_.start_noise_models[0]); + EXPECT_SAME_BETWEEN_FACTOR(1); + EXPECT_SAME_BETWEEN_FACTOR(2); + // Add Node at timestamp 3. + // Graph structure: + // P_0 -> N_0 <-B_0_1-> N_1 <-B_1_2-> N_2 <-B_2_3-> N_3 + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[3], factors_)); + EXPECT_EQ(nodes.size(), 4); + EXPECT_EQ(factors_.size(), 4); + EXPECT_SAME_NODE_POSE(params_.starting_time, params_.start_node); + EXPECT_SAME_PRIOR_FACTOR_POSE(0, params_.start_node); + EXPECT_SAME_PRIOR_NOISE(0, params_.start_noise_models[0]); + EXPECT_SAME_BETWEEN_FACTOR(1); + EXPECT_SAME_BETWEEN_FACTOR(2); + EXPECT_SAME_BETWEEN_FACTOR(3); + // Add Node in between timestamps 3 and 4. Node and between factor should be interpolated. + // Graph structure: + // P_0 -> N_0 <-B_0_1-> N_1 <-B_1_2-> N_2 <-B_2_3-> N_3 <-B_3_3.5-> N_3.5 + const lc::Time timestamp_3_4 = (timestamps_[3] + timestamps_[4]) / 2.0; + ASSERT_TRUE(pose_node_adder_->AddNode(timestamp_3_4, factors_)); + EXPECT_EQ(nodes.size(), 5); + EXPECT_EQ(factors_.size(), 5); + EXPECT_SAME_NODE_POSE(params_.starting_time, params_.start_node); + EXPECT_SAME_PRIOR_FACTOR_POSE(0, params_.start_node); + EXPECT_SAME_PRIOR_NOISE(0, params_.start_noise_models[0]); + EXPECT_SAME_BETWEEN_FACTOR(1); + EXPECT_SAME_BETWEEN_FACTOR(2); + EXPECT_SAME_BETWEEN_FACTOR(3); + EXPECT_SAME_BETWEEN_FACTOR_INTERPOLATED(3, 4, 0.5); +} + +// Test adding node before first measurement, should fail. +TEST_F(PoseNodeAdderTest, AddNodeTooSoon) { + ZeroInitialize(); + const auto& nodes = pose_node_adder_->nodes(); + EXPECT_EQ(nodes.size(), 1); + AddMeasurements(); + // Add node at timestamp 1. + // Graph structure: + // P_0 -> N_0 <-B_0_1-> N_1 + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[1], factors_)); + EXPECT_EQ(nodes.size(), 2); + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_PRIOR_FACTOR_POSE(0, params_.start_node); + EXPECT_SAME_BETWEEN_FACTOR(1); + // Add node before starting time. + ASSERT_FALSE(pose_node_adder_->AddNode(params_.starting_time - 1.0, factors_)); + EXPECT_EQ(nodes.size(), 2); + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_PRIOR_FACTOR_POSE(0, params_.start_node); + EXPECT_SAME_BETWEEN_FACTOR(1); +} + +// Test adding node after last measurement, should fail. +TEST_F(PoseNodeAdderTest, AddNodeTooLate) { + ZeroInitialize(); + const auto& nodes = pose_node_adder_->nodes(); + EXPECT_EQ(nodes.size(), 1); + AddMeasurements(); + // Add node at timestamp 1. + // Graph structure: + // P_0 -> N_0 <-B_0_1-> N_1 + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[1], factors_)); + EXPECT_EQ(nodes.size(), 2); + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_PRIOR_FACTOR_POSE(0, params_.start_node); + EXPECT_SAME_BETWEEN_FACTOR(1); + // Try to add node after last measurement time. + ASSERT_FALSE(pose_node_adder_->AddNode(timestamps_.back() + 1.0, factors_)); + EXPECT_EQ(nodes.size(), 2); + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_PRIOR_FACTOR_POSE(0, params_.start_node); + EXPECT_SAME_BETWEEN_FACTOR(1); +} + +// Test adding interpolated node between start node and first measurement. +TEST_F(PoseNodeAdderTest, AddNodeBetweenStartAndFirstMeasurement) { + ZeroInitialize(); + const auto& nodes = pose_node_adder_->nodes(); + EXPECT_EQ(nodes.size(), 1); + EXPECT_EQ(factors_.size(), 1); + AddMeasurements(); + // Add node in between start pose and 1st measurement + // Graph structure: + // P_0 -> N_0 <-B_0_0.5-> N_0.5 + const lc::Time timestamp_0_5 = (params_.starting_time + timestamps_[1]) / 2.0; + ASSERT_TRUE(pose_node_adder_->AddNode(timestamp_0_5, factors_)); + EXPECT_EQ(nodes.size(), 2); + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_BETWEEN_FACTOR_INTERPOLATED(0, 1, 0.5); +} + +// Adding the same node twice should result in no change to the graph size. +TEST_F(PoseNodeAdderTest, AddSameNode) { + ZeroInitialize(); + const auto& nodes = pose_node_adder_->nodes(); + EXPECT_EQ(nodes.size(), 1); + AddMeasurements(); + // Add node at timestamp 1. + // Graph structure: + // P_0 -> N_0 <-B_0_1-> N_1 + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[1], factors_)); + EXPECT_EQ(nodes.size(), 2); + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_PRIOR_FACTOR_POSE(0, params_.start_node); + EXPECT_SAME_BETWEEN_FACTOR(1); + // Add same node at timestamp 1, shouldn't change graph structure. + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[1], factors_)); + EXPECT_EQ(nodes.size(), 2); + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_PRIOR_FACTOR_POSE(0, params_.start_node); + EXPECT_SAME_BETWEEN_FACTOR(1); +} + +// Test adding node in between already added 1st and 2nd nodes. +TEST_F(PoseNodeAdderTest, SplitNode) { + ZeroInitialize(); + const auto& nodes = pose_node_adder_->nodes(); + EXPECT_EQ(nodes.size(), 1); + AddMeasurements(); + // Add node at timestamp 1. + // Graph structure: + // P_0 -> N_0 <-B_0_1-> N_1 + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[1], factors_)); + EXPECT_EQ(nodes.size(), 2); + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_PRIOR_FACTOR_POSE(0, params_.start_node); + EXPECT_SAME_BETWEEN_FACTOR(1); + // Add node at timestamp 2. + // Graph structure: + // P_0 -> N_0 <-B_0_1-> N_1 <-B_1_2-> N_2 + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[2], factors_)); + EXPECT_EQ(nodes.size(), 3); + EXPECT_EQ(factors_.size(), 3); + EXPECT_SAME_PRIOR_FACTOR_POSE(0, params_.start_node); + EXPECT_SAME_BETWEEN_FACTOR(1); + EXPECT_SAME_BETWEEN_FACTOR(2); + // Add node at timestamp in between 1st and 2nd measurement + const lc::Time timestamp_1_2 = (timestamps_[1] + timestamps_[2]) / 2.0; + ASSERT_TRUE(pose_node_adder_->AddNode(timestamp_1_2, factors_)); + EXPECT_EQ(nodes.size(), 4); + EXPECT_EQ(factors_.size(), 4); + EXPECT_SAME_PRIOR_FACTOR_POSE(0, params_.start_node); + EXPECT_SAME_BETWEEN_FACTOR(1); + EXPECT_SAME_BETWEEN_FACTOR_INTERPOLATED(1, 2, 0.5); + EXPECT_SAME_SECOND_BETWEEN_FACTOR_INTERPOLATED(1, 2, 0.5); +} + +// Test getting old keys from the pose node adder. +// Assumes keys start at 1 and increase by 1 +TEST_F(PoseNodeAdderTest, OldKeys) { + ZeroInitialize(); + AddMeasurements(); + // No keys should return when using timestamp before the start node. + EXPECT_TRUE(pose_node_adder_->OldKeys(-1, factors_).empty()); + { + // Start node key should return when using more recent timestamp than the start node. + const auto old_keys = pose_node_adder_->OldKeys(1, factors_); + ASSERT_EQ(old_keys.size(), 1); + EXPECT_EQ(old_keys[0], 1); + } + // Add node at 1st measurement timestamp. + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[1], factors_)); + { + // Start node and first node keys should return when given 2nd timestamp. + const auto old_keys = pose_node_adder_->OldKeys(timestamps_[2], factors_); + ASSERT_EQ(old_keys.size(), 2); + EXPECT_EQ(old_keys[0], 1); + EXPECT_EQ(old_keys[1], 2); + } + // Same timestamp shouldn't remove same timestamped node's key - + // only start node key should be old. + { + const auto old_keys = pose_node_adder_->OldKeys(timestamps_[1], factors_); + ASSERT_EQ(old_keys.size(), 1); + EXPECT_EQ(old_keys[0], 1); + } + // Add node at second timestamp. + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[2], factors_)); + { + // Start, first, and second node keys should return when given more recent timestamp. + const auto old_keys = pose_node_adder_->OldKeys(timestamps_[5], factors_); + ASSERT_EQ(old_keys.size(), 3); + EXPECT_EQ(old_keys[0], 1); + EXPECT_EQ(old_keys[1], 2); + EXPECT_EQ(old_keys[2], 3); + } + { + // Start and first node keys should return when given timestamp more recent than 1st + // node but older than second node. + const auto old_keys = pose_node_adder_->OldKeys((timestamps_[1] + timestamps_[2]) / 2.0, factors_); + ASSERT_EQ(old_keys.size(), 2); + EXPECT_EQ(old_keys[0], 1); + EXPECT_EQ(old_keys[1], 2); + } +} + +// Test slide window new start time when max duration violations occur. +TEST_F(PoseNodeAdderTest, NewStartTimeDurationViolation) { + params_.min_num_states = 0; + params_.max_num_states = 10; + params_.starting_time = 0.0; + params_.ideal_duration = 1.5; + pose_node_adder_.reset(new na::PoseNodeAdder(params_, node_adder_model_params_, std::make_shared<no::Values>())); + AddMeasurements(); + // Empty nodes, so expect invalid oldest time + EXPECT_TRUE(pose_node_adder_->SlideWindowNewStartTime() == boost::none); + pose_node_adder_->AddInitialNodesAndPriors(factors_); + // Duration: 0. States: 1 + // Min/Max states not applicable, less than ideal duration (0 < 1.5), so + // should return 0 + { + const auto new_oldest_time = pose_node_adder_->SlideWindowNewStartTime(); + ASSERT_TRUE(new_oldest_time != boost::none); + EXPECT_EQ(*new_oldest_time, 0.0); + } + // Add node. Duration: 1. States: 2 + // Min/Max states not applicable, less than ideal duration (1 < 1.5), so + // should return 0 + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[1], factors_)); + { + const auto new_oldest_time = pose_node_adder_->SlideWindowNewStartTime(); + ASSERT_TRUE(new_oldest_time != boost::none); + EXPECT_EQ(*new_oldest_time, 0.0); + } + // Add node. Duration: 2, states: 3 + // Duration > ideal duration, should remove start node and return 1st nodes' time + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[2], factors_)); + { + const auto new_oldest_time = pose_node_adder_->SlideWindowNewStartTime(); + ASSERT_TRUE(new_oldest_time != boost::none); + EXPECT_EQ(*new_oldest_time, timestamps_[1]); + } + // Add node. Duration: 3, states: 3 + // Duration > ideal duration, should remove start and 1st node and return 2nd nodes' time + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[3], factors_)); + { + const auto new_oldest_time = pose_node_adder_->SlideWindowNewStartTime(); + ASSERT_TRUE(new_oldest_time != boost::none); + EXPECT_EQ(*new_oldest_time, timestamps_[2]); + } +} + +// Test slide window new start time when max states violations occur. +TEST_F(PoseNodeAdderTest, NewStartTimeMinMaxStatesViolation) { + params_.min_num_states = 1; + params_.max_num_states = 3; + params_.starting_time = 0.0; + params_.ideal_duration = 100; + pose_node_adder_.reset(new na::PoseNodeAdder(params_, node_adder_model_params_, std::make_shared<no::Values>())); + const auto& nodes = pose_node_adder_->nodes(); + AddMeasurements(); + // Empty nodes, so expect invalid oldest time + EXPECT_TRUE(pose_node_adder_->SlideWindowNewStartTime() == boost::none); + EXPECT_EQ(nodes.size(), 0); + pose_node_adder_->AddInitialNodesAndPriors(factors_); + // 1 node <= min_states (1), so expect invalid oldest time + EXPECT_TRUE(pose_node_adder_->SlideWindowNewStartTime() == boost::none); + EXPECT_EQ(nodes.size(), 1); + // Add node. States: 2, less than ideal duration so should return 0. + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[1], factors_)); + EXPECT_EQ(nodes.size(), 2); + { + const auto new_oldest_time = pose_node_adder_->SlideWindowNewStartTime(); + ASSERT_TRUE(new_oldest_time != boost::none); + EXPECT_EQ(*new_oldest_time, 0.0); + } + // Add node. States: 3, less than ideal duration so should return 0. + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[2], factors_)); + EXPECT_EQ(nodes.size(), 3); + { + const auto new_oldest_time = pose_node_adder_->SlideWindowNewStartTime(); + ASSERT_TRUE(new_oldest_time != boost::none); + EXPECT_EQ(*new_oldest_time, 0.0); + } + // Add node. States: 4, > max num states, should remove 1st state. + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[3], factors_)); + EXPECT_EQ(nodes.size(), 4); + { + const auto new_oldest_time = pose_node_adder_->SlideWindowNewStartTime(); + ASSERT_TRUE(new_oldest_time != boost::none); + EXPECT_EQ(*new_oldest_time, timestamps_[1]); + } + // Add node. States: 5, > max num states, should remove 1st and 2nd state. + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[4], factors_)); + EXPECT_EQ(nodes.size(), 5); + { + const auto new_oldest_time = pose_node_adder_->SlideWindowNewStartTime(); + ASSERT_TRUE(new_oldest_time != boost::none); + EXPECT_EQ(*new_oldest_time, timestamps_[2]); + } +} + +// Test sliding window +TEST_F(PoseNodeAdderTest, SlideWindow) { + params_.start_node = gtsam::Pose3::identity(); + params_.starting_time = 0.0; + params_.min_num_states = 2; + params_.max_num_states = 5; + params_.ideal_duration = 1.5; + pose_node_adder_.reset(new na::PoseNodeAdder(params_, node_adder_model_params_, std::make_shared<no::Values>())); + pose_node_adder_->AddInitialNodesAndPriors(factors_); + + // Only start node exists + // Graph structure: + // P_0 -> N_0 + const auto& nodes = pose_node_adder_->nodes(); + EXPECT_EQ(nodes.size(), 1); + EXPECT_EQ(factors_.size(), 1); + EXPECT_EQ(*pose_node_adder_->StartTime(), params_.starting_time); + EXPECT_EQ(*pose_node_adder_->EndTime(), params_.starting_time); + AddMeasurements(); + // Add 1st node, nodes: 2, duration: 1 + // Graph structure: + // P_0 -> N_0 <-B_0_1-> N_1 + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[1], factors_)); + EXPECT_EQ(nodes.size(), 2); + EXPECT_EQ(factors_.size(), 2); + EXPECT_SAME_BETWEEN_FACTOR(1); + // Add 2nd node, nodes: 3, duration 2 + // Graph structure: + // P_0 -> N_0 <-B_0_1-> N_1 <-B_1_2-> N_2 + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[2], factors_)); + EXPECT_EQ(nodes.size(), 3); + EXPECT_EQ(factors_.size(), 3); + EXPECT_SAME_BETWEEN_FACTOR(1); + EXPECT_SAME_BETWEEN_FACTOR(2); + // Slide window with time older than oldest allowed time, nothing should happen + ASSERT_TRUE(pose_node_adder_->SlideWindow(-1, boost::none, gtsam::KeyVector(), params_.huber_k, factors_)); + EXPECT_EQ(nodes.size(), 3); + EXPECT_EQ(factors_.size(), 3); + EXPECT_SAME_BETWEEN_FACTOR(1); + EXPECT_SAME_BETWEEN_FACTOR(2); + // Add 3rd node, nodes: 4, duration 3 + // Graph structure: + // P_0 -> N_0 <-B_0_1-> N_1 <-B_1_2-> N_2 <-B_2_3-> N_3 + ASSERT_TRUE(pose_node_adder_->AddNode(timestamps_[3], factors_)); + EXPECT_EQ(nodes.size(), 4); + EXPECT_EQ(factors_.size(), 4); + EXPECT_SAME_BETWEEN_FACTOR(1); + EXPECT_SAME_BETWEEN_FACTOR(2); + EXPECT_SAME_BETWEEN_FACTOR(3); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[1] - 0.1)); + // Slide window + // Expect to remove first two nodes, so duration is 1 + const auto new_start_time = pose_node_adder_->SlideWindowNewStartTime(); + ASSERT_TRUE(new_start_time != boost::none); + EXPECT_EQ(*new_start_time, timestamps_[2]); + const auto old_keys = pose_node_adder_->OldKeys(*new_start_time, factors_); + EXPECT_EQ(old_keys.size(), 2); + ASSERT_TRUE(pose_node_adder_->SlideWindow(*new_start_time, boost::none, old_keys, params_.huber_k, factors_)); + // Graph structure: + // P_2 -> N_2 <-B_2_3-> N_3 + EXPECT_EQ(nodes.size(), 2); + // 2 nodes should be the 2 poses added after the start node + EXPECT_SAME_NODE_POSE(2); + EXPECT_SAME_NODE_POSE(3); + // Between factors aren't removed yet (removed in sliding window graph optimizer), + // so all between factors should remain, + // only prior should be removed/added (removed from start node, added to 1st node) + EXPECT_EQ(factors_.size(), 4); + // Indices changed since prior removed then added last, so factor index 0 -> between factor 1 and so on + EXPECT_SAME_BETWEEN_FACTOR_POSE(0, 1); + EXPECT_SAME_BETWEEN_FACTOR_POSE(1, 2); + EXPECT_SAME_BETWEEN_FACTOR_POSE(2, 3); + // Prior factor added last + { + const gtsam::Pose3 relative_pose = lc::GtPose(measurement_pose(0).inverse() * measurement_pose(2)); + const gtsam::Pose3 expected_prior_pose = params_.start_node * relative_pose; + EXPECT_SAME_PRIOR_FACTOR_POSE(3, expected_prior_pose); + } + // Since no marginals available, noise should default to start noise + EXPECT_SAME_PRIOR_NOISE(3, params_.start_noise_models[0]); + EXPECT_EQ(*pose_node_adder_->StartTime(), timestamps_[2]); + EXPECT_EQ(*pose_node_adder_->EndTime(), timestamps_[3]); + // Old measurements removed when slide window but lower bound kept in case needed for interpolation + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[1])); + EXPECT_TRUE(pose_node_adder_->CanAddNode(timestamps_[2])); + EXPECT_FALSE(pose_node_adder_->CanAddNode(timestamps_[0])); +} +// TODO(rsoussan): Test slide window with valid marginals to create valid prior covariances + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/node_adders/test/test_pose_node_adder.test b/localization/node_adders/test/test_pose_node_adder.test new file mode 100644 index 0000000000..185d52a1d5 --- /dev/null +++ b/localization/node_adders/test/test_pose_node_adder.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="node_adders" type="test_pose_node_adder" test-name="test_pose_node_adder" /> +</launch> diff --git a/localization/node_adders/test/test_utilities.cc b/localization/node_adders/test/test_utilities.cc new file mode 100644 index 0000000000..ea03501e8c --- /dev/null +++ b/localization/node_adders/test/test_utilities.cc @@ -0,0 +1,52 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "test_utilities.h" // NOLINT +#include <imu_integration/test_utilities.h> +#include <localization_common/logger.h> +#include <localization_common/utilities.h> + +namespace node_adders { +namespace ii = imu_integration; +namespace lc = localization_common; +namespace lm = localization_measurements; + +PoseNodeAdderParams DefaultPoseNodeAdderParams() { + PoseNodeAdderParams params; + params.starting_prior_translation_stddev = 0.1; + params.starting_prior_quaternion_stddev = 0.1; + // Base + params.start_node = gtsam::Pose3::identity(); + params.huber_k = 1.345; + params.add_priors = true; + params.starting_time = 0; + // Only kept if there are at least min_num_states and not more than max_num_states + params.ideal_duration = 5; + params.min_num_states = 5; + params.max_num_states = 20; + params.SetStartNoiseModels(); + return params; +} + +CombinedNavStateNodeAdderModelParams DefaultCombinedNavStateNodeAdderModelParams() { + CombinedNavStateNodeAdderModelParams params; + params.imu_integrator = ii::DefaultImuIntegratorParams(); + params.huber_k = 1.345; + return params; +} +} // namespace node_adders diff --git a/localization/graph_optimizer/src/graph_values.cc b/localization/node_adders/test/test_utilities.h similarity index 62% rename from localization/graph_optimizer/src/graph_values.cc rename to localization/node_adders/test/test_utilities.h index a9483caec1..1f92d52e2d 100644 --- a/localization/graph_optimizer/src/graph_values.cc +++ b/localization/node_adders/test/test_utilities.h @@ -16,14 +16,15 @@ * under the License. */ -#include <graph_optimizer/graph_values.h> +#ifndef NODE_ADDERS_TEST_UTILITIES_H_ // NOLINT +#define NODE_ADDERS_TEST_UTILITIES_H_ // NOLINT -namespace graph_optimizer { -namespace lc = localization_common; -namespace lm = localization_measurements; -GraphValues::GraphValues(std::shared_ptr<gtsam::Values> values) : values_(std::move(values)) {} +#include <node_adders/combined_nav_state_node_adder_model_params.h> +#include <node_adders/pose_node_adder_params.h> -const gtsam::Values& GraphValues::values() const { return *values_; } +namespace node_adders { +PoseNodeAdderParams DefaultPoseNodeAdderParams(); +CombinedNavStateNodeAdderModelParams DefaultCombinedNavStateNodeAdderModelParams(); +} // namespace node_adders -gtsam::Values& GraphValues::values() { return *values_; } -} // namespace graph_optimizer +#endif // NODE_ADDERS_TEST_UTILITIES_H_ // NOLINT diff --git a/localization/nodes/CMakeLists.txt b/localization/nodes/CMakeLists.txt new file mode 100644 index 0000000000..0832f53498 --- /dev/null +++ b/localization/nodes/CMakeLists.txt @@ -0,0 +1,106 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(nodes) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +find_package(Boost REQUIRED COMPONENTS serialization) + +# Find GTSAM +find_package(GTSAM REQUIRED) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + localization_common +) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +# include ff_nodelet to get ff_util header files since these aren't exposed elsewhere +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} + CATKIN_DEPENDS localization_common +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${GLOG_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + src/combined_nav_state_nodes.cc + src/values.cc +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_values + test/test_values.test + test/test_values.cc + ) + target_link_libraries(test_values + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_timestamped_nodes + test/test_timestamped_nodes.test + test/test_timestamped_nodes.cc + ) + target_link_libraries(test_timestamped_nodes + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_timestamped_combined_nodes + test/test_timestamped_combined_nodes.test + test/test_timestamped_combined_nodes.cc + ) + target_link_libraries(test_timestamped_combined_nodes + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/nodes/include/nodes/combined_nav_state_nodes.h b/localization/nodes/include/nodes/combined_nav_state_nodes.h new file mode 100644 index 0000000000..3c02982ceb --- /dev/null +++ b/localization/nodes/include/nodes/combined_nav_state_nodes.h @@ -0,0 +1,49 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef NODES_COMBINED_NAV_STATE_NODES_H_ +#define NODES_COMBINED_NAV_STATE_NODES_H_ + +#include <localization_common/combined_nav_state.h> +#include <nodes/timestamped_combined_nodes.h> + +namespace nodes { +class CombinedNavStateNodes : public TimestampedCombinedNodes<localization_common::CombinedNavState> { + using Base = TimestampedCombinedNodes<localization_common::CombinedNavState>; + + public: + explicit CombinedNavStateNodes(std::shared_ptr<Values> values); + + // For serialization only + CombinedNavStateNodes() = default; + + private: + gtsam::KeyVector AddNode(const localization_common::CombinedNavState& node) final; + + boost::optional<localization_common::CombinedNavState> GetNode(const gtsam::KeyVector& keys, + const localization_common::Time timestamp) const final; + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; +} // namespace nodes + +#endif // NODES_COMBINED_NAV_STATE_NODES_H_ diff --git a/localization/nodes/include/nodes/timestamped_combined_nodes.h b/localization/nodes/include/nodes/timestamped_combined_nodes.h new file mode 100644 index 0000000000..be20fe496d --- /dev/null +++ b/localization/nodes/include/nodes/timestamped_combined_nodes.h @@ -0,0 +1,386 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef NODES_TIMESTAMPED_COMBINED_NODES_H_ +#define NODES_TIMESTAMPED_COMBINED_NODES_H_ + +#include <localization_common/timestamped_set.h> +#include <localization_common/time.h> +#include <nodes/values.h> + +#include <gtsam/nonlinear/NonlinearFactorGraph.h> + +#include <boost/optional.hpp> +#include <boost/serialization/serialization.hpp> + +#include <map> +#include <utility> +#include <vector> + +namespace nodes { +template <typename NodeType> +// Container for timestamped nodes with multiple values per node. +// Enables a NodeAdder to couple multiple values at the same timestamp that are always added and updated together into a +// single combined node. For example, a visual-intertial NodeAdder may always add and update pose, velocity, and +// IMU-biases together and never individually, so these should be grouped into a combined node rather than treated +// separately. The Add and Node functions must be specialized for the desired combined node. For nodes with a single +// value, such as just a pose or position, use TimestampedNodes, which is a specialization of TimestampedCombinedNodes +// with CombinedType = false. +class TimestampedCombinedNodes { + public: + // Values should be provided by a graph optimizer and the same values + // should be passed to all timestamped nodes in the graph. + explicit TimestampedCombinedNodes(std::shared_ptr<Values> values); + + // For serialization only + TimestampedCombinedNodes() = default; + + virtual ~TimestampedCombinedNodes() = default; + + // Add a node at the provided timestamp and return the GTSAM keys that correspond to it. + // The keys are used in graph factors to connect a factor to a node. + // Note that a single node can have multiple keys, for example a pose velocity node + // may have a pose key and velocity key. The ordering of keys is set in the specialized + // Add(node) function for the NodeType. + gtsam::KeyVector Add(const localization_common::Time timestamp, const NodeType& node); + + // Returns a node at the provided timestamp if it exists. + boost::optional<NodeType> Node(const localization_common::Time timestamp) const; + + // Returns a portion of a combined node (or a full non-combined node) + // with the provided key if it exists. + // To return a combined node, use Node(timestamp) instead. + template <typename T> + boost::optional<T> Value(const gtsam::Key& key) const; + + // Returns the keys for a timestamped node given the timestamp if it exists. + // If not, an empty key vector is returned. + gtsam::KeyVector Keys(const localization_common::Time timestamp) const; + + // Removes a node at the provided timestamp if it exists. + // Returns if a node was successfully removed. + bool Remove(const localization_common::Time& timestamp); + + // Returns the number of nodes. This does not return the number of values, + // so if one combined node containing a pose and velocity exists, this will + // return 1 for example. + size_t size() const; + + // Returns if there node container is empty. + bool empty() const; + + // Returns the oldest timestamp of the nodes in the container. + // Returns boost::none if no nodes exists. + boost::optional<localization_common::Time> OldestTimestamp() const; + + // Returns the oldest nodes in the container. + // Returns boost::none if no nodes exists. + boost::optional<NodeType> OldestNode() const; + + // Returns the latest timestamp of the nodes in the container. + // Returns boost::none if no nodes exists. + boost::optional<localization_common::Time> LatestTimestamp() const; + + // Returns the latest node in the container. + // Returns boost::none if no nodes exists. + boost::optional<NodeType> LatestNode() const; + + // Returns lower and upper time bounds for the provided timestamp. Equal values are set as lower and upper bound. + std::pair<boost::optional<localization_common::Time>, boost::optional<localization_common::Time>> + LowerAndUpperBoundTimestamps(const localization_common::Time timestamp) const; + + // Return lower and upper node bounds for the provided timestamp. Equal values are set as lower and upper bound. + std::pair<boost::optional<NodeType>, boost::optional<NodeType>> LowerAndUpperBoundNodes( + const localization_common::Time timestamp) const; + + // Returns the closest node in time to the provided timestamp. + boost::optional<NodeType> ClosestNode(const localization_common::Time timestamp) const; + + // Returns the closest node timestamp to the provided timestamp. + boost::optional<localization_common::Time> ClosestTimestamp(const localization_common::Time timestamp) const; + + // Returns the lower bounded or equal in time timestamped node to the provided timestamp. + boost::optional<localization_common::TimestampedValue<NodeType>> LowerBoundOrEqual( + const localization_common::Time timestamp) const; + + // Returns all nodes older than the provided oldest_allowed_timestamp. + std::vector<NodeType> OldNodes(const localization_common::Time oldest_allowed_timestamp) const; + + // Returns keys for all nodes older than the provied oldest_allowed_timestamp. + gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_timestamp) const; + + // Removes nodes older than the provied timestamp. + // Returns the number of removed nodes. + int RemoveOldNodes(const localization_common::Time oldest_allowed_timestamp); + + // Returns a vector containing the timestamps of all nodes in the container. + // Timestamps are sorted from oldest to latest. + std::vector<localization_common::Time> Timestamps() const; + + // Returns the total duration of node timestamps in the container. + double Duration() const; + + // Returns whether the container contains a node at the provided timestamp. + bool Contains(const localization_common::Time timestamp) const; + + // Const accessor for internal gtsam values. + const gtsam::Values& gtsam_values() const; + + protected: + std::shared_ptr<Values> values_; + + private: + // Removes a node with the provided keys if it exists. + bool Remove(const gtsam::KeyVector& keys); + + // Adds a node and returns the keys associated with it. + virtual gtsam::KeyVector AddNode(const NodeType& node) = 0; + + // Helper function that returns a node with the provied keys and timestamp if it exists. + virtual boost::optional<NodeType> GetNode(const gtsam::KeyVector& keys, + const localization_common::Time timestamp) const = 0; + + // Helper function that returns a node with the provided timestamped keys if it exists. + boost::optional<NodeType> Node(const localization_common::TimestampedValue<gtsam::KeyVector>& timestamped_keys) const; + + friend class boost::serialization::access; + template <class ARCHIVE> + void serialize(ARCHIVE& ar, const unsigned int /*version*/); + + localization_common::TimestampedSet<gtsam::KeyVector> timestamp_keys_map_; +}; + +// Implementation +template <typename NodeType> +TimestampedCombinedNodes<NodeType>::TimestampedCombinedNodes(std::shared_ptr<Values> values) + : values_(std::move(values)) {} + +template <typename NodeType> +gtsam::KeyVector TimestampedCombinedNodes<NodeType>::Add(const localization_common::Time timestamp, + const NodeType& node) { + if (Contains(timestamp)) return gtsam::KeyVector(); + gtsam::KeyVector keys = AddNode(node); + timestamp_keys_map_.Add(timestamp, keys); + return keys; +} + +template <typename NodeType> +boost::optional<NodeType> TimestampedCombinedNodes<NodeType>::Node(const localization_common::Time timestamp) const { + const auto keys = Keys(timestamp); + if (keys.empty()) return boost::none; + return GetNode(keys, timestamp); +} + +template <typename NodeType> +boost::optional<NodeType> TimestampedCombinedNodes<NodeType>::Node( + const localization_common::TimestampedValue<gtsam::KeyVector>& timestamped_keys) const { + return GetNode(timestamped_keys.value, timestamped_keys.timestamp); +} + +template <typename NodeType> +template <typename T> +boost::optional<T> TimestampedCombinedNodes<NodeType>::Value(const gtsam::Key& key) const { + return values_->Value<T>(key); +} + +template <typename NodeType> +gtsam::KeyVector TimestampedCombinedNodes<NodeType>::Keys(const localization_common::Time timestamp) const { + if (!Contains(timestamp)) return {}; + return (timestamp_keys_map_.Get(timestamp))->value; +} + +template <typename NodeType> +bool TimestampedCombinedNodes<NodeType>::Remove(const localization_common::Time& timestamp) { + const auto value = timestamp_keys_map_.Get(timestamp); + if (!value) return false; + bool successful_removal = true; + successful_removal = successful_removal && timestamp_keys_map_.Remove(timestamp); + successful_removal = successful_removal && Remove(value->value); + return successful_removal; +} + +template <typename NodeType> +bool TimestampedCombinedNodes<NodeType>::Remove(const gtsam::KeyVector& keys) { + return values_->Remove(keys); +} + +template <typename NodeType> +size_t TimestampedCombinedNodes<NodeType>::size() const { + return timestamp_keys_map_.size(); +} + +template <typename NodeType> +bool TimestampedCombinedNodes<NodeType>::empty() const { + return timestamp_keys_map_.empty(); +} + +template <typename NodeType> +boost::optional<localization_common::Time> TimestampedCombinedNodes<NodeType>::OldestTimestamp() const { + const auto oldest = timestamp_keys_map_.Oldest(); + if (!oldest) return boost::none; + return oldest->timestamp; +} + +template <typename NodeType> +boost::optional<NodeType> TimestampedCombinedNodes<NodeType>::OldestNode() const { + const auto oldest = timestamp_keys_map_.Oldest(); + if (!oldest) return boost::none; + return Node(*oldest); +} + +template <typename NodeType> +boost::optional<localization_common::Time> TimestampedCombinedNodes<NodeType>::LatestTimestamp() const { + const auto latest = timestamp_keys_map_.Latest(); + if (!latest) return boost::none; + return latest->timestamp; +} + +template <typename NodeType> +boost::optional<NodeType> TimestampedCombinedNodes<NodeType>::LatestNode() const { + const auto latest = timestamp_keys_map_.Latest(); + if (!latest) return boost::none; + return Node(*latest); +} + +template <typename NodeType> +std::pair<boost::optional<localization_common::Time>, boost::optional<localization_common::Time>> +TimestampedCombinedNodes<NodeType>::LowerAndUpperBoundTimestamps(const localization_common::Time timestamp) const { + const auto lower_and_upper_bound = timestamp_keys_map_.LowerAndUpperBound(timestamp); + boost::optional<localization_common::Time> lower_bound; + if (!lower_and_upper_bound.first) + lower_bound = boost::none; + else + lower_bound = lower_and_upper_bound.first->timestamp; + boost::optional<localization_common::Time> upper_bound; + if (!lower_and_upper_bound.second) + upper_bound = boost::none; + else + upper_bound = lower_and_upper_bound.second->timestamp; + return {lower_bound, upper_bound}; +} + +template <typename NodeType> +std::pair<boost::optional<NodeType>, boost::optional<NodeType>> +TimestampedCombinedNodes<NodeType>::LowerAndUpperBoundNodes(const localization_common::Time timestamp) const { + const auto lower_and_upper_bound = timestamp_keys_map_.LowerAndUpperBound(timestamp); + boost::optional<NodeType> lower_bound; + if (!lower_and_upper_bound.first) + lower_bound = boost::none; + else + lower_bound = Node(*(lower_and_upper_bound.first)); + boost::optional<NodeType> upper_bound; + if (!lower_and_upper_bound.second) + upper_bound = boost::none; + else + upper_bound = Node(*(lower_and_upper_bound.second)); + return {lower_bound, upper_bound}; +} + +template <typename NodeType> +boost::optional<localization_common::TimestampedValue<NodeType>> TimestampedCombinedNodes<NodeType>::LowerBoundOrEqual( + const localization_common::Time timestamp) const { + const auto lower_bound_or_equal = timestamp_keys_map_.LowerBoundOrEqual(timestamp); + if (!lower_bound_or_equal) return boost::none; + const auto node = Node(lower_bound_or_equal->timestamp); + if (!node) return boost::none; + return localization_common::TimestampedValue<NodeType>(lower_bound_or_equal->timestamp, *node); +} + +template <typename NodeType> +std::vector<localization_common::Time> TimestampedCombinedNodes<NodeType>::Timestamps() const { + return timestamp_keys_map_.Timestamps(); +} + +template <typename NodeType> +double TimestampedCombinedNodes<NodeType>::Duration() const { + return timestamp_keys_map_.Duration(); +} + +template <typename NodeType> +std::vector<NodeType> TimestampedCombinedNodes<NodeType>::OldNodes( + const localization_common::Time oldest_allowed_timestamp) const { + const auto old_values = timestamp_keys_map_.OldValues(oldest_allowed_timestamp); + std::vector<NodeType> old_nodes; + for (const auto& old_value : old_values) { + const auto old_node = Node(old_value); + if (!old_node) { + LogError("OldNodes: Failed to get node for keys."); + continue; + } + old_nodes.emplace_back(*old_node); + } + return old_nodes; +} + +template <typename NodeType> +gtsam::KeyVector TimestampedCombinedNodes<NodeType>::OldKeys( + const localization_common::Time oldest_allowed_timestamp) const { + const auto old_timestamp_key_sets = timestamp_keys_map_.OldValues(oldest_allowed_timestamp); + gtsam::KeyVector all_old_keys; + for (const auto& old_timestamp_key_set : old_timestamp_key_sets) { + const auto& old_keys = old_timestamp_key_set.value; + all_old_keys.insert(all_old_keys.end(), old_keys.begin(), old_keys.end()); + } + return all_old_keys; +} + +template <typename NodeType> +int TimestampedCombinedNodes<NodeType>::RemoveOldNodes(const localization_common::Time oldest_allowed_timestamp) { + const auto old_values = timestamp_keys_map_.OldValues(oldest_allowed_timestamp); + timestamp_keys_map_.RemoveOldValues(oldest_allowed_timestamp); + int num_removed_nodes = 0; + for (const auto& old_value : old_values) + if (Remove(old_value.value)) ++num_removed_nodes; + return num_removed_nodes; +} + +template <typename NodeType> +boost::optional<NodeType> TimestampedCombinedNodes<NodeType>::ClosestNode( + const localization_common::Time timestamp) const { + const auto closest = timestamp_keys_map_.Closest(timestamp); + if (!closest) return boost::none; + return Node(*closest); +} + +template <typename NodeType> +boost::optional<localization_common::Time> TimestampedCombinedNodes<NodeType>::ClosestTimestamp( + const localization_common::Time timestamp) const { + const auto closest = timestamp_keys_map_.Closest(timestamp); + if (!closest) return boost::none; + return closest->timestamp; +} + +template <typename NodeType> +const gtsam::Values& TimestampedCombinedNodes<NodeType>::gtsam_values() const { + return values_->gtsam_values(); +} + +template <typename NodeType> +bool TimestampedCombinedNodes<NodeType>::Contains(const localization_common::Time timestamp) const { + return timestamp_keys_map_.Contains(timestamp); +} + +template <typename NodeType> +template <class ARCHIVE> +void TimestampedCombinedNodes<NodeType>::serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(values_); + ar& BOOST_SERIALIZATION_NVP(timestamp_keys_map_); +} +} // namespace nodes + +#endif // NODES_TIMESTAMPED_COMBINED_NODES_H_ diff --git a/localization/nodes/include/nodes/timestamped_nodes.h b/localization/nodes/include/nodes/timestamped_nodes.h new file mode 100644 index 0000000000..b496faf78d --- /dev/null +++ b/localization/nodes/include/nodes/timestamped_nodes.h @@ -0,0 +1,70 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef NODES_TIMESTAMPED_NODES_H_ +#define NODES_TIMESTAMPED_NODES_H_ + +#include <nodes/timestamped_combined_nodes.h> + +namespace nodes { +template <typename NodeType> +// Container for timestamped nodes with a single value per node. +// Stores the templated NodeType at various timestamps for use with a graph optimizer. +// Child class of TimestampedCombinedNodes which handles multiple values per node/timestamp. +class TimestampedNodes : public TimestampedCombinedNodes<NodeType> { + using Base = TimestampedCombinedNodes<NodeType>; + + public: + explicit TimestampedNodes(std::shared_ptr<Values> values); + + // For serialization only + TimestampedNodes() = default; + + private: + gtsam::KeyVector AddNode(const NodeType& node) final; + + boost::optional<NodeType> GetNode(const gtsam::KeyVector& keys, + const localization_common::Time timestamp) const final; + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; + +// Implementation +template <typename NodeType> +TimestampedNodes<NodeType>::TimestampedNodes(std::shared_ptr<Values> values) + : TimestampedCombinedNodes<NodeType>(values) {} + +template <typename NodeType> +gtsam::KeyVector TimestampedNodes<NodeType>::AddNode(const NodeType& node) { + const auto key = this->values_->Add(node); + return {key}; +} + +template <typename NodeType> +boost::optional<NodeType> TimestampedNodes<NodeType>::GetNode(const gtsam::KeyVector& keys, + const localization_common::Time timestamp) const { + // Assumes keys only has a single key since using non-combined type + return Base::template Value<NodeType>(keys[0]); +} +} // namespace nodes + +#endif // NODES_TIMESTAMPED_NODES_H_ diff --git a/localization/nodes/include/nodes/values.h b/localization/nodes/include/nodes/values.h new file mode 100644 index 0000000000..6c731eeedd --- /dev/null +++ b/localization/nodes/include/nodes/values.h @@ -0,0 +1,91 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef NODES_VALUES_H_ +#define NODES_VALUES_H_ + +#include <localization_common/logger.h> + +#include <gtsam/base/serialization.h> +#include <gtsam/nonlinear/Values.h> + +#include <boost/optional.hpp> + +namespace nodes { +// Wrapper class around GTSAM values. +// Use one instance of this per graph and pass the same Values shared_pointer +// to Nodes classes used in the same graph. +class Values { + public: + explicit Values(std::shared_ptr<gtsam::Values> values = std::make_shared<gtsam::Values>()); + + template <typename ValueType> + boost::optional<ValueType> Value(const gtsam::Key& key) const; + + // Returns key for newly added value + template <typename ValueType> + gtsam::Key Add(const ValueType& value); + + bool Remove(const gtsam::Key& key); + + // TODO(rsoussan): Test this + bool Remove(const gtsam::KeyVector& keys); + + bool Contains(const gtsam::Key& key) const; + + size_t size() const; + + const gtsam::Values& gtsam_values() const { return *values_; } + + gtsam::Values& gtsam_values() { return *values_; } + + private: + friend class boost::serialization::access; + template <class ARCHIVE> + void serialize(ARCHIVE& ar, const unsigned int /*version*/); + + std::shared_ptr<gtsam::Values> values_; + gtsam::Key latest_key_; +}; + +// Implementation +template <typename ValueType> +boost::optional<ValueType> Values::Value(const gtsam::Key& key) const { + try { + return values_->at<ValueType>(key); + } catch (...) { + return boost::none; + } +} + +template <typename ValueType> +gtsam::Key Values::Add(const ValueType& value) { + // Since latest_key_ is always incremented when a new key is added, + // we don't need to worry about it already being in values_. + values_->insert(++latest_key_, value); + return latest_key_; +} + +template <class ARCHIVE> +void Values::serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(values_); + ar& BOOST_SERIALIZATION_NVP(latest_key_); +} +} // namespace nodes + +#endif // NODES_VALUES_H_ diff --git a/localization/nodes/package.xml b/localization/nodes/package.xml new file mode 100644 index 0000000000..9369400539 --- /dev/null +++ b/localization/nodes/package.xml @@ -0,0 +1,19 @@ +<package> + <name>nodes</name> + <version>1.0.0</version> + <description> + The nodes package + </description> + <license> + Apache License, Version 2.0 + </license> + <author email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </author> + <maintainer email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </maintainer> + <buildtool_depend>catkin</buildtool_depend> + <build_depend>localization_common</build_depend> + <run_depend>localization_common</run_depend> +</package> diff --git a/localization/nodes/readme.md b/localization/nodes/readme.md new file mode 100644 index 0000000000..41ffb94996 --- /dev/null +++ b/localization/nodes/readme.md @@ -0,0 +1,16 @@ +\page nodes Nodes + +# Package Overview +The nodes package provides classes for adding nodes to a graph for optimization. Nodes are equivalent to state parameters that should always be added/removed/updated at the same time. For example, when performing visual-intertial odometry, a single node may contain pose, velocity and IMU bias values. For a simple localization graph, a single node may contain just a pose. For timestamped nodes containing a single type (such as just a pose), use the TimestampedNodes class. For timestamped nodes containing multiple types (such as the pose, velocity bias node), use the TimestampedCombinedNodes class and override the AddNode and GetNode methods. + +## Values +Wrapper around GTSAM values class that enables adding and removing values given their key. + +## TimestampedNodes +Class that enables adding and removing simple nodes containing a single type given their timestamp. + +## TimestampedCombinedNodes +Class that enables adding and removing combined nodes containing multiple types given their timestamp. The AddNode and GetNode functions must be overridden, where the AddNode method should yield a new key vector containing a key for each type in the node, and the GetNode method should generate a combined type given a key vector. Note the ordering of the keys in the key vector generated by AddNode and provided by GetNode must match. See CombinedNavStateNodes for an example. + +## CombinedNavStateNodes +TimestampedCombinedNodes for a combined nav state that contains a pose, velocity, and IMU bias. diff --git a/localization/nodes/src/combined_nav_state_nodes.cc b/localization/nodes/src/combined_nav_state_nodes.cc new file mode 100644 index 0000000000..840c60a273 --- /dev/null +++ b/localization/nodes/src/combined_nav_state_nodes.cc @@ -0,0 +1,44 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <nodes/combined_nav_state_nodes.h> + +#include <gtsam/geometry/Pose3.h> + +namespace nodes { +namespace lc = localization_common; + +CombinedNavStateNodes::CombinedNavStateNodes(std::shared_ptr<Values> values) + : TimestampedCombinedNodes<lc::CombinedNavState>(values) {} + +gtsam::KeyVector CombinedNavStateNodes::AddNode(const lc::CombinedNavState& combined_nav_state) { + const auto key_pose = values_->Add(combined_nav_state.pose()); + const auto key_velocity = values_->Add(combined_nav_state.velocity()); + const auto key_bias = values_->Add(combined_nav_state.bias()); + return {key_pose, key_velocity, key_bias}; +} + +boost::optional<lc::CombinedNavState> CombinedNavStateNodes::GetNode(const gtsam::KeyVector& keys, + const lc::Time timestamp) const { + const auto pose = Base::Value<gtsam::Pose3>(keys[0]); + const auto velocity = Base::Value<gtsam::Velocity3>(keys[1]); + const auto bias = Base::Value<gtsam::imuBias::ConstantBias>(keys[2]); + if (!pose || !velocity || !bias) return boost::none; + return lc::CombinedNavState(*pose, *velocity, *bias, timestamp); +} +} // namespace nodes diff --git a/localization/graph_localizer/include/graph_localizer/handrail_factor_adder_params.h b/localization/nodes/src/values.cc similarity index 54% rename from localization/graph_localizer/include/graph_localizer/handrail_factor_adder_params.h rename to localization/nodes/src/values.cc index 4428c8d448..f40ad2594f 100644 --- a/localization/graph_localizer/include/graph_localizer/handrail_factor_adder_params.h +++ b/localization/nodes/src/values.cc @@ -15,23 +15,26 @@ * License for the specific language governing permissions and limitations * under the License. */ +#include <nodes/values.h> -#ifndef GRAPH_LOCALIZER_HANDRAIL_FACTOR_ADDER_PARAMS_H_ -#define GRAPH_LOCALIZER_HANDRAIL_FACTOR_ADDER_PARAMS_H_ +namespace nodes { +Values::Values(std::shared_ptr<gtsam::Values> values) : values_(std::move(values)), latest_key_(0) {} -#include <graph_optimizer/factor_adder_params.h> +bool Values::Contains(const gtsam::Key& key) const { return values_->exists(key); } -#include <gtsam/geometry/Pose3.h> +bool Values::Remove(const gtsam::Key& key) { + if (!Contains(key)) return false; + values_->erase(key); + return true; +} -namespace graph_localizer { -struct HandrailFactorAdderParams : public graph_optimizer::FactorAdderParams { - double min_num_line_matches; - double min_num_plane_matches; - double point_to_line_stddev; - double point_to_plane_stddev; - gtsam::Pose3 body_T_perch_cam; - bool use_silu_for_point_to_line_segment_factor; -}; -} // namespace graph_localizer +bool Values::Remove(const gtsam::KeyVector& keys) { + bool removed_value = false; + for (const auto& key : keys) { + removed_value = Remove(key) || removed_value; + } + return removed_value; +} -#endif // GRAPH_LOCALIZER_HANDRAIL_FACTOR_ADDER_PARAMS_H_ +size_t Values::size() const { return values_->size(); } +} // namespace nodes diff --git a/localization/nodes/test/test_timestamped_combined_nodes.cc b/localization/nodes/test/test_timestamped_combined_nodes.cc new file mode 100644 index 0000000000..c86f626d9f --- /dev/null +++ b/localization/nodes/test/test_timestamped_combined_nodes.cc @@ -0,0 +1,718 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <nodes/timestamped_combined_nodes.h> +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> + +#include <gtest/gtest.h> + +namespace lc = localization_common; +namespace no = nodes; + +struct CombinedObject { + CombinedObject(const double val_a, const double val_b) : val_a(val_a), val_b(val_b) {} + bool operator==(const CombinedObject& rhs) const { return val_a == rhs.val_a && val_b == rhs.val_b; } + static CombinedObject Random() { return CombinedObject(lc::RandomDouble(), lc::RandomDouble()); } + double val_a; + double val_b; +}; + +class TestCombinedNodes : public no::TimestampedCombinedNodes<CombinedObject> { + using Base = no::TimestampedCombinedNodes<CombinedObject>; + + public: + explicit TestCombinedNodes(std::shared_ptr<no::Values> values) : Base(values) {} + + // For serialization only + TestCombinedNodes() = default; + + private: + gtsam::KeyVector AddNode(const CombinedObject& node) final { + const auto key_a = values_->Add(node.val_a); + const auto key_b = values_->Add(node.val_b); + return {key_a, key_b}; + } + + boost::optional<CombinedObject> GetNode(const gtsam::KeyVector& keys, + const localization_common::Time timestamp) const final { + const auto val_a = values_->Value<double>(keys[0]); + const auto val_b = values_->Value<double>(keys[1]); + if (!val_a || !val_b) return boost::none; + return CombinedObject(*val_a, *val_b); + } + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; + +TEST(TimestampedNodesTester, AddRemoveContainsEmptySize) { + std::shared_ptr<no::Values> values(new no::Values()); + TestCombinedNodes timestamped_nodes(values); + EXPECT_EQ(timestamped_nodes.size(), 0); + EXPECT_TRUE(timestamped_nodes.empty()); + + // Add element 1 + const CombinedObject node_1(-2, 23.8); + const localization_common::Time timestamp_1 = 1.0; + EXPECT_EQ(timestamped_nodes.Add(timestamp_1, node_1).size(), 2); + EXPECT_EQ(timestamped_nodes.size(), 1); + EXPECT_FALSE(timestamped_nodes.empty()); + { + EXPECT_TRUE(timestamped_nodes.Node(2.0) == boost::none); + const auto accessed_node = timestamped_nodes.Node(timestamp_1); + ASSERT_TRUE(accessed_node != boost::none); + EXPECT_EQ(*accessed_node, node_1); + EXPECT_TRUE(timestamped_nodes.Contains(timestamp_1)); + } + // Add element 2 + const CombinedObject node_2(123.2, 22.7); + const localization_common::Time timestamp_2 = 3.3; + EXPECT_EQ(timestamped_nodes.Add(timestamp_2, node_2).size(), 2); + EXPECT_EQ(timestamped_nodes.size(), 2); + EXPECT_FALSE(timestamped_nodes.empty()); + { + EXPECT_TRUE(timestamped_nodes.Node(7.0) == boost::none); + const auto accessed_node_1 = timestamped_nodes.Node(timestamp_1); + ASSERT_TRUE(accessed_node_1 != boost::none); + EXPECT_EQ(*accessed_node_1, node_1); + EXPECT_TRUE(timestamped_nodes.Contains(timestamp_1)); + const auto accessed_node_2 = timestamped_nodes.Node(timestamp_2); + ASSERT_TRUE(accessed_node_2 != boost::none); + EXPECT_EQ(*accessed_node_2, node_2); + EXPECT_TRUE(timestamped_nodes.Contains(timestamp_2)); + } + + // Remove element 1 + EXPECT_TRUE(timestamped_nodes.Remove(timestamp_1)); + EXPECT_TRUE(timestamped_nodes.Node(timestamp_1) == boost::none); + EXPECT_FALSE(timestamped_nodes.Contains(timestamp_1)); + EXPECT_TRUE(timestamped_nodes.Node(timestamp_2) != boost::none); + EXPECT_TRUE(timestamped_nodes.Contains(timestamp_2)); + EXPECT_TRUE(timestamped_nodes.Node(timestamp_2) != boost::none); + EXPECT_EQ(timestamped_nodes.size(), 1); + EXPECT_FALSE(timestamped_nodes.empty()); + { + const auto good_val = timestamped_nodes.Node(timestamp_2); + ASSERT_TRUE(good_val != boost::none); + EXPECT_EQ(*good_val, node_2); + } + + // Bad Remove + EXPECT_FALSE(timestamped_nodes.Remove(timestamp_1)); + EXPECT_FALSE(timestamped_nodes.Remove(100)); + + // Remove element 2 + EXPECT_TRUE(timestamped_nodes.Remove(timestamp_2)); + EXPECT_TRUE(timestamped_nodes.Node(timestamp_1) == boost::none); + EXPECT_FALSE(timestamped_nodes.Contains(timestamp_1)); + EXPECT_TRUE(timestamped_nodes.Node(timestamp_2) == boost::none); + EXPECT_FALSE(timestamped_nodes.Contains(timestamp_2)); + EXPECT_EQ(timestamped_nodes.size(), 0); + EXPECT_TRUE(timestamped_nodes.empty()); + { + const auto bad_val = timestamped_nodes.Node(timestamp_2); + EXPECT_TRUE(bad_val == boost::none); + } +} +TEST(TimestampedNodesTester, OldestLatest) { + std::shared_ptr<no::Values> values(new no::Values()); + TestCombinedNodes timestamped_nodes(values); + // No elements + { + EXPECT_TRUE(timestamped_nodes.OldestTimestamp() == boost::none); + EXPECT_TRUE(timestamped_nodes.OldestNode() == boost::none); + EXPECT_TRUE(timestamped_nodes.LatestTimestamp() == boost::none); + EXPECT_TRUE(timestamped_nodes.LatestNode() == boost::none); + } + const CombinedObject node_1 = CombinedObject::Random(); + const localization_common::Time timestamp_1 = 1.0; + ASSERT_EQ(timestamped_nodes.Add(timestamp_1, node_1).size(), 2); + // 1 element + { + const auto oldest_timestamp = timestamped_nodes.OldestTimestamp(); + ASSERT_TRUE(oldest_timestamp != boost::none); + EXPECT_EQ(*oldest_timestamp, timestamp_1); + const auto latest_timestamp = timestamped_nodes.LatestTimestamp(); + ASSERT_TRUE(latest_timestamp != boost::none); + EXPECT_EQ(*latest_timestamp, timestamp_1); + + const auto oldest_node = timestamped_nodes.OldestNode(); + ASSERT_TRUE(oldest_node != boost::none); + EXPECT_EQ(*oldest_node, node_1); + const auto latest_node = timestamped_nodes.LatestNode(); + ASSERT_TRUE(latest_node != boost::none); + EXPECT_EQ(*latest_node, node_1); + } + // 2 elements + const CombinedObject node_2 = CombinedObject::Random(); + const localization_common::Time timestamp_2 = 3.3; + ASSERT_EQ(timestamped_nodes.Add(timestamp_2, node_2).size(), 2); + { + const auto oldest_timestamp = timestamped_nodes.OldestTimestamp(); + ASSERT_TRUE(oldest_timestamp != boost::none); + EXPECT_EQ(*oldest_timestamp, timestamp_1); + const auto latest_timestamp = timestamped_nodes.LatestTimestamp(); + ASSERT_TRUE(latest_timestamp != boost::none); + EXPECT_EQ(*latest_timestamp, timestamp_2); + + const auto oldest_node = timestamped_nodes.OldestNode(); + ASSERT_TRUE(oldest_node != boost::none); + EXPECT_EQ(*oldest_node, node_1); + const auto latest_node = timestamped_nodes.LatestNode(); + ASSERT_TRUE(latest_node != boost::none); + EXPECT_EQ(*latest_node, node_2); + } + + // 3 elements + const CombinedObject node_3 = CombinedObject::Random(); + const localization_common::Time timestamp_3 = 19.3; + ASSERT_EQ(timestamped_nodes.Add(timestamp_3, node_3).size(), 2); + { + const auto oldest_timestamp = timestamped_nodes.OldestTimestamp(); + ASSERT_TRUE(oldest_timestamp != boost::none); + EXPECT_EQ(*oldest_timestamp, timestamp_1); + const auto latest_timestamp = timestamped_nodes.LatestTimestamp(); + ASSERT_TRUE(latest_timestamp != boost::none); + EXPECT_EQ(*latest_timestamp, timestamp_3); + + const auto oldest_node = timestamped_nodes.OldestNode(); + ASSERT_TRUE(oldest_node != boost::none); + EXPECT_EQ(*oldest_node, node_1); + const auto latest_node = timestamped_nodes.LatestNode(); + ASSERT_TRUE(latest_node != boost::none); + EXPECT_EQ(*latest_node, node_3); + } + + ASSERT_TRUE(timestamped_nodes.Remove(timestamp_1)); + { + const auto oldest_timestamp = timestamped_nodes.OldestTimestamp(); + ASSERT_TRUE(oldest_timestamp != boost::none); + EXPECT_EQ(*oldest_timestamp, timestamp_2); + const auto latest_timestamp = timestamped_nodes.LatestTimestamp(); + ASSERT_TRUE(latest_timestamp != boost::none); + EXPECT_EQ(*latest_timestamp, timestamp_3); + + const auto oldest_node = timestamped_nodes.OldestNode(); + ASSERT_TRUE(oldest_node != boost::none); + EXPECT_EQ(*oldest_node, node_2); + const auto latest_node = timestamped_nodes.LatestNode(); + ASSERT_TRUE(latest_node != boost::none); + EXPECT_EQ(*latest_node, node_3); + } + + ASSERT_TRUE(timestamped_nodes.Remove(timestamp_3)); + { + const auto oldest_timestamp = timestamped_nodes.OldestTimestamp(); + ASSERT_TRUE(oldest_timestamp != boost::none); + EXPECT_EQ(*oldest_timestamp, timestamp_2); + const auto latest_timestamp = timestamped_nodes.LatestTimestamp(); + ASSERT_TRUE(latest_timestamp != boost::none); + EXPECT_EQ(*latest_timestamp, timestamp_2); + + const auto oldest_node = timestamped_nodes.OldestNode(); + ASSERT_TRUE(oldest_node != boost::none); + EXPECT_EQ(*oldest_node, node_2); + const auto latest_node = timestamped_nodes.LatestNode(); + ASSERT_TRUE(latest_node != boost::none); + EXPECT_EQ(*latest_node, node_2); + } + + ASSERT_TRUE(timestamped_nodes.Remove(timestamp_2)); + { + EXPECT_TRUE(timestamped_nodes.OldestTimestamp() == boost::none); + EXPECT_TRUE(timestamped_nodes.OldestNode() == boost::none); + EXPECT_TRUE(timestamped_nodes.LatestTimestamp() == boost::none); + EXPECT_TRUE(timestamped_nodes.LatestNode() == boost::none); + } +} + +TEST(TimestampedNodesTester, LowerAndUpperBounds) { + std::shared_ptr<no::Values> values(new no::Values()); + TestCombinedNodes timestamped_nodes(values); + // No elements + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(1.0); + EXPECT_TRUE(lower_and_upper_bound_timestamps.first == boost::none); + EXPECT_TRUE(lower_and_upper_bound_timestamps.second == boost::none); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(1.0); + EXPECT_TRUE(lower_and_upper_bound_nodes.first == boost::none); + EXPECT_TRUE(lower_and_upper_bound_nodes.second == boost::none); + } + + // 1 element + const CombinedObject node_1 = CombinedObject::Random(); + const localization_common::Time timestamp_1 = 37.0; + ASSERT_EQ(timestamped_nodes.Add(timestamp_1, node_1).size(), 2); + // 1 element below + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(10.0); + EXPECT_TRUE(lower_and_upper_bound_timestamps.first == boost::none); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_1); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(10.0); + EXPECT_TRUE(lower_and_upper_bound_nodes.first == boost::none); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_1); + } + // 1 element above + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(57.3); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_1); + EXPECT_TRUE(lower_and_upper_bound_timestamps.second == boost::none); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(57.3); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_1); + EXPECT_TRUE(lower_and_upper_bound_nodes.second == boost::none); + } + // 1 element equal + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(timestamp_1); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_1); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_1); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(timestamp_1); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_1); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_1); + } + + // 2 elements + const CombinedObject node_2 = CombinedObject::Random(); + const localization_common::Time timestamp_2 = 2.33; + ASSERT_EQ(timestamped_nodes.Add(timestamp_2, node_2).size(), 2); + // 2 elements below + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(1.1); + EXPECT_TRUE(lower_and_upper_bound_timestamps.first == boost::none); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_2); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(1.1); + EXPECT_TRUE(lower_and_upper_bound_nodes.first == boost::none); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_2); + } + // 2 elements above + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(111.3); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_1); + EXPECT_TRUE(lower_and_upper_bound_timestamps.second == boost::none); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(111.3); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_1); + EXPECT_TRUE(lower_and_upper_bound_nodes.second == boost::none); + } + // 2 elements equal lower + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(timestamp_2); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_2); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_2); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(timestamp_2); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_2); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_2); + } + // 2 elements equal upper + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(timestamp_1); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_1); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_1); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(timestamp_1); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_1); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_1); + } + // 2 elements between + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(15.1); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_2); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_1); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(15.1); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_2); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_1); + } + + // 3 elements + const CombinedObject node_3 = CombinedObject::Random(); + const localization_common::Time timestamp_3 = 14.1; + ASSERT_EQ(timestamped_nodes.Add(timestamp_3, node_3).size(), 2); + // 3 elements lower between + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(7.11); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_2); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_3); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(7.11); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_2); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_3); + } + // 3 elements upper between + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(22.22); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_3); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_1); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(22.22); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_3); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_1); + } +} + +TEST(TimestampedNodesTester, LowerBoundOrEqual) { + std::shared_ptr<no::Values> values(new no::Values()); + TestCombinedNodes timestamped_nodes(values); + const CombinedObject node_1 = CombinedObject::Random(); + const localization_common::Time timestamp_1 = 3.1; + ASSERT_EQ(timestamped_nodes.Add(timestamp_1, node_1).size(), 2); + const CombinedObject node_2 = CombinedObject::Random(); + const localization_common::Time timestamp_2 = 5.78; + ASSERT_EQ(timestamped_nodes.Add(timestamp_2, node_2).size(), 2); + const CombinedObject node_3 = CombinedObject::Random(); + const localization_common::Time timestamp_3 = 7.88; + ASSERT_EQ(timestamped_nodes.Add(timestamp_3, node_3).size(), 2); + + const auto too_low_node = timestamped_nodes.LowerBoundOrEqual(1.23); + EXPECT_TRUE(too_low_node == boost::none); + const auto lowest_node = timestamped_nodes.LowerBoundOrEqual(4.11); + ASSERT_TRUE(lowest_node != boost::none); + EXPECT_EQ(lowest_node->value, node_1); + EXPECT_EQ(lowest_node->timestamp, timestamp_1); + const auto middle_node = timestamped_nodes.LowerBoundOrEqual(6.61); + ASSERT_TRUE(middle_node != boost::none); + EXPECT_EQ(middle_node->value, node_2); + EXPECT_EQ(middle_node->timestamp, timestamp_2); + const auto upper_node = timestamped_nodes.LowerBoundOrEqual(900); + ASSERT_TRUE(upper_node != boost::none); + EXPECT_EQ(upper_node->value, node_3); + EXPECT_EQ(upper_node->timestamp, timestamp_3); + const auto equal_node = timestamped_nodes.LowerBoundOrEqual(timestamp_2); + ASSERT_TRUE(equal_node != boost::none); + EXPECT_EQ(equal_node->value, node_2); + EXPECT_EQ(equal_node->timestamp, timestamp_2); +} + +TEST(TimestampedNodesTester, Closest) { + std::shared_ptr<no::Values> values(new no::Values()); + TestCombinedNodes timestamped_nodes(values); + const CombinedObject node_1 = CombinedObject::Random(); + const localization_common::Time timestamp_1 = 3.1; + ASSERT_EQ(timestamped_nodes.Add(timestamp_1, node_1).size(), 2); + const CombinedObject node_2 = CombinedObject::Random(); + const localization_common::Time timestamp_2 = 5.78; + ASSERT_EQ(timestamped_nodes.Add(timestamp_2, node_2).size(), 2); + const CombinedObject node_3 = CombinedObject::Random(); + const localization_common::Time timestamp_3 = 7.88; + ASSERT_EQ(timestamped_nodes.Add(timestamp_3, node_3).size(), 2); + + const auto below_lowest_node = timestamped_nodes.ClosestNode(1.23); + EXPECT_TRUE(below_lowest_node != boost::none); + EXPECT_EQ(*below_lowest_node, node_1); + const auto above_lowest_node = timestamped_nodes.ClosestNode(4.11); + ASSERT_TRUE(above_lowest_node != boost::none); + EXPECT_EQ(*above_lowest_node, node_1); + const auto below_middle_node = timestamped_nodes.ClosestNode(5.61); + ASSERT_TRUE(below_middle_node != boost::none); + EXPECT_EQ(*below_middle_node, node_2); + const auto above_middle_node = timestamped_nodes.ClosestNode(6.61); + ASSERT_TRUE(above_middle_node != boost::none); + EXPECT_EQ(*above_middle_node, node_2); + const auto below_upper_node = timestamped_nodes.ClosestNode(7.61); + ASSERT_TRUE(below_upper_node != boost::none); + EXPECT_EQ(*below_upper_node, node_3); + const auto above_upper_node = timestamped_nodes.ClosestNode(8.61); + ASSERT_TRUE(above_upper_node != boost::none); + EXPECT_EQ(*above_upper_node, node_3); + const auto equal_node = timestamped_nodes.ClosestNode(timestamp_2); + ASSERT_TRUE(equal_node != boost::none); + EXPECT_EQ(*equal_node, node_2); +} + +TEST(TimestampedNodesTester, OldKeysTimestampsAndNodes) { + std::shared_ptr<no::Values> values(new no::Values()); + TestCombinedNodes timestamped_nodes(values); + const double t0 = 0; + const CombinedObject node_0 = CombinedObject::Random(); + const double t1 = 1.001; + const CombinedObject node_1 = CombinedObject::Random(); + const double t2 = 2.100; + const CombinedObject node_2 = CombinedObject::Random(); + const double t3 = 3.0222; + const CombinedObject node_3 = CombinedObject::Random(); + const int k0 = 1; + const int k1 = 2; + const int k2 = 3; + const int k3 = 4; + const int k4 = 5; + const int k5 = 6; + const int k6 = 7; + const int k7 = 8; + ASSERT_EQ(timestamped_nodes.Add(t0, node_0).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t1, node_1).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t2, node_2).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t3, node_3).size(), 2); + { + const auto old_keys = timestamped_nodes.OldKeys(0); + EXPECT_EQ(old_keys.size(), 0); + const auto old_nodes = timestamped_nodes.OldNodes(0); + EXPECT_EQ(old_nodes.size(), 0); + } + { + const auto old_keys = timestamped_nodes.OldKeys(0.1); + EXPECT_EQ(old_keys.size(), 2); + EXPECT_EQ(old_keys[0], k0); + EXPECT_EQ(old_keys[1], k1); + const auto old_nodes = timestamped_nodes.OldNodes(0.1); + ASSERT_EQ(old_nodes.size(), 1); + EXPECT_EQ(old_nodes[0], node_0); + } + { + const auto old_keys = timestamped_nodes.OldKeys(1.7); + EXPECT_EQ(old_keys.size(), 4); + EXPECT_EQ(old_keys[0], k0); + EXPECT_EQ(old_keys[1], k1); + EXPECT_EQ(old_keys[2], k2); + EXPECT_EQ(old_keys[3], k3); + const auto old_nodes = timestamped_nodes.OldNodes(1.7); + ASSERT_EQ(old_nodes.size(), 2); + EXPECT_EQ(old_nodes[0], node_0); + EXPECT_EQ(old_nodes[1], node_1); + } + { + const auto old_keys = timestamped_nodes.OldKeys(2.333); + EXPECT_EQ(old_keys.size(), 6); + EXPECT_EQ(old_keys[0], k0); + EXPECT_EQ(old_keys[1], k1); + EXPECT_EQ(old_keys[2], k2); + EXPECT_EQ(old_keys[3], k3); + EXPECT_EQ(old_keys[4], k4); + EXPECT_EQ(old_keys[5], k5); + const auto old_nodes = timestamped_nodes.OldNodes(2.333); + ASSERT_EQ(old_nodes.size(), 3); + EXPECT_EQ(old_nodes[0], node_0); + EXPECT_EQ(old_nodes[1], node_1); + EXPECT_EQ(old_nodes[2], node_2); + } + { + const auto old_keys = timestamped_nodes.OldKeys(1999); + EXPECT_EQ(old_keys.size(), 8); + EXPECT_EQ(old_keys[0], k0); + EXPECT_EQ(old_keys[1], k1); + EXPECT_EQ(old_keys[2], k2); + EXPECT_EQ(old_keys[3], k3); + EXPECT_EQ(old_keys[4], k4); + EXPECT_EQ(old_keys[5], k5); + EXPECT_EQ(old_keys[6], k6); + EXPECT_EQ(old_keys[7], k7); + const auto old_nodes = timestamped_nodes.OldNodes(1999); + ASSERT_EQ(old_nodes.size(), 4); + EXPECT_EQ(old_nodes[0], node_0); + EXPECT_EQ(old_nodes[1], node_1); + EXPECT_EQ(old_nodes[2], node_2); + EXPECT_EQ(old_nodes[3], node_3); + } +} + +TEST(TimestampedNodesTester, RemoveOldNodes) { + { + std::shared_ptr<no::Values> values(new no::Values()); + TestCombinedNodes timestamped_nodes(values); + const double t0 = 0; + const CombinedObject node_0 = CombinedObject::Random(); + const double t1 = 1.001; + const CombinedObject node_1 = CombinedObject::Random(); + const double t2 = 2.100; + const CombinedObject node_2 = CombinedObject::Random(); + const double t3 = 3.0222; + const CombinedObject node_3 = CombinedObject::Random(); + ASSERT_EQ(timestamped_nodes.Add(t0, node_0).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t1, node_1).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t2, node_2).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t3, node_3).size(), 2); + const int num_nodes_removed = timestamped_nodes.RemoveOldNodes(0); + EXPECT_EQ(num_nodes_removed, 0); + EXPECT_EQ(timestamped_nodes.size(), 4); + } + + { + std::shared_ptr<no::Values> values(new no::Values()); + TestCombinedNodes timestamped_nodes(values); + const double t0 = 0; + const CombinedObject node_0 = CombinedObject::Random(); + const double t1 = 1.001; + const CombinedObject node_1 = CombinedObject::Random(); + const double t2 = 2.100; + const CombinedObject node_2 = CombinedObject::Random(); + const double t3 = 3.0222; + const CombinedObject node_3 = CombinedObject::Random(); + ASSERT_EQ(timestamped_nodes.Add(t0, node_0).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t1, node_1).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t2, node_2).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t3, node_3).size(), 2); + const int num_nodes_removed = timestamped_nodes.RemoveOldNodes(0.1); + EXPECT_EQ(num_nodes_removed, 1); + EXPECT_EQ(timestamped_nodes.size(), 3); + const auto timestamps = timestamped_nodes.Timestamps(); + EXPECT_EQ(timestamps[0], t1); + EXPECT_EQ(timestamps[1], t2); + EXPECT_EQ(timestamps[2], t3); + } + { + std::shared_ptr<no::Values> values(new no::Values()); + TestCombinedNodes timestamped_nodes(values); + const double t0 = 0; + const CombinedObject node_0 = CombinedObject::Random(); + const double t1 = 1.001; + const CombinedObject node_1 = CombinedObject::Random(); + const double t2 = 2.100; + const CombinedObject node_2 = CombinedObject::Random(); + const double t3 = 3.0222; + const CombinedObject node_3 = CombinedObject::Random(); + ASSERT_EQ(timestamped_nodes.Add(t0, node_0).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t1, node_1).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t2, node_2).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t3, node_3).size(), 2); + const int num_nodes_removed = timestamped_nodes.RemoveOldNodes(1.334); + EXPECT_EQ(num_nodes_removed, 2); + EXPECT_EQ(timestamped_nodes.size(), 2); + const auto timestamps = timestamped_nodes.Timestamps(); + EXPECT_EQ(timestamps[0], t2); + EXPECT_EQ(timestamps[1], t3); + } + + { + std::shared_ptr<no::Values> values(new no::Values()); + TestCombinedNodes timestamped_nodes(values); + const double t0 = 0; + const CombinedObject node_0 = CombinedObject::Random(); + const double t1 = 1.001; + const CombinedObject node_1 = CombinedObject::Random(); + const double t2 = 2.100; + const CombinedObject node_2 = CombinedObject::Random(); + const double t3 = 3.0222; + const CombinedObject node_3 = CombinedObject::Random(); + ASSERT_EQ(timestamped_nodes.Add(t0, node_0).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t1, node_1).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t2, node_2).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t3, node_3).size(), 2); + const int num_nodes_removed = timestamped_nodes.RemoveOldNodes(2.78); + EXPECT_EQ(num_nodes_removed, 3); + EXPECT_EQ(timestamped_nodes.size(), 1); + const auto timestamps = timestamped_nodes.Timestamps(); + EXPECT_EQ(timestamps[0], t3); + } + + { + std::shared_ptr<no::Values> values(new no::Values()); + TestCombinedNodes timestamped_nodes(values); + const double t0 = 0; + const CombinedObject node_0 = CombinedObject::Random(); + const double t1 = 1.001; + const CombinedObject node_1 = CombinedObject::Random(); + const double t2 = 2.100; + const CombinedObject node_2 = CombinedObject::Random(); + const double t3 = 3.0222; + const CombinedObject node_3 = CombinedObject::Random(); + ASSERT_EQ(timestamped_nodes.Add(t0, node_0).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t1, node_1).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t2, node_2).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t3, node_3).size(), 2); + const int num_nodes_removed = timestamped_nodes.RemoveOldNodes(1923.78); + EXPECT_EQ(num_nodes_removed, 4); + EXPECT_EQ(timestamped_nodes.size(), 0); + } +} + +TEST(TimestampedNodesTester, Duration) { + std::shared_ptr<no::Values> values(new no::Values()); + TestCombinedNodes timestamped_nodes(values); + const CombinedObject node_1 = CombinedObject::Random(); + const CombinedObject node_2 = CombinedObject::Random(); + const CombinedObject node_3 = CombinedObject::Random(); + EXPECT_EQ(timestamped_nodes.Duration(), 0); + ASSERT_EQ(timestamped_nodes.Add(1.0, node_1).size(), 2); + EXPECT_EQ(timestamped_nodes.Duration(), 0); + ASSERT_EQ(timestamped_nodes.Add(2.0, node_2).size(), 2); + EXPECT_NEAR(timestamped_nodes.Duration(), 1, 1e-6); + ASSERT_EQ(timestamped_nodes.Add(3.0, node_3).size(), 2); + EXPECT_NEAR(timestamped_nodes.Duration(), 2, 1e-6); +} + +TEST(TimestampedNodesTester, Timestamps) { + std::shared_ptr<no::Values> values(new no::Values()); + TestCombinedNodes timestamped_nodes(values); + { + const auto timestamps = timestamped_nodes.Timestamps(); + EXPECT_EQ(timestamps.size(), 0); + } + const double t0 = 0; + const double t1 = 1; + const double t2 = 2; + const double t3 = 3; + const CombinedObject node_0 = CombinedObject::Random(); + const CombinedObject node_1 = CombinedObject::Random(); + const CombinedObject node_2 = CombinedObject::Random(); + const CombinedObject node_3 = CombinedObject::Random(); + ASSERT_EQ(timestamped_nodes.Add(t0, node_0).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t1, node_1).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t2, node_2).size(), 2); + ASSERT_EQ(timestamped_nodes.Add(t3, node_3).size(), 2); + { + const auto timestamps = timestamped_nodes.Timestamps(); + EXPECT_EQ(timestamps[0], t0); + EXPECT_EQ(timestamps[1], t1); + EXPECT_EQ(timestamps[2], t2); + EXPECT_EQ(timestamps[3], t3); + } +} + +TEST(TimestampedNodesTester, Serialization) { + const TestCombinedNodes nodes; + const auto serialized_nodes = gtsam::serializeBinary(nodes); + TestCombinedNodes deserialized_nodes; + gtsam::deserializeBinary(serialized_nodes, deserialized_nodes); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/nodes/test/test_timestamped_combined_nodes.test b/localization/nodes/test/test_timestamped_combined_nodes.test new file mode 100644 index 0000000000..767995d5ef --- /dev/null +++ b/localization/nodes/test/test_timestamped_combined_nodes.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="nodes" type="test_timestamped_combined_nodes" test-name="test_timestamped_combined_nodes" /> +</launch> diff --git a/localization/nodes/test/test_timestamped_nodes.cc b/localization/nodes/test/test_timestamped_nodes.cc new file mode 100644 index 0000000000..81975041f1 --- /dev/null +++ b/localization/nodes/test/test_timestamped_nodes.cc @@ -0,0 +1,659 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <nodes/timestamped_nodes.h> +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> + +#include <gtest/gtest.h> + +namespace lc = localization_common; +namespace no = nodes; + +TEST(TimestampedNodesTester, AddRemoveContainsEmptySize) { + std::shared_ptr<no::Values> values(new no::Values()); + no::TimestampedNodes<double> timestamped_nodes(values); + EXPECT_EQ(timestamped_nodes.size(), 0); + EXPECT_TRUE(timestamped_nodes.empty()); + + // Add element 1 + const double node_1 = 100.3; + const localization_common::Time timestamp_1 = 1.0; + EXPECT_EQ(timestamped_nodes.Add(timestamp_1, node_1).size(), 1); + EXPECT_EQ(timestamped_nodes.size(), 1); + EXPECT_FALSE(timestamped_nodes.empty()); + { + EXPECT_TRUE(timestamped_nodes.Node(2.0) == boost::none); + const auto accessed_node = timestamped_nodes.Node(timestamp_1); + ASSERT_TRUE(accessed_node != boost::none); + EXPECT_EQ(*accessed_node, node_1); + EXPECT_TRUE(timestamped_nodes.Contains(timestamp_1)); + } + + // Add element 2 + const double node_2 = 100.3; + const localization_common::Time timestamp_2 = 3.3; + EXPECT_EQ(timestamped_nodes.Add(timestamp_2, node_2).size(), 1); + EXPECT_EQ(timestamped_nodes.size(), 2); + EXPECT_FALSE(timestamped_nodes.empty()); + { + EXPECT_TRUE(timestamped_nodes.Node(7.0) == boost::none); + const auto accessed_node_1 = timestamped_nodes.Node(timestamp_1); + ASSERT_TRUE(accessed_node_1 != boost::none); + EXPECT_EQ(*accessed_node_1, node_1); + EXPECT_TRUE(timestamped_nodes.Contains(timestamp_1)); + const auto accessed_node_2 = timestamped_nodes.Node(timestamp_2); + ASSERT_TRUE(accessed_node_2 != boost::none); + EXPECT_EQ(*accessed_node_2, node_2); + EXPECT_TRUE(timestamped_nodes.Contains(timestamp_2)); + } + + // Remove element 1 + EXPECT_TRUE(timestamped_nodes.Remove(timestamp_1)); + EXPECT_TRUE(timestamped_nodes.Node(timestamp_1) == boost::none); + EXPECT_FALSE(timestamped_nodes.Contains(timestamp_1)); + EXPECT_TRUE(timestamped_nodes.Node(timestamp_2) != boost::none); + EXPECT_TRUE(timestamped_nodes.Contains(timestamp_2)); + EXPECT_TRUE(timestamped_nodes.Node(timestamp_2) != boost::none); + EXPECT_EQ(timestamped_nodes.size(), 1); + EXPECT_FALSE(timestamped_nodes.empty()); + { + const auto good_val = timestamped_nodes.Node(timestamp_2); + ASSERT_TRUE(good_val != boost::none); + EXPECT_EQ(*good_val, node_2); + } + + // Bad Remove + EXPECT_FALSE(timestamped_nodes.Remove(timestamp_1)); + EXPECT_FALSE(timestamped_nodes.Remove(100)); + + // Remove element 2 + EXPECT_TRUE(timestamped_nodes.Remove(timestamp_2)); + EXPECT_TRUE(timestamped_nodes.Node(timestamp_1) == boost::none); + EXPECT_FALSE(timestamped_nodes.Contains(timestamp_1)); + EXPECT_TRUE(timestamped_nodes.Node(timestamp_2) == boost::none); + EXPECT_FALSE(timestamped_nodes.Contains(timestamp_2)); + EXPECT_EQ(timestamped_nodes.size(), 0); + EXPECT_TRUE(timestamped_nodes.empty()); + { + const auto bad_val = timestamped_nodes.Node(timestamp_2); + EXPECT_TRUE(bad_val == boost::none); + } +} + +TEST(TimestampedNodesTester, OldestLatest) { + std::shared_ptr<no::Values> values(new no::Values()); + no::TimestampedNodes<double> timestamped_nodes(values); + // No elements + { + EXPECT_TRUE(timestamped_nodes.OldestTimestamp() == boost::none); + EXPECT_TRUE(timestamped_nodes.OldestNode() == boost::none); + EXPECT_TRUE(timestamped_nodes.LatestTimestamp() == boost::none); + EXPECT_TRUE(timestamped_nodes.LatestNode() == boost::none); + } + const double node_1 = 101.0; + const localization_common::Time timestamp_1 = 1.0; + ASSERT_EQ(timestamped_nodes.Add(timestamp_1, node_1).size(), 1); + // 1 element + { + const auto oldest_timestamp = timestamped_nodes.OldestTimestamp(); + ASSERT_TRUE(oldest_timestamp != boost::none); + EXPECT_EQ(*oldest_timestamp, timestamp_1); + const auto latest_timestamp = timestamped_nodes.LatestTimestamp(); + ASSERT_TRUE(latest_timestamp != boost::none); + EXPECT_EQ(*latest_timestamp, timestamp_1); + + const auto oldest_node = timestamped_nodes.OldestNode(); + ASSERT_TRUE(oldest_node != boost::none); + EXPECT_EQ(*oldest_node, node_1); + const auto latest_node = timestamped_nodes.LatestNode(); + ASSERT_TRUE(latest_node != boost::none); + EXPECT_EQ(*latest_node, node_1); + } + // 2 elements + const double node_2 = 100.3; + const localization_common::Time timestamp_2 = 3.3; + ASSERT_EQ(timestamped_nodes.Add(timestamp_2, node_2).size(), 1); + { + const auto oldest_timestamp = timestamped_nodes.OldestTimestamp(); + ASSERT_TRUE(oldest_timestamp != boost::none); + EXPECT_EQ(*oldest_timestamp, timestamp_1); + const auto latest_timestamp = timestamped_nodes.LatestTimestamp(); + ASSERT_TRUE(latest_timestamp != boost::none); + EXPECT_EQ(*latest_timestamp, timestamp_2); + + const auto oldest_node = timestamped_nodes.OldestNode(); + ASSERT_TRUE(oldest_node != boost::none); + EXPECT_EQ(*oldest_node, node_1); + const auto latest_node = timestamped_nodes.LatestNode(); + ASSERT_TRUE(latest_node != boost::none); + EXPECT_EQ(*latest_node, node_2); + } + + // 3 elements + const double node_3 = 2100.3; + const localization_common::Time timestamp_3 = 19.3; + ASSERT_EQ(timestamped_nodes.Add(timestamp_3, node_3).size(), 1); + { + const auto oldest_timestamp = timestamped_nodes.OldestTimestamp(); + ASSERT_TRUE(oldest_timestamp != boost::none); + EXPECT_EQ(*oldest_timestamp, timestamp_1); + const auto latest_timestamp = timestamped_nodes.LatestTimestamp(); + ASSERT_TRUE(latest_timestamp != boost::none); + EXPECT_EQ(*latest_timestamp, timestamp_3); + + const auto oldest_node = timestamped_nodes.OldestNode(); + ASSERT_TRUE(oldest_node != boost::none); + EXPECT_EQ(*oldest_node, node_1); + const auto latest_node = timestamped_nodes.LatestNode(); + ASSERT_TRUE(latest_node != boost::none); + EXPECT_EQ(*latest_node, node_3); + } + + ASSERT_TRUE(timestamped_nodes.Remove(timestamp_1)); + { + const auto oldest_timestamp = timestamped_nodes.OldestTimestamp(); + ASSERT_TRUE(oldest_timestamp != boost::none); + EXPECT_EQ(*oldest_timestamp, timestamp_2); + const auto latest_timestamp = timestamped_nodes.LatestTimestamp(); + ASSERT_TRUE(latest_timestamp != boost::none); + EXPECT_EQ(*latest_timestamp, timestamp_3); + + const auto oldest_node = timestamped_nodes.OldestNode(); + ASSERT_TRUE(oldest_node != boost::none); + EXPECT_EQ(*oldest_node, node_2); + const auto latest_node = timestamped_nodes.LatestNode(); + ASSERT_TRUE(latest_node != boost::none); + EXPECT_EQ(*latest_node, node_3); + } + + ASSERT_TRUE(timestamped_nodes.Remove(timestamp_3)); + { + const auto oldest_timestamp = timestamped_nodes.OldestTimestamp(); + ASSERT_TRUE(oldest_timestamp != boost::none); + EXPECT_EQ(*oldest_timestamp, timestamp_2); + const auto latest_timestamp = timestamped_nodes.LatestTimestamp(); + ASSERT_TRUE(latest_timestamp != boost::none); + EXPECT_EQ(*latest_timestamp, timestamp_2); + + const auto oldest_node = timestamped_nodes.OldestNode(); + ASSERT_TRUE(oldest_node != boost::none); + EXPECT_EQ(*oldest_node, node_2); + const auto latest_node = timestamped_nodes.LatestNode(); + ASSERT_TRUE(latest_node != boost::none); + EXPECT_EQ(*latest_node, node_2); + } + + ASSERT_TRUE(timestamped_nodes.Remove(timestamp_2)); + { + EXPECT_TRUE(timestamped_nodes.OldestTimestamp() == boost::none); + EXPECT_TRUE(timestamped_nodes.OldestNode() == boost::none); + EXPECT_TRUE(timestamped_nodes.LatestTimestamp() == boost::none); + EXPECT_TRUE(timestamped_nodes.LatestNode() == boost::none); + } +} + +TEST(TimestampedNodesTester, LowerAndUpperBounds) { + std::shared_ptr<no::Values> values(new no::Values()); + no::TimestampedNodes<double> timestamped_nodes(values); + // No elements + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(1.0); + EXPECT_TRUE(lower_and_upper_bound_timestamps.first == boost::none); + EXPECT_TRUE(lower_and_upper_bound_timestamps.second == boost::none); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(1.0); + EXPECT_TRUE(lower_and_upper_bound_nodes.first == boost::none); + EXPECT_TRUE(lower_and_upper_bound_nodes.second == boost::none); + } + + // 1 element + const double node_1 = -77.0; + const localization_common::Time timestamp_1 = 37.0; + ASSERT_EQ(timestamped_nodes.Add(timestamp_1, node_1).size(), 1); + // 1 element below + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(10.0); + EXPECT_TRUE(lower_and_upper_bound_timestamps.first == boost::none); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_1); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(10.0); + EXPECT_TRUE(lower_and_upper_bound_nodes.first == boost::none); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_1); + } + // 1 element above + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(57.3); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_1); + EXPECT_TRUE(lower_and_upper_bound_timestamps.second == boost::none); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(57.3); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_1); + EXPECT_TRUE(lower_and_upper_bound_nodes.second == boost::none); + } + // 1 element equal + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(timestamp_1); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_1); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_1); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(timestamp_1); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_1); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_1); + } + + // 2 elements + const double node_2 = 512.0; + const localization_common::Time timestamp_2 = 2.33; + ASSERT_EQ(timestamped_nodes.Add(timestamp_2, node_2).size(), 1); + // 2 elements below + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(1.1); + EXPECT_TRUE(lower_and_upper_bound_timestamps.first == boost::none); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_2); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(1.1); + EXPECT_TRUE(lower_and_upper_bound_nodes.first == boost::none); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_2); + } + // 2 elements above + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(111.3); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_1); + EXPECT_TRUE(lower_and_upper_bound_timestamps.second == boost::none); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(111.3); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_1); + EXPECT_TRUE(lower_and_upper_bound_nodes.second == boost::none); + } + // 2 elements equal lower + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(timestamp_2); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_2); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_2); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(timestamp_2); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_2); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_2); + } + // 2 elements equal upper + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(timestamp_1); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_1); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_1); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(timestamp_1); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_1); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_1); + } + // 2 elements between + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(15.1); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_2); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_1); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(15.1); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_2); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_1); + } + + // 3 elements + const double node_3 = 291.1; + const localization_common::Time timestamp_3 = 14.1; + ASSERT_EQ(timestamped_nodes.Add(timestamp_3, node_3).size(), 1); + // 3 elements lower between + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(7.11); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_2); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_3); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(7.11); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_2); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_3); + } + // 3 elements upper between + { + const auto lower_and_upper_bound_timestamps = timestamped_nodes.LowerAndUpperBoundTimestamps(22.22); + ASSERT_TRUE(lower_and_upper_bound_timestamps.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.first), timestamp_3); + ASSERT_TRUE(lower_and_upper_bound_timestamps.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_timestamps.second), timestamp_1); + const auto lower_and_upper_bound_nodes = timestamped_nodes.LowerAndUpperBoundNodes(22.22); + ASSERT_TRUE(lower_and_upper_bound_nodes.first != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.first), node_3); + ASSERT_TRUE(lower_and_upper_bound_nodes.second != boost::none); + EXPECT_EQ(*(lower_and_upper_bound_nodes.second), node_1); + } +} + +TEST(TimestampedNodesTester, LowerBoundOrEqual) { + std::shared_ptr<no::Values> values(new no::Values()); + no::TimestampedNodes<double> timestamped_nodes(values); + const double node_1 = 1.23; + const localization_common::Time timestamp_1 = 3.1; + ASSERT_EQ(timestamped_nodes.Add(timestamp_1, node_1).size(), 1); + const double node_2 = 2.22; + const localization_common::Time timestamp_2 = 5.78; + ASSERT_EQ(timestamped_nodes.Add(timestamp_2, node_2).size(), 1); + const double node_3 = 3.98; + const localization_common::Time timestamp_3 = 7.88; + ASSERT_EQ(timestamped_nodes.Add(timestamp_3, node_3).size(), 1); + + const auto too_low_node = timestamped_nodes.LowerBoundOrEqual(1.23); + EXPECT_TRUE(too_low_node == boost::none); + const auto lowest_node = timestamped_nodes.LowerBoundOrEqual(4.11); + ASSERT_TRUE(lowest_node != boost::none); + EXPECT_EQ(lowest_node->value, node_1); + EXPECT_EQ(lowest_node->timestamp, timestamp_1); + const auto middle_node = timestamped_nodes.LowerBoundOrEqual(6.61); + ASSERT_TRUE(middle_node != boost::none); + EXPECT_EQ(middle_node->value, node_2); + EXPECT_EQ(middle_node->timestamp, timestamp_2); + const auto upper_node = timestamped_nodes.LowerBoundOrEqual(900); + ASSERT_TRUE(upper_node != boost::none); + EXPECT_EQ(upper_node->value, node_3); + EXPECT_EQ(upper_node->timestamp, timestamp_3); + const auto equal_node = timestamped_nodes.LowerBoundOrEqual(timestamp_2); + ASSERT_TRUE(equal_node != boost::none); + EXPECT_EQ(equal_node->value, node_2); + EXPECT_EQ(equal_node->timestamp, timestamp_2); +} + +TEST(TimestampedNodesTester, Closest) { + std::shared_ptr<no::Values> values(new no::Values()); + no::TimestampedNodes<double> timestamped_nodes(values); + const double node_1 = 1.23; + const localization_common::Time timestamp_1 = 3.1; + ASSERT_EQ(timestamped_nodes.Add(timestamp_1, node_1).size(), 1); + const double node_2 = 2.22; + const localization_common::Time timestamp_2 = 5.78; + ASSERT_EQ(timestamped_nodes.Add(timestamp_2, node_2).size(), 1); + const double node_3 = 3.98; + const localization_common::Time timestamp_3 = 7.88; + ASSERT_EQ(timestamped_nodes.Add(timestamp_3, node_3).size(), 1); + + const auto below_lowest_node = timestamped_nodes.ClosestNode(1.23); + EXPECT_TRUE(below_lowest_node != boost::none); + EXPECT_EQ(*below_lowest_node, node_1); + const auto above_lowest_node = timestamped_nodes.ClosestNode(4.11); + ASSERT_TRUE(above_lowest_node != boost::none); + EXPECT_EQ(*above_lowest_node, node_1); + const auto below_middle_node = timestamped_nodes.ClosestNode(5.61); + ASSERT_TRUE(below_middle_node != boost::none); + EXPECT_EQ(*below_middle_node, node_2); + const auto above_middle_node = timestamped_nodes.ClosestNode(6.61); + ASSERT_TRUE(above_middle_node != boost::none); + EXPECT_EQ(*above_middle_node, node_2); + const auto below_upper_node = timestamped_nodes.ClosestNode(7.61); + ASSERT_TRUE(below_upper_node != boost::none); + EXPECT_EQ(*below_upper_node, node_3); + const auto above_upper_node = timestamped_nodes.ClosestNode(8.61); + ASSERT_TRUE(above_upper_node != boost::none); + EXPECT_EQ(*above_upper_node, node_3); + const auto equal_node = timestamped_nodes.ClosestNode(timestamp_2); + ASSERT_TRUE(equal_node != boost::none); + EXPECT_EQ(*equal_node, node_2); +} + +TEST(TimestampedNodesTester, OldKeysTimestampsAndNodes) { + std::shared_ptr<no::Values> values(new no::Values()); + no::TimestampedNodes<double> timestamped_nodes(values); + const double t0 = 0; + const double n0 = lc::RandomDouble(); + const int k0 = 1; + const double t1 = 1.001; + const double n1 = lc::RandomDouble(); + const int k1 = 2; + const double t2 = 2.100; + const double n2 = lc::RandomDouble(); + const int k2 = 3; + const double t3 = 3.0222; + const double n3 = lc::RandomDouble(); + const int k3 = 4; + ASSERT_EQ(timestamped_nodes.Add(t0, n0).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t1, n1).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t2, n2).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t3, n3).size(), 1); + { + const auto old_keys = timestamped_nodes.OldKeys(0); + EXPECT_EQ(old_keys.size(), 0); + const auto old_nodes = timestamped_nodes.OldNodes(0); + EXPECT_EQ(old_nodes.size(), 0); + } + { + const auto old_keys = timestamped_nodes.OldKeys(0.1); + EXPECT_EQ(old_keys.size(), 1); + EXPECT_EQ(old_keys[0], k0); + const auto old_nodes = timestamped_nodes.OldNodes(0.1); + ASSERT_EQ(old_nodes.size(), 1); + EXPECT_EQ(old_nodes[0], n0); + } + { + const auto old_keys = timestamped_nodes.OldKeys(1.7); + EXPECT_EQ(old_keys.size(), 2); + EXPECT_EQ(old_keys[0], k0); + EXPECT_EQ(old_keys[1], k1); + const auto old_nodes = timestamped_nodes.OldNodes(1.7); + ASSERT_EQ(old_nodes.size(), 2); + EXPECT_EQ(old_nodes[0], n0); + EXPECT_EQ(old_nodes[1], n1); + } + { + const auto old_keys = timestamped_nodes.OldKeys(2.333); + EXPECT_EQ(old_keys.size(), 3); + EXPECT_EQ(old_keys[0], k0); + EXPECT_EQ(old_keys[1], k1); + EXPECT_EQ(old_keys[2], k2); + const auto old_nodes = timestamped_nodes.OldNodes(2.333); + ASSERT_EQ(old_nodes.size(), 3); + EXPECT_EQ(old_nodes[0], n0); + EXPECT_EQ(old_nodes[1], n1); + EXPECT_EQ(old_nodes[2], n2); + } + { + const auto old_keys = timestamped_nodes.OldKeys(1999); + EXPECT_EQ(old_keys.size(), 4); + EXPECT_EQ(old_keys[0], k0); + EXPECT_EQ(old_keys[1], k1); + EXPECT_EQ(old_keys[2], k2); + EXPECT_EQ(old_keys[3], k3); + const auto old_nodes = timestamped_nodes.OldNodes(1999); + ASSERT_EQ(old_nodes.size(), 4); + EXPECT_EQ(old_nodes[0], n0); + EXPECT_EQ(old_nodes[1], n1); + EXPECT_EQ(old_nodes[2], n2); + EXPECT_EQ(old_nodes[3], n3); + } +} + +TEST(TimestampedNodesTester, RemoveOldNodes) { + { + std::shared_ptr<no::Values> values(new no::Values()); + no::TimestampedNodes<double> timestamped_nodes(values); + const double t0 = 0; + const double n0 = lc::RandomDouble(); + const double t1 = 1.001; + const double n1 = lc::RandomDouble(); + const double t2 = 2.100; + const double n2 = lc::RandomDouble(); + const double t3 = 3.0222; + const double n3 = lc::RandomDouble(); + ASSERT_EQ(timestamped_nodes.Add(t0, n0).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t1, n1).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t2, n2).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t3, n3).size(), 1); + const int num_nodes_removed = timestamped_nodes.RemoveOldNodes(0); + EXPECT_EQ(num_nodes_removed, 0); + EXPECT_EQ(timestamped_nodes.size(), 4); + } + + { + std::shared_ptr<no::Values> values(new no::Values()); + no::TimestampedNodes<double> timestamped_nodes(values); + const double t0 = 0; + const double n0 = lc::RandomDouble(); + const double t1 = 1.001; + const double n1 = lc::RandomDouble(); + const double t2 = 2.100; + const double n2 = lc::RandomDouble(); + const double t3 = 3.0222; + const double n3 = lc::RandomDouble(); + ASSERT_EQ(timestamped_nodes.Add(t0, n0).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t1, n1).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t2, n2).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t3, n3).size(), 1); + const int num_nodes_removed = timestamped_nodes.RemoveOldNodes(0.1); + EXPECT_EQ(num_nodes_removed, 1); + EXPECT_EQ(timestamped_nodes.size(), 3); + const auto timestamps = timestamped_nodes.Timestamps(); + EXPECT_EQ(timestamps[0], t1); + EXPECT_EQ(timestamps[1], t2); + EXPECT_EQ(timestamps[2], t3); + } + { + std::shared_ptr<no::Values> values(new no::Values()); + no::TimestampedNodes<double> timestamped_nodes(values); + const double t0 = 0; + const double n0 = lc::RandomDouble(); + const double t1 = 1.001; + const double n1 = lc::RandomDouble(); + const double t2 = 2.100; + const double n2 = lc::RandomDouble(); + const double t3 = 3.0222; + const double n3 = lc::RandomDouble(); + ASSERT_EQ(timestamped_nodes.Add(t0, n0).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t1, n1).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t2, n2).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t3, n3).size(), 1); + const int num_nodes_removed = timestamped_nodes.RemoveOldNodes(1.334); + EXPECT_EQ(num_nodes_removed, 2); + EXPECT_EQ(timestamped_nodes.size(), 2); + const auto timestamps = timestamped_nodes.Timestamps(); + EXPECT_EQ(timestamps[0], t2); + EXPECT_EQ(timestamps[1], t3); + } + + { + std::shared_ptr<no::Values> values(new no::Values()); + no::TimestampedNodes<double> timestamped_nodes(values); + const double t0 = 0; + const double n0 = lc::RandomDouble(); + const double t1 = 1.001; + const double n1 = lc::RandomDouble(); + const double t2 = 2.100; + const double n2 = lc::RandomDouble(); + const double t3 = 3.0222; + const double n3 = lc::RandomDouble(); + ASSERT_EQ(timestamped_nodes.Add(t0, n0).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t1, n1).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t2, n2).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t3, n3).size(), 1); + const int num_nodes_removed = timestamped_nodes.RemoveOldNodes(2.78); + EXPECT_EQ(num_nodes_removed, 3); + EXPECT_EQ(timestamped_nodes.size(), 1); + const auto timestamps = timestamped_nodes.Timestamps(); + EXPECT_EQ(timestamps[0], t3); + } + + { + std::shared_ptr<no::Values> values(new no::Values()); + no::TimestampedNodes<double> timestamped_nodes(values); + const double t0 = 0; + const double n0 = lc::RandomDouble(); + const double t1 = 1.001; + const double n1 = lc::RandomDouble(); + const double t2 = 2.100; + const double n2 = lc::RandomDouble(); + const double t3 = 3.0222; + const double n3 = lc::RandomDouble(); + ASSERT_EQ(timestamped_nodes.Add(t0, n0).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t1, n1).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t2, n2).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t3, n3).size(), 1); + const int num_nodes_removed = timestamped_nodes.RemoveOldNodes(1923.78); + EXPECT_EQ(num_nodes_removed, 4); + EXPECT_EQ(timestamped_nodes.size(), 0); + } +} + +TEST(TimestampedNodesTester, Duration) { + std::shared_ptr<no::Values> values(new no::Values()); + no::TimestampedNodes<double> timestamped_nodes(values); + EXPECT_EQ(timestamped_nodes.Duration(), 0); + ASSERT_EQ(timestamped_nodes.Add(1.0, 1).size(), 1); + EXPECT_EQ(timestamped_nodes.Duration(), 0); + ASSERT_EQ(timestamped_nodes.Add(2.0, 2).size(), 1); + EXPECT_NEAR(timestamped_nodes.Duration(), 1, 1e-6); + ASSERT_EQ(timestamped_nodes.Add(3.0, 3).size(), 1); + EXPECT_NEAR(timestamped_nodes.Duration(), 2, 1e-6); +} + +TEST(TimestampedNodesTester, Timestamps) { + std::shared_ptr<no::Values> values(new no::Values()); + no::TimestampedNodes<double> timestamped_nodes(values); + { + const auto timestamps = timestamped_nodes.Timestamps(); + EXPECT_EQ(timestamps.size(), 0); + } + const double t0 = 0; + const double t1 = 1; + const double t2 = 2; + const double t3 = 3; + ASSERT_EQ(timestamped_nodes.Add(t0, t0).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t1, t1).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t2, t2).size(), 1); + ASSERT_EQ(timestamped_nodes.Add(t3, t3).size(), 1); + { + const auto timestamps = timestamped_nodes.Timestamps(); + EXPECT_EQ(timestamps[0], t0); + EXPECT_EQ(timestamps[1], t1); + EXPECT_EQ(timestamps[2], t2); + EXPECT_EQ(timestamps[3], t3); + } +} + +TEST(TimestampedNodesTester, Serialization) { + const no::TimestampedNodes<double> nodes; + const auto serialized_nodes = gtsam::serializeBinary(nodes); + no::TimestampedNodes<double> deserialized_nodes; + gtsam::deserializeBinary(serialized_nodes, deserialized_nodes); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/imu_augmentor/test/test_imu_augmentor.test b/localization/nodes/test/test_timestamped_nodes.test similarity index 93% rename from localization/imu_augmentor/test/test_imu_augmentor.test rename to localization/nodes/test/test_timestamped_nodes.test index 4e44c50ef7..2253a70b07 100644 --- a/localization/imu_augmentor/test/test_imu_augmentor.test +++ b/localization/nodes/test/test_timestamped_nodes.test @@ -16,5 +16,5 @@ <!-- permissions and limitations under the License. --> <launch> - <test pkg="imu_augmentor" type="test_imu_augmentor" test-name="test_imu_augmentor" /> + <test pkg="nodes" type="test_timestamped_nodes" test-name="test_timestamped_nodes" /> </launch> diff --git a/localization/nodes/test/test_values.cc b/localization/nodes/test/test_values.cc new file mode 100644 index 0000000000..8f467b0f3e --- /dev/null +++ b/localization/nodes/test/test_values.cc @@ -0,0 +1,107 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <nodes/values.h> +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> + +#include <gtest/gtest.h> + +namespace lc = localization_common; +namespace no = nodes; + +TEST(ValuesTester, AddRemove) { + no::Values values; + EXPECT_EQ(values.size(), 0); + + // Add element + const double element_1 = 100.3; + const auto key_1 = values.Add(element_1); + EXPECT_TRUE(values.Contains(key_1)); + EXPECT_FALSE(values.Contains(2)); + EXPECT_EQ(values.size(), 1); + { + const auto bad_key_val = values.Value<double>(2); + EXPECT_TRUE(bad_key_val == boost::none); + const auto bad_type_val = values.Value<int>(2); + EXPECT_TRUE(bad_type_val == boost::none); + const auto good_val = values.Value<double>(key_1); + ASSERT_TRUE(good_val != boost::none); + EXPECT_EQ(good_val, element_1); + } + + // Add element + const double element_2 = 37.1; + const auto key_2 = values.Add(element_2); + EXPECT_TRUE(values.Contains(key_1)); + EXPECT_TRUE(values.Contains(key_2)); + EXPECT_FALSE(values.Contains(300)); + EXPECT_EQ(values.size(), 2); + { + const auto bad_key_val = values.Value<double>(3); + EXPECT_TRUE(bad_key_val == boost::none); + const auto bad_type_val = values.Value<int>(key_2); + EXPECT_TRUE(bad_type_val == boost::none); + const auto good_val = values.Value<double>(key_1); + ASSERT_TRUE(good_val != boost::none); + EXPECT_EQ(good_val, element_1); + } + { + const auto good_val = values.Value<double>(key_2); + ASSERT_TRUE(good_val != boost::none); + EXPECT_EQ(good_val, element_2); + } + + // Remove + EXPECT_TRUE(values.Remove(key_1)); + EXPECT_FALSE(values.Contains(key_1)); + EXPECT_TRUE(values.Contains(key_2)); + EXPECT_EQ(values.size(), 1); + { + const auto good_val = values.Value<double>(key_2); + ASSERT_TRUE(good_val != boost::none); + EXPECT_EQ(good_val, element_2); + } + + // Bad Remove + EXPECT_FALSE(values.Remove(key_1)); + EXPECT_FALSE(values.Remove(100)); + + // Remove + EXPECT_TRUE(values.Remove(key_2)); + EXPECT_FALSE(values.Contains(key_1)); + EXPECT_FALSE(values.Contains(key_2)); + EXPECT_EQ(values.size(), 0); + { + const auto bad_val = values.Value<double>(key_2); + EXPECT_TRUE(bad_val == boost::none); + } +} + +TEST(ValuesTester, Serialization) { + const no::Values values; + const auto serialized_values = gtsam::serializeBinary(values); + no::Values deserialized_values; + gtsam::deserializeBinary(serialized_values, deserialized_values); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/nodes/test/test_values.test b/localization/nodes/test/test_values.test new file mode 100644 index 0000000000..29ae74d8f5 --- /dev/null +++ b/localization/nodes/test/test_values.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="nodes" type="test_values" test-name="test_values" /> +</launch> diff --git a/localization/optimizers/CMakeLists.txt b/localization/optimizers/CMakeLists.txt new file mode 100644 index 0000000000..e4eb55d15e --- /dev/null +++ b/localization/optimizers/CMakeLists.txt @@ -0,0 +1,90 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(optimizers) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + localization_common +) + +# Find GTSAM +find_package(GTSAM REQUIRED) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +# include ff_nodelet to get ff_util header files since these aren't exposed elsewhere +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS localization_common +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${GLOG_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + src/isam2_optimizer.cc + src/nonlinear_optimizer.cc + src/optimizer.cc +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_nonlinear_optimizer + test/test_nonlinear_optimizer.test + test/test_nonlinear_optimizer.cc + ) + target_link_libraries(test_nonlinear_optimizer + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/optimizers/include/optimizers/isam2_optimizer.h b/localization/optimizers/include/optimizers/isam2_optimizer.h new file mode 100644 index 0000000000..4d4cd66370 --- /dev/null +++ b/localization/optimizers/include/optimizers/isam2_optimizer.h @@ -0,0 +1,73 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef OPTIMIZERS_ISAM2_OPTIMIZER_H_ +#define OPTIMIZERS_ISAM2_OPTIMIZER_H_ + +#include <optimizers/isam2_optimizer_params.h> +#include <optimizers/optimizer.h> + +#include <gtsam/nonlinear/NonlinearFactorGraph.h> +#include <gtsam/nonlinear/ISAM2.h> +#include <gtsam/nonlinear/ISAM2Params.h> + +#include <boost/serialization/serialization.hpp> + +#include <memory> + +namespace optimizers { +class ISAM2Optimizer : public Optimizer { + public: + explicit ISAM2Optimizer(const ISAM2OptimizerParams& params); + + // Default constructor for serialization only + ISAM2Optimizer() = default; + + virtual ~ISAM2Optimizer() = default; + + // Performs Levenberg-Marquardt nonlinear optimization using GTSAM on the factor graph. + bool Optimize(const gtsam::NonlinearFactorGraph& factors, gtsam::Values& values) final; + + int iterations() const final; + + private: + // Set optimization params based on provided ISAM2OptimizerParams. + void SetOptimizationParams(); + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(isam2_params_); + ar& BOOST_SERIALIZATION_NVP(isam2_); + ar& BOOST_SERIALIZATION_NVP(previous_factors_); + ar& BOOST_SERIALIZATION_NVP(previous_values_); + } + + ISAM2OptimizerParams params_; + gtsam::ISAM2Params isam2_params_; + std::unique_ptr<gtsam::ISAM2> isam2_; + // Previous factors and values, stored since ISAM2 only expects new factors + // and values to be added during optimization. + gtsam::NonlinearFactorGraph previous_factors_; + gtsam::Values previous_values_; +}; +} // namespace optimizers + +#endif // OPTIMIZERS_ISAM2_OPTIMIZER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/handrail_params.h b/localization/optimizers/include/optimizers/isam2_optimizer_params.h similarity index 72% rename from localization/graph_localizer/include/graph_localizer/handrail_params.h rename to localization/optimizers/include/optimizers/isam2_optimizer_params.h index f605b2b60c..5e71b83302 100644 --- a/localization/graph_localizer/include/graph_localizer/handrail_params.h +++ b/localization/optimizers/include/optimizers/isam2_optimizer_params.h @@ -15,14 +15,13 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef GRAPH_LOCALIZER_HANDRAIL_PARAMS_H_ -#define GRAPH_LOCALIZER_HANDRAIL_PARAMS_H_ +#ifndef OPTIMIZERS_ISAM2_OPTIMIZER_PARAMS_H_ +#define OPTIMIZERS_ISAM2_OPTIMIZER_PARAMS_H_ -namespace graph_localizer { -struct HandrailParams { - double length; - double distance_to_wall; -}; -} // namespace graph_localizer +#include <optimizers/optimizer_params.h> -#endif // GRAPH_LOCALIZER_HANDRAIL_PARAMS_H_ +namespace optimizers { +struct ISAM2OptimizerParams : public OptimizerParams {}; +} // namespace optimizers + +#endif // OPTIMIZERS_ISAM2_OPTIMIZER_PARAMS_H_ diff --git a/localization/optimizers/include/optimizers/nonlinear_optimizer.h b/localization/optimizers/include/optimizers/nonlinear_optimizer.h new file mode 100644 index 0000000000..803f1b644d --- /dev/null +++ b/localization/optimizers/include/optimizers/nonlinear_optimizer.h @@ -0,0 +1,67 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef OPTIMIZERS_NONLINEAR_OPTIMIZER_H_ +#define OPTIMIZERS_NONLINEAR_OPTIMIZER_H_ + +#include <optimizers/nonlinear_optimizer_params.h> +#include <optimizers/optimizer.h> + +#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> +#include <gtsam/nonlinear/NonlinearFactorGraph.h> + +#include <boost/serialization/serialization.hpp> + +namespace optimizers { +class NonlinearOptimizer : public Optimizer { + public: + explicit NonlinearOptimizer(const NonlinearOptimizerParams& params); + + // Default constructor for serialization only + NonlinearOptimizer() = default; + + virtual ~NonlinearOptimizer() = default; + + // Performs Levenberg-Marquardt nonlinear optimization using GTSAM on the factor graph. + bool Optimize(const gtsam::NonlinearFactorGraph& factors, gtsam::Values& values) final; + + int iterations() const final; + + private: + // Set optimization params based on provided NonlinearOptimizerParams. + void SetOptimizationParams(); + + // Order nodes in the graph for keys that will be marginalized. + // Allows for more efficient optimization. + void SetOrdering(const gtsam::NonlinearFactorGraph& factors, const gtsam::KeyVector& old_keys); + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(levenberg_marquardt_params_); + } + + NonlinearOptimizerParams params_; + gtsam::LevenbergMarquardtParams levenberg_marquardt_params_; + int iterations_; +}; +} // namespace optimizers + +#endif // OPTIMIZERS_NONLINEAR_OPTIMIZER_H_ diff --git a/localization/optimizers/include/optimizers/nonlinear_optimizer_params.h b/localization/optimizers/include/optimizers/nonlinear_optimizer_params.h new file mode 100644 index 0000000000..283367b20e --- /dev/null +++ b/localization/optimizers/include/optimizers/nonlinear_optimizer_params.h @@ -0,0 +1,34 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef OPTIMIZERS_NONLINEAR_OPTIMIZER_PARAMS_H_ +#define OPTIMIZERS_NONLINEAR_OPTIMIZER_PARAMS_H_ + +#include <optimizers/optimizer_params.h> + +namespace optimizers { +struct NonlinearOptimizerParams : public OptimizerParams { + // Maximum optimization iterations to perform + int max_iterations; + // Verbose printing for Levenberg-Marquardt optimization + bool verbose; + // Use ceres-style params for nonlinear optimization + bool use_ceres_params; +}; +} // namespace optimizers + +#endif // OPTIMIZERS_NONLINEAR_OPTIMIZER_PARAMS_H_ diff --git a/localization/optimizers/include/optimizers/optimizer.h b/localization/optimizers/include/optimizers/optimizer.h new file mode 100644 index 0000000000..7ef4ad273a --- /dev/null +++ b/localization/optimizers/include/optimizers/optimizer.h @@ -0,0 +1,79 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef OPTIMIZERS_OPTIMIZER_H_ +#define OPTIMIZERS_OPTIMIZER_H_ + +#include <optimizers/optimizer_params.h> + +#include <gtsam/base/Matrix.h> +#include <gtsam/inference/Key.h> +#include <gtsam/nonlinear/Marginals.h> +#include <gtsam/nonlinear/NonlinearFactorGraph.h> + +#include <boost/optional.hpp> + +namespace optimizers { +// Base class optimizer. +class Optimizer { + public: + explicit Optimizer(const OptimizerParams& params); + virtual ~Optimizer() = default; + + // Performs optimization using the provided factors and values and updates the values + // with the optimized estimates. + virtual bool Optimize(const gtsam::NonlinearFactorGraph& factors, gtsam::Values& values) = 0; + + // Returns the covariance matrix for the provided value's key if it exists. + // By default uses the calculated marginals to compute the covariances. + virtual boost::optional<gtsam::Matrix> Covariance(const gtsam::Key& key) const; + + // Calculates the covariance matrix wrt two nodes using the provided keys. + // Requires a successful round of optimization to have been performed. + boost::optional<gtsam::Matrix> Covariance(const gtsam::Key& key_a, const gtsam::Key& key_b) const; + + // Returns marginals if they have been calculated. + boost::optional<const gtsam::Marginals&> marginals() const; + + // Returns number of optimization iterations used for most recent optimize call. + virtual int iterations() const = 0; + + protected: + // Calculates marginals. + void CalculateMarginals(const gtsam::NonlinearFactorGraph& factors, const gtsam::Values& values); + + private: + // Set solver used for calculating marginals. + void SetMarginalsFactorization(); + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(marginals_); + ar& BOOST_SERIALIZATION_NVP(marginals_factorization_); + } + + OptimizerParams params_; + boost::optional<gtsam::Marginals> marginals_; + gtsam::Marginals::Factorization marginals_factorization_; +}; +} // namespace optimizers + +#endif // OPTIMIZERS_OPTIMIZER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/serialization.h b/localization/optimizers/include/optimizers/optimizer_params.h similarity index 70% rename from localization/graph_localizer/include/graph_localizer/serialization.h rename to localization/optimizers/include/optimizers/optimizer_params.h index ba3b5c25c1..e874388054 100644 --- a/localization/graph_localizer/include/graph_localizer/serialization.h +++ b/localization/optimizers/include/optimizers/optimizer_params.h @@ -15,16 +15,16 @@ * License for the specific language governing permissions and limitations * under the License. */ - -#ifndef GRAPH_LOCALIZER_SERIALIZATION_H_ -#define GRAPH_LOCALIZER_SERIALIZATION_H_ - -#include <graph_localizer/graph_localizer.h> +#ifndef OPTIMIZERS_OPTIMIZER_PARAMS_H_ +#define OPTIMIZERS_OPTIMIZER_PARAMS_H_ #include <string> -namespace graph_localizer { -std::string SerializeBinary(const GraphLocalizer& graph_localizer); -} // namespace graph_localizer +namespace optimizers { +struct OptimizerParams { + // Type of factorization used to calculate marginals. Either "qr" or "cholesky". + std::string marginals_factorization; +}; +} // namespace optimizers -#endif // GRAPH_LOCALIZER_SERIALIZATION_H_ +#endif // OPTIMIZERS_OPTIMIZER_PARAMS_H_ diff --git a/localization/optimizers/package.xml b/localization/optimizers/package.xml new file mode 100644 index 0000000000..14f70ed00e --- /dev/null +++ b/localization/optimizers/package.xml @@ -0,0 +1,19 @@ +<package> + <name>optimizers</name> + <version>1.0.0</version> + <description> + The optimizers package + </description> + <license> + Apache License, Version 2.0 + </license> + <author email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </author> + <maintainer email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </maintainer> + <buildtool_depend>catkin</buildtool_depend> + <build_depend>localization_common</build_depend> + <run_depend>localization_common</run_depend> +</package> diff --git a/localization/optimizers/readme.md b/localization/optimizers/readme.md new file mode 100644 index 0000000000..4d56c92179 --- /dev/null +++ b/localization/optimizers/readme.md @@ -0,0 +1,11 @@ +\page optimizer + +# Package Overview +The optimizer package provides a base class optimizer that is used by the graph optimizer class. Optimizers perform optimization using GTSAM factors and values, for more information on these see the GTSAM package here: https://github.com/borglab/gtsam. Optimizers also provided covariance matrices for values given their keys. Two implementations are provided - the nonlinear optimizer and isam2 optimizer. + +## Nonlinear Optimizer +The nonlinear optimizer performs Levenberg-Marquardt nonlinear optimization. + +## ISAM2 Optimizer +The ISAM2 optimizer performs optimization using ISAM2. Note this is still in development and not yet tested. + diff --git a/localization/optimizers/src/isam2_optimizer.cc b/localization/optimizers/src/isam2_optimizer.cc new file mode 100644 index 0000000000..cc63c8d7ba --- /dev/null +++ b/localization/optimizers/src/isam2_optimizer.cc @@ -0,0 +1,63 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/logger.h> +#include <localization_common/utilities.h> +#include <optimizers/isam2_optimizer.h> + +namespace optimizers { +namespace lc = localization_common; + +ISAM2Optimizer::ISAM2Optimizer(const ISAM2OptimizerParams& params) : Optimizer(params), params_(params) { + SetOptimizationParams(); + isam2_.reset(new gtsam::ISAM2(isam2_params_)); +} + +bool ISAM2Optimizer::Optimize(const gtsam::NonlinearFactorGraph& factors, gtsam::Values& values) { + LogDebug("Optimize: Optimizing."); + + // Only add new factors. Assumes factors consist of previous_factors_ followed by + // new factors. + gtsam::NonlinearFactorGraph new_factors; + for (int i = previous_factors_.size(); i < factors.size(); ++i) { + new_factors.push_back(factors[i]); + } + + // Only add new values. + const boost::function<bool(gtsam::Key)> new_value = [this](gtsam::Key key) { + return !(previous_values_.exists(key)); + }; + gtsam::Values::Filtered<gtsam::Value> new_values = values.filter(new_value); + + const auto result = isam2_->update(new_factors, new_values); + // Update values based on latest linearization point + values = isam2_->getLinearizationPoint(); + CalculateMarginals(factors, values); + // Cache factors and values so they can be referenced for removing already existing + // factors and values added to the isam2 optimizer + previous_factors_ = factors; + previous_values_ = values; + return true; +} + +int ISAM2Optimizer::iterations() const { return 1; } + +void ISAM2Optimizer::SetOptimizationParams() { + // TODO(rsoussan): Add more params ISAM2OptimizerParams to custom set gtsam::ISAM2Params +} +} // namespace optimizers diff --git a/localization/optimizers/src/nonlinear_optimizer.cc b/localization/optimizers/src/nonlinear_optimizer.cc new file mode 100644 index 0000000000..2eb49aa2fc --- /dev/null +++ b/localization/optimizers/src/nonlinear_optimizer.cc @@ -0,0 +1,85 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <optimizers/nonlinear_optimizer.h> +#include <localization_common/logger.h> +#include <localization_common/utilities.h> + +namespace optimizers { +namespace lc = localization_common; + +NonlinearOptimizer::NonlinearOptimizer(const NonlinearOptimizerParams& params) : Optimizer(params), params_(params) { + SetOptimizationParams(); +} + +bool NonlinearOptimizer::Optimize(const gtsam::NonlinearFactorGraph& factors, gtsam::Values& values) { + LogDebug("Optimize: Optimizing."); + + // Perform Levenberg Marquart nonlinear optimization. + // Update the values with the newly optmized values. + gtsam::LevenbergMarquardtOptimizer optimizer(factors, values, levenberg_marquardt_params_); + + try { + values = optimizer.optimize(); + // Calculate marginals after optimizing so covariances and marginal factors + // can be generated if desired. + CalculateMarginals(factors, values); + iterations_ = optimizer.iterations(); + } catch (gtsam::IndeterminantLinearSystemException) { + LogError("Optimize: Graph optimization failed, indeterminant linear system."); + return false; + } catch (gtsam::InvalidNoiseModel) { + LogError("Optimize: Graph optimization failed, invalid noise model."); + return false; + } catch (gtsam::InvalidMatrixBlock) { + LogError("Optimize: Graph optimization failed, invalid matrix block."); + return false; + } catch (gtsam::InvalidDenseElimination) { + LogError("Optimize: Graph optimization failed, invalid dense elimination."); + return false; + } catch (const std::exception& exception) { + LogError("Optimize: Graph optimization failed, " << exception.what()); + return false; + } catch (...) { + LogError("Optimize: Graph optimization failed."); + return false; + } + return true; +} + +int NonlinearOptimizer::iterations() const { return iterations_; } + +void NonlinearOptimizer::SetOptimizationParams() { + // Initialize lm params + if (params_.verbose) { + levenberg_marquardt_params_.verbosityLM = gtsam::LevenbergMarquardtParams::VerbosityLM::TRYDELTA; + levenberg_marquardt_params_.verbosity = gtsam::NonlinearOptimizerParams::Verbosity::LINEAR; + } + if (params_.use_ceres_params) { + gtsam::LevenbergMarquardtParams::SetCeresDefaults(&levenberg_marquardt_params_); + } + + levenberg_marquardt_params_.maxIterations = params_.max_iterations; + levenberg_marquardt_params_.orderingType = gtsam::Ordering::COLAMD; +} + +void NonlinearOptimizer::SetOrdering(const gtsam::NonlinearFactorGraph& factors, const gtsam::KeyVector& old_keys) { + const auto ordering = gtsam::Ordering::ColamdConstrainedFirst(factors, old_keys); + levenberg_marquardt_params_.setOrdering(ordering); +} +} // namespace optimizers diff --git a/localization/optimizers/src/optimizer.cc b/localization/optimizers/src/optimizer.cc new file mode 100644 index 0000000000..f7bf42b4e9 --- /dev/null +++ b/localization/optimizers/src/optimizer.cc @@ -0,0 +1,76 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <optimizers/optimizer.h> +#include <localization_common/logger.h> + +namespace optimizers { + +Optimizer::Optimizer(const OptimizerParams& params) : params_(params) { SetMarginalsFactorization(); } + +boost::optional<gtsam::Matrix> Optimizer::Covariance(const gtsam::Key& key) const { + if (!marginals_) return boost::none; + try { + return marginals_->marginalCovariance(key); + } catch (...) { + LogError("Covariance: Failed to get covariance for key " << key); + return boost::none; + } +} + +boost::optional<gtsam::Matrix> Optimizer::Covariance(const gtsam::Key& key_a, const gtsam::Key& key_b) const { + if (!marginals_) return boost::none; + try { + return marginals_->jointMarginalCovariance({key_a, key_b}).fullMatrix(); + } catch (...) { + LogError("Covariance: Failed to get covariance between key " << key_a << " and " << key_b); + return boost::none; + } +} + +void Optimizer::CalculateMarginals(const gtsam::NonlinearFactorGraph& factors, const gtsam::Values& values) { + try { + marginals_ = gtsam::Marginals(factors, values, marginals_factorization_); + } catch (gtsam::IndeterminantLinearSystemException) { + LogError("Update: Indeterminant linear system error during computation of marginals."); + marginals_ = boost::none; + } catch (const std::exception& exception) { + LogError("Update: Computing marginals failed. " + std::string(exception.what())); + marginals_ = boost::none; + } catch (...) { + LogError("Update: Computing marginals failed."); + marginals_ = boost::none; + } +} + +boost::optional<const gtsam::Marginals&> Optimizer::marginals() const { + if (!marginals_) return boost::none; + return *marginals_; +} + +void Optimizer::SetMarginalsFactorization() { + if (params_.marginals_factorization == "qr") { + marginals_factorization_ = gtsam::Marginals::Factorization::QR; + } else if (params_.marginals_factorization == "cholesky") { + marginals_factorization_ = gtsam::Marginals::Factorization::CHOLESKY; + } else { + LogError("Optimizer: No marginals factorization entered, defaulting to qr."); + marginals_factorization_ = gtsam::Marginals::Factorization::QR; + } +} +} // namespace optimizers diff --git a/localization/optimizers/test/test_nonlinear_optimizer.cc b/localization/optimizers/test/test_nonlinear_optimizer.cc new file mode 100644 index 0000000000..9c24300247 --- /dev/null +++ b/localization/optimizers/test/test_nonlinear_optimizer.cc @@ -0,0 +1,93 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <optimizers/nonlinear_optimizer.h> + +#include <gtest/gtest.h> + +namespace lc = localization_common; +namespace op = optimizers; + +class NonlinearOptimizerTest : public ::testing::Test { + public: + NonlinearOptimizerTest() {} + + void SetUp() final { Initialize(DefaultNonlinearOptimizerParams()); } + + void Initialize(const op::NonlinearOptimizerParams& params) { + nonlinear_optimizer_.reset(new op::NonlinearOptimizer(params)); + } + + op::NonlinearOptimizerParams DefaultNonlinearOptimizerParams() { + op::NonlinearOptimizerParams params; + params.max_iterations = 10; + params.verbose = false; + params.use_ceres_params = false; + params.marginals_factorization = "qr"; + return params; + } + + std::unique_ptr<op::NonlinearOptimizer> nonlinear_optimizer_; + gtsam::Values values_; +}; + +// Optimize noisy value using perfect prior +TEST_F(NonlinearOptimizerTest, OptimizeNoisyValue) { + gtsam::NonlinearFactorGraph factors; + const auto pose_noise = gtsam::noiseModel::Isotropic::Sigma(6, 0.1); + + // Add perfect true pose to prior and noisy pose to values + const auto true_pose = lc::RandomPose(); + const auto noisy_pose = true_pose * lc::RandomPose(); + const gtsam::Key key(0); + gtsam::PriorFactor<gtsam::Pose3>::shared_ptr pose_prior_factor( + new gtsam::PriorFactor<gtsam::Pose3>(key, true_pose, pose_noise)); + factors.push_back(pose_prior_factor); + values_.insert(key, noisy_pose); + + { + // Marginals shouldn't exist before optimization is performed. + const auto marginals = nonlinear_optimizer_->marginals(); + EXPECT_TRUE(marginals == boost::none); + } + + // Optimize + nonlinear_optimizer_->Optimize(factors, values_); + + // Pose value should match prior after optimization + const auto optimized_pose = values_.at<gtsam::Pose3>(key); + EXPECT_MATRIX_NEAR(optimized_pose, true_pose, 1e-6); + + // Covariance should match prior noise + const auto covariance = nonlinear_optimizer_->Covariance(key); + ASSERT_TRUE(covariance != boost::none); + EXPECT_MATRIX_NEAR(gtsam::Matrix(*covariance), pose_noise->covariance(), 1e-6); + + // Marginals should be available + const auto marginals = nonlinear_optimizer_->marginals(); + EXPECT_TRUE(marginals != boost::none); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/optimizers/test/test_nonlinear_optimizer.test b/localization/optimizers/test/test_nonlinear_optimizer.test new file mode 100644 index 0000000000..5268e46792 --- /dev/null +++ b/localization/optimizers/test/test_nonlinear_optimizer.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="optimizers" type="test_nonlinear_optimizer" test-name="test_nonlinear_optimizer" /> +</launch> diff --git a/localization/parameter_reader/CMakeLists.txt b/localization/parameter_reader/CMakeLists.txt new file mode 100644 index 0000000000..30311277b8 --- /dev/null +++ b/localization/parameter_reader/CMakeLists.txt @@ -0,0 +1,105 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(parameter_reader) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + factor_adders + graph_optimizer + graph_vio + graph_localizer + localization_common + node_adders + sliding_window_graph_optimizer +) + +# Find GTSAM +find_package(GTSAM REQUIRED) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +# include ff_nodelet to get ff_util header files since these aren't exposed elsewhere +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS factor_adders graph_optimizer sliding_window_graph_optimizer graph_vio graph_localizer vision_common localization_common node_adders +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${GLOG_INCLUDE_DIRS} +) + +file(GLOB cc_files + "src/*.cc" +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + ${cc_files} +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_graph_vio_parameter_reader + test/test_graph_vio_parameter_reader.test + test/test_graph_vio_parameter_reader.cc + ) + target_link_libraries(test_graph_vio_parameter_reader + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_graph_localizer_parameter_reader + test/test_graph_localizer_parameter_reader.test + test/test_graph_localizer_parameter_reader.cc + ) + target_link_libraries(test_graph_localizer_parameter_reader + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/parameter_reader/include/parameter_reader/factor_adders.h b/localization/parameter_reader/include/parameter_reader/factor_adders.h new file mode 100644 index 0000000000..388d34a487 --- /dev/null +++ b/localization/parameter_reader/include/parameter_reader/factor_adders.h @@ -0,0 +1,60 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef PARAMETER_READER_FACTOR_ADDERS_H_ +#define PARAMETER_READER_FACTOR_ADDERS_H_ + +#include <config_reader/config_reader.h> +#include <factor_adders/factor_adder_params.h> +#include <factor_adders/loc_factor_adder_params.h> +#include <factor_adders/relative_pose_factor_adder_params.h> +#include <factor_adders/depth_odometry_factor_adder_params.h> +#include <factor_adders/standstill_factor_adder_params.h> +#include <factor_adders/vo_smart_projection_factor_adder_params.h> + +#include <gtsam/slam/SmartFactorParams.h> + +#include <string> + +namespace parameter_reader { +void LoadFactorAdderParams(config_reader::ConfigReader& config, factor_adders::FactorAdderParams& params, + const std::string& prefix = ""); + +void LoadLocFactorAdderParams(config_reader::ConfigReader& config, factor_adders::LocFactorAdderParams& params, + const std::string& prefix = "", const std::string& camera_name = "nav"); + +void LoadDepthOdometryFactorAdderParams(config_reader::ConfigReader& config, + factor_adders::DepthOdometryFactorAdderParams& params, + const std::string& prefix = "", const std::string& camera_name = "haz"); + +void LoadRelativePoseFactorAdderParams(config_reader::ConfigReader& config, + factor_adders::RelativePoseFactorAdderParams& params, + const std::string& prefix = ""); + +void LoadStandstillFactorAdderParams(config_reader::ConfigReader& config, + factor_adders::StandstillFactorAdderParams& params, + const std::string& prefix = ""); + +void LoadVoSmartProjectionFactorAdderParams(config_reader::ConfigReader& config, + factor_adders::VoSmartProjectionFactorAdderParams& params, + const std::string& prefix = ""); + +void LoadSmartProjectionParams(config_reader::ConfigReader& config, gtsam::SmartProjectionParams& params, + const std::string& prefix = ""); +} // namespace parameter_reader + +#endif // PARAMETER_READER_FACTOR_ADDERS_H_ diff --git a/localization/parameter_reader/include/parameter_reader/graph_localizer.h b/localization/parameter_reader/include/parameter_reader/graph_localizer.h new file mode 100644 index 0000000000..fb00af5e31 --- /dev/null +++ b/localization/parameter_reader/include/parameter_reader/graph_localizer.h @@ -0,0 +1,31 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef PARAMETER_READER_GRAPH_LOCALIZER_H_ +#define PARAMETER_READER_GRAPH_LOCALIZER_H_ + +#include <config_reader/config_reader.h> +#include <graph_localizer/graph_localizer_params.h> + +#include <string> + +namespace parameter_reader { +void LoadGraphLocalizerParams(config_reader::ConfigReader& config, graph_localizer::GraphLocalizerParams& params, + const std::string& prefix = ""); +} // namespace parameter_reader + +#endif // PARAMETER_READER_GRAPH_LOCALIZER_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/parameter_reader.h b/localization/parameter_reader/include/parameter_reader/graph_optimizer.h similarity index 73% rename from localization/graph_optimizer/include/graph_optimizer/parameter_reader.h rename to localization/parameter_reader/include/parameter_reader/graph_optimizer.h index f719c28a6b..f392040059 100644 --- a/localization/graph_optimizer/include/graph_optimizer/parameter_reader.h +++ b/localization/parameter_reader/include/parameter_reader/graph_optimizer.h @@ -15,14 +15,17 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef GRAPH_OPTIMIZER_PARAMETER_READER_H_ -#define GRAPH_OPTIMIZER_PARAMETER_READER_H_ +#ifndef PARAMETER_READER_GRAPH_OPTIMIZER_H_ +#define PARAMETER_READER_GRAPH_OPTIMIZER_H_ #include <config_reader/config_reader.h> #include <graph_optimizer/graph_optimizer_params.h> -namespace graph_optimizer { -void LoadGraphOptimizerParams(config_reader::ConfigReader& config, GraphOptimizerParams& params); -} // namespace graph_optimizer +#include <string> -#endif // GRAPH_OPTIMIZER_PARAMETER_READER_H_ +namespace parameter_reader { +void LoadGraphOptimizerParams(config_reader::ConfigReader& config, graph_optimizer::GraphOptimizerParams& params, + const std::string& prefix = ""); +} // namespace parameter_reader + +#endif // PARAMETER_READER_GRAPH_OPTIMIZER_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/serialization.h b/localization/parameter_reader/include/parameter_reader/graph_vio.h similarity index 65% rename from localization/graph_optimizer/include/graph_optimizer/serialization.h rename to localization/parameter_reader/include/parameter_reader/graph_vio.h index 2d54656b13..ba71895c6f 100644 --- a/localization/graph_optimizer/include/graph_optimizer/serialization.h +++ b/localization/parameter_reader/include/parameter_reader/graph_vio.h @@ -15,16 +15,17 @@ * License for the specific language governing permissions and limitations * under the License. */ +#ifndef PARAMETER_READER_GRAPH_VIO_H_ +#define PARAMETER_READER_GRAPH_VIO_H_ -#ifndef GRAPH_OPTIMIZER_SERIALIZATION_H_ -#define GRAPH_OPTIMIZER_SERIALIZATION_H_ - -#include <graph_optimizer/graph_optimizer.h> +#include <config_reader/config_reader.h> +#include <graph_vio/graph_vio_params.h> #include <string> -namespace graph_optimizer { -std::string SerializeBinary(const GraphLocalizer& graph_optimizer); -} // namespace graph_optimizer +namespace parameter_reader { +void LoadGraphVIOParams(config_reader::ConfigReader& config, graph_vio::GraphVIOParams& params, + const std::string& prefix = ""); +} // namespace parameter_reader -#endif // GRAPH_OPTIMIZER_SERIALIZATION_H_ +#endif // PARAMETER_READER_GRAPH_VIO_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_initializer_params.h b/localization/parameter_reader/include/parameter_reader/imu_integration.h similarity index 55% rename from localization/graph_localizer/include/graph_localizer/graph_initializer_params.h rename to localization/parameter_reader/include/parameter_reader/imu_integration.h index 90349b9ec6..7bbc7ec9b1 100644 --- a/localization/graph_localizer/include/graph_localizer/graph_initializer_params.h +++ b/localization/parameter_reader/include/parameter_reader/imu_integration.h @@ -15,25 +15,21 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef GRAPH_LOCALIZER_GRAPH_INITIALIZER_PARAMS_H_ -#define GRAPH_LOCALIZER_GRAPH_INITIALIZER_PARAMS_H_ +#ifndef PARAMETER_READER_IMU_INTEGRATION_H_ +#define PARAMETER_READER_IMU_INTEGRATION_H_ -#include <imu_integration/latest_imu_integrator_params.h> +#include <config_reader/config_reader.h> #include <imu_integration/imu_filter_params.h> - -#include <gtsam/geometry/Pose3.h> -#include <gtsam/base/Vector.h> +#include <imu_integration/imu_integrator_params.h> #include <string> -namespace graph_localizer { -// TODO(rsoussan): Clean this up, only use what is needed from imu integration params -struct GraphInitializerParams : public imu_integration::LatestImuIntegratorParams { - gtsam::Pose3 global_T_body_start; - gtsam::Vector3 global_V_body_start; - std::string imu_bias_filename; - int num_bias_estimation_measurements; -}; -} // namespace graph_localizer +namespace parameter_reader { +void LoadImuFilterParams(config_reader::ConfigReader& config, imu_integration::ImuFilterParams& params, + const std::string& prefix = ""); + +void LoadImuIntegratorParams(config_reader::ConfigReader& config, imu_integration::ImuIntegratorParams& params, + const std::string& prefix = ""); +} // namespace parameter_reader -#endif // GRAPH_LOCALIZER_GRAPH_INITIALIZER_PARAMS_H_ +#endif // PARAMETER_READER_IMU_INTEGRATION_H_ diff --git a/localization/parameter_reader/include/parameter_reader/node_adders.h b/localization/parameter_reader/include/parameter_reader/node_adders.h new file mode 100644 index 0000000000..d5b4f509e5 --- /dev/null +++ b/localization/parameter_reader/include/parameter_reader/node_adders.h @@ -0,0 +1,65 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef PARAMETER_READER_NODE_ADDERS_H_ +#define PARAMETER_READER_NODE_ADDERS_H_ + +#include <config_reader/config_reader.h> +#include <node_adders/combined_nav_state_node_adder.h> +#include <node_adders/combined_nav_state_node_adder_model_params.h> +#include <node_adders/pose_node_adder_params.h> +#include <node_adders/timestamped_node_adder_model_params.h> +#include <node_adders/timestamped_node_adder_params.h> + +#include <string> + +namespace parameter_reader { +template <typename NodeType> +void LoadBaseTimestampedNodeAdderParams(config_reader::ConfigReader& config, + node_adders::TimestampedNodeAdderParams<NodeType>& params, + const std::string& prefix = ""); + +void LoadTimestampedNodeAdderModelParams(config_reader::ConfigReader& config, + node_adders::TimestampedNodeAdderModelParams& params, + const std::string& prefix = ""); + +void LoadCombinedNavStateNodeAdderParams(config_reader::ConfigReader& config, + node_adders::CombinedNavStateNodeAdder::Params& params, + const std::string& prefix = ""); + +void LoadCombinedNavStateNodeAdderModelParams(config_reader::ConfigReader& config, + node_adders::CombinedNavStateNodeAdderModelParams& params, + const std::string& prefix = ""); + +void LoadPoseNodeAdderParams(config_reader::ConfigReader& config, node_adders::PoseNodeAdderParams& params, + const std::string& prefix = ""); + +// Implementation +template <typename NodeType> +void LoadBaseTimestampedNodeAdderParams(config_reader::ConfigReader& config, + node_adders::TimestampedNodeAdderParams<NodeType>& params, + const std::string& prefix) { + // Note, starting measurement and time should be set elsewhere, typically using measurements + LOAD_PARAM(params.huber_k, config, prefix); + LOAD_PARAM(params.add_priors, config, prefix); + LOAD_PARAM(params.ideal_duration, config, prefix); + LOAD_PARAM(params.min_num_states, config, prefix); + LOAD_PARAM(params.max_num_states, config, prefix); +} +} // namespace parameter_reader + +#endif // PARAMETER_READER_NODE_ADDERS_H_ diff --git a/localization/parameter_reader/include/parameter_reader/optimizers.h b/localization/parameter_reader/include/parameter_reader/optimizers.h new file mode 100644 index 0000000000..980098e4fc --- /dev/null +++ b/localization/parameter_reader/include/parameter_reader/optimizers.h @@ -0,0 +1,39 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef PARAMETER_READER_OPTIMIZERS_H_ +#define PARAMETER_READER_OPTIMIZERS_H_ + +#include <config_reader/config_reader.h> +#include <optimizers/isam2_optimizer_params.h> +#include <optimizers/nonlinear_optimizer_params.h> +#include <optimizers/optimizer_params.h> + +#include <string> + +namespace parameter_reader { +void LoadOptimizerParams(config_reader::ConfigReader& config, optimizers::OptimizerParams& params, + const std::string& prefix = ""); + +void LoadISAM2OptimizerParams(config_reader::ConfigReader& config, optimizers::ISAM2OptimizerParams& params, + const std::string& prefix = ""); + +void LoadNonlinearOptimizerParams(config_reader::ConfigReader& config, optimizers::NonlinearOptimizerParams& params, + const std::string& prefix = ""); +} // namespace parameter_reader + +#endif // PARAMETER_READER_OPTIMIZERS_H_ diff --git a/localization/parameter_reader/include/parameter_reader/sliding_window_graph_optimizer.h b/localization/parameter_reader/include/parameter_reader/sliding_window_graph_optimizer.h new file mode 100644 index 0000000000..a476cda433 --- /dev/null +++ b/localization/parameter_reader/include/parameter_reader/sliding_window_graph_optimizer.h @@ -0,0 +1,32 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef PARAMETER_READER_SLIDING_WINDOW_GRAPH_OPTIMIZER_H_ +#define PARAMETER_READER_SLIDING_WINDOW_GRAPH_OPTIMIZER_H_ + +#include <config_reader/config_reader.h> +#include <sliding_window_graph_optimizer/sliding_window_graph_optimizer_params.h> + +#include <string> + +namespace parameter_reader { +void LoadSlidingWindowGraphOptimizerParams(config_reader::ConfigReader& config, + sliding_window_graph_optimizer::SlidingWindowGraphOptimizerParams& params, + const std::string& prefix = ""); +} // namespace parameter_reader + +#endif // PARAMETER_READER_SLIDING_WINDOW_GRAPH_OPTIMIZER_H_ diff --git a/localization/parameter_reader/include/parameter_reader/vision_common.h b/localization/parameter_reader/include/parameter_reader/vision_common.h new file mode 100644 index 0000000000..666d1c488a --- /dev/null +++ b/localization/parameter_reader/include/parameter_reader/vision_common.h @@ -0,0 +1,39 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef PARAMETER_READER_VISION_COMMON_H_ +#define PARAMETER_READER_VISION_COMMON_H_ + +#include <config_reader/config_reader.h> +#include <vision_common/feature_tracker_params.h> +#include <vision_common/spaced_feature_tracker_params.h> +#include <vision_common/standstill_params.h> + +#include <string> + +namespace parameter_reader { +void LoadStandstillParams(config_reader::ConfigReader& config, vision_common::StandstillParams& params, + const std::string& prefix = ""); + +void LoadFeatureTrackerParams(config_reader::ConfigReader& config, vision_common::FeatureTrackerParams& params, + const std::string& prefix = ""); + +void LoadSpacedFeatureTrackerParams(config_reader::ConfigReader& config, + vision_common::SpacedFeatureTrackerParams& params, const std::string& prefix = ""); +} // namespace parameter_reader + +#endif // PARAMETER_READER_VISION_COMMON_H_ diff --git a/localization/parameter_reader/package.xml b/localization/parameter_reader/package.xml new file mode 100644 index 0000000000..7f6e403224 --- /dev/null +++ b/localization/parameter_reader/package.xml @@ -0,0 +1,34 @@ +<package> + <name>parameter_reader</name> + <version>1.0.0</version> + <description> + The parameter reader package + </description> + <license> + Apache License, Version 2.0 + </license> + <author email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </author> + <maintainer email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </maintainer> + <buildtool_depend>catkin</buildtool_depend> + <build_depend>factor_adders</build_depend> + <build_depend>localization_common</build_depend> + <build_depend>node_adders</build_depend> + <build_depend>graph_vio</build_depend> + <build_depend>graph_localizer</build_depend> + <build_depend>vision_common</build_depend> + <build_depend>graph_optimizer</build_depend> + <build_depend>sliding_window_graph_optimizer</build_depend> + <run_depend>factor_adders</run_depend> + <run_depend>localization_common</run_depend> + <run_depend>node_adders</run_depend> + <run_depend>graph_vio</run_depend> + <run_depend>graph_localizer</run_depend> + <run_depend>vision_common</run_depend> + <run_depend>graph_optimizer</run_depend> + <run_depend>sliding_window_graph_optimizer</run_depend> +</package> + diff --git a/localization/parameter_reader/readme.md b/localization/parameter_reader/readme.md new file mode 100644 index 0000000000..d5620206f0 --- /dev/null +++ b/localization/parameter_reader/readme.md @@ -0,0 +1,4 @@ +\page parameterreader Parameter Reader + +# Package Overview +The parameter reader package loads parameters from config files for various localization packages. diff --git a/localization/parameter_reader/src/factor_adders.cc b/localization/parameter_reader/src/factor_adders.cc new file mode 100644 index 0000000000..b800a6119c --- /dev/null +++ b/localization/parameter_reader/src/factor_adders.cc @@ -0,0 +1,131 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/utilities.h> +#include <msg_conversions/msg_conversions.h> +#include <parameter_reader/factor_adders.h> +#include <parameter_reader/vision_common.h> + +namespace parameter_reader { +namespace fa = factor_adders; +namespace lc = localization_common; +namespace mc = msg_conversions; + +void LoadFactorAdderParams(config_reader::ConfigReader& config, fa::FactorAdderParams& params, + const std::string& prefix) { + LOAD_PARAM(params.enabled, config, prefix); + LOAD_PARAM(params.huber_k, config, prefix); +} + +void LoadLocFactorAdderParams(config_reader::ConfigReader& config, fa::LocFactorAdderParams& params, + const std::string& prefix, const std::string& camera_name) { + LoadFactorAdderParams(config, params, prefix); + LOAD_PARAM(params.add_pose_priors, config, prefix); + LOAD_PARAM(params.add_projection_factors, config, prefix); + LOAD_PARAM(params.add_prior_if_projection_factors_fail, config, prefix); + LOAD_PARAM(params.prior_translation_stddev, config, prefix); + LOAD_PARAM(params.prior_quaternion_stddev, config, prefix); + LOAD_PARAM(params.scale_pose_noise_with_num_landmarks, config, prefix); + LOAD_PARAM(params.scale_projection_noise_with_num_landmarks, config, prefix); + LOAD_PARAM(params.scale_projection_noise_with_landmark_distance, config, prefix); + LOAD_PARAM(params.pose_noise_scale, config, prefix); + LOAD_PARAM(params.projection_noise_scale, config, prefix); + LOAD_PARAM(params.max_num_projection_factors, config, prefix); + LOAD_PARAM(params.min_num_matches_per_measurement, config, prefix); + LOAD_PARAM(params.max_valid_projection_error, config, prefix); + // Don't pass prefix for load transform or intrinsics since these are from camera config + params.body_T_cam = lc::LoadTransform(config, camera_name + "_cam_transform"); + // TODO(rsoussan): make not necessarily nav cam specific? + params.cam_intrinsics.reset(new gtsam::Cal3_S2(lc::LoadCameraIntrinsics(config, camera_name + "_cam"))); + params.cam_noise = + gtsam::noiseModel::Isotropic::Sigma(2, mc::LoadDouble(config, camera_name + "_cam_noise_stddev", prefix)); +} + +void LoadDepthOdometryFactorAdderParams(config_reader::ConfigReader& config, fa::DepthOdometryFactorAdderParams& params, + const std::string& prefix, const std::string& camera_name) { + LoadFactorAdderParams(config, params, prefix); + LOAD_PARAM(params.pose_covariance_scale, config, prefix); + LOAD_PARAM(params.point_noise_scale, config, prefix); + LOAD_PARAM(params.use_points_between_factor, config, prefix); + LOAD_PARAM(params.scale_point_between_factors_with_inverse_distance, config, prefix); + LOAD_PARAM(params.scale_point_between_factors_with_estimate_error, config, prefix); + LOAD_PARAM(params.reject_large_point_to_point_error, config, prefix); + LOAD_PARAM(params.point_to_point_error_threshold, config, prefix); + LOAD_PARAM(params.reject_large_translation_norm, config, prefix); + LOAD_PARAM(params.pose_translation_norm_threshold, config, prefix); + LOAD_PARAM(params.max_num_points_between_factors, config, prefix); + params.body_T_sensor = lc::LoadTransform(config, camera_name + "_cam_transform"); +} + +void LoadRelativePoseFactorAdderParams(config_reader::ConfigReader& config, fa::RelativePoseFactorAdderParams& params, + const std::string& prefix) { + LoadFactorAdderParams(config, params, prefix); + LOAD_PARAM(params.covariance_scale, config, prefix); +} + +void LoadStandstillFactorAdderParams(config_reader::ConfigReader& config, fa::StandstillFactorAdderParams& params, + const std::string& prefix) { + LoadFactorAdderParams(config, params, prefix); + LOAD_PARAM(params.add_velocity_prior, config, prefix); + LOAD_PARAM(params.add_pose_between_factor, config, prefix); + LOAD_PARAM(params.prior_velocity_stddev, config, prefix); + LOAD_PARAM(params.pose_between_factor_translation_stddev, config, prefix); + LOAD_PARAM(params.pose_between_factor_rotation_stddev, config, prefix); +} + +void LoadVoSmartProjectionFactorAdderParams(config_reader::ConfigReader& config, + fa::VoSmartProjectionFactorAdderParams& params, const std::string& prefix) { + LoadFactorAdderParams(config, params, prefix); + LoadSpacedFeatureTrackerParams(config, params.spaced_feature_tracker, prefix); + LOAD_PARAM(params.max_num_factors, config, prefix); + LOAD_PARAM(params.min_num_points_per_factor, config, prefix); + LOAD_PARAM(params.max_num_points_per_factor, config, prefix); + LOAD_PARAM(params.min_avg_distance_from_mean, config, prefix); + LOAD_PARAM(params.robust, config, prefix); + LOAD_PARAM(params.rotation_only_fallback, config, prefix); + LOAD_PARAM(params.fix_invalid_factors, config, prefix); + LOAD_PARAM(params.scale_noise_with_num_points, config, prefix); + LOAD_PARAM(params.noise_scale, config, prefix); + // TODO(rsoussan): make not necessarily nav cam specific? + params.body_T_cam = lc::LoadTransform(config, "nav_cam_transform"); + params.cam_intrinsics.reset(new gtsam::Cal3_S2(lc::LoadCameraIntrinsics(config, "nav_cam"))); + params.cam_noise = gtsam::noiseModel::Isotropic::Sigma(2, mc::LoadDouble(config, "nav_cam_noise_stddev", prefix)); + LoadSmartProjectionParams(config, params.smart_factor, prefix); +} + +void LoadSmartProjectionParams(config_reader::ConfigReader& config, gtsam::SmartProjectionParams& params, + const std::string& prefix) { + // Load params + const bool rotation_only_fallback = mc::LoadBool(config, "rotation_only_fallback", prefix); + const bool enable_EPI = mc::LoadBool(config, "enable_EPI", prefix); + const double landmark_distance_threshold = mc::LoadDouble(config, "landmark_distance_threshold", prefix); + const double dynamic_outlier_rejection_threshold = + mc::LoadDouble(config, "dynamic_outlier_rejection_threshold", prefix); + const double retriangulation_threshold = mc::LoadDouble(config, "retriangulation_threshold", prefix); + const bool verbose_cheirality = mc::LoadBool(config, "verbose_cheirality", prefix); + + // Set Params + params.verboseCheirality = verbose_cheirality; + params.setRankTolerance(1e-9); + params.setLandmarkDistanceThreshold(landmark_distance_threshold); + params.setDynamicOutlierRejectionThreshold(dynamic_outlier_rejection_threshold); + params.setRetriangulationThreshold(retriangulation_threshold); + if (rotation_only_fallback) params.setDegeneracyMode(gtsam::DegeneracyMode::HANDLE_INFINITY); + params.setEnableEPI(enable_EPI); +} +} // namespace parameter_reader diff --git a/localization/parameter_reader/src/graph_localizer.cc b/localization/parameter_reader/src/graph_localizer.cc new file mode 100644 index 0000000000..3752fda401 --- /dev/null +++ b/localization/parameter_reader/src/graph_localizer.cc @@ -0,0 +1,40 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <parameter_reader/factor_adders.h> +#include <parameter_reader/graph_localizer.h> +#include <parameter_reader/node_adders.h> +#include <parameter_reader/optimizers.h> +#include <parameter_reader/sliding_window_graph_optimizer.h> +#include <msg_conversions/msg_conversions.h> + +namespace parameter_reader { +namespace gl = graph_localizer; +namespace mc = msg_conversions; + +void LoadGraphLocalizerParams(config_reader::ConfigReader& config, gl::GraphLocalizerParams& params, + const std::string& prefix) { + LoadLocFactorAdderParams(config, params.ar_tag_loc_factor_adder, prefix + "gl_fa_loc_ar_", "dock"); + LoadLocFactorAdderParams(config, params.sparse_map_loc_factor_adder, prefix + "gl_fa_loc_sm_", "nav"); + LoadPoseNodeAdderParams(config, params.pose_node_adder, prefix + "gl_na_pose_"); + LoadTimestampedNodeAdderModelParams(config, params.pose_node_adder_model, prefix + "gl_na_pose_model_"); + LoadNonlinearOptimizerParams(config, params.nonlinear_optimizer, prefix + "gl_op_nl_"); + LoadSlidingWindowGraphOptimizerParams(config, params.sliding_window_graph_optimizer, prefix + "gl_go_sw_"); + LOAD_PARAM(params.max_vio_measurement_gap, config, prefix + "gl_"); +} +} // namespace parameter_reader diff --git a/localization/parameter_reader/src/graph_optimizer.cc b/localization/parameter_reader/src/graph_optimizer.cc new file mode 100644 index 0000000000..0533724d07 --- /dev/null +++ b/localization/parameter_reader/src/graph_optimizer.cc @@ -0,0 +1,32 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <parameter_reader/graph_optimizer.h> +#include <msg_conversions/msg_conversions.h> + +namespace parameter_reader { +namespace go = graph_optimizer; +namespace mc = msg_conversions; + +void LoadGraphOptimizerParams(config_reader::ConfigReader& config, go::GraphOptimizerParams& params, + const std::string& prefix) { + LOAD_PARAM(params.huber_k, config, prefix); + LOAD_PARAM(params.log_stats_on_destruction, config, prefix); + LOAD_PARAM(params.print_after_optimization, config, prefix); +} +} // namespace parameter_reader diff --git a/localization/parameter_reader/src/graph_vio.cc b/localization/parameter_reader/src/graph_vio.cc new file mode 100644 index 0000000000..daf01b15fd --- /dev/null +++ b/localization/parameter_reader/src/graph_vio.cc @@ -0,0 +1,42 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <parameter_reader/factor_adders.h> +#include <parameter_reader/graph_vio.h> +#include <parameter_reader/node_adders.h> +#include <parameter_reader/optimizers.h> +#include <parameter_reader/sliding_window_graph_optimizer.h> +#include <parameter_reader/vision_common.h> +#include <msg_conversions/msg_conversions.h> + +namespace parameter_reader { +namespace gv = graph_vio; +namespace mc = msg_conversions; + +void LoadGraphVIOParams(config_reader::ConfigReader& config, gv::GraphVIOParams& params, const std::string& prefix) { + LoadDepthOdometryFactorAdderParams(config, params.depth_odometry_factor_adder, prefix + "gv_fa_do_", "haz"); + LoadStandstillFactorAdderParams(config, params.standstill_factor_adder, prefix + "gv_fa_standstill_"); + LoadVoSmartProjectionFactorAdderParams(config, params.vo_smart_projection_factor_adder, "gv_fa_vo_"); + LoadCombinedNavStateNodeAdderParams(config, params.combined_nav_state_node_adder, prefix + "gv_na_cns_"); + LoadCombinedNavStateNodeAdderModelParams(config, params.combined_nav_state_node_adder_model, + prefix + "gv_na_cns_model_"); + LoadNonlinearOptimizerParams(config, params.nonlinear_optimizer, prefix + "gv_op_nl_"); + LoadSlidingWindowGraphOptimizerParams(config, params.sliding_window_graph_optimizer, prefix + "gv_go_sw_"); + LoadStandstillParams(config, params.standstill, prefix + "gv_standstill_"); +} +} // namespace parameter_reader diff --git a/localization/parameter_reader/src/imu_integration.cc b/localization/parameter_reader/src/imu_integration.cc new file mode 100644 index 0000000000..a6fd7136db --- /dev/null +++ b/localization/parameter_reader/src/imu_integration.cc @@ -0,0 +1,54 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <parameter_reader/imu_integration.h> +#include <localization_common/utilities.h> +#include <msg_conversions/msg_conversions.h> + +#include <string> + +namespace parameter_reader { +namespace ii = imu_integration; +namespace lc = localization_common; +namespace mc = msg_conversions; + +void LoadImuFilterParams(config_reader::ConfigReader& config, ii::ImuFilterParams& params, const std::string& prefix) { + LOAD_PARAM(params.quiet_accel, config, prefix + "if_"); + LOAD_PARAM(params.quiet_ang_vel, config, prefix + "if_"); + LOAD_PARAM(params.nominal_accel, config, prefix + "if_"); + LOAD_PARAM(params.nominal_ang_vel, config, prefix + "if_"); + LOAD_PARAM(params.aggressive_accel, config, prefix + "if_"); + LOAD_PARAM(params.aggressive_ang_vel, config, prefix + "if_"); +} + +void LoadImuIntegratorParams(config_reader::ConfigReader& config, ii::ImuIntegratorParams& params, + const std::string& prefix) { + params.gravity = lc::LoadVector3(config, "world_gravity_vector", prefix); + // Nullify gravity factor if ignore_gravity set to true. + const bool ignore_gravity = mc::LoadBool(config, "ignore_gravity", prefix + "ii_"); + if (ignore_gravity) params.gravity = gtsam::Vector3::Zero(); + params.body_T_imu = lc::LoadTransform(config, "imu_transform"); + LoadImuFilterParams(config, params.filter); + LOAD_PARAM(params.gyro_sigma, config, prefix + "ii_"); + LOAD_PARAM(params.accel_sigma, config, prefix + "ii_"); + LOAD_PARAM(params.accel_bias_sigma, config, prefix + "ii_"); + LOAD_PARAM(params.gyro_bias_sigma, config, prefix + "ii_"); + LOAD_PARAM(params.integration_variance, config, prefix + "ii_"); + LOAD_PARAM(params.bias_acc_omega_int, config, prefix + "ii_"); +} +} // namespace parameter_reader diff --git a/localization/parameter_reader/src/node_adders.cc b/localization/parameter_reader/src/node_adders.cc new file mode 100644 index 0000000000..150d9ff6c8 --- /dev/null +++ b/localization/parameter_reader/src/node_adders.cc @@ -0,0 +1,60 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/utilities.h> +#include <msg_conversions/msg_conversions.h> +#include <parameter_reader/imu_integration.h> +#include <parameter_reader/node_adders.h> + +namespace parameter_reader { +namespace lc = localization_common; +namespace mc = msg_conversions; +namespace na = node_adders; + +void LoadTimestampedNodeAdderModelParams(config_reader::ConfigReader& config, + na::TimestampedNodeAdderModelParams& params, const std::string& prefix) { + LOAD_PARAM(params.huber_k, config, prefix); +} + +void LoadCombinedNavStateNodeAdderParams(config_reader::ConfigReader& config, + na::CombinedNavStateNodeAdder::Params& params, const std::string& prefix) { + // Note that starting measurement, timestamp, and nodes come from measurements. + // Starting noise models should be loaded from params. + LoadBaseTimestampedNodeAdderParams<lc::CombinedNavState>(config, params, prefix); +} + +void LoadCombinedNavStateNodeAdderModelParams(config_reader::ConfigReader& config, + na::CombinedNavStateNodeAdderModelParams& params, + const std::string& prefix) { + // Don't use prefix for IMU integrator params since these come from their own + // config file. + LoadImuIntegratorParams(config, params.imu_integrator); + LoadTimestampedNodeAdderModelParams(config, params, prefix); +} + +void LoadPoseNodeAdderParams(config_reader::ConfigReader& config, na::PoseNodeAdderParams& params, + const std::string& prefix) { + LOAD_PARAM(params.starting_prior_translation_stddev, config, prefix); + LOAD_PARAM(params.starting_prior_quaternion_stddev, config, prefix); + LOAD_PARAM(params.starting_prior_translation_stddev, config, prefix); + LoadBaseTimestampedNodeAdderParams<gtsam::Pose3>(config, params, prefix); + // Set start noise models after params have been loaded, since this relies on huber_k value + params.SetStartNoiseModels(); + // Load start node from measurements +} +} // namespace parameter_reader diff --git a/localization/parameter_reader/src/optimizers.cc b/localization/parameter_reader/src/optimizers.cc new file mode 100644 index 0000000000..b85da9517e --- /dev/null +++ b/localization/parameter_reader/src/optimizers.cc @@ -0,0 +1,42 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <parameter_reader/optimizers.h> +#include <msg_conversions/msg_conversions.h> + +namespace parameter_reader { +namespace mc = msg_conversions; +namespace op = optimizers; + +void LoadOptimizerParams(config_reader::ConfigReader& config, op::OptimizerParams& params, const std::string& prefix) { + LOAD_PARAM(params.marginals_factorization, config, prefix); +} + +void LoadISAM2OptimizerParams(config_reader::ConfigReader& config, op::ISAM2OptimizerParams& params, + const std::string& prefix) { + LoadOptimizerParams(config, params, prefix); +} + +void LoadNonlinearOptimizerParams(config_reader::ConfigReader& config, op::NonlinearOptimizerParams& params, + const std::string& prefix) { + LoadOptimizerParams(config, params, prefix); + LOAD_PARAM(params.max_iterations, config, prefix); + LOAD_PARAM(params.verbose, config, prefix); + LOAD_PARAM(params.use_ceres_params, config, prefix); +} +} // namespace parameter_reader diff --git a/localization/parameter_reader/src/sliding_window_graph_optimizer.cc b/localization/parameter_reader/src/sliding_window_graph_optimizer.cc new file mode 100644 index 0000000000..f4e302b9b7 --- /dev/null +++ b/localization/parameter_reader/src/sliding_window_graph_optimizer.cc @@ -0,0 +1,34 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <parameter_reader/graph_optimizer.h> +#include <parameter_reader/sliding_window_graph_optimizer.h> +#include <msg_conversions/msg_conversions.h> + +namespace parameter_reader { +namespace go = graph_optimizer; +namespace mc = msg_conversions; +namespace sw = sliding_window_graph_optimizer; + +void LoadSlidingWindowGraphOptimizerParams(config_reader::ConfigReader& config, + sw::SlidingWindowGraphOptimizerParams& params, const std::string& prefix) { + LoadGraphOptimizerParams(config, params, prefix); + LOAD_PARAM(params.add_marginal_factors, config, prefix); + LOAD_PARAM(params.slide_window_before_optimization, config, prefix); +} +} // namespace parameter_reader diff --git a/localization/parameter_reader/src/vision_common.cc b/localization/parameter_reader/src/vision_common.cc new file mode 100644 index 0000000000..32eeb715ac --- /dev/null +++ b/localization/parameter_reader/src/vision_common.cc @@ -0,0 +1,43 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <parameter_reader/vision_common.h> +#include <msg_conversions/msg_conversions.h> + +namespace parameter_reader { +namespace mc = msg_conversions; +namespace vc = vision_common; + +void LoadStandstillParams(config_reader::ConfigReader& config, vc::StandstillParams& params, + const std::string& prefix) { + LOAD_PARAM(params.min_num_points_per_track, config, prefix); + LOAD_PARAM(params.duration, config, prefix); + LOAD_PARAM(params.max_avg_distance_from_mean, config, prefix); +} + +void LoadFeatureTrackerParams(config_reader::ConfigReader& config, vc::FeatureTrackerParams& params, + const std::string& prefix) { + LOAD_PARAM(params.remove_undetected_feature_tracks, config, prefix); +} + +void LoadSpacedFeatureTrackerParams(config_reader::ConfigReader& config, vc::SpacedFeatureTrackerParams& params, + const std::string& prefix) { + LoadFeatureTrackerParams(config, params, prefix); + LOAD_PARAM(params.measurement_spacing, config, prefix); +} +} // namespace parameter_reader diff --git a/localization/parameter_reader/test/test_graph_localizer_parameter_reader.cc b/localization/parameter_reader/test/test_graph_localizer_parameter_reader.cc new file mode 100644 index 0000000000..c3c050e603 --- /dev/null +++ b/localization/parameter_reader/test/test_graph_localizer_parameter_reader.cc @@ -0,0 +1,125 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <parameter_reader/graph_localizer.h> +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <node_adders/utilities.h> + +#include <gtest/gtest.h> + +namespace gl = graph_localizer; +namespace lc = localization_common; +namespace na = node_adders; +namespace pr = parameter_reader; + +class GraphLocalizerParameterReaderTest : public ::testing::Test { + public: + GraphLocalizerParameterReaderTest() {} + + void SetUp() final { + lc::SetEnvironmentConfigs(); + config_reader::ConfigReader config; + lc::LoadGraphLocalizerConfig(config); + pr::LoadGraphLocalizerParams(config, params_); + } + + gl::GraphLocalizerParams params_; +}; + +TEST_F(GraphLocalizerParameterReaderTest, SparseMapLocFactorAdderParams) { + const auto& params = params_.sparse_map_loc_factor_adder; + EXPECT_EQ(params.enabled, true); + EXPECT_NEAR(params.huber_k, 1.345, 1e-6); + EXPECT_EQ(params.add_pose_priors, false); + EXPECT_EQ(params.add_projection_factors, true); + EXPECT_EQ(params.add_prior_if_projection_factors_fail, true); + EXPECT_NEAR(params.prior_translation_stddev, 0.06, 1e-6); + EXPECT_NEAR(params.prior_quaternion_stddev, 0.06, 1e-6); + EXPECT_EQ(params.scale_pose_noise_with_num_landmarks, false); + EXPECT_EQ(params.scale_projection_noise_with_num_landmarks, false); + EXPECT_EQ(params.scale_projection_noise_with_landmark_distance, false); + EXPECT_NEAR(params.pose_noise_scale, 0.001, 1e-6); + EXPECT_NEAR(params.projection_noise_scale, 1.5, 1e-6); + EXPECT_EQ(params.max_num_projection_factors, 25); + EXPECT_EQ(params.min_num_matches_per_measurement, 5); + EXPECT_NEAR(params.max_valid_projection_error, 30, 1e-6); + // Taken using current nav cam extrinsics + const gtsam::Pose3 expected_body_T_cam(gtsam::Rot3(0.500, 0.500, 0.500, 0.500), + gtsam::Point3(0.1177, -0.0422, -0.0826)); + EXPECT_MATRIX_NEAR(params.body_T_cam, expected_body_T_cam, 1e-6); + // Taken using current nav cam intrinsics, undistorted with no skew so only expected + // non-zero focal lengths + gtsam::Vector5 expected_intrinsics; + expected_intrinsics = (gtsam::Vector(5) << 608.8073, 607.61439, 0, 0, 0).finished(); + EXPECT_MATRIX_NEAR(params.cam_intrinsics->vector(), expected_intrinsics, 1e-6); + const double expected_sigma = dynamic_cast<gtsam::noiseModel::Isotropic*>(params.cam_noise.get())->sigma(); + EXPECT_NEAR(0.1, expected_sigma, 1e-6); +} + +TEST_F(GraphLocalizerParameterReaderTest, PoseNodeAdderParams) { + const auto& params = params_.pose_node_adder; + EXPECT_NEAR(params.starting_prior_translation_stddev, 0.02, 1e-6); + EXPECT_NEAR(params.starting_prior_quaternion_stddev, 0.01, 1e-6); + std::vector<gtsam::SharedNoiseModel> start_noise_models; + constexpr double kTranslationStddev = 0.02; + constexpr double kQuaternionStddev = 0.01; + const gtsam::Vector6 pose_prior_noise_sigmas((gtsam::Vector(6) << kTranslationStddev, kTranslationStddev, + kTranslationStddev, kQuaternionStddev, kQuaternionStddev, + kQuaternionStddev) + .finished()); + const auto pose_noise = lc::Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(pose_prior_noise_sigmas)), params.huber_k); + EXPECT_MATRIX_NEAR(na::Covariance(pose_noise), na::Covariance(params.start_noise_models[0]), 1e-6); + EXPECT_NEAR(params.huber_k, 1.345, 1e-6); + EXPECT_EQ(params.add_priors, true); + EXPECT_NEAR(params.ideal_duration, 4, 1e-6); + EXPECT_EQ(params.min_num_states, 3); + EXPECT_EQ(params.max_num_states, 5); +} + +TEST_F(GraphLocalizerParameterReaderTest, PoseNodeAdderModelParams) { + const auto& params = params_.pose_node_adder_model; + EXPECT_NEAR(params.huber_k, 1.345, 1e-6); +} + +TEST_F(GraphLocalizerParameterReaderTest, NonlinearOptimizerParams) { + const auto& params = params_.nonlinear_optimizer; + EXPECT_EQ(params.marginals_factorization, "qr"); + EXPECT_EQ(params.max_iterations, 10); + EXPECT_EQ(params.verbose, false); + EXPECT_EQ(params.use_ceres_params, false); +} + +TEST_F(GraphLocalizerParameterReaderTest, SlidingWindowGraphOptimizerParams) { + const auto& params = params_.sliding_window_graph_optimizer; + EXPECT_NEAR(params.huber_k, 1.345, 1e-6); + EXPECT_EQ(params.log_stats_on_destruction, false); + EXPECT_EQ(params.print_after_optimization, false); + EXPECT_EQ(params.add_marginal_factors, false); + EXPECT_EQ(params.slide_window_before_optimization, true); +} + +TEST_F(GraphLocalizerParameterReaderTest, OtherParams) { EXPECT_NEAR(params_.max_vio_measurement_gap, 3, 1e-6); } + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/parameter_reader/test/test_graph_localizer_parameter_reader.test b/localization/parameter_reader/test/test_graph_localizer_parameter_reader.test new file mode 100644 index 0000000000..02b7ef08f5 --- /dev/null +++ b/localization/parameter_reader/test/test_graph_localizer_parameter_reader.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="parameter_reader" type="test_graph_localizer_parameter_reader" test-name="test_graph_localizer_parameter_reader" /> +</launch> diff --git a/localization/parameter_reader/test/test_graph_vio_parameter_reader.cc b/localization/parameter_reader/test/test_graph_vio_parameter_reader.cc new file mode 100644 index 0000000000..d00ba6e7cf --- /dev/null +++ b/localization/parameter_reader/test/test_graph_vio_parameter_reader.cc @@ -0,0 +1,155 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <parameter_reader/graph_vio.h> +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <node_adders/utilities.h> + +#include <gtest/gtest.h> + +namespace lc = localization_common; +namespace gv = graph_vio; +namespace na = node_adders; +namespace pr = parameter_reader; + +class GraphVIOParameterReaderTest : public ::testing::Test { + public: + GraphVIOParameterReaderTest() {} + + void SetUp() final { + lc::SetEnvironmentConfigs(); + config_reader::ConfigReader config; + lc::LoadGraphVIOConfig(config); + pr::LoadGraphVIOParams(config, params_); + } + + gv::GraphVIOParams params_; +}; + +TEST_F(GraphVIOParameterReaderTest, StandstillFactorAdderParams) { + const auto& params = params_.standstill_factor_adder; + EXPECT_EQ(params.enabled, true); + EXPECT_NEAR(params.huber_k, 1.345, 1e-6); + EXPECT_EQ(params.add_velocity_prior, true); + EXPECT_EQ(params.add_pose_between_factor, true); + EXPECT_NEAR(params.prior_velocity_stddev, 0.01, 1e-6); + EXPECT_NEAR(params.pose_between_factor_translation_stddev, 0.01, 1e-6); + EXPECT_NEAR(params.pose_between_factor_rotation_stddev, 0.01, 1e-6); +} + +TEST_F(GraphVIOParameterReaderTest, VOFactorAdderParams) { + const auto& params = params_.vo_smart_projection_factor_adder; + EXPECT_EQ(params.enabled, true); + EXPECT_NEAR(params.huber_k, 1.345, 1e-6); + // Spaced Feature Tracker Params + EXPECT_EQ(params.spaced_feature_tracker.remove_undetected_feature_tracks, true); + EXPECT_EQ(params.spaced_feature_tracker.measurement_spacing, 2); + EXPECT_EQ(params.max_num_factors, 8); + EXPECT_EQ(params.min_num_points_per_factor, 2); + EXPECT_EQ(params.max_num_points_per_factor, 6); + EXPECT_NEAR(params.min_avg_distance_from_mean, 0.075, 1e-6); + EXPECT_EQ(params.robust, true); + EXPECT_EQ(params.rotation_only_fallback, true); + EXPECT_EQ(params.fix_invalid_factors, true); + EXPECT_EQ(params.scale_noise_with_num_points, true); + EXPECT_NEAR(params.noise_scale, 2.0, 1e-6); + // Taken using current nav cam extrinsics + const gtsam::Pose3 expected_body_T_cam(gtsam::Rot3(0.500, 0.500, 0.500, 0.500), + gtsam::Point3(0.1177, -0.0422, -0.0826)); + EXPECT_MATRIX_NEAR(params.body_T_cam, expected_body_T_cam, 1e-6); + // Taken using current nav cam intrinsics, undistorted with no skew so only expected + // non-zero focal lengths + gtsam::Vector5 expected_intrinsics; + expected_intrinsics = (gtsam::Vector(5) << 608.8073, 607.61439, 0, 0, 0).finished(); + EXPECT_MATRIX_NEAR(params.cam_intrinsics->vector(), expected_intrinsics, 1e-6); + const double expected_sigma = dynamic_cast<gtsam::noiseModel::Isotropic*>(params.cam_noise.get())->sigma(); + EXPECT_NEAR(0.1, expected_sigma, 1e-6); + // Smart Factor + EXPECT_NEAR(params.smart_factor.triangulation.rankTolerance, 1e-9, 1e-6); + EXPECT_NEAR(params.smart_factor.triangulation.landmarkDistanceThreshold, 500, 1e-6); + EXPECT_NEAR(params.smart_factor.triangulation.dynamicOutlierRejectionThreshold, 50, 1e-6); + EXPECT_EQ(params.smart_factor.triangulation.enableEPI, false); + EXPECT_EQ(params.smart_factor.verboseCheirality, false); + EXPECT_NEAR(params.smart_factor.retriangulationThreshold, 1e-5, 1e-6); + EXPECT_EQ(params.smart_factor.degeneracyMode, gtsam::DegeneracyMode::HANDLE_INFINITY); +} + +TEST_F(GraphVIOParameterReaderTest, CombinedNavStateNodeAdderParams) { + const auto& params = params_.combined_nav_state_node_adder; + EXPECT_NEAR(params.huber_k, 1.345, 1e-6); + EXPECT_EQ(params.add_priors, true); + EXPECT_NEAR(params.ideal_duration, 3.25, 1e-6); + EXPECT_EQ(params.min_num_states, 3); + EXPECT_EQ(params.max_num_states, 8); +} + +TEST_F(GraphVIOParameterReaderTest, CombinedNavStateNodeAdderModelParams) { + const auto& params = params_.combined_nav_state_node_adder_model; + EXPECT_NEAR(params.huber_k, 1.345, 1e-6); + // IMU Integrator + EXPECT_MATRIX_NEAR(params.imu_integrator.gravity, Eigen::Vector3d::Zero(), 1e-6); + // Taken using current nav cam extrinsics + const gtsam::Pose3 expected_body_T_imu(gtsam::Rot3(0.70710678118, 0, 0, 0.70710678118), + gtsam::Point3(0.0386, 0.0247, -0.01016)); + EXPECT_MATRIX_NEAR(params.imu_integrator.body_T_imu, expected_body_T_imu, 1e-6); + EXPECT_NEAR(params.imu_integrator.gyro_sigma, 0.00001, 1e-6); + EXPECT_NEAR(params.imu_integrator.accel_sigma, 0.0005, 1e-6); + EXPECT_NEAR(params.imu_integrator.accel_bias_sigma, 0.0005, 1e-6); + EXPECT_NEAR(params.imu_integrator.gyro_bias_sigma, 0.0000035, 1e-6); + EXPECT_NEAR(params.imu_integrator.integration_variance, 0.0001, 1e-6); + EXPECT_NEAR(params.imu_integrator.bias_acc_omega_int, 0.00015, 1e-6); + // IMU filter + EXPECT_EQ(params.imu_integrator.filter.quiet_accel, "ButterO3S125Lp3N33_33"); + EXPECT_EQ(params.imu_integrator.filter.quiet_ang_vel, "ButterO1S125Lp3N33_33"); + EXPECT_EQ(params.imu_integrator.filter.nominal_accel, "ButterO3S125Lp3N41_66"); + EXPECT_EQ(params.imu_integrator.filter.nominal_ang_vel, "ButterO1S125Lp3N41_66"); + EXPECT_EQ(params.imu_integrator.filter.aggressive_accel, "ButterO3S125Lp3N46_66"); + EXPECT_EQ(params.imu_integrator.filter.aggressive_ang_vel, "ButterO1S125Lp3N46_66"); +} + +TEST_F(GraphVIOParameterReaderTest, NonlinearOptimizerParams) { + const auto& params = params_.nonlinear_optimizer; + EXPECT_EQ(params.marginals_factorization, "qr"); + EXPECT_EQ(params.max_iterations, 6); + EXPECT_EQ(params.verbose, false); + EXPECT_EQ(params.use_ceres_params, false); +} + +TEST_F(GraphVIOParameterReaderTest, SlidingWindowGraphOptimizerParams) { + const auto& params = params_.sliding_window_graph_optimizer; + EXPECT_NEAR(params.huber_k, 1.345, 1e-6); + EXPECT_EQ(params.log_stats_on_destruction, false); + EXPECT_EQ(params.print_after_optimization, false); + EXPECT_EQ(params.add_marginal_factors, false); + EXPECT_EQ(params.slide_window_before_optimization, true); +} + +TEST_F(GraphVIOParameterReaderTest, StandstillParams) { + const auto& params = params_.standstill; + EXPECT_EQ(params.min_num_points_per_track, 4); + EXPECT_NEAR(params.duration, 1, 1e-6); + EXPECT_NEAR(params.max_avg_distance_from_mean, 0.075, 1e-6); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/parameter_reader/test/test_graph_vio_parameter_reader.test b/localization/parameter_reader/test/test_graph_vio_parameter_reader.test new file mode 100644 index 0000000000..f92f0e5bcb --- /dev/null +++ b/localization/parameter_reader/test/test_graph_vio_parameter_reader.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="parameter_reader" type="test_graph_vio_parameter_reader" test-name="test_graph_vio_parameter_reader" /> +</launch> diff --git a/localization/readme.md b/localization/readme.md index 0e0f79d524..6fd4b3c974 100644 --- a/localization/readme.md +++ b/localization/readme.md @@ -11,42 +11,83 @@ See the following publications for more details: If you are using the localizer or mapping pipeline in academic work, please cite the relevant publications above. +# AstroLoc Library Quickstart +See [Quickstart](doc/astroloc_library_quickstart.pdf) for a tutorial on using the AstroLoc library and an example simple localizer. + +# Localizer Implementations +## GraphVIO +The graph_vio package performs VIO using image-based feature track measurements and estimates pose, velocity, and IMU bias values at each timestamp. + +<p align="center"> +<img src="./doc/images/graph_vio.png" width="550"> +</p> + +## GraphLocalizer +The graph_localizer package uses relative odometry measurements along with map-based localization measurements to perform localization for poses at each timestamp. + + +<p align="center"> +<img src="./doc/images/graph_localizer.png" width="550"> +</p> + +## Ros Wrappers +The ros_graph_localizer and ros_graph_vio packages wrap GraphVIO and GraphLocalizer objects with ROS for live or offline usage with ROS message types, and the ros_pose_extrapolator performs extrapolation of localization poses using relative odometry and interpolated IMU data. + +# Packages \subpage camera \subpage depthodometry +\subpage factoradders + +\subpage graphfactors + \subpage graphlocalizer \subpage graphoptimizer +\subpage graphvio + \subpage groundtruthlocalizer \subpage handrail -\subpage imuaugmentor - \subpage imuintegration \subpage interestpoint \subpage localizationcommon -\subpage localization_manager +\subpage localizationmanager \subpage localizationmeasurements -\subpage localization_node +\subpage localizationnode + +\subpage markertracking + +\subpage nodeadders + +\subpage nodes \subpage opticalflow \subpage optimizationcommon +\subpage optimizers + \subpage pointcloudcommon -\subpage markertracking +\subpage rosgraphlocalizer + +\subpage rosgraphvio + +\subpage rosposeextrapolator + +\subpage slidingwindowgraphoptimizer \subpage sparsemapping \subpage visioncommon -\subpage vive_localization +\subpage vivelocalization diff --git a/localization/ros_graph_localizer/CMakeLists.txt b/localization/ros_graph_localizer/CMakeLists.txt new file mode 100644 index 0000000000..3ab8bacca1 --- /dev/null +++ b/localization/ros_graph_localizer/CMakeLists.txt @@ -0,0 +1,120 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(ros_graph_localizer) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + camera + config_reader + depth_odometry + ff_util + ff_msgs + graph_localizer + localization_common + localization_measurements + msg_conversions + nodelet + parameter_reader + ros_graph_vio +) + +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS + camera + config_reader + depth_odometry + ff_util + ff_msgs + graph_localizer + localization_common + localization_measurements + msg_conversions + nodelet + parameter_reader + ros_graph_vio +) + +########### +## Build ## +########### +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +file(GLOB cc_files + "src/*.cc" +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + ${cc_files} +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_ros_graph_localizer_parameter_reader + test/test_ros_graph_localizer_parameter_reader.test + test/test_ros_graph_localizer_parameter_reader.cc + ) + target_link_libraries(test_ros_graph_localizer_parameter_reader + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) + diff --git a/localization/ros_graph_localizer/TODO.txt b/localization/ros_graph_localizer/TODO.txt new file mode 100644 index 0000000000..c03b0f4d83 --- /dev/null +++ b/localization/ros_graph_localizer/TODO.txt @@ -0,0 +1,55 @@ + - send pose/cov history from vio, add all poses/covs in pose node (update old values) (C) + - add support for this in timestamped_set? (updating old values) + + - add grah_vio! (A) + - inherit from sliding window optimizer + - add combined imu node adder, vo and standstill factor adders + - publish odometry message! + - add new message type? + - convert velocity from odom frame to global frame?? + - is velocity in global frame or body frame?? + - add better way of getting relative covariance?? + + - add graph_localizer! (C) + - use pose node updater with odometry message! + - add loc factor adder! + + - add parameter reader! (G) + - fail if parameter not read!! + - how to unit test these?? + + isam2 optimizer issues! + - need to account for values removed when sliding window! + - since these have keys, just check for removed keys! + - need to account for factors removed when sliding window! + (i.e. smart factors!) + - since these are pts, just make sets of ptr values and check for new and removed ones!!! + + - add ordering back to sliding window optimizer now that window is slid after optimization! + + - TODO: implement others! + + + +- TODO: how to access opt iterations???? +- move serialization.cc back to graph optimizer? + - make graph optimizer depend on graph_Factors? + - add test with graph optimizer, simple factor adder, and simple node updater!! + +- add graph vio to graph bag! + - run_graph_Bag, plot_graph_bag + - use graph bag???? + - copy graph bag and run for VIO only?? Or modify current graph bag tool?? + - (probably modify current tool?) +- how to add a second measurement to combined nav node update model????? (AE) + - why is this needed??? + - fan rate!!!!!! + - add to imu measurement??? +- move param loading and serialization from graph_vio/loc to respective packages! +- initialize graph with identity pose, set fixed!!! + - TODO: need to always have fixed identity pose as origin! how to do this? + - set start pose fixed after maginalize???? +- rename pimpredict in other packages to extrapolate + - tools: loc rvz plugins, imu bias tester +- update imu_augmentor, make sure it compiles! + - add tests? diff --git a/localization/ros_graph_localizer/include/ros_graph_localizer/parameter_reader.h b/localization/ros_graph_localizer/include/ros_graph_localizer/parameter_reader.h new file mode 100644 index 0000000000..d1657aab08 --- /dev/null +++ b/localization/ros_graph_localizer/include/ros_graph_localizer/parameter_reader.h @@ -0,0 +1,34 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef ROS_GRAPH_LOCALIZER_PARAMETER_READER_H_ +#define ROS_GRAPH_LOCALIZER_PARAMETER_READER_H_ + +#include <config_reader/config_reader.h> +#include <ros_graph_localizer/ros_graph_localizer_nodelet_params.h> +#include <ros_graph_localizer/ros_graph_localizer_wrapper_params.h> + +#include <string> + +namespace ros_graph_localizer { +void LoadRosGraphLocalizerWrapperParams(config_reader::ConfigReader& config, RosGraphLocalizerWrapperParams& params, + const std::string& prefix = ""); +void LoadRosGraphLocalizerNodeletParams(config_reader::ConfigReader& config, RosGraphLocalizerNodeletParams& params, + const std::string& prefix = ""); +} // namespace ros_graph_localizer + +#endif // ROS_GRAPH_LOCALIZER_PARAMETER_READER_H_ diff --git a/localization/ros_graph_localizer/include/ros_graph_localizer/ros_graph_localizer_nodelet.h b/localization/ros_graph_localizer/include/ros_graph_localizer/ros_graph_localizer_nodelet.h new file mode 100644 index 0000000000..ec4de78dff --- /dev/null +++ b/localization/ros_graph_localizer/include/ros_graph_localizer/ros_graph_localizer_nodelet.h @@ -0,0 +1,159 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef ROS_GRAPH_LOCALIZER_ROS_GRAPH_LOCALIZER_NODELET_H_ +#define ROS_GRAPH_LOCALIZER_ROS_GRAPH_LOCALIZER_NODELET_H_ + +#include <depth_odometry/depth_odometry_wrapper.h> +#include <ff_msgs/GraphVIOState.h> +#include <ff_msgs/VisualLandmarks.h> +#include <ff_msgs/Heartbeat.h> +#include <ff_msgs/ResetMap.h> +#include <ff_msgs/SetEkfInput.h> +#include <ff_util/ff_nodelet.h> +#include <localization_common/ros_timer.h> +#include <localization_common/timer.h> +#include <ros_graph_localizer/ros_graph_localizer_nodelet_params.h> +#include <ros_graph_localizer/ros_graph_localizer_wrapper.h> +#include <ros_graph_vio/ros_graph_vio_wrapper.h> + +#include <image_transport/image_transport.h> +#include <ros/node_handle.h> +#include <ros/publisher.h> +#include <ros/subscriber.h> +#include <sensor_msgs/PointCloud2.h> +#include <std_srvs/Empty.h> +#include <tf2_ros/transform_broadcaster.h> + +#include <string> +#include <vector> + +namespace ros_graph_localizer { +class RosGraphLocalizerNodelet : public ff_util::FreeFlyerNodelet { + public: + RosGraphLocalizerNodelet(); + + private: + // Subscribes to and advertises topics. Calls Run() to start processing loop. + void Initialize(ros::NodeHandle* nh) final; + + // Subscribes to and advertises topics. + void SubscribeAndAdvertise(ros::NodeHandle* nh); + + // Set mode for Localizer. Disables if mode is none, resets and enables if swtiched from none. + bool SetMode(ff_msgs::SetEkfInput::Request& req, ff_msgs::SetEkfInput::Response& res); + + // Disabled Localizer and VIO. Prevents any messages from being added, halts publishing + // messages, and halts updating Localizer and VIO. + void DisableLocalizerAndVIO(); + + // Enables Localizer and VIO. + void EnableLocalizerAndVIO(); + + // Whether Localizer and VIO are enabled. + bool localizer_and_vio_enabled() const; + + // Resets VIO using re-estimation of biases and resets the localizer. + bool ResetBiasesAndLocalizer(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + + // Resets VIO using biases in file (if available) and resets the localizer. + bool ResetBiasesFromFileAndResetLocalizer(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + + // Resets VIO using biases in file (if available) and resets the localizer. + bool ResetBiasesFromFileAndResetLocalizer(); + + // Resets the localizer. + void ResetAndEnableLocalizer(); + + // Resets the localizer and clears the sparse map matched projections buffer + // so old measurements aren't used with a new map. + bool ResetMap(ff_msgs::ResetMap::Request& req, ff_msgs::ResetMap::Response& res); + + // Publish latest graph localizer state msg. + void PublishGraphLocalizerState(); + + // Publishes empty reset message. + void PublishReset() const; + + // Publishes Loc pose message and other graph messages if Localizer is enabled. + void PublishGraphLocalizerMessages(); + + // Publishes heartbeat message. + void PublishHeartbeat(); + + // Publishes world_T_body transform + void PublishWorldTBodyTF(); + + // Publishes world_T_dock transform + void PublishWorldTDockTF(); + + // Passes ar tag visual landmarks msg to ros_graph_localizer_wrapper if Localizer is enabled. + void ARVisualLandmarksCallback(const ff_msgs::VisualLandmarks::ConstPtr& visual_landmarks_msg); + + // Passes sparse map visual landmarks msg to ros_graph_localizer_wrapper if Localizer is enabled. + void SparseMapVisualLandmarksCallback(const ff_msgs::VisualLandmarks::ConstPtr& visual_landmarks_msg); + + // Passes feature points msg to ros_graph_vio_wrapper if VIO is enabled. + void FeaturePointsCallback(const ff_msgs::Feature2dArray::ConstPtr& feature_array_msg); + + // Passes depth odometry msg to ros_graph_vio_wrapper if VIO is enabled. + void DepthOdometryCallback(const ff_msgs::DepthOdometry::ConstPtr& depth_odom_msg); + + // Passes depth point cloud msg to depth_odometry_wrapper if VIO is enabled. + void DepthPointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg); + + // Passes depth image msg to depth_odometry_wrapper if VIO is enabled. + void DepthImageCallback(const sensor_msgs::ImageConstPtr& image_msg); + + // Passes IMU msg to ros_graph_vio_wrapper if VIO is enabled. + void ImuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg); + + // Passes flight mode msg to ros_graph_vio_wrapper if VIO is enabled. + void FlightModeCallback(ff_msgs::FlightMode::ConstPtr const& mode); + + // Adds messages to ros_graph_localizer_wrapper from callback queue, updates + // the ros_graph_localizer_wrapper, and pubishes messages. + // Runs iteratively on start up at a max 100Hz rate. + void Run(); + + ros_graph_localizer::RosGraphLocalizerWrapper ros_graph_localizer_wrapper_; + ros_graph_vio::RosGraphVIOWrapper ros_graph_vio_wrapper_; + depth_odometry::DepthOdometryWrapper depth_odometry_wrapper_; + ros::NodeHandle private_nh_; + ros::CallbackQueue private_queue_; + bool localizer_and_vio_enabled_ = true; + ros::Subscriber sparse_map_vl_sub_, ar_tag_vl_sub_; + ros::Publisher graph_loc_pub_, reset_pub_, heartbeat_pub_; + tf2_ros::TransformBroadcaster transform_pub_; + ros::ServiceServer bias_srv_, bias_from_file_srv_, reset_map_srv_, reset_srv_, input_mode_srv_; + std::string platform_name_; + ff_msgs::Heartbeat heartbeat_; + RosGraphLocalizerNodeletParams params_; + int last_mode_ = -1; + + ros::Time last_heartbeat_time_; + ros::Time last_tf_body_time_; + ros::Time last_tf_dock_time_; + + // VIO + ros::Publisher graph_vio_state_pub_, graph_vio_pub_, depth_odom_pub_; + ros::Subscriber imu_sub_, depth_point_cloud_sub_, depth_odom_sub_, fp_sub_, flight_mode_sub_; + image_transport::Subscriber depth_image_sub_; +}; +} // namespace ros_graph_localizer + +#endif // ROS_GRAPH_LOCALIZER_ROS_GRAPH_LOCALIZER_NODELET_H_ diff --git a/localization/ros_graph_localizer/include/ros_graph_localizer/ros_graph_localizer_nodelet_params.h b/localization/ros_graph_localizer/include/ros_graph_localizer/ros_graph_localizer_nodelet_params.h new file mode 100644 index 0000000000..c98efa8350 --- /dev/null +++ b/localization/ros_graph_localizer/include/ros_graph_localizer/ros_graph_localizer_nodelet_params.h @@ -0,0 +1,40 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef ROS_GRAPH_LOCALIZER_ROS_GRAPH_LOCALIZER_NODELET_PARAMS_H_ +#define ROS_GRAPH_LOCALIZER_ROS_GRAPH_LOCALIZER_NODELET_PARAMS_H_ + +namespace ros_graph_localizer { +struct RosGraphLocalizerNodeletParams { + int max_graph_vio_state_buffer_size; + int max_vl_matched_projections_buffer_size; + int max_ar_tag_matched_projections_buffer_size; + int max_imu_buffer_size; + int max_feature_point_buffer_size; + int max_depth_odom_buffer_size; + int max_depth_image_buffer_size; + int max_depth_cloud_buffer_size; + // Run depth odometry in nodelet + bool run_depth_odometry; + // Receive depth odometry messages from external nodelet + bool subscribe_to_depth_odometry; + // Publish depth odometry message if running in nodelet + bool publish_depth_odometry; +}; +} // namespace ros_graph_localizer + +#endif // ROS_GRAPH_LOCALIZER_ROS_GRAPH_LOCALIZER_NODELET_PARAMS_H_ diff --git a/localization/ros_graph_localizer/include/ros_graph_localizer/ros_graph_localizer_wrapper.h b/localization/ros_graph_localizer/include/ros_graph_localizer/ros_graph_localizer_wrapper.h new file mode 100644 index 0000000000..65fc1b06c0 --- /dev/null +++ b/localization/ros_graph_localizer/include/ros_graph_localizer/ros_graph_localizer_wrapper.h @@ -0,0 +1,118 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef ROS_GRAPH_LOCALIZER_ROS_GRAPH_LOCALIZER_WRAPPER_H_ +#define ROS_GRAPH_LOCALIZER_ROS_GRAPH_LOCALIZER_WRAPPER_H_ + +#include <ff_msgs/EkfState.h> +#include <ff_msgs/Feature2dArray.h> +#include <ff_msgs/FlightMode.h> +#include <ff_msgs/GraphVIOState.h> +#include <ff_msgs/GraphLocState.h> +#include <ff_msgs/VisualLandmarks.h> +// #include <ff_msgs/SerializedGraph.h> +#include <graph_localizer/graph_localizer.h> +#include <imu_integration/imu_integrator.h> +#include <localization_measurements/fan_speed_mode.h> +#include <localization_measurements/imu_measurement.h> +#include <localization_common/pose_interpolater.h> +#include <localization_common/timestamped_set.h> +#include <ros_graph_localizer/ros_graph_localizer_wrapper_params.h> + +#include <sensor_msgs/Imu.h> + +#include <memory> +#include <string> + +namespace ros_graph_localizer { +// Converts ROS messages and passes these to the GraphLocalizer graph. +// Creates msgs from pose nodes in the graph. +// Waits until a valid map-match is received to initialize the localizer. +class RosGraphLocalizerWrapper { + public: + explicit RosGraphLocalizerWrapper(const std::string& graph_config_path_prefix = ""); + + // Load configs for graph_localizer + void LoadConfigs(const std::string& graph_config_path_prefix); + + // Store IMU msgs for world_T_dock estimation + void ImuCallback(const sensor_msgs::Imu& imu_msg); + + // Add flight mode msg to IMU filter. + void FlightModeCallback(const ff_msgs::FlightMode& flight_mode); + + // Add sparse map visual landmarks msg to graph_localizer. + void SparseMapVisualLandmarksCallback(const ff_msgs::VisualLandmarks& visual_landmarks_msg); + + // Add AR tag visual landmarks msg to graph_localizer. + void ARVisualLandmarksCallback(const ff_msgs::VisualLandmarks& visual_landmarks_msg); + + // Add graph vio state to graph localizer. + bool GraphVIOStateCallback(const ff_msgs::GraphVIOState& graph_vio_state_msg); + + // Updates the graph_localizer if it has been initialized. + void Update(); + + // Returns whether the graph_localizer has been initialized. + bool Initialized() const; + + // Resets the graph. + void ResetLocalizer(); + + // Resets the world_T_dock frame relative transform. + void ResetWorldTDock(); + + // Returns the latest timestamp in the graph if it exists. + boost::optional<localization_common::Time> LatestTimestamp() const; + + // Returns the latest pose in the graph if it exists. + boost::optional<gtsam::Pose3> LatestPose() const; + + // Returns latest world_T_dock if it exists. + boost::optional<gtsam::Pose3> WorldTDock() const; + + // Creates graph loc state msg using latest pose and graph information + // in graph localizer. + // Returns boost::none if no state is available or no changes have occured since last msg. + boost::optional<ff_msgs::GraphLocState> GraphLocStateMsg(); + + // Accessor to graph localizer + std::unique_ptr<graph_localizer::GraphLocalizer>& graph_localizer(); + + // Const accessor to graph localizer + const std::unique_ptr<graph_localizer::GraphLocalizer>& graph_localizer() const; + + private: + // Initialize the graph + void Initialize(); + + std::unique_ptr<graph_localizer::GraphLocalizer> graph_localizer_; + std::unique_ptr<imu_integration::ImuIntegrator> imu_integrator_; + localization_common::PoseInterpolater odom_interpolator_; + graph_localizer::GraphLocalizerParams params_; + RosGraphLocalizerWrapperParams wrapper_params_; + localization_common::TimestampedSet<ff_msgs::GraphVIOState> vio_measurement_buffer_; + boost::optional<localization_common::CombinedNavState> latest_vio_state_; + boost::optional<localization_common::Time> latest_msg_time_; + boost::optional<localization_common::Time> last_vio_msg_time_; + boost::optional<gtsam::Pose3> world_T_dock_; + int latest_num_detected_ml_features_; + int latest_num_detected_ar_features_; +}; +} // namespace ros_graph_localizer + +#endif // ROS_GRAPH_LOCALIZER_ROS_GRAPH_LOCALIZER_WRAPPER_H_ diff --git a/localization/ros_graph_localizer/include/ros_graph_localizer/ros_graph_localizer_wrapper_params.h b/localization/ros_graph_localizer/include/ros_graph_localizer/ros_graph_localizer_wrapper_params.h new file mode 100644 index 0000000000..e36ae43c2f --- /dev/null +++ b/localization/ros_graph_localizer/include/ros_graph_localizer/ros_graph_localizer_wrapper_params.h @@ -0,0 +1,45 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef ROS_GRAPH_LOCALIZER_ROS_GRAPH_LOCALIZER_WRAPPER_PARAMS_H_ +#define ROS_GRAPH_LOCALIZER_ROS_GRAPH_LOCALIZER_WRAPPER_PARAMS_H_ + +#include <imu_integration/imu_integrator_params.h> + +#include <boost/serialization/serialization.hpp> + +namespace ros_graph_localizer { +struct RosGraphLocalizerWrapperParams { + imu_integration::ImuIntegratorParams imu_integrator; + bool extrapolate_dock_pose_with_imu; + int max_relative_vio_buffer_size; + double max_duration_between_vl_msgs; + + private: + // Serialization function + friend class boost::serialization::access; + template <class ARCHIVE> + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(imu_integrator); + ar& BOOST_SERIALIZATION_NVP(extrapolate_dock_pose_with_imu); + ar& BOOST_SERIALIZATION_NVP(max_relative_vio_buffer_size); + ar& BOOST_SERIALIZATION_NVP(max_duration_between_vl_msgs); + } +}; +} // namespace ros_graph_localizer + +#endif // ROS_GRAPH_LOCALIZER_ROS_GRAPH_LOCALIZER_WRAPPER_PARAMS_H_ diff --git a/localization/graph_localizer/launch/graph_localizer.launch b/localization/ros_graph_localizer/launch/ros_graph_localizer.launch similarity index 92% rename from localization/graph_localizer/launch/graph_localizer.launch rename to localization/ros_graph_localizer/launch/ros_graph_localizer.launch index c208161605..05a8fd6f53 100644 --- a/localization/graph_localizer/launch/graph_localizer.launch +++ b/localization/ros_graph_localizer/launch/ros_graph_localizer.launch @@ -15,13 +15,13 @@ <!-- implied. See the License for the specific language governing --> <!-- permissions and limitations under the License. --> <launch> - <arg name="name" default="graph_localizer" /> + <arg name="name" default="ros_graph_localizer" /> <arg name="manager" default="" /> <arg name="terminal" default="" /> <include file="$(find ff_util)/launch/ff_nodelet.launch"> <arg name="name" value="$(arg name)" /> <arg name="manager" value="$(arg manager)" /> <arg name="terminal" value="$(arg terminal)" /> - <arg name="class" value="graph_localizer/GraphLocalizerNodelet" /> + <arg name="class" value="ros_graph_localizer/RosGraphLocalizerNodelet" /> </include> </launch> diff --git a/localization/ros_graph_localizer/nodelet_plugins.xml b/localization/ros_graph_localizer/nodelet_plugins.xml new file mode 100644 index 0000000000..c8539ffa83 --- /dev/null +++ b/localization/ros_graph_localizer/nodelet_plugins.xml @@ -0,0 +1,11 @@ +<library path="lib/libros_graph_localizer"> + <class name="ros_graph_localizer/RosGraphLocalizerNodelet" + type="ros_graph_localizer::RosGraphLocalizerNodelet" + base_class_type="nodelet::Nodelet"> + <description> + This nodelet wraps the graph localizer and graph vio + </description> + </class> +</library> + + diff --git a/localization/ros_graph_localizer/package.xml b/localization/ros_graph_localizer/package.xml new file mode 100644 index 0000000000..6ec3b75b71 --- /dev/null +++ b/localization/ros_graph_localizer/package.xml @@ -0,0 +1,54 @@ +<package> + <name>ros_graph_localizer</name> + <version>1.0.0</version> + <description> + The graph localizer package + </description> + <license> + Apache License, Version 2.0 + </license> + <author email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </author> + <maintainer email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </maintainer> + <buildtool_depend>catkin</buildtool_depend> + <build_depend>nodelet</build_depend> + <build_depend>camera</build_depend> + <build_depend>config_reader</build_depend> + <build_depend>depth_odometry</build_depend> + <build_depend>factor_adders</build_depend> + <build_depend>ff_util</build_depend> + <build_depend>ff_msgs</build_depend> + <build_depend>graph_factors</build_depend> + <build_depend>graph_localizer</build_depend> + <build_depend>imu_integration</build_depend> + <build_depend>localization_common</build_depend> + <build_depend>localization_measurements</build_depend> + <build_depend>msg_conversions</build_depend> + <build_depend>node_updaters</build_depend> + <build_depend>parameter_reader</build_depend> + <build_depend>ros_graph_vio</build_depend> + <build_depend>vision_common</build_depend> + <run_depend>nodelet</run_depend> + <run_depend>camera</run_depend> + <run_depend>config_reader</run_depend> + <run_depend>depth_odometry</run_depend> + <run_depend>factor_adders</run_depend> + <run_depend>ff_util</run_depend> + <run_depend>ff_msgs</run_depend> + <run_depend>graph_factors</run_depend> + <run_depend>graph_localizer</run_depend> + <run_depend>imu_integration</run_depend> + <run_depend>localization_common</run_depend> + <run_depend>localization_measurements</run_depend> + <run_depend>msg_conversions</run_depend> + <run_depend>node_updaters</run_depend> + <run_depend>parameter_reader</run_depend> + <run_depend>ros_graph_vio</run_depend> + <run_depend>vision_common</run_depend> + <export> + <nodelet plugin="${prefix}/nodelet_plugins.xml" /> + </export> +</package> diff --git a/localization/ros_graph_localizer/readme.md b/localization/ros_graph_localizer/readme.md new file mode 100644 index 0000000000..7df1ac7861 --- /dev/null +++ b/localization/ros_graph_localizer/readme.md @@ -0,0 +1,26 @@ +\page rosgraphlocalizer Ros Graph Localizer + +# Package Overview +Ros wrapper for the graph localizer. The graph localizer wrapper takes ros messages, convers these to localization measurements, and passes these to the graph localizer. The ros graph localizer nodelet handles live subscribing and publishing to ros topics. It runs the ros_graph_localizer, ros_graph_vio, and depth_odometry correspondence estimation sequential and passes input messages to each of these. + +## RosGraphLocalizerWrapper +Contains the GraphLocalizer. Converts ROS messages to localization measurements and provides these to the localizer. + +## RosGraphVIONodelet +Subscribes to ROS messages for online use and publishes GraphVIO messages and TFs. Contains a RosGraphLocalizerWrapper and passes messages to this. + +# Inputs +* `/hw/depth_haz/extended/amplitude_int` +* `/hw/depth_haz/points` +* `/hw/imu` +* `/loc/ar/features` +* `/loc/of/features` +* `/loc/ml/features` +* `/loc/depth/odom` +* `/loc/depth/odom` +* `/mob/flight_mode` + +# Outputs +* `/graph_loc/state` +* `/graph_vio/state` +* `/loc/depth/odom` diff --git a/localization/ros_graph_localizer/src/parameter_reader.cc b/localization/ros_graph_localizer/src/parameter_reader.cc new file mode 100644 index 0000000000..474fc3a56c --- /dev/null +++ b/localization/ros_graph_localizer/src/parameter_reader.cc @@ -0,0 +1,49 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <msg_conversions/msg_conversions.h> +#include <parameter_reader/imu_integration.h> +#include <ros_graph_localizer/parameter_reader.h> + +namespace ros_graph_localizer { +namespace mc = msg_conversions; +namespace pr = parameter_reader; + +void LoadRosGraphLocalizerWrapperParams(config_reader::ConfigReader& config, RosGraphLocalizerWrapperParams& params, + const std::string& prefix) { + pr::LoadImuIntegratorParams(config, params.imu_integrator); + LOAD_PARAM(params.extrapolate_dock_pose_with_imu, config, "rgl_" + prefix); + LOAD_PARAM(params.max_relative_vio_buffer_size, config, "rgl_" + prefix); + LOAD_PARAM(params.max_duration_between_vl_msgs, config, "rgl_" + prefix); +} + +void LoadRosGraphLocalizerNodeletParams(config_reader::ConfigReader& config, RosGraphLocalizerNodeletParams& params, + const std::string& prefix) { + LOAD_PARAM(params.max_graph_vio_state_buffer_size, config, "rgl_" + prefix); + LOAD_PARAM(params.max_vl_matched_projections_buffer_size, config, "rgl_" + prefix); + LOAD_PARAM(params.max_ar_tag_matched_projections_buffer_size, config, "rgl_" + prefix); + LOAD_PARAM(params.max_imu_buffer_size, config, prefix + "rgl_"); + LOAD_PARAM(params.max_feature_point_buffer_size, config, prefix + "rgl_"); + LOAD_PARAM(params.max_depth_odom_buffer_size, config, prefix + "rgl_"); + LOAD_PARAM(params.max_depth_image_buffer_size, config, prefix + "rgl_"); + LOAD_PARAM(params.max_depth_cloud_buffer_size, config, prefix + "rgl_"); + LOAD_PARAM(params.run_depth_odometry, config, prefix + "rgl_"); + LOAD_PARAM(params.publish_depth_odometry, config, prefix + "rgl_"); + LOAD_PARAM(params.subscribe_to_depth_odometry, config, prefix + "rgl_"); +} +} // namespace ros_graph_localizer diff --git a/localization/ros_graph_localizer/src/ros_graph_localizer_nodelet.cc b/localization/ros_graph_localizer/src/ros_graph_localizer_nodelet.cc new file mode 100644 index 0000000000..542e5e244a --- /dev/null +++ b/localization/ros_graph_localizer/src/ros_graph_localizer_nodelet.cc @@ -0,0 +1,302 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <ff_common/ff_names.h> +#include <localization_common/logger.h> +#include <localization_common/utilities.h> +#include <msg_conversions/msg_conversions.h> +#include <ros_graph_localizer/parameter_reader.h> +#include <ros_graph_localizer/ros_graph_localizer_nodelet.h> + +#include <std_msgs/Empty.h> + +namespace ros_graph_localizer { +namespace lc = localization_common; +namespace mc = msg_conversions; +namespace rv = ros_graph_vio; + +RosGraphLocalizerNodelet::RosGraphLocalizerNodelet() : ff_util::FreeFlyerNodelet(NODE_GRAPH_LOC, true) { + private_nh_.setCallbackQueue(&private_queue_); + heartbeat_.node = GetName(); + heartbeat_.nodelet_manager = ros::this_node::getName(); + + config_reader::ConfigReader config; + config.AddFile("localization/ros_graph_localizer.config"); + lc::LoadGraphLocalizerConfig(config); + LoadRosGraphLocalizerNodeletParams(config, params_); + last_heartbeat_time_ = ros::Time::now(); +} + +void RosGraphLocalizerNodelet::Initialize(ros::NodeHandle* nh) { + // Setup the platform name + platform_name_ = GetPlatform(); + platform_name_ = (platform_name_.empty() ? "" : platform_name_ + "/"); + + ff_common::InitFreeFlyerApplication(getMyArgv()); + SubscribeAndAdvertise(nh); + Run(); +} + +void RosGraphLocalizerNodelet::SubscribeAndAdvertise(ros::NodeHandle* nh) { + graph_vio_state_pub_ = nh->advertise<ff_msgs::GraphVIOState>(TOPIC_GRAPH_VIO_STATE, 10); + graph_loc_pub_ = nh->advertise<ff_msgs::GraphLocState>(TOPIC_GRAPH_LOC_STATE, 10); + reset_pub_ = nh->advertise<std_msgs::Empty>(TOPIC_GNC_EKF_RESET, 10); + heartbeat_pub_ = nh->advertise<ff_msgs::Heartbeat>(TOPIC_HEARTBEAT, 5, true); + if (params_.publish_depth_odometry) { + depth_odom_pub_ = nh->advertise<ff_msgs::DepthOdometry>(TOPIC_LOCALIZATION_DEPTH_ODOM, 10); + } + imu_sub_ = private_nh_.subscribe(TOPIC_HARDWARE_IMU, params_.max_imu_buffer_size, + &RosGraphLocalizerNodelet::ImuCallback, this, ros::TransportHints().tcpNoDelay()); + + if (params_.subscribe_to_depth_odometry) { + depth_odom_sub_ = + private_nh_.subscribe(TOPIC_LOCALIZATION_DEPTH_ODOM, params_.max_depth_odom_buffer_size, + &RosGraphLocalizerNodelet::DepthOdometryCallback, this, ros::TransportHints().tcpNoDelay()); + } + + if (params_.run_depth_odometry) { + const std::string depth_point_cloud_topic = static_cast<std::string>(TOPIC_HARDWARE_PICOFLEXX_PREFIX) + + static_cast<std::string>(TOPIC_HARDWARE_NAME_HAZ_CAM) + + static_cast<std::string>(TOPIC_HARDWARE_PICOFLEXX_SUFFIX); + depth_point_cloud_sub_ = private_nh_.subscribe<sensor_msgs::PointCloud2>( + depth_point_cloud_topic, params_.max_depth_cloud_buffer_size, &RosGraphLocalizerNodelet::DepthPointCloudCallback, + this, ros::TransportHints().tcpNoDelay()); + image_transport::ImageTransport depth_image_transport(private_nh_); + const std::string depth_image_topic = static_cast<std::string>(TOPIC_HARDWARE_PICOFLEXX_PREFIX) + + static_cast<std::string>(TOPIC_HARDWARE_NAME_HAZ_CAM) + + static_cast<std::string>(TOPIC_HARDWARE_PICOFLEXX_SUFFIX_EXTENDED) + + static_cast<std::string>(TOPIC_HARDWARE_PICOFLEXX_SUFFIX_AMPLITUDE_IMAGE); + depth_image_sub_ = depth_image_transport.subscribe(depth_image_topic, params_.max_depth_image_buffer_size, + &RosGraphLocalizerNodelet::DepthImageCallback, this); + } + + fp_sub_ = + private_nh_.subscribe(TOPIC_LOCALIZATION_OF_FEATURES, params_.max_feature_point_buffer_size, + &RosGraphLocalizerNodelet::FeaturePointsCallback, this, ros::TransportHints().tcpNoDelay()); + flight_mode_sub_ = + private_nh_.subscribe(TOPIC_MOBILITY_FLIGHT_MODE, 10, &RosGraphLocalizerNodelet::FlightModeCallback, this); + ar_tag_vl_sub_ = private_nh_.subscribe( + TOPIC_LOCALIZATION_AR_FEATURES, params_.max_ar_tag_matched_projections_buffer_size, + &RosGraphLocalizerNodelet::ARVisualLandmarksCallback, this, ros::TransportHints().tcpNoDelay()); + sparse_map_vl_sub_ = private_nh_.subscribe( + TOPIC_LOCALIZATION_ML_FEATURES, params_.max_vl_matched_projections_buffer_size, + &RosGraphLocalizerNodelet::SparseMapVisualLandmarksCallback, this, ros::TransportHints().tcpNoDelay()); + bias_srv_ = + private_nh_.advertiseService(SERVICE_GNC_EKF_INIT_BIAS, &RosGraphLocalizerNodelet::ResetBiasesAndLocalizer, this); + bias_from_file_srv_ = private_nh_.advertiseService( + SERVICE_GNC_EKF_INIT_BIAS_FROM_FILE, &RosGraphLocalizerNodelet::ResetBiasesFromFileAndResetLocalizer, this); + reset_map_srv_ = + private_nh_.advertiseService(SERVICE_LOCALIZATION_RESET_MAP_LOC, &RosGraphLocalizerNodelet::ResetMap, this); + reset_srv_ = private_nh_.advertiseService(SERVICE_GNC_EKF_RESET, + &RosGraphLocalizerNodelet::ResetBiasesFromFileAndResetLocalizer, this); + input_mode_srv_ = private_nh_.advertiseService(SERVICE_GNC_EKF_SET_INPUT, &RosGraphLocalizerNodelet::SetMode, this); +} + +bool RosGraphLocalizerNodelet::SetMode(ff_msgs::SetEkfInput::Request& req, ff_msgs::SetEkfInput::Response& res) { + const auto input_mode = req.mode; + if (input_mode == ff_msgs::SetEkfInputRequest::MODE_NONE) { + LogInfo("Received Mode None request, turning off Localizer."); + DisableLocalizerAndVIO(); + } else if (last_mode_ == ff_msgs::SetEkfInputRequest::MODE_NONE) { + LogInfo( + "Received Mode request that is not None and current mode is " + "None, resetting Localizer."); + ResetAndEnableLocalizer(); + } + + // Reset localizer when switch between ar mode + if (input_mode == ff_msgs::SetEkfInputRequest::MODE_AR_TAGS && + last_mode_ != ff_msgs::SetEkfInputRequest::MODE_AR_TAGS) { + LogInfo("SetMode: Switching to AR_TAG mode."); + // Reset world_T_dock when switch back to ar mode + ros_graph_localizer_wrapper_.ResetWorldTDock(); + } + + last_mode_ = input_mode; + return true; +} + +void RosGraphLocalizerNodelet::DisableLocalizerAndVIO() { localizer_and_vio_enabled_ = false; } + +void RosGraphLocalizerNodelet::EnableLocalizerAndVIO() { localizer_and_vio_enabled_ = true; } + +bool RosGraphLocalizerNodelet::localizer_and_vio_enabled() const { return localizer_and_vio_enabled_; } + +bool RosGraphLocalizerNodelet::ResetBiasesAndLocalizer(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { + DisableLocalizerAndVIO(); + ros_graph_vio_wrapper_.ResetBiasesAndVIO(); + ros_graph_localizer_wrapper_.ResetLocalizer(); + PublishReset(); + EnableLocalizerAndVIO(); + return true; +} + +bool RosGraphLocalizerNodelet::ResetBiasesFromFileAndResetLocalizer(std_srvs::Empty::Request& req, + std_srvs::Empty::Response& res) { + return ResetBiasesFromFileAndResetLocalizer(); +} + +bool RosGraphLocalizerNodelet::ResetBiasesFromFileAndResetLocalizer() { + DisableLocalizerAndVIO(); + ros_graph_vio_wrapper_.ResetBiasesFromFileAndResetVIO(); + ros_graph_localizer_wrapper_.ResetLocalizer(); + PublishReset(); + EnableLocalizerAndVIO(); + return true; +} + +void RosGraphLocalizerNodelet::ResetAndEnableLocalizer() { + DisableLocalizerAndVIO(); + ros_graph_localizer_wrapper_.ResetLocalizer(); + PublishReset(); + EnableLocalizerAndVIO(); +} + +bool RosGraphLocalizerNodelet::ResetMap(ff_msgs::ResetMap::Request& req, ff_msgs::ResetMap::Response& res) { + // Clear sparse map measurement buffer + sparse_map_vl_sub_ = private_nh_.subscribe( + TOPIC_LOCALIZATION_ML_FEATURES, params_.max_vl_matched_projections_buffer_size, + &RosGraphLocalizerNodelet::SparseMapVisualLandmarksCallback, this, ros::TransportHints().tcpNoDelay()); + ResetAndEnableLocalizer(); + return true; +} + +void RosGraphLocalizerNodelet::PublishGraphLocalizerState() { + const auto msg = ros_graph_localizer_wrapper_.GraphLocStateMsg(); + if (msg) graph_loc_pub_.publish(*msg); +} + +void RosGraphLocalizerNodelet::FeaturePointsCallback(const ff_msgs::Feature2dArray::ConstPtr& feature_array_msg) { + if (!localizer_and_vio_enabled()) return; + ros_graph_vio_wrapper_.FeaturePointsCallback(*feature_array_msg); +} + +void RosGraphLocalizerNodelet::DepthPointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg) { + if (!localizer_and_vio_enabled()) return; + const auto depth_odometry_msgs = depth_odometry_wrapper_.PointCloudCallback(point_cloud_msg); + for (const auto& depth_odometry_msg : depth_odometry_msgs) { + ros_graph_vio_wrapper_.DepthOdometryCallback(depth_odometry_msg); + if (params_.publish_depth_odometry) { + depth_odom_pub_.publish(depth_odometry_msg); + } + } +} + +void RosGraphLocalizerNodelet::DepthImageCallback(const sensor_msgs::ImageConstPtr& image_msg) { + if (!localizer_and_vio_enabled()) return; + const auto depth_odometry_msgs = depth_odometry_wrapper_.ImageCallback(image_msg); + for (const auto& depth_odometry_msg : depth_odometry_msgs) { + ros_graph_vio_wrapper_.DepthOdometryCallback(depth_odometry_msg); + if (params_.publish_depth_odometry) { + depth_odom_pub_.publish(depth_odometry_msg); + } + } +} + +void RosGraphLocalizerNodelet::DepthOdometryCallback(const ff_msgs::DepthOdometry::ConstPtr& depth_odom_msg) { + if (!localizer_and_vio_enabled()) return; + ros_graph_vio_wrapper_.DepthOdometryCallback(*depth_odom_msg); +} + +void RosGraphLocalizerNodelet::ImuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg) { + if (!localizer_and_vio_enabled()) return; + ros_graph_vio_wrapper_.ImuCallback(*imu_msg); + ros_graph_localizer_wrapper_.ImuCallback(*imu_msg); +} + +void RosGraphLocalizerNodelet::FlightModeCallback(ff_msgs::FlightMode::ConstPtr const& mode) { + ros_graph_vio_wrapper_.FlightModeCallback(*mode); + ros_graph_localizer_wrapper_.FlightModeCallback(*mode); +} + +void RosGraphLocalizerNodelet::PublishReset() const { + std_msgs::Empty msg; + reset_pub_.publish(msg); +} + +void RosGraphLocalizerNodelet::PublishHeartbeat() { + heartbeat_.header.stamp = ros::Time::now(); + if ((heartbeat_.header.stamp - last_heartbeat_time_).toSec() < 1.0) return; + heartbeat_pub_.publish(heartbeat_); + last_heartbeat_time_ = heartbeat_.header.stamp; +} + +void RosGraphLocalizerNodelet::PublishGraphLocalizerMessages() { + if (!localizer_and_vio_enabled()) return; + PublishGraphLocalizerState(); +} + +void RosGraphLocalizerNodelet::PublishWorldTDockTF() { + const auto world_T_dock = ros_graph_localizer_wrapper_.WorldTDock(); + if (!world_T_dock) return; + const auto world_T_dock_tf = + lc::PoseToTF(*world_T_dock, "world", "dock/body", lc::TimeFromRosTime(ros::Time::now()), platform_name_); + // If the rate is higher than the sim time, prevent sending repeat tfs + if (world_T_dock_tf.header.stamp == last_tf_dock_time_) return; + last_tf_dock_time_ = world_T_dock_tf.header.stamp; + transform_pub_.sendTransform(world_T_dock_tf); +} + +void RosGraphLocalizerNodelet::ARVisualLandmarksCallback( + const ff_msgs::VisualLandmarks::ConstPtr& visual_landmarks_msg) { + if (!localizer_and_vio_enabled()) return; + ros_graph_localizer_wrapper_.ARVisualLandmarksCallback(*visual_landmarks_msg); + PublishWorldTDockTF(); +} + +void RosGraphLocalizerNodelet::SparseMapVisualLandmarksCallback( + const ff_msgs::VisualLandmarks::ConstPtr& visual_landmarks_msg) { + if (!localizer_and_vio_enabled()) return; + // Avoid adding sparse map measurements when in AR mode + if (last_mode_ == ff_msgs::SetEkfInputRequest::MODE_AR_TAGS) return; + ros_graph_localizer_wrapper_.SparseMapVisualLandmarksCallback(*visual_landmarks_msg); +} + +void RosGraphLocalizerNodelet::Run() { + ros::Rate rate(100); + // Load Biases from file by default + // Biases reestimated if a intialize bias service call is received + ResetBiasesFromFileAndResetLocalizer(); + while (ros::ok()) { + private_queue_.callAvailable(); + if (localizer_and_vio_enabled()) { + ros_graph_vio_wrapper_.Update(); + // Pass pose covariance interpolater used for relative factors + // from graph vio to graph localizer + if (ros_graph_vio_wrapper_.Initialized() && ros_graph_localizer_wrapper_.Initialized()) { + ros_graph_localizer_wrapper_.graph_localizer()->SetPoseCovarianceInterpolater( + ros_graph_vio_wrapper_.graph_vio()->MarginalsPoseCovarianceInterpolater()); + } + + const auto graph_vio_state_msg = ros_graph_vio_wrapper_.GraphVIOStateMsg(); + if (!graph_vio_state_msg) { + LogDebugEveryN(100, "PublishVIOState: Failed to get vio state msg."); + } else { + graph_vio_state_pub_.publish(*graph_vio_state_msg); + ros_graph_localizer_wrapper_.GraphVIOStateCallback(*graph_vio_state_msg); + } + ros_graph_localizer_wrapper_.Update(); + PublishGraphLocalizerMessages(); + } + PublishHeartbeat(); + rate.sleep(); + } +} +} // namespace ros_graph_localizer + +PLUGINLIB_EXPORT_CLASS(ros_graph_localizer::RosGraphLocalizerNodelet, nodelet::Nodelet); diff --git a/localization/ros_graph_localizer/src/ros_graph_localizer_wrapper.cc b/localization/ros_graph_localizer/src/ros_graph_localizer_wrapper.cc new file mode 100644 index 0000000000..4b911ec0bf --- /dev/null +++ b/localization/ros_graph_localizer/src/ros_graph_localizer_wrapper.cc @@ -0,0 +1,314 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <config_reader/config_reader.h> +#include <graph_factors/loc_pose_factor.h> +#include <graph_factors/loc_projection_factor.h> +#include <localization_common/logger.h> +#include <localization_common/utilities.h> +#include <localization_measurements/measurement_conversions.h> +#include <msg_conversions/msg_conversions.h> +#include <parameter_reader/graph_localizer.h> +#include <ros_graph_localizer/ros_graph_localizer_wrapper.h> +#include <ros_graph_localizer/parameter_reader.h> + +#include <Eigen/Core> + +namespace ros_graph_localizer { +namespace gl = graph_localizer; +namespace ii = imu_integration; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace mc = msg_conversions; +namespace pr = parameter_reader; + +RosGraphLocalizerWrapper::RosGraphLocalizerWrapper(const std::string& graph_config_path_prefix) + : latest_num_detected_ml_features_(0), latest_num_detected_ar_features_(0) { + LoadConfigs(graph_config_path_prefix); + imu_integrator_.reset(new ii::ImuIntegrator(wrapper_params_.imu_integrator)); + vio_measurement_buffer_ = lc::TimestampedSet<ff_msgs::GraphVIOState>(wrapper_params_.max_relative_vio_buffer_size); + odom_interpolator_ = lc::PoseInterpolater(wrapper_params_.max_relative_vio_buffer_size); +} + +void RosGraphLocalizerWrapper::LoadConfigs(const std::string& graph_config_path_prefix) { + config_reader::ConfigReader config; + config.AddFile("localization/imu_filter.config"); + config.AddFile("localization/imu_integrator.config"); + config.AddFile("localization/ros_graph_localizer.config"); + lc::LoadGraphLocalizerConfig(config, graph_config_path_prefix); + pr::LoadGraphLocalizerParams(config, params_); + LoadRosGraphLocalizerWrapperParams(config, wrapper_params_); +} + +void RosGraphLocalizerWrapper::SparseMapVisualLandmarksCallback(const ff_msgs::VisualLandmarks& visual_landmarks_msg) { + latest_num_detected_ml_features_ = static_cast<int>(visual_landmarks_msg.landmarks.size()); + // Make sure enough landmarks are in the measurement for it to be valid + if (latest_num_detected_ml_features_ < params_.sparse_map_loc_factor_adder.min_num_matches_per_measurement) { + return; + } + + const auto msg_time = lc::TimeFromHeader(visual_landmarks_msg.header); + // Initialize with pose estimate if not initialized yet. + // Ensure vio data exists before msg time so no gaps occur between first + // sparse map measurement and future interpolated vio measurements. + if (!Initialized() && !vio_measurement_buffer_.empty()) { + const auto oldest_vio_measurement_time = vio_measurement_buffer_.OldestTimestamp(); + if (msg_time < *oldest_vio_measurement_time) { + LogDebug( + "SparseMapVisualLandmarksCallback: Initial vl msg time older than oldest buffered vio time, failed to " + "initialize graph localizer."); + return; + } + const auto world_T_body = lc::PoseFromMsgWithExtrinsics(visual_landmarks_msg.pose, + params_.sparse_map_loc_factor_adder.body_T_cam.inverse()); + params_.pose_node_adder.start_node = world_T_body; + params_.pose_node_adder.starting_time = msg_time; + graph_localizer_.reset(new gl::GraphLocalizer(params_)); + imu_integrator_.reset(new ii::ImuIntegrator(wrapper_params_.imu_integrator)); + LogInfo("SparseMapVisualLandmarksCallback: Initialized Localizer."); + // Only need the first vio measurement before the initial vl msg time + // to ensure valid pose interpolation. + vio_measurement_buffer_.RemoveBelowLowerBoundValues(msg_time); + // Add all subsequent measurements, remove from buffer once added. + while (!vio_measurement_buffer_.empty()) { + const auto graph_vio_msg = vio_measurement_buffer_.RemoveOldest(); + if (!GraphVIOStateCallback(graph_vio_msg->value)) return; + } + } else if (Initialized()) { // Otherwise add measurement to graph + const auto world_T_body = lc::PoseFromMsgWithExtrinsics(visual_landmarks_msg.pose, + params_.sparse_map_loc_factor_adder.body_T_cam.inverse()); + graph_localizer_->AddSparseMapMatchedProjectionsMeasurement( + lm::MakeMatchedProjectionsMeasurement(visual_landmarks_msg)); + } +} + +void RosGraphLocalizerWrapper::ARVisualLandmarksCallback(const ff_msgs::VisualLandmarks& visual_landmarks_msg) { + latest_num_detected_ar_features_ = static_cast<int>(visual_landmarks_msg.landmarks.size()); + if (latest_num_detected_ar_features_ < params_.ar_tag_loc_factor_adder.min_num_matches_per_measurement) { + return; + } + + // Set world_T_dock using the pose estimate from provided msg and the latest VIO extrapolated pose estimate + // since the dock pose in the message is relative to the dock frame + // and not the global frame + if (!world_T_dock_) { + const auto world_T_latest_graph_body = LatestPose(); + const auto latest_graph_timestamp = LatestTimestamp(); + if (!world_T_latest_graph_body || !latest_graph_timestamp) { + LogDebug("ARVisualLandmarksCallback: Failed to get latest pose and timestamp."); + return; + } + + if (odom_interpolator_.empty()) { + LogError("ARVisualLandmarksCallback: No odometry poses available for extrapolation."); + return; + } + const auto dock_time = lc::TimeFromHeader(visual_landmarks_msg.header); + const auto latest_odom_time = *(odom_interpolator_.LatestTimestamp()); + if (std::abs(dock_time - latest_odom_time) > 0.5) { + LogWarning("ARVisualLandmarksCallback: Latest odometry time vs. dock time " + << std::abs(dock_time - latest_odom_time) << " larger than 0.5 seconds."); + } + + // Extrapolate up to dock time if odometry messages exist that are more recent than this, + // otherwise extrapolate to latest odometry message and later add IMU data + const auto extrapolation_time = dock_time > latest_odom_time ? latest_odom_time : dock_time; + const auto latest_graph_body_T_odom_body = odom_interpolator_.Relative(*latest_graph_timestamp, extrapolation_time); + if (!latest_graph_body_T_odom_body) { + LogError("ARVisualLandmarksCallback: Failed to get latest_graph_body_T_odom_body for provided times."); + return; + } + + gtsam::Pose3 latest_graph_body_T_dock_body = lc::GtPose(*latest_graph_body_T_odom_body); + // If dock time is more recent than latest odometry estimate, also add latest IMU data since latest odom time. + if (wrapper_params_.extrapolate_dock_pose_with_imu && dock_time > latest_odom_time) { + if (!latest_vio_state_) { + LogError("ARVisualLandmarksCallback: Latest VIO state not available."); + return; + } + if (imu_integrator_->size() < 2 || !imu_integrator_->Latest()) { + LogError("ARVisualLandmarksCallback: Not enough IMU data available for extrapolation."); + return; + } + + const auto latest_imu_time = *(imu_integrator_->LatestTimestamp()); + const auto imu_extrapolation_time = dock_time > latest_imu_time ? latest_imu_time : dock_time; + const auto imu_extrapolated_latest_vio_state = + imu_integrator_->Extrapolate(*latest_vio_state_, imu_extrapolation_time); + if (!imu_extrapolated_latest_vio_state) { + LogError("ARVisualLandmarksCallback: Failed to extrapolate with IMU data."); + return; + } + const gtsam::Pose3& odom_T_imu_extrapolated_vio_state = imu_extrapolated_latest_vio_state->pose(); + const gtsam::Pose3& odom_T_latest_vio_body = latest_vio_state_->pose(); + const gtsam::Pose3 latest_vio_body_T_imu_extrapolated_body = + odom_T_latest_vio_body.inverse() * odom_T_imu_extrapolated_vio_state; + latest_graph_body_T_dock_body = latest_graph_body_T_dock_body * latest_vio_body_T_imu_extrapolated_body; + } + + const auto world_T_body = *world_T_latest_graph_body * latest_graph_body_T_dock_body; + const auto dock_T_body = + lc::PoseFromMsgWithExtrinsics(visual_landmarks_msg.pose, params_.ar_tag_loc_factor_adder.body_T_cam.inverse()); + + world_T_dock_ = world_T_body * dock_T_body.inverse(); + LogInfo("ARVisualLandmarksCallback: Initialized world_T_dock."); + } + if (Initialized()) { + // Frame change the ar tag measurement from the dock to world frame before + // passing to the localizer. + const auto frame_changed_ar_measurements = lm::FrameChangeMatchedProjectionsMeasurement( + lm::MakeMatchedProjectionsMeasurement(visual_landmarks_msg), *world_T_dock_); + graph_localizer_->AddArTagMatchedProjectionsMeasurement(frame_changed_ar_measurements); + } +} + +bool RosGraphLocalizerWrapper::GraphVIOStateCallback(const ff_msgs::GraphVIOState& graph_vio_state_msg) { + const auto timestamp = lc::TimeFromHeader(graph_vio_state_msg.header); + // Remove old IMU measurements no longer needed for dock extrapolation. + imu_integrator_->RemoveOldValues(timestamp); + // Buffer measurements before initialization so they can be added once initialized. + if (!Initialized()) { + vio_measurement_buffer_.Add(timestamp, graph_vio_state_msg); + return true; + } + + // Check if gap in vio msgs is too large, reset localizer if so. + if (last_vio_msg_time_ && (timestamp - *last_vio_msg_time_) > params_.max_vio_measurement_gap) { + LogError("GraphVIOStateCallback: VIO msg gap exceeded, resetting localizer. Msg time: " + << std::setprecision(15) << timestamp << ", last msg time: " << *last_vio_msg_time_ + << ", max gap: " << params_.max_vio_measurement_gap); + ResetLocalizer(); + return false; + } + + // Check if gap since last vl msg is too large, reset localizer if so. + if (latest_msg_time_ && (timestamp - *latest_msg_time_ > wrapper_params_.max_duration_between_vl_msgs)) { + LogWarning("GraphVIOStateCallback: Long time elapsed since last vl measurement, resetting localizer."); + ResetLocalizer(); + return false; + } + + // Otherwise add directly to graph localizer. + const auto& latest_combined_nav_state_msg = graph_vio_state_msg.combined_nav_states.combined_nav_states.back(); + const auto latest_combined_nav_state = lc::CombinedNavStateFromMsg(latest_combined_nav_state_msg); + const auto latest_covariances = lc::CombinedNavStateCovariancesFromMsg(latest_combined_nav_state_msg); + const auto latest_correlation_covariances = lc::CorrelationCovariancesFromMsg(latest_combined_nav_state_msg); + const lm::PoseWithCovarianceMeasurement pose_measurement( + latest_combined_nav_state.pose(), latest_covariances.pose_covariance(), latest_combined_nav_state.timestamp(), + latest_correlation_covariances); + graph_localizer_->AddPoseMeasurement(pose_measurement); + last_vio_msg_time_ = timestamp; + latest_vio_state_ = latest_combined_nav_state; + odom_interpolator_.Add(latest_combined_nav_state.timestamp(), lc::EigenPose(latest_combined_nav_state.pose())); + return true; +} + +void RosGraphLocalizerWrapper::ImuCallback(const sensor_msgs::Imu& imu_msg) { + imu_integrator_->AddImuMeasurement(lm::ImuMeasurement(imu_msg)); +} + +void RosGraphLocalizerWrapper::FlightModeCallback(const ff_msgs::FlightMode& flight_mode) { + imu_integrator_->SetFanSpeedMode(lm::ConvertFanSpeedMode(flight_mode.speed)); +} + +void RosGraphLocalizerWrapper::Update() { + if (Initialized()) graph_localizer_->Update(); +} + +bool RosGraphLocalizerWrapper::Initialized() const { return graph_localizer_ != nullptr; } + +void RosGraphLocalizerWrapper::ResetLocalizer() { + LogInfo("ResetLocalizer: Resetting localizer."); + if (!Initialized()) { + LogDebug("ResetLocalizer: Localizer not initialized, nothing to do."); + return; + } + graph_localizer_.reset(); + imu_integrator_.reset(new ii::ImuIntegrator(wrapper_params_.imu_integrator)); + vio_measurement_buffer_.Clear(); + odom_interpolator_.Clear(); + latest_vio_state_ = boost::none; + latest_msg_time_ = boost::none; + last_vio_msg_time_ = boost::none; + world_T_dock_ = boost::none; +} + +boost::optional<lc::Time> RosGraphLocalizerWrapper::LatestTimestamp() const { + if (!Initialized()) { + LogDebugEveryN(200, "LatestTimestamp: Localizer not yet initialized"); + return boost::none; + } + return graph_localizer_->pose_nodes().LatestTimestamp(); +} + +boost::optional<gtsam::Pose3> RosGraphLocalizerWrapper::LatestPose() const { + if (!Initialized()) { + LogDebugEveryN(200, "LatestPose: Localizer not yet initialized"); + return boost::none; + } + return graph_localizer_->pose_nodes().LatestNode(); +} + +void RosGraphLocalizerWrapper::ResetWorldTDock() { world_T_dock_ = boost::none; } + +boost::optional<gtsam::Pose3> RosGraphLocalizerWrapper::WorldTDock() const { return world_T_dock_; } + +std::unique_ptr<graph_localizer::GraphLocalizer>& RosGraphLocalizerWrapper::graph_localizer() { + return graph_localizer_; +} + +const std::unique_ptr<graph_localizer::GraphLocalizer>& RosGraphLocalizerWrapper::graph_localizer() const { + return graph_localizer_; +} + +boost::optional<ff_msgs::GraphLocState> RosGraphLocalizerWrapper::GraphLocStateMsg() { + if (!Initialized()) { + LogDebugEveryN(200, "GraphLocStateMsg: Localizer not yet initialized"); + return boost::none; + } + const auto latest_timestamp = *LatestTimestamp(); + // Avoid sending repeat msgs. + if (latest_msg_time_ && *latest_msg_time_ == latest_timestamp) { + LogDebugEveryN(2000, "GraphLocStateMsg: No new localization states."); + return boost::none; + } + latest_msg_time_ = latest_timestamp; + odom_interpolator_.RemoveBelowLowerBoundValues(latest_timestamp); + ff_msgs::GraphLocState msg; + const auto latest_pose = *LatestPose(); + const auto latest_keys = graph_localizer_->pose_nodes().Keys(latest_timestamp); + const auto latest_pose_covariance = *(graph_localizer_->Covariance(latest_keys[0])); + lc::PoseToMsg(latest_pose, msg.pose.pose); + mc::EigenCovarianceToMsg(latest_pose_covariance, msg.pose.covariance); + lc::TimeToHeader(latest_timestamp, msg.header); + msg.header.frame_id = "world"; + msg.child_frame_id = "body"; + msg.num_detected_ar_features = latest_num_detected_ar_features_; + msg.num_detected_ml_features = latest_num_detected_ml_features_; + msg.num_ml_projection_factors = graph_localizer_->NumFactors<gtsam::LocProjectionFactor<>>(); + msg.num_ml_pose_factors = graph_localizer_->NumFactors<gtsam::LocPoseFactor>(); + msg.optimization_iterations = graph_localizer_->optimization_iterations_averager().last_value(); + msg.optimization_time = graph_localizer_->optimization_timer().last_value(); + msg.update_time = graph_localizer_->update_timer().last_value(); + msg.duration = graph_localizer_->Duration(); + msg.num_states = graph_localizer_->num_values(); + latest_num_detected_ml_features_ = 0; + latest_num_detected_ar_features_ = 0; + return msg; +} +} // namespace ros_graph_localizer diff --git a/localization/ros_graph_localizer/test/test_ros_graph_localizer_parameter_reader.cc b/localization/ros_graph_localizer/test/test_ros_graph_localizer_parameter_reader.cc new file mode 100644 index 0000000000..8cc8703b7e --- /dev/null +++ b/localization/ros_graph_localizer/test/test_ros_graph_localizer_parameter_reader.cc @@ -0,0 +1,56 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <node_adders/utilities.h> +#include <ros_graph_localizer/parameter_reader.h> + +#include <gtest/gtest.h> + +namespace lc = localization_common; +namespace rl = ros_graph_localizer; + +class RosGraphLocalizerParameterReaderTest : public ::testing::Test { + public: + RosGraphLocalizerParameterReaderTest() {} + + void SetUp() final { + lc::SetEnvironmentConfigs(); + config_reader::ConfigReader config; + config.AddFile("localization/ros_graph_localizer.config"); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + rl::LoadRosGraphLocalizerNodeletParams(config, params_); + } + + rl::RosGraphLocalizerNodeletParams params_; +}; + +TEST_F(RosGraphLocalizerParameterReaderTest, RosGraphLocalizerNodeletParams) { + EXPECT_EQ(params_.max_graph_vio_state_buffer_size, 10); + EXPECT_EQ(params_.max_vl_matched_projections_buffer_size, 10); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/ros_graph_localizer/test/test_ros_graph_localizer_parameter_reader.test b/localization/ros_graph_localizer/test/test_ros_graph_localizer_parameter_reader.test new file mode 100644 index 0000000000..bc3e486e26 --- /dev/null +++ b/localization/ros_graph_localizer/test/test_ros_graph_localizer_parameter_reader.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="ros_graph_localizer" type="test_ros_graph_localizer_parameter_reader" test-name="test_ros_graph_localizer_parameter_reader" /> +</launch> diff --git a/localization/ros_graph_vio/CMakeLists.txt b/localization/ros_graph_vio/CMakeLists.txt new file mode 100644 index 0000000000..f7a6d5d273 --- /dev/null +++ b/localization/ros_graph_vio/CMakeLists.txt @@ -0,0 +1,101 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(ros_graph_vio) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + ff_util + ff_msgs + graph_vio + parameter_reader + msg_conversions + localization_common + localization_measurements +) + +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS + camera + config_reader + ff_util + ff_msgs + graph_vio + localization_common + localization_measurements + parameter_reader + msg_conversions +) + +########### +## Build ## +########### +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +file(GLOB cc_files + "src/*.cc" +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + ${cc_files} +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_ros_graph_vio_parameter_reader + test/test_ros_graph_vio_parameter_reader.test + test/test_ros_graph_vio_parameter_reader.cc + ) + target_link_libraries(test_ros_graph_vio_parameter_reader + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/ros_graph_vio/TODO.txt b/localization/ros_graph_vio/TODO.txt new file mode 100644 index 0000000000..c03b0f4d83 --- /dev/null +++ b/localization/ros_graph_vio/TODO.txt @@ -0,0 +1,55 @@ + - send pose/cov history from vio, add all poses/covs in pose node (update old values) (C) + - add support for this in timestamped_set? (updating old values) + + - add grah_vio! (A) + - inherit from sliding window optimizer + - add combined imu node adder, vo and standstill factor adders + - publish odometry message! + - add new message type? + - convert velocity from odom frame to global frame?? + - is velocity in global frame or body frame?? + - add better way of getting relative covariance?? + + - add graph_localizer! (C) + - use pose node updater with odometry message! + - add loc factor adder! + + - add parameter reader! (G) + - fail if parameter not read!! + - how to unit test these?? + + isam2 optimizer issues! + - need to account for values removed when sliding window! + - since these have keys, just check for removed keys! + - need to account for factors removed when sliding window! + (i.e. smart factors!) + - since these are pts, just make sets of ptr values and check for new and removed ones!!! + + - add ordering back to sliding window optimizer now that window is slid after optimization! + + - TODO: implement others! + + + +- TODO: how to access opt iterations???? +- move serialization.cc back to graph optimizer? + - make graph optimizer depend on graph_Factors? + - add test with graph optimizer, simple factor adder, and simple node updater!! + +- add graph vio to graph bag! + - run_graph_Bag, plot_graph_bag + - use graph bag???? + - copy graph bag and run for VIO only?? Or modify current graph bag tool?? + - (probably modify current tool?) +- how to add a second measurement to combined nav node update model????? (AE) + - why is this needed??? + - fan rate!!!!!! + - add to imu measurement??? +- move param loading and serialization from graph_vio/loc to respective packages! +- initialize graph with identity pose, set fixed!!! + - TODO: need to always have fixed identity pose as origin! how to do this? + - set start pose fixed after maginalize???? +- rename pimpredict in other packages to extrapolate + - tools: loc rvz plugins, imu bias tester +- update imu_augmentor, make sure it compiles! + - add tests? diff --git a/localization/ros_graph_vio/include/ros_graph_vio/imu_bias_initializer.h b/localization/ros_graph_vio/include/ros_graph_vio/imu_bias_initializer.h new file mode 100644 index 0000000000..a4aa39571c --- /dev/null +++ b/localization/ros_graph_vio/include/ros_graph_vio/imu_bias_initializer.h @@ -0,0 +1,81 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef ROS_GRAPH_VIO_IMU_BIAS_INITIALIZER_H_ +#define ROS_GRAPH_VIO_IMU_BIAS_INITIALIZER_H_ + +#include <imu_integration/dynamic_imu_filter.h> +#include <localization_measurements/fan_speed_mode.h> +#include <localization_measurements/imu_measurement.h> +#include <msg_conversions/msg_conversions.h> +#include <ros_graph_vio/imu_bias_initializer_params.h> + +#include <gtsam/navigation/ImuBias.h> + +#include <vector> + +namespace ros_graph_vio { +struct ImuBiasWithStddev { + ImuBiasWithStddev(const gtsam::imuBias::ConstantBias& bias, const Eigen::Vector3d& accelerometer_bias_stddev, + const Eigen::Vector3d& gyro_bias_stddev) + : bias(bias), accelerometer_bias_stddev(accelerometer_bias_stddev), gyro_bias_stddev(gyro_bias_stddev) {} + gtsam::imuBias::ConstantBias bias; + Eigen::Vector3d accelerometer_bias_stddev; + Eigen::Vector3d gyro_bias_stddev; +}; + +// Buffers IMU measurements (and flight speed mode if available to help filter speed specific vibration noise) and +// estimates IMU biases. Assumes standstill behavior so the estimated bias is simply the average of the buffered +// measurements. Saves biases to a file and optionally loads from a file as well. +// If using in a gravity environment, assumes the IMU orientation wrt gravity doesn't +// change, so gravity is considered a constant IMU bias that contributes to the estimated IMU bias. +class ImuBiasInitializer { + public: + // Construct with params. + explicit ImuBiasInitializer(const ImuBiasInitializerParams& params); + + // Add fan speed mode measurement for IMU bias filter. + void AddFanSpeedModeMeasurement(const localization_measurements::FanSpeedMode fan_speed_mode); + + // Add IMU measurement. Estimate biases if enough measurements have been received and + // save this to a file. + void AddImuMeasurement(const localization_measurements::ImuMeasurement& imu_measurement); + + // Returns bias if it is available. + boost::optional<ImuBiasWithStddev> Bias() const; + + // Manually sets the bias and saves it to file. + void UpdateBias(const ImuBiasWithStddev& bias); + + // Clears measurement buffer, filter, and estimated bias. + void Reset(); + + // Loads the IMU bias from a file (filename set in params). + bool LoadFromFile(); + + // Save biases to a file (filename set in params). + bool SaveToFile() const; + + private: + boost::optional<ImuBiasWithStddev> imu_bias_; + std::unique_ptr<imu_integration::DynamicImuFilter> imu_bias_filter_; + std::vector<localization_measurements::ImuMeasurement> imu_bias_measurements_; + ImuBiasInitializerParams params_; +}; +} // namespace ros_graph_vio + +#endif // ROS_GRAPH_VIO_IMU_BIAS_INITIALIZER_H_ diff --git a/localization/ros_graph_vio/include/ros_graph_vio/imu_bias_initializer_params.h b/localization/ros_graph_vio/include/ros_graph_vio/imu_bias_initializer_params.h new file mode 100644 index 0000000000..1666493c47 --- /dev/null +++ b/localization/ros_graph_vio/include/ros_graph_vio/imu_bias_initializer_params.h @@ -0,0 +1,33 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef ROS_GRAPH_VIO_IMU_BIAS_INITIALIZER_PARAMS_H_ +#define ROS_GRAPH_VIO_IMU_BIAS_INITIALIZER_PARAMS_H_ + +#include <imu_integration/imu_filter_params.h> + +#include <string> + +namespace ros_graph_vio { +struct ImuBiasInitializerParams { + std::string imu_bias_filename; + imu_integration::ImuFilterParams filter; + int num_bias_estimation_measurements; +}; +} // namespace ros_graph_vio + +#endif // ROS_GRAPH_VIO_IMU_BIAS_INITIALIZER_PARAMS_H_ diff --git a/localization/imu_augmentor/include/imu_augmentor/imu_augmentor.h b/localization/ros_graph_vio/include/ros_graph_vio/parameter_reader.h similarity index 53% rename from localization/imu_augmentor/include/imu_augmentor/imu_augmentor.h rename to localization/ros_graph_vio/include/ros_graph_vio/parameter_reader.h index 3c7ccd2c30..158f5872b7 100644 --- a/localization/imu_augmentor/include/imu_augmentor/imu_augmentor.h +++ b/localization/ros_graph_vio/include/ros_graph_vio/parameter_reader.h @@ -15,24 +15,21 @@ * License for the specific language governing permissions and limitations * under the License. */ +#ifndef ROS_GRAPH_VIO_PARAMETER_READER_H_ +#define ROS_GRAPH_VIO_PARAMETER_READER_H_ -#ifndef IMU_AUGMENTOR_IMU_AUGMENTOR_H_ -#define IMU_AUGMENTOR_IMU_AUGMENTOR_H_ - -#include <imu_augmentor/imu_augmentor_params.h> -#include <imu_integration/imu_integrator.h> -#include <localization_common/combined_nav_state.h> +#include <config_reader/config_reader.h> +#include <ros_graph_vio/imu_bias_initializer_params.h> +#include <ros_graph_vio/ros_graph_vio_wrapper_params.h> #include <string> -namespace imu_augmentor { -class ImuAugmentor : public imu_integration::ImuIntegrator { - public: - explicit ImuAugmentor(const ImuAugmentorParams& params); +namespace ros_graph_vio { +void LoadImuBiasInitializerParams(config_reader::ConfigReader& config, ImuBiasInitializerParams& params, + const std::string& prefix = ""); - void PimPredict(const localization_common::CombinedNavState& latest_combined_nav_state, - localization_common::CombinedNavState& latest_imu_augmented_combined_nav_state); -}; -} // namespace imu_augmentor +void LoadRosGraphVIOWrapperParams(config_reader::ConfigReader& config, RosGraphVIOWrapperParams& params, + const std::string& prefix = ""); +} // namespace ros_graph_vio -#endif // IMU_AUGMENTOR_IMU_AUGMENTOR_H_ +#endif // ROS_GRAPH_VIO_PARAMETER_READER_H_ diff --git a/localization/ros_graph_vio/include/ros_graph_vio/ros_graph_vio_wrapper.h b/localization/ros_graph_vio/include/ros_graph_vio/ros_graph_vio_wrapper.h new file mode 100644 index 0000000000..3aa1f1a600 --- /dev/null +++ b/localization/ros_graph_vio/include/ros_graph_vio/ros_graph_vio_wrapper.h @@ -0,0 +1,102 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef ROS_GRAPH_VIO_ROS_GRAPH_VIO_WRAPPER_H_ +#define ROS_GRAPH_VIO_ROS_GRAPH_VIO_WRAPPER_H_ + +#include <ff_msgs/DepthOdometry.h> +#include <ff_msgs/Feature2dArray.h> +#include <ff_msgs/FlightMode.h> +#include <ff_msgs/GraphVIOState.h> +// #include <ff_msgs/SerializedGraph.h> +#include <graph_vio/graph_vio.h> +#include <localization_measurements/fan_speed_mode.h> +#include <localization_measurements/imu_measurement.h> +#include <ros_graph_vio/imu_bias_initializer.h> +#include <ros_graph_vio/ros_graph_vio_wrapper_params.h> + +#include <sensor_msgs/Imu.h> + +#include <string> +#include <utility> + +namespace ros_graph_vio { +// Converts ROS messages and passes these to the GraphVIO graph. +// Initializes the biases fro the graph using IMU messages. +// Creates msgs from latest combined nav states in the graph. +class RosGraphVIOWrapper { + public: + explicit RosGraphVIOWrapper(const std::string& graph_config_path_prefix = ""); + + // Load configs for graph_vio and IMU bias initializer. + void LoadConfigs(const std::string& graph_config_path_prefix); + + // Add feature points msg to graph_vio. + void FeaturePointsCallback(const ff_msgs::Feature2dArray& feature_array_msg); + + // Add imu msg to graph_vio and IMU bias initializer if necessary. + // If the graph hasn't been initialized and an imu bias is + // available in the initializer, initializes the graph. + void ImuCallback(const sensor_msgs::Imu& imu_msg); + + // Add depth odometry msg to graph_vio. + void DepthOdometryCallback(const ff_msgs::DepthOdometry& depth_odometry_msg); + + // Add flight mode msg to graph_vio and IMU bias initializer. + void FlightModeCallback(const ff_msgs::FlightMode& flight_mode); + + // Updates the graph_vio if it has been initialized. + void Update(); + + // Returns whether the graph_vio has been initialized. + bool Initialized() const; + + // Resets the graph with the latest biases. + void ResetVIO(); + + // Resets the graph and biases. Biases need to be estimated again by the bias initializer. + void ResetBiasesAndVIO(); + + // Resets the graph and and loads biases from a saved file. + void ResetBiasesFromFileAndResetVIO(); + + // Creates a GraphVIOState msg using the history + // of nav states and covariances in graph_vio. + // Returns boost::none if no states available or no changes + // have occured since last msg. + boost::optional<ff_msgs::GraphVIOState> GraphVIOStateMsg(); + + // Const accessor to graph_vio object + const std::unique_ptr<graph_vio::GraphVIO>& graph_vio() const; + + // Accessor to graph_vio object + std::unique_ptr<graph_vio::GraphVIO>& graph_vio(); + + private: + // Initialize the graph if the IMU bias is initialized. + void Initialize(); + + std::unique_ptr<graph_vio::GraphVIO> graph_vio_; + std::unique_ptr<ImuBiasInitializer> imu_bias_initializer_; + graph_vio::GraphVIOParams params_; + boost::optional<localization_common::Time> latest_msg_time_; + RosGraphVIOWrapperParams wrapper_params_; + int feature_point_count_; +}; +} // namespace ros_graph_vio + +#endif // ROS_GRAPH_VIO_ROS_GRAPH_VIO_WRAPPER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/feature_tracker_params.h b/localization/ros_graph_vio/include/ros_graph_vio/ros_graph_vio_wrapper_params.h similarity index 62% rename from localization/graph_localizer/include/graph_localizer/feature_tracker_params.h rename to localization/ros_graph_vio/include/ros_graph_vio/ros_graph_vio_wrapper_params.h index 149052c312..543340f675 100644 --- a/localization/graph_localizer/include/graph_localizer/feature_tracker_params.h +++ b/localization/ros_graph_vio/include/ros_graph_vio/ros_graph_vio_wrapper_params.h @@ -15,16 +15,17 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef GRAPH_LOCALIZER_FEATURE_TRACKER_PARAMS_H_ -#define GRAPH_LOCALIZER_FEATURE_TRACKER_PARAMS_H_ +#ifndef ROS_GRAPH_VIO_ROS_GRAPH_VIO_WRAPPER_PARAMS_H_ +#define ROS_GRAPH_VIO_ROS_GRAPH_VIO_WRAPPER_PARAMS_H_ -namespace graph_localizer { -struct FeatureTrackerParams { - // Max duration, feature tracker trims measurements outside of this window or outside of graph window - double sliding_window_duration; - int smart_projection_adder_measurement_spacing; - bool use_allowed_timestamps; +namespace ros_graph_vio { +struct RosGraphVIOWrapperParams { + double starting_pose_translation_stddev; + double starting_pose_quaternion_stddev; + double starting_velocity_stddev_scale; + double starting_accel_bias_stddev_scale; + double starting_gyro_bias_stddev_scale; }; -} // namespace graph_localizer +} // namespace ros_graph_vio -#endif // GRAPH_LOCALIZER_FEATURE_TRACKER_PARAMS_H_ +#endif // ROS_GRAPH_VIO_ROS_GRAPH_VIO_WRAPPER_PARAMS_H_ diff --git a/localization/ros_graph_vio/package.xml b/localization/ros_graph_vio/package.xml new file mode 100644 index 0000000000..02545c018e --- /dev/null +++ b/localization/ros_graph_vio/package.xml @@ -0,0 +1,45 @@ +<package> + <name>ros_graph_vio</name> + <version>1.0.0</version> + <description> + The graph VIO package + </description> + <license> + Apache License, Version 2.0 + </license> + <author email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </author> + <maintainer email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </maintainer> + <buildtool_depend>catkin</buildtool_depend> + <build_depend>camera</build_depend> + <build_depend>config_reader</build_depend> + <build_depend>factor_adders</build_depend> + <build_depend>ff_util</build_depend> + <build_depend>ff_msgs</build_depend> + <build_depend>graph_factors</build_depend> + <build_depend>graph_vio</build_depend> + <build_depend>imu_integration</build_depend> + <build_depend>localization_common</build_depend> + <build_depend>localization_measurements</build_depend> + <build_depend>msg_conversions</build_depend> + <build_depend>node_updaters</build_depend> + <build_depend>parameter_reader</build_depend> + <build_depend>vision_common</build_depend> + <run_depend>camera</run_depend> + <run_depend>config_reader</run_depend> + <run_depend>factor_adders</run_depend> + <run_depend>ff_util</run_depend> + <run_depend>ff_msgs</run_depend> + <run_depend>graph_factors</run_depend> + <run_depend>graph_vio</run_depend> + <run_depend>imu_integration</run_depend> + <run_depend>localization_common</run_depend> + <run_depend>localization_measurements</run_depend> + <run_depend>msg_conversions</run_depend> + <run_depend>node_updaters</run_depend> + <run_depend>parameter_reader</run_depend> + <run_depend>vision_common</run_depend> +</package> diff --git a/localization/ros_graph_vio/readme.md b/localization/ros_graph_vio/readme.md new file mode 100644 index 0000000000..6ac0462461 --- /dev/null +++ b/localization/ros_graph_vio/readme.md @@ -0,0 +1,19 @@ +\page rosgraphVIO Ros Graph VIO + +# Package Overview +Ros wrapper for the graph VIO. The graph VIO wrapper takes ros messages, convers these to localization measurements, and passes these to the GraphVIO object. The ros graph VIO nodelet handles live subscribing and publishing to ros topics. + +## RosGraphVIOWrapper +Contains the GraphVIO. Converts ROS messages to localization measurements and provides these to the VIO object. + +## ImuBiasInitializer +Assumes standstill and estimates IMU biases and stddevs from a sequence of IMU measurements. + +# Inputs +* `/hw/imu` +* `/loc/of/features` +* `/loc/depth/odom` +* `/mob/flight_mode` + +# Outputs +* `/graph_vio/state` diff --git a/localization/ros_graph_vio/src/imu_bias_initializer.cc b/localization/ros_graph_vio/src/imu_bias_initializer.cc new file mode 100644 index 0000000000..be5e5bac54 --- /dev/null +++ b/localization/ros_graph_vio/src/imu_bias_initializer.cc @@ -0,0 +1,146 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/logger.h> +#include <ros_graph_vio/imu_bias_initializer.h> + +#include <iostream> +#include <string> + +namespace ros_graph_vio { +namespace lm = localization_measurements; +ImuBiasInitializer::ImuBiasInitializer(const ImuBiasInitializerParams& params) : params_(params) { Reset(); } + +void ImuBiasInitializer::AddFanSpeedModeMeasurement(const lm::FanSpeedMode fan_speed_mode) { + imu_bias_filter_->SetFanSpeedMode(fan_speed_mode); +} + +void ImuBiasInitializer::AddImuMeasurement(const localization_measurements::ImuMeasurement& imu_measurement) { + // Nothing to do if bias already estimated. + if (imu_bias_) return; + + // Add filtered measurement. + const auto filtered_imu_measurement = imu_bias_filter_->AddMeasurement(imu_measurement); + if (filtered_imu_measurement) { + imu_bias_measurements_.emplace_back(*filtered_imu_measurement); + } + // Nothing to do if not enough measurements have been received. + if (static_cast<int>(imu_bias_measurements_.size()) < params_.num_bias_estimation_measurements) return; + + // Estimate bias if enough measurements have been received. + // Assumes standstill, so simply average accelerometer and gyro measurements. + Eigen::Vector3d sum_of_acceleration_measurements = Eigen::Vector3d::Zero(); + Eigen::Vector3d sum_of_angular_velocity_measurements = Eigen::Vector3d::Zero(); + for (const auto& imu_measurement : imu_bias_measurements_) { + sum_of_acceleration_measurements += imu_measurement.acceleration; + sum_of_angular_velocity_measurements += imu_measurement.angular_velocity; + } + + const Eigen::Vector3d accelerometer_bias = sum_of_acceleration_measurements / imu_bias_measurements_.size(); + const Eigen::Vector3d gyro_bias = sum_of_angular_velocity_measurements / imu_bias_measurements_.size(); + LogDebug("Estimated accelerometer bias: " << std::endl << accelerometer_bias.matrix()); + LogDebug("Estimated gyro bias: " << std::endl << gyro_bias.matrix()); + + // Compute standard deviations + Eigen::Vector3d accelerometer_variance = Eigen::Vector3d::Zero(); + Eigen::Vector3d gyro_variance = Eigen::Vector3d::Zero(); + for (const auto& imu_measurement : imu_bias_measurements_) { + accelerometer_variance += (imu_measurement.acceleration - accelerometer_bias).cwiseAbs2(); + gyro_variance += (imu_measurement.angular_velocity - gyro_bias).cwiseAbs2(); + } + accelerometer_variance /= static_cast<double>(imu_bias_measurements_.size()); + gyro_variance /= static_cast<double>(imu_bias_measurements_.size()); + imu_bias_ = + ImuBiasWithStddev(gtsam::imuBias::ConstantBias(accelerometer_bias, gyro_bias), + Eigen::Vector3d(accelerometer_variance.cwiseSqrt()), Eigen::Vector3d(gyro_variance.cwiseSqrt())); + SaveToFile(); +} + +boost::optional<ImuBiasWithStddev> ImuBiasInitializer::Bias() const { return imu_bias_; } + +void ImuBiasInitializer::UpdateBias(const ImuBiasWithStddev& bias) { + imu_bias_ = bias; + SaveToFile(); +} + +void ImuBiasInitializer::Reset() { + imu_bias_ = boost::none; + imu_bias_filter_.reset(new imu_integration::DynamicImuFilter(params_.filter)); + imu_bias_measurements_.clear(); +} + +bool ImuBiasInitializer::LoadFromFile() { + std::ifstream imu_bias_file(params_.imu_bias_filename); + if (!imu_bias_file.is_open()) { + LogDebug("LoadFromFile: Failed to read imu bias file."); + return false; + } + + Eigen::Vector3d accelerometer_bias, gyro_bias, accelerometer_bias_stddev, gyro_bias_stddev; + for (int line_num = 0; line_num < 4; ++line_num) { + std::string line; + if (!std::getline(imu_bias_file, line)) { + LogError("LoadFromFile: Failed to get line from imu bias file."); + return false; + } + std::istringstream line_stream(line); + std::string val; + for (int val_index = 0; val_index < 3; ++val_index) { + if (!std::getline(line_stream, val, ',')) { + LogError("LoadFromFile: Failed to get value from imu bias file."); + return false; + } + if (line_num == 0) { + accelerometer_bias[val_index] = std::stold(val); + } else if (line_num == 1) { + gyro_bias[val_index] = std::stold(val); + } else if (line_num == 2) { + accelerometer_bias_stddev[val_index] = std::stold(val); + } else { + gyro_bias_stddev[val_index] = std::stold(val); + } + } + } + imu_bias_ = ImuBiasWithStddev(gtsam::imuBias::ConstantBias(accelerometer_bias, gyro_bias), accelerometer_bias_stddev, + gyro_bias_stddev); + return true; +} + +bool ImuBiasInitializer::SaveToFile() const { + if (!imu_bias_) { + LogError("SaveToFile: No IMU bias available."); + return false; + } + std::ofstream imu_bias_file(params_.imu_bias_filename); + if (!imu_bias_file.is_open()) { + LogError("SaveToFile: Failed to create imu bias output file."); + return false; + } + const auto& accel_bias = imu_bias_->bias.accelerometer(); + imu_bias_file << accel_bias.x() << "," << accel_bias.y() << "," << accel_bias.z() << std::endl; + const auto& gyro_bias = imu_bias_->bias.gyroscope(); + imu_bias_file << gyro_bias.x() << "," << gyro_bias.y() << "," << gyro_bias.z() << std::endl; + const auto& accelerometer_bias_stddev = imu_bias_->accelerometer_bias_stddev; + imu_bias_file << accelerometer_bias_stddev.x() << "," << accelerometer_bias_stddev.y() << "," + << accelerometer_bias_stddev.z() << std::endl; + const auto& gyro_bias_stddev = imu_bias_->gyro_bias_stddev; + imu_bias_file << gyro_bias_stddev.x() << "," << gyro_bias_stddev.y() << "," << gyro_bias_stddev.z() << std::endl; + imu_bias_file.close(); + return true; +} +} // namespace ros_graph_vio diff --git a/localization/ros_graph_vio/src/parameter_reader.cc b/localization/ros_graph_vio/src/parameter_reader.cc new file mode 100644 index 0000000000..a5f0af9946 --- /dev/null +++ b/localization/ros_graph_vio/src/parameter_reader.cc @@ -0,0 +1,42 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <msg_conversions/msg_conversions.h> +#include <parameter_reader/imu_integration.h> +#include <ros_graph_vio/parameter_reader.h> + +namespace ros_graph_vio { +namespace mc = msg_conversions; +namespace pr = parameter_reader; + +void LoadImuBiasInitializerParams(config_reader::ConfigReader& config, ImuBiasInitializerParams& params, + const std::string& prefix) { + pr::LoadImuFilterParams(config, params.filter); + LOAD_PARAM(params.imu_bias_filename, config, prefix + "ibi_"); + LOAD_PARAM(params.num_bias_estimation_measurements, config, prefix + "ibi_"); +} + +void LoadRosGraphVIOWrapperParams(config_reader::ConfigReader& config, RosGraphVIOWrapperParams& params, + const std::string& prefix) { + LOAD_PARAM(params.starting_pose_translation_stddev, config, prefix + "rgv_"); + LOAD_PARAM(params.starting_pose_quaternion_stddev, config, prefix + "rgv_"); + LOAD_PARAM(params.starting_velocity_stddev_scale, config, prefix + "rgv_"); + LOAD_PARAM(params.starting_accel_bias_stddev_scale, config, prefix + "rgv_"); + LOAD_PARAM(params.starting_gyro_bias_stddev_scale, config, prefix + "rgv_"); +} +} // namespace ros_graph_vio diff --git a/localization/ros_graph_vio/src/ros_graph_vio_wrapper.cc b/localization/ros_graph_vio/src/ros_graph_vio_wrapper.cc new file mode 100644 index 0000000000..bedbe1f7ea --- /dev/null +++ b/localization/ros_graph_vio/src/ros_graph_vio_wrapper.cc @@ -0,0 +1,224 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <config_reader/config_reader.h> +#include <localization_common/logger.h> +#include <localization_common/utilities.h> +#include <localization_measurements/measurement_conversions.h> +#include <msg_conversions/msg_conversions.h> +#include <parameter_reader/graph_vio.h> +#include <ros_graph_vio/parameter_reader.h> +#include <ros_graph_vio/ros_graph_vio_wrapper.h> + +#include <Eigen/Core> + +namespace ros_graph_vio { +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace mc = msg_conversions; +namespace pr = parameter_reader; + +RosGraphVIOWrapper::RosGraphVIOWrapper(const std::string& graph_config_path_prefix) { + LoadConfigs(graph_config_path_prefix); +} + +void RosGraphVIOWrapper::LoadConfigs(const std::string& graph_config_path_prefix) { + config_reader::ConfigReader config; + config.AddFile("localization/imu_bias_initializer.config"); + config.AddFile("localization/imu_filter.config"); + config.AddFile("localization/ros_graph_vio.config"); + lc::LoadGraphVIOConfig(config, graph_config_path_prefix); + pr::LoadGraphVIOParams(config, params_); + LoadRosGraphVIOWrapperParams(config, wrapper_params_); + ImuBiasInitializerParams imu_bias_initializer_params; + LoadImuBiasInitializerParams(config, imu_bias_initializer_params); + imu_bias_initializer_.reset(new ImuBiasInitializer(imu_bias_initializer_params)); +} + +void RosGraphVIOWrapper::FeaturePointsCallback(const ff_msgs::Feature2dArray& feature_array_msg) { + if (Initialized()) graph_vio_->AddFeaturePointsMeasurement(lm::MakeFeaturePointsMeasurement(feature_array_msg)); + ++feature_point_count_; +} + +void RosGraphVIOWrapper::DepthOdometryCallback(const ff_msgs::DepthOdometry& depth_odometry_msg) { + if (Initialized()) graph_vio_->AddDepthOdometryMeasurement(lm::MakeDepthOdometryMeasurement(depth_odometry_msg)); +} + +void RosGraphVIOWrapper::ImuCallback(const sensor_msgs::Imu& imu_msg) { + const auto imu_measurement = lm::ImuMeasurement(imu_msg); + imu_bias_initializer_->AddImuMeasurement(imu_measurement); + if (!Initialized() && imu_bias_initializer_->Bias()) { + // Set initial nav state. Use bias from initializer and + // assume zero initial velocity. Set initial pose to identity. + const lc::CombinedNavState initial_state(gtsam::Pose3::identity(), gtsam::Velocity3::Zero(), + imu_bias_initializer_->Bias()->bias, imu_measurement.timestamp); + params_.combined_nav_state_node_adder.start_node = initial_state; + params_.combined_nav_state_node_adder.starting_time = initial_state.timestamp(); + // Set starting state noise using params + const gtsam::Vector6 pose_noise_sigmas( + (gtsam::Vector(6) << wrapper_params_.starting_pose_translation_stddev, + wrapper_params_.starting_pose_translation_stddev, wrapper_params_.starting_pose_translation_stddev, + wrapper_params_.starting_pose_quaternion_stddev, wrapper_params_.starting_pose_quaternion_stddev, + wrapper_params_.starting_pose_quaternion_stddev) + .finished()); + const auto pose_noise = + lc::Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(pose_noise_sigmas)), + params_.combined_nav_state_node_adder.huber_k); + params_.combined_nav_state_node_adder.start_noise_models.emplace_back(pose_noise); + // Set starting velocity noise using accel bias stddev - assumes stddev in accel measurements correlate + // to uncertainty in initial velocity + const auto& accel_bias_stddev = imu_bias_initializer_->Bias()->accelerometer_bias_stddev; + const auto& gyro_bias_stddev = imu_bias_initializer_->Bias()->gyro_bias_stddev; + const gtsam::Vector3 velocity_noise_sigmas = accel_bias_stddev * wrapper_params_.starting_velocity_stddev_scale; + const auto velocity_noise = + lc::Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(velocity_noise_sigmas)), + params_.combined_nav_state_node_adder.huber_k); + params_.combined_nav_state_node_adder.start_noise_models.emplace_back(velocity_noise); + + // Set starting bias noise using scaled calculated bias stddevs + const gtsam::Vector6 bias_noise_sigmas( + (gtsam::Vector(6) << accel_bias_stddev * wrapper_params_.starting_accel_bias_stddev_scale, + gyro_bias_stddev * wrapper_params_.starting_gyro_bias_stddev_scale) + .finished()); + const auto bias_noise = + lc::Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref<const Eigen::VectorXd>(bias_noise_sigmas)), + params_.combined_nav_state_node_adder.huber_k); + params_.combined_nav_state_node_adder.start_noise_models.emplace_back(bias_noise); + graph_vio_.reset(new graph_vio::GraphVIO(params_)); + LogInfo("ImuCallback: Initialized VIO."); + } + if (Initialized()) graph_vio_->AddImuMeasurement(imu_measurement); +} + +void RosGraphVIOWrapper::FlightModeCallback(const ff_msgs::FlightMode& flight_mode) { + const auto fan_speed_mode = lm::ConvertFanSpeedMode(flight_mode.speed); + if (Initialized()) graph_vio_->SetFanSpeedMode(fan_speed_mode); + imu_bias_initializer_->AddFanSpeedModeMeasurement(fan_speed_mode); +} + +void RosGraphVIOWrapper::Update() { + if (Initialized()) graph_vio_->Update(); +} + +bool RosGraphVIOWrapper::Initialized() const { return graph_vio_ != nullptr; } + +void RosGraphVIOWrapper::ResetVIO() { + LogInfo("ResetVIO: Resetting vio."); + if (!Initialized()) { + LogError("ResetVIO: VIO not initialized, nothing to do."); + return; + } + const auto latest_combined_nav_state = graph_vio_->combined_nav_state_nodes().LatestNode(); + if (!latest_combined_nav_state) { + LogError("ResetVIO: Failed to get latest combined nav state."); + return; + } + const auto keys = graph_vio_->combined_nav_state_nodes().Keys(latest_combined_nav_state->timestamp()); + if (keys.empty() || keys.size() != 3) { + LogError("ResetVIO: Failed to get latest keys."); + return; + } + + const auto imu_bias_covariance = graph_vio_->Covariance(keys[2]); + if (!imu_bias_covariance) { + LogError("ResetVIO: Failed to get latest combined nav state and IMU bias covariance."); + return; + } + const Eigen::Vector3d accelerometer_bias_stddev(std::sqrt((*imu_bias_covariance)(0, 0)), + std::sqrt((*imu_bias_covariance)(1, 1)), + std::sqrt((*imu_bias_covariance)(2, 2))); + const Eigen::Vector3d gyro_bias_stddev(std::sqrt((*imu_bias_covariance)(3, 3)), + std::sqrt((*imu_bias_covariance)(4, 4)), + std::sqrt((*imu_bias_covariance)(5, 5))); + imu_bias_initializer_->UpdateBias( + ImuBiasWithStddev(latest_combined_nav_state->bias(), accelerometer_bias_stddev, gyro_bias_stddev)); + graph_vio_.reset(); +} + +void RosGraphVIOWrapper::ResetBiasesAndVIO() { + LogInfo("ResetBiasAndVIO: Resetting biases and vio."); + imu_bias_initializer_->Reset(); + graph_vio_.reset(); +} + +void RosGraphVIOWrapper::ResetBiasesFromFileAndResetVIO() { + LogInfo("ResetBiasAndVIO: Resetting biases from file and resetting vio."); + imu_bias_initializer_->LoadFromFile(); + graph_vio_.reset(); +} + +boost::optional<ff_msgs::GraphVIOState> RosGraphVIOWrapper::GraphVIOStateMsg() { + if (!Initialized()) { + LogDebugEveryN(200, "Graph VIO not yet initialized."); + return boost::none; + } + const auto& nodes = graph_vio_->combined_nav_state_nodes(); + const auto times = nodes.Timestamps(); + // Avoid sending msgs before enough optical flow measurements have been incorporated in the graph + if (times.size() < 3) { + LogWarningEveryN(200, "Too few nodes in Graph VIO, waiting for more optical flow measurements."); + return boost::none; + } + + // Avoid sending repeat msgs + if (times.empty() || (latest_msg_time_ && times.back() == *latest_msg_time_)) { + LogDebugEveryN(2000, "No new VIO states."); + return boost::none; + } + const lc::Time latest_time = times.back(); + ff_msgs::GraphVIOState msg; + + // Only add latest state since this is used for localization relative factors + const auto combined_nav_state = nodes.Node(latest_time); + const auto keys = nodes.Keys(latest_time); + if (!combined_nav_state || keys.empty() || keys.size() != 3) { + LogError("CombinedNavStateArrayMsg: Failed to get combined nav state and keys."); + return boost::none; + } + const auto pose_covariance = graph_vio_->Covariance(keys[0]); + const auto velocity_covariance = graph_vio_->Covariance(keys[1]); + const auto imu_bias_covariance = graph_vio_->Covariance(keys[2]); + if (!pose_covariance || !velocity_covariance || !imu_bias_covariance) { + LogError("CombinedNavStateArrayMsg: Failed to get combined nav state covariances."); + return boost::none; + } + + msg.combined_nav_states.combined_nav_states.push_back( + lc::CombinedNavStateToMsg(*combined_nav_state, *pose_covariance, *velocity_covariance, *imu_bias_covariance)); + + lc::TimeToHeader(*(nodes.LatestTimestamp()), msg.header); + msg.child_frame_id = "odom"; + msg.standstill = graph_vio_->standstill(); + msg.estimating_bias = !Initialized() && !imu_bias_initializer_->Bias(); + msg.num_detected_of_features = graph_vio_->feature_tracker().size(); + msg.num_of_factors = graph_vio_->NumFactors<factor_adders::RobustSmartFactor>(); + msg.num_depth_factors = graph_vio_->NumFactors<gtsam::PointToPointBetweenFactor>(); + msg.optimization_iterations = graph_vio_->optimization_iterations_averager().last_value(); + msg.optimization_time = graph_vio_->optimization_timer().last_value(); + msg.update_time = graph_vio_->update_timer().last_value(); + // Divide values by three since each state contains three values (pose, velocity, biases) + msg.num_states = graph_vio_->num_values() / 3.0; + msg.duration = graph_vio_->Duration(); + latest_msg_time_ = latest_time; + return msg; +} + +const std::unique_ptr<graph_vio::GraphVIO>& RosGraphVIOWrapper::graph_vio() const { return graph_vio_; } + +std::unique_ptr<graph_vio::GraphVIO>& RosGraphVIOWrapper::graph_vio() { return graph_vio_; } +} // namespace ros_graph_vio diff --git a/localization/ros_graph_vio/test/test_ros_graph_vio_parameter_reader.cc b/localization/ros_graph_vio/test/test_ros_graph_vio_parameter_reader.cc new file mode 100644 index 0000000000..93c7f11c78 --- /dev/null +++ b/localization/ros_graph_vio/test/test_ros_graph_vio_parameter_reader.cc @@ -0,0 +1,69 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <node_adders/utilities.h> +#include <ros_graph_vio/parameter_reader.h> + +#include <ros/package.h> + +#include <gtest/gtest.h> + +namespace lc = localization_common; +namespace rv = ros_graph_vio; + +class RosGraphVIOParameterReaderTest : public ::testing::Test { + public: + RosGraphVIOParameterReaderTest() {} + + void SetUp() final { + lc::SetEnvironmentConfigs(); + config_reader::ConfigReader config; + config.AddFile("localization/imu_bias_initializer.config"); + config.AddFile("localization/imu_filter.config"); + config.AddFile("localization/ros_graph_vio.config"); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + + rv::LoadImuBiasInitializerParams(config, imu_params_); + } + + rv::ImuBiasInitializerParams imu_params_; +}; + +TEST_F(RosGraphVIOParameterReaderTest, ImuBiasInitializerParams) { + const std::string astrobee_configs_path = ros::package::getPath("astrobee"); + EXPECT_EQ(imu_params_.imu_bias_filename, astrobee_configs_path + "/resources/imu_bias.config"); + EXPECT_EQ(imu_params_.num_bias_estimation_measurements, 100); + // IMU filter + EXPECT_EQ(imu_params_.filter.quiet_accel, "ButterO3S125Lp3N33_33"); + EXPECT_EQ(imu_params_.filter.quiet_ang_vel, "ButterO1S125Lp3N33_33"); + EXPECT_EQ(imu_params_.filter.nominal_accel, "ButterO3S125Lp3N41_66"); + EXPECT_EQ(imu_params_.filter.nominal_ang_vel, "ButterO1S125Lp3N41_66"); + EXPECT_EQ(imu_params_.filter.aggressive_accel, "ButterO3S125Lp3N46_66"); + EXPECT_EQ(imu_params_.filter.aggressive_ang_vel, "ButterO1S125Lp3N46_66"); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/ros_graph_vio/test/test_ros_graph_vio_parameter_reader.test b/localization/ros_graph_vio/test/test_ros_graph_vio_parameter_reader.test new file mode 100644 index 0000000000..ae811eeca3 --- /dev/null +++ b/localization/ros_graph_vio/test/test_ros_graph_vio_parameter_reader.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="ros_graph_vio" type="test_ros_graph_vio_parameter_reader" test-name="test_ros_graph_vio_parameter_reader" /> +</launch> diff --git a/localization/imu_augmentor/CMakeLists.txt b/localization/ros_pose_extrapolator/CMakeLists.txt similarity index 76% rename from localization/imu_augmentor/CMakeLists.txt rename to localization/ros_pose_extrapolator/CMakeLists.txt index de41cf6c1f..16b52ca4e6 100644 --- a/localization/imu_augmentor/CMakeLists.txt +++ b/localization/ros_pose_extrapolator/CMakeLists.txt @@ -16,7 +16,7 @@ #under the License. cmake_minimum_required(VERSION 3.0) -project(imu_augmentor) +project(ros_pose_extrapolator) ## Compile as C++14, supported in ROS Kinetic and newer add_compile_options(-std=c++14) @@ -29,22 +29,24 @@ find_package(Eigen3 REQUIRED) ## Find catkin macros and libraries find_package(catkin2 REQUIRED COMPONENTS - nodelet ff_util imu_integration localization_common localization_measurements + nodelet + parameter_reader ) catkin_package( INCLUDE_DIRS include ${GTSAM_INCLUDE_DIR} ${GLOG_INCLUDE_DIRS} LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} gtsam CATKIN_DEPENDS - nodelet ff_util imu_integration localization_common localization_measurements + nodelet + parameter_reader ) ########### @@ -61,31 +63,29 @@ include_directories( # Declare C++ libraries add_library(${PROJECT_NAME} - src/imu_augmentor.cc - src/imu_augmentor_nodelet.cc - src/imu_augmentor_wrapper.cc + src/parameter_reader.cc + src/ros_pose_extrapolator_nodelet.cc + src/ros_pose_extrapolator_wrapper.cc ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(test_imu_augmentor - test/test_imu_augmentor.test - test/test_imu_augmentor.cc - test/test_utilities.cc - ) - target_link_libraries(test_imu_augmentor - imu_augmentor ${catkin_LIBRARIES} - ) - find_package(rostest REQUIRED) - add_rostest_gtest(test_imu_augmentor_wrapper - test/test_imu_augmentor_wrapper.test - test/test_imu_augmentor_wrapper.cc - test/test_utilities.cc + find_package(rostest REQUIRED) + # add_rostest_gtest(test_ros_pose_extrapolator_wrapper + # test/test_ros_pose_extrapolator_wrapper.test + # test/test_ros_pose_extrapolator_wrapper.cc + # test/test_utilities.cc + # ) + # target_link_libraries(test_ros_pose_extrapolator_wrapper + # ros_pose_extrapolator ${catkin_LIBRARIES} + # ) + add_rostest_gtest(test_ros_pose_extrapolator_parameter_reader + test/test_ros_pose_extrapolator_parameter_reader.test + test/test_ros_pose_extrapolator_parameter_reader.cc ) - target_link_libraries(test_imu_augmentor_wrapper - imu_augmentor ${catkin_LIBRARIES} + target_link_libraries(test_ros_pose_extrapolator_parameter_reader + ${PROJECT_NAME} ${catkin_LIBRARIES} ) endif() diff --git a/localization/ros_pose_extrapolator/include/ros_pose_extrapolator/parameter_reader.h b/localization/ros_pose_extrapolator/include/ros_pose_extrapolator/parameter_reader.h new file mode 100644 index 0000000000..10e5b7188d --- /dev/null +++ b/localization/ros_pose_extrapolator/include/ros_pose_extrapolator/parameter_reader.h @@ -0,0 +1,31 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef ROS_POSE_EXTRAPOLATOR_PARAMETER_READER_H_ +#define ROS_POSE_EXTRAPOLATOR_PARAMETER_READER_H_ + +#include <config_reader/config_reader.h> +#include <ros_pose_extrapolator/ros_pose_extrapolator_params.h> + +#include <string> + +namespace ros_pose_extrapolator { +void LoadRosPoseExtrapolatorParams(config_reader::ConfigReader& config, RosPoseExtrapolatorParams& params, + const std::string& prefix = ""); +} // namespace ros_pose_extrapolator + +#endif // ROS_POSE_EXTRAPOLATOR_PARAMETER_READER_H_ diff --git a/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_nodelet.h b/localization/ros_pose_extrapolator/include/ros_pose_extrapolator/ros_pose_extrapolator_nodelet.h similarity index 63% rename from localization/imu_augmentor/include/imu_augmentor/imu_augmentor_nodelet.h rename to localization/ros_pose_extrapolator/include/ros_pose_extrapolator/ros_pose_extrapolator_nodelet.h index a8f2609124..1aee41e207 100644 --- a/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_nodelet.h +++ b/localization/ros_pose_extrapolator/include/ros_pose_extrapolator/ros_pose_extrapolator_nodelet.h @@ -15,15 +15,16 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef IMU_AUGMENTOR_IMU_AUGMENTOR_NODELET_H_ -#define IMU_AUGMENTOR_IMU_AUGMENTOR_NODELET_H_ +#ifndef ROS_POSE_EXTRAPOLATOR_ROS_POSE_EXTRAPOLATOR_NODELET_H_ +#define ROS_POSE_EXTRAPOLATOR_ROS_POSE_EXTRAPOLATOR_NODELET_H_ #include <ff_msgs/EkfState.h> #include <ff_msgs/FlightMode.h> -#include <ff_msgs/GraphState.h> +#include <ff_msgs/GraphLocState.h> +#include <ff_msgs/GraphVIOState.h> #include <ff_msgs/Heartbeat.h> #include <ff_util/ff_nodelet.h> -#include <imu_augmentor/imu_augmentor_wrapper.h> +#include <ros_pose_extrapolator/ros_pose_extrapolator_wrapper.h> #include <ros/node_handle.h> #include <ros/publisher.h> @@ -35,10 +36,10 @@ #include <string> -namespace imu_augmentor { -class ImuAugmentorNodelet : public ff_util::FreeFlyerNodelet { +namespace ros_pose_extrapolator { +class RosPoseExtrapolatorNodelet : public ff_util::FreeFlyerNodelet { public: - ImuAugmentorNodelet(); + RosPoseExtrapolatorNodelet(); private: void Initialize(ros::NodeHandle* nh) final; @@ -49,9 +50,11 @@ class ImuAugmentorNodelet : public ff_util::FreeFlyerNodelet { void FlightModeCallback(ff_msgs::FlightMode::ConstPtr const& mode); - void LocalizationStateCallback(const ff_msgs::GraphState::ConstPtr& loc_msg); + void LocalizationStateCallback(const ff_msgs::GraphLocState::ConstPtr& graph_loc_state_msg); - boost::optional<ff_msgs::EkfState> PublishLatestImuAugmentedLocalizationState(); + void GraphVIOStateCallback(const ff_msgs::GraphVIOState::ConstPtr& graph_vio_state_msg); + + boost::optional<ff_msgs::EkfState> PublishLatestExtrapolatedLocalizationState(); void PublishPoseAndTwistAndTransform(const ff_msgs::EkfState& loc_msg); @@ -59,11 +62,11 @@ class ImuAugmentorNodelet : public ff_util::FreeFlyerNodelet { void PublishHeartbeat(); - imu_augmentor::ImuAugmentorWrapper imu_augmentor_wrapper_; + ros_pose_extrapolator::RosPoseExtrapolatorWrapper ros_pose_extrapolator_wrapper_; std::string platform_name_; - ros::NodeHandle imu_nh_, loc_nh_; - ros::CallbackQueue imu_queue_, loc_queue_; - ros::Subscriber imu_sub_, flight_mode_sub_, state_sub_; + ros::NodeHandle imu_nh_, loc_nh_, vio_nh_; + ros::CallbackQueue imu_queue_, loc_queue_, vio_queue_; + ros::Subscriber imu_sub_, flight_mode_sub_, loc_sub_, vio_sub_; ros::Publisher state_pub_, pose_pub_, twist_pub_, heartbeat_pub_; ff_msgs::Heartbeat heartbeat_; tf2_ros::TransformBroadcaster transform_pub_; @@ -71,6 +74,6 @@ class ImuAugmentorNodelet : public ff_util::FreeFlyerNodelet { ros::Time last_heartbeat_time_; boost::optional<ros::Time> last_state_msg_time_; }; -} // namespace imu_augmentor +} // namespace ros_pose_extrapolator -#endif // IMU_AUGMENTOR_IMU_AUGMENTOR_NODELET_H_ +#endif // ROS_POSE_EXTRAPOLATOR_ROS_POSE_EXTRAPOLATOR_NODELET_H_ diff --git a/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_params.h b/localization/ros_pose_extrapolator/include/ros_pose_extrapolator/ros_pose_extrapolator_params.h similarity index 68% rename from localization/imu_augmentor/include/imu_augmentor/imu_augmentor_params.h rename to localization/ros_pose_extrapolator/include/ros_pose_extrapolator/ros_pose_extrapolator_params.h index 1af9af07e9..a4e6432472 100644 --- a/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_params.h +++ b/localization/ros_pose_extrapolator/include/ros_pose_extrapolator/ros_pose_extrapolator_params.h @@ -15,15 +15,17 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef IMU_AUGMENTOR_IMU_AUGMENTOR_PARAMS_H_ -#define IMU_AUGMENTOR_IMU_AUGMENTOR_PARAMS_H_ +#ifndef ROS_POSE_EXTRAPOLATOR_ROS_POSE_EXTRAPOLATOR_PARAMS_H_ +#define ROS_POSE_EXTRAPOLATOR_ROS_POSE_EXTRAPOLATOR_PARAMS_H_ #include <imu_integration/imu_integrator_params.h> -namespace imu_augmentor { -struct ImuAugmentorParams : imu_integration::ImuIntegratorParams { +namespace ros_pose_extrapolator { +struct RosPoseExtrapolatorParams { + imu_integration::ImuIntegratorParams imu_integrator; bool standstill_enabled; + int max_relative_vio_buffer_size; }; -} // namespace imu_augmentor +} // namespace ros_pose_extrapolator -#endif // IMU_AUGMENTOR_IMU_AUGMENTOR_PARAMS_H_ +#endif // ROS_POSE_EXTRAPOLATOR_ROS_POSE_EXTRAPOLATOR_PARAMS_H_ diff --git a/localization/ros_pose_extrapolator/include/ros_pose_extrapolator/ros_pose_extrapolator_wrapper.h b/localization/ros_pose_extrapolator/include/ros_pose_extrapolator/ros_pose_extrapolator_wrapper.h new file mode 100644 index 0000000000..83aa2476c0 --- /dev/null +++ b/localization/ros_pose_extrapolator/include/ros_pose_extrapolator/ros_pose_extrapolator_wrapper.h @@ -0,0 +1,92 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef ROS_POSE_EXTRAPOLATOR_ROS_POSE_EXTRAPOLATOR_WRAPPER_H_ +#define ROS_POSE_EXTRAPOLATOR_ROS_POSE_EXTRAPOLATOR_WRAPPER_H_ + +#include <ff_msgs/EkfState.h> +#include <ff_msgs/FlightMode.h> +#include <ff_msgs/GraphLocState.h> +#include <ff_msgs/GraphVIOState.h> +#include <imu_integration/imu_integrator.h> +#include <localization_common/combined_nav_state.h> +#include <localization_common/combined_nav_state_covariances.h> +#include <localization_common/pose_interpolater.h> +#include <localization_common/rate_timer.h> +#include <localization_measurements/imu_measurement.h> +#include <ros_pose_extrapolator/ros_pose_extrapolator_params.h> + +#include <geometry_msgs/PoseWithCovarianceStamped.h> +#include <sensor_msgs/Imu.h> + +#include <memory> +#include <string> +#include <utility> + +namespace ros_pose_extrapolator { +// Extrapolates latest localization pose using VIO odometry and IMU measurements. +// No extrapolation is performed during standstill to avoid adding jittery IMU data. +class RosPoseExtrapolatorWrapper { + public: + explicit RosPoseExtrapolatorWrapper(const std::string& graph_config_path_prefix = ""); + + explicit RosPoseExtrapolatorWrapper(const RosPoseExtrapolatorParams& params); + + // Updates latest loc pose with msg pose. + void LocalizationStateCallback(const ff_msgs::GraphLocState& graph_loc_state_msg); + + // Updates odometry pose interpolator with latest VIO msg pose. + // Also stores latest velocity and bias estimates for future creation of latest + // combined nav state. + void GraphVIOStateCallback(const ff_msgs::GraphVIOState& graph_vio_state_msg); + + // Updates IMU interpolator with latest IMU measurement. + void ImuCallback(const sensor_msgs::Imu& imu_msg); + + // Updates IMU interpolator with latest flight mode to adjust IMU measurement filter appropriately. + void FlightModeCallback(const ff_msgs::FlightMode& flight_mode); + + // Extrapolates latest loc pose using relative VIO odometry measurements and finally + // interpolated IMU data for timestamps past the last VIO measurement. + boost::optional<std::pair<localization_common::CombinedNavState, localization_common::CombinedNavStateCovariances>> + LatestExtrapolatedStateAndCovariances(); + + // Constructs EkfState msg (historically named, contains pose/velocity/bias and covariances) + // using extrapolated loc pose and covariance. + boost::optional<ff_msgs::EkfState> LatestExtrapolatedLocalizationMsg(); + + private: + // Initializes extrapolator based on provided params. + void Initialize(const RosPoseExtrapolatorParams& params); + + // Returns if currently in standstill. Uses latest VIO measurement to determine this. + bool standstill() const; + + std::unique_ptr<imu_integration::ImuIntegrator> imu_integrator_; + boost::optional<ff_msgs::GraphLocState> latest_loc_msg_; + boost::optional<ff_msgs::GraphVIOState> latest_vio_msg_; + boost::optional<localization_common::CombinedNavState> latest_extrapolated_vio_state_; + boost::optional<localization_common::CombinedNavStateCovariances> latest_vio_state_covariances_; + boost::optional<gtsam::Pose3> world_T_odom_; + std::unique_ptr<gtsam::TangentPreintegration> preintegration_helper_; + RosPoseExtrapolatorParams params_; + boost::optional<bool> standstill_; + localization_common::RateTimer loc_state_timer_ = localization_common::RateTimer("Loc State Msg"); + localization_common::PoseInterpolater odom_interpolator_; +}; +} // namespace ros_pose_extrapolator +#endif // ROS_POSE_EXTRAPOLATOR_ROS_POSE_EXTRAPOLATOR_WRAPPER_H_ diff --git a/localization/imu_augmentor/launch/imu_augmentor.launch b/localization/ros_pose_extrapolator/launch/ros_pose_extrapolator.launch similarity index 92% rename from localization/imu_augmentor/launch/imu_augmentor.launch rename to localization/ros_pose_extrapolator/launch/ros_pose_extrapolator.launch index fd7fabc34f..761d779f2c 100644 --- a/localization/imu_augmentor/launch/imu_augmentor.launch +++ b/localization/ros_pose_extrapolator/launch/ros_pose_extrapolator.launch @@ -15,13 +15,13 @@ <!-- implied. See the License for the specific language governing --> <!-- permissions and limitations under the License. --> <launch> - <arg name="name" default="imu_augmentor" /> + <arg name="name" default="ros_pose_extrapolator" /> <arg name="manager" default="" /> <arg name="terminal" default="" /> <include file="$(find ff_util)/launch/ff_nodelet.launch"> <arg name="name" value="$(arg name)" /> <arg name="manager" value="$(arg manager)" /> <arg name="terminal" value="$(arg terminal)" /> - <arg name="class" value="imu_augmentor/ImuAugmentorNodelet" /> + <arg name="class" value="ros_pose_extrapolator/RosPoseExtrapolatorNodelet" /> </include> </launch> diff --git a/localization/ros_pose_extrapolator/nodelet_plugins.xml b/localization/ros_pose_extrapolator/nodelet_plugins.xml new file mode 100644 index 0000000000..324b476926 --- /dev/null +++ b/localization/ros_pose_extrapolator/nodelet_plugins.xml @@ -0,0 +1,11 @@ +<library path="lib/libros_pose_extrapolator"> + <class name="ros_pose_extrapolator/RosPoseExtrapolatorNodelet" + type="ros_pose_extrapolator::RosPoseExtrapolatorNodelet" + base_class_type="nodelet::Nodelet"> + <description> + This nodelet wraps the ros_pose_extrapolator + </description> + </class> +</library> + + diff --git a/localization/imu_augmentor/package.xml b/localization/ros_pose_extrapolator/package.xml similarity index 84% rename from localization/imu_augmentor/package.xml rename to localization/ros_pose_extrapolator/package.xml index 503996e830..4f6b97f487 100644 --- a/localization/imu_augmentor/package.xml +++ b/localization/ros_pose_extrapolator/package.xml @@ -1,8 +1,8 @@ <package> - <name>imu_augmentor</name> + <name>ros_pose_extrapolator</name> <version>1.0.0</version> <description> - Imu augmentor package + Ros pose extrapolator package </description> <license> Apache License, Version 2.0 @@ -14,16 +14,18 @@ Astrobee Flight Software </maintainer> <buildtool_depend>catkin</buildtool_depend> - <build_depend>nodelet</build_depend> <build_depend>ff_util</build_depend> <build_depend>imu_integration</build_depend> <build_depend>localization_common</build_depend> <build_depend>localization_measurements</build_depend> - <run_depend>nodelet</run_depend> + <build_depend>nodelet</build_depend> + <build_depend>parameter_reader</build_depend> <run_depend>ff_util</run_depend> <run_depend>imu_integration</run_depend> <run_depend>localization_common</run_depend> <run_depend>localization_measurements</run_depend> + <run_depend>nodelet</run_depend> + <run_depend>parameter_reader</run_depend> <export> <nodelet plugin="${prefix}/nodelet_plugins.xml" /> </export> diff --git a/localization/ros_pose_extrapolator/readme.md b/localization/ros_pose_extrapolator/readme.md new file mode 100644 index 0000000000..82e2cb5da5 --- /dev/null +++ b/localization/ros_pose_extrapolator/readme.md @@ -0,0 +1,19 @@ +\page rosposeextrapolator Ros Pose Extrapolator + +# Package Overview +Extrapolates localization estimates using relative odometry and IMU measurements. +The ros pose extrapolator wrapper takes ros messages, converts these to localization measurements, and performs the required extrapolation. The ros pose extrapolator nodelet handles live subscribing and publishing to ros topics. + +## RosPoseExtrapolatorWrapper +Converts ROS messages to localization measurements and extrapolates localization estimates using relative odometry and interpolated IMU measurements. + +## RosPoseExtrapolatorNodelet +Subscribes to ROS messages for online use and publishes localization messages and TFs. Contains a RosPoseExtrapolatorWrapper and passes messages to this. + +# Inputs +* `/hw/imu` +* `/graph_localizer/state` +* `/graph_vio/state` + +# Outputs +* `/gnc/Ekf` (message named Ekf for historical reasons) diff --git a/localization/ros_pose_extrapolator/src/parameter_reader.cc b/localization/ros_pose_extrapolator/src/parameter_reader.cc new file mode 100644 index 0000000000..ae7c2c7da6 --- /dev/null +++ b/localization/ros_pose_extrapolator/src/parameter_reader.cc @@ -0,0 +1,33 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <parameter_reader/imu_integration.h> +#include <ros_pose_extrapolator/parameter_reader.h> +#include <msg_conversions/msg_conversions.h> + +namespace ros_pose_extrapolator { +namespace mc = msg_conversions; +namespace pr = parameter_reader; + +void LoadRosPoseExtrapolatorParams(config_reader::ConfigReader& config, RosPoseExtrapolatorParams& params, + const std::string& prefix) { + pr::LoadImuIntegratorParams(config, params.imu_integrator); + LOAD_PARAM(params.standstill_enabled, config, prefix + "rpe_"); + LOAD_PARAM(params.max_relative_vio_buffer_size, config, prefix + "rpe_"); +} +} // namespace ros_pose_extrapolator diff --git a/localization/imu_augmentor/src/imu_augmentor_nodelet.cc b/localization/ros_pose_extrapolator/src/ros_pose_extrapolator_nodelet.cc similarity index 56% rename from localization/imu_augmentor/src/imu_augmentor_nodelet.cc rename to localization/ros_pose_extrapolator/src/ros_pose_extrapolator_nodelet.cc index 15e4ca1504..2160f6ac9c 100644 --- a/localization/imu_augmentor/src/imu_augmentor_nodelet.cc +++ b/localization/ros_pose_extrapolator/src/ros_pose_extrapolator_nodelet.cc @@ -17,26 +17,27 @@ */ #include <ff_common/ff_names.h> -#include <imu_augmentor/imu_augmentor_nodelet.h> #include <localization_common/logger.h> #include <localization_common/utilities.h> +#include <ros_pose_extrapolator/ros_pose_extrapolator_nodelet.h> #include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/TwistStamped.h> -namespace imu_augmentor { +namespace ros_pose_extrapolator { namespace lc = localization_common; -ImuAugmentorNodelet::ImuAugmentorNodelet() : ff_util::FreeFlyerNodelet(NODE_IMU_AUG, true) { +RosPoseExtrapolatorNodelet::RosPoseExtrapolatorNodelet() : ff_util::FreeFlyerNodelet(NODE_POSE_EXTR, true) { imu_nh_.setCallbackQueue(&imu_queue_); loc_nh_.setCallbackQueue(&loc_queue_); + vio_nh_.setCallbackQueue(&vio_queue_); heartbeat_.node = GetName(); heartbeat_.nodelet_manager = ros::this_node::getName(); last_time_ = ros::Time::now(); last_heartbeat_time_ = ros::Time::now(); } -void ImuAugmentorNodelet::Initialize(ros::NodeHandle* nh) { +void RosPoseExtrapolatorNodelet::Initialize(ros::NodeHandle* nh) { // Setup the platform name platform_name_ = GetPlatform(); platform_name_ = (platform_name_.empty() ? "" : platform_name_ + "/"); @@ -45,46 +46,53 @@ void ImuAugmentorNodelet::Initialize(ros::NodeHandle* nh) { Run(); } -void ImuAugmentorNodelet::SubscribeAndAdvertise(ros::NodeHandle* nh) { +void RosPoseExtrapolatorNodelet::SubscribeAndAdvertise(ros::NodeHandle* nh) { state_pub_ = nh->advertise<ff_msgs::EkfState>(TOPIC_GNC_EKF, 1); pose_pub_ = nh->advertise<geometry_msgs::PoseStamped>(TOPIC_LOCALIZATION_POSE, 1); twist_pub_ = nh->advertise<geometry_msgs::TwistStamped>(TOPIC_LOCALIZATION_TWIST, 1); heartbeat_pub_ = nh->advertise<ff_msgs::Heartbeat>(TOPIC_HEARTBEAT, 5, true); - imu_sub_ = imu_nh_.subscribe(TOPIC_HARDWARE_IMU, 100, &ImuAugmentorNodelet::ImuCallback, this, + imu_sub_ = imu_nh_.subscribe(TOPIC_HARDWARE_IMU, 100, &RosPoseExtrapolatorNodelet::ImuCallback, this, ros::TransportHints().tcpNoDelay()); // Use the imu nh so that speed mode changes arrive in order wrt IMU msgs - flight_mode_sub_ = imu_nh_.subscribe(TOPIC_MOBILITY_FLIGHT_MODE, 10, &ImuAugmentorNodelet::FlightModeCallback, this); - state_sub_ = loc_nh_.subscribe(TOPIC_GRAPH_LOC_STATE, 1, &ImuAugmentorNodelet::LocalizationStateCallback, this, - ros::TransportHints().tcpNoDelay()); + flight_mode_sub_ = + imu_nh_.subscribe(TOPIC_MOBILITY_FLIGHT_MODE, 10, &RosPoseExtrapolatorNodelet::FlightModeCallback, this); + loc_sub_ = loc_nh_.subscribe(TOPIC_GRAPH_LOC_STATE, 1, &RosPoseExtrapolatorNodelet::LocalizationStateCallback, this, + ros::TransportHints().tcpNoDelay()); + vio_sub_ = vio_nh_.subscribe(TOPIC_GRAPH_VIO_STATE, 1, &RosPoseExtrapolatorNodelet::GraphVIOStateCallback, this, + ros::TransportHints().tcpNoDelay()); +} + +void RosPoseExtrapolatorNodelet::ImuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg) { + ros_pose_extrapolator_wrapper_.ImuCallback(*imu_msg); } -void ImuAugmentorNodelet::ImuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg) { - imu_augmentor_wrapper_.ImuCallback(*imu_msg); +void RosPoseExtrapolatorNodelet::FlightModeCallback(ff_msgs::FlightMode::ConstPtr const& mode) { + ros_pose_extrapolator_wrapper_.FlightModeCallback(*mode); } -void ImuAugmentorNodelet::FlightModeCallback(ff_msgs::FlightMode::ConstPtr const& mode) { - imu_augmentor_wrapper_.FlightModeCallback(*mode); +void RosPoseExtrapolatorNodelet::GraphVIOStateCallback(const ff_msgs::GraphVIOState::ConstPtr& graph_vio_state_msg) { + ros_pose_extrapolator_wrapper_.GraphVIOStateCallback(*graph_vio_state_msg); } -void ImuAugmentorNodelet::LocalizationStateCallback(const ff_msgs::GraphState::ConstPtr& loc_msg) { - imu_augmentor_wrapper_.LocalizationStateCallback(*loc_msg); +void RosPoseExtrapolatorNodelet::LocalizationStateCallback(const ff_msgs::GraphLocState::ConstPtr& loc_msg) { + ros_pose_extrapolator_wrapper_.LocalizationStateCallback(*loc_msg); } -boost::optional<ff_msgs::EkfState> ImuAugmentorNodelet::PublishLatestImuAugmentedLocalizationState() { - const auto latest_imu_augmented_loc_msg = imu_augmentor_wrapper_.LatestImuAugmentedLocalizationMsg(); - if (!latest_imu_augmented_loc_msg) { - LogDebugEveryN(100, "PublishLatestImuAugmentedLocalizationState: Failed to get latest imu augmented loc msg."); +boost::optional<ff_msgs::EkfState> RosPoseExtrapolatorNodelet::PublishLatestExtrapolatedLocalizationState() { + const auto latest_extrapolated_loc_msg = ros_pose_extrapolator_wrapper_.LatestExtrapolatedLocalizationMsg(); + if (!latest_extrapolated_loc_msg) { + LogDebugEveryN(100, "PublishLatestExtrapolatedLocalizationState: Failed to get latest imu augmented loc msg."); return boost::none; } // Avoid sending repeat messages - if (last_state_msg_time_ && (latest_imu_augmented_loc_msg->header.stamp == *last_state_msg_time_)) return boost::none; - state_pub_.publish(*latest_imu_augmented_loc_msg); - last_state_msg_time_ = latest_imu_augmented_loc_msg->header.stamp; - return latest_imu_augmented_loc_msg; + if (last_state_msg_time_ && (latest_extrapolated_loc_msg->header.stamp == *last_state_msg_time_)) return boost::none; + state_pub_.publish(*latest_extrapolated_loc_msg); + last_state_msg_time_ = latest_extrapolated_loc_msg->header.stamp; + return latest_extrapolated_loc_msg; } -void ImuAugmentorNodelet::PublishPoseAndTwistAndTransform(const ff_msgs::EkfState& loc_msg) { +void RosPoseExtrapolatorNodelet::PublishPoseAndTwistAndTransform(const ff_msgs::EkfState& loc_msg) { // Publish pose geometry_msgs::PoseStamped pose_msg; pose_msg.header = loc_msg.header; @@ -114,19 +122,20 @@ void ImuAugmentorNodelet::PublishPoseAndTwistAndTransform(const ff_msgs::EkfStat transform_pub_.sendTransform(transform_msg); } -void ImuAugmentorNodelet::PublishHeartbeat() { +void RosPoseExtrapolatorNodelet::PublishHeartbeat() { heartbeat_.header.stamp = ros::Time::now(); if ((heartbeat_.header.stamp - last_heartbeat_time_).toSec() < 1.0) return; heartbeat_pub_.publish(heartbeat_); last_heartbeat_time_ = heartbeat_.header.stamp; } -void ImuAugmentorNodelet::Run() { +void RosPoseExtrapolatorNodelet::Run() { ros::Rate rate(100); while (ros::ok()) { imu_queue_.callAvailable(); + vio_queue_.callAvailable(); loc_queue_.callAvailable(); - const auto loc_msg = PublishLatestImuAugmentedLocalizationState(); + const auto loc_msg = PublishLatestExtrapolatedLocalizationState(); if (loc_msg) { PublishPoseAndTwistAndTransform(*loc_msg); } @@ -134,6 +143,6 @@ void ImuAugmentorNodelet::Run() { rate.sleep(); } } -} // namespace imu_augmentor +} // namespace ros_pose_extrapolator -PLUGINLIB_EXPORT_CLASS(imu_augmentor::ImuAugmentorNodelet, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(ros_pose_extrapolator::RosPoseExtrapolatorNodelet, nodelet::Nodelet); diff --git a/localization/ros_pose_extrapolator/src/ros_pose_extrapolator_wrapper.cc b/localization/ros_pose_extrapolator/src/ros_pose_extrapolator_wrapper.cc new file mode 100644 index 0000000000..7cd94489e2 --- /dev/null +++ b/localization/ros_pose_extrapolator/src/ros_pose_extrapolator_wrapper.cc @@ -0,0 +1,203 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <config_reader/config_reader.h> +#include <imu_integration/utilities.h> +#include <localization_common/logger.h> +#include <localization_common/utilities.h> +#include <localization_measurements/imu_measurement.h> +#include <localization_measurements/measurement_conversions.h> +#include <msg_conversions/msg_conversions.h> +#include <ros_pose_extrapolator/parameter_reader.h> +#include <ros_pose_extrapolator/ros_pose_extrapolator_wrapper.h> + +namespace ros_pose_extrapolator { +namespace ii = imu_integration; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace mc = msg_conversions; + +RosPoseExtrapolatorWrapper::RosPoseExtrapolatorWrapper(const std::string& graph_config_path_prefix) { + config_reader::ConfigReader config; + config.AddFile("localization/imu_filter.config"); + config.AddFile("localization/imu_integrator.config"); + config.AddFile("localization/ros_pose_extrapolator.config"); + config.AddFile("transforms.config"); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + RosPoseExtrapolatorParams params; + LoadRosPoseExtrapolatorParams(config, params); + Initialize(params); +} + +RosPoseExtrapolatorWrapper::RosPoseExtrapolatorWrapper(const RosPoseExtrapolatorParams& params) { Initialize(params); } + +void RosPoseExtrapolatorWrapper::Initialize(const RosPoseExtrapolatorParams& params) { + params_ = params; + imu_integrator_.reset(new ii::ImuIntegrator(params_.imu_integrator)); + odom_interpolator_ = lc::PoseInterpolater(params_.max_relative_vio_buffer_size); + + // Preintegration_helper_ is only being used to frame change and remove centrifugal acceleration, so body_T_imu is the + // only parameter needed. + boost::shared_ptr<gtsam::PreintegrationParams> preintegration_params(new gtsam::PreintegrationParams()); + preintegration_params->setBodyPSensor(params_.imu_integrator.body_T_imu); + preintegration_helper_.reset(new gtsam::TangentPreintegration(preintegration_params, gtsam::imuBias::ConstantBias())); +} + +void RosPoseExtrapolatorWrapper::GraphVIOStateCallback(const ff_msgs::GraphVIOState& graph_vio_state_msg) { + latest_vio_msg_ = graph_vio_state_msg; + const auto& latest_state_msg = graph_vio_state_msg.combined_nav_states.combined_nav_states.back(); + const auto latest_vio_state = lc::CombinedNavStateFromMsg(latest_state_msg); + latest_vio_state_covariances_ = lc::CombinedNavStateCovariancesFromMsg(latest_state_msg); + // Reset extrapolated vio state when receive new VIO message so IMU extrapolation + // restarts using this state. + latest_extrapolated_vio_state_ = latest_vio_state; + odom_interpolator_.Add(latest_vio_state.timestamp(), lc::EigenPose(latest_vio_state.pose())); + standstill_ = graph_vio_state_msg.standstill; + // Remove old measurements no longer needed for extrapolation. + imu_integrator_->RemoveOldValues(latest_vio_state.timestamp()); +} + +void RosPoseExtrapolatorWrapper::LocalizationStateCallback(const ff_msgs::GraphLocState& loc_msg) { + latest_loc_msg_ = loc_msg; + const auto world_T_body = lc::PoseFromMsg(loc_msg.pose.pose); + // TODO(rsoussan): Store and use loc covariances + const auto loc_timestamp = lc::TimeFromHeader(loc_msg.header); + // Odom interpolator needs lower bound estimate for interpolation of first pose. + odom_interpolator_.RemoveBelowLowerBoundValues(loc_timestamp); + // Update world_T_odom estimate. Fix drift in odometry by getting odom_T_body + // at the same timestamp as world_T_body and computing offset wrt world frame. + const auto loc_timestamp_odom_T_body = odom_interpolator_.Interpolate(loc_timestamp); + if (!loc_timestamp_odom_T_body) { + LogError("LocalizationStateCallback: Failed to get odom_T_body for provided loc time."); + return; + } + const auto loc_timestamp_body_T_odom = lc::GtPose(*loc_timestamp_odom_T_body).inverse(); + world_T_odom_ = world_T_body * loc_timestamp_body_T_odom; +} + +bool RosPoseExtrapolatorWrapper::standstill() const { + if (!params_.standstill_enabled) return false; + // If uninitialized, return not at standstill + if (!standstill_) return false; + return *standstill_; +} + +void RosPoseExtrapolatorWrapper::ImuCallback(const sensor_msgs::Imu& imu_msg) { + imu_integrator_->AddImuMeasurement(lm::ImuMeasurement(imu_msg)); +} + +void RosPoseExtrapolatorWrapper::FlightModeCallback(const ff_msgs::FlightMode& flight_mode) { + imu_integrator_->SetFanSpeedMode(lm::ConvertFanSpeedMode(flight_mode.speed)); +} + +boost::optional<std::pair<lc::CombinedNavState, lc::CombinedNavStateCovariances>> +RosPoseExtrapolatorWrapper::LatestExtrapolatedStateAndCovariances() { + if (!world_T_odom_ || !latest_extrapolated_vio_state_) { + LogWarningEveryN(200, + "LatestExtrapolatedStateAndCovariances: Not enough information available to create desired data."); + return boost::none; + } + + // 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) { + const auto latest_extrapolated_state = imu_integrator_->ExtrapolateLatest(*latest_extrapolated_vio_state_); + if (!latest_extrapolated_state) { + LogError("LatestExtrapolatedCombinedNavStateAndCovariances: Failed to extrapolate latest vio state."); + return boost::none; + } + latest_extrapolated_vio_state_ = *latest_extrapolated_state; + } + + // Convert from odom frame to world frame + const gtsam::Pose3 extrapolated_world_T_body = (*world_T_odom_) * latest_extrapolated_vio_state_->pose(); + // Rotate body velocity from odom frame to world frame. + const gtsam::Vector3 extrapolated_world_F_body_velocity = + world_T_odom_->rotation() * latest_extrapolated_vio_state_->velocity(); + // Use latest bias estimate and use latest IMU time as extrapolated timestamp if available. + // Even if at standstill, the timestamp should be the latest one available. + const auto timestamp = imu_integrator_->LatestTimestamp() ? *(imu_integrator_->LatestTimestamp()) + : latest_extrapolated_vio_state_->timestamp(); + const lc::CombinedNavState extrapolated_state(extrapolated_world_T_body, extrapolated_world_F_body_velocity, + latest_extrapolated_vio_state_->bias(), timestamp); + + // TODO(rsoussan): propogate uncertainties from imu integrator, odom_interpolator, and localization estimate + return std::pair<lc::CombinedNavState, lc::CombinedNavStateCovariances>{extrapolated_state, + *latest_vio_state_covariances_}; +} + +boost::optional<ff_msgs::EkfState> RosPoseExtrapolatorWrapper::LatestExtrapolatedLocalizationMsg() { + if (!latest_loc_msg_ || !latest_vio_msg_) { + LogDebugEveryN(200, "LatestExtrapolatedLocalizationMsg: No latest loc msg available."); + return boost::none; + } + + const auto latest_extrapolated_state_and_covariances = LatestExtrapolatedStateAndCovariances(); + if (!latest_extrapolated_state_and_covariances) { + LogWarningEveryN( + 200, "LatestExtrapolatedLocalizationMsg: Failed to get latest imu augmented nav state and covariances."); + return boost::none; + } + + // Get feature counts and other info from latest_loc_msg + ff_msgs::EkfState latest_extrapolated_loc_msg; + latest_extrapolated_loc_msg.header = latest_loc_msg_->header; + latest_extrapolated_loc_msg.child_frame_id = latest_loc_msg_->child_frame_id; + // Controller can't handle a confidence other than 0. + latest_extrapolated_loc_msg.confidence = 0; + // Prevent overflow of uin8_t + latest_extrapolated_loc_msg.of_count = latest_vio_msg_->num_of_factors <= 255 ? latest_vio_msg_->num_of_factors : 255; + latest_extrapolated_loc_msg.ml_count = + latest_loc_msg_->num_ml_projection_factors <= 255 ? latest_loc_msg_->num_ml_projection_factors : 255; + latest_extrapolated_loc_msg.estimating_bias = latest_vio_msg_->estimating_bias; + + // Update nav state and covariances with latest imu measurements + lc::CombinedNavStateToLocMsg(latest_extrapolated_state_and_covariances->first, latest_extrapolated_loc_msg); + lc::CombinedNavStateCovariancesToLocMsg(latest_extrapolated_state_and_covariances->second, + latest_extrapolated_loc_msg); + + // Add latest bias corrected acceleration and angular velocity to loc msg + const auto latest_imu_measurement = imu_integrator_->Latest(); + if (!latest_imu_measurement) { + LogError("LatestExtrapolatedLocalizationMsg: Failed to get latest measurement."); + return boost::none; + } + const auto& latest_bias = latest_extrapolated_state_and_covariances->first.bias(); + auto latest_bias_corrected_acceleration = + latest_bias.correctAccelerometer(latest_imu_measurement->value.acceleration); + const auto latest_bias_corrected_angular_velocity = + latest_bias.correctGyroscope(latest_imu_measurement->value.angular_velocity); + // Correct for gravity if needed + if (!params_.imu_integrator.gravity.isZero()) { + const gtsam::Pose3& global_T_body_latest = latest_extrapolated_state_and_covariances->first.pose(); + latest_bias_corrected_acceleration = + lc::RemoveGravityFromAccelerometerMeasurement(params_.imu_integrator.gravity, params_.imu_integrator.body_T_imu, + global_T_body_latest, latest_bias_corrected_acceleration); + } + // Frame change measurements to body frame, correct for centripetal accel + const auto corrected_measurements = preintegration_helper_->correctMeasurementsBySensorPose( + latest_bias_corrected_acceleration, latest_bias_corrected_angular_velocity); + + mc::VectorToMsg(corrected_measurements.first, latest_extrapolated_loc_msg.accel); + mc::VectorToMsg(corrected_measurements.second, latest_extrapolated_loc_msg.omega); + return latest_extrapolated_loc_msg; +} +} // namespace ros_pose_extrapolator diff --git a/localization/imu_augmentor/test/test_imu_augmentor_wrapper.cc b/localization/ros_pose_extrapolator/test/test_imu_augmentor_wrapper.cc similarity index 100% rename from localization/imu_augmentor/test/test_imu_augmentor_wrapper.cc rename to localization/ros_pose_extrapolator/test/test_imu_augmentor_wrapper.cc diff --git a/localization/imu_augmentor/test/test_imu_augmentor_wrapper.test b/localization/ros_pose_extrapolator/test/test_imu_augmentor_wrapper.test similarity index 100% rename from localization/imu_augmentor/test/test_imu_augmentor_wrapper.test rename to localization/ros_pose_extrapolator/test/test_imu_augmentor_wrapper.test diff --git a/localization/ros_pose_extrapolator/test/test_ros_pose_extrapolator_parameter_reader.cc b/localization/ros_pose_extrapolator/test/test_ros_pose_extrapolator_parameter_reader.cc new file mode 100644 index 0000000000..9b42baa943 --- /dev/null +++ b/localization/ros_pose_extrapolator/test/test_ros_pose_extrapolator_parameter_reader.cc @@ -0,0 +1,77 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <node_adders/utilities.h> +#include <ros_pose_extrapolator/parameter_reader.h> + +#include <gtest/gtest.h> + +namespace lc = localization_common; +namespace rp = ros_pose_extrapolator; + +class RosPoseExtrapolatorParameterReaderTest : public ::testing::Test { + public: + RosPoseExtrapolatorParameterReaderTest() {} + + void SetUp() final { + lc::SetEnvironmentConfigs(); + config_reader::ConfigReader config; + config.AddFile("localization/imu_filter.config"); + config.AddFile("localization/imu_integrator.config"); + config.AddFile("localization/ros_pose_extrapolator.config"); + config.AddFile("transforms.config"); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + rp::LoadRosPoseExtrapolatorParams(config, params_); + } + + rp::RosPoseExtrapolatorParams params_; +}; + +TEST_F(RosPoseExtrapolatorParameterReaderTest, RosPoseExtrapolatorParams) { + EXPECT_EQ(params_.standstill_enabled, true); + // IMU Integrator + EXPECT_MATRIX_NEAR(params_.imu_integrator.gravity, Eigen::Vector3d::Zero(), 1e-6); + // Taken using current nav cam extrinsics + const gtsam::Pose3 expected_body_T_imu(gtsam::Rot3(0.70710678118, 0, 0, 0.70710678118), + gtsam::Point3(0.0386, 0.0247, -0.01016)); + EXPECT_MATRIX_NEAR(params_.imu_integrator.body_T_imu, expected_body_T_imu, 1e-6); + EXPECT_NEAR(params_.imu_integrator.gyro_sigma, 0.00001, 1e-6); + EXPECT_NEAR(params_.imu_integrator.accel_sigma, 0.0005, 1e-6); + EXPECT_NEAR(params_.imu_integrator.accel_bias_sigma, 0.0005, 1e-6); + EXPECT_NEAR(params_.imu_integrator.gyro_bias_sigma, 0.0000035, 1e-6); + EXPECT_NEAR(params_.imu_integrator.integration_variance, 0.0001, 1e-6); + EXPECT_NEAR(params_.imu_integrator.bias_acc_omega_int, 0.00015, 1e-6); + // IMU filter + EXPECT_EQ(params_.imu_integrator.filter.quiet_accel, "ButterO3S125Lp3N33_33"); + EXPECT_EQ(params_.imu_integrator.filter.quiet_ang_vel, "ButterO1S125Lp3N33_33"); + EXPECT_EQ(params_.imu_integrator.filter.nominal_accel, "ButterO3S125Lp3N41_66"); + EXPECT_EQ(params_.imu_integrator.filter.nominal_ang_vel, "ButterO1S125Lp3N41_66"); + EXPECT_EQ(params_.imu_integrator.filter.aggressive_accel, "ButterO3S125Lp3N46_66"); + EXPECT_EQ(params_.imu_integrator.filter.aggressive_ang_vel, "ButterO1S125Lp3N46_66"); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/ros_pose_extrapolator/test/test_ros_pose_extrapolator_parameter_reader.test b/localization/ros_pose_extrapolator/test/test_ros_pose_extrapolator_parameter_reader.test new file mode 100644 index 0000000000..6f47398a64 --- /dev/null +++ b/localization/ros_pose_extrapolator/test/test_ros_pose_extrapolator_parameter_reader.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="ros_pose_extrapolator" type="test_ros_pose_extrapolator_parameter_reader" test-name="test_ros_pose_extrapolator_parameter_reader" /> +</launch> diff --git a/localization/imu_augmentor/test/test_utilities.cc b/localization/ros_pose_extrapolator/test/test_utilities.cc similarity index 100% rename from localization/imu_augmentor/test/test_utilities.cc rename to localization/ros_pose_extrapolator/test/test_utilities.cc diff --git a/localization/imu_augmentor/test/test_utilities.h b/localization/ros_pose_extrapolator/test/test_utilities.h similarity index 100% rename from localization/imu_augmentor/test/test_utilities.h rename to localization/ros_pose_extrapolator/test/test_utilities.h diff --git a/localization/sliding_window_graph_optimizer/CMakeLists.txt b/localization/sliding_window_graph_optimizer/CMakeLists.txt new file mode 100644 index 0000000000..412214bbec --- /dev/null +++ b/localization/sliding_window_graph_optimizer/CMakeLists.txt @@ -0,0 +1,91 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(sliding_window_graph_optimizer) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + graph_optimizer + factor_adders + localization_common + node_adders +) + +# Find GTSAM +find_package(GTSAM REQUIRED) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +# include ff_nodelet to get ff_util header files since these aren't exposed elsewhere +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS factor_adders graph_optimizer localization_common node_adders +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${GLOG_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + src/sliding_window_graph_optimizer.cc +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_sliding_window_graph_optimizer + test/test_sliding_window_graph_optimizer.test + test/test_sliding_window_graph_optimizer.cc + ) + target_link_libraries(test_sliding_window_graph_optimizer + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/sliding_window_graph_optimizer/include/sliding_window_graph_optimizer/sliding_window_graph_optimizer.h b/localization/sliding_window_graph_optimizer/include/sliding_window_graph_optimizer/sliding_window_graph_optimizer.h new file mode 100644 index 0000000000..e03484e641 --- /dev/null +++ b/localization/sliding_window_graph_optimizer/include/sliding_window_graph_optimizer/sliding_window_graph_optimizer.h @@ -0,0 +1,130 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef SLIDING_WINDOW_GRAPH_OPTIMIZER_SLIDING_WINDOW_GRAPH_OPTIMIZER_H_ +#define SLIDING_WINDOW_GRAPH_OPTIMIZER_SLIDING_WINDOW_GRAPH_OPTIMIZER_H_ + +#include <graph_optimizer/graph_optimizer.h> +#include <localization_common/time.h> +#include <node_adders/sliding_window_node_adder.h> +#include <sliding_window_graph_optimizer/sliding_window_graph_optimizer_params.h> + +#include <boost/serialization/serialization.hpp> + +#include <utility> +#include <vector> + +namespace sliding_window_graph_optimizer { +// GraphOptimizer that performs sliding window optimization. +// Calculates the desired window size in duration and number of nodes for each sliding window node adder +// using the SlidingWindowNodeAdders in the graph. +// Removes old nodes outside of window size along with the factors that depend on them after optimization. +// Adds either prior factors using marginalized covariances for new start nodes or adds +// marginal factors consisting of linearized errors for each factor removed (set desired behavior using params). +// Add sliding window node adders using the AddSlidingWindowNodeAdder() function (instead of AddNodeAdder() function) to +// ensure SlideWindow() is called for the sliding window node adder. +class SlidingWindowGraphOptimizer : public graph_optimizer::GraphOptimizer { + public: + SlidingWindowGraphOptimizer(const SlidingWindowGraphOptimizerParams& params, + std::unique_ptr<optimizers::Optimizer> optimizer); + + virtual ~SlidingWindowGraphOptimizer() = default; + + // Default constructor for serialization only + SlidingWindowGraphOptimizer() {} + + // Add sliding window node adders with this to ensure SlideWindow() is called + // for the node adder when sliding the window for the graph. + // If this function is used, the node adder does not need to be added with + // the GraphOptimizer::AddNodeAdder() function. + void AddSlidingWindowNodeAdder(std::shared_ptr<node_adders::SlidingWindowNodeAdder> sliding_window_node_adder); + + // Adds factors up to the latest measurements, optimizes the graph, and slides the window. + // Slides the window before or after optimization depending on params. + // See SlideWindow() function comments for more details on sliding the window. + bool Update(); + + // Returns const reference to update timer + const localization_common::Timer& update_timer() const; + + // Duration of the graph, calculated using the earliest start time + // and latest end time using all node adders in the graph. + double Duration() const; + + private: + // Removes nodes older than calculated new start time (see NewStartTime() for how this is calculated). + // Removes any factors depending on a removed node and optionally adds marginalized + // factors containing linearized errors for each removed factors. + // Optionally adds prior factors using marginalized covariances to new start nodes. + bool SlideWindow(); + + // Creates marginal factors using linearized errors of removed factors. + gtsam::NonlinearFactorGraph MarginalFactors(const gtsam::NonlinearFactorGraph& old_factors, + const gtsam::KeyVector& old_keys, + const gtsam::GaussianFactorGraph::Eliminate& eliminate_function) const; + + // Old keys for nodes. Calculated and accumulated for each sliding window node adder. + gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time) const; + + // Calculates start and end time limits for new factors to add. + // Uses max end time to ensure all possible new factors are added, and uses oldest node adder + // start time to ensure no factors older than oldest graph node are added. + std::pair<localization_common::Time, localization_common::Time> WindowStartAndEndTimes() const; + + // Calculates new start time for the graph using the earliest of the sliding window node adders + // desired new start times. Ensures this doesn't occur after the current end time. + boost::optional<localization_common::Time> NewStartTime() const; + + // Desired new start time that will constrain the graph to a certain window size. + // Calculated using the latest of the desired start times for each sliding window node adder + // so that all node adder window sizes are <= their desired size. + boost::optional<localization_common::Time> IdealNodeAddersNewStartTime() const; + + // Earliest start time of the sliding window node adders. + boost::optional<localization_common::Time> EarliestNodeAdderStartTime() const; + + // Latest end time of the sliding window node adders. + boost::optional<localization_common::Time> LatestNodeAdderEndTime() const; + + // Removes any factors containg a key in the provided keys. + // Prunes measurements and keys from cumulative factors instead of removing them. + // Returns removed factors. + gtsam::NonlinearFactorGraph RemoveFactors(const gtsam::KeyVector& keys_to_remove); + + // Add averagers and timers for logging + void AddAveragersAndTimers(); + + // Serialization function + friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(GraphOptimizer); + ar& BOOST_SERIALIZATION_NVP(params_); + ar& BOOST_SERIALIZATION_NVP(sliding_window_node_adders_); + ar& BOOST_SERIALIZATION_NVP(update_timer_); + } + + SlidingWindowGraphOptimizerParams params_; + std::vector<std::shared_ptr<node_adders::SlidingWindowNodeAdder>> sliding_window_node_adders_; + + // Logging + localization_common::Timer update_timer_ = localization_common::Timer("Update"); +}; +} // namespace sliding_window_graph_optimizer + +#endif // SLIDING_WINDOW_GRAPH_OPTIMIZER_SLIDING_WINDOW_GRAPH_OPTIMIZER_H_ diff --git a/localization/sliding_window_graph_optimizer/include/sliding_window_graph_optimizer/sliding_window_graph_optimizer_params.h b/localization/sliding_window_graph_optimizer/include/sliding_window_graph_optimizer/sliding_window_graph_optimizer_params.h new file mode 100644 index 0000000000..4171727a86 --- /dev/null +++ b/localization/sliding_window_graph_optimizer/include/sliding_window_graph_optimizer/sliding_window_graph_optimizer_params.h @@ -0,0 +1,35 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef SLIDING_WINDOW_GRAPH_OPTIMIZER_SLIDING_WINDOW_GRAPH_OPTIMIZER_PARAMS_H_ +#define SLIDING_WINDOW_GRAPH_OPTIMIZER_SLIDING_WINDOW_GRAPH_OPTIMIZER_PARAMS_H_ + +#include <graph_optimizer/graph_optimizer_params.h> + +namespace sliding_window_graph_optimizer { +struct SlidingWindowGraphOptimizerParams : public graph_optimizer::GraphOptimizerParams { + // Add marginal factors to the graph after sliding the window. + // Marginal factors consist of linearized errors for factors removed after sliding the window. + bool add_marginal_factors; + // Slide window before or after optimization. Sliding before optimization + // ensures a set window size is used during optimization while sliding after + // allows more nodes and factors to be included. + bool slide_window_before_optimization; +}; +} // namespace sliding_window_graph_optimizer + +#endif // SLIDING_WINDOW_GRAPH_OPTIMIZER_SLIDING_WINDOW_GRAPH_OPTIMIZER_PARAMS_H_ diff --git a/localization/sliding_window_graph_optimizer/package.xml b/localization/sliding_window_graph_optimizer/package.xml new file mode 100644 index 0000000000..34cb47bbc9 --- /dev/null +++ b/localization/sliding_window_graph_optimizer/package.xml @@ -0,0 +1,25 @@ +<package> + <name>sliding_window_graph_optimizer</name> + <version>1.0.0</version> + <description> + The sliding window graph optimizer package + </description> + <license> + Apache License, Version 2.0 + </license> + <author email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </author> + <maintainer email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </maintainer> + <buildtool_depend>catkin</buildtool_depend> + <build_depend>factor_adders</build_depend> + <build_depend>graph_optimizer</build_depend> + <build_depend>localization_common</build_depend> + <build_depend>node_adders</build_depend> + <run_depend>factor_adders</run_depend> + <run_depend>graph_optimizer</run_depend> + <run_depend>localization_common</run_depend> + <run_depend>node_adders</run_depend> +</package> diff --git a/localization/sliding_window_graph_optimizer/readme.md b/localization/sliding_window_graph_optimizer/readme.md new file mode 100644 index 0000000000..8f66eb12f9 --- /dev/null +++ b/localization/sliding_window_graph_optimizer/readme.md @@ -0,0 +1,9 @@ +\page slidingwindowgraphoptimizer Sliding Window Graph Optimizer + +# Package Overview +The SlidingWindowGraphOptimizer class extends the graph optimizer and uses sliding window node adders to perform graph-based optimization. In each Update() call, it adds new factors and nodes from respective factor and node adders and slides the window before or after optimization as desired. Sliding the window consists of removing old nodes and their dependent factors as determined by the set max desired window duration and number of states in the provided params. See the node_adder package for more information on the SlidingWindowNodeAdder class. + +<p align="center"> +<img src="../doc/images/sliding_window_graph_optimizer.png" width="550"> +</p> + diff --git a/localization/sliding_window_graph_optimizer/src/sliding_window_graph_optimizer.cc b/localization/sliding_window_graph_optimizer/src/sliding_window_graph_optimizer.cc new file mode 100644 index 0000000000..9ee3f9221c --- /dev/null +++ b/localization/sliding_window_graph_optimizer/src/sliding_window_graph_optimizer.cc @@ -0,0 +1,224 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <graph_factors/cumulative_factor.h> +#include <localization_common/logger.h> +#include <localization_common/utilities.h> +#include <sliding_window_graph_optimizer/sliding_window_graph_optimizer.h> + +#include <gtsam/nonlinear/LinearContainerFactor.h> + +namespace sliding_window_graph_optimizer { +namespace lc = localization_common; +namespace na = node_adders; + +SlidingWindowGraphOptimizer::SlidingWindowGraphOptimizer(const SlidingWindowGraphOptimizerParams& params, + std::unique_ptr<optimizers::Optimizer> optimizer) + : params_(params), GraphOptimizer(params, std::move(optimizer)) { + AddAveragersAndTimers(); +} + +void SlidingWindowGraphOptimizer::AddSlidingWindowNodeAdder( + std::shared_ptr<na::SlidingWindowNodeAdder> sliding_window_node_adder) { + sliding_window_node_adders_.emplace_back(sliding_window_node_adder); + AddNodeAdder(sliding_window_node_adder); +} + +bool SlidingWindowGraphOptimizer::Update() { + LogDebug("Update: Updating."); + update_timer_.Start(); + AddFactors(WindowStartAndEndTimes()); + if (params_.slide_window_before_optimization) SlideWindow(); + GraphOptimizer::Optimize(); + if (!params_.slide_window_before_optimization) SlideWindow(); + update_timer_.Stop(); + return true; +} + +bool SlidingWindowGraphOptimizer::SlideWindow() { + const auto new_start_time = NewStartTime(); + if (!new_start_time) { + LogDebug( + "SlideWindow: Failed to get new start time, node adders may not have enough nodes to slide window. Not sliding " + "window."); + return false; + } + const auto old_keys = OldKeys(*new_start_time); + const auto old_factors = RemoveFactors(old_keys); + if (params_.add_marginal_factors) { + const auto marginal_factors = MarginalFactors(old_factors, old_keys, gtsam::EliminateQR); + for (const auto& marginal_factor : marginal_factors) { + factors().push_back(marginal_factor); + } + } + + for (auto& sliding_window_node_adder : sliding_window_node_adders_) + sliding_window_node_adder->SlideWindow(*new_start_time, marginals(), old_keys, params_.huber_k, factors()); + return true; +} + +// Adapted from gtsam::BatchFixedLagSmoother +gtsam::NonlinearFactorGraph SlidingWindowGraphOptimizer::MarginalFactors( + const gtsam::NonlinearFactorGraph& old_factors, const gtsam::KeyVector& old_keys, + const gtsam::GaussianFactorGraph::Eliminate& eliminate_function) const { + // Old keys not present in old factors. This shouldn't occur. + if (old_keys.size() == 0) { + LogDebug("MarginalFactors: No old keys provided."); + return old_factors; + } + + // Linearize Graph + const auto linearized_graph = old_factors.linearize(gtsam_values()); + const auto linear_marginal_factors = + *(linearized_graph->eliminatePartialMultifrontal(old_keys, eliminate_function).second); + return gtsam::LinearContainerFactor::ConvertLinearGraph(linear_marginal_factors, gtsam_values()); +} + +gtsam::KeyVector SlidingWindowGraphOptimizer::OldKeys(const localization_common::Time oldest_allowed_time) const { + gtsam::KeyVector all_old_keys; + for (const auto& sliding_window_node_adder : sliding_window_node_adders_) { + const auto old_keys = sliding_window_node_adder->OldKeys(oldest_allowed_time, factors()); + all_old_keys.insert(all_old_keys.end(), old_keys.begin(), old_keys.end()); + } + return all_old_keys; +} + +std::pair<lc::Time, lc::Time> SlidingWindowGraphOptimizer::WindowStartAndEndTimes() const { + auto start_time = EarliestNodeAdderStartTime(); + // Shouldn't occur since node adders initialize their first nodes when added to the graph. + if (!start_time) start_time = 0; + // Add all new factors possible. + // When sliding window before optimization, this will keep the latest factors and + // compute the relative start time wrt them. + // When sliding window after optimization, this may increase the window size too much. + // TODO(rsoussan): Add param to set window size (start time) if sliding window after optimization? + const lc::Time end_time = std::numeric_limits<double>::max(); + return {*start_time, end_time}; +} + +boost::optional<lc::Time> SlidingWindowGraphOptimizer::NewStartTime() const { + const auto ideal_new_start_time = IdealNodeAddersNewStartTime(); + if (!ideal_new_start_time) { + LogDebug("NewStartTime: No start times available."); + return boost::none; + } + // Ensure that new start time isn't more recent than current end time + // since then priors couldn't be added for the new start nodes. + const auto end_time = LatestNodeAdderEndTime(); + if (!end_time) { + LogDebug("NewStartTime: No end time available."); + return boost::none; + } + if (*end_time < *ideal_new_start_time) + LogError("NewStartTime: Ideal start time is more recent than current end time."); + return std::min(*end_time, *ideal_new_start_time); +} + +boost::optional<lc::Time> SlidingWindowGraphOptimizer::IdealNodeAddersNewStartTime() const { + boost::optional<lc::Time> new_start_time; + for (const auto& sliding_window_node_adder : sliding_window_node_adders_) { + const auto node_adder_new_start_time = sliding_window_node_adder->SlideWindowNewStartTime(); + if (node_adder_new_start_time) { + // Use max here so the latest start time is chosen and window sizes for all node adders + // are <= desired sizes. + new_start_time = + new_start_time ? std::max(*node_adder_new_start_time, *new_start_time) : *node_adder_new_start_time; + } + } + + return new_start_time; +} + +boost::optional<lc::Time> SlidingWindowGraphOptimizer::EarliestNodeAdderStartTime() const { + boost::optional<lc::Time> earliest_start_time; + for (const auto& sliding_window_node_adder : sliding_window_node_adders_) { + const auto node_adder_start_time = sliding_window_node_adder->StartTime(); + if (node_adder_start_time) { + earliest_start_time = + earliest_start_time ? std::min(*earliest_start_time, *node_adder_start_time) : *node_adder_start_time; + } + } + return earliest_start_time; +} + +boost::optional<lc::Time> SlidingWindowGraphOptimizer::LatestNodeAdderEndTime() const { + boost::optional<lc::Time> latest_end_time; + for (const auto& sliding_window_node_adder : sliding_window_node_adders_) { + const auto node_adder_end_time = sliding_window_node_adder->EndTime(); + if (node_adder_end_time) { + latest_end_time = latest_end_time ? std::max(*latest_end_time, *node_adder_end_time) : *node_adder_end_time; + } + } + return latest_end_time; +} + +double SlidingWindowGraphOptimizer::Duration() const { + double duration; + const auto latest_end_time = LatestNodeAdderEndTime(); + const auto earliest_start_time = EarliestNodeAdderStartTime(); + if (!latest_end_time || !earliest_start_time) return 0; + return *latest_end_time - *earliest_start_time; +} + +gtsam::NonlinearFactorGraph SlidingWindowGraphOptimizer::RemoveFactors(const gtsam::KeyVector& keys_to_remove) { + gtsam::NonlinearFactorGraph removed_factors; + if (keys_to_remove.empty()) return removed_factors; + // Create set for quick lookup + std::unordered_set<gtsam::Key> keys_to_remove_set; + for (const auto& key : keys_to_remove) { + keys_to_remove_set.emplace(key); + } + + for (auto factor_it = factors().begin(); factor_it != factors().end();) { + const auto cumulative_factor = dynamic_cast<const gtsam::CumulativeFactor*>(factor_it->get()); + // TODO(rsoussan): Add function to create factor from keys and measurements that are removed! + // Return this with removed factors. + // Try to remove old keys from cumulative factors + if (cumulative_factor) { + *factor_it = cumulative_factor->PrunedCopy(keys_to_remove_set); + // Remove pruned copy if it is invalid + if (!(*factor_it)) { + factor_it = factors().erase(factor_it); + continue; + } else { + ++factor_it; + continue; + } + } + bool remove_factor = false; + const auto& factor_keys = (*factor_it)->keys(); + for (const auto& factor_key : factor_keys) { + if (keys_to_remove_set.count(factor_key) > 0) { + remove_factor = true; + } + } + if (!remove_factor) { + ++factor_it; + } else { + removed_factors.push_back(*factor_it); + factor_it = factors().erase(factor_it); + } + } + return removed_factors; +} + +void SlidingWindowGraphOptimizer::AddAveragersAndTimers() { stats_logger().AddTimer(update_timer_); } + +const localization_common::Timer& SlidingWindowGraphOptimizer::update_timer() const { return update_timer_; } + +} // namespace sliding_window_graph_optimizer diff --git a/localization/sliding_window_graph_optimizer/test/test_sliding_window_graph_optimizer.cc b/localization/sliding_window_graph_optimizer/test/test_sliding_window_graph_optimizer.cc new file mode 100644 index 0000000000..d875227ee0 --- /dev/null +++ b/localization/sliding_window_graph_optimizer/test/test_sliding_window_graph_optimizer.cc @@ -0,0 +1,380 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <factor_adders/loc_factor_adder.h> +#include <graph_factors/robust_smart_projection_pose_factor.h> +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <localization_measurements/matched_projections_measurement.h> +#include <localization_measurements/pose_measurement.h> +#include <node_adders/pose_node_adder.h> +#include <node_adders/pose_node_adder_model.h> +#include <node_adders/pose_node_adder_params.h> +#include <nodes/values.h> +#include <optimizers/optimizer.h> +#include <sliding_window_graph_optimizer/sliding_window_graph_optimizer.h> + +#include <gtest/gtest.h> + +namespace fa = factor_adders; +namespace go = graph_optimizer; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace na = node_adders; +namespace no = nodes; +namespace op = optimizers; +namespace sw = sliding_window_graph_optimizer; + +class SimpleOptimizer : public op::Optimizer { + public: + explicit SimpleOptimizer(const op::OptimizerParams& params) : op::Optimizer(params) {} + + bool Optimize(const gtsam::NonlinearFactorGraph& factors, gtsam::Values& values) final { return true; } + + boost::optional<gtsam::Matrix> Covariance(const gtsam::Key& key) const final { + return gtsam::Matrix(gtsam::Matrix6::Identity()); + } + + int iterations() const final { return 0; } +}; + +// For testing new window start time and end time +class DummySlidingWindowNodeAdder : public na::SlidingWindowNodeAdder { + public: + bool SlideWindow(const localization_common::Time oldest_allowed_timestamp, + const boost::optional<const gtsam::Marginals&>& marginals, const gtsam::KeyVector& old_keys, + const double huber_k, gtsam::NonlinearFactorGraph& factors) final { + return true; + } + + boost::optional<localization_common::Time> SlideWindowNewStartTime() const final { return new_start_time_; } + + gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time, + const gtsam::NonlinearFactorGraph& graph) const final { + return gtsam::KeyVector(); + } + + boost::optional<localization_common::Time> StartTime() const final { return start_time_; } + + boost::optional<localization_common::Time> EndTime() const final { return end_time_; } + + void AddInitialNodesAndPriors(gtsam::NonlinearFactorGraph& graph) final { return; } + + bool AddNode(const localization_common::Time timestamp, gtsam::NonlinearFactorGraph& factors) final { return true; } + + bool CanAddNode(const localization_common::Time timestamp) const final { return true; } + + gtsam::KeyVector Keys(const localization_common::Time timestamp) const final { return gtsam::KeyVector(); } + + lc::Time new_start_time_; + lc::Time start_time_; + lc::Time end_time_; +}; + +class SlidingWindowGraphOptimizerTest : public ::testing::Test { + public: + SlidingWindowGraphOptimizerTest() {} + + void SetUp() final { Initialize(); } + + void Initialize() { + std::unique_ptr<SimpleOptimizer> optimizer(new SimpleOptimizer(DefaultOptimizerParams())); + sliding_window_graph_optimizer_.reset( + new sw::SlidingWindowGraphOptimizer(DefaultSlidingWindowGraphOptimizerParams(), std::move(optimizer))); + std::shared_ptr<no::TimestampedNodes<gtsam::Pose3>> timestamped_pose_nodes( + new no::TimestampedNodes<gtsam::Pose3>(sliding_window_graph_optimizer_->values())); + pose_node_adder_.reset( + new na::PoseNodeAdder(DefaultPoseNodeAdderParams(), DefaultPoseNodeAdderModelParams(), timestamped_pose_nodes)); + // Add initial measurement to pose node adder + const lm::PoseWithCovarianceMeasurement pose_measurement(gtsam::Pose3::identity(), + Eigen::Matrix<double, 6, 6>::Identity(), 0); + pose_node_adder_->AddMeasurement(pose_measurement); + + dummy_node_adder_.reset(new DummySlidingWindowNodeAdder()); + loc_factor_adder_params_ = DefaultLocFactorAdderParams(); + loc_factor_adder_.reset(new fa::LocFactorAdder<na::PoseNodeAdder>(loc_factor_adder_params_, pose_node_adder_)); + sliding_window_graph_optimizer_->AddSlidingWindowNodeAdder(pose_node_adder_); + sliding_window_graph_optimizer_->AddFactorAdder(loc_factor_adder_); + } + + void AddLocMeasurement(const double time) { + lm::MatchedProjectionsMeasurement loc_measurement; + // Need at least one matched projection for measurement to be valid. + lm::MatchedProjection matched_projection(gtsam::Point2(), gtsam::Point3(), time); + loc_measurement.matched_projections.emplace_back(matched_projection); + loc_measurement.global_T_cam = lc::RandomPose(); + loc_measurement.timestamp = time; + loc_factor_adder_->AddMeasurement(loc_measurement); + loc_measurements_.emplace_back(loc_measurement); + } + + void AddPoseMeasurement(const double time) { + const lm::PoseWithCovarianceMeasurement pose_measurement(lc::RandomPoseWithCovariance(), time); + pose_node_adder_->AddMeasurement(pose_measurement); + } + + sw::SlidingWindowGraphOptimizerParams DefaultSlidingWindowGraphOptimizerParams() { + sw::SlidingWindowGraphOptimizerParams params; + params.add_marginal_factors = false; + params.slide_window_before_optimization = true; + params.huber_k = 1.345; + params.log_stats_on_destruction = false; + params.print_after_optimization = false; + return params; + } + + na::TimestampedNodeAdderModelParams DefaultPoseNodeAdderModelParams() { + na::TimestampedNodeAdderModelParams params; + params.huber_k = 1.345; + return params; + } + + // TODO(rsoussan): Get this from node adders package + na::PoseNodeAdderParams DefaultPoseNodeAdderParams() { + na::PoseNodeAdderParams params; + params.starting_prior_translation_stddev = 0.1; + params.starting_prior_quaternion_stddev = 0.2; + params.start_node = gtsam::Pose3::identity(); + params.huber_k = 1.345; + params.add_priors = true; + params.starting_time = 0; + // Only kept if there are at least min_num_states and not more than max_num_states + params.ideal_duration = 1; + params.min_num_states = 1; + params.max_num_states = 4; + params.SetStartNoiseModels(); + return params; + } + + fa::LocFactorAdderParams DefaultLocFactorAdderParams() { + fa::LocFactorAdderParams params; + params.add_pose_priors = true; + params.add_projection_factors = false; + params.prior_translation_stddev = 0.1; + params.prior_quaternion_stddev = 0.2; + params.scale_pose_noise_with_num_landmarks = false; + params.pose_noise_scale = 1; + params.min_num_matches_per_measurement = 0; + params.body_T_cam = gtsam::Pose3::identity(); + params.enabled = true; + params.huber_k = 1.345; + return params; + } + + op::OptimizerParams DefaultOptimizerParams() { + op::OptimizerParams params; + params.marginals_factorization = "qr"; + return params; + } + + // TODO(rsoussan): Merge this with loc factor adder test + void EXPECT_SAME_LOC_POSE_FACTOR(const int factor_index, const int measurement_index) { + const auto pose_factor = + dynamic_cast<gtsam::LocPoseFactor*>(sliding_window_graph_optimizer_->factors()[factor_index].get()); + ASSERT_TRUE(pose_factor); + const gtsam::Pose3 factor_pose = pose_factor->prior(); + const gtsam::Pose3 measurement_pose = + loc_measurements_[measurement_index].global_T_cam * loc_factor_adder_params_.body_T_cam.inverse(); + EXPECT_MATRIX_NEAR(factor_pose, measurement_pose, 1e-6); + } + + std::unique_ptr<sw::SlidingWindowGraphOptimizer> sliding_window_graph_optimizer_; + std::shared_ptr<fa::LocFactorAdder<na::PoseNodeAdder>> loc_factor_adder_; + fa::LocFactorAdderParams loc_factor_adder_params_; + std::shared_ptr<na::PoseNodeAdder> pose_node_adder_; + std::shared_ptr<DummySlidingWindowNodeAdder> dummy_node_adder_; + std::vector<lm::MatchedProjectionsMeasurement> loc_measurements_; +}; + +TEST_F(SlidingWindowGraphOptimizerTest, SlideWindowDurationViolation) { + // Initial node and prior should be added for pose node adder + EXPECT_EQ(sliding_window_graph_optimizer_->num_factors(), 1); + EXPECT_EQ(sliding_window_graph_optimizer_->num_values(), 1); + // Add first measurements + // Add loc measurement at pose node initial time so no new pose nodes are added. + AddLocMeasurement(0); + // Update graph + EXPECT_TRUE(sliding_window_graph_optimizer_->Update()); + // Pose node times: + // 0 + // Pose node num nodes: 1 + // Pose node duration: 0 + // Pose node limits: duration = 1, min_nodes = 1, max_nodes = 4 + // No violations, nothing should be removed. + EXPECT_EQ(sliding_window_graph_optimizer_->num_values(), 1); + // Expect 2 factors (prior and measurement on first node) + EXPECT_EQ(sliding_window_graph_optimizer_->num_factors(), 2); + EXPECT_SAME_LOC_POSE_FACTOR(1, 0); + // Add second measurements + AddLocMeasurement(1); + AddPoseMeasurement(1); + // Update graph + EXPECT_TRUE(sliding_window_graph_optimizer_->Update()); + // Pose node times: + // 0 1 + // Pose node num nodes: 2 + // Pose node duration: 1 + // Pose node limits: duration = 1, min_nodes = 1, max_nodes = 4 + // No violations, nothing should be removed. + EXPECT_EQ(sliding_window_graph_optimizer_->num_values(), 2); + // Expect 4 factors (prior and measurement on first node, between factor, measurement on second node) + EXPECT_EQ(sliding_window_graph_optimizer_->num_factors(), 4); + EXPECT_SAME_LOC_POSE_FACTOR(1, 0); + EXPECT_SAME_LOC_POSE_FACTOR(3, 1); + // Add third measurements + AddLocMeasurement(2); + AddPoseMeasurement(2); + // Update graph + EXPECT_TRUE(sliding_window_graph_optimizer_->Update()); + // Pose node times: + // 0 1 2 + // Pose node num nodes: 3 + // Pose node duration: 2 + // Pose node limits: duration = 1, min_nodes = 1, max_nodes = 4 + // Duration too large, oldest node should be removed. + // Pose slide window pose node times: + // 1 2 + // Pose node num nodes: 2 + // Pose node duration: 1 + EXPECT_EQ(sliding_window_graph_optimizer_->num_values(), 2); + // Expect 4 factors (prior and measurement on first node, between factor, measurement on second node) + EXPECT_EQ(sliding_window_graph_optimizer_->num_factors(), 4); + // Pose prior removed then added last + EXPECT_SAME_LOC_POSE_FACTOR(0, 1); + EXPECT_SAME_LOC_POSE_FACTOR(2, 2); +} + +TEST_F(SlidingWindowGraphOptimizerTest, SlideWindowNumNodesViolation) { + // Initial node and prior should be added for pose node adder + EXPECT_EQ(sliding_window_graph_optimizer_->num_factors(), 1); + EXPECT_EQ(sliding_window_graph_optimizer_->num_values(), 1); + // Add 3 nodes to populate graph with 4 nodes (max) + const double time_increment = 0.1; + for (int i = 1; i <= 3; ++i) { + AddLocMeasurement(time_increment * i); + AddPoseMeasurement(time_increment * i); + } + EXPECT_TRUE(sliding_window_graph_optimizer_->Update()); + // Pose node times: + // 0, 0.1, 0.2, 0.3 + // Pose node num nodes: 4 + // Pose node duration: 0.3 + // Pose node limits: duration = 1, min_nodes = 1, max_nodes = 4 + // No violations, nothing should be removed. + EXPECT_EQ(sliding_window_graph_optimizer_->num_values(), 4); + // Expect 7 factors (prior, three between factors, 3 measurements) + EXPECT_EQ(sliding_window_graph_optimizer_->num_factors(), 7); + // Violate num nodes limit + // Add second measurements + AddLocMeasurement(0.4); + AddPoseMeasurement(0.4); + EXPECT_TRUE(sliding_window_graph_optimizer_->Update()); + // Pose node times: + // 0, 0.1, 0.2, 0.3, 0.4 + // Pose node num nodes: 5 + // Pose node duration: 0.4 + // Pose node limits: duration = 1, min_nodes = 1, max_nodes = 4 + // Too many nodes, first node should be removed. + // Pose slide window pose node times: + // 0.1, 0.2, 0.3, 0.4 + // Pose node num nodes: 4 + // Pose node duration: 0.3 + EXPECT_EQ(sliding_window_graph_optimizer_->num_values(), 4); + // Expect 8 factors (prior, three between factors, 4 measurements) + EXPECT_EQ(sliding_window_graph_optimizer_->num_factors(), 8); +} + +TEST_F(SlidingWindowGraphOptimizerTest, TwoAddersNewStartTime) { + sliding_window_graph_optimizer_->AddSlidingWindowNodeAdder(dummy_node_adder_); + // Initial node and prior should be added for pose node adder + EXPECT_EQ(sliding_window_graph_optimizer_->num_factors(), 1); + EXPECT_EQ(sliding_window_graph_optimizer_->num_values(), 1); + // Add 3 nodes to populate graph with 4 nodes (max) + const double time_increment = 0.1; + for (int i = 1; i <= 3; ++i) { + AddLocMeasurement(time_increment * i); + AddPoseMeasurement(time_increment * i); + } + // Set second node adder new start time to 0.2 so first two nodes should be removed + dummy_node_adder_->new_start_time_ = 0.2; + // Set other times to be negligable + dummy_node_adder_->start_time_ = 0; + dummy_node_adder_->end_time_ = 100; + EXPECT_TRUE(sliding_window_graph_optimizer_->Update()); + // Pose node times: + // 0.2, 0.3 + // Pose node num nodes: 2 + // Pose node duration: 0.1 + EXPECT_EQ(sliding_window_graph_optimizer_->num_values(), 2); + // Expect 3 factors (prior, one between factor, 2 measurements) + EXPECT_EQ(sliding_window_graph_optimizer_->num_factors(), 4); +} + +TEST_F(SlidingWindowGraphOptimizerTest, SlideWindowPruneCumulativeFactor) { + using SmartFactorCalibration = gtsam::Cal3_S2; + using RobustSmartFactor = gtsam::RobustSmartProjectionPoseFactor<SmartFactorCalibration>; + // Initial node and prior should be added for pose node adder + EXPECT_EQ(sliding_window_graph_optimizer_->num_factors(), 1); + EXPECT_EQ(sliding_window_graph_optimizer_->num_values(), 1); + // Add 4 nodes to populate graph with 5 nodes (1 over max) + const double time_increment = 0.1; + for (int i = 1; i <= 4; ++i) { + AddLocMeasurement(time_increment * i); + AddPoseMeasurement(time_increment * i); + } + // Hackily add a cumulative factor (smart factor) using all pose nodes. + { + auto smart_factor = boost::make_shared<RobustSmartFactor>(gtsam::noiseModel::Isotropic::Sigma(2, 0.1), + boost::make_shared<gtsam::Cal3_S2>(), gtsam::Pose3(), + gtsam::SmartProjectionParams(), true, true, 1.345); + for (int i = 1; i <= 5; ++i) { + smart_factor->add(gtsam::Point2(i + 1, i + 2), gtsam::Key(i)); + } + sliding_window_graph_optimizer_->factors().push_back(smart_factor); + } + EXPECT_TRUE(sliding_window_graph_optimizer_->Update()); + // Pose node times: + // 0, 0.1, 0.2, 0.3, 0.4 + // Pose node num nodes: 5 + // Pose node duration: 0.4 + // Pose node limits: duration = 1, min_nodes = 1, max_nodes = 4 + // 1st node should be removed. + EXPECT_EQ(sliding_window_graph_optimizer_->num_values(), 4); + // Expect 9 factors (prior, three between factors, 4 measurements, 1 smart factor) + EXPECT_EQ(sliding_window_graph_optimizer_->num_factors(), 9); + // Smart factor should be in first index since added before updating graph. + const auto smart_factor = dynamic_cast<const RobustSmartFactor*>(sliding_window_graph_optimizer_->factors()[0].get()); + ASSERT_TRUE(smart_factor); + EXPECT_EQ(smart_factor->keys().size(), 4); + for (int i = 0; i < 4; ++i) { + // Keys are offset by 1, and 1st key is removed + const int key = i + 2; + EXPECT_EQ(smart_factor->keys()[i], key); + // Point is (key+1, key+2) + EXPECT_MATRIX_NEAR(smart_factor->measured()[i], gtsam::Point2(key + 1, key + 2), 1e-6); + } +} + +// TODO(rsoussan): Test adding marginal factors + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/sliding_window_graph_optimizer/test/test_sliding_window_graph_optimizer.test b/localization/sliding_window_graph_optimizer/test/test_sliding_window_graph_optimizer.test new file mode 100644 index 0000000000..138464fd8a --- /dev/null +++ b/localization/sliding_window_graph_optimizer/test/test_sliding_window_graph_optimizer.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="sliding_window_graph_optimizer" type="test_sliding_window_graph_optimizer" test-name="test_sliding_window_graph_optimizer" /> +</launch> diff --git a/localization/sparse_mapping/tools/generate_hugin.py b/localization/sparse_mapping/tools/generate_hugin.py index b19895fdfc..6966b0d51b 100755 --- a/localization/sparse_mapping/tools/generate_hugin.py +++ b/localization/sparse_mapping/tools/generate_hugin.py @@ -77,23 +77,12 @@ def list_images_in_map(mapfile): def parse_args(): parser = argparse.ArgumentParser(description="Generates/updates hugin files.") + parser.add_argument("-map_name", type=str, required=True, help="Input surf map.") parser.add_argument( - "-map_name", - type=str, - required=True, - help="Input surf map.", + "-input_hugin", type=str, required=False, help="Input Hugin pto file." ) parser.add_argument( - "-input_hugin", - type=str, - required=False, - help="Input Hugin pto file.", - ) - parser.add_argument( - "-output_hugin", - type=str, - required=False, - help="Output Hugin pto file.", + "-output_hugin", type=str, required=False, help="Output Hugin pto file." ) parser.add_argument( "-work_dir", diff --git a/localization/sparse_mapping/tools/partition_image_sequences.cc b/localization/sparse_mapping/tools/partition_image_sequences.cc index d05c4d1e90..6a10500404 100644 --- a/localization/sparse_mapping/tools/partition_image_sequences.cc +++ b/localization/sparse_mapping/tools/partition_image_sequences.cc @@ -472,9 +472,7 @@ int main(int argc, char** argv) { "view-images,v", po::bool_switch(&view_images)->default_value(false), "View images with projected features and error ratios for each provided image in the image directory. " "Helpful for tuning the error ratio or visualizing rotation movement while the script is running.")( - "config-path,c", po::value<std::string>()->required(), - "Full path to astrobee/src/astrobee directory location, e.g. ~/astrobee/src/astrobee.")( - "robot-config-file,r", po::value<std::string>(&robot_config_file)->default_value("config/robots/bumble.config"), + "robot-config-file,r", po::value<std::string>(&robot_config_file)->default_value("bumble.config"), "robot config file"); po::positional_options_description p; p.add("image-directory", 1); @@ -493,13 +491,12 @@ int main(int argc, char** argv) { } const std::string image_directory = vm["image-directory"].as<std::string>(); - const std::string config_path = vm["config-path"].as<std::string>(); // Only pass program name to free flyer so that boost command line options // are ignored when parsing gflags. int ff_argc = 1; ff_common::InitFreeFlyerApplication(&ff_argc, &argv); - lc::SetEnvironmentConfigs(config_path, "iss", robot_config_file); + lc::SetEnvironmentConfigs(robot_config_file); config_reader::ConfigReader config; config.AddFile("cameras.config"); if (!config.ReadFiles()) { diff --git a/localization/tutorial_examples/CMakeLists.txt b/localization/tutorial_examples/CMakeLists.txt new file mode 100644 index 0000000000..688f40bbe9 --- /dev/null +++ b/localization/tutorial_examples/CMakeLists.txt @@ -0,0 +1,88 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +cmake_minimum_required(VERSION 3.0) +project(tutorial_examples) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +# Find GTSAM +find_package(GTSAM REQUIRED) + +# System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + factor_adders + graph_factors + imu_integration + localization_common + localization_measurements + node_adders + sliding_window_graph_optimizer + vision_common +) + +# Find OpenCV +LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") +find_package(OpenCV4WithXFeatures REQUIRED) + + +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} + CATKIN_DEPENDS + factor_adders + graph_factors + imu_integration + localization_common + localization_measurements + node_adders + sliding_window_graph_optimizer + vision_common +) + +########### +## Build ## +########### +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_simple_localizer + test/test_simple_localizer.test + test/test_simple_localizer.cc + ) + target_link_libraries(test_simple_localizer + ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_simple_odometry + test/test_simple_odometry.test + test/test_simple_odometry.cc + ) + target_link_libraries(test_simple_odometry + ${catkin_LIBRARIES} + ) + +endif() diff --git a/localization/tutorial_examples/include/tutorial_examples/simple_localizer.h b/localization/tutorial_examples/include/tutorial_examples/simple_localizer.h new file mode 100644 index 0000000000..6eafb87028 --- /dev/null +++ b/localization/tutorial_examples/include/tutorial_examples/simple_localizer.h @@ -0,0 +1,91 @@ +/* Copyright (c) 2017, United States Government, as represented + * by the Administrator of the National Aeronautics and Space + * Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, + * Version 2.0 (the "License"); you may not use this file except + * in compliance with the License. You may obtain a copy of the + * License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific + * language governing permissions and limitations under the + * License. + */ + +#ifndef TUTORIAL_EXAMPLES_SIMPLE_LOCALIZER_H_ +#define TUTORIAL_EXAMPLES_SIMPLE_LOCALIZER_H_ + +#include <factor_adders/pose_factor_adder.h> +#include <localization_measurements/pose_measurement.h> +#include <node_adders/pose_node_adder.h> +#include <optimizers/nonlinear_optimizer.h> +#include <sliding_window_graph_optimizer/sliding_window_graph_optimizer.h> +#include <tutorial_examples/simple_localizer_params.h> + +namespace tutorial_examples { +// Simple localizer that adds absolute pose factors using +// absolute pose measurements (for example, from matches to a +// preexisting map) and adds pose nodes and relative factors +// using relative pose measurements (for example, from an +// odometry provider). Uses a sliding window graph optimizer +// and Levenberg-Marquardt nonlinear optimization. +class SimpleLocalizer : public sliding_window_graph_optimizer:: + SlidingWindowGraphOptimizer { + public: + // Initialize and register the node and factor adders. + explicit SimpleLocalizer(const SimpleLocalizerParams& params) + : SlidingWindowGraphOptimizer( + params.sliding_window_graph_optimizer, + std::make_unique<optimizers::NonlinearOptimizer>( + params.nonlinear_optimizer)) { + // Initialize node and factor adders + node_adder_ = std::make_shared<node_adders::PoseNodeAdder>( + params.pose_node_adder, params.pose_node_adder_model, + values()); + factor_adder_ = + std::make_shared<factor_adders::PoseFactorAdder< + node_adders::PoseNodeAdder>>(params.pose_factor_adder, + node_adder_); + // Register node and factor adders + AddSlidingWindowNodeAdder(node_adder_); + AddFactorAdder(factor_adder_); + } + + // Adds an odometry pose measurement to the pose node + // adder. Assumes the pose is in the odometry frame. + void AddOdometryMeasurement( + const localization_measurements:: + PoseWithCovarianceMeasurement& measurement) { + node_adder_->AddMeasurement(measurement); + } + + // Adds an absolute pose measurement to the pose + // factor adder. Assumes the pose is in the world frame. + void AddPoseMeasurement( + const localization_measurements:: + PoseWithCovarianceMeasurement& measurement) { + factor_adder_->AddMeasurement(measurement); + } + + // Const accessor for optimized pose nodes. + const nodes::TimestampedNodes<gtsam::Pose3>& + timestamped_nodes() const { + return node_adder_->nodes(); + } + + private: + std::shared_ptr< + factor_adders::PoseFactorAdder<node_adders::PoseNodeAdder>> + factor_adder_; + std::shared_ptr<node_adders::PoseNodeAdder> node_adder_; +}; +} // namespace tutorial_examples + +#endif // TUTORIAL_EXAMPLES_SIMPLE_LOCALIZER_H_ diff --git a/localization/tutorial_examples/include/tutorial_examples/simple_localizer_params.h b/localization/tutorial_examples/include/tutorial_examples/simple_localizer_params.h new file mode 100644 index 0000000000..c45f1634a0 --- /dev/null +++ b/localization/tutorial_examples/include/tutorial_examples/simple_localizer_params.h @@ -0,0 +1,76 @@ +/* Copyright (c) 2017, United States Government, as represented + * by the Administrator of the National Aeronautics and Space + * Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, + * Version 2.0 (the "License"); you may not use this file except + * in compliance with the License. You may obtain a copy of the + * License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific + * language governing permissions and limitations under the + * License. + */ + +#ifndef TUTORIAL_EXAMPLES_SIMPLE_LOCALIZER_PARAMS_H_ +#define TUTORIAL_EXAMPLES_SIMPLE_LOCALIZER_PARAMS_H_ + +#include <factor_adders/pose_factor_adder.h> +#include <node_adders/pose_node_adder.h> +#include <node_adders/pose_node_adder_model.h> +#include <node_adders/pose_node_adder_params.h> +#include <optimizers/nonlinear_optimizer_params.h> +#include <sliding_window_graph_optimizer/sliding_window_graph_optimizer_params.h> + +namespace tutorial_examples { +struct SimpleLocalizerParams { + // Initialize params with some default values. + // For more details on the meaning of each param, see the + // respective header file. + SimpleLocalizerParams() { + // Factor Adder + pose_factor_adder.enabled = true; + pose_factor_adder.huber_k = 1.345; + // Node Adder + pose_node_adder.huber_k = 1.345; + pose_node_adder.add_priors = true; + pose_node_adder.starting_time = 0.0; + pose_node_adder.ideal_duration = 10.0; + pose_node_adder.min_num_states = 2; + pose_node_adder.max_num_states = 20; + // Node Adder Model + pose_node_adder_model.huber_k = 1.345; + // Nonlinear Optimizer + nonlinear_optimizer.marginals_factorization = "qr"; + nonlinear_optimizer.max_iterations = 10; + nonlinear_optimizer.verbose = false; + nonlinear_optimizer.use_ceres_params = false; + // Sliding Window Graph Optimizer + sliding_window_graph_optimizer.huber_k = 1.345; + sliding_window_graph_optimizer.log_stats_on_destruction = + false; + sliding_window_graph_optimizer.print_after_optimization = + false; + sliding_window_graph_optimizer.add_marginal_factors = false; + sliding_window_graph_optimizer + .slide_window_before_optimization = true; + } + + factor_adders::PoseFactorAdderParams pose_factor_adder; + node_adders::PoseNodeAdderParams pose_node_adder; + node_adders::PoseNodeAdderModel::Params pose_node_adder_model; + optimizers::NonlinearOptimizerParams nonlinear_optimizer; + sliding_window_graph_optimizer:: + SlidingWindowGraphOptimizerParams + sliding_window_graph_optimizer; +}; +} // namespace tutorial_examples + +#endif // TUTORIAL_EXAMPLES_SIMPLE_LOCALIZER_PARAMS_H_ diff --git a/localization/tutorial_examples/include/tutorial_examples/simple_odometry.h b/localization/tutorial_examples/include/tutorial_examples/simple_odometry.h new file mode 100644 index 0000000000..409e9ccbed --- /dev/null +++ b/localization/tutorial_examples/include/tutorial_examples/simple_odometry.h @@ -0,0 +1,93 @@ +/* Copyright (c) 2017, United States Government, as represented + * by the Administrator of the National Aeronautics and Space + * Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, + * Version 2.0 (the "License"); you may not use this file except + * in compliance with the License. You may obtain a copy of the + * License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific + * language governing permissions and limitations under the + * License. + */ + +#ifndef TUTORIAL_EXAMPLES_SIMPLE_ODOMETRY_H_ +#define TUTORIAL_EXAMPLES_SIMPLE_ODOMETRY_H_ + +#include <factor_adders/relative_pose_factor_adder.h> +#include <localization_measurements/imu_measurement.h> +#include <localization_measurements/relative_pose_with_covariance_measurement.h> +#include <node_adders/combined_nav_state_node_adder.h> +#include <optimizers/nonlinear_optimizer.h> +#include <sliding_window_graph_optimizer/sliding_window_graph_optimizer.h> +#include <tutorial_examples/simple_odometry_params.h> + +namespace tutorial_examples { +// Simple Odometry that adds relative pose factors using +// relative pose measurements (for example, from matching +// successive sensor data) and adds pose, velocity, IMU bias +// nodes using IMU measurements. Uses a sliding window graph +// optimizer and Levenberg-Marquardt nonlinear optimization. +class SimpleOdometry : public sliding_window_graph_optimizer:: + SlidingWindowGraphOptimizer { + public: + // Initialize and register the node and factor adders. + explicit SimpleOdometry(const SimpleOdometryParams& params) + : SlidingWindowGraphOptimizer( + params.sliding_window_graph_optimizer, + std::make_unique<optimizers::NonlinearOptimizer>( + params.nonlinear_optimizer)) { + // Initialize node and factor adders + node_adder_ = + std::make_shared<node_adders::CombinedNavStateNodeAdder>( + params.combined_nav_state_node_adder, + params.combined_nav_state_node_adder_model, values()); + factor_adder_ = + std::make_shared<factor_adders::RelativePoseFactorAdder< + node_adders::CombinedNavStateNodeAdder>>( + params.relative_pose_factor_adder, node_adder_); + // Register node and factor adders + AddSlidingWindowNodeAdder(node_adder_); + AddFactorAdder(factor_adder_); + } + + // Adds an IMU measurement to the combined nav state + // node adder. + void AddImuMeasurement( + const localization_measurements::ImuMeasurement& + measurement) { + node_adder_->AddMeasurement(measurement); + } + + // Adds relative pose measurement to the relative pose node + // adder. Assumes the pose is in the odometry frame. + void AddRelativePoseMeasurement( + const localization_measurements:: + RelativePoseWithCovarianceMeasurement& measurement) { + factor_adder_->AddMeasurement(measurement); + } + + // Const accessor for combined nav state nodes + // containing a pose, velocity, and IMU bias in each node. + const nodes::CombinedNavStateNodes& timestamped_nodes() const { + return node_adder_->nodes(); + } + + private: + std::shared_ptr<factor_adders::RelativePoseFactorAdder< + node_adders::CombinedNavStateNodeAdder>> + factor_adder_; + std::shared_ptr<node_adders::CombinedNavStateNodeAdder> + node_adder_; +}; +} // namespace tutorial_examples + +#endif // TUTORIAL_EXAMPLES_SIMPLE_ODOMETRY_H_ diff --git a/localization/tutorial_examples/include/tutorial_examples/simple_odometry_params.h b/localization/tutorial_examples/include/tutorial_examples/simple_odometry_params.h new file mode 100644 index 0000000000..2bf1de3162 --- /dev/null +++ b/localization/tutorial_examples/include/tutorial_examples/simple_odometry_params.h @@ -0,0 +1,96 @@ +/* Copyright (c) 2017, United States Government, as represented + * by the Administrator of the National Aeronautics and Space + * Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, + * Version 2.0 (the "License"); you may not use this file except + * in compliance with the License. You may obtain a copy of the + * License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific + * language governing permissions and limitations under the + * License. + */ + +#ifndef TUTORIAL_EXAMPLES_SIMPLE_ODOMETRY_PARAMS_H_ +#define TUTORIAL_EXAMPLES_SIMPLE_ODOMETRY_PARAMS_H_ + +#include <factor_adders/pose_factor_adder.h> +#include <node_adders/combined_nav_state_node_adder.h> +#include <node_adders/pose_node_adder_model.h> +#include <node_adders/pose_node_adder_params.h> +#include <optimizers/nonlinear_optimizer_params.h> +#include <sliding_window_graph_optimizer/sliding_window_graph_optimizer_params.h> + +namespace tutorial_examples { +struct SimpleOdometryParams { + // Initialize params with some default values. + // For more details on the meaning of each param, see the + // respective header file. + SimpleOdometryParams() { + // Factor Adder + relative_pose_factor_adder.enabled = true; + relative_pose_factor_adder.huber_k = 1.345; + // Node Adder + combined_nav_state_node_adder.huber_k = 1.345; + combined_nav_state_node_adder.add_priors = true; + combined_nav_state_node_adder.starting_time = 0.0; + combined_nav_state_node_adder.ideal_duration = 10.0; + combined_nav_state_node_adder.min_num_states = 2; + combined_nav_state_node_adder.max_num_states = 20; + // Node Adder Model + combined_nav_state_node_adder_model.huber_k = 1.345; + combined_nav_state_node_adder_model.imu_integrator.gravity = + Eigen::Vector3d::Zero(); + combined_nav_state_node_adder_model.imu_integrator + .body_T_imu = gtsam::Pose3::identity(); + combined_nav_state_node_adder_model.imu_integrator + .gyro_sigma = 0.01; + combined_nav_state_node_adder_model.imu_integrator + .accel_sigma = 0.01; + combined_nav_state_node_adder_model.imu_integrator + .accel_bias_sigma = 0.01; + combined_nav_state_node_adder_model.imu_integrator + .gyro_bias_sigma = 0.01; + combined_nav_state_node_adder_model.imu_integrator + .integration_variance = 0.01; + combined_nav_state_node_adder_model.imu_integrator + .bias_acc_omega_int = 0.01; + // Nonlinear Optimizer + nonlinear_optimizer.marginals_factorization = "qr"; + nonlinear_optimizer.max_iterations = 10; + nonlinear_optimizer.verbose = false; + nonlinear_optimizer.use_ceres_params = false; + // Sliding Window Graph Optimizer + sliding_window_graph_optimizer.huber_k = 1.345; + sliding_window_graph_optimizer.log_stats_on_destruction = + false; + sliding_window_graph_optimizer.print_after_optimization = + false; + sliding_window_graph_optimizer.add_marginal_factors = false; + sliding_window_graph_optimizer + .slide_window_before_optimization = true; + } + + factor_adders::RelativePoseFactorAdderParams + relative_pose_factor_adder; + node_adders::TimestampedNodeAdderParams< + localization_common::CombinedNavState> + combined_nav_state_node_adder; + node_adders::CombinedNavStateNodeAdderModel::Params + combined_nav_state_node_adder_model; + optimizers::NonlinearOptimizerParams nonlinear_optimizer; + sliding_window_graph_optimizer:: + SlidingWindowGraphOptimizerParams + sliding_window_graph_optimizer; +}; +} // namespace tutorial_examples + +#endif // TUTORIAL_EXAMPLES_SIMPLE_ODOMETRY_PARAMS_H_ diff --git a/localization/tutorial_examples/package.xml b/localization/tutorial_examples/package.xml new file mode 100644 index 0000000000..f21e104224 --- /dev/null +++ b/localization/tutorial_examples/package.xml @@ -0,0 +1,33 @@ +<package> + <name>tutorial_examples</name> + <version>1.0.0</version> + <description> + Tutorial examples + </description> + <license> + Apache License, Version 2.0 + </license> + <author email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </author> + <maintainer email="astrobee-fsw@nx.arc.nasa.gov"> + Astrobee Flight Software + </maintainer> + <buildtool_depend>catkin</buildtool_depend> + <build_depend>factor_adders</build_depend> + <build_depend>graph_factors</build_depend> + <build_depend>imu_integration</build_depend> + <build_depend>localization_common</build_depend> + <build_depend>localization_measurements</build_depend> + <build_depend>node_adders</build_depend> + <build_depend>sliding_window_graph_optimizer</build_depend> + <build_depend>vision_common</build_depend> + <run_depend>factor_adders</run_depend> + <run_depend>graph_factors</run_depend> + <run_depend>imu_integration</run_depend> + <run_depend>localization_common</run_depend> + <run_depend>localization_measurements</run_depend> + <run_depend>node_adders</run_depend> + <run_depend>sliding_window_graph_optimizer</run_depend> + <run_depend>vision_common</run_depend> +</package> diff --git a/localization/tutorial_examples/readme.md b/localization/tutorial_examples/readme.md new file mode 100644 index 0000000000..83f259b440 --- /dev/null +++ b/localization/tutorial_examples/readme.md @@ -0,0 +1,4 @@ +\page tutorialexamples Tutorial Examples + +# Package Overview +Examples for using the AstroLoc state estimation library. See the tests for various examples and reference the astroloc_library_quickstart.pdf in the localization/doc directory for more information on the library. diff --git a/localization/tutorial_examples/test/test_simple_localizer.cc b/localization/tutorial_examples/test/test_simple_localizer.cc new file mode 100644 index 0000000000..40628ad1e7 --- /dev/null +++ b/localization/tutorial_examples/test/test_simple_localizer.cc @@ -0,0 +1,194 @@ +/* Copyright (c) 2017, United States Government, as represented + * by the Administrator of the National Aeronautics and Space + * Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, + * Version 2.0 (the "License"); you may not use this file except + * in compliance with the License. You may obtain a copy of the + * License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific + * language governing permissions and limitations under the + * License. + */ + +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <tutorial_examples/simple_localizer.h> + +#include <gtest/gtest.h> + +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace te = tutorial_examples; + +// Generate a random pose measurement at the provided timestamp. +lm::PoseWithCovarianceMeasurement +RandomPoseWithCovarianceMeasurement(const lc::Time time) { + return lm::PoseWithCovarianceMeasurement( + lc::RandomPoseWithCovariance(), time); +} + +// Adds perfect odometry and pose measurements to the graph. +// Eventually adds enough measurements to slide the window. +TEST(SimpleLocalizerTest, Interface) { + // Create random odometry measurements for timestamps 0 - 9. + std::vector<lm::PoseWithCovarianceMeasurement> + odometry_measurements; + for (int i = 0; i < 10; ++i) { + odometry_measurements.emplace_back( + RandomPoseWithCovarianceMeasurement(i)); + } + + // Create random pose measurements for timestamps 0 - 9. + std::vector<lm::PoseWithCovarianceMeasurement> + pose_measurements; + pose_measurements.emplace_back( + RandomPoseWithCovarianceMeasurement(0)); + for (int i = 1; i < 10; ++i) { + // Create perfect pose measurement using first pose + // measurement and subsequent relative odometry. + const auto relative_pose = + odometry_measurements[0].pose.inverse() * + odometry_measurements[i].pose; + const auto pose = pose_measurements[0].pose * relative_pose; + pose_measurements.emplace_back( + lm::PoseWithCovarianceMeasurement( + pose, lc::RandomPoseCovariance(), i)); + } + + // Initialize localizer using the first pose measurement. + te::SimpleLocalizerParams params; + params.pose_node_adder.ideal_duration = 3.0; + params.pose_node_adder.starting_time = + pose_measurements[0].timestamp; + params.pose_node_adder.start_node = pose_measurements[0].pose; + params.pose_node_adder.start_noise_models.emplace_back( + gtsam::noiseModel::Gaussian::Covariance( + pose_measurements[0].covariance)); + te::SimpleLocalizer localizer(params); + + // Add initial odometry estimate so it can be used + // to compute future relative odometry estimates. + localizer.AddOdometryMeasurement(odometry_measurements[0]); + + // Adds initial nodes and priors since no pose measurements + // have been added to the factor adder yet. Optimizes the + // graph. + localizer.Update(); + + // Initial graph should have the starting pose and prior + // factor. + // Expected graph structure: + // I_0 -> N_0 + // Here I_i is the initial prior pose factor, + // N_i is a pose node, + // Index i represents the timestamp. + // Nodes: 1 + // Factors: Initial Prior: 1, Prior Measurement: 0, Between: 0 + EXPECT_EQ(localizer.timestamped_nodes().size(), 1); + EXPECT_EQ(lc::NumFactors<gtsam::PriorFactor<gtsam::Pose3>>( + localizer.factors()), + 1); + EXPECT_EQ(lc::NumFactors<gtsam::BetweenFactor<gtsam::Pose3>>( + localizer.factors()), + 0); + // First node should match first pose measurement pose. + EXPECT_MATRIX_NEAR( + localizer.timestamped_nodes().Node(0)->matrix(), + pose_measurements[0].pose, 1e-6); + + // Add 2 subsequent pose and odometry measurements + for (int i = 1; i < 3; ++i) { + localizer.AddOdometryMeasurement(odometry_measurements[i]); + localizer.AddPoseMeasurement(pose_measurements[i]); + } + + // Adds pose nodes and pose prior factors for new pose + // measurements. Adds relative pose factors between these nodes + // using new odometry measurements. Optimizes the graph. + localizer.Update(); + // Expected graph structure: + // M_1 M_9 + // | | + // V V + // I_0 -> N_0 <-B_0_1-> N_1 <-B_1_2-> N_2 + // M_i is a prior pose factor from a pose measurement, + // B_i_j is a pose between factor from odometry measurements. + // Indices i and j represent timestamps. + // Nodes: 3 + // Factors: Initial Prior: 1, Prior Measurement: 2, Between: 2 + EXPECT_EQ(localizer.timestamped_nodes().size(), 3); + EXPECT_EQ(localizer.factors().size(), 5); + EXPECT_EQ(lc::NumFactors<gtsam::PriorFactor<gtsam::Pose3>>( + localizer.factors()), + 3); + EXPECT_EQ(lc::NumFactors<gtsam::BetweenFactor<gtsam::Pose3>>( + localizer.factors()), + 2); + // Check optimized pose nodes + EXPECT_MATRIX_NEAR( + localizer.timestamped_nodes().Node(0)->matrix(), + pose_measurements[0].pose, 1e-6); + EXPECT_MATRIX_NEAR( + localizer.timestamped_nodes().Node(1)->matrix(), + pose_measurements[1].pose, 1e-6); + EXPECT_MATRIX_NEAR( + localizer.timestamped_nodes().Node(2)->matrix(), + pose_measurements[2].pose, 1e-6); + + // Add 2 more subsequent pose and odometry measurements + for (int i = 3; i < 5; ++i) { + localizer.AddOdometryMeasurement(odometry_measurements[i]); + localizer.AddPoseMeasurement(pose_measurements[i]); + } + + // Graph has surpassed 3 second duration limit, first node + // and between factor should be removed. + localizer.Update(); + // Expected graph structure: + // M_1 M_2 M_3 M_4 + // | | | | + // V V V V + // I_1 -> N_1 <-B_1_2-> N_2 <-B_2_3-> N_3 <-B_3_4-> N_4 + // Nodes: 4 + // Factors: Initial Prior: 1, Prior Measurement: 4, Between: 3 + EXPECT_EQ(localizer.timestamped_nodes().size(), 4); + EXPECT_EQ(lc::NumFactors<gtsam::PriorFactor<gtsam::Pose3>>( + localizer.factors()), + 5); + EXPECT_EQ(lc::NumFactors<gtsam::BetweenFactor<gtsam::Pose3>>( + localizer.factors()), + 3); + // Check optimized pose nodes + EXPECT_MATRIX_NEAR( + localizer.timestamped_nodes().Node(1)->matrix(), + pose_measurements[1].pose, 1e-6); + EXPECT_MATRIX_NEAR( + localizer.timestamped_nodes().Node(2)->matrix(), + pose_measurements[2].pose, 1e-6); + EXPECT_MATRIX_NEAR( + localizer.timestamped_nodes().Node(3)->matrix(), + pose_measurements[3].pose, 1e-6); + EXPECT_MATRIX_NEAR( + localizer.timestamped_nodes().Node(4)->matrix(), + pose_measurements[4].pose, 1e-6); + + // Compute covariance example for node at timestamp 2.0 + const auto keys = localizer.timestamped_nodes().Keys(2.0); + const auto covariance = localizer.Covariance(keys[0]); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/tutorial_examples/test/test_simple_localizer.test b/localization/tutorial_examples/test/test_simple_localizer.test new file mode 100644 index 0000000000..9c26874713 --- /dev/null +++ b/localization/tutorial_examples/test/test_simple_localizer.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="tutorial_examples" type="test_simple_localizer" test-name="test_simple_localizer" /> +</launch> diff --git a/localization/tutorial_examples/test/test_simple_odometry.cc b/localization/tutorial_examples/test/test_simple_odometry.cc new file mode 100644 index 0000000000..ba8a82a456 --- /dev/null +++ b/localization/tutorial_examples/test/test_simple_odometry.cc @@ -0,0 +1,171 @@ +/* Copyright (c) 2017, United States Government, as represented + * by the Administrator of the National Aeronautics and Space + * Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, + * Version 2.0 (the "License"); you may not use this file except + * in compliance with the License. You may obtain a copy of the + * License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, + * either express or implied. See the License for the specific + * language governing permissions and limitations under the + * License. + */ + +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <tutorial_examples/simple_odometry.h> + +#include <gtest/gtest.h> + +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace te = tutorial_examples; + +// Adds perfect relative pose and IMU measurements to the graph. +TEST(SimpleOdometryTest, Interface) { + // Initialize odometry at identity with a non-zero velocity and + // IMU bias. + te::SimpleOdometryParams params; + const gtsam::Pose3 start_pose = gtsam::Pose3::identity(); + // Normally the initial velocity and biases estimates come + // from some initialization method. + const gtsam::Vector3 start_velocity = Eigen::Vector3d(1, 2, 3); + const gtsam::Vector3 start_accel_bias = Eigen::Vector3d(0.01, 0.02, 0.03); + const gtsam::Vector3 start_gyro_bias = Eigen::Vector3d(0.04, 0.05, 0.06); + params.combined_nav_state_node_adder.starting_time = 0.0; + params.combined_nav_state_node_adder.start_node = lc::CombinedNavState( + start_pose, start_velocity, gtsam::imuBias::ConstantBias(start_accel_bias, start_gyro_bias), 0.0); + // Noise model ordering matching node insertion order, + // which is pose, velocity, biases for the + // combined nav state node adder. + params.combined_nav_state_node_adder.start_noise_models.emplace_back(gtsam::noiseModel::Isotropic::Sigma(6, 0.1)); + params.combined_nav_state_node_adder.start_noise_models.emplace_back(gtsam::noiseModel::Isotropic::Sigma(3, 0.2)); + params.combined_nav_state_node_adder.start_noise_models.emplace_back(gtsam::noiseModel::Isotropic::Sigma(6, 0.3)); + te::SimpleOdometry odometry(params); + + // Adds initial nodes and priors since no relative pose + // measurements have been added to the factor adder yet. + // Optimizes the graph. + odometry.Update(); + + // Initial graph should have the starting combined nav state + // and prior factors. + // Expected graph structure: + // I_0 -> N_0 + // Here I_i is the initial combined nav state prior factors, + // containing pose, velocity, and IMU bias priors, so 3 total. + // N_i is a combined nav state node containing a + // pose, velocity, and IMU bias. + // Index i represents the timestamp. + // Nodes: 1 + // Factors: Initial Priors: 3, Relative Pose Measurement: 0, + // Combined Nav State Between: 0 + EXPECT_EQ(odometry.timestamped_nodes().size(), 1); + EXPECT_EQ(lc::NumFactors<gtsam::PriorFactor<gtsam::Pose3>>(odometry.factors()), 1); + EXPECT_EQ(lc::NumFactors<gtsam::PriorFactor<gtsam::Velocity3>>(odometry.factors()), 1); + EXPECT_EQ(lc::NumFactors<gtsam::PriorFactor<gtsam::imuBias::ConstantBias>>(odometry.factors()), 1); + EXPECT_EQ(lc::NumFactors<gtsam::BetweenFactor<gtsam::Pose3>>(odometry.factors()), 0); + EXPECT_EQ(lc::NumFactors<gtsam::CombinedImuFactor>(odometry.factors()), 0); + // First node should match start node. + EXPECT_MATRIX_NEAR(odometry.timestamped_nodes().Node(0)->pose(), + params.combined_nav_state_node_adder.start_node.pose(), 1e-6); + EXPECT_MATRIX_NEAR(odometry.timestamped_nodes().Node(0)->velocity(), + params.combined_nav_state_node_adder.start_node.velocity(), 1e-6); + EXPECT_MATRIX_NEAR(odometry.timestamped_nodes().Node(0)->bias().vector(), + params.combined_nav_state_node_adder.start_node.bias().vector(), 1e-6); + + // Create relative pose and IMU measurements for timestamps 0 + // - 9. Use constant acceleration and zero angular velocity + // motion model. + const Eigen::Vector3d acceleration(0.5, 0.6, 0.7); + const Eigen::Vector3d angular_velocity = Eigen::Vector3d::Zero(); + // Initialize measured biases and velocities to start values. + // Since in this test we assume biases don't change over time + // make these constant. + const Eigen::Vector3d accel_bias = start_accel_bias; + const Eigen::Vector3d gyro_bias = start_gyro_bias; + Eigen::Vector3d velocity = start_velocity; + std::vector<lm::RelativePoseWithCovarianceMeasurement> relative_pose_measurements; + std::vector<lm::ImuMeasurement> imu_measurements; + // Add initial zero acceleration measurement + odometry.AddImuMeasurement(lm::ImuMeasurement(accel_bias, gyro_bias, 0)); + for (int i = 1; i < 3; ++i) { + const lm::ImuMeasurement imu_measurement(acceleration + accel_bias, angular_velocity + gyro_bias, i); + imu_measurements.emplace_back(imu_measurement); + // Motion model for dt = 1.0 sec. + const gtsam::Pose3 relative_pose = gtsam::Pose3(gtsam::Rot3::identity(), 0.5 * acceleration + velocity); + velocity += acceleration; + relative_pose_measurements.emplace_back( + lm::RelativePoseWithCovarianceMeasurement(relative_pose, lc::RandomPoseCovariance(), i - 1, i)); + } + + // Add 2 IMU and relative pose measurements to the odometry + // graph + for (int i = 0; i < 2; ++i) { + odometry.AddImuMeasurement(imu_measurements[i]); + odometry.AddRelativePoseMeasurement(relative_pose_measurements[i]); + } + + // Adds relative pose factors and CombinedNavState nodes for + // new relative pose measurements. Adds CombinedIMU factors + // between these nodes using new IMU measurements. Optimizes + // the graph. + odometry.Update(); + // Expected graph structure: + // <-B_0_1-> <-B_1_2-> + // I_0 -> N_0 N_1 N_2 + // <-R_0_1-> <-R_1_2-> + // R_i_j is a relative pose factor from relative pose + // measurements. B_i_j is a CombinedIMU factor from IMU + // measurements. Indices i and j represent timestamps. Nodes: 3 + // Factors: Initial Priors: 3, Relative Pose: 2, CombinedIMU: 2 + EXPECT_EQ(odometry.timestamped_nodes().size(), 3); + EXPECT_EQ(lc::NumFactors<gtsam::PriorFactor<gtsam::Pose3>>(odometry.factors()), 1); + EXPECT_EQ(lc::NumFactors<gtsam::PriorFactor<gtsam::Velocity3>>(odometry.factors()), 1); + EXPECT_EQ(lc::NumFactors<gtsam::PriorFactor<gtsam::imuBias::ConstantBias>>(odometry.factors()), 1); + EXPECT_EQ(lc::NumFactors<gtsam::BetweenFactor<gtsam::Pose3>>(odometry.factors()), 2); + EXPECT_EQ(lc::NumFactors<gtsam::CombinedImuFactor>(odometry.factors()), 2); + // Check optimized pose nodes + // First node should match start node. + EXPECT_MATRIX_NEAR(odometry.timestamped_nodes().Node(0)->pose(), + params.combined_nav_state_node_adder.start_node.pose(), 1e-6); + EXPECT_MATRIX_NEAR(odometry.timestamped_nodes().Node(0)->velocity(), + params.combined_nav_state_node_adder.start_node.velocity(), 1e-6); + EXPECT_MATRIX_NEAR(odometry.timestamped_nodes().Node(0)->bias().vector(), + params.combined_nav_state_node_adder.start_node.bias().vector(), 1e-6); + // Second node + EXPECT_MATRIX_NEAR( + odometry.timestamped_nodes().Node(1)->pose(), + gtsam::Pose3(params.combined_nav_state_node_adder.start_node.pose() * relative_pose_measurements[0].relative_pose), + 1e-6); + EXPECT_MATRIX_NEAR(odometry.timestamped_nodes().Node(1)->velocity(), + params.combined_nav_state_node_adder.start_node.velocity() + acceleration, 1e-6); + EXPECT_MATRIX_NEAR(odometry.timestamped_nodes().Node(1)->bias().vector(), + params.combined_nav_state_node_adder.start_node.bias().vector(), 1e-6); + // Third node + EXPECT_MATRIX_NEAR( + odometry.timestamped_nodes().Node(2)->pose(), + gtsam::Pose3(params.combined_nav_state_node_adder.start_node.pose() * relative_pose_measurements[0].relative_pose * + relative_pose_measurements[1].relative_pose), + 1e-6); + EXPECT_MATRIX_NEAR(odometry.timestamped_nodes().Node(2)->velocity(), + gtsam::Velocity3(params.combined_nav_state_node_adder.start_node.velocity() + acceleration * 2), + 1e-6); + EXPECT_MATRIX_NEAR(odometry.timestamped_nodes().Node(2)->bias().vector(), + params.combined_nav_state_node_adder.start_node.bias().vector(), 1e-6); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/tutorial_examples/test/test_simple_odometry.test b/localization/tutorial_examples/test/test_simple_odometry.test new file mode 100644 index 0000000000..6cbb302fc7 --- /dev/null +++ b/localization/tutorial_examples/test/test_simple_odometry.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="tutorial_examples" type="test_simple_odometry" test-name="test_simple_odometry" /> +</launch> diff --git a/localization/vision_common/CMakeLists.txt b/localization/vision_common/CMakeLists.txt index bae72dab59..5c024fe2d9 100644 --- a/localization/vision_common/CMakeLists.txt +++ b/localization/vision_common/CMakeLists.txt @@ -60,6 +60,8 @@ add_library(${PROJECT_NAME} src/lk_optical_flow_feature_detector_and_matcher.cc src/parameter_reader.cc src/pose_estimation.cc + src/spaced_feature_track.cc + src/spaced_feature_tracker.cc src/surf_feature_detector_and_matcher.cc src/test_utilities.cc src/utilities.cc @@ -69,6 +71,13 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} gtsam ${OpenCV_LIBRARI if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) + add_rostest_gtest(test_feature_tracker + test/test_feature_tracker.test + test/test_feature_tracker.cc + ) + target_link_libraries(test_feature_tracker + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) add_rostest_gtest(test_inverse_depth_measurement test/test_inverse_depth_measurement.test test/test_inverse_depth_measurement.cc @@ -111,6 +120,20 @@ if(CATKIN_ENABLE_TESTING) target_link_libraries(test_pose_estimation ${PROJECT_NAME} ${catkin_LIBRARIES} ) + add_rostest_gtest(test_spaced_feature_track + test/test_spaced_feature_track.test + test/test_spaced_feature_track.cc + ) + target_link_libraries(test_spaced_feature_track + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_spaced_feature_tracker + test/test_spaced_feature_tracker.test + test/test_spaced_feature_tracker.cc + ) + target_link_libraries(test_spaced_feature_tracker + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) add_rostest_gtest(test_utilities test/test_utilities.test test/test_utilities.cc diff --git a/localization/localization_measurements/include/localization_measurements/feature_point.h b/localization/vision_common/include/vision_common/feature_point.h similarity index 71% rename from localization/localization_measurements/include/localization_measurements/feature_point.h rename to localization/vision_common/include/vision_common/feature_point.h index 8de06d54a7..c86806d53c 100644 --- a/localization/localization_measurements/include/localization_measurements/feature_point.h +++ b/localization/vision_common/include/vision_common/feature_point.h @@ -16,8 +16,8 @@ * under the License. */ -#ifndef LOCALIZATION_MEASUREMENTS_FEATURE_POINT_H_ -#define LOCALIZATION_MEASUREMENTS_FEATURE_POINT_H_ +#ifndef VISION_COMMON_FEATURE_POINT_H_ +#define VISION_COMMON_FEATURE_POINT_H_ #include <localization_common/time.h> @@ -25,18 +25,20 @@ #include <vector> -namespace localization_measurements { +namespace vision_common { using FeatureId = int; using ImageId = int; - +// Point belonging to an image space feature track. +// Contains an image_id corresponding to the image it belongs to and a feature_track_id +// corresponding to the feature track it belongs to. struct FeaturePoint { - FeaturePoint(const double u, const double v, const ImageId image_id, const FeatureId feature_id, + FeaturePoint(const double u, const double v, const ImageId image_id, const FeatureId feature_track_id, const localization_common::Time timestamp) - : image_point(u, v), image_id(image_id), feature_id(feature_id), timestamp(timestamp) {} + : image_point(u, v), image_id(image_id), feature_track_id(feature_track_id), timestamp(timestamp) {} FeaturePoint() {} gtsam::Point2 image_point; ImageId image_id; - FeatureId feature_id; + FeatureId feature_track_id; localization_common::Time timestamp; private: @@ -46,12 +48,12 @@ struct FeaturePoint { void serialize(ARCHIVE& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_NVP(image_point); ar& BOOST_SERIALIZATION_NVP(image_id); - ar& BOOST_SERIALIZATION_NVP(feature_id); + ar& BOOST_SERIALIZATION_NVP(feature_track_id); ar& BOOST_SERIALIZATION_NVP(timestamp); } }; using FeaturePoints = std::vector<FeaturePoint>; -} // namespace localization_measurements +} // namespace vision_common -#endif // LOCALIZATION_MEASUREMENTS_FEATURE_POINT_H_ +#endif // VISION_COMMON_FEATURE_POINT_H_ diff --git a/localization/vision_common/include/vision_common/feature_track.h b/localization/vision_common/include/vision_common/feature_track.h new file mode 100644 index 0000000000..d4bc040770 --- /dev/null +++ b/localization/vision_common/include/vision_common/feature_track.h @@ -0,0 +1,49 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef VISION_COMMON_FEATURE_TRACK_H_ +#define VISION_COMMON_FEATURE_TRACK_H_ + +#include <localization_common/timestamped_set.h> +#include <vision_common/feature_point.h> + +namespace vision_common { +// A Feature track consists of a set of timestamped feature point detections and +// an id. +class FeatureTrack : public localization_common::TimestampedSet<FeaturePoint> { + public: + explicit FeatureTrack(const FeatureId id) : id_(id) {} + // Default constructor for serialization only. + FeatureTrack() = default; + virtual ~FeatureTrack() = default; + const FeatureId& id() const { return id_; } + + private: + // Serialization function + friend class boost::serialization::access; + template <class ARCHIVE> + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(id_); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(localization_common::TimestampedSet<FeaturePoint>); + } + + FeatureId id_; +}; +} // namespace vision_common + +#endif // VISION_COMMON_FEATURE_TRACK_H_ diff --git a/localization/vision_common/include/vision_common/feature_tracker.h b/localization/vision_common/include/vision_common/feature_tracker.h new file mode 100644 index 0000000000..8fe97c73de --- /dev/null +++ b/localization/vision_common/include/vision_common/feature_tracker.h @@ -0,0 +1,194 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef VISION_COMMON_FEATURE_TRACKER_H_ +#define VISION_COMMON_FEATURE_TRACKER_H_ + +#include <localization_common/time.h> +#include <vision_common/feature_point.h> +#include <vision_common/feature_track.h> +#include <vision_common/feature_tracker_params.h> + +#include <map> +#include <vector> + +namespace vision_common { +template <typename FeatureTrackType = FeatureTrack> +class FeatureTracker { + public: + using IdFeatureTrackMap = std::map<FeatureId, FeatureTrackType>; + explicit FeatureTracker(const FeatureTrackerParams& params); + + // Default constructor only for serialization + FeatureTracker() = default; + + virtual ~FeatureTracker() = default; + + // Add new feature points to existing or new tracks. Optionally removes + // any existing tracks that weren't detected in passed feature_points. + virtual void Update(const FeaturePoints& feature_points); + + // Remove any points older than oldest_allowed_time from each feature track. + // Removes any feature tracks that subsequently have no more detections. + virtual void RemoveOldPoints(const localization_common::Time oldest_allowed_time); + + // Returns a reference to the feature tracks. + const IdFeatureTrackMap& feature_tracks() const; + + // Returns feature track references ordered from longest to shortest + std::vector<std::reference_wrapper<const FeatureTrackType>> FeatureTracksLengthOrdered() const; + + // Returns the number of feature tracks. + size_t size() const; + + // Returns if no feature tracks exist. + bool empty() const; + + // Deletes all feature tracks. + virtual void Clear(); + + private: + // Add new feature point to an existing track with the same track id + // create a new track if this doesn't exist. + void AddOrUpdateTrack(const FeaturePoint& feature_point); + + // Remove any feature tracks without detections at the provided timestamp + void RemoveUndetectedFeatureTracks(const localization_common::Time& time); + + // Remove any feature tracks with no detections. + void RemoveEmptyFeatureTracks(); + + // Serialization function + friend class boost::serialization::access; + template <class ARCHIVE> + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(id_feature_track_map_); + ar& BOOST_SERIALIZATION_NVP(params_); + } + + IdFeatureTrackMap id_feature_track_map_; + FeatureTrackerParams params_; +}; + +// Implementation +template <typename FeatureTrackType> +FeatureTracker<FeatureTrackType>::FeatureTracker(const FeatureTrackerParams& params) : params_(params) {} + +template <typename FeatureTrackType> +void FeatureTracker<FeatureTrackType>::Update(const FeaturePoints& feature_points) { + if (feature_points.empty() && params_.remove_undetected_feature_tracks) { + Clear(); + LogDebug("UpdateFeatureTracks: Removed all feature tracks."); + return; + } + + const int starting_num_feature_tracks = size(); + LogDebug("UpdateFeatureTracks: Starting num feature tracks: " << starting_num_feature_tracks); + for (const auto& feature_point : feature_points) { + AddOrUpdateTrack(feature_point); + } + const int post_add_num_feature_tracks = size(); + LogDebug("UpdateFeatureTracks: Added feature tracks: " << post_add_num_feature_tracks - starting_num_feature_tracks); + + if (params_.remove_undetected_feature_tracks) { + const auto feature_points_timestamp = feature_points.front().timestamp; + RemoveUndetectedFeatureTracks(feature_points_timestamp); + } + const int removed_num_feature_tracks = post_add_num_feature_tracks - size(); + LogDebug("UpdateFeatureTracks: Removed feature tracks: " << removed_num_feature_tracks); + LogDebug("UpdateFeatureTracks: Final total num feature tracks: " << size()); +} + +template <typename FeatureTrackType> +void FeatureTracker<FeatureTrackType>::RemoveOldPoints(const localization_common::Time oldest_allowed_time) { + for (auto& feature_track : id_feature_track_map_) { + feature_track.second.RemoveOldValues(oldest_allowed_time); + } + RemoveEmptyFeatureTracks(); +} + +template <typename FeatureTrackType> +const typename FeatureTracker<FeatureTrackType>::IdFeatureTrackMap& FeatureTracker<FeatureTrackType>::feature_tracks() + const { + return id_feature_track_map_; +} + +template <typename FeatureTrackType> +std::vector<std::reference_wrapper<const FeatureTrackType>> +FeatureTracker<FeatureTrackType>::FeatureTracksLengthOrdered() const { + std::map<int, int> length_id_map; + for (const auto& feature_track : feature_tracks()) { + length_id_map.insert({feature_track.second.size(), feature_track.first}); + } + std::vector<std::reference_wrapper<const FeatureTrackType>> feature_tracks_length_ordered; + // Add tracks in order from longest to shortest + for (auto it = length_id_map.crbegin(); it != length_id_map.crend(); ++it) { + feature_tracks_length_ordered.emplace_back((this->feature_tracks().at(it->second))); + } + + return feature_tracks_length_ordered; +} + +template <typename FeatureTrackType> +size_t FeatureTracker<FeatureTrackType>::size() const { + return id_feature_track_map_.size(); +} + +template <typename FeatureTrackType> +bool FeatureTracker<FeatureTrackType>::empty() const { + return id_feature_track_map_.empty(); +} + +template <typename FeatureTrackType> +void FeatureTracker<FeatureTrackType>::Clear() { + id_feature_track_map_.clear(); +} + +template <typename FeatureTrackType> +void FeatureTracker<FeatureTrackType>::AddOrUpdateTrack(const FeaturePoint& feature_point) { + if (id_feature_track_map_.count(feature_point.feature_track_id) == 0) { + id_feature_track_map_[feature_point.feature_track_id] = FeatureTrackType(feature_point.feature_track_id); + } + id_feature_track_map_[feature_point.feature_track_id].Add(feature_point.timestamp, feature_point); +} + +template <typename FeatureTrackType> +void FeatureTracker<FeatureTrackType>::RemoveUndetectedFeatureTracks( + const localization_common::Time& feature_point_timestamp) { + for (auto feature_it = id_feature_track_map_.cbegin(); feature_it != id_feature_track_map_.cend();) { + if (!feature_it->second.Contains(feature_point_timestamp)) { + feature_it = id_feature_track_map_.erase(feature_it); + } else { + ++feature_it; + } + } +} + +template <typename FeatureTrackType> +void FeatureTracker<FeatureTrackType>::RemoveEmptyFeatureTracks() { + for (auto feature_it = id_feature_track_map_.cbegin(); feature_it != id_feature_track_map_.cend();) { + if (feature_it->second.empty()) { + feature_it = id_feature_track_map_.erase(feature_it); + } else { + ++feature_it; + } + } +} +} // namespace vision_common + +#endif // VISION_COMMON_FEATURE_TRACKER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/feature_counts.h b/localization/vision_common/include/vision_common/feature_tracker_params.h similarity index 67% rename from localization/graph_localizer/include/graph_localizer/feature_counts.h rename to localization/vision_common/include/vision_common/feature_tracker_params.h index 15fa121066..b81b47ba7f 100644 --- a/localization/graph_localizer/include/graph_localizer/feature_counts.h +++ b/localization/vision_common/include/vision_common/feature_tracker_params.h @@ -15,25 +15,14 @@ * License for the specific language governing permissions and limitations * under the License. */ +#ifndef VISION_COMMON_FEATURE_TRACKER_PARAMS_H_ +#define VISION_COMMON_FEATURE_TRACKER_PARAMS_H_ -#ifndef GRAPH_LOCALIZER_FEATURE_COUNTS_H_ -#define GRAPH_LOCALIZER_FEATURE_COUNTS_H_ - -namespace graph_localizer { - -struct FeatureCounts { - void Reset() { - vl = 0; - of = 0; - ar = 0; - depth = 0; - } - - int vl = 0; - int of = 0; - int ar = 0; - int depth = 0; +namespace vision_common { +struct FeatureTrackerParams { + // Remove a feature track if not detected in the most recently provided set of feature points. + bool remove_undetected_feature_tracks; }; -} // namespace graph_localizer +} // namespace vision_common -#endif // GRAPH_LOCALIZER_FEATURE_COUNTS_H_ +#endif // VISION_COMMON_FEATURE_TRACKER_PARAMS_H_ diff --git a/localization/vision_common/include/vision_common/spaced_feature_track.h b/localization/vision_common/include/vision_common/spaced_feature_track.h new file mode 100644 index 0000000000..4ce148e8d0 --- /dev/null +++ b/localization/vision_common/include/vision_common/spaced_feature_track.h @@ -0,0 +1,66 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef VISION_COMMON_SPACED_FEATURE_TRACK_H_ +#define VISION_COMMON_SPACED_FEATURE_TRACK_H_ + +#include <vision_common/feature_track.h> + +#include <set> +#include <vector> + +namespace vision_common { +// Feature track with additional methods to return downsampled or +// set duration feature tracks. +// Allows for maximally spaced tracks, downsampled tracks given a set spacing, +// and downsampled tracks using only allowabled timestamps. +class SpacedFeatureTrack : public FeatureTrack { + public: + explicit SpacedFeatureTrack(const FeatureId id); + // Default constructor for serialization only. + SpacedFeatureTrack() = default; + + virtual ~SpacedFeatureTrack() = default; + + // Returns the latest set of points spaced by the provided spacing. + // Starts sampling with the latest point. + // Return vector is ordered from oldest to latest points. + std::vector<FeaturePoint> LatestSpacedPoints(const int spacing = 0) const; + + // Returns the max spacing usable for a feature track + // such that the total number of points in the feature + // track does not exceed max_num_points. + int MaxSpacing(const int max_num_points) const; + + // Returns the second latest point's timestamp in the feature track. + boost::optional<localization_common::Time> SecondLatestTimestamp() const; + + private: + bool SpacingFits(const int spacing, const int max_num_points) const; + int ClosestSpacing(const int ideal_spacing, const int ideal_max_num_points) const; + + // Serialization function + friend class boost::serialization::access; + template <class ARCHIVE> + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(FeatureTrack); + } +}; +} // namespace vision_common + +#endif // VISION_COMMON_SPACED_FEATURE_TRACK_H_ diff --git a/localization/vision_common/include/vision_common/spaced_feature_tracker.h b/localization/vision_common/include/vision_common/spaced_feature_tracker.h new file mode 100644 index 0000000000..d9c23be120 --- /dev/null +++ b/localization/vision_common/include/vision_common/spaced_feature_tracker.h @@ -0,0 +1,70 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef VISION_COMMON_SPACED_FEATURE_TRACKER_H_ +#define VISION_COMMON_SPACED_FEATURE_TRACKER_H_ + +#include <localization_common/time.h> +#include <localization_common/timestamped_set.h> +#include <vision_common/feature_tracker.h> +#include <vision_common/spaced_feature_track.h> +#include <vision_common/spaced_feature_tracker_params.h> + +#include <set> +#include <vector> + +namespace vision_common { +class SpacedFeatureTracker : public FeatureTracker<SpacedFeatureTrack> { + using Base = FeatureTracker<SpacedFeatureTrack>; + + public: + explicit SpacedFeatureTracker(const SpacedFeatureTrackerParams& params); + + // Default constructor only for serialization + SpacedFeatureTracker() = default; + + // Updates FeatureTracker and updates allowed timestamps. + void Update(const FeaturePoints& feature_points) final; + + // Removes old points from FeatureTracker and allowed timestamps. + void RemoveOldPoints(const localization_common::Time oldest_allowed_time) final; + + // Clears FeatureTracker and allowed timestamps. + void Clear() final; + + // Returns feature tracks spaced using allowed timestamps. + // Skips feature tracks with no allowed timestamps. + std::vector<FeaturePoints> SpacedFeatureTracks() const; + + private: + // Serialization function + friend class boost::serialization::access; + template <class ARCHIVE> + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(allowed_timestamps_); + ar& BOOST_SERIALIZATION_NVP(params_); + } + + int measurement_count_; + std::set<localization_common::Time> allowed_timestamps_; + SpacedFeatureTrackerParams params_; +}; +} // namespace vision_common + +#endif // VISION_COMMON_SPACED_FEATURE_TRACKER_H_ diff --git a/localization/graph_optimizer/include/graph_optimizer/graph_action_completer_type.h b/localization/vision_common/include/vision_common/spaced_feature_tracker_params.h similarity index 64% rename from localization/graph_optimizer/include/graph_optimizer/graph_action_completer_type.h rename to localization/vision_common/include/vision_common/spaced_feature_tracker_params.h index 9fe744bc25..92d42ed6f5 100644 --- a/localization/graph_optimizer/include/graph_optimizer/graph_action_completer_type.h +++ b/localization/vision_common/include/vision_common/spaced_feature_tracker_params.h @@ -15,19 +15,16 @@ * License for the specific language governing permissions and limitations * under the License. */ +#ifndef VISION_COMMON_SPACED_FEATURE_TRACKER_PARAMS_H_ +#define VISION_COMMON_SPACED_FEATURE_TRACKER_PARAMS_H_ -#ifndef GRAPH_OPTIMIZER_GRAPH_ACTION_COMPLETER_TYPE_H_ -#define GRAPH_OPTIMIZER_GRAPH_ACTION_COMPLETER_TYPE_H_ +#include <vision_common/feature_tracker_params.h> -namespace graph_optimizer { -// TODO(rsoussan): Generalize this better -enum class GraphActionCompleterType { - ARTagLocProjectionFactor, - LocProjectionFactor, - None, - ProjectionFactor, - SmartFactor +namespace vision_common { +struct SpacedFeatureTrackerParams : FeatureTrackerParams { + // Spacing between measurements used for allowed timestamps. + int measurement_spacing; }; -} // namespace graph_optimizer +} // namespace vision_common -#endif // GRAPH_OPTIMIZER_GRAPH_ACTION_COMPLETER_TYPE_H_ +#endif // VISION_COMMON_SPACED_FEATURE_TRACKER_PARAMS_H_ diff --git a/localization/vision_common/include/vision_common/standstill_params.h b/localization/vision_common/include/vision_common/standstill_params.h new file mode 100644 index 0000000000..47df251bf8 --- /dev/null +++ b/localization/vision_common/include/vision_common/standstill_params.h @@ -0,0 +1,45 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef VISION_COMMON_STANDSTILL_PARAMS_H_ +#define VISION_COMMON_STANDSTILL_PARAMS_H_ + +#include <boost/serialization/serialization.hpp> + +namespace vision_common { +struct StandstillParams { + // Min number of points for a track to be used for standstill calculation. + int min_num_points_per_track; + // Max track duration to consider for standstill. + double duration; + // Max average deviation for image point detections in feature tracks for standstill. + double max_avg_distance_from_mean; + + private: + // Serialization function + // TODO(rsoussan): Fix this, causing compile error + /*friend class boost::serialization::access; + template <class Archive> + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_NVP(min_num_points_per_track); + ar& BOOST_SERIALIZATION_NVP(duration); + ar& BOOST_SERIALIZATION_NVP(max_avg_distance_from_mean); + }*/ +}; +} // namespace vision_common + +#endif // VISION_COMMON_STANDSTILL_PARAMS_H_ diff --git a/localization/vision_common/include/vision_common/utilities.h b/localization/vision_common/include/vision_common/utilities.h index a444be98c6..64cb890679 100644 --- a/localization/vision_common/include/vision_common/utilities.h +++ b/localization/vision_common/include/vision_common/utilities.h @@ -18,6 +18,10 @@ #ifndef VISION_COMMON_UTILITIES_H_ #define VISION_COMMON_UTILITIES_H_ +#include <localization_common/timestamped_set.h> +#include <vision_common/feature_point.h> +#include <vision_common/standstill_params.h> + #include <Eigen/Geometry> #include <gtsam/base/Matrix.h> @@ -25,6 +29,8 @@ #include <opencv2/core/eigen.hpp> #include <opencv2/opencv.hpp> +#include <vector> + namespace vision_common { template <typename T> Eigen::Matrix<T, 2, 1> RelativeCoordinates(const Eigen::Matrix<T, 2, 1>& absolute_point, @@ -49,6 +55,16 @@ Eigen::Vector2d ProjectWithDistortion(const Eigen::Vector3d& cam_t_point, const Eigen::Isometry3d Isometry3d(const cv::Mat& rodrigues_rotation_cv, const cv::Mat& translation_cv); +double AverageDistanceFromMean(const std::vector<FeaturePoint>& points); + +double AverageDistanceFromMean( + const std::vector<localization_common::TimestampedValue<FeaturePoint>>& timestamped_points); + +// Detect standstill using average distance from mean for a history of points +// in feature tracks as determined by params. +template <typename FeatureTracksMapType> +bool Standstill(const FeatureTracksMapType& feature_tracks, const StandstillParams& params); + // Implementation template <typename T> Eigen::Matrix<T, 2, 1> RelativeCoordinates(const Eigen::Matrix<T, 2, 1>& absolute_point, @@ -82,6 +98,33 @@ Eigen::Vector2d ProjectWithDistortion(const Eigen::Vector3d& cam_t_point, const const DISTORTER distorter; return distorter.Distort(distortion_params, intrinsics, undistorted_image_point); } + +template <typename FeatureTracksMapType> +bool Standstill(const FeatureTracksMapType& feature_tracks, const StandstillParams& params) { + if (feature_tracks.empty()) return false; + const auto& latest = feature_tracks.cbegin()->second.Latest(); + if (!latest) return false; + const localization_common::Time oldest_allowed_time = latest->timestamp - params.duration; + // Check for standstill via low disparity for all feature tracks + double total_average_distance_from_mean = 0; + int num_valid_feature_tracks = 0; + for (const auto& feature_track : feature_tracks) { + // Only use recent values + const double average_distance_from_mean = + AverageDistanceFromMean(feature_track.second.LatestValues(oldest_allowed_time)); + // Only consider long enough feature tracks for standstill candidates + if (static_cast<int>(feature_track.second.size()) >= params.min_num_points_per_track) { + total_average_distance_from_mean += average_distance_from_mean; + ++num_valid_feature_tracks; + } + } + + double average_distance_from_mean = 0; + if (num_valid_feature_tracks > 0) + average_distance_from_mean = total_average_distance_from_mean / num_valid_feature_tracks; + + return (num_valid_feature_tracks >= 5 && average_distance_from_mean <= params.max_avg_distance_from_mean); +} } // namespace vision_common #endif // VISION_COMMON_UTILITIES_H_ diff --git a/localization/vision_common/readme.md b/localization/vision_common/readme.md index 70843ee460..89f4b30c02 100644 --- a/localization/vision_common/readme.md +++ b/localization/vision_common/readme.md @@ -11,3 +11,5 @@ Brisk, surf, and lucas kanade optical flow feature detector and matchers are pro ### Utilities The utilities files provide helper functions for the distortion models and templated implementations based on the distortion type of image space projections, ransac pnp, and a reprojection pose estimate function that utilities ransac pnp to find an initial pose estimate for a set of 3d and corresponding image points before performing nonliner optimization to refine this pose estimate. +### FeatureTracker +The FeatureTracker maintains feature tracks for image based measurements. Feature tracks can be queried using feature ids or by returning the N longest feature tracks. The feature tracker also slides the window for feature tracks to remove old measurements if necessary. diff --git a/localization/vision_common/src/spaced_feature_track.cc b/localization/vision_common/src/spaced_feature_track.cc new file mode 100644 index 0000000000..60f6e64921 --- /dev/null +++ b/localization/vision_common/src/spaced_feature_track.cc @@ -0,0 +1,73 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <vision_common/spaced_feature_track.h> +#include <localization_common/logger.h> + +namespace vision_common { +namespace lc = localization_common; +SpacedFeatureTrack::SpacedFeatureTrack(const FeatureId id) : FeatureTrack(id) {} + +std::vector<FeaturePoint> SpacedFeatureTrack::LatestSpacedPoints(const int spacing) const { + std::vector<FeaturePoint> latest_points; + int i = 0; + // Start with latest points + for (auto point_it = set().rbegin(); point_it != set().rend(); ++point_it) { + if (i++ % (spacing + 1) != 0) continue; + latest_points.push_back(point_it->second); + } + // Flip so oldest points are first + std::reverse(latest_points.begin(), latest_points.end()); + return latest_points; +} + +bool SpacedFeatureTrack::SpacingFits(const int spacing, const int max_num_points) const { + // Since we include the latest point, the points included for spacing and max_num_points + // is 1 for the latest point plus a point at intervals of spacing + 1 for max_num_points - 1 (excludes latest point). + // 1 0 0 1 0 0 1 0 0 -> here a 1 indicates a point used, the first 1 is the latest point, and the spacing is 2. + // In this case if max_num_points <= 3 this suceeds and otherwise this fails as fewer than 3 points would be included + // with the desired spacing. + return ((spacing + 1) * (max_num_points - 1) + 1) <= static_cast<int>(size()); +} + +int SpacedFeatureTrack::MaxSpacing(const int max_num_points) const { + // Avoid divide by zero and other corner cases + if (max_num_points <= 1 || static_cast<int>(size()) <= 0) return 0; + // Derived using equation from SpacingFits + // (spacing + 1 ) * (max_num_points - 1) + 1 = size + // -> spacing = (size - max_num_points)/(max_num_points - 1) + return std::max(0, (static_cast<int>(size()) - max_num_points) / (max_num_points - 1)); +} + +int SpacedFeatureTrack::ClosestSpacing(const int ideal_spacing, const int ideal_max_num_points) const { + // Check Ideal Case + if (SpacingFits(ideal_spacing, ideal_max_num_points)) return ideal_spacing; + // Check too few points case + if (static_cast<int>(size()) <= ideal_max_num_points) return 0; + // Derive new optimal spacing for ideal_max_num_points + for (int spacing = ideal_spacing - 1; spacing >= 0; --spacing) { + if (SpacingFits(spacing, ideal_max_num_points)) return spacing; + } + return 0; +} + +boost::optional<lc::Time> SpacedFeatureTrack::SecondLatestTimestamp() const { + if (size() < 2) return boost::none; + return std::next(set().crbegin())->first; +} +} // namespace vision_common diff --git a/localization/vision_common/src/spaced_feature_tracker.cc b/localization/vision_common/src/spaced_feature_tracker.cc new file mode 100644 index 0000000000..f7d2877b80 --- /dev/null +++ b/localization/vision_common/src/spaced_feature_tracker.cc @@ -0,0 +1,59 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <vision_common/spaced_feature_tracker.h> +#include <localization_common/logger.h> + +namespace vision_common { +namespace lc = localization_common; +SpacedFeatureTracker::SpacedFeatureTracker(const SpacedFeatureTrackerParams& params) + : Base(params), params_(params), measurement_count_(0) {} + +void SpacedFeatureTracker::Update(const FeaturePoints& feature_points) { + Base::Update(feature_points); + // Space out measurements by provided spacing + if (!feature_points.empty() && ((measurement_count_++ % (params_.measurement_spacing + 1)) == 0)) + allowed_timestamps_.emplace(feature_points.front().timestamp); +} + +void SpacedFeatureTracker::RemoveOldPoints(const localization_common::Time oldest_allowed_time) { + Base::RemoveOldPoints(oldest_allowed_time); + // Remove allowed measurements older than oldest allowed time + allowed_timestamps_.erase(allowed_timestamps_.begin(), allowed_timestamps_.lower_bound(oldest_allowed_time)); +} + +void SpacedFeatureTracker::Clear() { + Base::Clear(); + allowed_timestamps_.clear(); +} + +std::vector<FeaturePoints> SpacedFeatureTracker::SpacedFeatureTracks() const { + std::vector<FeaturePoints> spaced_feature_tracks; + for (const auto& feature_track : feature_tracks()) { + const auto downsampled_track = feature_track.second.DownsampledValues(allowed_timestamps_); + if (downsampled_track.empty()) continue; + FeaturePoints points; + for (const auto& point : downsampled_track) { + points.emplace_back(point.value); + } + spaced_feature_tracks.emplace_back(points); + } + + return spaced_feature_tracks; +} +} // namespace vision_common diff --git a/localization/vision_common/src/utilities.cc b/localization/vision_common/src/utilities.cc index 191e302217..cb317f1f96 100644 --- a/localization/vision_common/src/utilities.cc +++ b/localization/vision_common/src/utilities.cc @@ -64,4 +64,30 @@ Eigen::Isometry3d Isometry3d(const cv::Mat& rodrigues_rotation_cv, const cv::Mat cv::cv2eigen(rotation_cv, rotation); return lc::Isometry3d(translation, rotation); } + +double AverageDistanceFromMean(const std::vector<lc::TimestampedValue<FeaturePoint>>& timestamped_points) { + // TODO(rsoussan): Do this more efficiently/avoid conversion + std::vector<FeaturePoint> points; + for (const auto& timestamped_point : timestamped_points) { + points.emplace_back(timestamped_point.value); + } + return AverageDistanceFromMean(points); +} + +double AverageDistanceFromMean(const std::vector<FeaturePoint>& points) { + // Calculate mean point and avg distance from mean + Eigen::Vector2d sum_of_points = Eigen::Vector2d::Zero(); + for (const auto& point : points) { + sum_of_points += point.image_point; + } + const Eigen::Vector2d mean_point = sum_of_points / points.size(); + + double sum_of_distances_from_mean = 0; + for (const auto& point : points) { + const Eigen::Vector2d mean_centered_point = point.image_point - mean_point; + sum_of_distances_from_mean += mean_centered_point.norm(); + } + const double average_distance_from_mean = sum_of_distances_from_mean / points.size(); + return average_distance_from_mean; +} } // namespace vision_common diff --git a/localization/vision_common/test/test_feature_tracker.cc b/localization/vision_common/test/test_feature_tracker.cc new file mode 100644 index 0000000000..79842edb49 --- /dev/null +++ b/localization/vision_common/test/test_feature_tracker.cc @@ -0,0 +1,418 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <vision_common/feature_tracker.h> +#include <vision_common/test_utilities.h> + +#include <gtest/gtest.h> + +namespace lc = localization_common; +namespace vc = vision_common; + +class FeatureTrackerTest : public ::testing::Test { + public: + FeatureTrackerTest() {} + + // Adds increasing measurements then decreasing measurements. + // track 0: ..... + // track 1: ... + // track 2: . + // Here the x axis is the timestamp and the dot indicates whether a measurement + // occured for the track. + void SetUp() final { + const int num_timestamps = + num_timestamps_with_increasing_measurements_ + num_timestamps_with_decreasing_measurements_; + for (int time = 0; time < num_timestamps; ++time) { + std::vector<vc::FeaturePoint> points; + // Add fewer older measurements initially to tracks to help test removing old measurements. + if (time < num_timestamps_with_increasing_measurements_) { + for (int id = 0; id <= time; ++id) { + // Make image points different for different measurements + const vc::FeaturePoint p(id * 10 + time, id * 10 + time + 1, time + 1, id, time); + points_.emplace_back(p); + points.emplace_back(p); + } + timestamped_points_.emplace_back(points); + } else { + // Add fewer newer measurements later to tracks to help test removing undetected tracks. + for (int id = 0; id < num_timestamps - time; ++id) { + // Make image points different for different measurements + const vc::FeaturePoint p(id * 10 + time, id * 10 + time + 1, time + 1, id, time); + points_.emplace_back(p); + points.emplace_back(p); + } + timestamped_points_.emplace_back(points); + } + } + } + + void EXPECT_SAME_POINT(const vc::FeaturePoint& p_a, const vc::FeaturePoint& p_b) { + EXPECT_EQ(p_a.image_id, p_b.image_id); + EXPECT_EQ(p_a.feature_track_id, p_b.feature_track_id); + EXPECT_MATRIX_NEAR(p_a.image_point, p_b.image_point, 1e-6); + EXPECT_NEAR(p_a.timestamp, p_b.timestamp, 1e-6); + } + + void EXPECT_SAME_POINT(const int timestamp_index, const int measurement_index) { + const auto& added_point = timestamped_points_[timestamp_index][measurement_index]; + const auto id = added_point.feature_track_id; + const auto time = added_point.timestamp; + const auto& track = feature_tracker_->feature_tracks().at(id); + const auto& track_point = track.Get(time); + ASSERT_TRUE(track_point != boost::none); + EXPECT_SAME_POINT(added_point, track_point->value); + } + + void EXPECT_TRACK_SIZE_EQ(const int track_index, const int size) { + EXPECT_EQ(feature_tracker_->feature_tracks().at(track_index).size(), size); + } + + void InitializeWithRemoval() { + vc::FeatureTrackerParams params; + params.remove_undetected_feature_tracks = true; + feature_tracker_.reset(new vc::FeatureTracker<>(params)); + } + + void InitializeWithoutRemoval() { + vc::FeatureTrackerParams params; + params.remove_undetected_feature_tracks = false; + feature_tracker_.reset(new vc::FeatureTracker<>(params)); + } + + const int num_tracks_ = 3; + const int num_timestamps_with_increasing_measurements_ = 2; + const int num_timestamps_with_decreasing_measurements_ = 3; + vc::FeaturePoints points_; + std::vector<vc::FeaturePoints> timestamped_points_; + std::unique_ptr<vc::FeatureTracker<>> feature_tracker_; +}; + +TEST_F(FeatureTrackerTest, SizeEmptyClear) { + InitializeWithoutRemoval(); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); + + // Add measurements + feature_tracker_->Update(points_); + EXPECT_EQ(feature_tracker_->size(), num_tracks_); + EXPECT_FALSE(feature_tracker_->empty()); + + // Remove measurements + feature_tracker_->Clear(); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); +} + +// Without removal, tracks without detections should continue +// to exist. +TEST_F(FeatureTrackerTest, AddMeasurentsWithoutRemoval) { + InitializeWithoutRemoval(); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); + + // Add first set of timestamped measurements + feature_tracker_->Update(timestamped_points_[0]); + EXPECT_EQ(feature_tracker_->size(), 1); + { + // Only one point exists in first measurement + EXPECT_TRACK_SIZE_EQ(0, 1); + EXPECT_SAME_POINT(0, 0); + } + // Add second set of timestamped measurements + feature_tracker_->Update(timestamped_points_[1]); + EXPECT_EQ(feature_tracker_->size(), 2); + { + // Check first track which should have two points + EXPECT_TRACK_SIZE_EQ(0, 2); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + // Check second track which should have one point at second timestamp + EXPECT_TRACK_SIZE_EQ(1, 1); + EXPECT_SAME_POINT(1, 1); + } + // Add 3rd set of timestamped measurements + feature_tracker_->Update(timestamped_points_[2]); + EXPECT_EQ(feature_tracker_->size(), 3); + { + // Check first track which should have 3 points + EXPECT_TRACK_SIZE_EQ(0, 3); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + // Check second track which should have 2 points + EXPECT_TRACK_SIZE_EQ(1, 2); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + // Check third track which should have one point + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } + // Add 4th set of timestamped measurements + feature_tracker_->Update(timestamped_points_[3]); + EXPECT_EQ(feature_tracker_->size(), 3); + { + // Check first track which should have 4 points + EXPECT_TRACK_SIZE_EQ(0, 4); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + EXPECT_SAME_POINT(3, 0); + // Check second track which should have 3 points + EXPECT_TRACK_SIZE_EQ(1, 3); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + EXPECT_SAME_POINT(3, 1); + // Check third track which should have 1 point + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } + // Add 5th set of timestamped measurements + feature_tracker_->Update(timestamped_points_[4]); + EXPECT_EQ(feature_tracker_->size(), 3); + { + // Check first track which should have 5 points + EXPECT_TRACK_SIZE_EQ(0, 5); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + EXPECT_SAME_POINT(3, 0); + EXPECT_SAME_POINT(4, 0); + // Check second track which should have 3 points + EXPECT_TRACK_SIZE_EQ(1, 3); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + EXPECT_SAME_POINT(3, 1); + // Check third track which should have 1 points + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } + // Add empty measurement + // Tracks should be unaffected + feature_tracker_->Update(vc::FeaturePoints()); + EXPECT_EQ(feature_tracker_->size(), 3); + { + // Check first track which should have 5 points + EXPECT_TRACK_SIZE_EQ(0, 5); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + EXPECT_SAME_POINT(3, 0); + EXPECT_SAME_POINT(4, 0); + // Check second track which should have 3 points + EXPECT_TRACK_SIZE_EQ(1, 3); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + EXPECT_SAME_POINT(3, 1); + // Check third track which should have 1 points + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } +} + +// With removal tracks without detections should gradually be +// removed. +TEST_F(FeatureTrackerTest, AddMeasurentsWithRemoval) { + InitializeWithRemoval(); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); + + // Add first set of timestamped measurements + feature_tracker_->Update(timestamped_points_[0]); + EXPECT_EQ(feature_tracker_->size(), 1); + { + // Only one point exists in first measurement + EXPECT_TRACK_SIZE_EQ(0, 1); + EXPECT_SAME_POINT(0, 0); + } + // Add second set of timestamped measurements + feature_tracker_->Update(timestamped_points_[1]); + EXPECT_EQ(feature_tracker_->size(), 2); + { + // Check first track which should have two points + EXPECT_TRACK_SIZE_EQ(0, 2); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + // Check second track which should have one point at second timestamp + EXPECT_TRACK_SIZE_EQ(1, 1); + EXPECT_SAME_POINT(1, 1); + } + // Add 3rd set of timestamped measurements + feature_tracker_->Update(timestamped_points_[2]); + EXPECT_EQ(feature_tracker_->size(), 3); + { + // Check first track which should have 3 points + EXPECT_TRACK_SIZE_EQ(0, 3); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + // Check second track which should have 2 points + EXPECT_TRACK_SIZE_EQ(1, 2); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + // Check third track which should have one point + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } + + // Now since measurements taper off, tracks should start to be removed! + // Add 4th set of timestamped measurements + // 3rd track has no measurements so it should be removed + feature_tracker_->Update(timestamped_points_[3]); + EXPECT_EQ(feature_tracker_->size(), 2); + { + // Check first track which should have 4 points + EXPECT_TRACK_SIZE_EQ(0, 4); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + EXPECT_SAME_POINT(3, 0); + // Check second track which should have 3 points + EXPECT_TRACK_SIZE_EQ(1, 3); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + EXPECT_SAME_POINT(3, 1); + } + // Add 5th set of timestamped measurements + // 2nd track has no measurements so it should be removed + feature_tracker_->Update(timestamped_points_[4]); + EXPECT_EQ(feature_tracker_->size(), 1); + { + // Check first track which should have 5 points + EXPECT_TRACK_SIZE_EQ(0, 5); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + EXPECT_SAME_POINT(3, 0); + EXPECT_SAME_POINT(4, 0); + } + + // Add empty measurement + // Should remove all tracks + feature_tracker_->Update(vc::FeaturePoints()); + EXPECT_TRUE(feature_tracker_->empty()); +} + +TEST_F(FeatureTrackerTest, RemoveOldPoints) { + InitializeWithoutRemoval(); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); + + // Add first 3 measurements + feature_tracker_->Update(timestamped_points_[0]); + feature_tracker_->Update(timestamped_points_[1]); + feature_tracker_->Update(timestamped_points_[2]); + EXPECT_EQ(feature_tracker_->size(), 3); + { + // Check first track which should have 3 points + EXPECT_TRACK_SIZE_EQ(0, 3); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + // Check second track which should have 2 points + EXPECT_TRACK_SIZE_EQ(1, 2); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + // Check third track which should have one point + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } + // Remove first measurement occuring at t = 0 + { + const lc::Time oldest_allowed_time = 1; + feature_tracker_->RemoveOldPoints(oldest_allowed_time); + EXPECT_EQ(feature_tracker_->size(), 3); + // Check first track which should have 2 points + EXPECT_TRACK_SIZE_EQ(0, 2); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + // Check second track which should have 2 points + EXPECT_TRACK_SIZE_EQ(1, 2); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + // Check third track which should have one point + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } + // Remove second measurement occuring at t = 1 + { + const lc::Time oldest_allowed_time = 1.5; + feature_tracker_->RemoveOldPoints(oldest_allowed_time); + EXPECT_EQ(feature_tracker_->size(), 3); + // Check first track which should have 1 point + EXPECT_TRACK_SIZE_EQ(0, 1); + EXPECT_SAME_POINT(2, 0); + // Check second track which should have 1 point + EXPECT_TRACK_SIZE_EQ(1, 1); + EXPECT_SAME_POINT(2, 1); + // Check third track which should have one point + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } + // Remove third measurement occuring at t = 2 + { + const lc::Time oldest_allowed_time = 2.1; + feature_tracker_->RemoveOldPoints(oldest_allowed_time); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); + } +} + +TEST_F(FeatureTrackerTest, LengthOrdered) { + InitializeWithoutRemoval(); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); + + // Create points with increasingly more points for increasing id + std::vector<vc::FeaturePoints> timestamped_points(3); + for (int id = 0; id < 3; ++id) { + std::vector<vc::FeaturePoint> points; + for (int time = 0; time <= id; ++time) { + // Make image points different for different measurements + const vc::FeaturePoint p(id * 10 + time, id * 10 + time + 1, time + 1, id, time); + timestamped_points[time].emplace_back(p); + } + } + EXPECT_EQ(timestamped_points[0].size(), 3); + EXPECT_EQ(timestamped_points[1].size(), 2); + EXPECT_EQ(timestamped_points[2].size(), 1); + feature_tracker_->Update(timestamped_points[0]); + feature_tracker_->Update(timestamped_points[1]); + feature_tracker_->Update(timestamped_points[2]); + + EXPECT_EQ(feature_tracker_->size(), 3); + + // Check track lengths + EXPECT_TRACK_SIZE_EQ(0, 1); + EXPECT_TRACK_SIZE_EQ(1, 2); + EXPECT_TRACK_SIZE_EQ(2, 3); + + const auto length_ordered_tracks = feature_tracker_->FeatureTracksLengthOrdered(); + EXPECT_EQ(length_ordered_tracks[0].get().size(), 3); + EXPECT_EQ(length_ordered_tracks[0].get().id(), 2); + EXPECT_EQ(length_ordered_tracks[1].get().size(), 2); + EXPECT_EQ(length_ordered_tracks[1].get().id(), 1); + EXPECT_EQ(length_ordered_tracks[2].get().size(), 1); + EXPECT_EQ(length_ordered_tracks[2].get().id(), 0); +} +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/vision_common/test/test_feature_tracker.test b/localization/vision_common/test/test_feature_tracker.test new file mode 100644 index 0000000000..2476c46710 --- /dev/null +++ b/localization/vision_common/test/test_feature_tracker.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="vision_common" type="test_feature_tracker" test-name="test_feature_tracker" /> +</launch> diff --git a/localization/vision_common/test/test_spaced_feature_track.cc b/localization/vision_common/test/test_spaced_feature_track.cc new file mode 100644 index 0000000000..2e9941d781 --- /dev/null +++ b/localization/vision_common/test/test_spaced_feature_track.cc @@ -0,0 +1,207 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <vision_common/spaced_feature_track.h> +#include <vision_common/test_utilities.h> + +#include <gtest/gtest.h> + +namespace lc = localization_common; +namespace vc = vision_common; + +class SpacedFeatureTrackTest : public ::testing::Test { + public: + SpacedFeatureTrackTest() : track_(feature_track_id_) {} + + void SetUp() final { + for (int i = 0; i < num_points_; ++i) { + const lc::Time time = i; + const vc::FeaturePoint p(i, i + 1, i + 2, feature_track_id_, time); + track_.Add(time, p); + points_.emplace_back(p); + } + } + + void EXPECT_SAME_POINT(const vc::FeaturePoint& p_a, const vc::FeaturePoint& p_b) { + EXPECT_EQ(p_a.image_id, p_b.image_id); + EXPECT_EQ(p_a.feature_track_id, p_b.feature_track_id); + EXPECT_MATRIX_NEAR(p_a.image_point, p_b.image_point, 1e-6); + EXPECT_NEAR(p_a.timestamp, p_b.timestamp, 1e-6); + } + + void EXPECT_SAME_POINT(const vc::FeaturePoint& p_a, const int index) { EXPECT_SAME_POINT(p_a, points_[index]); } + const vc::FeatureId feature_track_id_ = 4; + const int num_points_ = 10; + vc::SpacedFeatureTrack track_; + std::vector<vc::FeaturePoint> points_; +}; + +TEST_F(SpacedFeatureTrackTest, LatestSpacedPoints) { + vc::SpacedFeatureTrack track(1); + // Test empty track + { + const auto points = track.LatestSpacedPoints(0); + EXPECT_EQ(points.size(), 0); + } + { + const auto points = track.LatestSpacedPoints(10); + EXPECT_EQ(points.size(), 0); + } + + // Test single point track + track.Add(points_[0].timestamp, points_[0]); + { + const auto points = track.LatestSpacedPoints(0); + ASSERT_EQ(points.size(), 1); + EXPECT_SAME_POINT(points[0], points_[0]); + } + { + const auto points = track.LatestSpacedPoints(5); + ASSERT_EQ(points.size(), 1); + EXPECT_SAME_POINT(points[0], points_[0]); + } + + // Test two point track + track.Add(points_[1].timestamp, points_[1]); + { + const auto points = track.LatestSpacedPoints(0); + ASSERT_EQ(points.size(), 2); + EXPECT_SAME_POINT(points[0], points_[0]); + EXPECT_SAME_POINT(points[1], points_[1]); + } + { + const auto points = track.LatestSpacedPoints(1); + ASSERT_EQ(points.size(), 1); + EXPECT_SAME_POINT(points[0], points_[1]); + } + { + const auto points = track.LatestSpacedPoints(4); + ASSERT_EQ(points.size(), 1); + EXPECT_SAME_POINT(points[0], points_[1]); + } + + // Test multi point track + { + const auto points = track_.LatestSpacedPoints(0); + ASSERT_EQ(points.size(), 10); + for (int i = 0; i < num_points_; ++i) EXPECT_SAME_POINT(points[i], points_[i]); + } + { + const auto points = track_.LatestSpacedPoints(1); + ASSERT_EQ(points.size(), 5); + // Expect p1, p3, p5, p7, p9 + for (int i = 0; i < 5; ++i) EXPECT_SAME_POINT(points[i], points_[2 * i + 1]); + } + { + const auto points = track_.LatestSpacedPoints(2); + ASSERT_EQ(points.size(), 4); + // Expect p0, p3, p6, p9 + for (int i = 0; i < 4; ++i) EXPECT_SAME_POINT(points[i], points_[3 * i]); + } + { + const auto points = track_.LatestSpacedPoints(3); + ASSERT_EQ(points.size(), 3); + // Expect p1, p5, p9 + EXPECT_SAME_POINT(points[0], points_[1]); + EXPECT_SAME_POINT(points[1], points_[5]); + EXPECT_SAME_POINT(points[2], points_[9]); + } +} + +TEST_F(SpacedFeatureTrackTest, MaxSpacing) { + vc::SpacedFeatureTrack track(1); + // Test empty track + { + EXPECT_EQ(track.MaxSpacing(10), 0); + EXPECT_EQ(track.MaxSpacing(0), 0); + } + // Test single point track + { + track.Add(points_[0].timestamp, points_[0]); + EXPECT_EQ(track.MaxSpacing(1), 0); + EXPECT_EQ(track.MaxSpacing(0), 0); + } + // Test two point track + { + track.Add(points_[1].timestamp, points_[1]); + EXPECT_EQ(track.MaxSpacing(0), 0); + EXPECT_EQ(track.MaxSpacing(1), 0); + EXPECT_EQ(track.MaxSpacing(2), 0); + EXPECT_EQ(track.MaxSpacing(10), 0); + } + + // Test multi point track + EXPECT_EQ(track_.MaxSpacing(0), 0); + EXPECT_EQ(track_.MaxSpacing(1), 0); + // Should result in p0, p9. Spacing is 8. + EXPECT_EQ(track_.MaxSpacing(2), 8); + // For 10 points with max 3 points, max spacing of 3 results in: + // p0, p4, p8 + // A spacing of 4 would result in: + // p0, p5, p10 + // but p10 doesn't exist, so this is invalid. + EXPECT_EQ(track_.MaxSpacing(3), 3); + // For 10 points with max 4 points, max spacing of 2 results in: + // p0, p3, p6, p9 + EXPECT_EQ(track_.MaxSpacing(4), 2); + // For 10 points with max 5 points, max spacing of 1 results in: + // p0, p2, p4, p6, p8 + EXPECT_EQ(track_.MaxSpacing(5), 1); + // For 10 points with max 6 points, max spacing of 0 results in: + // p0, p1, p2, p3, p4, p5, p6 + EXPECT_EQ(track_.MaxSpacing(6), 0); + EXPECT_EQ(track_.MaxSpacing(7), 0); + EXPECT_EQ(track_.MaxSpacing(8), 0); + EXPECT_EQ(track_.MaxSpacing(9), 0); + EXPECT_EQ(track_.MaxSpacing(10000), 0); +} + +TEST_F(SpacedFeatureTrackTest, SecondLatestTimestamp) { + vc::SpacedFeatureTrack track(1); + // Test empty track + { + const auto t = track.SecondLatestTimestamp(); + EXPECT_TRUE(t == boost::none); + } + // Test single point track + { + track.Add(points_[0].timestamp, points_[0]); + const auto t = track.SecondLatestTimestamp(); + EXPECT_TRUE(t == boost::none); + } + // Test two point track + { + track.Add(points_[1].timestamp, points_[1]); + const auto t = track.SecondLatestTimestamp(); + ASSERT_TRUE(t != boost::none); + EXPECT_NEAR(*t, points_[0].timestamp, 1e-6); + } + + // Test multi point track + const auto t = track_.SecondLatestTimestamp(); + ASSERT_TRUE(t != boost::none); + EXPECT_NEAR(*t, points_[num_points_ - 2].timestamp, 1e-6); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/vision_common/test/test_spaced_feature_track.test b/localization/vision_common/test/test_spaced_feature_track.test new file mode 100644 index 0000000000..e48d404952 --- /dev/null +++ b/localization/vision_common/test/test_spaced_feature_track.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="vision_common" type="test_spaced_feature_track" test-name="test_spaced_feature_track" /> +</launch> diff --git a/localization/vision_common/test/test_spaced_feature_tracker.cc b/localization/vision_common/test/test_spaced_feature_tracker.cc new file mode 100644 index 0000000000..b100d0dfc7 --- /dev/null +++ b/localization/vision_common/test/test_spaced_feature_tracker.cc @@ -0,0 +1,557 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <vision_common/spaced_feature_tracker.h> +#include <vision_common/test_utilities.h> + +#include <gtest/gtest.h> + +namespace lc = localization_common; +namespace vc = vision_common; + +class SpacedFeatureTrackerTest : public ::testing::Test { + public: + SpacedFeatureTrackerTest() {} + + // Adds increasing measurements then decreasing measurements. + // track 0: ..... + // track 1: ... + // track 2: . + // Here the x axis is the timestamp and the dot indicates whether a measurement + // occured for the track. + void SetUp() final { + const int num_timestamps = + num_timestamps_with_increasing_measurements_ + num_timestamps_with_decreasing_measurements_; + for (int time = 0; time < num_timestamps; ++time) { + std::vector<vc::FeaturePoint> points; + // Add fewer older measurements initially to tracks to help test removing old measurements. + if (time < num_timestamps_with_increasing_measurements_) { + for (int id = 0; id <= time; ++id) { + // Make image points different for different measurements + const vc::FeaturePoint p(id * 10 + time, id * 10 + time + 1, time + 1, id, time); + points_.emplace_back(p); + points.emplace_back(p); + } + timestamped_points_.emplace_back(points); + } else { + // Add fewer newer measurements later to tracks to help test removing undetected tracks. + for (int id = 0; id < num_timestamps - time; ++id) { + // Make image points different for different measurements + const vc::FeaturePoint p(id * 10 + time, id * 10 + time + 1, time + 1, id, time); + points_.emplace_back(p); + points.emplace_back(p); + } + timestamped_points_.emplace_back(points); + } + } + } + + void EXPECT_SAME_POINT(const vc::FeaturePoint& p_a, const vc::FeaturePoint& p_b) { + EXPECT_EQ(p_a.image_id, p_b.image_id); + EXPECT_EQ(p_a.feature_track_id, p_b.feature_track_id); + EXPECT_MATRIX_NEAR(p_a.image_point, p_b.image_point, 1e-6); + EXPECT_NEAR(p_a.timestamp, p_b.timestamp, 1e-6); + } + + void EXPECT_SAME_POINT(const int timestamp_index, const int measurement_index) { + const auto& added_point = timestamped_points_[timestamp_index][measurement_index]; + const auto id = added_point.feature_track_id; + const auto time = added_point.timestamp; + const auto& track = feature_tracker_->feature_tracks().at(id); + const auto& track_point = track.Get(time); + ASSERT_TRUE(track_point != boost::none); + EXPECT_SAME_POINT(added_point, track_point->value); + } + + void EXPECT_TRACK_SIZE_EQ(const int track_index, const int size) { + EXPECT_EQ(feature_tracker_->feature_tracks().at(track_index).size(), size); + } + + void InitializeWithRemoval(const int measurement_spacing = 2) { + vc::SpacedFeatureTrackerParams params; + params.measurement_spacing = measurement_spacing; + params.remove_undetected_feature_tracks = true; + feature_tracker_.reset(new vc::SpacedFeatureTracker(params)); + } + + void InitializeWithoutRemoval(const int measurement_spacing = 2) { + vc::SpacedFeatureTrackerParams params; + params.measurement_spacing = measurement_spacing; + params.remove_undetected_feature_tracks = false; + feature_tracker_.reset(new vc::SpacedFeatureTracker(params)); + } + + const int num_tracks_ = 3; + const int num_timestamps_with_increasing_measurements_ = 2; + const int num_timestamps_with_decreasing_measurements_ = 3; + vc::FeaturePoints points_; + std::vector<vc::FeaturePoints> timestamped_points_; + std::unique_ptr<vc::SpacedFeatureTracker> feature_tracker_; +}; + +TEST_F(SpacedFeatureTrackerTest, SizeEmptyClear) { + InitializeWithoutRemoval(); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); + + // Add measurements + feature_tracker_->Update(points_); + EXPECT_EQ(feature_tracker_->size(), num_tracks_); + EXPECT_FALSE(feature_tracker_->empty()); + + // Remove measurements + feature_tracker_->Clear(); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); +} + +// Without removal, tracks without detections should continue +// to exist. +TEST_F(SpacedFeatureTrackerTest, AddMeasurentsWithoutRemoval) { + InitializeWithoutRemoval(); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); + + // Add first set of timestamped measurements + feature_tracker_->Update(timestamped_points_[0]); + EXPECT_EQ(feature_tracker_->size(), 1); + { + // Only one point exists in first measurement + EXPECT_TRACK_SIZE_EQ(0, 1); + EXPECT_SAME_POINT(0, 0); + } + // Add second set of timestamped measurements + feature_tracker_->Update(timestamped_points_[1]); + EXPECT_EQ(feature_tracker_->size(), 2); + { + // Check first track which should have two points + EXPECT_TRACK_SIZE_EQ(0, 2); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + // Check second track which should have one point at second timestamp + EXPECT_TRACK_SIZE_EQ(1, 1); + EXPECT_SAME_POINT(1, 1); + } + // Add 3rd set of timestamped measurements + feature_tracker_->Update(timestamped_points_[2]); + EXPECT_EQ(feature_tracker_->size(), 3); + { + // Check first track which should have 3 points + EXPECT_TRACK_SIZE_EQ(0, 3); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + // Check second track which should have 2 points + EXPECT_TRACK_SIZE_EQ(1, 2); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + // Check third track which should have one point + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } + // Add 4th set of timestamped measurements + feature_tracker_->Update(timestamped_points_[3]); + EXPECT_EQ(feature_tracker_->size(), 3); + { + // Check first track which should have 4 points + EXPECT_TRACK_SIZE_EQ(0, 4); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + EXPECT_SAME_POINT(3, 0); + // Check second track which should have 3 points + EXPECT_TRACK_SIZE_EQ(1, 3); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + EXPECT_SAME_POINT(3, 1); + // Check third track which should have 1 point + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } + // Add 5th set of timestamped measurements + feature_tracker_->Update(timestamped_points_[4]); + EXPECT_EQ(feature_tracker_->size(), 3); + { + // Check first track which should have 5 points + EXPECT_TRACK_SIZE_EQ(0, 5); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + EXPECT_SAME_POINT(3, 0); + EXPECT_SAME_POINT(4, 0); + // Check second track which should have 3 points + EXPECT_TRACK_SIZE_EQ(1, 3); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + EXPECT_SAME_POINT(3, 1); + // Check third track which should have 1 points + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } + // Add empty measurement + // Tracks should be unaffected + feature_tracker_->Update(vc::FeaturePoints()); + EXPECT_EQ(feature_tracker_->size(), 3); + { + // Check first track which should have 5 points + EXPECT_TRACK_SIZE_EQ(0, 5); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + EXPECT_SAME_POINT(3, 0); + EXPECT_SAME_POINT(4, 0); + // Check second track which should have 3 points + EXPECT_TRACK_SIZE_EQ(1, 3); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + EXPECT_SAME_POINT(3, 1); + // Check third track which should have 1 points + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } +} + +// With removal tracks without detections should gradually be +// removed. +TEST_F(SpacedFeatureTrackerTest, AddMeasurentsWithRemoval) { + InitializeWithRemoval(); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); + + // Add first set of timestamped measurements + feature_tracker_->Update(timestamped_points_[0]); + EXPECT_EQ(feature_tracker_->size(), 1); + { + // Only one point exists in first measurement + EXPECT_TRACK_SIZE_EQ(0, 1); + EXPECT_SAME_POINT(0, 0); + } + // Add second set of timestamped measurements + feature_tracker_->Update(timestamped_points_[1]); + EXPECT_EQ(feature_tracker_->size(), 2); + { + // Check first track which should have two points + EXPECT_TRACK_SIZE_EQ(0, 2); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + // Check second track which should have one point at second timestamp + EXPECT_TRACK_SIZE_EQ(1, 1); + EXPECT_SAME_POINT(1, 1); + } + // Add 3rd set of timestamped measurements + feature_tracker_->Update(timestamped_points_[2]); + EXPECT_EQ(feature_tracker_->size(), 3); + { + // Check first track which should have 3 points + EXPECT_TRACK_SIZE_EQ(0, 3); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + // Check second track which should have 2 points + EXPECT_TRACK_SIZE_EQ(1, 2); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + // Check third track which should have one point + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } + + // Now since measurements taper off, tracks should start to be removed! + // Add 4th set of timestamped measurements + // 3rd track has no measurements so it should be removed + feature_tracker_->Update(timestamped_points_[3]); + EXPECT_EQ(feature_tracker_->size(), 2); + { + // Check first track which should have 4 points + EXPECT_TRACK_SIZE_EQ(0, 4); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + EXPECT_SAME_POINT(3, 0); + // Check second track which should have 3 points + EXPECT_TRACK_SIZE_EQ(1, 3); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + EXPECT_SAME_POINT(3, 1); + } + // Add 5th set of timestamped measurements + // 2nd track has no measurements so it should be removed + feature_tracker_->Update(timestamped_points_[4]); + EXPECT_EQ(feature_tracker_->size(), 1); + { + // Check first track which should have 5 points + EXPECT_TRACK_SIZE_EQ(0, 5); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + EXPECT_SAME_POINT(3, 0); + EXPECT_SAME_POINT(4, 0); + } + + // Add empty measurement + // Should remove all tracks + feature_tracker_->Update(vc::FeaturePoints()); + EXPECT_TRUE(feature_tracker_->empty()); +} + +TEST_F(SpacedFeatureTrackerTest, RemoveOldPoints) { + InitializeWithoutRemoval(); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); + + // Add first 3 measurements + feature_tracker_->Update(timestamped_points_[0]); + feature_tracker_->Update(timestamped_points_[1]); + feature_tracker_->Update(timestamped_points_[2]); + EXPECT_EQ(feature_tracker_->size(), 3); + { + // Check first track which should have 3 points + EXPECT_TRACK_SIZE_EQ(0, 3); + EXPECT_SAME_POINT(0, 0); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + // Check second track which should have 2 points + EXPECT_TRACK_SIZE_EQ(1, 2); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + // Check third track which should have one point + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } + // Remove first measurement occuring at t = 0 + { + const lc::Time oldest_allowed_time = 1; + feature_tracker_->RemoveOldPoints(oldest_allowed_time); + EXPECT_EQ(feature_tracker_->size(), 3); + // Check first track which should have 2 points + EXPECT_TRACK_SIZE_EQ(0, 2); + EXPECT_SAME_POINT(1, 0); + EXPECT_SAME_POINT(2, 0); + // Check second track which should have 2 points + EXPECT_TRACK_SIZE_EQ(1, 2); + EXPECT_SAME_POINT(1, 1); + EXPECT_SAME_POINT(2, 1); + // Check third track which should have one point + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } + // Remove second measurement occuring at t = 1 + { + const lc::Time oldest_allowed_time = 1.5; + feature_tracker_->RemoveOldPoints(oldest_allowed_time); + EXPECT_EQ(feature_tracker_->size(), 3); + // Check first track which should have 1 point + EXPECT_TRACK_SIZE_EQ(0, 1); + EXPECT_SAME_POINT(2, 0); + // Check second track which should have 1 point + EXPECT_TRACK_SIZE_EQ(1, 1); + EXPECT_SAME_POINT(2, 1); + // Check third track which should have one point + EXPECT_TRACK_SIZE_EQ(2, 1); + EXPECT_SAME_POINT(2, 2); + } + // Remove third measurement occuring at t = 2 + { + const lc::Time oldest_allowed_time = 2.1; + feature_tracker_->RemoveOldPoints(oldest_allowed_time); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); + } +} + +TEST_F(SpacedFeatureTrackerTest, LengthOrdered) { + InitializeWithoutRemoval(); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); + + // Create points with increasingly more points for increasing id + std::vector<vc::FeaturePoints> timestamped_points(3); + for (int id = 0; id < 3; ++id) { + std::vector<vc::FeaturePoint> points; + for (int time = 0; time <= id; ++time) { + // Make image points different for different measurements + const vc::FeaturePoint p(id * 10 + time, id * 10 + time + 1, time + 1, id, time); + timestamped_points[time].emplace_back(p); + } + } + EXPECT_EQ(timestamped_points[0].size(), 3); + EXPECT_EQ(timestamped_points[1].size(), 2); + EXPECT_EQ(timestamped_points[2].size(), 1); + feature_tracker_->Update(timestamped_points[0]); + feature_tracker_->Update(timestamped_points[1]); + feature_tracker_->Update(timestamped_points[2]); + + EXPECT_EQ(feature_tracker_->size(), 3); + + // Check track lengths + EXPECT_TRACK_SIZE_EQ(0, 1); + EXPECT_TRACK_SIZE_EQ(1, 2); + EXPECT_TRACK_SIZE_EQ(2, 3); + + const auto length_ordered_tracks = feature_tracker_->FeatureTracksLengthOrdered(); + EXPECT_EQ(length_ordered_tracks[0].get().size(), 3); + EXPECT_EQ(length_ordered_tracks[0].get().id(), 2); + EXPECT_EQ(length_ordered_tracks[1].get().size(), 2); + EXPECT_EQ(length_ordered_tracks[1].get().id(), 1); + EXPECT_EQ(length_ordered_tracks[2].get().size(), 1); + EXPECT_EQ(length_ordered_tracks[2].get().id(), 0); +} + +TEST_F(SpacedFeatureTrackerTest, SpacedTracksSpacing2) { + const int measurement_spacing = 2; + InitializeWithoutRemoval(measurement_spacing); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); + + // Add first set of timestamped measurements + feature_tracker_->Update(timestamped_points_[0]); + EXPECT_EQ(feature_tracker_->size(), 1); + { + const auto tracks = feature_tracker_->SpacedFeatureTracks(); + EXPECT_EQ(tracks.size(), 1); + const auto& track_0 = tracks[0]; + EXPECT_EQ(track_0.size(), 1); + EXPECT_SAME_POINT(track_0[0], timestamped_points_[0][0]); + } + // Add second set of timestamped measurements + feature_tracker_->Update(timestamped_points_[1]); + EXPECT_EQ(feature_tracker_->size(), 2); + { + const auto tracks = feature_tracker_->SpacedFeatureTracks(); + // Second track should be empty and skipped + EXPECT_EQ(tracks.size(), 1); + // First track should still only have one point + const auto& track_0 = tracks[0]; + EXPECT_EQ(track_0.size(), 1); + EXPECT_SAME_POINT(track_0[0], timestamped_points_[0][0]); + } + + // Add 3rd set of timestamped measurements + feature_tracker_->Update(timestamped_points_[2]); + EXPECT_EQ(feature_tracker_->size(), 3); + { + const auto tracks = feature_tracker_->SpacedFeatureTracks(); + // Second and third track should be empty and skipped + EXPECT_EQ(tracks.size(), 1); + // First track should still only have one point + const auto& track_0 = tracks[0]; + EXPECT_EQ(track_0.size(), 1); + EXPECT_SAME_POINT(track_0[0], timestamped_points_[0][0]); + } + + // Add 4th set of timestamped measurements + feature_tracker_->Update(timestamped_points_[3]); + EXPECT_EQ(feature_tracker_->size(), 3); + { + const auto tracks = feature_tracker_->SpacedFeatureTracks(); + // Second track should now have allowed measurements + EXPECT_EQ(tracks.size(), 2); + // First track should have two points + const auto& track_0 = tracks[0]; + EXPECT_EQ(track_0.size(), 2); + EXPECT_SAME_POINT(track_0[0], timestamped_points_[0][0]); + EXPECT_SAME_POINT(track_0[1], timestamped_points_[3][0]); + // Second track should have one point + const auto& track_1 = tracks[1]; + EXPECT_EQ(track_1.size(), 1); + EXPECT_SAME_POINT(track_1[0], timestamped_points_[3][1]); + } + // Add 5th set of timestamped measurements + feature_tracker_->Update(timestamped_points_[4]); + EXPECT_EQ(feature_tracker_->size(), 3); + { + const auto tracks = feature_tracker_->SpacedFeatureTracks(); + EXPECT_EQ(tracks.size(), 2); + // First track should still have two points + const auto& track_0 = tracks[0]; + EXPECT_EQ(track_0.size(), 2); + EXPECT_SAME_POINT(track_0[0], timestamped_points_[0][0]); + EXPECT_SAME_POINT(track_0[1], timestamped_points_[3][0]); + // Second track should still have one point + const auto& track_1 = tracks[1]; + EXPECT_EQ(track_1.size(), 1); + EXPECT_SAME_POINT(track_1[0], timestamped_points_[3][1]); + } +} + +TEST_F(SpacedFeatureTrackerTest, SpacedTracksSpacing0) { + const int measurement_spacing = 0; + InitializeWithoutRemoval(measurement_spacing); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); + + // Simple test with one track + const int num_times = 10; + std::vector<vc::FeaturePoints> timestamped_points(num_times); + std::vector<vc::FeaturePoint> points; + for (int time = 0; time < num_times; ++time) { + // Make image points different for different measurements + const vc::FeaturePoint p(time, time + 1, time + 1, 0, time); + timestamped_points[time].emplace_back(p); + } + + // All measurements should be available + for (int time = 0; time < num_times; ++time) { + feature_tracker_->Update(timestamped_points[time]); + EXPECT_EQ(feature_tracker_->size(), 1); + const auto tracks = feature_tracker_->SpacedFeatureTracks(); + EXPECT_EQ(tracks.size(), 1); + const auto& track_0 = tracks[0]; + EXPECT_EQ(track_0.size(), time + 1); + for (int i = 0; i < time; ++i) EXPECT_SAME_POINT(track_0[i], timestamped_points[i][0]); + } +} + +TEST_F(SpacedFeatureTrackerTest, SpacedTracksSpacing3) { + const int measurement_spacing = 3; + InitializeWithoutRemoval(measurement_spacing); + EXPECT_EQ(feature_tracker_->size(), 0); + EXPECT_TRUE(feature_tracker_->empty()); + + // Simple test with one track + const int num_times = 10; + std::vector<vc::FeaturePoints> timestamped_points(num_times); + std::vector<vc::FeaturePoint> points; + for (int time = 0; time < num_times; ++time) { + // Make image points different for different measurements + const vc::FeaturePoint p(time, time + 1, time + 1, 0, time); + timestamped_points[time].emplace_back(p); + } + + // Every 4th measurement should be available + for (int time = 0; time < num_times; ++time) { + feature_tracker_->Update(timestamped_points[time]); + EXPECT_EQ(feature_tracker_->size(), 1); + const auto tracks = feature_tracker_->SpacedFeatureTracks(); + EXPECT_EQ(tracks.size(), 1); + const auto& track_0 = tracks[0]; + const int expected_size = time / 4 + 1; + EXPECT_EQ(track_0.size(), expected_size); + for (int i = 0; i < expected_size; ++i) { + EXPECT_SAME_POINT(track_0[i], timestamped_points[4 * i][0]); + } + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/vision_common/test/test_spaced_feature_tracker.test b/localization/vision_common/test/test_spaced_feature_tracker.test new file mode 100644 index 0000000000..055a2904b9 --- /dev/null +++ b/localization/vision_common/test/test_spaced_feature_tracker.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="vision_common" type="test_spaced_feature_tracker" test-name="test_spaced_feature_tracker" /> +</launch> diff --git a/shared/ff_common/include/ff_common/ff_names.h b/shared/ff_common/include/ff_common/ff_names.h index 90cd4ee72a..3bb69a8979 100644 --- a/shared/ff_common/include/ff_common/ff_names.h +++ b/shared/ff_common/include/ff_common/ff_names.h @@ -116,17 +116,19 @@ #define NODE_PERCH_CTL "perch_ctl" #define NODE_EKF "ekf" #define NODE_GRAPH_LOC "graph_loc" -#define NODE_IMU_AUG "imu_aug" -#define NODE_IMU_BIAS_TESTER "imu_bias_tester" +#define NODE_GRAPH_VIO "graph_vio" +#define NODE_POSE_EXTR "pose_extr" #define NODE_SIM_LOC "sim_loc" #define NODE_FAM "fam" #define TOPIC_GRAPH_LOC "graph_loc/graph" +#define TOPIC_GRAPH_VIO "graph_vio/graph" #define TOPIC_GRAPH_LOC_STATE "graph_loc/state" +#define TOPIC_GRAPH_VIO_STATE "graph_vio/state" #define TOPIC_AR_TAG_POSE "ar_tag/pose" #define TOPIC_SPARSE_MAPPING_POSE "sparse_mapping/pose" -#define TOPIC_IMU_BIAS_TESTER_POSE "imu_bias_tester/pose" -#define TOPIC_IMU_BIAS_TESTER_VELOCITY "imu_bias_tester/velocity" +#define TOPIC_IMU_BIAS_EXTRAPOLATOR_POSE "imu_bias_extrapolator/pose" +#define TOPIC_IMU_BIAS_EXTRAPOLATOR_VELOCITY "imu_bias_extrapolator/velocity" #define ACTION_GNC_CTL_CONTROL "gnc/control" @@ -284,6 +286,7 @@ #define NODE_MAPPED_LANDMARKS "ml" #define NODE_VIVE_LOCALIZATION "vive_localization" +#define TOPIC_VIO_POSE "vio/pose" #define TOPIC_LOCALIZATION_POSE "loc/pose" #define TOPIC_LOCALIZATION_TWIST "loc/twist" #define TOPIC_LOCALIZATION_TRUTH "loc/truth/pose" diff --git a/shared/msg_conversions/include/msg_conversions/msg_conversions.h b/shared/msg_conversions/include/msg_conversions/msg_conversions.h index 25cbd8e5eb..39a63ed062 100644 --- a/shared/msg_conversions/include/msg_conversions/msg_conversions.h +++ b/shared/msg_conversions/include/msg_conversions/msg_conversions.h @@ -37,6 +37,12 @@ #include <string> +// Helper macro to automatically load a parameter for a basic type (int/bool/double). +// Assumes function is called as LOAD_PARAM(param.val, config, prefix) and assumes the format of +// param.y for the variable name (passing simply y will fail.) +#define LOAD_PARAM(val, config, prefix) \ + msg_conversions::Load((config), (val), std::string(#val).substr(std::string(#val).find_last_of(".") + 1), (prefix)) + namespace msg_conversions { Eigen::Vector3d ros_point_to_eigen_vector(const geometry_msgs::Point& p); @@ -88,12 +94,26 @@ bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Po bool SingleBoolTrue(const std::initializer_list<bool>& bools); // Alternative format for loading configs -Eigen::Isometry3d LoadEigenTransform(config_reader::ConfigReader& config, const std::string& transform_config_name); -double LoadDouble(config_reader::ConfigReader& config, const std::string& config_name); -float LoadFloat(config_reader::ConfigReader& config, const std::string& config_name); -int LoadInt(config_reader::ConfigReader& config, const std::string& config_name); -bool LoadBool(config_reader::ConfigReader& config, const std::string& config_name); -std::string LoadString(config_reader::ConfigReader& config, const std::string& config_name); +Eigen::Isometry3d LoadEigenTransform(config_reader::ConfigReader& config, const std::string& transform_config_name, + const std::string& prefix = ""); +double LoadDouble(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix = ""); +float LoadFloat(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix = ""); +int LoadInt(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix = ""); +bool LoadBool(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix = ""); +std::string LoadString(config_reader::ConfigReader& config, const std::string& config_name, + const std::string& prefix = ""); +// Overloads for parameter loading that enable LOAD_PARAM macro to work +void Load(config_reader::ConfigReader& config, float& val, const std::string& config_name, + const std::string& prefix = ""); +void Load(config_reader::ConfigReader& config, double& val, const std::string& config_name, + const std::string& prefix = ""); +void Load(config_reader::ConfigReader& config, int& val, const std::string& config_name, + const std::string& prefix = ""); +void Load(config_reader::ConfigReader& config, bool& val, const std::string& config_name, + const std::string& prefix = ""); +void Load(config_reader::ConfigReader& config, std::string& val, const std::string& config_name, + const std::string& prefix = ""); + void EigenPoseToMsg(const Eigen::Isometry3d& pose, geometry_msgs::Pose& msg_pose); void EigenPoseToMsg(const Eigen::Isometry3d& pose, geometry_msgs::Transform& msg_transform); void VariancesToCovDiag(const Eigen::Vector3d& variances, float* const cov_diag); @@ -139,13 +159,24 @@ void RotationToMsg(const RotationType& rotation, MsgRotationType& msg_rotation) msg_rotation.z = rotation.z(); } -template <typename ArrayType> -void EigenCovarianceToMsg(const Eigen::Matrix<double, 6, 6>& covariance, ArrayType& covariance_array) { - for (int i = 0; i < 6; ++i) { - for (int j = 0; j < 6; ++j) { - covariance_array[i*6 + j] = covariance(i, j); +template <typename ArrayType, int Dim> +void EigenCovarianceToMsg(const Eigen::Matrix<double, Dim, Dim>& covariance, ArrayType& covariance_array) { + for (int i = 0; i < Dim; ++i) { + for (int j = 0; j < Dim; ++j) { + covariance_array[i*Dim + j] = covariance(i, j); + } + } +} + +template <int Dim, typename ArrayType> +Eigen::Matrix<double, Dim, Dim> EigenCovarianceFromMsg(const ArrayType& covariance_array) { + Eigen::Matrix<double, Dim, Dim> covariance; + for (int i = 0; i < Dim; ++i) { + for (int j = 0; j < Dim; ++j) { + covariance(i, j) = covariance_array[i*Dim + j]; } } + return covariance; } } // namespace msg_conversions diff --git a/shared/msg_conversions/src/msg_conversions.cc b/shared/msg_conversions/src/msg_conversions.cc index dea94e3e2d..1e1e8f4e2f 100644 --- a/shared/msg_conversions/src/msg_conversions.cc +++ b/shared/msg_conversions/src/msg_conversions.cc @@ -312,47 +312,69 @@ tf2::Transform ros_pose_to_tf2_transform(const geometry_msgs::Pose& p) { return transform; } -Eigen::Isometry3d LoadEigenTransform(config_reader::ConfigReader& config, const std::string& transform_config_name) { +Eigen::Isometry3d LoadEigenTransform(config_reader::ConfigReader& config, const std::string& transform_config_name, + const std::string& prefix) { Eigen::Vector3d body_t_sensor; Eigen::Quaterniond body_Q_sensor; - if (!msg_conversions::config_read_transform(&config, transform_config_name.c_str(), &body_t_sensor, &body_Q_sensor)) - ROS_FATAL_STREAM("Unspecified transform config: " << transform_config_name); + if (!msg_conversions::config_read_transform(&config, (prefix + transform_config_name).c_str(), &body_t_sensor, + &body_Q_sensor)) + ROS_FATAL_STREAM("Unspecified transform config: " << prefix + transform_config_name); Eigen::Isometry3d body_T_sensor = Eigen::Isometry3d::Identity(); body_T_sensor.translation() = body_t_sensor; body_T_sensor.linear() = body_Q_sensor.toRotationMatrix(); return body_T_sensor; } -float LoadFloat(config_reader::ConfigReader& config, const std::string& config_name) { +float LoadFloat(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix) { float val; - if (!config.GetReal(config_name.c_str(), &val)) ROS_FATAL_STREAM("Failed to load " << config_name); + if (!config.GetReal((prefix + config_name).c_str(), &val)) + ROS_FATAL_STREAM("Failed to load " << prefix + config_name); return val; } -double LoadDouble(config_reader::ConfigReader& config, const std::string& config_name) { +double LoadDouble(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix) { double val; - if (!config.GetReal(config_name.c_str(), &val)) ROS_FATAL_STREAM("Failed to load " << config_name); + if (!config.GetReal((prefix + config_name).c_str(), &val)) + ROS_FATAL_STREAM("Failed to load " << prefix + config_name); return val; } -int LoadInt(config_reader::ConfigReader& config, const std::string& config_name) { +int LoadInt(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix) { int val; - if (!config.GetInt(config_name.c_str(), &val)) ROS_FATAL_STREAM("Failed to load " << config_name); + if (!config.GetInt((prefix + config_name).c_str(), &val)) ROS_FATAL_STREAM("Failed to load " << prefix + config_name); return val; } -bool LoadBool(config_reader::ConfigReader& config, const std::string& config_name) { +bool LoadBool(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix) { bool val; - if (!config.GetBool(config_name.c_str(), &val)) ROS_FATAL_STREAM("Failed to load " << config_name); + if (!config.GetBool((prefix + config_name).c_str(), &val)) + ROS_FATAL_STREAM("Failed to load " << prefix + config_name); return val; } -std::string LoadString(config_reader::ConfigReader& config, const std::string& config_name) { +std::string LoadString(config_reader::ConfigReader& config, const std::string& config_name, const std::string& prefix) { std::string val; - if (!config.GetStr(config_name.c_str(), &val)) ROS_FATAL_STREAM("Failed to load " << config_name); + if (!config.GetStr((prefix + config_name).c_str(), &val)) ROS_FATAL_STREAM("Failed to load " << prefix + config_name); return val; } +void Load(config_reader::ConfigReader& config, float& val, const std::string& config_name, const std::string& prefix) { + val = LoadFloat(config, config_name, prefix); +} +void Load(config_reader::ConfigReader& config, double& val, const std::string& config_name, const std::string& prefix) { + val = LoadDouble(config, config_name, prefix); +} +void Load(config_reader::ConfigReader& config, int& val, const std::string& config_name, const std::string& prefix) { + val = LoadInt(config, config_name, prefix); +} +void Load(config_reader::ConfigReader& config, bool& val, const std::string& config_name, const std::string& prefix) { + val = LoadBool(config, config_name, prefix); +} +void Load(config_reader::ConfigReader& config, std::string& val, const std::string& config_name, + const std::string& prefix) { + val = LoadString(config, config_name, prefix); +} + void EigenPoseToMsg(const Eigen::Isometry3d& pose, geometry_msgs::Pose& msg_pose) { RotationToMsg(Eigen::Quaterniond(pose.linear()), msg_pose.orientation); VectorToMsg(pose.translation(), msg_pose.position); diff --git a/tools/bag_processing/scripts/check_bag_for_gaps.py b/tools/bag_processing/scripts/check_bag_for_gaps.py index 67f81e2952..7f42d98635 100755 --- a/tools/bag_processing/scripts/check_bag_for_gaps.py +++ b/tools/bag_processing/scripts/check_bag_for_gaps.py @@ -53,6 +53,6 @@ print(("Bag file " + args.bagfile + " does not exist.")) sys.exit() - utilities.get_topic_rates( + utilities.utilities.get_topic_rates( args.bagfile, args.topic, args.max_time_diff, not args.use_receive_time, True ) diff --git a/tools/calibration/tools/create_undistorted_images.cc b/tools/calibration/tools/create_undistorted_images.cc index a70630e751..62dd4fb37d 100644 --- a/tools/calibration/tools/create_undistorted_images.cc +++ b/tools/calibration/tools/create_undistorted_images.cc @@ -86,7 +86,7 @@ int main(int argc, char** argv) { int ff_argc = 1; ff_common::InitFreeFlyerApplication(&ff_argc, &argv); - lc::SetEnvironmentConfigs(config_path, world, robot_config_file); + lc::SetEnvironmentConfigs(world, robot_config_file); config_reader::ConfigReader config; config.AddFile("geometry.config"); config.AddFile("cameras.config"); diff --git a/tools/calibration/tools/run_camera_target_based_intrinsics_calibrator.cc b/tools/calibration/tools/run_camera_target_based_intrinsics_calibrator.cc index af46925dd2..ec7f9cc68f 100644 --- a/tools/calibration/tools/run_camera_target_based_intrinsics_calibrator.cc +++ b/tools/calibration/tools/run_camera_target_based_intrinsics_calibrator.cc @@ -160,7 +160,7 @@ int main(int argc, char** argv) { int ff_argc = 1; ff_common::InitFreeFlyerApplication(&ff_argc, &argv); - lc::SetEnvironmentConfigs(config_path, world, robot_config_file); + lc::SetEnvironmentConfigs(world, robot_config_file); config_reader::ConfigReader config; config.AddFile("geometry.config"); config.AddFile("cameras.config"); diff --git a/tools/imu_bias_tester/CMakeLists.txt b/tools/imu_bias_tester/CMakeLists.txt deleted file mode 100644 index 1dab1ca189..0000000000 --- a/tools/imu_bias_tester/CMakeLists.txt +++ /dev/null @@ -1,92 +0,0 @@ -#Copyright(c) 2017, United States Government, as represented by the -#Administrator of the National Aeronautics and Space Administration. -# -#All rights reserved. -# -#The Astrobee platform is licensed under the Apache License, Version 2.0 -#(the "License"); you may not use this file except in compliance with the -#License.You may obtain a copy of the License at -# -#http: // www.apache.org/licenses/LICENSE-2.0 -# -#Unless required by applicable law or agreed to in writing, software -#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the -#License for the specific language governing permissions and limitations -#under the License. - -cmake_minimum_required(VERSION 3.0) -project(imu_bias_tester) - -if (USE_ROS) - ## Compile as C++14, supported in ROS Kinetic and newer - add_compile_options(-std=c++14) - - ## Find catkin macros and libraries - find_package(catkin2 REQUIRED COMPONENTS - roscpp - nodelet - ff_util - ff_msgs - config_reader - imu_integration - ) - - catkin_package( - LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} - INCLUDE_DIRS include ${GTSAM_INCLUDE_DIR} ${GLOG_INCLUDE_DIRS} - CATKIN_DEPENDS roscpp ff_util ff_msgs config_reader imu_integration - ) - - ########### - ## Build ## - ########### - # Specify additional locations of header files - include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ) - - # Declare C++ libraries - add_library(${PROJECT_NAME} - src/imu_bias_tester.cc - src/imu_bias_tester_nodelet.cc - src/imu_bias_tester_wrapper.cc - ) - add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - - - ############# - ## Install ## - ############# - - # Mark libraries for installation - install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} - ) - - # Mark nodelet_plugin for installation - install(FILES nodelet_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - ) - - # Mark cpp header files for installation - install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE - ) - - # Mark launch files for installation - install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - PATTERN ".svn" EXCLUDE) - -else (USE_ROS) - find_package(catkin REQUIRED COMPONENTS) - catkin_package() -endif (USE_ROS) diff --git a/tools/imu_bias_tester/include/imu_bias_tester/imu_bias_tester.h b/tools/imu_bias_tester/include/imu_bias_tester/imu_bias_tester.h deleted file mode 100644 index 3027b6d4de..0000000000 --- a/tools/imu_bias_tester/include/imu_bias_tester/imu_bias_tester.h +++ /dev/null @@ -1,52 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef IMU_BIAS_TESTER_IMU_BIAS_TESTER_H_ -#define IMU_BIAS_TESTER_IMU_BIAS_TESTER_H_ - -#include <imu_integration/imu_integrator.h> -#include <imu_integration/imu_integrator_params.h> -#include <localization_common/combined_nav_state.h> - -#include <map> -#include <vector> - -namespace imu_bias_tester { -class ImuBiasTester : public imu_integration::ImuIntegrator { - public: - explicit ImuBiasTester(const imu_integration::ImuIntegratorParams& params); - - std::vector<localization_common::CombinedNavState> PimPredict( - const localization_common::CombinedNavState& combined_nav_state); - - private: - // Integrates imu measurements between lower and upper bound nav states using lower bound's biases - void IntegrateAndRemoveMeasurements(const localization_common::CombinedNavState& lower_bound_state, - const localization_common::Time& upper_bound_timestamp, - std::vector<localization_common::CombinedNavState>& predicted_states); - void RemoveOldCombinedNavStatesIfNeeded(); - bool Initialized(); - void Initialize(const localization_common::CombinedNavState& combined_nav_state); - - std::map<localization_common::Time, localization_common::CombinedNavState> combined_nav_states_; - bool initialized_; - boost::optional<localization_common::CombinedNavState> last_predicted_combined_nav_state_; -}; -} // namespace imu_bias_tester - -#endif // IMU_BIAS_TESTER_IMU_BIAS_TESTER_H_ diff --git a/tools/imu_bias_tester/include/imu_bias_tester/imu_bias_tester_nodelet.h b/tools/imu_bias_tester/include/imu_bias_tester/imu_bias_tester_nodelet.h deleted file mode 100644 index 1da3ae4058..0000000000 --- a/tools/imu_bias_tester/include/imu_bias_tester/imu_bias_tester_nodelet.h +++ /dev/null @@ -1,56 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#ifndef IMU_BIAS_TESTER_IMU_BIAS_TESTER_NODELET_H_ -#define IMU_BIAS_TESTER_IMU_BIAS_TESTER_NODELET_H_ - -#include <ff_msgs/GraphState.h> -#include <ff_util/ff_nodelet.h> -#include <imu_bias_tester/imu_bias_tester_wrapper.h> - -#include <ros/node_handle.h> -#include <ros/publisher.h> -#include <ros/subscriber.h> -#include <sensor_msgs/Imu.h> - -#include <boost/optional.hpp> - -namespace imu_bias_tester { -class ImuBiasTesterNodelet : public ff_util::FreeFlyerNodelet { - public: - ImuBiasTesterNodelet(); - - private: - void Initialize(ros::NodeHandle* nh) final; - - void SubscribeAndAdvertise(ros::NodeHandle* nh); - - void ImuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg); - - void LocalizationStateCallback(const ff_msgs::GraphState::ConstPtr& loc_msg); - - void Run(); - - imu_bias_tester::ImuBiasTesterWrapper imu_bias_tester_wrapper_; - ros::NodeHandle imu_nh_, loc_nh_; - ros::CallbackQueue imu_queue_, loc_queue_; - ros::Subscriber imu_sub_, state_sub_; - ros::Publisher pose_pub_, velocity_pub_; -}; -} // namespace imu_bias_tester - -#endif // IMU_BIAS_TESTER_IMU_BIAS_TESTER_NODELET_H_ diff --git a/tools/imu_bias_tester/include/imu_bias_tester/imu_bias_tester_wrapper.h b/tools/imu_bias_tester/include/imu_bias_tester/imu_bias_tester_wrapper.h deleted file mode 100644 index f314da70cb..0000000000 --- a/tools/imu_bias_tester/include/imu_bias_tester/imu_bias_tester_wrapper.h +++ /dev/null @@ -1,44 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ -#ifndef IMU_BIAS_TESTER_IMU_BIAS_TESTER_WRAPPER_H_ -#define IMU_BIAS_TESTER_IMU_BIAS_TESTER_WRAPPER_H_ - -#include <ff_msgs/GraphState.h> -#include <imu_bias_tester/imu_bias_tester.h> -#include <localization_common/combined_nav_state.h> - -#include <geometry_msgs/PoseStamped.h> -#include <sensor_msgs/Imu.h> - -#include <memory> -#include <string> -#include <vector> - -namespace imu_bias_tester { -class ImuBiasTesterWrapper { - public: - explicit ImuBiasTesterWrapper(const std::string& graph_config_path_prefix = ""); - // Returns predicted nav states - std::vector<localization_common::CombinedNavState> LocalizationStateCallback(const ff_msgs::GraphState& loc_msg); - void ImuCallback(const sensor_msgs::Imu& imu_msg); - - private: - std::unique_ptr<ImuBiasTester> imu_bias_tester_; -}; -} // namespace imu_bias_tester -#endif // IMU_BIAS_TESTER_IMU_BIAS_TESTER_WRAPPER_H_ diff --git a/tools/imu_bias_tester/launch/imu_bias_tester.launch b/tools/imu_bias_tester/launch/imu_bias_tester.launch deleted file mode 100644 index fd50c6a9f8..0000000000 --- a/tools/imu_bias_tester/launch/imu_bias_tester.launch +++ /dev/null @@ -1,27 +0,0 @@ -<!-- Copyright (c) 2017, United States Government, as represented by the --> -<!-- Administrator of the National Aeronautics and Space Administration. --> -<!-- --> -<!-- All rights reserved. --> -<!-- --> -<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> -<!-- (the "License"); you may not use this file except in compliance with --> -<!-- the License. You may obtain a copy of the License at --> -<!-- --> -<!-- http://www.apache.org/licenses/LICENSE-2.0 --> -<!-- --> -<!-- Unless required by applicable law or agreed to in writing, software --> -<!-- distributed under the License is distributed on an "AS IS" BASIS, --> -<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> -<!-- implied. See the License for the specific language governing --> -<!-- permissions and limitations under the License. --> -<launch> - <arg name="name" default="imu_bias_tester" /> - <arg name="manager" default="" /> - <arg name="terminal" default="" /> - <include file="$(find ff_util)/launch/ff_nodelet.launch"> - <arg name="name" value="$(arg name)" /> - <arg name="manager" value="$(arg manager)" /> - <arg name="terminal" value="$(arg terminal)" /> - <arg name="class" value="imu_bias_tester/ImuBiasTesterNodelet" /> - </include> -</launch> diff --git a/tools/imu_bias_tester/nodelet_plugins.xml b/tools/imu_bias_tester/nodelet_plugins.xml deleted file mode 100644 index d507253f52..0000000000 --- a/tools/imu_bias_tester/nodelet_plugins.xml +++ /dev/null @@ -1,11 +0,0 @@ -<library path="lib/libimu_bias_tester"> - <class name="imu_bias_tester/ImuBiasTesterNodelet" - type="imu_bias_tester::ImuBiasTesterNodelet" - base_class_type="nodelet::Nodelet"> - <description> - This nodelet wraps the imu_bias_tester - </description> - </class> -</library> - - diff --git a/tools/imu_bias_tester/package.xml b/tools/imu_bias_tester/package.xml deleted file mode 100644 index d12a9ccdd6..0000000000 --- a/tools/imu_bias_tester/package.xml +++ /dev/null @@ -1,34 +0,0 @@ -<package> - <name>imu_bias_tester</name> - <version>1.0.0</version> - <description> - Imu bias tester package - </description> - <license> - Apache License, Version 2.0 - </license> - <author email="astrobee-fsw@nx.arc.nasa.gov"> - Astrobee Flight Software - </author> - <maintainer email="astrobee-fsw@nx.arc.nasa.gov"> - Astrobee Flight Software - </maintainer> - <buildtool_depend>catkin</buildtool_depend> - <buildtool_depend>catkin</buildtool_depend> - <build_depend>roscpp</build_depend> - <build_depend>nodelet</build_depend> - <build_depend>ff_util</build_depend> - <build_depend>ff_msgs</build_depend> - <build_depend>config_reader</build_depend> - <build_depend>imu_integration</build_depend> - <run_depend>roscpp</run_depend> - <run_depend>nodelet</run_depend> - <run_depend>ff_util</run_depend> - <run_depend>ff_msgs</run_depend> - <run_depend>config_reader</run_depend> - <run_depend>imu_integration</run_depend> - <export> - <nodelet plugin="${prefix}/nodelet_plugins.xml" /> - </export> - -</package> diff --git a/tools/imu_bias_tester/readme.md b/tools/imu_bias_tester/readme.md deleted file mode 100644 index 85178796d0..0000000000 --- a/tools/imu_bias_tester/readme.md +++ /dev/null @@ -1,21 +0,0 @@ -\page imubiastester IMU Bias Tester - -## ImuBiasTester -The ImuBiasTester integrates IMU measurements using the latest estimated IMU biases from graph_state messages. -The integrated measurements are published as pose messages. -These poses can then be plotted against localization groundtruth to measure the accuracy of the estimated IMU biases. - -# Usage -Launch the nodelet online or while playing a recored bagfile using -`roslaunch imu_bias_tester imu_bias_tester.launch` -to generated IMU bias poses. Ensure that localization estimates and IMU messages are -available as these are required for IMU bias pose estimation. -Record the results to a bagfile and plot using the `plot_results.py` script in -localization_analysis. See the localization_analysis readme for more details on plotting results. - -# Inputs -* `/hw/imu` -* `graph_loc/state` - -# Outputs -* `imu_bias_tester/pose` diff --git a/tools/imu_bias_tester/src/imu_bias_tester.cc b/tools/imu_bias_tester/src/imu_bias_tester.cc deleted file mode 100644 index b8dfbf85dc..0000000000 --- a/tools/imu_bias_tester/src/imu_bias_tester.cc +++ /dev/null @@ -1,82 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <imu_bias_tester/imu_bias_tester.h> -#include <imu_integration/utilities.h> - -#include <glog/logging.h> - -namespace imu_bias_tester { -namespace ii = imu_integration; -namespace lc = localization_common; -namespace lm = localization_measurements; -ImuBiasTester::ImuBiasTester(const ii::ImuIntegratorParams& params) : ii::ImuIntegrator(params), initialized_(false) {} - -std::vector<lc::CombinedNavState> ImuBiasTester::PimPredict(const lc::CombinedNavState& combined_nav_state) { - combined_nav_states_.emplace(combined_nav_state.timestamp(), combined_nav_state); - if (Empty()) return {}; - RemoveOldCombinedNavStatesIfNeeded(); - std::vector<lc::CombinedNavState> predicted_states; - // Wait until imu measurements are newer than upper bound nav state to avoid prematurely integrating - // measurements with an old nav state bias - while (combined_nav_states_.size() >= 2) { - auto lower_bound_state_it = combined_nav_states_.begin(); - const auto upper_bound_timestamp = std::next(lower_bound_state_it)->first; - if (*LatestTime() < upper_bound_timestamp) break; - if (!Initialized()) Initialize(lower_bound_state_it->second); - IntegrateAndRemoveMeasurements(lower_bound_state_it->second, upper_bound_timestamp, predicted_states); - combined_nav_states_.erase(lower_bound_state_it); - } - return predicted_states; -} - -void ImuBiasTester::IntegrateAndRemoveMeasurements(const lc::CombinedNavState& lower_bound_state, - const lc::Time& upper_bound_timestamp, - std::vector<lc::CombinedNavState>& predicted_states) { - RemoveOldMeasurements(lower_bound_state.timestamp()); - auto pim = ii::Pim(lower_bound_state.bias(), pim_params()); - // Reset pim after each integration since pim uses beginning orientation and velocity for - // gravity integration and initial velocity integration. - for (auto measurement_it = measurements().upper_bound(last_predicted_combined_nav_state_->timestamp()); - measurement_it != measurements().cend() && measurement_it->first < upper_bound_timestamp; ++measurement_it) { - pim.resetIntegrationAndSetBias(lower_bound_state.bias()); - auto time = last_predicted_combined_nav_state_->timestamp(); - ii::AddMeasurement(measurement_it->second, time, pim); - last_predicted_combined_nav_state_ = ii::PimPredict(*last_predicted_combined_nav_state_, pim); - predicted_states.emplace_back(*last_predicted_combined_nav_state_); - } - RemoveOldMeasurements(upper_bound_timestamp); -} - -bool ImuBiasTester::Initialized() { return initialized_; } -void ImuBiasTester::Initialize(const lc::CombinedNavState& combined_nav_state) { - last_predicted_combined_nav_state_ = combined_nav_state; - initialized_ = true; -} - -void ImuBiasTester::RemoveOldCombinedNavStatesIfNeeded() { - if (Empty()) return; - const auto oldest_imu_time = *OldestTime(); - auto oldest_state_it = combined_nav_states_.begin(); - // Remove nav states older than oldest imu measurement as long as they aren't the lower bound - while (combined_nav_states_.size() >= 2 && oldest_state_it->first < oldest_imu_time && - std::next(oldest_state_it)->first < oldest_imu_time) { - oldest_state_it = combined_nav_states_.erase(oldest_state_it); - } -} -} // namespace imu_bias_tester diff --git a/tools/imu_bias_tester/src/imu_bias_tester_wrapper.cc b/tools/imu_bias_tester/src/imu_bias_tester_wrapper.cc deleted file mode 100644 index c3f95caf68..0000000000 --- a/tools/imu_bias_tester/src/imu_bias_tester_wrapper.cc +++ /dev/null @@ -1,54 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <config_reader/config_reader.h> -#include <imu_bias_tester/imu_bias_tester_wrapper.h> -#include <imu_integration/utilities.h> -#include <localization_common/utilities.h> -#include <localization_measurements/imu_measurement.h> - -#include <localization_common/logger.h> - -namespace imu_bias_tester { -namespace ii = imu_integration; -namespace lc = localization_common; -namespace lm = localization_measurements; -ImuBiasTesterWrapper::ImuBiasTesterWrapper(const std::string& graph_config_path_prefix) { - config_reader::ConfigReader config; - config.AddFile("transforms.config"); - config.AddFile("geometry.config"); - lc::LoadGraphLocalizerConfig(config, graph_config_path_prefix); - - if (!config.ReadFiles()) { - LogFatal("Failed to read config files."); - } - - imu_integration::ImuIntegratorParams params; - ii::LoadImuIntegratorParams(config, params); - imu_bias_tester_.reset(new ImuBiasTester(params)); -} - -std::vector<lc::CombinedNavState> ImuBiasTesterWrapper::LocalizationStateCallback(const ff_msgs::GraphState& loc_msg) { - const auto combined_nav_state = lc::CombinedNavStateFromMsg(loc_msg); - return imu_bias_tester_->PimPredict(combined_nav_state); -} - -void ImuBiasTesterWrapper::ImuCallback(const sensor_msgs::Imu& imu_msg) { - imu_bias_tester_->BufferImuMeasurement(lm::ImuMeasurement(imu_msg)); -} -} // namespace imu_bias_tester diff --git a/tools/localization_analysis/CMakeLists.txt b/tools/localization_analysis/CMakeLists.txt index 653c8ec448..e07979f6f0 100644 --- a/tools/localization_analysis/CMakeLists.txt +++ b/tools/localization_analysis/CMakeLists.txt @@ -33,10 +33,14 @@ if (USE_ROS) ff_msgs ff_common graph_localizer - imu_augmentor - imu_bias_tester + graph_vio lk_optical_flow localization_node + msg_conversions + parameter_reader + ros_graph_localizer + ros_graph_vio + ros_pose_extrapolator ) # Find GTSAM @@ -45,7 +49,8 @@ if (USE_ROS) catkin_package( LIBRARIES ${PROJECT_NAME} ${OpenCV_LIBRARIES} INCLUDE_DIRS include - CATKIN_DEPENDS roscpp rosbag camera depth_odometry ff_util ff_msgs ff_common graph_localizer imu_augmentor imu_bias_tester lk_optical_flow localization_node + CATKIN_DEPENDS roscpp rosbag camera depth_odometry ff_util ff_msgs ff_common graph_localizer + msg_conversions lk_optical_flow localization_node parameter_reader ros_graph_localizer ros_graph_vio ros_pose_extrapolator ) ########### @@ -60,12 +65,14 @@ if (USE_ROS) # Declare C++ libraries add_library(${PROJECT_NAME} - src/bag_imu_filterer.cc + #src/bag_imu_filterer.cc src/depth_odometry_adder.cc - src/graph_bag.cc + src/feature_track_image_adder.cc src/graph_localizer_simulator.cc - src/imu_bias_tester_adder.cc + src/graph_vio_simulator.cc + src/imu_bias_extrapolator.cc src/live_measurement_simulator.cc + src/offline_replay.cc src/map_matcher.cc src/parameter_reader.cc src/sparse_mapping_pose_adder.cc @@ -81,27 +88,27 @@ if (USE_ROS) ${PROJECT_NAME} gflags gtsam ${catkin_LIBRARIES}) ## Declare a C++ executable: run_bag_imu_filterer - add_executable(run_bag_imu_filterer tools/run_bag_imu_filterer.cc) - add_dependencies(run_bag_imu_filterer ${catkin_EXPORTED_TARGETS}) - target_link_libraries(run_bag_imu_filterer - ${PROJECT_NAME} gflags gtsam ${catkin_LIBRARIES}) + #add_executable(run_bag_imu_filterer tools/run_bag_imu_filterer.cc) + #add_dependencies(run_bag_imu_filterer ${catkin_EXPORTED_TARGETS}) + #target_link_libraries(run_bag_imu_filterer + # ${PROJECT_NAME} gflags gtsam ${catkin_LIBRARIES}) - ## Declare a C++ executable: run_bag_imu_filterer + ## Declare a C++ executable: run_depth_odometry_adder add_executable(run_depth_odometry_adder tools/run_depth_odometry_adder.cc) add_dependencies(run_depth_odometry_adder ${catkin_EXPORTED_TARGETS}) target_link_libraries(run_depth_odometry_adder ${PROJECT_NAME} ${catkin_LIBRARIES}) - ## Declare a C++ executable: run_graph_bag - add_executable(run_graph_bag tools/run_graph_bag.cc) - add_dependencies(run_graph_bag ${catkin_EXPORTED_TARGETS}) - target_link_libraries(run_graph_bag + ## Declare a C++ executable: run_offline_replay + add_executable(run_offline_replay tools/run_offline_replay.cc) + add_dependencies(run_offline_replay ${catkin_EXPORTED_TARGETS}) + target_link_libraries(run_offline_replay ${PROJECT_NAME} gflags gtsam ${catkin_LIBRARIES}) - ## Declare a C++ executable: run_imu_bias_tester_adder - add_executable(run_imu_bias_tester_adder tools/run_imu_bias_tester_adder.cc) - add_dependencies(run_imu_bias_tester_adder ${catkin_EXPORTED_TARGETS}) - target_link_libraries(run_imu_bias_tester_adder + ## Declare a C++ executable: run_imu_bias_extrapolator + add_executable(run_imu_bias_extrapolator tools/run_imu_bias_extrapolator.cc) + add_dependencies(run_imu_bias_extrapolator ${catkin_EXPORTED_TARGETS}) + target_link_libraries(run_imu_bias_extrapolator ${PROJECT_NAME} gflags gtsam ${catkin_LIBRARIES}) ## Declare a C++ executable: run_map_matcher @@ -114,7 +121,18 @@ if (USE_ROS) add_executable(run_sparse_mapping_pose_adder tools/run_sparse_mapping_pose_adder.cc) add_dependencies(run_sparse_mapping_pose_adder ${catkin_EXPORTED_TARGETS}) target_link_libraries(run_sparse_mapping_pose_adder - ${PROJECT_NAME} gflags gtsam ${catkin_LIBRARIES}) + ${PROJECT_NAME} gflags gtsam ${catkin_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_offline_replay_parameter_reader + test/test_offline_replay_parameter_reader.test + test/test_offline_replay_parameter_reader.cc + ) + target_link_libraries(test_offline_replay_parameter_reader + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) +endif() ############# ## Install ## @@ -128,18 +146,18 @@ install(TARGETS ${PROJECT_NAME} ) # Install C++ executables -install(TARGETS run_bag_imu_filterer DESTINATION bin) +#install(TARGETS run_bag_imu_filterer DESTINATION bin) install(TARGETS run_depth_odometry_adder DESTINATION bin) -install(TARGETS run_graph_bag DESTINATION bin) -install(TARGETS run_imu_bias_tester_adder DESTINATION bin) +install(TARGETS run_offline_replay DESTINATION bin) +install(TARGETS run_imu_bias_extrapolator DESTINATION bin) install(TARGETS run_map_matcher DESTINATION bin) install(TARGETS run_sparse_mapping_pose_adder DESTINATION bin) install(CODE "execute_process( COMMAND ln -s ../../bin/convert_depth_msg share/${PROJECT_NAME} - COMMAND ln -s ../../bin/run_bag_imu_filterer share/${PROJECT_NAME} + #COMMAND ln -s ../../bin/run_bag_imu_filterer share/${PROJECT_NAME} COMMAND ln -s ../../bin/run_depth_odometry_adder share/${PROJECT_NAME} - COMMAND ln -s ../../bin/run_graph_bag share/${PROJECT_NAME} - COMMAND ln -s ../../bin/run_imu_bias_tester_adder share/${PROJECT_NAME} + COMMAND ln -s ../../bin/run_offline_replay share/${PROJECT_NAME} + COMMAND ln -s ../../bin/run_imu_bias_extrapolator share/${PROJECT_NAME} COMMAND ln -s ../../bin/run_map_matcher share/${PROJECT_NAME} COMMAND ln -s ../../bin/run_sparse_mapping_pose_adder share/${PROJECT_NAME} WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} diff --git a/tools/localization_analysis/include/localization_analysis/imu_bias_tester_adder.h b/tools/localization_analysis/include/localization_analysis/ar_tag_pose_adder.h similarity index 65% rename from tools/localization_analysis/include/localization_analysis/imu_bias_tester_adder.h rename to tools/localization_analysis/include/localization_analysis/ar_tag_pose_adder.h index 38f489931a..adcaa95ae4 100644 --- a/tools/localization_analysis/include/localization_analysis/imu_bias_tester_adder.h +++ b/tools/localization_analysis/include/localization_analysis/ar_tag_pose_adder.h @@ -16,10 +16,10 @@ * under the License. */ -#ifndef LOCALIZATION_ANALYSIS_IMU_BIAS_TESTER_ADDER_H_ -#define LOCALIZATION_ANALYSIS_IMU_BIAS_TESTER_ADDER_H_ +#ifndef LOCALIZATION_ANALYSIS_AR_TAG_POSE_ADDER_H_ +#define LOCALIZATION_ANALYSIS_AR_TAG_POSE_ADDER_H_ -#include <imu_bias_tester/imu_bias_tester_wrapper.h> +#include <gtsam/geometry/Pose3.h> #include <rosbag/bag.h> #include <rosbag/view.h> @@ -27,16 +27,19 @@ #include <string> namespace localization_analysis { -class ImuBiasTesterAdder { +// Reads through a bag file and adds ar tag poses using ml/features/pose messages +// and body_T_dock_cam extrinsics +class ArTagPoseAdder { public: - ImuBiasTesterAdder(const std::string& input_bag_name, const std::string& output_bag_name); - void AddPredictions(); + ArTagPoseAdder(const std::string& input_bag_name, const std::string& output_bag_name, + const gtsam::Pose3& dock_cam_T_body); + void AddPoses(); private: - imu_bias_tester::ImuBiasTesterWrapper imu_bias_tester_wrapper_; rosbag::Bag input_bag_; rosbag::Bag output_bag_; + gtsam::Pose3 dock_cam_T_body_; }; } // end namespace localization_analysis -#endif // LOCALIZATION_ANALYSIS_IMU_BIAS_TESTER_ADDER_H_ +#endif // LOCALIZATION_ANALYSIS_AR_TAG_POSE_ADDER_H_ diff --git a/tools/localization_analysis/include/localization_analysis/feature_track_image_adder.h b/tools/localization_analysis/include/localization_analysis/feature_track_image_adder.h new file mode 100644 index 0000000000..b1339ce0d7 --- /dev/null +++ b/tools/localization_analysis/include/localization_analysis/feature_track_image_adder.h @@ -0,0 +1,47 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef LOCALIZATION_ANALYSIS_FEATURE_TRACK_IMAGE_ADDER_H_ +#define LOCALIZATION_ANALYSIS_FEATURE_TRACK_IMAGE_ADDER_H_ + +#include <camera/camera_params.h> +#include <factor_adders/vo_smart_projection_factor_adder.h> +#include <vision_common/spaced_feature_tracker.h> + +#include <opencv2/core/mat.hpp> +#include <sensor_msgs/Image.h> + +#include <vector> + +namespace localization_analysis { +void FeatureTrackImage(const vision_common::SpacedFeatureTracker& feature_tracker, + const camera::CameraParameters& camera_params, cv::Mat& feature_track_image); + +void MarkSmartFactorPoints(const std::vector<boost::shared_ptr<const factor_adders::RobustSmartFactor>> smart_factors, + const camera::CameraParameters& camera_params, const gtsam::Values& values, + cv::Mat& feature_track_image); + +boost::optional<sensor_msgs::ImagePtr> CreateFeatureTrackImage( + const sensor_msgs::ImageConstPtr& image_msg, const vision_common::SpacedFeatureTracker& feature_tracker, + const camera::CameraParameters& camera_params, + const std::vector<boost::shared_ptr<const factor_adders::RobustSmartFactor>>& smart_factors, + const gtsam::Values& values); + +cv::Point2f Distort(const Eigen::Vector2d& undistorted_point, const camera::CameraParameters& params); +} // namespace localization_analysis + +#endif // LOCALIZATION_ANALYSIS_FEATURE_TRACK_IMAGE_ADDER_H_ diff --git a/tools/localization_analysis/include/localization_analysis/graph_localizer_simulator.h b/tools/localization_analysis/include/localization_analysis/graph_localizer_simulator.h index cacff454de..b6dbd96b51 100644 --- a/tools/localization_analysis/include/localization_analysis/graph_localizer_simulator.h +++ b/tools/localization_analysis/include/localization_analysis/graph_localizer_simulator.h @@ -19,44 +19,41 @@ #ifndef LOCALIZATION_ANALYSIS_GRAPH_LOCALIZER_SIMULATOR_H_ #define LOCALIZATION_ANALYSIS_GRAPH_LOCALIZER_SIMULATOR_H_ -#include <ff_msgs/Feature2dArray.h> -#include <ff_msgs/FlightMode.h> +#include <ff_msgs/GraphVIOState.h> #include <ff_msgs/VisualLandmarks.h> #include <localization_analysis/graph_localizer_simulator_params.h> -#include <graph_localizer/graph_localizer_wrapper.h> #include <localization_common/time.h> - -#include <sensor_msgs/Imu.h> +#include <ros_graph_localizer/ros_graph_localizer_wrapper.h> #include <string> #include <vector> namespace localization_analysis { -class GraphLocalizerSimulator : public graph_localizer::GraphLocalizerWrapper { +// Buffers msgs and passes these to the graph localizer after a simluated +// optimization time occurs (as set in the params). Enables a set optimization time +// to be simulated regardless of the hardware used for offline replay. +class GraphLocalizerSimulator : public ros_graph_localizer::RosGraphLocalizerWrapper { public: GraphLocalizerSimulator(const GraphLocalizerSimulatorParams& params, const std::string& graph_config_path_prefix); - void BufferOpticalFlowMsg(const ff_msgs::Feature2dArray& feature_array_msg); - - void BufferVLVisualLandmarksMsg(const ff_msgs::VisualLandmarks& visual_landmarks_msg); - - void BufferARVisualLandmarksMsg(const ff_msgs::VisualLandmarks& visual_landmarks_msg); - void BufferImuMsg(const sensor_msgs::Imu& imu_msg); void BufferFlightModeMsg(const ff_msgs::FlightMode& flight_mode_msg); - void BufferDepthOdometryMsg(const ff_msgs::DepthOdometry& depth_odometry_msg); + void BufferGraphVIOStateMsg(const ff_msgs::GraphVIOState& graph_vio_state_msg); + + void BufferVLVisualLandmarksMsg(const ff_msgs::VisualLandmarks& visual_landmarks_msg); + + void BufferARVisualLandmarksMsg(const ff_msgs::VisualLandmarks& visual_landmarks_msg); bool AddMeasurementsAndUpdateIfReady(const localization_common::Time& current_time); private: - std::vector<ff_msgs::Feature2dArray> of_msg_buffer_; - std::vector<ff_msgs::DepthOdometry> depth_odometry_msg_buffer_; - std::vector<ff_msgs::VisualLandmarks> vl_msg_buffer_; - std::vector<ff_msgs::VisualLandmarks> ar_msg_buffer_; std::vector<sensor_msgs::Imu> imu_msg_buffer_; std::vector<ff_msgs::FlightMode> flight_mode_msg_buffer_; + std::vector<ff_msgs::GraphVIOState> vio_msg_buffer_; + std::vector<ff_msgs::VisualLandmarks> vl_msg_buffer_; + std::vector<ff_msgs::VisualLandmarks> ar_msg_buffer_; boost::optional<localization_common::Time> last_update_time_; GraphLocalizerSimulatorParams params_; }; diff --git a/tools/localization_analysis/include/localization_analysis/graph_localizer_simulator_params.h b/tools/localization_analysis/include/localization_analysis/graph_localizer_simulator_params.h index 6c9dc155bb..66f48da091 100644 --- a/tools/localization_analysis/include/localization_analysis/graph_localizer_simulator_params.h +++ b/tools/localization_analysis/include/localization_analysis/graph_localizer_simulator_params.h @@ -20,6 +20,8 @@ namespace localization_analysis { struct GraphLocalizerSimulatorParams { + // Simulated optimization time. Messages that are received during optimization + // are added once this duration has passed. double optimization_time; }; } // namespace localization_analysis diff --git a/tools/localization_analysis/include/localization_analysis/graph_vio_simulator.h b/tools/localization_analysis/include/localization_analysis/graph_vio_simulator.h new file mode 100644 index 0000000000..f4ff7f6ed3 --- /dev/null +++ b/tools/localization_analysis/include/localization_analysis/graph_vio_simulator.h @@ -0,0 +1,64 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_ANALYSIS_GRAPH_VIO_SIMULATOR_H_ +#define LOCALIZATION_ANALYSIS_GRAPH_VIO_SIMULATOR_H_ + +#include <ff_msgs/Feature2dArray.h> +#include <ff_msgs/FlightMode.h> +#include <ff_msgs/VisualLandmarks.h> +#include <localization_analysis/graph_vio_simulator_params.h> +#include <localization_common/time.h> +#include <ros_graph_vio/ros_graph_vio_wrapper.h> + +#include <sensor_msgs/Image.h> +#include <sensor_msgs/Imu.h> +#include <sensor_msgs/PointCloud2.h> + +#include <string> +#include <vector> + +namespace localization_analysis { +// Buffers msgs and passes these to the graph VIO after a simluated +// optimization time occurs (as set in the params). Enables a set optimization time +// to be simulated regardless of the hardware used for offline replay. +class GraphVIOSimulator : public ros_graph_vio::RosGraphVIOWrapper { + public: + GraphVIOSimulator(const GraphVIOSimulatorParams& params, const std::string& graph_config_path_prefix); + + void BufferOpticalFlowMsg(const ff_msgs::Feature2dArray& feature_array_msg); + + void BufferImuMsg(const sensor_msgs::Imu& imu_msg); + + void BufferFlightModeMsg(const ff_msgs::FlightMode& flight_mode_msg); + + void BufferDepthOdometryMsg(const ff_msgs::DepthOdometry& depth_odometry_msg); + + bool AddMeasurementsAndUpdateIfReady(const localization_common::Time& current_time); + + private: + std::vector<ff_msgs::Feature2dArray> of_msg_buffer_; + std::vector<ff_msgs::DepthOdometry> depth_odometry_msg_buffer_; + std::vector<sensor_msgs::Imu> imu_msg_buffer_; + std::vector<ff_msgs::FlightMode> flight_mode_msg_buffer_; + boost::optional<localization_common::Time> last_update_time_; + GraphVIOSimulatorParams params_; +}; +} // namespace localization_analysis + +#endif // LOCALIZATION_ANALYSIS_GRAPH_VIO_SIMULATOR_H_ diff --git a/tools/localization_analysis/include/localization_analysis/graph_vio_simulator_params.h b/tools/localization_analysis/include/localization_analysis/graph_vio_simulator_params.h new file mode 100644 index 0000000000..41be9152d0 --- /dev/null +++ b/tools/localization_analysis/include/localization_analysis/graph_vio_simulator_params.h @@ -0,0 +1,29 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ +#ifndef LOCALIZATION_ANALYSIS_GRAPH_VIO_SIMULATOR_PARAMS_H_ +#define LOCALIZATION_ANALYSIS_GRAPH_VIO_SIMULATOR_PARAMS_H_ + +namespace localization_analysis { +struct GraphVIOSimulatorParams { + // Simulated optimization time. Messages that are received during optimization + // are added once this duration has passed. + double optimization_time; +}; +} // namespace localization_analysis + +#endif // LOCALIZATION_ANALYSIS_GRAPH_VIO_SIMULATOR_PARAMS_H_ diff --git a/tools/localization_analysis/include/localization_analysis/imu_bias_extrapolator.h b/tools/localization_analysis/include/localization_analysis/imu_bias_extrapolator.h new file mode 100644 index 0000000000..39013b0fde --- /dev/null +++ b/tools/localization_analysis/include/localization_analysis/imu_bias_extrapolator.h @@ -0,0 +1,52 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef LOCALIZATION_ANALYSIS_IMU_BIAS_EXTRAPOLATOR_H_ +#define LOCALIZATION_ANALYSIS_IMU_BIAS_EXTRAPOLATOR_H_ + +#include <ff_msgs/GraphVIOState.h> +#include <localization_common/timestamped_set.h> +#include <imu_integration/imu_integrator.h> + +#include <rosbag/bag.h> +#include <rosbag/view.h> + +#include <string> +#include <vector> + +namespace localization_analysis { +class ImuBiasExtrapolator { + public: + ImuBiasExtrapolator(const std::string& input_bag_name, const std::string& output_bag_name); + bool Initialized(); + void Initialize(const localization_common::CombinedNavState& combined_nav_state); + std::vector<localization_common::CombinedNavState> VIOStateCallback( + const ff_msgs::GraphVIOState& graph_vio_state_msg); + void AddExtrapolatedStates(); + + private: + std::unique_ptr<imu_integration::ImuIntegrator> imu_integrator_; + localization_common::TimestampedSet<localization_common::CombinedNavState> combined_nav_states_; + localization_common::CombinedNavState latest_extrapolated_state_; + bool initialized_; + rosbag::Bag input_bag_; + rosbag::Bag output_bag_; +}; +} // end namespace localization_analysis + +#endif // LOCALIZATION_ANALYSIS_IMU_BIAS_EXTRAPOLATOR_H_ 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 6216748e58..f16503b5df 100644 --- a/tools/localization_analysis/include/localization_analysis/live_measurement_simulator.h +++ b/tools/localization_analysis/include/localization_analysis/live_measurement_simulator.h @@ -19,6 +19,7 @@ #ifndef LOCALIZATION_ANALYSIS_LIVE_MEASUREMENT_SIMULATOR_H_ #define LOCALIZATION_ANALYSIS_LIVE_MEASUREMENT_SIMULATOR_H_ +#include <depth_odometry/depth_odometry_wrapper.h> #include <ff_common/utils.h> #include <ff_msgs/DepthOdometry.h> #include <ff_msgs/Feature2dArray.h> @@ -41,6 +42,8 @@ #include <utility> namespace localization_analysis { +// Buffers sensor messages and passes them to the localizer or VIO once the simulated delay for each message has +// passed. Creates optical flow and sparse mapping msgs from image msgs if desired. class LiveMeasurementSimulator { public: explicit LiveMeasurementSimulator(const LiveMeasurementSimulatorParams& params); @@ -65,9 +68,12 @@ 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_; const std::string kImageTopic_; + std::string topic_localization_depth_image_; + std::string topic_localization_depth_cloud_; std::unique_ptr<rosbag::View> view_; boost::optional<rosbag::View::iterator> view_it_; std::map<localization_common::Time, sensor_msgs::ImageConstPtr> img_buffer_; diff --git a/tools/localization_analysis/include/localization_analysis/live_measurement_simulator_params.h b/tools/localization_analysis/include/localization_analysis/live_measurement_simulator_params.h index 0bf6c2b80c..ff42219202 100644 --- a/tools/localization_analysis/include/localization_analysis/live_measurement_simulator_params.h +++ b/tools/localization_analysis/include/localization_analysis/live_measurement_simulator_params.h @@ -27,14 +27,18 @@ struct LiveMeasurementSimulatorParams { MessageBufferParams imu; MessageBufferParams flight_mode; MessageBufferParams depth_odometry; + MessageBufferParams depth_image; + MessageBufferParams depth_cloud; MessageBufferParams of; MessageBufferParams vl; MessageBufferParams ar; MessageBufferParams img; + MessageBufferParams vio; std::string bag_name; std::string map_file; std::string image_topic; - bool use_image_features; + bool use_bag_image_feature_msgs; + bool use_bag_depth_odom_msgs; bool save_optical_flow_images; }; } // namespace localization_analysis diff --git a/tools/localization_analysis/include/localization_analysis/message_buffer.h b/tools/localization_analysis/include/localization_analysis/message_buffer.h index 79de82b24c..2b114e32de 100644 --- a/tools/localization_analysis/include/localization_analysis/message_buffer.h +++ b/tools/localization_analysis/include/localization_analysis/message_buffer.h @@ -25,6 +25,8 @@ #include <map> namespace localization_analysis { +// Buffers messages and only provides them once the provided delay has passed. +// Optionally drops messages that are too close together in time. template <typename MessageType> class MessageBuffer { public: diff --git a/tools/localization_analysis/include/localization_analysis/message_buffer_params.h b/tools/localization_analysis/include/localization_analysis/message_buffer_params.h index 7f845d8c05..0f5d67294e 100644 --- a/tools/localization_analysis/include/localization_analysis/message_buffer_params.h +++ b/tools/localization_analysis/include/localization_analysis/message_buffer_params.h @@ -20,6 +20,7 @@ namespace localization_analysis { struct MessageBufferParams { + // Only make message available once this delay has passed. double msg_delay; // Some message providers drop messages too close together in time. Drop all messages < min_msg_spacing from each // other. diff --git a/tools/localization_analysis/include/localization_analysis/graph_bag.h b/tools/localization_analysis/include/localization_analysis/offline_replay.h similarity index 50% rename from tools/localization_analysis/include/localization_analysis/graph_bag.h rename to tools/localization_analysis/include/localization_analysis/offline_replay.h index 51c36219a6..3f969e908f 100644 --- a/tools/localization_analysis/include/localization_analysis/graph_bag.h +++ b/tools/localization_analysis/include/localization_analysis/offline_replay.h @@ -16,15 +16,16 @@ * under the License. */ -#ifndef LOCALIZATION_ANALYSIS_GRAPH_BAG_H_ -#define LOCALIZATION_ANALYSIS_GRAPH_BAG_H_ +#ifndef LOCALIZATION_ANALYSIS_OFFLINE_REPLAY_H_ +#define LOCALIZATION_ANALYSIS_OFFLINE_REPLAY_H_ #include <camera/camera_params.h> #include <localization_analysis/graph_localizer_simulator.h> -#include <localization_analysis/graph_bag_params.h> -#include <imu_bias_tester/imu_bias_tester_wrapper.h> +#include <localization_analysis/graph_vio_simulator.h> +#include <localization_analysis/offline_replay_params.h> +// #include <imu_bias_tester/imu_bias_tester_wrapper.h> #include <localization_analysis/live_measurement_simulator.h> -#include <imu_augmentor/imu_augmentor_wrapper.h> +#include <ros_pose_extrapolator/ros_pose_extrapolator_wrapper.h> #include <rosbag/bag.h> #include <sensor_msgs/Image.h> @@ -35,29 +36,32 @@ namespace localization_analysis { // Reads through a bag file and passes relevant messages to graph localizer -// wrapper. Uses LiveMeasurementSimulator which contains its own instances of sensor parsers (lk_optical_flow, -// localizer (for sparse map matching)) and passes output to graph localizer -// wrapper so this does not require a ROS core and can parse bags more quickly. Saves output to a new bagfile. -class GraphBag { +// and VIO wrappers. Uses the LiveMeasurementSimulator which contains its own instances of sensor parsers +// (lk_optical_flow, localizer (for sparse map matching)) with optional measurement delays and passes the output of +// these sensor parses to the respective graph objects. This tool avoids using rosbag play and ROS core and enables +// parsing bags more quickly. Saves output to a new bagfile. +class OfflineReplay { public: - GraphBag(const std::string& bag_name, const std::string& map_file, const std::string& image_topic, - const std::string& results_bag, const std::string& output_stats_file, const bool use_image_features = true, - const std::string& graph_config_path_prefix = ""); + OfflineReplay(const std::string& bag_name, const std::string& map_file, const std::string& image_topic, + const std::string& results_bag, const std::string& output_stats_file, + const bool use_bag_image_feature_msgs = true, const std::string& graph_config_path_prefix = ""); void Run(); private: - void InitializeGraph(); - void SaveOpticalFlowTracksImage(const sensor_msgs::ImageConstPtr& image_msg, - const GraphLocalizerSimulator& graph_localizer); + void InitializeGraphs(); + /*void SaveOpticalFlowTracksImage(const sensor_msgs::ImageConstPtr& image_msg, + const GraphLocalizerSimulator& graph_localizer);*/ std::unique_ptr<GraphLocalizerSimulator> graph_localizer_simulator_; + std::unique_ptr<GraphVIOSimulator> graph_vio_simulator_; std::unique_ptr<LiveMeasurementSimulator> live_measurement_simulator_; rosbag::Bag results_bag_; - imu_bias_tester::ImuBiasTesterWrapper imu_bias_tester_wrapper_; - imu_augmentor::ImuAugmentorWrapper imu_augmentor_wrapper_; + // imu_bias_tester::ImuBiasTesterWrapper imu_bias_tester_wrapper_; + ros_pose_extrapolator::RosPoseExtrapolatorWrapper pose_extrapolator_wrapper_; std::string output_stats_file_; const std::string kFeatureTracksImageTopic_ = "feature_track_image"; - GraphBagParams params_; + boost::optional<ff_msgs::VisualLandmarks> latest_ar_msg_; + OfflineReplayParams params_; }; } // end namespace localization_analysis -#endif // LOCALIZATION_ANALYSIS_GRAPH_BAG_H_ +#endif // LOCALIZATION_ANALYSIS_OFFLINE_REPLAY_H_ diff --git a/tools/localization_analysis/include/localization_analysis/graph_bag_params.h b/tools/localization_analysis/include/localization_analysis/offline_replay_params.h similarity index 69% rename from tools/localization_analysis/include/localization_analysis/graph_bag_params.h rename to tools/localization_analysis/include/localization_analysis/offline_replay_params.h index 6cca66e6ec..3981cb591d 100644 --- a/tools/localization_analysis/include/localization_analysis/graph_bag_params.h +++ b/tools/localization_analysis/include/localization_analysis/offline_replay_params.h @@ -15,8 +15,8 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef LOCALIZATION_ANALYSIS_GRAPH_BAG_PARAMS_H_ -#define LOCALIZATION_ANALYSIS_GRAPH_BAG_PARAMS_H_ +#ifndef LOCALIZATION_ANALYSIS_OFFLINE_REPLAY_PARAMS_H_ +#define LOCALIZATION_ANALYSIS_OFFLINE_REPLAY_PARAMS_H_ #include <camera/camera_params.h> @@ -25,14 +25,19 @@ #include <memory> namespace localization_analysis { -struct GraphBagParams { +struct OfflineReplayParams { + // Save images with optical flow feature tracks to the output bagfile. bool save_optical_flow_images; + // Log the relative time since the start of the bagfile. bool log_relative_time; std::unique_ptr<camera::CameraParameters> nav_cam_params; gtsam::Pose3 body_T_nav_cam; + gtsam::Pose3 body_T_dock_cam; + // Min number of ar landmarks for an AR msg to be valid. int ar_min_num_landmarks; + // Min number of sparse mapping landmarks for a Sparse Mapping msg to be valid. int sparse_mapping_min_num_landmarks; }; } // namespace localization_analysis -#endif // LOCALIZATION_ANALYSIS_GRAPH_BAG_PARAMS_H_ +#endif // LOCALIZATION_ANALYSIS_OFFLINE_REPLAY_PARAMS_H_ diff --git a/tools/localization_analysis/include/localization_analysis/parameter_reader.h b/tools/localization_analysis/include/localization_analysis/parameter_reader.h index 632a606480..e61e8bdafe 100644 --- a/tools/localization_analysis/include/localization_analysis/parameter_reader.h +++ b/tools/localization_analysis/include/localization_analysis/parameter_reader.h @@ -19,20 +19,23 @@ #define LOCALIZATION_ANALYSIS_PARAMETER_READER_H_ #include <config_reader/config_reader.h> -#include <localization_analysis/graph_bag_params.h> #include <localization_analysis/graph_localizer_simulator_params.h> +#include <localization_analysis/graph_vio_simulator_params.h> #include <localization_analysis/live_measurement_simulator_params.h> #include <localization_analysis/message_buffer_params.h> +#include <localization_analysis/offline_replay_params.h> #include <string> namespace localization_analysis { -void LoadMessageBufferParams(config_reader::ConfigReader& config, MessageBufferParams& params); +void LoadMessageBufferParams(config_reader::ConfigReader& config, MessageBufferParams& params, + const std::string& prefix = ""); void LoadLiveMeasurementSimulatorParams(config_reader::ConfigReader& config, const std::string& bag_name, const std::string& map_file, const std::string& image_topic, LiveMeasurementSimulatorParams& params); void LoadGraphLocalizerSimulatorParams(config_reader::ConfigReader& config, GraphLocalizerSimulatorParams& params); -void LoadGraphBagParams(config_reader::ConfigReader& config, GraphBagParams& params); +void LoadGraphVIOSimulatorParams(config_reader::ConfigReader& config, GraphVIOSimulatorParams& params); +void LoadOfflineReplayParams(config_reader::ConfigReader& config, OfflineReplayParams& params); } // namespace localization_analysis #endif // LOCALIZATION_ANALYSIS_PARAMETER_READER_H_ diff --git a/tools/localization_analysis/include/localization_analysis/utilities.h b/tools/localization_analysis/include/localization_analysis/utilities.h index 4a7a655978..1268ff2fb2 100644 --- a/tools/localization_analysis/include/localization_analysis/utilities.h +++ b/tools/localization_analysis/include/localization_analysis/utilities.h @@ -19,9 +19,10 @@ #define LOCALIZATION_ANALYSIS_UTILITIES_H_ #include <camera/camera_params.h> -#include <graph_localizer/feature_tracker.h> #include <graph_localizer/graph_localizer.h> #include <localization_common/utilities.h> +#include <localization_measurements/timestamped_pose.h> +#include <vision_common/feature_tracker.h> #include <opencv2/core/mat.hpp> @@ -32,34 +33,21 @@ #include <vector> namespace localization_analysis { -// TODO(rsoussan): put these somewhere else! -using Calibration = gtsam::Cal3_S2; -using Camera = gtsam::PinholePose<Calibration>; -using SmartFactor = gtsam::RobustSmartProjectionPoseFactor<Calibration>; - -void FeatureTrackImage(const graph_localizer::FeatureTrackIdMap& feature_tracks, - const camera::CameraParameters& camera_params, cv::Mat& feature_track_image); -void MarkSmartFactorPoints(const std::vector<const SmartFactor*> smart_factors, - const camera::CameraParameters& camera_params, const double feature_track_min_separation, - const int max_num_factors, cv::Mat& feature_track_image); -boost::optional<sensor_msgs::ImagePtr> CreateFeatureTrackImage( - const sensor_msgs::ImageConstPtr& image_msg, const graph_localizer::FeatureTrackIdMap& feature_tracks, - const camera::CameraParameters& camera_params, const std::vector<const SmartFactor*>& smart_factors = {}); - -cv::Point2f Distort(const Eigen::Vector2d& undistorted_point, const camera::CameraParameters& params); - -std::vector<const SmartFactor*> SmartFactors(const graph_localizer::GraphLocalizer& graph); - bool string_ends_with(const std::string& str, const std::string& ending); -void SaveImuBiasTesterPredictedStates( - const std::vector<localization_common::CombinedNavState>& imu_bias_tester_predicted_states, rosbag::Bag& bag); - template <typename MsgType> void SaveMsg(const MsgType& msg, const std::string& topic, rosbag::Bag& bag) { const ros::Time timestamp = localization_common::RosTimeFromHeader(msg.header); bag.write("/" + topic, timestamp, msg); } + +geometry_msgs::PoseStamped PoseMsg(const Eigen::Isometry3d& global_T_body, const std_msgs::Header& header); + +geometry_msgs::PoseStamped PoseMsg(const Eigen::Isometry3d& global_T_body, const localization_common::Time time); + +geometry_msgs::PoseStamped PoseMsg(const gtsam::Pose3& global_T_body, const localization_common::Time time); + +geometry_msgs::PoseStamped PoseMsg(const localization_measurements::TimestampedPose& timestamped_pose); } // namespace localization_analysis #endif // LOCALIZATION_ANALYSIS_UTILITIES_H_ diff --git a/tools/localization_analysis/package.xml b/tools/localization_analysis/package.xml index 3f9f969edc..20d9a424ec 100644 --- a/tools/localization_analysis/package.xml +++ b/tools/localization_analysis/package.xml @@ -23,10 +23,15 @@ <build_depend>ff_msgs</build_depend> <build_depend>ff_common</build_depend> <build_depend>graph_localizer</build_depend> + <build_depend>graph_vio</build_depend> <build_depend>imu_augmentor</build_depend> - <build_depend>imu_bias_tester</build_depend> <build_depend>lk_optical_flow</build_depend> <build_depend>localization_node</build_depend> + <build_depend>msg_conversions</build_depend> + <build_depend>parameter_reader</build_depend> + <build_depend>ros_graph_localizer</build_depend> + <build_depend>ros_graph_vio</build_depend> + <build_depend>ros_pose_extrapolator</build_depend> <run_depend>eigen_conversions</run_depend> <run_depend>graph_localizer</run_depend> <run_depend>roscpp</run_depend> @@ -37,8 +42,12 @@ <run_depend>ff_msgs</run_depend> <run_depend>ff_common</run_depend> <run_depend>graph_localizer</run_depend> - <run_depend>imu_augmentor</run_depend> - <run_depend>imu_bias_tester</run_depend> + <run_depend>graph_vio</run_depend> <run_depend>lk_optical_flow</run_depend> <run_depend>localization_node</run_depend> + <run_depend>msg_conversions</run_depend> + <run_depend>parameter_reader</run_depend> + <run_depend>ros_graph_localizer</run_depend> + <run_depend>ros_graph_vio</run_depend> + <run_depend>ros_pose_extrapolator</run_depend> </package> diff --git a/tools/localization_analysis/readme.md b/tools/localization_analysis/readme.md index d00edbc392..0f30d5cd23 100644 --- a/tools/localization_analysis/readme.md +++ b/tools/localization_analysis/readme.md @@ -26,7 +26,7 @@ The filtered data is saved to a bagfile and can be plotted and analyzed using th ## `run_depth_odometry_adder` Generates depth odometry relative poses using recorded depth camera data and saves these to a bag file. -## `run_graph_bag` +## `run_offline_replay` Runs graph localization on a bagfile and saves the results to a new bagfile. The results can be plotted using the `plot_results.py` script. ## `run_imu_bias_tester_adder` @@ -84,6 +84,6 @@ Plots localization information for a bagfile to a pdf. Uses either sparse mappin from the input bagfile or a seperate bagfile containing groundtruth poses for comparison with the localization estimates. -## `run_graph_bag_and_plot_results` +## `run_offline_replay_and_plot_results` Generates localization estimates and plots results for a provided bagfile. diff --git a/tools/localization_analysis/scripts/bag_and_parameter_sweep.py b/tools/localization_analysis/scripts/bag_and_parameter_sweeper.py similarity index 60% rename from tools/localization_analysis/scripts/bag_and_parameter_sweep.py rename to tools/localization_analysis/scripts/bag_and_parameter_sweeper.py index 76b4d5d51e..dc9c080535 100755 --- a/tools/localization_analysis/scripts/bag_and_parameter_sweep.py +++ b/tools/localization_analysis/scripts/bag_and_parameter_sweeper.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. @@ -28,12 +28,13 @@ import pandas as pd -import bag_sweep +import bag_sweeper import localization_common.utilities as lu -import parameter_sweep -import plot_parameter_sweep_results +import parameter_sweep_results_plotter +import parameter_sweeper +# Save parameter sweep value ranges used during parameter sweep to a csv file def save_ranges(param_range_directory, output_directory): individual_ranges_file = os.path.join( param_range_directory, "individual_value_ranges.csv" @@ -62,7 +63,8 @@ def add_job_id_mean_row(job_id, dataframes, mean_dataframe, names): return mean_dataframe.append(job_id_mean_dataframe, ignore_index=True) -def average_parameter_sweep_results(combined_results_csv_files, directory): +# Save the average of each parameter sweep on a set of bagfiles to a new csv file +def average_parameter_sweep_results(combined_results_csv_files, directory, prefix): dataframes = [pd.read_csv(file) for file in combined_results_csv_files] if not dataframes: print("Failed to create dataframes") @@ -73,41 +75,61 @@ def average_parameter_sweep_results(combined_results_csv_files, directory): # Save job id mean in order, so nth row coressponds to nth job id in final mean dataframe for job_id in range(jobs): mean_dataframe = add_job_id_mean_row(job_id, dataframes, mean_dataframe, names) - mean_dataframe_file = os.path.join(directory, "bag_and_param_sweep_stats.csv") + mean_dataframe_file = os.path.join( + directory, prefix + "_bag_and_param_sweep_stats.csv" + ) mean_dataframe.to_csv(mean_dataframe_file, index=False) -def bag_and_parameter_sweep(graph_bag_params_list, output_dir): - combined_results_csv_files = [] +# Perform a parameter sweep on each provided offline replay param set +def bag_and_parameter_sweep(offline_replay_params_list, output_dir): + loc_combined_results_csv_files = [] + vio_combined_results_csv_files = [] param_range_directory = None - for graph_bag_params in graph_bag_params_list: + for offline_replay_params in offline_replay_params_list: # Save parameter sweep output in different directory for each bagfile, name directory using bagfile - bag_name_prefix = os.path.splitext(os.path.basename(graph_bag_params.bagfile))[ - 0 - ] + bag_name_prefix = os.path.splitext( + os.path.basename(offline_replay_params.bagfile) + )[0] bag_output_dir = os.path.join(output_dir, bag_name_prefix) - param_range_directory_for_bag = parameter_sweep.make_values_and_parameter_sweep( - bag_output_dir, - graph_bag_params.bagfile, - graph_bag_params.map_file, - graph_bag_params.image_topic, - graph_bag_params.config_path, - graph_bag_params.robot_config_file, - graph_bag_params.world, - graph_bag_params.use_image_features, - graph_bag_params.groundtruth_bagfile, - graph_bag_params.rmse_rel_start_time, - graph_bag_params.rmse_rel_end_time, + param_range_directory_for_bag = ( + parameter_sweeper.make_values_and_parameter_sweep( + bag_output_dir, + offline_replay_params.bagfile, + offline_replay_params.map_file, + offline_replay_params.image_topic, + offline_replay_params.robot_config_file, + offline_replay_params.world, + offline_replay_params.use_bag_image_feature_msgs, + offline_replay_params.groundtruth_bagfile, + ) ) if not param_range_directory: param_range_directory = param_range_directory_for_bag - combined_results_csv_files.append( - os.path.join(bag_output_dir, "param_sweep_combined_results.csv") + loc_combined_results_csv_files.append( + os.path.join(bag_output_dir, "loc_param_sweep_combined_results.csv") + ) + vio_combined_results_csv_files.append( + os.path.join(bag_output_dir, "vio_param_sweep_combined_results.csv") ) - average_parameter_sweep_results(combined_results_csv_files, output_dir) + average_parameter_sweep_results(loc_combined_results_csv_files, output_dir, "loc") + average_parameter_sweep_results(vio_combined_results_csv_files, output_dir, "vio") save_ranges(param_range_directory, output_dir) +def create_results_plots(output_dir, prefix): + combined_results_file = os.path.join( + output_dir, prefix + "_bag_and_param_sweep_stats.csv" + ) + value_combos_file = os.path.join(output_dir, "all_value_combos.csv") + results_pdf_file = os.path.join( + output_dir, prefix + "_bag_and_param_sweep_results.pdf" + ) + parameter_sweep_results_plotter.create_plots( + results_pdf_file, combined_results_file, value_combos_file + ) + + if __name__ == "__main__": parser = argparse.ArgumentParser( description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter @@ -116,21 +138,22 @@ def bag_and_parameter_sweep(graph_bag_params_list, output_dir): "config_file", help="Config file containing information for bag sweep. See bag_sweep.py script for more details.", ) - parser.add_argument("output_dir", help="Output directory where results are saved.") + parser.add_argument( + "-o", + "--output-dir", + default=None, + help="Full path to output directory where files will be saved. If not specified, timestamped directory will be created in current path.", + ) args = parser.parse_args() if not os.path.isfile(args.config_file): print(("Config file " + args.config_file + " does not exist.")) sys.exit() - if os.path.isdir(args.output_dir): + if args.output_dir is not None and os.path.isdir(args.output_dir): print(("Output directory " + args.output_dir + " already exists.")) sys.exit() output_dir = lu.create_directory(args.output_dir) - graph_bag_params_list = bag_sweep.load_params(args.config_file) - bag_and_parameter_sweep(graph_bag_params_list, output_dir) - combined_results_file = os.path.join(output_dir, "bag_and_param_sweep_stats.csv") - value_combos_file = os.path.join(output_dir, "all_value_combos.csv") - results_pdf_file = os.path.join(output_dir, "bag_and_param_sweep_results.pdf") - plot_parameter_sweep_results.create_plots( - results_pdf_file, combined_results_file, value_combos_file - ) + offline_replay_params_list = bag_sweeper.load_params(args.config_file) + bag_and_parameter_sweep(offline_replay_params_list, output_dir) + create_results_plots(output_dir, "loc") + create_results_plots(output_dir, "vio") diff --git a/tools/localization_analysis/scripts/bag_sweep.py b/tools/localization_analysis/scripts/bag_sweep.py deleted file mode 100755 index 6c21e3484c..0000000000 --- a/tools/localization_analysis/scripts/bag_sweep.py +++ /dev/null @@ -1,195 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. -""" -The bag sweep tool runs the graph bag tool in parallel on multiple bag files. -It takes a config file with bag names, map names, and robot configs and produces -pdfs and result bagfiles for each entry. -""" - - -import argparse -import csv -import itertools -import multiprocessing -import os -import sys - -import average_results -import multiprocessing_helpers -import plot_bag_sweep_results - - -class GraphBagParams(object): - def __init__( - self, - bagfile, - map_file, - image_topic, - use_image_features, - groundtruth_bagfile, - rmse_rel_start_time, - rmse_rel_end_time, - ): - self.bagfile = bagfile - self.map_file = map_file - self.image_topic = image_topic - self.use_image_features = use_image_features - self.groundtruth_bagfile = groundtruth_bagfile - self.rmse_rel_start_time = rmse_rel_start_time - self.rmse_rel_end_time = rmse_rel_end_time - - -def load_params(param_file): - graph_bag_params_list = [] - with open(param_file) as param_csvfile: - reader = csv.reader(param_csvfile, delimiter=" ") - for row in reader: - graph_bag_params_list.append( - GraphBagParams( - row[0], - row[1], - row[2], - row[3], - row[4], - row[5], - row[6], - ) - ) - - return graph_bag_params_list - - -def combine_results_in_csv_file(graph_bag_params, output_dir): - # Don't save this as *stats.csv otherwise it will be including when averaging bag results in average_results.py - combined_results_csv_file = os.path.join(output_dir, "bag_sweep_stats_combined.csv") - output_csv_files = [] - bag_names = [] - for params in graph_bag_params: - bag_name = os.path.splitext(os.path.basename(params.bagfile))[0] - bag_names.append(bag_name) - output_csv_files.append(os.path.join(output_dir, bag_name + "_stats.csv")) - combined_dataframe = average_results.combined_results(output_csv_files) - combined_dataframe.insert(0, "Bag", bag_names) - combined_dataframe.to_csv(combined_results_csv_file, index=False) - return combined_results_csv_file - - -def check_params(graph_bag_params_list): - for params in graph_bag_params_list: - if not os.path.isfile(params.bagfile): - print(("Bagfile " + params.bagfile + " does not exist.")) - sys.exit() - if not os.path.isfile(params.map_file): - print(("Map file " + params.map_file + " does not exist.")) - sys.exit() - if not os.path.isfile(params.groundtruth_bagfile): - print(("Bagfile " + params.groundtruth_bagfile + " does not exist.")) - sys.exit() - - -# Add traceback so errors are forwarded, otherwise -# some errors are suppressed due to the multiprocessing -# library call -@multiprocessing_helpers.full_traceback -def run_graph_bag(params, output_dir): - bag_name = os.path.splitext(os.path.basename(params.bagfile))[0] - output_bag_path = os.path.join(output_dir, bag_name + "_results.bag") - output_csv_file = os.path.join(output_dir, bag_name + "_stats.csv") - run_command = ( - "rosrun localization_analysis run_graph_bag " - + params.bagfile - + " " - + params.map_file - + " -i " - + params.image_topic - + " -o " - + output_bag_path - + " -s " - + output_csv_file - + " -f " - + params.use_image_features - ) - os.system(run_command) - output_pdf_file = os.path.join(output_dir, bag_name + "_output.pdf") - plot_command = ( - "rosrun localization_analysis plot_results.py " - + output_bag_path - + " --output-file " - + output_pdf_file - + " --output-csv-file " - + output_csv_file - + " -g " - + params.groundtruth_bagfile - + " --rmse-rel-start-time " - + params.rmse_rel_start_time - + " --rmse-rel-end-time " - + params.rmse_rel_end_time - ) - os.system(plot_command) - - -# Helper that unpacks arguments and calls original function -# Aides running jobs in parallel as pool only supports -# passing a single argument to workers -def run_graph_bag_helper(zipped_vals): - return run_graph_bag(*zipped_vals) - - -def bag_sweep(config_file, output_dir): - graph_bag_params_list = load_params(config_file) - check_params(graph_bag_params_list) - num_processes = 15 - pool = multiprocessing.Pool(num_processes) - # izip arguments so we can pass as one argument to pool worker - pool.map( - run_graph_bag_helper, - list(zip(graph_bag_params_list, itertools.repeat(output_dir))), - ) - combined_results_csv_file = combine_results_in_csv_file( - graph_bag_params_list, output_dir - ) - output_file = os.path.join(output_dir, "bag_sweep_results.pdf") - plot_bag_sweep_results.create_plot(output_file, combined_results_csv_file) - - -if __name__ == "__main__": - - class Formatter( - argparse.RawTextHelpFormatter, argparse.RawDescriptionHelpFormatter - ): - pass - - parser = argparse.ArgumentParser(description=__doc__, formatter_class=Formatter) - parser.add_argument( - "config_file", - help="Config file containing bag names, map names, image topics, config path, robot config, and world. A new line should be used for each bagfile. Example:\n /home/bag_name.bag /home/map_name.map /mgt/img_sampler/nav_cam/image_record /home/astrobee/src/astrobee config/robots/bumble.config iss false \n /home/bag_name_2.bag /home/map_name.map /mgt/img_sampler/nav_cam/image_record /home/astrobee/src/astrobee config/robots/bumble.config iss false", - ) - parser.add_argument( - "output_dir", help="Output directory where results files are saved." - ) - args = parser.parse_args() - if not os.path.isfile(args.config_file): - print(("Config file " + args.config_file + " does not exist.")) - sys.exit() - if os.path.isdir(args.output_dir): - print(("Output directory " + args.output_dir + " already exists.")) - sys.exit() - os.makedirs(args.output_dir) - - bag_sweep(args.config_file, args.output_dir) diff --git a/tools/localization_analysis/scripts/bag_sweeper.py b/tools/localization_analysis/scripts/bag_sweeper.py new file mode 100755 index 0000000000..5317ccdeb1 --- /dev/null +++ b/tools/localization_analysis/scripts/bag_sweeper.py @@ -0,0 +1,228 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +""" +The bag sweep tool runs offline replay in parallel on multiple bag files. It takes a config file with bag names, map names, and robot configs and produces pdfs and result bagfiles for each entry. +""" + + +import argparse +import csv +import itertools +import multiprocessing +import os +import sys + +import matplotlib + +matplotlib.use("pdf") +import bag_sweep_results_plotter +import matplotlib.pyplot as plt +import pandas as pd +from matplotlib.backends.backend_pdf import PdfPages + +import localization_common.utilities as lu +import plotters +import results_averager + + +class OfflineReplayParams(object): + def __init__( + self, + bagfile, + map_file, + image_topic, + robot_config_file, + world, + use_bag_image_feature_msgs, + groundtruth_bagfile, + ): + self.bagfile = bagfile + self.map_file = map_file + self.image_topic = image_topic + self.robot_config_file = robot_config_file + self.world = world + self.use_bag_image_feature_msgs = use_bag_image_feature_msgs + self.groundtruth_bagfile = groundtruth_bagfile + + +# Load params from the config file +def load_params(param_file): + offline_replay_params_list = [] + with open(param_file) as param_csvfile: + reader = csv.reader(param_csvfile, delimiter=" ") + for row in reader: + offline_replay_params_list.append( + OfflineReplayParams( + row[0], row[1], row[2], row[3], row[4], row[5], row[6] + ) + ) + + return offline_replay_params_list + + +# Combine results in a single csv file for plotting later +def combine_results_in_csv_file(offline_replay_params, output_dir, prefix): + # Don't save this as *stats.csv otherwise it will be including when averaging bag results in average_results.py + combined_results_csv_file = os.path.join(output_dir, "bag_sweep_stats_combined.csv") + output_csv_files = [] + bag_names = [] + for params in offline_replay_params: + bag_name = os.path.splitext(os.path.basename(params.bagfile))[0] + bag_names.append(bag_name) + output_csv_files.append( + os.path.join(output_dir, bag_name + "_" + prefix + "_results.csv") + ) + combined_dataframe = results_averager.combined_results(output_csv_files) + combined_dataframe.insert(0, "Bag", bag_names) + combined_dataframe.to_csv(combined_results_csv_file, index=False) + return combined_results_csv_file + + +# Ensure files in params exist +def check_params(offline_replay_params_list): + for params in offline_replay_params_list: + if not os.path.isfile(params.bagfile): + print(("Bagfile " + params.bagfile + " does not exist.")) + sys.exit() + if not os.path.isfile(params.map_file): + print(("Map file " + params.map_file + " does not exist.")) + sys.exit() + if not os.path.isfile(params.groundtruth_bagfile): + print(("Bagfile " + params.groundtruth_bagfile + " does not exist.")) + sys.exit() + + +# Add traceback so errors are forwarded, otherwise +# some errors are suppressed due to the multiprocessing +# library call +@lu.full_traceback +def run_offline_replay(params, output_dir): + bag_name = os.path.splitext(os.path.basename(params.bagfile))[0] + output_bag_path = os.path.join(output_dir, bag_name + "_results.bag") + output_csv_file = os.path.join(output_dir, bag_name + "_stats.csv") + vio_output_file = os.path.join(output_dir, bag_name + "_vio_output.pdf") + loc_output_file = os.path.join(output_dir, bag_name + "_loc_output.pdf") + vio_results_csv_file = os.path.join(output_dir, bag_name + "_vio_results.csv") + loc_results_csv_file = os.path.join(output_dir, bag_name + "_loc_results.csv") + run_command = ( + "rosrun localization_analysis run_offline_replay_and_plot_results.py " + + params.bagfile + + " " + + params.map_file + + " -r " + + params.robot_config_file + + " -w " + + params.world + + " -i " + + params.image_topic + + " -o " + + output_bag_path + + " --results-csv-file " + + output_csv_file + + " --vio-output-file " + + vio_output_file + + " --loc-output-file " + + loc_output_file + + " --vio-results-csv-file " + + vio_results_csv_file + + " --loc-results-csv-file " + + loc_results_csv_file + + " -g " + + params.groundtruth_bagfile + ) + if not params.use_bag_image_feature_msgs: + run_command += " --generate-image-features " + os.system(run_command) + + +# Helper that unpacks arguments and calls original function +# Aides running jobs in parallel as pool only supports +# passing a single argument to workers +def run_offline_replay_helper(zipped_vals): + return run_offline_replay(*zipped_vals) + + +# Perform bag sweep using config file, save results to output directory. +def bag_sweep(config_file, output_dir): + offline_replay_params_list = load_params(config_file) + check_params(offline_replay_params_list) + num_processes = 15 + pool = multiprocessing.Pool(num_processes) + # izip arguments so we can pass as one argument to pool worker + pool.map( + run_offline_replay_helper, + list(zip(offline_replay_params_list, itertools.repeat(output_dir))), + ) + # Plot Loc results + loc_combined_results_csv_file = combine_results_in_csv_file( + offline_replay_params_list, output_dir, "loc" + ) + loc_output_file = os.path.join(output_dir, "loc_bag_sweep_results.pdf") + loc_dataframe = pd.read_csv(loc_combined_results_csv_file) + loc_rmse_plotter = plotters.BagRMSEsPlotter(loc_dataframe) + with PdfPages(loc_output_file) as pdf: + loc_rmse_plotter.plot(pdf, "Graph Loc Poses rmse") + loc_rmse_plotter.plot(pdf, "Graph Loc Poses Orientation rmse") + loc_rmse_plotter.plot(pdf, "Extrapolated Loc Poses rmse") + loc_rmse_plotter.plot(pdf, "Extrapolated Loc Poses Orientation rmse") + loc_rmse_plotter.plot(pdf, "Extrapolated Integrated Velocity Poses rmse") + loc_rmse_plotter.plot( + pdf, "Extrapolated Integrated Velocity Poses Orientation rmse" + ) + # Plot VIO results + vio_combined_results_csv_file = combine_results_in_csv_file( + offline_replay_params_list, output_dir, "vio" + ) + vio_output_file = os.path.join(output_dir, "vio_bag_sweep_results.pdf") + vio_dataframe = pd.read_csv(vio_combined_results_csv_file) + vio_rmse_plotter = plotters.BagRMSEsPlotter(vio_dataframe) + with PdfPages(vio_output_file) as pdf: + vio_rmse_plotter.plot(pdf, "Graph VIO Poses rmse") + vio_rmse_plotter.plot(pdf, "Graph VIO Poses Orientation rmse") + vio_rmse_plotter.plot(pdf, "Graph VIO Integrated Velocity Poses rmse") + vio_rmse_plotter.plot( + pdf, "Graph VIO Integrated Velocity Poses Orientation rmse" + ) + + +if __name__ == "__main__": + + class Formatter( + argparse.RawTextHelpFormatter, argparse.RawDescriptionHelpFormatter + ): + pass + + parser = argparse.ArgumentParser(description=__doc__, formatter_class=Formatter) + parser.add_argument( + "config_file", + help="Config file containing bag names, map names, image topics, robot config, and world. A new line should be used for each bagfile. Example:\n /home/bag_name.bag /home/map_name.map /mgt/img_sampler/nav_cam/image_record bumble.config iss false /home/groundtruth.bag \n /home/bag_name_2.bag /home/map_name.map /mgt/img_sampler/nav_cam/image_record bumble.config iss false /home/groundtruth.bag", + ) + parser.add_argument( + "output_dir", help="Output directory where results files are saved." + ) + args = parser.parse_args() + if not os.path.isfile(args.config_file): + print(("Config file " + args.config_file + " does not exist.")) + sys.exit() + if os.path.isdir(args.output_dir): + print(("Output directory " + args.output_dir + " already exists.")) + sys.exit() + os.makedirs(args.output_dir) + + bag_sweep(args.config_file, args.output_dir) diff --git a/tools/localization_analysis/scripts/config_creator.py b/tools/localization_analysis/scripts/config_creator.py index 6fcd757dec..df690d0a7d 100644 --- a/tools/localization_analysis/scripts/config_creator.py +++ b/tools/localization_analysis/scripts/config_creator.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. @@ -36,7 +36,7 @@ def fill_in_values(original_config, value_map, new_config): def make_value_map(values, value_names): value_map = {} if len(values) != len(value_names): - print("values and value_names not same length!") + print("Values and value_names not the same length.") exit() for index, value_name in enumerate(value_names): diff --git a/tools/localization_analysis/scripts/depth_odometry_parameter_sweep.py b/tools/localization_analysis/scripts/depth_odometry_parameter_sweep.py index ac5da7c1c1..faa4474eb9 100755 --- a/tools/localization_analysis/scripts/depth_odometry_parameter_sweep.py +++ b/tools/localization_analysis/scripts/depth_odometry_parameter_sweep.py @@ -33,11 +33,11 @@ import os import numpy as np +import plot_parameter_sweep_results import config_creator import localization_common.utilities as lu import parameter_sweep_utilities -import plot_parameter_sweep_results # Run depth odometry with values. diff --git a/tools/localization_analysis/scripts/depth_odometry_results_plotter.py b/tools/localization_analysis/scripts/depth_odometry_results_plotter.py new file mode 100755 index 0000000000..42f60c5637 --- /dev/null +++ b/tools/localization_analysis/scripts/depth_odometry_results_plotter.py @@ -0,0 +1,161 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +""" +Creates plots for Depth Odometry results compared with groundtruth. +""" + +import argparse +import os +import sys + +import matplotlib + +import message_reader +import plot_conversions +import plotters +import results_savers + +matplotlib.use("pdf") +import csv +import math + +import geometry_msgs +import matplotlib.pyplot as plt +import rosbag +from matplotlib.backends.backend_pdf import PdfPages + + +def plot_depth_odom_results(pdf, results_csv_file, groundtruth_poses, depth_odometries): + absolute_depth_odom_relative_poses = ( + plot_conversions.absolute_poses_from_relative_poses( + plot_conversions.poses_from_depth_odometries(depth_odometries), + groundtruth_poses, + ) + ) + depth_odom_relative_poses_plotter = plotters.MultiPosePlotter( + "Time (s)", "Position (m)", "Depth Odometry vs. Groundtruth Position", True + ) + depth_odom_relative_poses_plotter.add_poses( + "Groundtruth Poses", + groundtruth_poses, + linestyle="None", + marker="o", + markeredgewidth=0.1, + markersize=1.5, + ) + depth_odom_relative_poses_plotter.add_poses( + "Depth Odometry Poses", absolute_depth_odom_relative_poses, linestyle="-" + ) + depth_odom_relative_poses_plotter.plot(pdf) + + num_features_plotter = plot_conversions.num_features_plotter_from_depth_odometries( + depth_odometries + ) + num_features_plotter.plot(pdf) + + runtime_plotter = plot_conversions.runtime_plotter_from_depth_odometries( + depth_odometries + ) + runtime_plotter.plot(pdf) + + +# Loads poses from the provided bagfile, generates plots, and saves results to a pdf and csv file. +# The csv file contains results in (TODO: define format). +# The RMSE rel start and end time define the time range for evaluating the RMSE, in case the bag starts or ends with little/uninteresting movement +# that shouldn't be included in the RMSE calculation. +# The groundtruth bag must have the same start time as other bagfile, otherwise RMSE calculations will be flawed +def load_data_and_create_depth_odom_plots( + bagfile, + output_pdf_file, + results_csv_file="depth_odom_results.csv", + groundtruth_bagfile=None, + rmse_rel_start_time=0, + rmse_rel_end_time=-1, +): + # Load bagfile with depth_odom results + bag = rosbag.Bag(bagfile) + # Load bagfile with groundtruth poses. Assume groundtruth is in the same results bag + # if no separate bagfile for groundtruth is provided. + groundtruth_bag = rosbag.Bag(groundtruth_bagfile) if groundtruth_bagfile else bag + bag_start_time = bag.get_start_time() + + # Load groundtruth poses + # Use sparse mapping poses as groundtruth. + groundtruth_poses = [] + message_reader.load_poses( + groundtruth_poses, "/sparse_mapping/pose", groundtruth_bag, bag_start_time + ) + + depth_odometries = message_reader.load_depth_odometries( + "/loc/depth/odom", bag, bag_start_time + ) + bag.close() + + with PdfPages(output_pdf_file) as pdf: + plot_depth_odom_results( + pdf, results_csv_file, groundtruth_poses, depth_odometries + ) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument("bagfile", help="Input bagfile.") + parser.add_argument( + "--output-file", default="depth_odom_output.pdf", help="Output pdf file." + ) + parser.add_argument( + "--results-csv-file", + default="depth_odom_results.csv", + help="Output csv file containing results stats.", + ) + parser.add_argument( + "-g", + "--groundtruth-bagfile", + default=None, + help="Optional bagfile containing groundtruth poses to use as a comparison for poses in the input bagfile. If none provided, sparse mapping poses are used as groundtruth from the input bagfile if available.", + ) + parser.add_argument( + "--rmse-rel-start-time", + type=float, + default=0, + help="Optional start time for plots.", + ) + parser.add_argument( + "--rmse-rel-end-time", + type=float, + default=-1, + help="Optional end time for plots.", + ) + args = parser.parse_args() + if not os.path.isfile(args.bagfile): + print(("Bag file " + args.bagfile + " does not exist.")) + sys.exit() + if args.groundtruth_bagfile and not os.path.isfile(args.groundtruth_bagfile): + print(("Groundtruth Bag file " + args.groundtruth_bagfile + " does not exist.")) + sys.exit() + load_data_and_create_depth_odom_plots( + args.bagfile, + args.output_file, + args.results_csv_file, + args.groundtruth_bagfile, + args.rmse_rel_start_time, + args.rmse_rel_end_time, + ) diff --git a/tools/localization_analysis/scripts/get_average_opt_and_update_times.py b/tools/localization_analysis/scripts/get_average_opt_and_update_times.py deleted file mode 100755 index 2f57aeb0d4..0000000000 --- a/tools/localization_analysis/scripts/get_average_opt_and_update_times.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. -""" -Prints stats for the optimization and update times of the graph localizer -from the recorded graph_state messages in the provided bagfile. -""" - - -import argparse -import os -import sys - -import numpy as np -import rosbag - - -def get_average_opt_and_update_times(bagfile): - with rosbag.Bag(bagfile, "r") as bag: - optimization_times = [] - update_times = [] - for _, msg, _ in bag.read_messages(["/graph_loc/state"]): - optimization_times.append(msg.optimization_time) - update_times.append(msg.update_time) - - mean_optimization_time = np.mean(optimization_times) - min_optimization_time = np.min(optimization_times) - max_optimization_time = np.max(optimization_times) - stddev_optimization_time = np.std(optimization_times) - print(("Mean optimization time: " + str(mean_optimization_time))) - print(("Min optimization time: " + str(min_optimization_time))) - print(("Max optimization time: " + str(max_optimization_time))) - print(("Stddev optimization time: " + str(stddev_optimization_time))) - - mean_update_time = np.mean(update_times) - min_update_time = np.min(update_times) - max_update_time = np.max(update_times) - stddev_update_time = np.std(update_times) - print(("Mean update time: " + str(mean_update_time))) - print(("Min update time: " + str(min_update_time))) - print(("Max update time: " + str(max_update_time))) - print(("Stddev update time: " + str(stddev_update_time))) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser( - description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter - ) - parser.add_argument("bagfile", help="Input bagfile.") - args = parser.parse_args() - if not os.path.isfile(args.bagfile): - print(("Bag file " + args.bagfile + " does not exist.")) - sys.exit() - - get_average_opt_and_update_times(args.bagfile) diff --git a/tools/localization_analysis/scripts/groundtruth_sweep.py b/tools/localization_analysis/scripts/groundtruth_sweep.py index 299fc5f56b..f0479a58f7 100755 --- a/tools/localization_analysis/scripts/groundtruth_sweep.py +++ b/tools/localization_analysis/scripts/groundtruth_sweep.py @@ -29,9 +29,9 @@ import os import sys -import localization_common.utilities as lu import multiprocessing_helpers -import utilities + +import localization_common.utilities as lu class GroundtruthParams(object): @@ -58,14 +58,7 @@ def load_params(param_file): reader = csv.reader(param_csvfile, delimiter=" ") for row in reader: groundtruth_params_list.append( - GroundtruthParams( - row[0], - row[1], - row[2], - row[3], - row[4], - row[5], - ) + GroundtruthParams(row[0], row[1], row[2], row[3], row[4], row[5]) ) return groundtruth_params_list diff --git a/tools/localization_analysis/scripts/imu_analyzer.py b/tools/localization_analysis/scripts/imu_analyzer.py index b204cb7f6a..499fbcbd13 100755 --- a/tools/localization_analysis/scripts/imu_analyzer.py +++ b/tools/localization_analysis/scripts/imu_analyzer.py @@ -27,9 +27,9 @@ import os import sys +import imu_measurements import matplotlib -import imu_measurements import plot_helpers matplotlib.use("pdf") diff --git a/tools/localization_analysis/scripts/loc_results_plotter.py b/tools/localization_analysis/scripts/loc_results_plotter.py new file mode 100755 index 0000000000..eab6de7596 --- /dev/null +++ b/tools/localization_analysis/scripts/loc_results_plotter.py @@ -0,0 +1,333 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +""" +Creates plots for Localization results. Adds plots for poses and detectected +features counts. Optionally plots groundtruth poses along with other pose data if groundtruth is provided. +""" + +import argparse +import os +import sys + +import matplotlib + +import message_reader +import plot_conversions +import plotters +import results_savers + +matplotlib.use("pdf") +import csv +import math + +import geometry_msgs +import matplotlib.pyplot as plt +import rosbag +from matplotlib.backends.backend_pdf import PdfPages + + +def plot_loc_results( + pdf, + results_csv_file, + groundtruth_poses, + graph_loc_states, + extrapolated_loc_states, + ar_tag_poses, + imu_accelerations, +): + poses_plotter = plotters.MultiPosePlotter( + "Time (s)", "Position (m)", "Loc vs. Groundtruth Position", True + ) + poses_plotter.add_poses( + "Groundtruth Poses", + groundtruth_poses, + linestyle="None", + marker="o", + markeredgewidth=0.1, + markersize=1.5, + ) + + graph_loc_poses = plot_conversions.poses_from_graph_loc_states(graph_loc_states) + poses_plotter.add_poses("Graph Loc Poses", graph_loc_poses, linestyle="-") + + poses_plotter.add_poses( + "AR Tag Poses", + ar_tag_poses, + linestyle="None", + marker="x", + markeredgewidth=0.1, + markersize=1.5, + ) + poses_plotter.plot(pdf) + results_savers.save_rmse( + graph_loc_poses, groundtruth_poses, results_csv_file, pdf, "Graph Loc Poses" + ) + + if extrapolated_loc_states: + extrapolated_poses_plotter = plotters.MultiPosePlotter( + "Time (s)", + "Position (m)", + "Extrapolated Loc vs. Groundtruth Position", + True, + ) + extrapolated_poses_plotter.add_poses( + "Groundtruth Poses", + groundtruth_poses, + linestyle="None", + marker="o", + markeredgewidth=0.1, + markersize=1.5, + ) + + extrapolated_loc_poses = plot_conversions.poses_from_extrapolated_loc_states( + extrapolated_loc_states + ) + extrapolated_poses_plotter.add_poses( + "Extrapolated Loc Poses", extrapolated_loc_poses, linestyle="-" + ) + extrapolated_poses_plotter.plot(pdf) + results_savers.save_rmse( + extrapolated_loc_poses, + groundtruth_poses, + results_csv_file, + pdf, + "Extrapolated Loc Poses", + ) + + extrapolated_velocities_plotter = plotters.MultiVector3dPlotter( + "Time (s)", "Velocity (m/s)", "Extrapolated Velocities", True + ) + extrapolated_velocity_plotter = ( + plot_conversions.velocity_plotter_from_extrapolated_loc_states( + extrapolated_loc_states + ) + ) + extrapolated_velocities_plotter.add(extrapolated_velocity_plotter) + extrapolated_velocities_plotter.plot(pdf) + + extrapolated_integrated_velocity_poses = plot_conversions.absolute_poses_from_integrated_extrapolated_loc_state_velocities( + extrapolated_loc_states, groundtruth_poses + ) + extrapolated_integrated_velocity_poses_plotter = plotters.MultiPosePlotter( + "Time (s)", + "Position (m)", + "Extrapolated Loc Integrated Velocities vs. Groundtruth Position", + True, + ) + extrapolated_integrated_velocity_poses_plotter.add_poses( + "Groundtruth Poses", + groundtruth_poses, + linestyle="None", + marker="o", + markeredgewidth=0.1, + markersize=1.5, + ) + extrapolated_integrated_velocity_poses_plotter.add_poses( + "Extrapolted Loc Integrated Velocity Poses", + extrapolated_integrated_velocity_poses, + linestyle="-", + ) + extrapolated_integrated_velocity_poses_plotter.plot_positions(pdf) + results_savers.save_rmse( + extrapolated_integrated_velocity_poses, + groundtruth_poses, + results_csv_file, + pdf, + "Extrapolated Integrated Velocity Poses", + ) + + extrapolated_accelerations_plotter = plotters.MultiVector3dPlotter( + "Time (s)", "Acceleration (m/s^2)", "Bias Corrected Accelerations", True + ) + extrapolated_acceleration_plotter = ( + plot_conversions.acceleration_plotter_from_extrapolated_loc_states( + extrapolated_loc_states + ) + ) + extrapolated_accelerations_plotter.add(extrapolated_acceleration_plotter) + extrapolated_accelerations_plotter.plot(pdf) + + if imu_accelerations: + imu_accelerations_plotter = plotters.MultiVector3dPlotter( + "Time (s)", "Acceleration (m/s^2)", "Raw IMU Accelerations", True + ) + imu_acceleration_plotter = ( + plot_conversions.acceleration_plotter_from_imu_accelerations( + imu_accelerations + ) + ) + imu_accelerations_plotter.add(imu_acceleration_plotter) + imu_accelerations_plotter.plot(pdf) + + ml_count_plotter = plot_conversions.ml_feature_count_plotter_from_graph_loc_states( + graph_loc_states + ) + ml_count_plotter.plot(pdf) + + ar_count_plotter = plot_conversions.ar_feature_count_plotter_from_graph_loc_states( + graph_loc_states + ) + ar_count_plotter.plot(pdf) + + ml_num_pose_factors_plotter = ( + plot_conversions.ml_pose_factor_count_plotter_from_graph_loc_states( + graph_loc_states + ) + ) + ml_num_pose_factors_plotter.plot(pdf) + + ml_num_projection_factors_plotter = ( + plot_conversions.ml_projection_factor_count_plotter_from_graph_loc_states( + graph_loc_states + ) + ) + ml_num_projection_factors_plotter.plot(pdf) + + num_states_plotter = plot_conversions.num_states_plotter_from_states( + graph_loc_states + ) + num_states_plotter.plot(pdf) + + duration_plotter = plot_conversions.duration_plotter_from_states(graph_loc_states) + duration_plotter.plot(pdf) + + optimization_time_plotter = plot_conversions.optimization_time_plotter_from_states( + graph_loc_states + ) + optimization_time_plotter.plot(pdf) + + update_time_plotter = plot_conversions.update_time_plotter_from_states( + graph_loc_states + ) + update_time_plotter.plot(pdf) + + optimization_iterations_plotter = ( + plot_conversions.optimization_iterations_plotter_from_states(graph_loc_states) + ) + optimization_iterations_plotter.plot(pdf) + + +# Loads poses from the provided bagfile, generates plots, and saves results to a pdf and csv file. +# The csv file contains results in (TODO: define format). +# The RMSE rel start and end time define the time range for evaluating the RMSE, in case the bag starts or ends with little/uninteresting movement +# that shouldn't be included in the RMSE calculation. +# The groundtruth bag must have the same start time as other bagfile, otherwise RMSE calculations will be flawed +def load_data_and_create_loc_plots( + bagfile, + output_pdf_file, + results_csv_file="loc_results.csv", + groundtruth_bagfile=None, + rmse_rel_start_time=0, + rmse_rel_end_time=-1, +): + # Load bagfile with localization results + bag = rosbag.Bag(bagfile) + # Load bagfile with groundtruth poses. Assume groundtruth is in the same results bag + # if no separate bagfile for groundtruth is provided. + groundtruth_bag = rosbag.Bag(groundtruth_bagfile) if groundtruth_bagfile else bag + bag_start_time = bag.get_start_time() + + # Load groundtruth poses + # Use sparse mapping poses as groundtruth. + groundtruth_poses = [] + message_reader.load_poses( + groundtruth_poses, "/sparse_mapping/pose", groundtruth_bag, bag_start_time + ) + + # Load graph localization states + graph_loc_states = [] + message_reader.load_graph_loc_states( + graph_loc_states, "/graph_loc/state", bag, bag_start_time + ) + + # Load extrapolated localization states + extrapolated_loc_states = [] + message_reader.load_extrapolated_loc_states( + extrapolated_loc_states, "/gnc/ekf", bag, bag_start_time + ) + + # Load AR Tag poses + ar_tag_poses = [] + message_reader.load_poses(ar_tag_poses, "/ar_tag/pose", bag, bag_start_time) + + # Load IMU data + imu_accelerations = [] + message_reader.load_imu_accelerations( + imu_accelerations, "/hw/imu", bag, bag_start_time + ) + bag.close() + + with PdfPages(output_pdf_file) as pdf: + plot_loc_results( + pdf, + results_csv_file, + groundtruth_poses, + graph_loc_states, + extrapolated_loc_states, + ar_tag_poses, + imu_accelerations, + ) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument("bagfile", help="Input bagfile.") + parser.add_argument( + "--output-file", default="loc_output.pdf", help="Output pdf file." + ) + parser.add_argument( + "--results-csv-file", + default="loc_results.csv", + help="Output csv file containing localization stats.", + ) + parser.add_argument( + "-g", + "--groundtruth-bagfile", + default=None, + help="Optional bagfile containing groundtruth poses to use as a comparison for poses in the input bagfile. If none provided, sparse mapping poses are used as groundtruth from the input bagfile if available.", + ) + parser.add_argument( + "--rmse-rel-start-time", + type=float, + default=0, + help="Optional start time for plots.", + ) + parser.add_argument( + "--rmse-rel-end-time", + type=float, + default=-1, + help="Optional end time for plots.", + ) + args = parser.parse_args() + if not os.path.isfile(args.bagfile): + print(("Bag file " + args.bagfile + " does not exist.")) + sys.exit() + if args.groundtruth_bagfile and not os.path.isfile(args.groundtruth_bagfile): + print(("Groundtruth Bag file " + args.groundtruth_bagfile + " does not exist.")) + sys.exit() + load_data_and_create_loc_plots( + args.bagfile, + args.output_file, + args.results_csv_file, + args.groundtruth_bagfile, + args.rmse_rel_start_time, + args.rmse_rel_end_time, + ) diff --git a/tools/localization_analysis/scripts/loc_states.py b/tools/localization_analysis/scripts/loc_states.py deleted file mode 100644 index 1623a5ad55..0000000000 --- a/tools/localization_analysis/scripts/loc_states.py +++ /dev/null @@ -1,72 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import poses -import vector3ds - - -class LocStates(poses.Poses): - def __init__(self, loc_type, topic): - super(LocStates, self).__init__(loc_type, topic) - self.num_detected_of_features = [] - self.num_detected_ml_features = [] - self.num_of_factors = [] - self.num_ml_projection_factors = [] - self.accelerations = vector3ds.Vector3ds() - self.velocities = vector3ds.Vector3ds() - self.angular_velocities = vector3ds.Vector3ds() - self.accelerometer_biases = vector3ds.Vector3ds() - self.gyro_biases = vector3ds.Vector3ds() - self.position_covariances = vector3ds.Vector3ds() - self.orientation_covariances = vector3ds.Vector3ds() - self.velocity_covariances = vector3ds.Vector3ds() - self.accelerometer_bias_covariances = vector3ds.Vector3ds() - self.gyro_bias_covariances = vector3ds.Vector3ds() - - def add_loc_state(self, msg, bag_start_time=0): - self.add_pose_msg(msg.pose, msg.header.stamp, bag_start_time) - if hasattr(msg, "num_detected_of_features"): - self.num_detected_of_features.append(msg.num_detected_of_features) - if hasattr(msg, "num_detected_ml_features"): - self.num_detected_ml_features.append(msg.num_detected_ml_features) - if hasattr(msg, "num_of_factors"): - self.num_of_factors.append(msg.num_of_factors) - if hasattr(msg, "num_ml_projection_factors"): - self.num_ml_projection_factors.append(msg.num_ml_projection_factors) - if hasattr(msg, "accel"): - self.accelerations.add_vector3d(msg.accel) - self.velocities.add_vector3d(msg.velocity) - if hasattr(msg, "omega"): - self.angular_velocities.add_vector3d(msg.omega) - self.accelerometer_biases.add_vector3d(msg.accel_bias) - self.gyro_biases.add_vector3d(msg.gyro_bias) - # See GraphState.msg or EkfState.msg for cov_diag offsets - self.position_covariances.add( - msg.cov_diag[12], msg.cov_diag[13], msg.cov_diag[14] - ) - self.orientation_covariances.add( - msg.cov_diag[0], msg.cov_diag[1], msg.cov_diag[2] - ) - self.velocity_covariances.add(msg.cov_diag[6], msg.cov_diag[7], msg.cov_diag[8]) - self.accelerometer_bias_covariances.add( - msg.cov_diag[9], msg.cov_diag[10], msg.cov_diag[11] - ) - self.gyro_bias_covariances.add( - msg.cov_diag[3], msg.cov_diag[4], msg.cov_diag[5] - ) diff --git a/tools/localization_analysis/scripts/make_groundtruth.py b/tools/localization_analysis/scripts/make_groundtruth.py index af6f24cfa8..fa93646649 100755 --- a/tools/localization_analysis/scripts/make_groundtruth.py +++ b/tools/localization_analysis/scripts/make_groundtruth.py @@ -69,9 +69,9 @@ ) parser.add_argument( "--generate-image-features", - dest="use_image_features", - action="store_false", - help="Use image features msgs from bagfile or generate features from images.", + dest="generate_image_features", + action="store_true", + help="Generate image features instead of using image features msgs from bagfile.", ) parser.add_argument( "--no-histogram-equalization", @@ -120,7 +120,7 @@ groundtruth_pdf = "groundtruth.pdf" groundtruth_csv = "groundtruth.csv" make_groundtruth_command = ( - "rosrun localization_analysis run_graph_bag_and_plot_results.py " + "rosrun localization_analysis run_offline_replay_and_plot_results.py " + bagfile + " " + groundtruth_map_file @@ -128,21 +128,31 @@ + args.image_topic + " -o " + groundtruth_bag - + " --output-file " + + " --loc-output-file " + + "loc_" + + groundtruth_pdf + + " --vio-output-file " + + "vio_" + groundtruth_pdf - + " --output-csv-file " + + " --loc-results-csv-file " + + "loc_" + + groundtruth_csv + + " --vio-results-csv-file " + + "vio_" + groundtruth_csv + " --generate-image-features" ) lu.run_command_and_save_output(make_groundtruth_command, "make_groundtruth.txt") - os.rename("run_graph_bag_command.txt", "groundtruth_run_graph_bag_command.txt") + os.rename( + "run_offline_replay_command.txt", "groundtruth_run_offline_replay_command.txt" + ) if args.loc_map != "": loc_results_bag = bag_prefix + "_results.bag" loc_pdf = "loc_results.pdf" loc_csv = "loc_results.csv" get_loc_results_command = ( - "rosrun localization_analysis run_graph_bag_and_plot_results.py " + "rosrun localization_analysis run_offline_replay_and_plot_results.py " + bagfile + " " + args.loc_map @@ -150,14 +160,24 @@ + args.image_topic + " -o " + loc_results_bag - + " --output-file " + + " --loc-output-file " + + "loc_" + loc_pdf - + " --output-csv-file " + + " --vio-output-file " + + "vio_" + + loc_pdf + + " --loc-results-csv-file " + + "loc_" + + loc_csv + + " --vio-results-csv-file " + + "vio_" + loc_csv + " -g " + groundtruth_bag ) - if not args.use_image_features: + if args.generate_image_features: get_loc_results_command += " --generate-image-features" lu.run_command_and_save_output(get_loc_results_command, "get_loc_results.txt") - os.rename("run_graph_bag_command.txt", "loc_run_graph_bag_command.txt") + os.rename( + "run_offline_replay_command.txt", "loc_run_offline_replay_command.txt" + ) diff --git a/tools/localization_analysis/scripts/message_conversions.py b/tools/localization_analysis/scripts/message_conversions.py new file mode 100644 index 0000000000..6998c6df42 --- /dev/null +++ b/tools/localization_analysis/scripts/message_conversions.py @@ -0,0 +1,218 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +import numpy as np +import scipy.spatial.transform + +import states + + +# Subtract the bag start time from the timestamp +# to make start time relative to the bag start time +def relative_timestamp(timestamp, bag_start_time): + return timestamp.secs + 1e-9 * timestamp.nsecs - bag_start_time + + +# Helper function to return orientation and position from a pose msg. +def orientation_position_from_msg(pose_msg): + orientation = scipy.spatial.transform.Rotation.from_quat( + [ + pose_msg.pose.orientation.x, + pose_msg.pose.orientation.y, + pose_msg.pose.orientation.z, + pose_msg.pose.orientation.w, + ] + ) + position = states.Position( + [pose_msg.pose.position.x, pose_msg.pose.position.y, pose_msg.pose.position.z] + ) + return orientation, position + + +# Helper function to return orientation, position, and timestamp from a pose msg. +def orientation_position_timestamp_from_msg(pose_msg, bag_start_time=0): + orientation, position = orientation_position_from_msg(pose_msg) + timestamp = relative_timestamp(pose_msg.header.stamp, bag_start_time) + return orientation, position, timestamp + + +# Create a timestamped pose with covariance from a pose msg using relative bag time. +def timestamped_pose_from_msg(pose_msg, bag_start_time=0): + orientation, position, timestamp = orientation_position_timestamp_from_msg( + pose_msg, bag_start_time + ) + return states.TimestampedPose(orientation, position, timestamp) + + +# Create a timestamped pose with covariance from an odometry msg using relative bag time. +def timestamped_pose_from_odometry_msg(odometry_msg, bag_start_time=0): + orientation, position = orientation_position_from_msg( + odometry_msg.body_F_source_T_target + ) + timestamp = relative_timestamp(odometry_msg.source_time, bag_start_time) + return states.TimestampedPose(orientation, position, timestamp) + + +# Create a pose from a pose msg +def pose_from_msg(pose_msg): + orientation, position = orientation_position_from_msg(pose_msg) + return states.Pose(orientation, position) + + +# Create a pose with covariance from a pose msg +def pose_with_covariance_from_msg(pose_msg): + orientation, position = orientation_position_from_msg(pose_msg) + return states.PoseWithCovariance(orientation, position, pose_msg.covariance) + + +# Create a timestamped pose with covariance from a pose msg using relative bag time. +def timestamped_pose_with_covariance_from_msg(pose_msg, bag_start_time=0): + orientation, position, timestamp = orientation_position_timestamp_from_msg( + pose_msg, bag_start_time + ) + return states.TimestampedPoseWithCovariance( + orientation, position, pose_msgs.covariance, timestamp + ) + + +# Create a timestamped velocity from a velocity msg using relative bag time. +def timestamped_velocity_from_msg(velocity_msg, bag_start_time=0): + timestamp = relative_timestamp(velocity_msg.header.stamp, bag_start_time) + return states.TimestampedVelocity( + velocity_msg.velocity.x, + velocity_msg.velocity.y, + velocity_msg.velocity.z, + timestamp, + ) + + +# Create a velocity from a velocity msg. +def velocity_from_msg(velocity_msg): + return states.Velocity( + velocity_msg.velocity.x, velocity_msg.velocity.y, velocity_msg.velocity.z + ) + + +# Create an acceleration from an acceleration msg. +def acceleration_from_msg(acceleration_msg): + return states.Acceleration( + acceleration_msg.accel.x, acceleration_msg.accel.y, acceleration_msg.accel.z + ) + + +# Create an acceleration from an imu msg. +def timestamped_acceleration_from_imu_msg(imu_msg, bag_start_time=0): + timestamp = relative_timestamp(imu_msg.header.stamp, bag_start_time) + return states.TimestampedAcceleration( + imu_msg.linear_acceleration.x, + imu_msg.linear_acceleration.y, + imu_msg.linear_acceleration.z, + timestamp, + ) + + +# Create a timestamped velocity from a velocity msg using relative bag time. +def velocity_with_covariance_from_msg(velocity_msg, bag_start_time=0): + return states.VelocityWithCovariance( + velocity_msg.velocity.x, + velocity_msg.velocity.y, + velocity_msg.velocity.z, + velocity_msg.covariance, + ) + + +# Create a depth odometry object from a msg using relative bag time. +def depth_odometry_from_msg(msg, bag_start_time=0): + depth_odometry = states.DepthOdometry() + depth_odometry.timestamp = relative_timestamp(msg.header.stamp, bag_start_time) + depth_odometry.pose_with_covariance = pose_with_covariance_from_msg( + msg.odometry.body_F_source_T_target + ) + depth_odometry.num_features = len(msg.correspondences) + depth_odometry.runtime = msg.runtime + return depth_odometry + + +# Create a graph vio state from a msg using relative bag time. +def graph_vio_state_from_msg(msg, bag_start_time=0): + graph_vio_state = states.GraphVIOState() + # TODO: load all combined nav states??? + graph_vio_state.timestamp = relative_timestamp(msg.header.stamp, bag_start_time) + latest_state = msg.combined_nav_states.combined_nav_states[-1] + graph_vio_state.pose_with_covariance = pose_with_covariance_from_msg( + latest_state.pose.pose + ) + graph_vio_state.velocity_with_covariance = velocity_with_covariance_from_msg( + latest_state.velocity + ) + # TODO: make function for this? + accelerometer_bias = states.AccelerometerBias( + latest_state.imu_bias.accelerometer_bias.x, + latest_state.imu_bias.accelerometer_bias.y, + latest_state.imu_bias.accelerometer_bias.z, + ) + gyro_bias = states.GyroscopeBias( + latest_state.imu_bias.gyroscope_bias.x, + latest_state.imu_bias.gyroscope_bias.y, + latest_state.imu_bias.gyroscope_bias.z, + ) + # TODO: load covariance? + graph_vio_state.imu_bias_with_covariance = states.ImuBias( + accelerometer_bias, gyro_bias + ) + graph_vio_state.num_detected_of_features = msg.num_detected_of_features + graph_vio_state.num_of_factors = msg.num_of_factors + graph_vio_state.num_depth_factors = msg.num_depth_factors + graph_vio_state.num_states = msg.num_states + graph_vio_state.standstill = int(msg.standstill) + graph_vio_state.optimization_iterations = msg.optimization_iterations + graph_vio_state.optimization_time = msg.optimization_time + graph_vio_state.update_time = msg.update_time + graph_vio_state.duration = msg.duration + return graph_vio_state + + +# Create a graph loc state from a msg using relative bag time. +def graph_loc_state_from_msg(msg, bag_start_time=0): + graph_loc_state = states.GraphLocState() + graph_loc_state.timestamp = relative_timestamp(msg.header.stamp, bag_start_time) + graph_loc_state.pose_with_covariance = pose_with_covariance_from_msg(msg.pose) + graph_loc_state.num_detected_ar_features = msg.num_detected_ar_features + graph_loc_state.num_detected_ml_features = msg.num_detected_ml_features + graph_loc_state.optimization_iterations = msg.optimization_iterations + graph_loc_state.optimization_time = msg.optimization_time + graph_loc_state.update_time = msg.update_time + graph_loc_state.duration = msg.duration + graph_loc_state.num_factors = msg.num_factors + graph_loc_state.num_ml_projection_factors = msg.num_ml_projection_factors + graph_loc_state.num_ml_pose_factors = msg.num_ml_pose_factors + graph_loc_state.num_states = msg.num_states + return graph_loc_state + + +# Create a extrapolated loc state from a msg using relative bag time. +def extrapolated_loc_state_from_msg(msg, bag_start_time=0): + extrapolated_loc_state = states.ExtrapolatedLocState() + extrapolated_loc_state.timestamp = relative_timestamp( + msg.header.stamp, bag_start_time + ) + extrapolated_loc_state.pose = pose_from_msg(msg) + extrapolated_loc_state.velocity = velocity_from_msg(msg) + extrapolated_loc_state.acceleration = acceleration_from_msg(msg) + return extrapolated_loc_state diff --git a/tools/localization_analysis/scripts/message_reader.py b/tools/localization_analysis/scripts/message_reader.py new file mode 100755 index 0000000000..504c513de4 --- /dev/null +++ b/tools/localization_analysis/scripts/message_reader.py @@ -0,0 +1,94 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +""" +Functions for loading messages from ros bags. +""" + +import rosbag + +import message_conversions + + +# Load poses from a bag file for a given topic. +# Start at the provided bag start time. +def load_poses(timestamped_poses, topic, bag, bag_start_time): + for msg_topic, msg, t in bag.read_messages(topic): + if msg_topic == topic: + timestamped_poses.append( + message_conversions.timestamped_pose_from_msg(msg, bag_start_time) + ) + + +# Start at the provided bag start time. +def load_depth_odometries(topic, bag, bag_start_time): + depth_odometries = [] + for msg_topic, msg, t in bag.read_messages(topic): + if msg_topic == topic: + depth_odometries.append( + message_conversions.depth_odometry_from_msg(msg, bag_start_time) + ) + return depth_odometries + + +# Load graph vio states from a bag file for a given topic. +# Start at the provided bag start time. +def load_graph_vio_states(graph_vio_states, topic, bag, bag_start_time): + for msg_topic, msg, t in bag.read_messages(topic): + if msg_topic == topic: + graph_vio_states.append( + message_conversions.graph_vio_state_from_msg(msg, bag_start_time) + ) + + +# Load graph loc states from a bag file for a given topic. +# Start at the provided bag start time. +def load_graph_loc_states(graph_loc_states, topic, bag, bag_start_time): + for msg_topic, msg, t in bag.read_messages(topic): + if msg_topic == topic: + graph_loc_states.append( + message_conversions.graph_loc_state_from_msg(msg, bag_start_time) + ) + + +# Load extrapolated loc states from a bag file for a given topic. +# Start at the provided bag start time. +def load_extrapolated_loc_states(extrapolated_loc_states, topic, bag, bag_start_time): + for msg_topic, msg, t in bag.read_messages(topic): + if msg_topic == topic: + extrapolated_loc_states.append( + message_conversions.extrapolated_loc_state_from_msg(msg, bag_start_time) + ) + + +# Load IMU accelerations from a bag file for a given topic. +# Start at the provided bag start time. +def load_imu_accelerations(imu_accelerations, topic, bag, bag_start_time): + for msg_topic, msg, t in bag.read_messages(topic): + if msg_topic == topic: + imu_accelerations.append( + message_conversions.timestamped_acceleration_from_imu_msg( + msg, bag_start_time + ) + ) + + +# Returns if the topic is available in the bag file +def has_topic(bag, topic): + topics = bag.get_type_and_topic_info().topics + return topic in topics diff --git a/tools/localization_analysis/scripts/orientations.py b/tools/localization_analysis/scripts/orientations.py deleted file mode 100644 index 2e851702a7..0000000000 --- a/tools/localization_analysis/scripts/orientations.py +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. -import scipy.spatial.transform - - -class Orientations: - def __init__(self): - self.yaws = [] - self.pitches = [] - self.rolls = [] - - def get_euler(self, index): - return [self.yaws[index], self.pitches[index], self.rolls[index]] - - def get_rotation(self, index): - return scipy.spatial.transform.Rotation.from_euler( - "ZYX", self.get_euler(index), degrees=True - ) - - def add_euler(self, euler_angles): - self.add(euler_angles[0], euler_angles[1], euler_angles[2]) - - def add(self, yaw, pitch, roll): - self.yaws.append(yaw) - self.pitches.append(pitch) - self.rolls.append(roll) diff --git a/tools/localization_analysis/scripts/plot_parameter_sweep_results.py b/tools/localization_analysis/scripts/parameter_sweep_results_plotter.py similarity index 61% rename from tools/localization_analysis/scripts/plot_parameter_sweep_results.py rename to tools/localization_analysis/scripts/parameter_sweep_results_plotter.py index 7f9f68a8d0..a42e38b079 100755 --- a/tools/localization_analysis/scripts/plot_parameter_sweep_results.py +++ b/tools/localization_analysis/scripts/parameter_sweep_results_plotter.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. @@ -31,10 +31,10 @@ from matplotlib.backends.backend_pdf import PdfPages -def create_plot(pdf, csv_file, value_combos_file, prefix=""): +def create_plot(pdf, csv_file, value_combos_file, rmse_type): dataframe = pd.read_csv(csv_file) try: - rmses = dataframe[prefix + "rmse"] + rmses = dataframe[rmse_type] except: return x_axis_vals = [] @@ -70,8 +70,8 @@ def create_plot(pdf, csv_file, value_combos_file, prefix=""): markeredgewidth=0.1, markersize=10.5, ) - plt.ylabel(prefix + " RMSE") - plt.title(prefix + " RMSE vs. " + x_axis_label) + plt.ylabel(rmse_type) + plt.title(rmse_type + " vs. " + x_axis_label) x_range = x_axis_vals[len(x_axis_vals) - 1] - x_axis_vals[0] first_x_val = x_axis_vals[0] last_x_val = x_axis_vals[len(x_axis_vals) - 1] @@ -81,7 +81,7 @@ def create_plot(pdf, csv_file, value_combos_file, prefix=""): and last_x_val != 0 and abs(math.log10(last_x_val) - math.log10(first_x_val)) > 3 ): - plt.xscale("log", basex=10) + plt.xscale("log", base=10) # Extend x axis on either side using a log scale to make data more visible if first_x_val < last_x_val: plt.xlim([first_x_val * 0.1, last_x_val * 10.0]) @@ -100,34 +100,9 @@ def create_plot(pdf, csv_file, value_combos_file, prefix=""): plt.close() -# TODO(rsoussan): Make this more generic, don't hardcode each RMSE type def create_plots(output_file, csv_file, value_combos_file): + dataframe = pd.read_csv(csv_file) + rmses = dataframe.columns with PdfPages(output_file) as pdf: - # Graph RMSEs - create_plot(pdf, csv_file, value_combos_file) - create_plot(pdf, csv_file, value_combos_file, "orientation_") - create_plot(pdf, csv_file, value_combos_file, "integrated_") - create_plot(pdf, csv_file, value_combos_file, "rel_") - create_plot(pdf, csv_file, value_combos_file, "rel_orientation_") - create_plot(pdf, csv_file, value_combos_file, "rel_integrated_") - # IMU Augmented RMSEs - create_plot(pdf, csv_file, value_combos_file, "imu_augmented_") - create_plot(pdf, csv_file, value_combos_file, "imu_augmented_orientation_") - create_plot(pdf, csv_file, value_combos_file, "imu_augmented_integrated_") - create_plot(pdf, csv_file, value_combos_file, "rel_imu_augmented_") - create_plot(pdf, csv_file, value_combos_file, "rel_imu_augmented_orientation_") - create_plot(pdf, csv_file, value_combos_file, "rel_imu_augmented_integrated_") - - # IMU Bias Tester RMSEs - create_plot(pdf, csv_file, value_combos_file, "imu_bias_tester_") - create_plot(pdf, csv_file, value_combos_file, "imu_bias_tester_orientation_") - create_plot(pdf, csv_file, value_combos_file, "rel_imu_bias_tester_") - create_plot( - pdf, csv_file, value_combos_file, "rel_imu_bias_tester_orientation_" - ) - - # Depth Odometry RMSEs - create_plot(pdf, csv_file, value_combos_file, "depth_odometry_") - create_plot(pdf, csv_file, value_combos_file, "depth_odometry_orientation_") - create_plot(pdf, csv_file, value_combos_file, "rel_depth_odometry_") - create_plot(pdf, csv_file, value_combos_file, "rel_depth_odometry_orientation_") + for rmse_type in rmses: + create_plot(pdf, csv_file, value_combos_file, rmse_type) diff --git a/tools/localization_analysis/scripts/parameter_sweep_utilities.py b/tools/localization_analysis/scripts/parameter_sweep_utilities.py index 5e71fee0fa..64fb7bddd7 100755 --- a/tools/localization_analysis/scripts/parameter_sweep_utilities.py +++ b/tools/localization_analysis/scripts/parameter_sweep_utilities.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. @@ -21,16 +21,18 @@ import itertools import os -import average_results +import results_averager -def concat_results(job_ids, directory, stats_filename): +def concat_results(job_ids, directory, stats_filename, prefix): results_csv_files = [] for job_id in job_ids: results_csv_files.append(os.path.join(directory, str(job_id), stats_filename)) # Results are written in job id order - combined_results = average_results.combined_results(results_csv_files) - combined_results_file = os.path.join(directory, "param_sweep_combined_results.csv") + combined_results = results_averager.combined_results(results_csv_files) + combined_results_file = os.path.join( + directory, prefix + "_param_sweep_combined_results.csv" + ) combined_results.to_csv(combined_results_file, index=False) diff --git a/tools/localization_analysis/scripts/parameter_sweep.py b/tools/localization_analysis/scripts/parameter_sweeper.py similarity index 57% rename from tools/localization_analysis/scripts/parameter_sweep.py rename to tools/localization_analysis/scripts/parameter_sweeper.py index d4f00ebbc1..016ea60796 100755 --- a/tools/localization_analysis/scripts/parameter_sweep.py +++ b/tools/localization_analysis/scripts/parameter_sweeper.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. @@ -17,9 +17,9 @@ # License for the specific language governing permissions and limitations # under the License. """ -Runs a parameter sweep for the graph localizer using the provided bagfile. +Runs a parameter sweep for the graph localizer or graph vio using the provided bagfile. Parameters to sweep on are set in the make_value_ranges function and applied to -the graph_localizer.config file. +the graph_localizer/vio.config file. All combinations of provided parameters are used for the sweep and the results are plotted for various RMSEs. """ @@ -33,15 +33,35 @@ import os import numpy as np +import rospkg -import average_results import config_creator import localization_common.utilities as lu +import parameter_sweep_results_plotter import parameter_sweep_utilities -import plot_parameter_sweep_results -# Run graph localizer with values. +# Create new config file with substituted values +def make_config_file(config_filename, new_output_dir, config_path, values, value_names): + config_filepath = os.path.join(config_path, config_filename) + new_config_filepath = os.path.join(new_output_dir, config_filename) + config_creator.make_config( + values, value_names, config_filepath, new_config_filepath + ) + + +def create_results_plots(output_dir, prefix): + combined_results_file = os.path.join( + output_dir, prefix + "_param_sweep_combined_results.csv" + ) + value_combos_file = os.path.join(output_dir, "all_value_combos.csv") + results_pdf_file = os.path.join(output_dir, prefix + "_param_sweep_results.pdf") + parameter_sweep_results_plotter.create_plots( + results_pdf_file, combined_results_file, value_combos_file + ) + + +# Run graph localizer/vio with values. # Add traceback so errors are forwarded, otherwise # some errors are suppressed due to the multiprocessing # library call @@ -54,28 +74,34 @@ def test_values( bag_file, map_file, image_topic, - config_path, robot_config, world, - use_image_features, + use_bag_image_msgs, groundtruth_bagfile, - rmse_rel_start_time, - rmse_rel_end_time, ): - new_output_dir = os.path.join(output_dir, str(job_id)) - os.mkdir(new_output_dir) - graph_config_filepath = os.path.join( - config_path, "config", "graph_localizer.config" + new_output_dir_prefix = os.path.join(output_dir, str(job_id)) + new_output_dir = os.path.join(new_output_dir_prefix, "localization") + os.makedirs(new_output_dir) + config_path = os.path.join( + rospkg.RosPack().get_path("astrobee"), "config/localization" ) - new_graph_config_filepath = os.path.join(new_output_dir, "graph_localizer.config") - config_creator.make_config( - values, value_names, graph_config_filepath, new_graph_config_filepath + make_config_file( + "graph_localizer.config", new_output_dir, config_path, values, value_names + ) + make_config_file( + "graph_vio.config", new_output_dir, config_path, values, value_names + ) + make_config_file( + "imu_integrator.config", new_output_dir, config_path, values, value_names + ) + make_config_file( + "imu_filter.config", new_output_dir, config_path, 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 + "/" + graph_config_prefix = new_output_dir_prefix + "/" run_command = ( - "rosrun localization_analysis run_graph_bag " + "rosrun localization_analysis run_offline_replay " + bag_file + " " + map_file @@ -90,26 +116,47 @@ def test_values( + " -g " + graph_config_prefix + " -f " - + str(use_image_features) + + str(use_bag_image_msgs) ) if image_topic is not None: run_command += " -i " + image_topic os.system(run_command) - output_pdf_file = os.path.join(new_output_dir, str(job_id) + "_output.pdf") - output_csv_file = os.path.join(new_output_dir, "graph_stats.csv") + + # Use stored sparse mapping poses from output bag if no groundtruth bag provided + if not groundtruth_bagfile: + groundtruth_bagfile = output_bag + + # Plot Loc results + output_pdf_file = os.path.join( + new_output_dir_prefix, str(job_id) + "_loc_output.pdf" + ) + results_csv_file = os.path.join(new_output_dir_prefix, "loc_graph_stats.csv") + plot_command = ( + "rosrun localization_analysis loc_results_plotter.py " + + output_bag + + " --output-file " + + output_pdf_file + + " --results-csv-file " + + results_csv_file + + " -g " + + groundtruth_bagfile + ) + os.system(plot_command) + + # Plot VIO results + output_pdf_file = os.path.join( + new_output_dir_prefix, str(job_id) + "_vio_output.pdf" + ) + results_csv_file = os.path.join(new_output_dir_prefix, "vio_graph_stats.csv") plot_command = ( - "rosrun localization_analysis plot_results.py " + "rosrun localization_analysis vio_results_plotter.py " + output_bag + " --output-file " + output_pdf_file - + " --output-csv-file " - + output_csv_file + + " --results-csv-file " + + results_csv_file + " -g " + groundtruth_bagfile - + " --rmse-rel-start-time " - + str(rmse_rel_start_time) - + " --rmse-rel-end-time " - + str(rmse_rel_end_time) ) os.system(plot_command) @@ -121,6 +168,7 @@ def test_values_helper(zipped_vals): return test_values(*zipped_vals) +# Create parallel jobs for each generated parameter sweep config set def parameter_sweep( all_value_combos, value_names, @@ -128,13 +176,10 @@ def parameter_sweep( bag_file, map_file, image_topic, - config_path, robot_config, world, - use_image_features, + use_bag_image_msgs, groundtruth_bagfile, - rmse_rel_start_time=0, - rmse_rel_end_time=-1, ): job_ids = list(range(len(all_value_combos))) num_processes = 15 @@ -151,29 +196,37 @@ def parameter_sweep( itertools.repeat(bag_file), itertools.repeat(map_file), itertools.repeat(image_topic), - itertools.repeat(config_path), itertools.repeat(robot_config), itertools.repeat(world), - itertools.repeat(use_image_features), + itertools.repeat(use_bag_image_msgs), itertools.repeat(groundtruth_bagfile), - itertools.repeat(rmse_rel_start_time), - itertools.repeat(rmse_rel_end_time), ) ), ) - parameter_sweep_utilities.concat_results(job_ids, output_dir, "graph_stats.csv") + parameter_sweep_utilities.concat_results( + job_ids, output_dir, "loc_graph_stats.csv", "loc" + ) + parameter_sweep_utilities.concat_results( + job_ids, output_dir, "vio_graph_stats.csv", "vio" + ) +# Create values for defined config options to sweep on def make_value_ranges(): value_ranges = [] value_names = [] steps = 10 # tune num smart factors - # value_ranges.append(np.logspace(-1, -6, steps, endpoint=True)) - # value_names.append('accel_bias_sigma') - value_ranges.append(np.linspace(0, 500, steps, endpoint=True)) - value_names.append("smart_projection_adder_feature_track_min_separation") + # 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(-3, -7, steps, endpoint=True)) + # value_names.append("ii_bias_acc_omega_int") + + # value_ranges.append(np.logspace(0, -5, steps, endpoint=True)) + # value_names.append("gv_fa_vo_retriangulation_threshold") # q_gyro # .001 -> 2 deg @@ -185,18 +238,16 @@ def make_value_ranges(): return value_ranges, value_names +# Perform the parameter sweep using defined range of values def make_values_and_parameter_sweep( output_dir, bag_file, map_file, image_topic, - config_path, robot_config, world, - use_image_features, + use_bag_image_msgs, groundtruth_bagfile, - rmse_rel_start_time=0, - rmse_rel_end_time=-1, ): output_dir = lu.create_directory(output_dir) print(("Output directory for results is {}".format(output_dir))) @@ -220,20 +271,14 @@ def make_values_and_parameter_sweep( bag_file, map_file, image_topic, - config_path, robot_config, world, - use_image_features, + use_bag_image_msgs, groundtruth_bagfile, - rmse_rel_start_time, - rmse_rel_end_time, - ) - combined_results_file = os.path.join(output_dir, "param_sweep_combined_results.csv") - value_combos_file = os.path.join(output_dir, "all_value_combos.csv") - results_pdf_file = os.path.join(output_dir, "param_sweep_results.pdf") - plot_parameter_sweep_results.create_plots( - results_pdf_file, combined_results_file, value_combos_file ) + + create_results_plots(output_dir, "loc") + create_results_plots(output_dir, "vio") return output_dir @@ -243,13 +288,22 @@ def make_values_and_parameter_sweep( ) parser.add_argument("bag_file", help="Full path to bagfile.") parser.add_argument("map_file", help="Full path to map file.") - parser.add_argument("image_topic", help="Image topic.") - parser.add_argument("config_path", help="Full path to config path.") - parser.add_argument("robot_config", help="Relative path to robot config.") - parser.add_argument("world", help="World being used.") parser.add_argument( - "--generate-image-features", - dest="use_image_features", + "-i", + "--image-topic", + default="/mgt/img_sampler/nav_cam/image_record", + help="Image topic.", + ) + parser.add_argument( + "-r", + "--robot-config", + default="bumble.config", + help="Relative path to robot config.", + ) + parser.add_argument("-w", "--world", default="iss", help="World being used.") + parser.add_argument( + "--use-bag-image-feature-msgs", + dest="use_bag_image_feature_msgs", action="store_false", help="Use image features msgs from bagfile or generate features from images.", ) @@ -261,18 +315,14 @@ def make_values_and_parameter_sweep( help="Full path to output directory where files will be saved. If not specified, timestamped directory will be created in current path.", ) args = parser.parse_args() - # Default set groundtruth bagfile to normal bagfile - if not args.groundtruth_bagfile: - args.groundtruth_bagfile = args.bag_file make_values_and_parameter_sweep( args.directory, args.bag_file, args.map_file, args.image_topic, - args.config_path, args.robot_config, args.world, - args.use_image_features, + args.use_bag_image_feature_msgs, args.groundtruth_bagfile, ) diff --git a/tools/localization_analysis/scripts/plot_all_results.py b/tools/localization_analysis/scripts/plot_all_results.py deleted file mode 100755 index a06bd26ae9..0000000000 --- a/tools/localization_analysis/scripts/plot_all_results.py +++ /dev/null @@ -1,63 +0,0 @@ -#!/usr/bin/env python -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. -""" -Plots localization results for each bagfile in a directory. -Assumes no custom start and end times are provided and no special -groundtruth bags are available for each bagfile. See -plot_results.py for more details on results plotting. -""" - - -import argparse -import os - -if __name__ == "__main__": - parser = argparse.ArgumentParser( - description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter - ) - parser.add_argument( - "--output-dir", - default="", - help="Output directory where generated pdfs for bagfiles are saved.", - ) - args = parser.parse_args() - - # Find bagfiles with bag prefix in current directory, fail if none found - bag_names = [ - bag for bag in os.listdir(".") if os.path.isfile(bag) and bag.endswith(".bag") - ] - bag_names = sorted(set(bag_names)) - if len(bag_names) == 0: - print("No bag files found") - sys.exit() - else: - print(("Found " + str(len(bag_names)) + " bags.")) - - for bag_name in bag_names: - output_file_name = ( - os.path.splitext(os.path.basename(bag_name))[0] + "_output.pdf" - ) - if args.output_dir: - output_file_name = args.output_dir + "/" + output_file_name - plot_command = ( - "rosrun localization_analysis plot_results.py " - + bag_name - + " --output-file " - + output_file_name - ) - os.system(plot_command) diff --git a/tools/localization_analysis/scripts/plot_bag_sweep_results.py b/tools/localization_analysis/scripts/plot_bag_sweep_results.py deleted file mode 100755 index d38e4581c3..0000000000 --- a/tools/localization_analysis/scripts/plot_bag_sweep_results.py +++ /dev/null @@ -1,443 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import argparse - -import matplotlib - -matplotlib.use("pdf") -import os -import sys - -import matplotlib.pyplot as plt -import pandas as pd -from matplotlib.backends.backend_pdf import PdfPages - - -def save_rmse_results_to_csv( - rmses, prefix="", rmses_2=None, label_1=None, label_2=None -): - mean_rmses_dataframe = pd.DataFrame() - labels = [] - if label_1 and label_2 and rmses_2 is not None: - labels.append(label_1) - labels.append(label_2) - if labels: - mean_rmses_dataframe["Label"] = labels - mean_rmses_list = [] - mean_rmses_list.append(rmses.mean()) - relative_rmses = [] - relative_change_in_rmses = [] - if rmses_2 is not None: - mean_rmses_list.append(rmses_2.mean()) - relative_rmses.append(mean_rmses_list[0] / mean_rmses_list[1]) - relative_rmses.append(mean_rmses_list[1] / mean_rmses_list[0]) - relative_change_in_rmses.append(mean_rmses_list[0] / mean_rmses_list[1] - 1.0) - relative_change_in_rmses.append(mean_rmses_list[1] / mean_rmses_list[0] - 1.0) - mean_rmses_dataframe["rel_" + prefix + "rmse_%"] = relative_rmses - mean_rmses_dataframe[ - "rel_" + prefix + "rmse_delta_%" - ] = relative_change_in_rmses - mean_rmses_dataframe["mean_" + prefix + "rmse"] = mean_rmses_list - mean_rmses_csv_file = "mean_rmses.csv" - mean_rmses_dataframe.to_csv(mean_rmses_csv_file, index=False, mode="a") - return mean_rmses_list, labels, relative_rmses, relative_change_in_rmses - - -def rmse_plot( - pdf, - x_axis_vals, - shortened_bag_names, - rmses, - prefix="", - label_1="", - rmses_2=None, - label_2="", -): - plt.figure() - plt.plot( - x_axis_vals, - rmses, - "b", - label=label_1, - linestyle="None", - marker="o", - markeredgewidth=0.1, - markersize=10.5, - ) - if rmses_2 is not None: - plt.plot( - x_axis_vals, - rmses_2, - "r", - label=label_2, - linestyle="None", - marker="o", - markeredgewidth=0.1, - markersize=10.5, - ) - plt.legend(prop={"size": 8}, bbox_to_anchor=(1.05, 1)) - plt.xticks(x_axis_vals, shortened_bag_names, fontsize=7, rotation=20) - plt.ylabel(prefix + " RMSE") - plt.title(prefix + " RMSE vs. Bag") - x_range = x_axis_vals[len(x_axis_vals) - 1] - x_axis_vals[0] - x_buffer = x_range * 0.1 - # Extend x axis on either side to make data more visible - plt.xlim([x_axis_vals[0] - x_buffer, x_axis_vals[len(x_axis_vals) - 1] + x_buffer]) - plt.tight_layout() - pdf.savefig() - plt.close() - - -def save_rmse_stats_to_plot( - pdf, rmses, prefix="", label_1="", rmses_2=None, label_2="" -): - # Plot mean rmses - ( - mean_rmses, - labels, - relative_rmses, - relative_change_in_rmses, - ) = save_rmse_results_to_csv(rmses, prefix, rmses_2, label_1, label_2) - - mean_rmses_1_string = prefix + "rmse: " + str(mean_rmses[0]) - if labels: - mean_rmses_1_string += ", label: " + labels[0] - plt.figure() - plt.axis("off") - plt.text(0.0, 1.0, mean_rmses_1_string) - if len(mean_rmses) > 1: - mean_rmses_2_string = prefix + "rmse: " + str(mean_rmses[1]) - if labels: - mean_rmses_2_string += ", label: " + labels[1] - plt.text(0.0, 0.85, mean_rmses_2_string) - relative_rmses_string = prefix + "rel rmse %: " + str(100 * relative_rmses[0]) - plt.text(0.0, 0.7, relative_rmses_string) - relative_rmses_change_string = ( - prefix + "rel change in rmse %: " + str(100 * relative_change_in_rmses[0]) - ) - plt.text(0.0, 0.55, relative_rmses_change_string) - pdf.savefig() - - -def rmse_plots( - pdf, - x_axis_vals, - shortened_bag_names, - rmses, - integrated_rmses, - orientation_rmses, - prefix="", - label_1="", - rmses_2=None, - integrated_rmses_2=None, - orientation_rmses_2=None, - label_2="", -): - rmse_plot( - pdf, - x_axis_vals, - shortened_bag_names, - rmses, - prefix + " ", - label_1, - rmses_2, - label_2, - ) - if integrated_rmses is not None: - rmse_plot( - pdf, - x_axis_vals, - shortened_bag_names, - integrated_rmses, - prefix + " Integrated ", - label_1, - integrated_rmses_2, - label_2, - ) - rmse_plot( - pdf, - x_axis_vals, - shortened_bag_names, - orientation_rmses, - prefix + " Orientation ", - label_1, - orientation_rmses_2, - label_2, - ) - - save_rmse_stats_to_plot(pdf, rmses, prefix + " ", label_1, rmses_2, label_2) - if integrated_rmses is not None: - save_rmse_stats_to_plot( - pdf, - integrated_rmses, - prefix + " Integrated ", - label_1, - integrated_rmses_2, - label_2, - ) - save_rmse_stats_to_plot( - pdf, - orientation_rmses, - prefix + " Orientation ", - label_1, - orientation_rmses_2, - label_2, - ) - - -def create_plot( - output_file, csv_file, label_1="", csv_file_2=None, label_2="", imu_augmented_2=True -): - dataframe = pd.read_csv(csv_file) - dataframe.sort_values(by=["Bag"], inplace=True) - # Graph rmses - rmses = dataframe["rmse"] - rel_rmses = dataframe["rel_rmse"] - integrated_rmses = dataframe["integrated_rmse"] - rel_integrated_rmses = dataframe["rel_integrated_rmse"] - orientation_rmses = dataframe["orientation_rmse"] - rel_orientation_rmses = dataframe["rel_orientation_rmse"] - # IMU augmented rmses - imu_augmented_rmses = dataframe["imu_augmented_rmse"] - rel_imu_augmented_rmses = dataframe["rel_imu_augmented_rmse"] - imu_augmented_integrated_rmses = dataframe["imu_augmented_integrated_rmse"] - rel_imu_augmented_integrated_rmses = dataframe["rel_imu_augmented_integrated_rmse"] - imu_augmented_orientation_rmses = dataframe["imu_augmented_orientation_rmse"] - rel_imu_augmented_orientation_rmses = dataframe[ - "rel_imu_augmented_orientation_rmse" - ] - # IMU bias tester rmses - imu_bias_tester_rmses = dataframe["imu_bias_tester_rmse"] - rel_imu_bias_tester_rmses = dataframe["rel_imu_bias_tester_rmse"] - imu_bias_tester_orientation_rmses = dataframe["imu_bias_tester_orientation_rmse"] - rel_imu_bias_tester_orientation_rmses = dataframe[ - "rel_imu_bias_tester_orientation_rmse" - ] - - bag_names = dataframe["Bag"].tolist() - max_name_length = 45 - shortened_bag_names = [ - bag_name[-1 * max_name_length :] - if len(bag_name) > max_name_length - else bag_name - for bag_name in bag_names - ] - x_axis_vals = list(range(len(shortened_bag_names))) - rmses_2 = None - rel_rmses_2 = None - integrated_rmses_2 = None - rel_integrated_rmses_2 = None - orientation_rmses_2 = None - rel_orientation_rmses_2 = None - imu_augmented_rmses_2 = None - rel_imu_augmented_rmses_2 = None - imu_augmented_integrated_rmses_2 = None - rel_imu_augmented_integrated_rmses_2 = None - imu_augmented_orientation_rmses_2 = None - rel_imu_augmented_orientation_rmses_2 = None - imu_bias_tester_rmses_2 = None - rel_imu_bias_tester_rmses_2 = None - imu_bias_tester_orientation_rmses_2 = None - rel_imu_bias_tester_orientation_rmses_2 = None - - if csv_file_2: - dataframe_2 = pd.read_csv(csv_file_2) - dataframe_2.sort_values(by=["Bag"], inplace=True) - # Graph rmses - rmses_2 = dataframe_2["rmse"] - rel_rmses_2 = dataframe_2["rel_rmse"] - integrated_rmses_2 = dataframe_2["integrated_rmse"] - rel_integrated_rmses_2 = dataframe_2["rel_integrated_rmse"] - orientation_rmses_2 = dataframe_2["orientation_rmse"] - rel_orientation_rmses_2 = dataframe_2["rel_orientation_rmse"] - if imu_augmented_2: - # IMU augmented rmses - imu_augmented_rmses_2 = dataframe_2["imu_augmented_rmse"] - rel_imu_augmented_rmses_2 = dataframe_2["rel_imu_augmented_rmse"] - imu_augmented_integrated_rmses_2 = dataframe_2[ - "imu_augmented_integrated_rmse" - ] - rel_imu_augmented_integrated_rmses_2 = dataframe_2[ - "rel_imu_augmented_integrated_rmse" - ] - imu_augmented_orientation_rmses_2 = dataframe_2[ - "imu_augmented_orientation_rmse" - ] - rel_imu_augmented_orientation_rmses_2 = dataframe_2[ - "rel_imu_augmented_orientation_rmse" - ] - # IMU bias tester rmses - imu_bias_tester_rmses_2 = dataframe_2["imu_bias_tester_rmse"] - rel_imu_bias_tester_rmses_2 = dataframe_2["rel_imu_bias_tester_rmse"] - imu_bias_tester_orientation_rmses_2 = dataframe_2[ - "imu_bias_tester_orientation_rmse" - ] - rel_imu_bias_tester_orientation_rmses_2 = dataframe_2[ - "rel_imu_bias_tester_orientation_rmse" - ] - - bag_names_2 = dataframe_2["Bag"].tolist() - if bag_names != bag_names_2: - print("Bag names for first and second csv file are not the same") - exit() - with PdfPages(output_file) as pdf: - rmse_plots( - pdf, - x_axis_vals, - shortened_bag_names, - rmses, - integrated_rmses, - orientation_rmses, - "", - label_1, - rmses_2, - integrated_rmses_2, - orientation_rmses_2, - label_2, - ) - rmse_plots( - pdf, - x_axis_vals, - shortened_bag_names, - rel_rmses, - rel_integrated_rmses, - rel_orientation_rmses, - "rel", - label_1, - rel_rmses_2, - rel_integrated_rmses_2, - rel_orientation_rmses_2, - label_2, - ) - if imu_augmented_2: - rmse_plots( - pdf, - x_axis_vals, - shortened_bag_names, - imu_augmented_rmses, - imu_augmented_integrated_rmses, - imu_augmented_orientation_rmses, - "imu_augmented", - label_1, - imu_augmented_rmses_2, - imu_augmented_integrated_rmses_2, - imu_augmented_orientation_rmses_2, - label_2, - ) - rmse_plots( - pdf, - x_axis_vals, - shortened_bag_names, - rel_imu_augmented_rmses, - rel_imu_augmented_integrated_rmses, - rel_imu_augmented_orientation_rmses, - "rel_imu_augmented", - label_1, - rel_imu_augmented_rmses_2, - rel_imu_augmented_integrated_rmses_2, - rel_imu_augmented_orientation_rmses_2, - label_2, - ) - rmse_plots( - pdf, - x_axis_vals, - shortened_bag_names, - imu_bias_tester_rmses, - None, - imu_bias_tester_orientation_rmses, - "imu_bias_tester", - label_1, - imu_bias_tester_rmses_2, - None, - imu_bias_tester_orientation_rmses_2, - label_2, - ) - rmse_plots( - pdf, - x_axis_vals, - shortened_bag_names, - rel_imu_bias_tester_rmses, - None, - rel_imu_bias_tester_orientation_rmses, - "rel_imu_bias_tester", - label_1, - rel_imu_bias_tester_rmses_2, - None, - rel_imu_bias_tester_orientation_rmses_2, - label_2, - ) - - else: - rmse_plots( - pdf, - x_axis_vals, - shortened_bag_names, - imu_augmented_rmses, - imu_augmented_integrated_rmses, - imu_augmented_orientation_rmses, - "imu_augmented", - label_1, - rmses_2, - integrated_rmses_2, - orientation_rmses_2, - label_2 + " no imu aug", - ) - rmse_plots( - pdf, - x_axis_vals, - shortened_bag_names, - rel_imu_augmented_rmses, - rel_imu_augmented_integrated_rmses, - rel_imu_augmented_orientation_rmses, - "rel_imu_augmented", - label_1, - rel_rmses_2, - rel_integrated_rmses_2, - rel_orientation_rmses_2, - label_2 + " no imu aug", - ) - rmse_plots( - pdf, - x_axis_vals, - shortened_bag_names, - imu_bias_tester_rmses, - None, - imu_bias_tester_orientation_rmses, - "imu_bias_tester", - label_1, - imu_bias_tester_rmses_2, - None, - imu_bias_tester_orientation_rmses_2, - label_2, - ) - rmse_plots( - pdf, - x_axis_vals, - shortened_bag_names, - rel_imu_bias_tester_rmses, - None, - rel_imu_bias_tester_orientation_rmses, - "rel_imu_bias_tester", - label_1, - rel_imu_bias_tester_rmses_2, - None, - rel_imu_bias_tester_orientation_rmses_2, - label_2, - ) diff --git a/tools/localization_analysis/scripts/plot_conversions.py b/tools/localization_analysis/scripts/plot_conversions.py new file mode 100644 index 0000000000..3332129a96 --- /dev/null +++ b/tools/localization_analysis/scripts/plot_conversions.py @@ -0,0 +1,799 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +import sys + +import numpy as np +import scipy.spatial.transform + +import plotters +import states + + +# Return list of 3 lists, one each for x, y, z values in poses +def xyz_vectors_from_poses(poses): + xs = [pose.position.x for pose in poses] + ys = [pose.position.y for pose in poses] + zs = [pose.position.z for pose in poses] + return [xs, ys, zs] + + +# Return list of 3 lists, one each for y, p, r values in poses +def ypr_vectors_from_poses(poses): + ys = [euler_angles[0] for euler_angles in (pose.euler_angles() for pose in poses)] + ps = [euler_angles[1] for euler_angles in (pose.euler_angles() for pose in poses)] + rs = [euler_angles[2] for euler_angles in (pose.euler_angles() for pose in poses)] + return [ys, ps, rs] + + +# Return list of 3 lists, one each for x, y, z values in velocities +def xyz_velocity_vectors_from_graph_vio_states(graph_vio_states): + xs = [state.velocity_with_covariance.x for state in graph_vio_states] + ys = [state.velocity_with_covariance.y for state in graph_vio_states] + zs = [state.velocity_with_covariance.z for state in graph_vio_states] + return [xs, ys, zs] + + +# Return list of 3 lists, one each for x, y, z values in velocities +def xyz_velocity_vectors_from_extrapolated_loc_states(extrapolated_loc_states): + xs = [state.velocity.x for state in extrapolated_loc_states] + ys = [state.velocity.y for state in extrapolated_loc_states] + zs = [state.velocity.z for state in extrapolated_loc_states] + return [xs, ys, zs] + + +# Return list of 3 lists, one each for x, y, z values in accelerations +def xyz_acceleration_vectors_from_extrapolated_loc_states(extrapolated_loc_states): + xs = [state.acceleration.x for state in extrapolated_loc_states] + ys = [state.acceleration.y for state in extrapolated_loc_states] + zs = [state.acceleration.z for state in extrapolated_loc_states] + return [xs, ys, zs] + + +# Return list of 3 lists, one each for x, y, z values in accelerations +def xyz_acceleration_vectors_from_imu_accelerations(imu_accelerations): + xs = [acceleration.x for acceleration in imu_accelerations] + ys = [acceleration.y for acceleration in imu_accelerations] + zs = [acceleration.z for acceleration in imu_accelerations] + return [xs, ys, zs] + + return [xs, ys, zs] + + +# Return list of 3 lists, one each for x, y, z values in IMU accelerometer bias +def xyz_accel_bias_vectors_from_graph_vio_states(graph_vio_states): + xs = [ + state.imu_bias_with_covariance.accelerometer_bias.x + for state in graph_vio_states + ] + ys = [ + state.imu_bias_with_covariance.accelerometer_bias.y + for state in graph_vio_states + ] + zs = [ + state.imu_bias_with_covariance.accelerometer_bias.z + for state in graph_vio_states + ] + return [xs, ys, zs] + + +# Return list of 3 lists, one each for x, y, z values in IMU gyro bias +def xyz_gyro_bias_vectors_from_graph_vio_states(graph_vio_states): + xs = [state.imu_bias_with_covariance.gyroscope_bias.x for state in graph_vio_states] + ys = [state.imu_bias_with_covariance.gyroscope_bias.y for state in graph_vio_states] + zs = [state.imu_bias_with_covariance.gyroscope_bias.z for state in graph_vio_states] + return [xs, ys, zs] + + +# Return list of times for given timestamped objects +def times_from_timestamped_objects(timestamped_objects): + return [obj.timestamp for obj in timestamped_objects] + + +# Return list of diagonal position covariance norms from poses +def diagonal_position_covariance_norms_from_poses(poses): + # Assumes covariance organized as orientation then position + # RR RP + # RP PP + # Thus position diagonals occur at (3, 3), (4, 4), (5, 5) for the 6x6 matrix. + # The msg stores covariances as an array with 36 values (from 0-35) where (x, y) occurs at + # x + 6*y. So (3, 3) -> 21, (4, 4) -> 28, (5, 5) -> 35 + return [ + np.linalg.norm(pose.covariance[21], pose.covariance[28], pose.covariance[35]) + for pose in poses + ] + + +# Return list of timestamped poses with covariances from graph vio states +def timestamped_poses_with_covariance_from_graph_vio_states(graph_vio_states): + return [ + graph_vio_state.timestamped_pose_with_covariance() + for graph_vio_state in graph_vio_states + ] + + +# Return list of timestamped velocities with covariances from graph vio states +def timestamped_velocities_with_covariance_from_graph_vio_states(graph_vio_states): + return [ + graph_vio_state.timestamped_velocity_with_covariance() + for graph_vio_state in graph_vio_states + ] + + +# Return list of timestamped imu bias with covariances from graph vio states +def timestamped_imu_biases_with_covariance_from_graph_vio_states(graph_vio_states): + return [ + graph_vio_state.timestamped_imu_bias_with_covariance() + for graph_vio_state in graph_vio_states + ] + + +# Return list of graph vio poses from graph vio states. +# Poses are adjusted to start at the corresponding groundtruth pose at the earliest corresponding timestamp +# so they can be plotted against groundtruth poses. The earliest corresponding timestamp is the first timestamp +# with a dt <= max_diff compared to a graph_vio_state. +def adjusted_graph_vio_poses_from_graph_vio_states( + graph_vio_states, groundtruth_poses, max_diff=0.1 +): + graph_vio_state_times = np.array([state.timestamp for state in graph_vio_states]) + world_T_vio = None + for groundtruth_pose in groundtruth_poses: + closest_matching_graph_vio_state_index = np.argmin( + np.abs(graph_vio_state_times - groundtruth_pose.timestamp) + ) + timestamp_diff = np.abs( + graph_vio_state_times[closest_matching_graph_vio_state_index] + - groundtruth_pose.timestamp + ) + if timestamp_diff <= max_diff: + closest_graph_vio_state = graph_vio_states[ + closest_matching_graph_vio_state_index + ] + world_T_vio = ( + groundtruth_pose + * graph_vio_states[ + closest_matching_graph_vio_state_index + ].pose_with_covariance.inverse() + ) + break + if not world_T_vio: + print("Failed to find corresponding groundtruth pose to graph VIO poses") + sys.exit(0) + adjusted_graph_vio_poses = [] + for graph_vio_state in graph_vio_states: + adjusted_pose = world_T_vio * states.TimestampedPose( + graph_vio_state.pose_with_covariance.orientation, + graph_vio_state.pose_with_covariance.position, + graph_vio_state.timestamp, + ) + adjusted_graph_vio_poses.append( + states.TimestampedPose( + adjusted_pose.orientation, + adjusted_pose.position, + graph_vio_state.timestamp, + ) + ) + return adjusted_graph_vio_poses + + +# Return list of imu bias extrapolated poses. +# Poses are adjusted to start at the corresponding groundtruth pose at the earliest corresponding timestamp +# so they can be plotted against groundtruth poses. The earliest corresponding timestamp is the first timestamp +# with a dt <= max_diff compared to a imu bias extrapolated pose. +def absolute_poses_from_imu_bias_extrapolated_poses( + imu_bias_extrapolated_poses, groundtruth_poses, max_diff=0.1 +): + imu_bias_times = np.array( + [imu_bias.timestamp for imu_bias in imu_bias_extrapolated_poses] + ) + world_T_vio = None + for groundtruth_pose in groundtruth_poses: + closest_matching_imu_bias_index = np.argmin( + np.abs(imu_bias_times - groundtruth_pose.timestamp) + ) + timestamp_diff = np.abs( + imu_bias_times[closest_matching_imu_bias_index] - groundtruth_pose.timestamp + ) + if timestamp_diff <= max_diff: + closest_imu_bias = imu_bias_extrapolated_poses[ + closest_matching_imu_bias_index + ] + world_T_vio = ( + groundtruth_pose + * imu_bias_extrapolated_poses[closest_matching_imu_bias_index].inverse() + ) + break + if not world_T_vio: + print("Failed to find corresponding groundtruth pose to graph VIO poses") + sys.exit(0) + adjusted_imu_bias_poses = [] + for imu_bias in imu_bias_extrapolated_poses: + adjusted_pose = world_T_vio * states.TimestampedPose( + imu_bias.orientation, imu_bias.position, imu_bias.timestamp + ) + adjusted_imu_bias_poses.append( + states.TimestampedPose( + adjusted_pose.orientation, adjusted_pose.position, imu_bias.timestamp + ) + ) + return adjusted_imu_bias_poses + + +# Return list of absolute poses. +# Poses are adjusted to start at the corresponding groundtruth pose at the earliest corresponding timestamp +# so they can be plotted against groundtruth poses. The earliest corresponding timestamp is the first timestamp +# with a dt <= max_diff compared to a imu bias extrapolated pose. +def absolute_poses_from_relative_poses(relative_poses, groundtruth_poses, max_diff=0.1): + relative_pose_times = np.array( + [relative_pose.timestamp for relative_pose in relative_poses] + ) + world_T_odom = None + for groundtruth_pose in groundtruth_poses: + closest_matching_relative_pose_index = np.argmin( + np.abs(relative_pose_times - groundtruth_pose.timestamp) + ) + timestamp_diff = np.abs( + relative_pose_times[closest_matching_relative_pose_index] + - groundtruth_pose.timestamp + ) + if timestamp_diff <= max_diff: + closest_relative_pose = relative_poses[closest_matching_relative_pose_index] + world_T_odom = ( + groundtruth_pose + * relative_poses[closest_matching_relative_pose_index].inverse() + ) + break + if not world_T_odom: + print("Failed to find corresponding groundtruth pose to graph VIO poses") + sys.exit(0) + adjusted_relative_pose_poses = [] + previous_relative_pose = None + for relative_pose in relative_poses: + pose = states.TimestampedPose( + relative_pose.orientation, relative_pose.position, relative_pose.timestamp + ) + if previous_relative_pose: + pose = previous_relative_pose * pose + adjusted_pose = world_T_odom * pose + adjusted_relative_pose_poses.append( + states.TimestampedPose( + adjusted_pose.orientation, + adjusted_pose.position, + relative_pose.timestamp, + ) + ) + previous_relative_pose = pose + return adjusted_relative_pose_poses + + +# Return list of poses from integrated velocities. +# Poses are adjusted to start at the corresponding groundtruth pose at the earliest corresponding timestamp +# so they can be plotted against groundtruth poses. The earliest corresponding timestamp is the first timestamp +# with a dt <= max_diff compared to a velocity. +def absolute_poses_from_integrated_velocities( + timestamped_velocities, + groundtruth_poses, + frame_change_velocity=False, + max_diff=0.1, + poses=None, +): + times = np.array([velocity.timestamp for velocity in timestamped_velocities]) + start_index = None + start_groundtruth_index = None + starting_groundtruth_pose = None + for i in range(len(groundtruth_poses)): + closest_matching_index = np.argmin( + np.abs(times - groundtruth_poses[i].timestamp) + ) + timestamp_diff = np.abs( + times[closest_matching_index] - groundtruth_poses[i].timestamp + ) + if timestamp_diff <= max_diff: + start_index = closest_matching_index + starting_groundtruth_pose = groundtruth_poses[i] + start_groundtruth_index = i + break + if start_index is None: + print("Failed to find corresponding groundtruth pose to graph VIO poses") + sys.exit(0) + if frame_change_velocity: + return integrate_velocities_with_frame_change( + timestamped_velocities[start_index:], + starting_groundtruth_pose, + groundtruth_poses, + start_groundtruth_index, + poses[start_index:], + ) + else: + return integrate_velocities( + timestamped_velocities[start_index:], starting_groundtruth_pose + ) + + +# Return list of poses from integrated graph_vio velocities. +# Poses are adjusted to start at the corresponding groundtruth pose at the earliest corresponding timestamp +# so they can be plotted against groundtruth poses. The earliest corresponding timestamp is the first timestamp +# with a dt <= max_diff compared to a velocity. +def absolute_poses_from_integrated_graph_vio_state_velocities( + graph_vio_states, groundtruth_poses, max_diff=0.1 +): + velocities = [ + states.TimestampedVelocity( + graph_vio_state.velocity_with_covariance.x, + graph_vio_state.velocity_with_covariance.y, + graph_vio_state.velocity_with_covariance.z, + graph_vio_state.timestamp, + ) + for graph_vio_state in graph_vio_states + ] + poses = [ + states.TimestampedPose( + graph_vio_state.pose_with_covariance.orientation, + graph_vio_state.pose_with_covariance.position, + graph_vio_state.timestamp, + ) + for graph_vio_state in graph_vio_states + ] + + return absolute_poses_from_integrated_velocities( + velocities, groundtruth_poses, True, max_diff, poses + ) + + +# Return list of poses from integrated extrapolated loc velocities. +# Poses are adjusted to start at the corresponding groundtruth pose at the earliest corresponding timestamp +# so they can be plotted against groundtruth poses. The earliest corresponding timestamp is the first timestamp +# with a dt <= max_diff compared to a velocity. +def absolute_poses_from_integrated_extrapolated_loc_state_velocities( + extrapolated_loc_states, groundtruth_poses, max_diff=0.1 +): + velocities = [ + states.TimestampedVelocity( + extrapolated_loc_state.velocity.x, + extrapolated_loc_state.velocity.y, + extrapolated_loc_state.velocity.z, + extrapolated_loc_state.timestamp, + ) + for extrapolated_loc_state in extrapolated_loc_states + ] + return absolute_poses_from_integrated_velocities( + velocities, groundtruth_poses, False, max_diff + ) + + +# Integrates graph vio velocities and appends these to a starting pose to generate absolute pose estimates +def integrate_velocities(velocities, starting_pose): + # Calculate relative x,y,z increments using v*dt for each increment + # Succesively add these increments to starting pose to generate future poses + x = starting_pose.position.x + y = starting_pose.position.y + z = starting_pose.position.z + integrated_poses = [] + for i in range(len(velocities) - 1): + integrated_poses.append( + states.TimestampedPose( + scipy.spatial.transform.Rotation.from_rotvec([0, 0, 0]), + states.Position([x, y, z]), + velocities[i].timestamp, + ) + ) + dt = velocities[i + 1].timestamp - velocities[i].timestamp + dx = dt * velocities[i].x + dy = dt * velocities[i].y + dz = dt * velocities[i].z + x += dx + y += dy + z += dz + return integrated_poses + + +# Integrates graph vio velocities and appends these to a starting pose to generate absolute pose estimates +# Assumes velocities are in a separate odom frame and uses the closest groundtruth pose to rotete +# each velocity to the world frame. +def integrate_velocities_with_frame_change( + velocities, + starting_pose, + groundtruth_poses, + starting_groundtruth_index, + poses, + max_diff=0.5, +): + # Calculate relative x,y,z increments using v*dt for each increment + # Succesively add these increments to starting pose to generate future poses + x = starting_pose.position.x + y = starting_pose.position.y + z = starting_pose.position.z + groundtruth_index = starting_groundtruth_index + # Initialize world_R_odom + world_R_odom = groundtruth_poses[groundtruth_index] * poses[0].inverse() + world_R_odom.position = states.Position([0, 0, 0]) + integrated_poses = [] + for i in range(len(velocities) - 1): + if groundtruth_index + 1 < len(groundtruth_poses): + # Update groundtruth index if successive groundtruth pose timestamp is closer to + # current velocity estimate + current_gt_time_diff = abs( + groundtruth_poses[groundtruth_index].timestamp - velocities[i].timestamp + ) + next_gt_time_diff = abs( + groundtruth_poses[groundtruth_index + 1].timestamp + - velocities[i].timestamp + ) + if next_gt_time_diff < current_gt_time_diff: + groundtruth_index = groundtruth_index + 1 + # Update gt world_R_odom if time diffs between gt pose and velocity are close enough + if next_gt_time_diff < max_diff: + # Update world_R_odom + world_R_odom = ( + groundtruth_poses[groundtruth_index] * poses[i].inverse() + ) + world_R_odom.position = states.Position([0, 0, 0]) + integrated_poses.append( + states.TimestampedPose( + scipy.spatial.transform.Rotation.from_rotvec([0, 0, 0]), + states.Position([x, y, z]), + velocities[i].timestamp, + ) + ) + dt = velocities[i + 1].timestamp - velocities[i].timestamp + dx = dt * velocities[i].x + dy = dt * velocities[i].y + dz = dt * velocities[i].z + # Frame change velocity + odom_F_rel_velocity_pose = states.TimestampedPose( + scipy.spatial.transform.Rotation.from_rotvec([0, 0, 0]), + states.Position([dx, dy, dz]), + velocities[i].timestamp, + ) + world_F_rel_velocity_pose = world_R_odom * odom_F_rel_velocity_pose + dx = world_F_rel_velocity_pose.position.x + dy = world_F_rel_velocity_pose.position.y + dz = world_F_rel_velocity_pose.position.z + + x += dx + y += dy + z += dz + return integrated_poses + + +# Return list of ml measurement counts from graph loc states +def ml_feature_counts_from_graph_loc_states(graph_loc_states): + return [ + graph_loc_state.num_detected_ml_features for graph_loc_state in graph_loc_states + ] + + +# Return list of ar measurement counts from graph loc states +def ar_feature_counts_from_graph_loc_states(graph_loc_states): + return [ + graph_loc_state.num_detected_ar_features for graph_loc_state in graph_loc_states + ] + + +# Return list of optical flow feature counts from graph vio states +def optical_flow_feature_counts_from_graph_vio_states(graph_vio_states): + return [ + graph_vio_state.num_detected_of_features for graph_vio_state in graph_vio_states + ] + + +# Return list of optical flow factor counts from graph vio states +def optical_flow_factor_counts_from_graph_vio_states(graph_vio_states): + return [graph_vio_state.num_of_factors for graph_vio_state in graph_vio_states] + + +# Return list of depth factor counts from graph vio states +def depth_factor_counts_from_graph_vio_states(graph_vio_states): + return [graph_vio_state.num_depth_factors for graph_vio_state in graph_vio_states] + + +# Return list of ml pose factor counts from graph loc states +def ml_pose_factor_counts_from_graph_loc_states(graph_loc_states): + return [graph_loc_state.num_ml_pose_factors for graph_loc_state in graph_loc_states] + + +# Return list of ml projection factor counts from graph loc states +def ml_projection_factor_counts_from_graph_loc_states(graph_loc_states): + return [ + graph_loc_state.num_ml_projection_factors + for graph_loc_state in graph_loc_states + ] + + +# Return list of standstill occurances times from states +def standstill_from_states(states): + return [int(state.standstill) for state in states] + + +# Return list of optimization iterations from states +def optimization_iterations_from_states(states): + return [state.optimization_iterations for state in states] + + +# Return list of num states from states +def num_states_from_states(states): + return [state.num_states for state in states] + + +# Return list of duration times from states +def durations_from_states(states): + return [state.duration for state in states] + + +# Return list of optimization times from states +def optimization_times_from_states(states): + return [state.optimization_time for state in states] + + +# Return list of update times from states +def update_times_from_states(states): + return [state.update_time for state in states] + + +# Return list of num features from depth odometries +def num_features_from_depth_odometries(depth_odometries): + return [depth_odometry.num_features for depth_odometry in depth_odometries] + + +# Return list of runtimes from depth odometries +def runtimes_from_depth_odometries(depth_odometries): + return [depth_odometry.runtime for depth_odometry in depth_odometries] + + +# Return list of timestamped poses from depth odometries +def poses_from_depth_odometries(depth_odometries): + return [ + states.TimestampedPose( + depth_odometry.pose_with_covariance.orientation, + depth_odometry.pose_with_covariance.position, + depth_odometry.timestamp, + ) + for depth_odometry in depth_odometries + ] + + +# Return list of timestamped poses from graph loc states +def poses_from_graph_loc_states(graph_loc_states): + return [ + states.TimestampedPose( + graph_loc_state.pose_with_covariance.orientation, + graph_loc_state.pose_with_covariance.position, + graph_loc_state.timestamp, + ) + for graph_loc_state in graph_loc_states + ] + + +# Return list of timestamped poses from extrapolated loc states +def poses_from_extrapolated_loc_states(extrapolated_loc_states): + return [ + states.TimestampedPose( + extrapolated_loc_state.pose.orientation, + extrapolated_loc_state.pose.position, + extrapolated_loc_state.timestamp, + ) + for extrapolated_loc_state in extrapolated_loc_states + ] + + +def velocity_plotter_from_graph_vio_states(graph_vio_states): + xs, ys, zs = xyz_velocity_vectors_from_graph_vio_states(graph_vio_states) + times = times_from_timestamped_objects(graph_vio_states) + return plotters.Vector3dPlotter( + "Graph VIO Velocity", times, xs, ys, zs, ["X", "Y", "Z"], marker="o" + ) + + +def velocity_plotter_from_extrapolated_loc_states(extrapolated_loc_states): + xs, ys, zs = xyz_velocity_vectors_from_extrapolated_loc_states( + extrapolated_loc_states + ) + times = times_from_timestamped_objects(extrapolated_loc_states) + return plotters.Vector3dPlotter( + "Extrapolated Loc Velocity", times, xs, ys, zs, ["X", "Y", "Z"] + ) + + +def acceleration_plotter_from_extrapolated_loc_states(extrapolated_loc_states): + xs, ys, zs = xyz_acceleration_vectors_from_extrapolated_loc_states( + extrapolated_loc_states + ) + times = times_from_timestamped_objects(extrapolated_loc_states) + return plotters.Vector3dPlotter( + "Extrapolated Loc Acceleration", times, xs, ys, zs, ["X", "Y", "Z"] + ) + + +def acceleration_plotter_from_imu_accelerations(imu_accelerations): + xs, ys, zs = xyz_acceleration_vectors_from_imu_accelerations(imu_accelerations) + times = times_from_timestamped_objects(imu_accelerations) + return plotters.Vector3dPlotter( + "Raw IMU Acceleration", times, xs, ys, zs, ["X", "Y", "Z"] + ) + + +def accel_bias_plotter_from_graph_vio_states(graph_vio_states): + xs, ys, zs = xyz_accel_bias_vectors_from_graph_vio_states(graph_vio_states) + times = times_from_timestamped_objects(graph_vio_states) + return plotters.Vector3dPlotter( + "Graph VIO Accel. Bias", times, xs, ys, zs, ["X", "Y", "Z"] + ) + + +def gyro_bias_plotter_from_graph_vio_states(graph_vio_states): + xs, ys, zs = xyz_gyro_bias_vectors_from_graph_vio_states(graph_vio_states) + times = times_from_timestamped_objects(graph_vio_states) + return plotters.Vector3dPlotter( + "Graph VIO Gyro Bias", times, xs, ys, zs, ["X", "Y", "Z"] + ) + + +def ml_feature_count_plotter_from_graph_loc_states(graph_loc_states): + counts = ml_feature_counts_from_graph_loc_states(graph_loc_states) + times = times_from_timestamped_objects(graph_loc_states) + return plotters.ValuePlotter( + "Graph Loc ML Feature Counts", times, counts, "Time (s)", "Num Features", "ML" + ) + + +def ar_feature_count_plotter_from_graph_loc_states(graph_loc_states): + counts = ar_feature_counts_from_graph_loc_states(graph_loc_states) + times = times_from_timestamped_objects(graph_loc_states) + return plotters.ValuePlotter( + "Graph Loc AR Feature Counts", times, counts, "Time (s)", "Num Features", "AR" + ) + + +def optical_flow_feature_count_plotter_from_graph_vio_states(graph_vio_states): + counts = optical_flow_feature_counts_from_graph_vio_states(graph_vio_states) + times = times_from_timestamped_objects(graph_vio_states) + return plotters.ValuePlotter( + "Graph VIO OF Counts", times, counts, "Time (s)", "Num Features", "OF" + ) + + +def optical_flow_factor_count_plotter_from_graph_vio_states(graph_vio_states): + counts = optical_flow_factor_counts_from_graph_vio_states(graph_vio_states) + times = times_from_timestamped_objects(graph_vio_states) + return plotters.ValuePlotter( + "Graph VIO OF Factors", times, counts, "Time (s)", "Num Factors", "OF" + ) + + +def depth_factor_count_plotter_from_graph_vio_states(graph_vio_states): + counts = depth_factor_counts_from_graph_vio_states(graph_vio_states) + times = times_from_timestamped_objects(graph_vio_states) + return plotters.ValuePlotter( + "Graph VIO Depth Factors", times, counts, "Time (s)", "Num Factors", "Depth" + ) + + +def ml_pose_factor_count_plotter_from_graph_loc_states(graph_loc_states): + counts = ml_pose_factor_counts_from_graph_loc_states(graph_loc_states) + times = times_from_timestamped_objects(graph_loc_states) + return plotters.ValuePlotter( + "Graph Loc ML Pose Factors", times, counts, "Time (s)", "Num Factors", "ML" + ) + + +def ml_projection_factor_count_plotter_from_graph_loc_states(graph_loc_states): + counts = ml_projection_factor_counts_from_graph_loc_states(graph_loc_states) + times = times_from_timestamped_objects(graph_loc_states) + return plotters.ValuePlotter( + "Graph Loc ML Projection Factors", + times, + counts, + "Time (s)", + "Num Factors", + "ML", + ) + + +def standstill_plotter_from_states(states): + standstill = standstill_from_states(states) + times = times_from_timestamped_objects(states) + return plotters.ValuePlotter( + "Standstill Occurances", + times, + standstill, + "Time (s)", + "Standstill (True/False)", + "Standstill", + ) + + +def optimization_iterations_plotter_from_states(states): + optimization_iterations = optimization_iterations_from_states(states) + times = times_from_timestamped_objects(states) + return plotters.ValuePlotter( + "Num Graph Optimization Iterations", + times, + optimization_iterations, + "Time (s)", + "Num Iterations", + "Num Optimization Iterations", + ) + + +def num_states_plotter_from_states(states): + num_states = num_states_from_states(states) + times = times_from_timestamped_objects(states) + return plotters.ValuePlotter( + "Num Graph States", times, num_states, "Time (s)", "Num States", "Num States" + ) + + +def duration_plotter_from_states(states): + durations = durations_from_states(states) + times = times_from_timestamped_objects(states) + return plotters.ValuePlotter( + "Duration", times, durations, "Time (s)", "Duration (s)", "Duration" + ) + + +def optimization_time_plotter_from_states(states): + optimization_times = optimization_times_from_states(states) + times = times_from_timestamped_objects(states) + return plotters.ValuePlotter( + "Optimization Times", + times, + optimization_times, + "Time (s)", + "Opt. Time (s)", + "Opt. Time", + ) + + +def update_time_plotter_from_states(states): + update_times = update_times_from_states(states) + times = times_from_timestamped_objects(states) + return plotters.ValuePlotter( + "Update Times", + times, + update_times, + "Time (s)", + "Update Time (s)", + "Update Time", + ) + + +def runtime_plotter_from_depth_odometries(depth_odometries): + runtimes = runtimes_from_depth_odometries(depth_odometries) + times = times_from_timestamped_objects(depth_odometries) + return plotters.ValuePlotter( + "Depth Odometry Runtime", + times, + runtimes, + "Time (s)", + "Runtimes (s)", + "Depth Odometry Runtimes", + ) + + +def num_features_plotter_from_depth_odometries(depth_odometries): + num_features = num_features_from_depth_odometries(depth_odometries) + times = times_from_timestamped_objects(depth_odometries) + return plotters.ValuePlotter( + "Depth Odometry Num Features", + times, + num_features, + "Time (s)", + "Num Features", + "Depth Odometry Num Features", + ) diff --git a/tools/localization_analysis/scripts/plot_helpers.py b/tools/localization_analysis/scripts/plot_helpers.py index 33e9aa7118..6d879b2c0c 100644 --- a/tools/localization_analysis/scripts/plot_helpers.py +++ b/tools/localization_analysis/scripts/plot_helpers.py @@ -19,9 +19,6 @@ import matplotlib -import poses -import vector3ds - matplotlib.use("pdf") import matplotlib.pyplot as plt diff --git a/tools/localization_analysis/scripts/plot_results.py b/tools/localization_analysis/scripts/plot_results.py deleted file mode 100755 index d8ff6fd13b..0000000000 --- a/tools/localization_analysis/scripts/plot_results.py +++ /dev/null @@ -1,980 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. -""" -Plots the localization information from a bagfile to a pdf. Generates -plots for localization poses, velocities, covariances, integrated velocities, -IMU bias values, IMU bias tester poses, feature counts, and more. -""" - -import argparse -import os -import sys - -import matplotlib - -import loc_states -import plot_helpers -import poses -import rmse_utilities -import utilities -import vector3d_plotter -import velocities - -matplotlib.use("pdf") -import csv -import math - -import geometry_msgs -import matplotlib.pyplot as plt -import rosbag -from matplotlib.backends.backend_pdf import PdfPages - - -def l2_map(vector3ds): - return [ - math.sqrt(x_y_z[0] * x_y_z[0] + x_y_z[1] * x_y_z[1] + x_y_z[2] * x_y_z[2]) - for x_y_z in zip(vector3ds.xs, vector3ds.ys, vector3ds.zs) - ] - - -def add_graph_plots( - pdf, - sparse_mapping_poses, - ar_tag_poses, - graph_localization_states, - imu_augmented_graph_localization_poses, -): - colors = ["r", "b", "g"] - position_plotter = vector3d_plotter.Vector3dPlotter( - "Time (s)", "Position (m)", "Graph vs. Sparse Mapping Position", True - ) - position_plotter.add_pose_position( - sparse_mapping_poses, - linestyle="None", - marker="o", - markeredgewidth=0.1, - markersize=1.5, - ) - if ar_tag_poses.times: - position_plotter.add_pose_position( - ar_tag_poses, - linestyle="None", - marker="x", - markeredgewidth=0.1, - markersize=1.5, - ) - position_plotter.add_pose_position(graph_localization_states) - position_plotter.plot(pdf) - - # orientations - orientation_plotter = vector3d_plotter.Vector3dPlotter( - "Time (s)", "Orientation (deg)", "Graph vs. Sparse Mapping Orientation", True - ) - orientation_plotter.add_pose_orientation( - sparse_mapping_poses, - linestyle="None", - marker="o", - markeredgewidth=0.1, - markersize=1.5, - ) - if ar_tag_poses.times: - orientation_plotter.add_pose_orientation( - ar_tag_poses, - linestyle="None", - marker="x", - markeredgewidth=0.1, - markersize=1.5, - ) - orientation_plotter.add_pose_orientation(graph_localization_states) - orientation_plotter.plot(pdf) - - # Imu Augmented Loc vs. Loc - position_plotter = vector3d_plotter.Vector3dPlotter( - "Time (s)", "Position (m)", "Graph vs. IMU Augmented Graph Position", True - ) - position_plotter.add_pose_position( - graph_localization_states, - linestyle="None", - marker="o", - markeredgewidth=0.1, - markersize=1.5, - ) - - position_plotter.add_pose_position( - imu_augmented_graph_localization_poses, linewidth=0.5 - ) - position_plotter.plot(pdf) - - # orientations - orientation_plotter = vector3d_plotter.Vector3dPlotter( - "Time (s)", - "Orientation (deg)", - "Graph vs. IMU Augmented Graph Orientation", - True, - ) - orientation_plotter.add_pose_orientation( - graph_localization_states, marker="o", markeredgewidth=0.1, markersize=1.5 - ) - orientation_plotter.add_pose_orientation( - imu_augmented_graph_localization_poses, linewidth=0.5 - ) - orientation_plotter.plot(pdf) - - # Velocity - plt.figure() - plot_helpers.plot_vector3ds( - graph_localization_states.velocities, graph_localization_states.times, "Vel." - ) - plt.xlabel("Time (s)") - plt.ylabel("Velocities") - plt.title("Graph Velocities") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - # Integrated Velocities - integrated_graph_localization_states = utilities.integrate_velocities( - graph_localization_states - ) - plot_positions( - pdf, integrated_graph_localization_states, sparse_mapping_poses, ar_tag_poses - ) - - -# TODO(rsoussan): Use this for other pose plotting in this script -def plot_poses(pdf, poses, sparse_mapping_poses, ar_tag_poses): - plot_positions(pdf, poses, sparse_mapping_poses, ar_tag_poses) - plot_orientations(pdf, poses, sparse_mapping_poses, ar_tag_poses) - - -def plot_positions(pdf, position_poses, sparse_mapping_poses, ar_tag_poses): - position_plotter = vector3d_plotter.Vector3dPlotter( - "Time (s)", - "Position (m)", - position_poses.pose_type + " vs. Sparse Mapping Position", - True, - ) - position_plotter.add_pose_position( - sparse_mapping_poses, - linestyle="None", - marker="o", - markeredgewidth=0.1, - markersize=1.5, - ) - if ar_tag_poses.times: - position_plotter.add_pose_position( - ar_tag_poses, - linestyle="None", - marker="x", - markeredgewidth=0.1, - markersize=1.5, - ) - position_plotter.add_pose_position(position_poses) - position_plotter.plot(pdf) - - -def plot_orientations(pdf, poses, sparse_mapping_poses, ar_tag_poses): - orientation_plotter = vector3d_plotter.Vector3dPlotter( - "Time (s)", - "Orientation (deg)", - poses.pose_type + " vs. Sparse Mapping Orientation", - True, - ) - orientation_plotter.add_pose_orientation( - sparse_mapping_poses, - linestyle="None", - marker="o", - markeredgewidth=0.1, - markersize=1.5, - ) - if ar_tag_poses.times: - orientation_plotter.add_pose_orientation( - ar_tag_poses, - linestyle="None", - marker="x", - markeredgewidth=0.1, - markersize=1.5, - ) - orientation_plotter.add_pose_orientation(poses) - orientation_plotter.plot(pdf) - - -def plot_covariances(pdf, times, covariances, name): - plt.figure() - title = name + " Covariance" - plt.plot(times, l2_map(covariances), "r", linewidth=0.5, label=title) - plt.title(title) - plt.xlabel("Time (s)") - plt.ylabel(title) - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - -def plot_features( - feature_counts, - times, - label, - color, - linestyle="None", - marker="o", - markeredgewidth=0.1, - markersize=1.5, -): - plt.plot( - times, - feature_counts, - color, - linestyle=linestyle, - marker=marker, - markeredgewidth=markeredgewidth, - markersize=markersize, - label=label, - ) - - -def add_feature_count_plots(pdf, graph_localization_states): - plt.figure() - plot_features( - graph_localization_states.num_detected_ml_features, - graph_localization_states.times, - "Det. VL", - "b", - marker="o", - markeredgewidth=0.1, - markersize=1.5, - ) - plot_features( - graph_localization_states.num_ml_projection_factors, - graph_localization_states.times, - "VL Proj Factors", - "r", - marker="x", - markeredgewidth=0.1, - markersize=1.5, - ) - plt.xlabel("Time (s)") - plt.ylabel("ML Feature Counts") - plt.title("ML Feature Counts") - plt.legend(prop={"size": 6}) - plt.ylim(ymin=-1) - pdf.savefig() - plt.close() - - plt.figure() - plot_features( - graph_localization_states.num_detected_of_features, - graph_localization_states.times, - "Det. OF", - "b", - marker="o", - markeredgewidth=0.1, - markersize=1.5, - ) - plot_features( - graph_localization_states.num_of_factors, - graph_localization_states.times, - "OF Factors", - "r", - marker="x", - markeredgewidth=0.1, - markersize=1.5, - ) - plt.xlabel("Time (s)") - plt.ylabel("Optical Flow Feature Counts") - plt.title("Optical Flow Feature Counts") - plt.legend(prop={"size": 6}) - plt.ylim(ymin=-1) - pdf.savefig() - plt.close() - - -def add_other_vector3d_plots( - pdf, imu_augmented_graph_localization_states, sparse_mapping_poses, ar_tag_poses -): - colors = ["r", "b", "g"] - - # Acceleration - plt.figure() - plot_helpers.plot_vector3ds( - imu_augmented_graph_localization_states.accelerations, - imu_augmented_graph_localization_states.times, - "Acc.", - ) - plt.xlabel("Time (s)") - plt.ylabel("Acceleration (m/s^2)") - plt.title("Acceleration") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - # Biases - # Plot Accelerometer Biases on different pages since they can start with quite different - # values, plotting on the same page will lead to a large y axis scale and hide subtle changes. - plt.figure() - plt.plot( - imu_augmented_graph_localization_states.times, - imu_augmented_graph_localization_states.accelerometer_biases.xs, - "r", - ) - plt.xlabel("Time (s)") - plt.ylabel("Accelerometer Biases (X)") - plt.title("Accelerometer Biases (X)") - pdf.savefig() - plt.close() - - plt.figure() - plt.plot( - imu_augmented_graph_localization_states.times, - imu_augmented_graph_localization_states.accelerometer_biases.ys, - "r", - ) - plt.xlabel("Time (s)") - plt.ylabel("Accelerometer Biases (Y)") - plt.title("Accelerometer Biases (Y)") - pdf.savefig() - plt.close() - - plt.figure() - plt.plot( - imu_augmented_graph_localization_states.times, - imu_augmented_graph_localization_states.accelerometer_biases.zs, - "r", - ) - plt.xlabel("Time (s)") - plt.ylabel("Accelerometer Biases (Z)") - plt.title("Accelerometer Biases (Z)") - pdf.savefig() - plt.close() - - plt.figure() - plt.plot( - imu_augmented_graph_localization_states.times, - imu_augmented_graph_localization_states.gyro_biases.xs, - "r", - ) - plt.xlabel("Time (s)") - plt.ylabel("Gyro Biases (X)") - plt.title("Gyro Biases (X)") - pdf.savefig() - plt.close() - - plt.figure() - plt.plot( - imu_augmented_graph_localization_states.times, - imu_augmented_graph_localization_states.gyro_biases.ys, - "r", - ) - plt.xlabel("Time (s)") - plt.ylabel("Gyro Biases (Y)") - plt.title("Gyro Biases (Y)") - pdf.savefig() - plt.close() - - plt.figure() - plt.plot( - imu_augmented_graph_localization_states.times, - imu_augmented_graph_localization_states.gyro_biases.zs, - "r", - ) - plt.xlabel("Time (s)") - plt.ylabel("Gyro Biases (Z)") - plt.title("Gyro Biases (Z)") - pdf.savefig() - plt.close() - - # Angular Velocity - plt.figure() - plot_helpers.plot_vector3ds( - imu_augmented_graph_localization_states.angular_velocities, - imu_augmented_graph_localization_states.times, - "Ang. Vel.", - ) - plt.xlabel("Time (s)") - plt.ylabel("Angular Velocities") - plt.title("Angular Velocities") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - # Velocity - plt.figure() - plot_helpers.plot_vector3ds( - imu_augmented_graph_localization_states.velocities, - imu_augmented_graph_localization_states.times, - "Vel.", - ) - plt.xlabel("Time (s)") - plt.ylabel("Velocities") - plt.title("IMU Augmented Graph Velocities") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - # Integrated Velocities - plt.figure() - plot_helpers.plot_positions( - sparse_mapping_poses, - colors, - linestyle="None", - marker="o", - markeredgewidth=0.1, - markersize=1.5, - ) - if ar_tag_poses.times: - plot_helpers.plot_positions( - ar_tag_poses, - colors, - linestyle="None", - marker="x", - markeredgewidth=0.1, - markersize=1.5, - ) - integrated_imu_augmented_graph_localization_states = utilities.integrate_velocities( - imu_augmented_graph_localization_states - ) - plot_helpers.plot_positions( - integrated_imu_augmented_graph_localization_states, colors, linewidth=0.5 - ) - plt.xlabel("Time (s)") - plt.ylabel("Position (m)") - plt.title("Integrated IMU Augmented Graph Velocities vs. Sparse Mapping Position") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - # Position covariance - plt.figure() - plt.plot( - imu_augmented_graph_localization_states.times, - l2_map(imu_augmented_graph_localization_states.position_covariances), - "r", - linewidth=0.5, - label="Position Covariance", - ) - plt.title("Position Covariance") - plt.xlabel("Time (s)") - plt.ylabel("Position Covariance") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - # Orientation covariance - plt.figure() - plt.plot( - imu_augmented_graph_localization_states.times, - l2_map(imu_augmented_graph_localization_states.orientation_covariances), - "r", - linewidth=0.5, - label="Orientation Covariance", - ) - plt.title("Orientation Covariance (Quaternion)") - plt.xlabel("Time (s)") - plt.ylabel("Orientation Covariance") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - # Velocity covariance - plt.figure() - plt.plot( - imu_augmented_graph_localization_states.times, - l2_map(imu_augmented_graph_localization_states.velocity_covariances), - "r", - linewidth=0.5, - label="Velocity Covariance", - ) - plt.title("Velocity Covariance") - plt.xlabel("Time (s)") - plt.ylabel("Velocity Covariance") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - # Accel Bias covariance - plt.figure() - plt.plot( - imu_augmented_graph_localization_states.times, - l2_map(imu_augmented_graph_localization_states.accelerometer_bias_covariances), - "r", - linewidth=0.5, - label="Accelerometer Bias Covariance", - ) - plt.title("Accelerometer Bias Covariance") - plt.xlabel("Time (s)") - plt.ylabel("Accelerometer Bias Covariance") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - # Gyro Bias covariance - plt.figure() - plt.plot( - imu_augmented_graph_localization_states.times, - l2_map(imu_augmented_graph_localization_states.gyro_bias_covariances), - "r", - linewidth=0.5, - label="Gyro Bias Covariance", - ) - plt.title("Gyro Bias Covariance") - plt.xlabel("Time (s)") - plt.ylabel("Gyro Bias Covariance") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - -def plot_loc_state_stats( - pdf, - localization_states, - sparse_mapping_poses, - output_csv_file, - prefix="", - atol=0, - plot_integrated_velocities=True, - rmse_rel_start_time=0, - rmse_rel_end_time=-1, -): - plot_loc_state_stats_abs( - pdf, - localization_states, - sparse_mapping_poses, - output_csv_file, - prefix, - atol, - plot_integrated_velocities, - rmse_rel_start_time, - rmse_rel_end_time, - ) - plot_loc_state_stats_rel( - pdf, - localization_states, - sparse_mapping_poses, - output_csv_file, - prefix, - atol, - plot_integrated_velocities, - rmse_rel_start_time, - rmse_rel_end_time, - ) - - -def plot_loc_state_stats_abs( - pdf, - localization_states, - sparse_mapping_poses, - output_csv_file, - prefix="", - atol=0, - plot_integrated_velocities=True, - rmse_rel_start_time=0, - rmse_rel_end_time=-1, -): - rmse = rmse_utilities.rmse_timestamped_poses( - localization_states, - sparse_mapping_poses, - True, - atol, - rmse_rel_start_time, - rmse_rel_end_time, - ) - integrated_rmse = [] - if plot_integrated_velocities: - integrated_localization_states = utilities.integrate_velocities( - localization_states - ) - integrated_rmse = rmse_utilities.rmse_timestamped_poses( - integrated_localization_states, - sparse_mapping_poses, - False, - atol, - rmse_rel_start_time, - rmse_rel_end_time, - ) - stats = ( - prefix - + " pos rmse: " - + str(rmse[0]) - + "\n" - + "orientation rmse: " - + str(rmse[1]) - ) - if plot_integrated_velocities: - stats += "\n" + "integrated rmse: " + str(integrated_rmse[0]) - with open(output_csv_file, "a") as output_csv: - csv_writer = csv.writer(output_csv, lineterminator="\n") - csv_writer.writerow([prefix + "rmse", str(rmse[0])]) - csv_writer.writerow([prefix + "orientation_rmse", str(rmse[1])]) - if plot_integrated_velocities: - csv_writer.writerow([prefix + "integrated_rmse", str(integrated_rmse[0])]) - plt.figure() - plt.axis("off") - plt.text(0.0, 0.5, stats) - pdf.savefig() - - -def plot_loc_state_stats_rel( - pdf, - localization_states, - sparse_mapping_poses, - output_csv_file, - prefix="", - atol=0, - plot_integrated_velocities=True, - rmse_rel_start_time=0, - rmse_rel_end_time=-1, -): - rmse = rmse_utilities.rmse_timestamped_poses_relative( - localization_states, - sparse_mapping_poses, - True, - atol, - rmse_rel_start_time, - rmse_rel_end_time, - ) - integrated_rmse = [] - if plot_integrated_velocities: - integrated_localization_states = utilities.integrate_velocities( - localization_states - ) - integrated_rmse = rmse_utilities.rmse_timestamped_poses_relative( - integrated_localization_states, - sparse_mapping_poses, - False, - atol, - rmse_rel_start_time, - rmse_rel_end_time, - ) - stats = ( - prefix - + " rel pos rmse: " - + str(rmse[0]) - + "\n" - + "rel orientation rmse: " - + str(rmse[1]) - ) - if plot_integrated_velocities: - stats += "\n" + "rel integrated rmse: " + str(integrated_rmse[0]) - - with open(output_csv_file, "a") as output_csv: - csv_writer = csv.writer(output_csv, lineterminator="\n") - csv_writer.writerow(["rel_" + prefix + "rmse", str(rmse[0])]) - csv_writer.writerow(["rel_" + prefix + "orientation_rmse", str(rmse[1])]) - if plot_integrated_velocities: - csv_writer.writerow( - ["rel_" + prefix + "integrated_rmse", str(integrated_rmse[0])] - ) - plt.figure() - plt.axis("off") - plt.text(0.0, 0.5, stats) - pdf.savefig() - - -def add_imu_bias_tester_poses(pdf, imu_bias_tester_poses, sparse_mapping_poses): - colors = ["r", "b", "g"] - plt.figure() - plot_helpers.plot_positions( - sparse_mapping_poses, - colors, - linestyle="None", - marker="o", - markeredgewidth=0.1, - markersize=1.5, - ) - plot_helpers.plot_positions(imu_bias_tester_poses, colors, linewidth=0.5) - plt.xlabel("Time (s)") - plt.ylabel("Position (m)") - plt.title("Imu Bias Tester vs. Sparse Mapping Position") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - # orientations - plt.figure() - plot_helpers.plot_orientations( - sparse_mapping_poses, - colors, - linestyle="None", - marker="o", - markeredgewidth=0.1, - markersize=1.5, - ) - plot_helpers.plot_orientations(imu_bias_tester_poses, colors, linewidth=0.5) - plt.xlabel("Time (s)") - plt.ylabel("Orienation (deg)") - plt.title("Imu Bias Tester vs. Sparse Mapping Orientation") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - -def add_imu_bias_tester_velocities(pdf, imu_bias_tester_velocities): - plt.figure() - plot_helpers.plot_vector3ds( - imu_bias_tester_velocities.velocities, imu_bias_tester_velocities.times, "Vel." - ) - plt.xlabel("Time (s)") - plt.ylabel("Velocities") - plt.title("IMU Bias Tester Velocities") - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - -def add_other_loc_plots( - pdf, - graph_localization_states, - imu_augmented_graph_localization_states, - sparse_mapping_poses, - ar_tag_poses, -): - add_feature_count_plots(pdf, graph_localization_states) - add_other_vector3d_plots( - pdf, imu_augmented_graph_localization_states, sparse_mapping_poses, ar_tag_poses - ) - - -def load_pose_msgs(vec_of_poses, bag, bag_start_time): - topics = [poses.topic for poses in vec_of_poses] - for topic, msg, t in bag.read_messages(topics): - for poses in vec_of_poses: - if poses.topic == topic: - poses.add_msg(msg, msg.header.stamp, bag_start_time) - break - - -def load_odometry_msgs(vec_of_poses, bag, bag_start_time): - topics = [poses.topic for poses in vec_of_poses] - for topic, msg, t in bag.read_messages(topics): - for poses in vec_of_poses: - if poses.topic == topic: - poses.add_msg_with_covariance( - msg.odometry.body_F_source_T_target, - msg.header.stamp, - bag_start_time, - ) - break - - -def load_pose_with_cov_msgs(vec_of_poses, bag, bag_start_time): - topics = [poses.topic for poses in vec_of_poses] - for topic, msg, t in bag.read_messages(topics): - for poses in vec_of_poses: - if poses.topic == topic: - poses.add_msg_with_covariance( - msg.pose, msg.header.stamp, bag_start_time - ) - break - - -def load_loc_state_msgs(vec_of_loc_states, bag, bag_start_time): - topics = [loc_states.topic for loc_states in vec_of_loc_states] - for topic, msg, t in bag.read_messages(topics): - for loc_states in vec_of_loc_states: - if loc_states.topic == topic: - loc_states.add_loc_state(msg, bag_start_time) - break - - -def load_velocity_msgs(velocities, bag, bag_start_time): - topics = [velocities.topic] - for topic, msg, t in bag.read_messages(topics): - velocities.add_msg(msg, msg.header.stamp, bag_start_time) - - -def has_topic(bag, topic): - topics = bag.get_type_and_topic_info().topics - return topic in topics - - -# Groundtruth bag must have the same start time as other bagfile, otherwise RMSE calculations will be flawed -def create_plots( - bagfile, - output_pdf_file, - output_csv_file="results.csv", - groundtruth_bagfile=None, - rmse_rel_start_time=0, - rmse_rel_end_time=-1, -): - bag = rosbag.Bag(bagfile) - groundtruth_bag = rosbag.Bag(groundtruth_bagfile) if groundtruth_bagfile else bag - bag_start_time = bag.get_start_time() - - has_imu_augmented_graph_localization_state = has_topic(bag, "/gnc/ekf") - has_imu_bias_tester_poses = has_topic(bag, "/imu_bias_tester/pose") - sparse_mapping_poses = poses.Poses("Sparse Mapping", "/sparse_mapping/pose") - ar_tag_poses = poses.Poses("AR Tag", "/ar_tag/pose") - imu_bias_tester_poses = poses.Poses("Imu Bias Tester", "/imu_bias_tester/pose") - vec_of_poses = [ar_tag_poses, imu_bias_tester_poses] - load_pose_msgs(vec_of_poses, bag, bag_start_time) - has_depth_odom = has_topic(bag, "/loc/depth/odom") - depth_odom_relative_poses = poses.Poses("Depth Odom", "/loc/depth/odom") - load_odometry_msgs([depth_odom_relative_poses], bag, bag_start_time) - groundtruth_vec_of_poses = [sparse_mapping_poses] - load_pose_msgs(groundtruth_vec_of_poses, groundtruth_bag, bag_start_time) - - graph_localization_states = loc_states.LocStates( - "Graph Localization", "/graph_loc/state" - ) - imu_augmented_graph_localization_states = loc_states.LocStates( - "Imu Augmented Graph Localization", "/gnc/ekf" - ) - vec_of_loc_states = [ - graph_localization_states, - imu_augmented_graph_localization_states, - ] - load_loc_state_msgs(vec_of_loc_states, bag, bag_start_time) - - imu_bias_tester_velocities = velocities.Velocities( - "Imu Bias Tester", "/imu_bias_tester/velocity" - ) - load_velocity_msgs(imu_bias_tester_velocities, bag, bag_start_time) - - bag.close() - - with PdfPages(output_pdf_file) as pdf: - add_graph_plots( - pdf, - sparse_mapping_poses, - ar_tag_poses, - graph_localization_states, - imu_augmented_graph_localization_states, - ) - if has_imu_bias_tester_poses: - add_imu_bias_tester_poses(pdf, imu_bias_tester_poses, sparse_mapping_poses) - add_imu_bias_tester_velocities(pdf, imu_bias_tester_velocities) - if has_imu_augmented_graph_localization_state: - add_other_loc_plots( - pdf, - graph_localization_states, - imu_augmented_graph_localization_states, - sparse_mapping_poses, - ar_tag_poses, - ) - else: - add_other_loc_plots( - pdf, graph_localization_states, graph_localization_states - ) - if has_depth_odom: - depth_odom_poses = utilities.make_absolute_poses_from_relative_poses( - sparse_mapping_poses, depth_odom_relative_poses, "Depth Odometry" - ) - plot_poses(pdf, depth_odom_poses, sparse_mapping_poses, ar_tag_poses) - # Note that for absolute time difference tolerance depth images and groudtruth use different sensor data - # and therefore have less similar timestamps. This timestamp difference reduces the accuracy of depth odometry - # groundtruth comparison. - plot_loc_state_stats( - pdf, - depth_odom_poses, - sparse_mapping_poses, - output_csv_file, - "depth_odometry_", - 0.01, - False, - rmse_rel_start_time=rmse_rel_start_time, - rmse_rel_end_time=rmse_rel_end_time, - ) - plot_covariances( - pdf, - depth_odom_relative_poses.times, - depth_odom_relative_poses.covariances.position, - "Depth Odometry Position", - ) - plot_covariances( - pdf, - depth_odom_relative_poses.times, - depth_odom_relative_poses.covariances.orientation, - "Depth Odometry Orientation", - ) - plot_loc_state_stats( - pdf, - graph_localization_states, - sparse_mapping_poses, - output_csv_file, - rmse_rel_start_time=rmse_rel_start_time, - rmse_rel_end_time=rmse_rel_end_time, - ) - plot_loc_state_stats( - pdf, - imu_augmented_graph_localization_states, - sparse_mapping_poses, - output_csv_file, - "imu_augmented_", - 0.01, - rmse_rel_start_time=rmse_rel_start_time, - rmse_rel_end_time=rmse_rel_end_time, - ) - if has_imu_bias_tester_poses: - plot_loc_state_stats( - pdf, - imu_bias_tester_poses, - sparse_mapping_poses, - output_csv_file, - "imu_bias_tester_", - 0.01, - False, - rmse_rel_start_time=rmse_rel_start_time, - rmse_rel_end_time=rmse_rel_end_time, - ) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser( - description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter - ) - parser.add_argument("bagfile", help="Input bagfile.") - parser.add_argument("--output-file", default="output.pdf", help="Output pdf file.") - parser.add_argument( - "--output-csv-file", - default="results.csv", - help="Output csv file containing localization stats.", - ) - parser.add_argument( - "-g", - "--groundtruth-bagfile", - default=None, - help="Optional bagfile containing groundtruth poses to use as a comparison for poses in the input bagfile. If none provided, sparse mapping poses are used as groundtruth from the input bagfile if available.", - ) - parser.add_argument( - "--rmse-rel-start-time", - type=float, - default=0, - help="Optional start time for plots.", - ) - parser.add_argument( - "--rmse-rel-end-time", - type=float, - default=-1, - help="Optional end time for plots.", - ) - args = parser.parse_args() - if not os.path.isfile(args.bagfile): - print(("Bag file " + args.bagfile + " does not exist.")) - sys.exit() - if args.groundtruth_bagfile and not os.path.isfile(args.groundtruth_bagfile): - print(("Groundtruth Bag file " + args.groundtruth_bagfile + " does not exist.")) - sys.exit() - create_plots( - args.bagfile, - args.output_file, - args.output_csv_file, - args.groundtruth_bagfile, - args.rmse_rel_start_time, - args.rmse_rel_end_time, - ) diff --git a/tools/localization_analysis/scripts/plotters.py b/tools/localization_analysis/scripts/plotters.py new file mode 100644 index 0000000000..3d4b45318c --- /dev/null +++ b/tools/localization_analysis/scripts/plotters.py @@ -0,0 +1,481 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +import matplotlib + +import plot_conversions + +matplotlib.use("pdf") +import sys + +import matplotlib.pyplot as plt +import numpy as np + + +# Class for plotting 3d vector values on the same and separate plots. +class Vector3dPlotter(object): + def __init__( + self, + name, + x_axis_vals, + x_vals, + y_vals, + z_vals, + labels, + colors=["r", "b", "g"], + linestyle="-", + linewidth=1, + marker=None, + markeredgewidth=None, + markersize=1, + ): + self.x_vals = x_vals + self.y_vals = y_vals + self.z_vals = z_vals + self.x_axis_vals = x_axis_vals + self.name = name + self.x_label = name + " " + labels[0] + self.y_label = name + " " + labels[1] + self.z_label = name + " " + labels[2] + self.x_color = colors[0] + self.y_color = colors[1] + self.z_color = colors[2] + self.linestyle = linestyle + self.linewidth = linewidth + self.marker = marker + self.markeredgewidth = markeredgewidth + self.markersize = markersize + + # Plot x, y, and z values on the same plot. + def plot_xyz(self): + self.plot_x() + self.plot_y() + self.plot_z() + + # Plot x values to the plot. + def plot_x(self): + plt.plot( + self.x_axis_vals, + self.x_vals, + label=self.x_label, + color=self.x_color, + linestyle=self.linestyle, + marker=self.marker, + markeredgewidth=self.markeredgewidth, + markersize=self.markersize, + ) + + # Plot y values to the plot. + def plot_y(self): + plt.plot( + self.x_axis_vals, + self.y_vals, + label=self.y_label, + color=self.y_color, + linewidth=self.linewidth, + linestyle=self.linestyle, + marker=self.marker, + markeredgewidth=self.markeredgewidth, + markersize=self.markersize, + ) + + # Plot z values to the plot. + def plot_z(self): + plt.plot( + self.x_axis_vals, + self.z_vals, + label=self.z_label, + color=self.z_color, + linestyle=self.linestyle, + marker=self.marker, + markeredgewidth=self.markeredgewidth, + markersize=self.markersize, + ) + + +# Plotter that allows plotting multiple vector3d values in the same plots. +# Pass vector3d values to plot using the add function. +# Optionally also plot individual plots for each vector component (x/y/z) if individual plots is true. +class MultiVector3dPlotter: + def __init__(self, xlabel, ylabel, title, individual_plots=False): + self.xlabel = xlabel + self.ylabel = ylabel + self.title = title + self.individual_plots = individual_plots + self.vector3d_plotters = [] + + # Adds vector3d plotter to be plot + def add(self, vector3d_plotter): + self.vector3d_plotters.append(vector3d_plotter) + + # Plot each of the vector3d values. Optionally plot individual axes on seperate plots + # if individual_plots set to True. + def plot(self, pdf): + plt.figure() + for vector3d_plotter in self.vector3d_plotters: + vector3d_plotter.plot_xyz() + plt.xlabel(self.xlabel) + plt.ylabel(self.ylabel) + plt.title(self.title) + plt.legend(prop={"size": 6}) + pdf.savefig() + plt.close() + + if self.individual_plots: + self.plot_xs(pdf) + self.plot_ys(pdf) + self.plot_zs(pdf) + + # Plot x values. + def plot_xs(self, pdf): + plt.figure() + for vector3d_plotter in self.vector3d_plotters: + vector3d_plotter.plot_x() + plt.xlabel(self.xlabel) + plt.ylabel(self.ylabel) + plt.title(self.title) + plt.legend(prop={"size": 6}) + pdf.savefig() + plt.close() + + # Plot y values. + def plot_ys(self, pdf): + plt.figure() + for vector3d_plotter in self.vector3d_plotters: + vector3d_plotter.plot_y() + plt.xlabel(self.xlabel) + plt.ylabel(self.ylabel) + plt.title(self.title) + plt.legend(prop={"size": 6}) + pdf.savefig() + plt.close() + + # Plot z values. + def plot_zs(self, pdf): + plt.figure() + for vector3d_plotter in self.vector3d_plotters: + vector3d_plotter.plot_z() + plt.xlabel(self.xlabel) + plt.ylabel(self.ylabel) + plt.title(self.title) + plt.legend(prop={"size": 6}) + pdf.savefig() + plt.close() + + +# Clip rotation angles so they stay between 0 and 360 +def unwrap_in_degrees(angles): + return np.rad2deg(np.unwrap(np.deg2rad(angles))) + + +# Class for plotting YPR orientation values on the same and separate plots. +class OrientationPlotter(Vector3dPlotter): + def __init__( + self, + name, + times, + y_vals, + p_vals, + r_vals, + colors=["r", "b", "g"], + linestyle="-", + linewidth=1, + marker=None, + markeredgewidth=None, + markersize=1, + ): + super(OrientationPlotter, self).__init__( + name, + times, + unwrap_in_degrees(y_vals), + unwrap_in_degrees(p_vals), + unwrap_in_degrees(r_vals), + ["Orientation (Yaw)", "Orientation (Roll)", "Orientation (Pitch)"], + colors, + linestyle, + linewidth, + marker, + markeredgewidth, + markersize, + ) + + +# Class for plotting XYZ position values on the same and separate plots. +class PositionPlotter(Vector3dPlotter): + def __init__( + self, + name, + times, + x_vals, + y_vals, + z_vals, + colors=["r", "b", "g"], + linestyle="-", + linewidth=1, + marker=None, + markeredgewidth=None, + markersize=1, + ): + super(PositionPlotter, self).__init__( + name, + times, + x_vals, + y_vals, + z_vals, + ["Position (X)", "Position (Y)", "Position (Z)"], + colors, + linestyle, + linewidth, + marker, + markeredgewidth, + markersize, + ) + + +# Class for plotting values. +class ValuePlotter(object): + def __init__( + self, + title, + x_axis_vals, + y_axis_vals, + xlabel, + ylabel, + label="", + linestyle="-", + linewidth=1, + marker=None, + markeredgewidth=None, + markersize=1, + ): + self.title = title + self.x_axis_vals = x_axis_vals + self.y_axis_vals = y_axis_vals + self.xlabel = xlabel + self.ylabel = ylabel + self.label = label + self.linestyle = linestyle + self.linewidth = linewidth + self.marker = marker + self.markeredgewidth = markeredgewidth + self.markersize = markersize + + # Plot values + def plot(self, pdf): + plt.figure() + plt.plot( + self.x_axis_vals, + self.y_axis_vals, + label=self.label, + linestyle=self.linestyle, + marker=self.marker, + markeredgewidth=self.markeredgewidth, + markersize=self.markersize, + ) + plt.xlabel(self.xlabel) + plt.ylabel(self.ylabel) + plt.title(self.title) + plt.legend(prop={"size": 6}) + pdf.savefig() + plt.close() + + +# Convert a list of angles from radians to degrees. +def radians_to_degrees(angles): + return np.rad2deg(np.unwrap(np.deg2rad(angles))) + + +# Plotter that allows plotting multiple pose positions and orientations in the same plots. +# Pass poses to plot using the add_poses function. +# Optionally also plot individual plots for each position or orientation axis (x/y/z or r/p/y) if individual plots is true. +class MultiPosePlotter: + def __init__(self, xlabel, ylabel, title, individual_plots=True): + self.xlabel = xlabel + self.ylabel = ylabel + self.title = title + self.orientations_plotter = MultiVector3dPlotter( + xlabel, ylabel, title, individual_plots + ) + self.positions_plotter = MultiVector3dPlotter( + xlabel, ylabel, title, individual_plots + ) + + # Add poses to position and orientation plots + def add_poses( + self, + name, + timestamped_poses, + colors=["r", "b", "g"], + linestyle="-", + linewidth=1, + marker=None, + markeredgewidth=None, + markersize=1, + ): + self.add_pose_positions( + name, + timestamped_poses, + colors, + linestyle, + linewidth, + marker, + markeredgewidth, + markersize, + ) + + self.add_pose_orientations( + name, + timestamped_poses, + colors, + linestyle, + linewidth, + marker, + markeredgewidth, + markersize, + ) + + # Add poses to position plots. + def add_pose_positions( + self, + name, + timestamped_poses, + colors=["r", "b", "g"], + linestyle="-", + linewidth=1, + marker=None, + markeredgewidth=None, + markersize=1, + ): + + xyz_vectors = plot_conversions.xyz_vectors_from_poses(timestamped_poses) + times = plot_conversions.times_from_timestamped_objects(timestamped_poses) + position_plotter = PositionPlotter( + name, + times, + xyz_vectors[0], + xyz_vectors[1], + xyz_vectors[2], + colors, + linestyle, + linewidth, + marker, + markeredgewidth, + markersize, + ) + self.positions_plotter.add(position_plotter) + + # Add poses to orientation plots + def add_pose_orientations( + self, + name, + timestamped_poses, + colors=["r", "b", "g"], + linestyle="-", + linewidth=1, + marker=None, + markeredgewidth=None, + markersize=1, + ): + ypr_vectors = plot_conversions.ypr_vectors_from_poses(timestamped_poses) + times = plot_conversions.times_from_timestamped_objects(timestamped_poses) + orientation_plotter = OrientationPlotter( + name, + times, + radians_to_degrees(ypr_vectors[0]), + radians_to_degrees(ypr_vectors[1]), + radians_to_degrees(ypr_vectors[2]), + colors, + linestyle, + linewidth, + marker, + markeredgewidth, + markersize, + ) + self.orientations_plotter.add(orientation_plotter) + + # Plot each of the added pose position values. Optionally plot individual axes on seperate plots + # if individual_plots set to True. + def plot_positions(self, pdf): + self.positions_plotter.plot(pdf) + + # Plot each of the added pose orientation values. Optionally plot individual axes on seperate plots + # if individual_plots set to True. + def plot_orientations(self, pdf): + self.orientations_plotter.plot(pdf) + + # Plot each of the added pose values. Plots positions and orientations in different plots. Optionally plot individual axes on seperate plots + # if individual_plots set to True. + def plot(self, pdf): + self.plot_positions(pdf) + self.plot_orientations(pdf) + + +# Plot RMSEs vs. bag file for a set of bag files +class BagRMSEsPlotter: + def __init__(self, dataframe): + dataframe.sort_values(by=["Bag"], inplace=True) + self.dataframe = dataframe + # Get bag name abbreviations + bag_names = dataframe["Bag"].tolist() + max_name_length = 45 + shortened_bag_names = [ + ( + bag_name[-1 * max_name_length :] + if len(bag_name) > max_name_length + else bag_name + ) + for bag_name in bag_names + ] + self.bag_names = shortened_bag_names + self.bag_name_indices = list(range(len(shortened_bag_names))) + + def plot( + self, + pdf, + rmse_name, + ): + rmses = self.dataframe[rmse_name] + plt.figure() + plt.plot( + self.bag_name_indices, + rmses, + "b", + linestyle="None", + marker="o", + markeredgewidth=0.1, + markersize=10.5, + ) + plt.xticks(self.bag_name_indices, self.bag_names, fontsize=7, rotation=20) + plt.ylabel(rmse_name + " RMSE") + plt.title(rmse_name + " RMSE vs. Bag") + x_range = ( + self.bag_name_indices[len(self.bag_name_indices) - 1] + - self.bag_name_indices[0] + ) + x_buffer = x_range * 0.1 + # Extend x axis on either side to make data more visible + plt.xlim( + [ + self.bag_name_indices[0] - x_buffer, + self.bag_name_indices[len(self.bag_name_indices) - 1] + x_buffer, + ] + ) + plt.tight_layout() + pdf.savefig() + plt.close() diff --git a/tools/localization_analysis/scripts/pose_covariances.py b/tools/localization_analysis/scripts/pose_covariances.py deleted file mode 100644 index 43f3388030..0000000000 --- a/tools/localization_analysis/scripts/pose_covariances.py +++ /dev/null @@ -1,52 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import vector3ds - - -class PoseCovariances: - def __init__(self): - self.position = vector3ds.Vector3ds() - self.orientation = vector3ds.Vector3ds() - - # Assumes 6x6 covariance matrix stored in row major order - # 0 1 2 3 4 5 - # 6 7 8 9 10 11 - # 12 13 14 15 16 17 - # 18 19 20 21 22 23 - # 24 25 26 27 28 29 - # 30 31 32 33 34 35 - # Then the position submatrix is - # 0 1 2 - # 6 7 8 - # 12 13 14 - # And the orientation submatrix is - # 21 22 23 - # 27 28 29 - # 33 34 35 - def add(self, covariance_vector): - x = covariance_vector[0] - y = covariance_vector[7] - z = covariance_vector[14] - self.position.add(x, y, z) - - rot_x = covariance_vector[21] - rot_y = covariance_vector[28] - rot_z = covariance_vector[35] - self.orientation.add(rot_x, rot_y, rot_z) diff --git a/tools/localization_analysis/scripts/poses.py b/tools/localization_analysis/scripts/poses.py deleted file mode 100644 index bdcd086151..0000000000 --- a/tools/localization_analysis/scripts/poses.py +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import numpy as np -import scipy.spatial.transform - -import orientations -import pose -import pose_covariances -import vector3ds - - -class Poses(object): - def __init__(self, pose_type, topic): - self.positions = vector3ds.Vector3ds() - self.orientations = orientations.Orientations() - self.covariances = pose_covariances.PoseCovariances() - self.times = [] - self.pose_type = pose_type - self.topic = topic - - def init_from_poses(self, poses_list, times): - for pose in poses_list: - self.add_pose(pose) - self.times = times - - def add_pose(self, pose): - self.add_orientation_and_position( - pose.orientation, pose.position[0], pose.position[1], pose.position[2] - ) - - def add_orientation_and_position(self, orientation, x, y, z): - self.positions.add(x, y, z) - euler_angles = orientation.as_euler("ZYX", degrees=True) - self.orientations.add(euler_angles[0], euler_angles[1], euler_angles[2]) - - def add_covariance_msg(self, covariance): - self.covariances.add(covariance) - - def add_pose_msg(self, pose_msg, timestamp, bag_start_time=0): - orientation = scipy.spatial.transform.Rotation.from_quat( - [ - pose_msg.orientation.x, - pose_msg.orientation.y, - pose_msg.orientation.z, - pose_msg.orientation.w, - ] - ) - self.add_orientation_and_position( - orientation, pose_msg.position.x, pose_msg.position.y, pose_msg.position.z - ) - self.times.append(timestamp.secs + 1e-9 * timestamp.nsecs - bag_start_time) - - def add_msg_with_covariance(self, msg, timestamp, bag_start_time=0): - self.add_pose_msg(msg.pose, timestamp, bag_start_time) - self.add_covariance_msg(msg.covariance) - - def add_msg(self, msg, timestamp, bag_start_time=0): - self.add_pose_msg(msg.pose, timestamp, bag_start_time) - - def position_vector(self, index): - return [ - self.positions.xs[index], - self.positions.ys[index], - self.positions.zs[index], - ] - - def pose(self, index): - return pose.Pose( - self.orientations.get_rotation(index), np.array(self.position_vector(index)) - ) diff --git a/tools/localization_analysis/scripts/average_results.py b/tools/localization_analysis/scripts/results_averager.py similarity index 85% rename from tools/localization_analysis/scripts/average_results.py rename to tools/localization_analysis/scripts/results_averager.py index 702a5b9479..25dddbf97b 100755 --- a/tools/localization_analysis/scripts/average_results.py +++ b/tools/localization_analysis/scripts/results_averager.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. @@ -34,13 +34,11 @@ def combined_results(csv_files): exit() names = dataframes[0].iloc[:, 0] combined_dataframes = pd.DataFrame(None, None, names) - for dataframe in dataframes: - trimmed_dataframe = pd.DataFrame( - dataframe.transpose().values[1:2], columns=names - ) - combined_dataframes = combined_dataframes.append( - trimmed_dataframe, ignore_index=True - ) + trimmed_dataframes = [ + pd.DataFrame(dataframe.transpose().values[1:2], columns=names) + for dataframe in dataframes + ] + combined_dataframes = pd.concat(trimmed_dataframes, ignore_index=True) return combined_dataframes diff --git a/tools/localization_analysis/scripts/results_savers.py b/tools/localization_analysis/scripts/results_savers.py new file mode 100644 index 0000000000..608480e5ed --- /dev/null +++ b/tools/localization_analysis/scripts/results_savers.py @@ -0,0 +1,60 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +import csv + +import matplotlib.pyplot as plt + +import rmse_calculators + + +# Computes and saves rmse for poses compared to groundtruth poses. +# Optionally pass the max allowed time difference between poses defined to be +# at the same timestamp, along with the start and end time bounds for computing +# same timestamp poses (passing an end time of -1 is equivalent to having no upper bound time). +def save_rmse( + poses, + groundtruth_poses, + csv_file, + pdf, + prefix="", + max_allowed_time_diff=0.01, + start_time=0, + end_time=-1, +): + rmse = rmse_calculators.pose_rmse( + poses, groundtruth_poses, max_allowed_time_diff, start_time, end_time + ) + + stats = ( + prefix + + " pos rmse: " + + str(rmse[0]) + + "\n" + + "orientation rmse: " + + str(rmse[1]) + ) + with open(csv_file, "a") as output_csv: + csv_writer = csv.writer(output_csv, lineterminator="\n") + csv_writer.writerow([prefix + " rmse", " " + str(rmse[0])]) + csv_writer.writerow([prefix + " Orientation rmse", " " + str(rmse[1])]) + plt.figure() + plt.axis("off") + plt.text(0.0, 0.5, stats) + pdf.savefig() diff --git a/tools/localization_analysis/scripts/rmse_calculators.py b/tools/localization_analysis/scripts/rmse_calculators.py new file mode 100644 index 0000000000..fa4a41261b --- /dev/null +++ b/tools/localization_analysis/scripts/rmse_calculators.py @@ -0,0 +1,172 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +import bisect +import math + +import numpy as np +import scipy.spatial.transform + + +# Assumes poses_a and poses_b are sorted in time. +# Optionally pass the max allowed time difference between poses defined to be +# at the same timestamp, along with the start and end time bounds for computing +# same timestamp poses (passing an end time of -1 is equivalent to having no upper bound time). +def get_same_timestamp_poses( + poses_a, poses_b, max_allowed_time_diff=0.01, start_time=0, end_time=-1 +): + same_poses_a = [] + same_poses_b = [] + poses_a_size = len(poses_a) + poses_b_size = len(poses_b) + a_index = 0 + b_index = 0 + # Increment a and b index as needed. Add same timestamped poses to trimmed poses containers + while (a_index < poses_a_size) and (b_index < poses_b_size): + a_time = poses_a[a_index].timestamp + b_time = poses_b[b_index].timestamp + + # Check if times are within given start and end time bounds + if a_time < start_time: + a_index += 1 + continue + if b_time < start_time: + b_index += 1 + continue + # end_time less than zero indicates no bound on end time + if end_time >= 0: + if a_time > end_time or b_time > end_time: + break + + if np.isclose(a_time, b_time, rtol=0, atol=max_allowed_time_diff): + same_poses_a.append(poses_a[a_index]) + same_poses_b.append(poses_b[b_index]) + a_index += 1 + b_index += 1 + elif a_time < b_time: + a_index += 1 + else: + b_index += 1 + return same_poses_a, same_poses_b + + +# Computes squared position difference between position vectors a and b +def position_squared_difference(a, b): + difference_vec = np.subtract(a, b) + return np.inner(difference_vec, difference_vec) + + +# Computes squared orientation difference using square of radian difference of +# rotations in axis angle representation +def orientation_squared_difference(world_R_a, world_R_b): + a_R_b = world_R_a.inv() * world_R_b + return np.inner(a_R_b.as_rotvec(), a_R_b.as_rotvec()) + + +# Computes RMSE between two sequences of poses. Only uses poses with the same timestamp. +# Optionally pass the max allowed time difference between poses defined to be +# at the same timestamp, along with the start and end time bounds for computing +# same timestamp poses (passing an end time of -1 is equivalent to having no upper bound time). +def pose_rmse(poses_a, poses_b, max_allowed_time_diff=0.01, start_time=0, end_time=-1): + same_poses_a, same_poses_b = get_same_timestamp_poses( + poses_a, poses_b, max_allowed_time_diff, start_time, end_time + ) + assert len(same_poses_a) == len(same_poses_b), "Length mismatch of poses" + num_poses = len(same_poses_a) + mean_squared_position_error = 0 + mean_squared_orientation_error = 0 + for index in range(num_poses): + # Position Error + a_vec = same_poses_a[index].position + b_vec = same_poses_b[index].position + position_squared_error = position_squared_difference(a_vec, b_vec) + # Use rolling mean to avoid overflow + mean_squared_position_error += ( + position_squared_error - mean_squared_position_error + ) / (index + 1) + # Orientation Error + a_rot = same_poses_a[index].orientation + b_rot = same_poses_b[index].orientation + orientation_squared_error = orientation_squared_difference(a_rot, b_rot) + mean_squared_orientation_error += ( + orientation_squared_error - mean_squared_orientation_error + ) / (index + 1) + position_rmse = math.sqrt(mean_squared_position_error) + orientation_rmse = math.sqrt(mean_squared_orientation_error) + return position_rmse, orientation_rmse + + +## Relative RMSE between two sequences of poses. Only uses poses with the same timestamp +## Optionally pass the max allowed time difference between poses defined to be +## at the same timestamp, along with the start and end time bounds for computing +## same timestamp poses (passing an end time of -1 is equivalent to having no upper bound time). +# def rmse_timestamped_poses_relative( +# poses_a, +# poses_b, +# max_allowed_time_diff=0.01, +# start_time=0, +# end_time=-1, +# min_relative_elapsed_time=10, +# max_relative_elapsed_time=20, +# ): +# same_poses_a, same_poses_b = get_same_timestamp_poses( +# poses_a, poses_b, max_allowed_time_diff, start_time, end_time +# ) +# assert len(same_poses_a.times) == len( +# same_poses_b.times +# ), "Length mismatch of poses" +# num_poses = len(same_poses_a.times) +# mean_squared_position_error = 0 +# mean_squared_orientation_error = 0 +# count = 0 +# for index1 in range(num_poses): +# # Position Error +# a_vec1 = same_poses_a.positions.get_numpy_vector(index1) +# b_vec1 = same_poses_b.positions.get_numpy_vector(index1) +# time1 = same_poses_a.times[index1] +# index2 = bisect.bisect_left( +# same_poses_a.times, time1 + min_relative_elapsed_time +# ) +# if index2 == len(same_poses_a.times): +# continue +# time2 = same_poses_a.times[index2] +# if time2 - time1 > max_relative_elapsed_time: +# continue +# a_vec2 = same_poses_a.positions.get_numpy_vector(index2) +# b_vec2 = same_poses_b.positions.get_numpy_vector(index2) +# a_rel_vec = a_vec2 - a_vec1 +# b_rel_vec = b_vec2 - b_vec1 +# +# position_squared_error = position_squared_difference(a_rel_vec, b_rel_vec) +# count += 1 +# # Use rolling mean to avoid overflow +# mean_squared_position_error += ( +# position_squared_error - mean_squared_position_error +# ) / float(count) +# # Orientation Error +# # TODO(rsoussan): Add relative calculations for orientations +# a_rot = same_poses_a.orientations.get_rotation(index1) +# b_rot = same_poses_b.orientations.get_rotation(index1) +# orientation_squared_error = orientation_squared_difference(a_rot, b_rot) +# mean_squared_orientation_error += ( +# orientation_squared_error - mean_squared_orientation_error +# ) / float(count) +# position_rmse = math.sqrt(mean_squared_position_error) +# orientation_rmse = math.sqrt(mean_squared_orientation_error) +# return position_rmse, orientation_rmse diff --git a/tools/localization_analysis/scripts/rmse_utilities.py b/tools/localization_analysis/scripts/rmse_utilities.py deleted file mode 100644 index 19ebc613e4..0000000000 --- a/tools/localization_analysis/scripts/rmse_utilities.py +++ /dev/null @@ -1,193 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import bisect -import math - -import numpy as np -import scipy.spatial.transform - -import poses - - -# Assumes poses_a and poses_b are sorted in time -def get_same_timestamp_poses( - poses_a, - poses_b, - add_orientations=True, - abs_tol=0, - rel_start_time=0, - rel_end_time=-1, -): - trimmed_poses_a = poses.Poses(poses_a.pose_type, poses_a.topic) - trimmed_poses_b = poses.Poses(poses_b.pose_type, poses_b.topic) - poses_a_size = len(poses_a.times) - poses_b_size = len(poses_b.times) - a_index = 0 - b_index = 0 - # Increment a and b index as needed. Add same timestamped poses to trimmed poses containers - while (a_index < poses_a_size) and (b_index < poses_b_size): - a_time = poses_a.times[a_index] - b_time = poses_b.times[b_index] - - # Check if times are within given start and end time bounds - if a_time < rel_start_time: - a_index += 1 - continue - if b_time < rel_start_time: - b_index += 1 - continue - # rel_end_time less than zero indicates no bound on end time - if rel_end_time >= 0: - if a_time > rel_end_time or b_time > rel_end_time: - break - - if np.isclose(a_time, b_time, rtol=0, atol=abs_tol): - trimmed_poses_a.positions.add_vector3d( - poses_a.positions.get_vector3d(a_index) - ) - trimmed_poses_a.times.append(poses_a.times[a_index]) - trimmed_poses_b.positions.add_vector3d( - poses_b.positions.get_vector3d(b_index) - ) - trimmed_poses_b.times.append(poses_b.times[b_index]) - if add_orientations: - trimmed_poses_a.orientations.add_euler( - poses_a.orientations.get_euler(a_index) - ) - trimmed_poses_b.orientations.add_euler( - poses_b.orientations.get_euler(b_index) - ) - a_index += 1 - b_index += 1 - elif a_time < b_time: - a_index += 1 - else: - b_index += 1 - return trimmed_poses_a, trimmed_poses_b - - -def position_squared_difference(a, b): - difference_vec = np.subtract(a, b) - return np.inner(difference_vec, difference_vec) - - -# Uses square of radian difference of rotations in axis angle representation -def orientation_squared_difference(world_R_a, world_R_b): - a_R_b = world_R_a.inv() * world_R_b - return np.inner(a_R_b.as_rotvec(), a_R_b.as_rotvec()) - - -# RMSE between two sequences of poses. Only uses poses with the same timestamp -def rmse_timestamped_poses( - poses_a, - poses_b, - add_orientation_rmse=True, - abs_tol=0, - rel_start_time=0, - rel_end_time=-1, -): - trimmed_poses_a, trimmed_poses_b = get_same_timestamp_poses( - poses_a, poses_b, add_orientation_rmse, abs_tol, rel_start_time, rel_end_time - ) - assert len(trimmed_poses_a.times) == len( - trimmed_poses_b.times - ), "Length mismatch of poses" - num_poses = len(trimmed_poses_a.times) - mean_squared_position_error = 0 - mean_squared_orientation_error = 0 - for index in range(num_poses): - # Position Error - a_vec = trimmed_poses_a.positions.get_numpy_vector(index) - b_vec = trimmed_poses_b.positions.get_numpy_vector(index) - position_squared_error = position_squared_difference(a_vec, b_vec) - # Use rolling mean to avoid overflow - mean_squared_position_error += ( - position_squared_error - mean_squared_position_error - ) / (index + 1) - # Orientation Error - if add_orientation_rmse: - a_rot = trimmed_poses_a.orientations.get_rotation(index) - b_rot = trimmed_poses_b.orientations.get_rotation(index) - orientation_squared_error = orientation_squared_difference(a_rot, b_rot) - mean_squared_orientation_error += ( - orientation_squared_error - mean_squared_orientation_error - ) / (index + 1) - position_rmse = math.sqrt(mean_squared_position_error) - orientation_rmse = math.sqrt(mean_squared_orientation_error) - return position_rmse, orientation_rmse - - -# Relative RMSE between two sequences of poses. Only uses poses with the same timestamp -def rmse_timestamped_poses_relative( - poses_a, - poses_b, - add_orientation_rmse=True, - abs_tol=0, - rel_start_time=0, - rel_end_time=-1, - min_relative_elapsed_time=10, - max_relative_elapsed_time=20, -): - trimmed_poses_a, trimmed_poses_b = get_same_timestamp_poses( - poses_a, poses_b, add_orientation_rmse, abs_tol, rel_start_time, rel_end_time - ) - assert len(trimmed_poses_a.times) == len( - trimmed_poses_b.times - ), "Length mismatch of poses" - num_poses = len(trimmed_poses_a.times) - mean_squared_position_error = 0 - mean_squared_orientation_error = 0 - count = 0 - for index1 in range(num_poses): - # Position Error - a_vec1 = trimmed_poses_a.positions.get_numpy_vector(index1) - b_vec1 = trimmed_poses_b.positions.get_numpy_vector(index1) - time1 = trimmed_poses_a.times[index1] - index2 = bisect.bisect_left( - trimmed_poses_a.times, time1 + min_relative_elapsed_time - ) - if index2 == len(trimmed_poses_a.times): - continue - time2 = trimmed_poses_a.times[index2] - if time2 - time1 > max_relative_elapsed_time: - continue - a_vec2 = trimmed_poses_a.positions.get_numpy_vector(index2) - b_vec2 = trimmed_poses_b.positions.get_numpy_vector(index2) - a_rel_vec = a_vec2 - a_vec1 - b_rel_vec = b_vec2 - b_vec1 - - position_squared_error = position_squared_difference(a_rel_vec, b_rel_vec) - count += 1 - # Use rolling mean to avoid overflow - mean_squared_position_error += ( - position_squared_error - mean_squared_position_error - ) / float(count) - # Orientation Error - if add_orientation_rmse: - # TODO(rsoussan): Add relative calculations for orientations - a_rot = trimmed_poses_a.orientations.get_rotation(index1) - b_rot = trimmed_poses_b.orientations.get_rotation(index1) - orientation_squared_error = orientation_squared_difference(a_rot, b_rot) - mean_squared_orientation_error += ( - orientation_squared_error - mean_squared_orientation_error - ) / float(count) - position_rmse = math.sqrt(mean_squared_position_error) - orientation_rmse = math.sqrt(mean_squared_orientation_error) - return position_rmse, orientation_rmse diff --git a/tools/localization_analysis/scripts/run_graph_bag_and_plot_results.py b/tools/localization_analysis/scripts/run_offline_replay_and_plot_results.py similarity index 53% rename from tools/localization_analysis/scripts/run_graph_bag_and_plot_results.py rename to tools/localization_analysis/scripts/run_offline_replay_and_plot_results.py index de807ecc16..84f19353d2 100755 --- a/tools/localization_analysis/scripts/run_graph_bag_and_plot_results.py +++ b/tools/localization_analysis/scripts/run_offline_replay_and_plot_results.py @@ -40,6 +40,10 @@ default="/mgt/img_sampler/nav_cam/image_record", help="Image topic.", ) + parser.add_argument( + "-r", "--robot-config-file", default="bumble.config", help="Robot config file." + ) + parser.add_argument("-w", "--world", default="iss", help="World (iss or granite).") parser.add_argument( "-o", "--output-bagfile", default="results.bag", help="Output bagfile." ) @@ -47,10 +51,13 @@ "--generate-image-features", dest="use_image_features", action="store_false", - help="Use image features msgs from bagfile or generate features from images.", + help="Generate image features instead of using image features msgs from bagfile.", ) - parser.add_argument("--output-file", default="output.pdf") - parser.add_argument("--output-csv-file", default="results.csv") + parser.add_argument("--vio-output-file", default="vio_output.pdf") + parser.add_argument("--loc-output-file", default="loc_output.pdf") + parser.add_argument("--vio-results-csv-file", default="vio_results.csv") + parser.add_argument("--loc-results-csv-file", default="loc_results.csv") + parser.add_argument("--results-csv-file", default="results.csv") parser.add_argument("-g", "--groundtruth-bagfile", default=None) parser.add_argument( "--directory", @@ -64,11 +71,17 @@ if not os.path.isfile(args.map_file): print("Map file " + args.map_file + " does not exist.") sys.exit() - if os.path.isfile(args.output_file): - print("Output file " + args.output_file + " already exist.") + if os.path.isfile(args.vio_output_file): + print("VIO output file " + args.vio_output_file + " already exists.") + sys.exit() + if os.path.isfile(args.loc_output_file): + print("Loc output file " + args.loc_output_file + " already exists.") sys.exit() - if os.path.isfile(args.output_csv_file): - print("Output csv file " + args.output_csv_file + " already exist.") + if os.path.isfile(args.vio_results_csv_file): + print("VIO results csv file " + args.vio_results_csv_file + " already exists.") + sys.exit() + if os.path.isfile(args.loc_results_csv_file): + print("Loc results csv file " + args.loc_results_csv_file + " already exists.") sys.exit() if args.groundtruth_bagfile and not os.path.isfile(args.groundtruth_bagfile): print("Groundtruth bag " + args.groundtruth_bagfile + " does not exist.") @@ -78,27 +91,52 @@ map_file = os.path.abspath(args.map_file) # Run localizer - run_graph_bag_command = ( - "rosrun localization_analysis run_graph_bag " + run_offline_replay_command = ( + "rosrun localization_analysis run_offline_replay " + bagfile + " " + map_file - + " --use-image-features " + + " --use-bag-image-feature-msgs " + (str(args.use_image_features)).lower() + " -o " + args.output_bagfile + + " -r " + + args.robot_config_file + + " -w " + + args.world + + " -s " + + args.results_csv_file + ) + lu.run_command_and_save_output( + run_offline_replay_command, "run_offline_replay_command.txt" ) - lu.run_command_and_save_output(run_graph_bag_command, "run_graph_bag_command.txt") # Plot results - plot_results_command = ( - "rosrun localization_analysis plot_results.py " + plot_vio_results_command = ( + "rosrun localization_analysis vio_results_plotter.py " + + args.output_bagfile + + " --output-file " + + args.vio_output_file + + " --results-csv-file " + + args.vio_results_csv_file + ) + if args.groundtruth_bagfile: + plot_vio_results_command += " -g " + args.groundtruth_bagfile + lu.run_command_and_save_output( + plot_vio_results_command, "plot_vio_results_command.txt" + ) + + # Plot loc results + plot_loc_results_command = ( + "rosrun localization_analysis loc_results_plotter.py " + args.output_bagfile + " --output-file " - + args.output_file - + " --output-csv-file " - + args.output_csv_file + + args.loc_output_file + + " --results-csv-file " + + args.loc_results_csv_file ) if args.groundtruth_bagfile: - plot_results_command += " -g " + args.groundtruth_bagfile - lu.run_command_and_save_output(plot_results_command, "plot_results_command.txt") + plot_loc_results_command += " -g " + args.groundtruth_bagfile + lu.run_command_and_save_output( + plot_loc_results_command, "plot_loc_results_command.txt" + ) diff --git a/tools/localization_analysis/scripts/states.py b/tools/localization_analysis/scripts/states.py new file mode 100644 index 0000000000..ff38bcb281 --- /dev/null +++ b/tools/localization_analysis/scripts/states.py @@ -0,0 +1,216 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +import numpy as np +import scipy.spatial.transform + +# States used for plotting/analysis tools + + +# Acceleration object +class Acceleration(object): + def __init__(self, x, y, z): + self.x = x + self.y = y + self.z = z + + +# Accelerometer Bias object +class AccelerometerBias(object): + def __init__(self, x, y, z): + self.x = x + self.y = y + self.z = z + + +# DepthOdometry object containing information from a DepthOdometry Msg +class DepthOdometry: + def __init__(self): + self.timestamp = None + self.pose_with_covariance = None + self.num_features = None + self.runtime = None + + +# ExtrapolatedLocState object containing information from a Ekf Msg +class ExtrapolatedLocState: + def __init__(self): + self.timestamp = None + self.pose = None + self.velocity = None + self.acceleration = None + + +# GraphLocState object containing information from a GraphLocState Msg +class GraphLocState: + def __init__(self): + self.timestamp = None + self.pose_with_covariance = None + self.num_detected_ar_features = None + self.num_detected_ml_features = None + self.optimization_iterations = None + self.optimization_time = None + self.update_time = None + self.num_factors = None + self.num_ml_projection_factors = None + self.num_ml_pose_factors = None + self.num_states = None + self.duration = None + + +# GraphVIOState object containing information from a GraphVIOState Msg +class GraphVIOState: + def __init__(self): + self.timestamp = None + self.pose_with_covariance = None + self.velocity_with_covariance = None + self.imu_bias_with_covariance = None + self.num_detected_of_features = None + self.num_of_factors = None + self.num_depth_factors = None + self.num_states = None + self.optimization_iterations = None + self.optimization_time = None + self.update_time = None + self.duration = None + self.standstill = None + + +# Gyroscope Bias object +class GyroscopeBias(object): + def __init__(self, x, y, z): + self.x = x + self.y = y + self.z = z + + +class ImuBias(object): + def __init__(self, accelerometer_bias, gyroscope_bias): + self.accelerometer_bias = accelerometer_bias + self.gyroscope_bias = gyroscope_bias + + +# Class that contains an imu bias and covariance. +class ImuBiasWithCovariance(ImuBias): + def __init__(self, accelerometer_bias, gyroscope_bias, covariance): + super(ImuBiasWithCovariance, self).__init__(accelerometer_bias, gyroscope_bias) + self.covariance = covariance + + +# Position class that contains x, y, and z values. +class Position(np.ndarray): + def __new__(cls, position_vector): + self = np.asarray(position_vector).view(cls) + self.x = position_vector[0] + self.y = position_vector[1] + self.z = position_vector[2] + return self + + +# Pose class that contains an orientation and position and supports pose multiplication. +class Pose(object): + def __init__(self, orientation, position): + self.orientation = orientation + self.position = position + + # Right multiply the pose by pose_b and return the resulting pose. + def __mul__(self, pose_b): + new_orientation = self.orientation * pose_b.orientation + new_position = Position(self.orientation.apply(pose_b.position) + self.position) + return Pose(new_orientation, new_position) + + # Invert the pose + def inverse(self): + new_orientation = self.orientation.inv() + new_position = Position(-1.0 * new_orientation.apply(self.position)) + return Pose(new_orientation, new_position) + + # Returns the orientation as ZYX euler angles (YPR). + def euler_angles(self): + return self.orientation.as_euler("ZYX", degrees=True) + + # Returns the position. + def position(self): + return self.position + + +# Class that contains a pose and covariance. +class PoseWithCovariance(Pose): + def __init__(self, orientation, position, covariance): + super(PoseWithCovariance, self).__init__(orientation, position) + self.covariance = covariance + + +# Velocity object +class Velocity(object): + def __init__(self, x, y, z): + self.x = x + self.y = y + self.z = z + + +# Class that contains a velocity and covariance. +class VelocityWithCovariance(Velocity): + def __init__(self, x, y, z, covariance): + super(VelocityWithCovariance, self).__init__(x, y, z) + self.covariance = covariance + + +# Class that contains a timestamped acceleration +class TimestampedAcceleration(Acceleration): + def __init__(self, x, y, z, timestamp): + super(TimestampedAcceleration, self).__init__(x, y, z) + self.timestamp = timestamp + + +# Class that contains a timestamped imu_bias and covariance. +class TimestampedImuBiasWithCovariance(ImuBiasWithCovariance): + def __init__(self, imu_bias, covariance, timestamp): + self.imu_bias = imu_bias + self.covariance = covariance + self.timestamp = timestamp + + +# Class that contains a timestamped pose. +class TimestampedPose(Pose): + def __init__(self, orientation, position, timestamp): + super(TimestampedPose, self).__init__(orientation, position) + self.timestamp = timestamp + + +# Class that contains a timestamped pose and covariance. +class TimestampedPoseWithCovariance(PoseWithCovariance): + def __init__(self, orientation, position, covariance, timestamp): + super(TimestampedPoseWithCovariance, self).__init__(orientation, position) + self.timestamp = timestamp + + +# Class that contains a timestamped velocity and covariance. +class TimestampedVelocity(Velocity): + def __init__(self, x, y, z, timestamp): + super(TimestampedVelocity, self).__init__(x, y, z) + self.timestamp = timestamp + + +# Class that contains a timestamped velocity and covariance. +class TimestampedVelocityWithCovariance(VelocityWithCovariance): + def __init__(self, velocity, covariance, timestamp): + self.velocity = velocity + self.covariance = covariance + self.timestamp = timestamp diff --git a/tools/localization_analysis/scripts/test_rmse_utilities.py b/tools/localization_analysis/scripts/test_rmse_calculators.py similarity index 99% rename from tools/localization_analysis/scripts/test_rmse_utilities.py rename to tools/localization_analysis/scripts/test_rmse_calculators.py index 2b788d63fd..f9018e95e1 100644 --- a/tools/localization_analysis/scripts/test_rmse_utilities.py +++ b/tools/localization_analysis/scripts/test_rmse_calculators.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. @@ -21,7 +21,6 @@ import unittest import numpy as np - import poses import rmse_utilities diff --git a/tools/localization_analysis/scripts/utilities.py b/tools/localization_analysis/scripts/utilities.py deleted file mode 100644 index dd391c9441..0000000000 --- a/tools/localization_analysis/scripts/utilities.py +++ /dev/null @@ -1,114 +0,0 @@ -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import numpy as np - -import pose -import poses - - -def make_absolute_poses_from_relative_poses(absolute_poses, relative_poses, name): - starting_relative_time = relative_poses.times[0] - np_times = np.array(absolute_poses.times) - closest_index = np.argmin(np.abs(np_times - starting_relative_time)) - start_pose = absolute_poses.pose(closest_index) - new_pose = start_pose - new_poses_list = [start_pose] - new_poses_times = [absolute_poses.times[closest_index]] - for index in range(len(relative_poses.times)): - relative_pose = relative_poses.pose(index) - new_pose = new_pose * relative_pose - new_poses_list.append(new_pose) - new_poses_times.append(relative_poses.times[index]) - new_poses = poses.Poses(name, "") - new_poses.init_from_poses(new_poses_list, new_poses_times) - return new_poses - - -def integrate_velocities(localization_states): - delta_times = [ - j - i - for i, j in zip(localization_states.times[:-1], localization_states.times[1:]) - ] - # Make sure times are same length as velocities, ignore last velocity - delta_times.append(0) - # TODO(rsoussan): Integrate angular velocities? - # TODO(rsoussan): central difference instead? - x_increments = [ - velocity * delta_t - for velocity, delta_t in zip(localization_states.velocities.xs, delta_times) - ] - y_increments = [ - velocity * delta_t - for velocity, delta_t in zip(localization_states.velocities.ys, delta_times) - ] - z_increments = [ - velocity * delta_t - for velocity, delta_t in zip(localization_states.velocities.zs, delta_times) - ] - - return add_increments_to_absolute_pose( - x_increments, - y_increments, - z_increments, - localization_states.positions.xs[0], - localization_states.positions.ys[0], - localization_states.positions.zs[0], - localization_states.times, - "Integrated Graph Velocities", - ) - - -def add_increments_to_absolute_pose( - x_increments, - y_increments, - z_increments, - starting_x, - starting_y, - starting_z, - times, - poses_name="Increment Poses", -): - integrated_positions = poses.Poses(poses_name, "") - cumulative_x_increments = np.cumsum(x_increments) - integrated_positions.positions.xs = [ - starting_x + cumulative_x_increment - for cumulative_x_increment in cumulative_x_increments - ] - cumulative_y_increments = np.cumsum(y_increments) - integrated_positions.positions.ys = [ - starting_y + cumulative_y_increment - for cumulative_y_increment in cumulative_y_increments - ] - cumulative_z_increments = np.cumsum(z_increments) - integrated_positions.positions.zs = [ - starting_z + cumulative_z_increment - for cumulative_z_increment in cumulative_z_increments - ] - - # Add start positions - integrated_positions.positions.xs.insert(0, starting_x) - integrated_positions.positions.ys.insert(0, starting_y) - integrated_positions.positions.zs.insert(0, starting_z) - - # Remove last elements (no timestamp for these) - del integrated_positions.positions.xs[-1] - del integrated_positions.positions.ys[-1] - del integrated_positions.positions.zs[-1] - - integrated_positions.times = times - return integrated_positions diff --git a/tools/localization_analysis/scripts/vector3d_plotter.py b/tools/localization_analysis/scripts/vector3d_plotter.py deleted file mode 100644 index 620959709b..0000000000 --- a/tools/localization_analysis/scripts/vector3d_plotter.py +++ /dev/null @@ -1,220 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import matplotlib - -import poses -import vector3ds - -matplotlib.use("pdf") -import matplotlib.pyplot as plt -import numpy as np - - -def unwrap_in_degrees(angles): - return np.rad2deg(np.unwrap(np.deg2rad(angles))) - - -class Vector3dPlotter: - def __init__(self, xlabel, ylabel, title, individual_plots): - self.xlabel = xlabel - self.ylabel = ylabel - self.title = title - self.individual_plots = individual_plots - self.y_vals_vec = [] - - def add_pose_position( - self, - pose, - colors=["r", "b", "g"], - linestyle="-", - linewidth=1, - marker=None, - markeredgewidth=None, - markersize=1, - ): - position_plotter = Vector3dYVals( - pose.pose_type, - pose.times, - pose.positions.xs, - pose.positions.ys, - pose.positions.zs, - ["Pos. (X)", "Pos. (Y)", "Pos. (Z)"], - colors, - linestyle, - linewidth, - marker, - markeredgewidth, - markersize, - ) - self.add_y_vals(position_plotter) - - def add_pose_orientation( - self, - pose, - colors=["r", "b", "g"], - linestyle="-", - linewidth=1, - marker=None, - markeredgewidth=None, - markersize=1, - ): - orientation_plotter = Vector3dYVals( - pose.pose_type, - pose.times, - unwrap_in_degrees(pose.orientations.yaws), - unwrap_in_degrees(pose.orientations.rolls), - unwrap_in_degrees(pose.orientations.pitches), - ["Orientation (Yaw)", "Orientation (Roll)", "Orientation (Pitch)"], - colors, - linestyle, - linewidth, - marker, - markeredgewidth, - markersize, - ) - self.add_y_vals(orientation_plotter) - - def add_y_vals(self, y_vals): - self.y_vals_vec.append(y_vals) - - def plot(self, pdf, individual_plots=True): - plt.figure() - for y_vals in self.y_vals_vec: - y_vals.full_plot() - plt.xlabel(self.xlabel) - plt.ylabel(self.ylabel) - plt.title(self.title) - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - if individual_plots: - self.plot_xs(pdf) - self.plot_ys(pdf) - self.plot_zs(pdf) - - def plot_xs(self, pdf): - plt.figure() - for y_vals in self.y_vals_vec: - y_vals.plot_x() - plt.xlabel(self.xlabel) - plt.ylabel(self.ylabel) - plt.title(self.title) - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - def plot_ys(self, pdf): - plt.figure() - for y_vals in self.y_vals_vec: - y_vals.plot_y() - plt.xlabel(self.xlabel) - plt.ylabel(self.ylabel) - plt.title(self.title) - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - def plot_zs(self, pdf): - plt.figure() - for y_vals in self.y_vals_vec: - y_vals.plot_z() - plt.xlabel(self.xlabel) - plt.ylabel(self.ylabel) - plt.title(self.title) - plt.legend(prop={"size": 6}) - pdf.savefig() - plt.close() - - -class Vector3dYVals: - def __init__( - self, - name, - x_axis_vals, - x_vals, - y_vals, - z_vals, - labels, - colors=["r", "b", "g"], - linestyle="-", - linewidth=1, - marker=None, - markeredgewidth=None, - markersize=1, - ): - self.x_vals = x_vals - self.y_vals = y_vals - self.z_vals = z_vals - self.x_axis_vals = x_axis_vals - self.name = name - self.x_label = name + " " + labels[0] - self.y_label = name + " " + labels[1] - self.z_label = name + " " + labels[2] - self.x_color = colors[0] - self.y_color = colors[1] - self.z_color = colors[2] - self.linestyle = linestyle - self.linewidth = linewidth - self.marker = marker - self.markeredgewidth = markeredgewidth - self.markersize = markersize - - def full_plot(self): - self.plot_x() - self.plot_y() - self.plot_z() - - def plot_x(self): - plt.plot( - self.x_axis_vals, - self.x_vals, - label=self.x_label, - color=self.x_color, - linestyle=self.linestyle, - marker=self.marker, - markeredgewidth=self.markeredgewidth, - markersize=self.markersize, - ) - - def plot_y(self): - plt.plot( - self.x_axis_vals, - self.y_vals, - label=self.y_label, - color=self.y_color, - linewidth=self.linewidth, - linestyle=self.linestyle, - marker=self.marker, - markeredgewidth=self.markeredgewidth, - markersize=self.markersize, - ) - - def plot_z(self): - plt.plot( - self.x_axis_vals, - self.z_vals, - label=self.z_label, - color=self.z_color, - linestyle=self.linestyle, - marker=self.marker, - markeredgewidth=self.markeredgewidth, - markersize=self.markersize, - ) diff --git a/tools/localization_analysis/scripts/vector3ds.py b/tools/localization_analysis/scripts/vector3ds.py deleted file mode 100644 index 387ebdd790..0000000000 --- a/tools/localization_analysis/scripts/vector3ds.py +++ /dev/null @@ -1,45 +0,0 @@ -#!/usr/bin/python -# -# Copyright (c) 2017, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. -# -# All rights reserved. -# -# The Astrobee platform is licensed under the Apache License, Version 2.0 -# (the "License"); you may not use this file except in compliance with the -# License. You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations -# under the License. - -import numpy as np - -import vector3d - - -class Vector3ds: - def __init__(self): - self.xs = [] - self.ys = [] - self.zs = [] - - def add(self, x, y, z): - self.xs.append(x) - self.ys.append(y) - self.zs.append(z) - - def add_vector3d(self, vector3d): - self.xs.append(vector3d.x) - self.ys.append(vector3d.y) - self.zs.append(vector3d.z) - - def get_vector3d(self, index): - return vector3d.Vector3d(self.xs[index], self.ys[index], self.zs[index]) - - def get_numpy_vector(self, index): - return np.array([self.xs[index], self.ys[index], self.zs[index]]) diff --git a/tools/localization_analysis/scripts/vio_results_plotter.py b/tools/localization_analysis/scripts/vio_results_plotter.py new file mode 100755 index 0000000000..16659b1214 --- /dev/null +++ b/tools/localization_analysis/scripts/vio_results_plotter.py @@ -0,0 +1,353 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +""" +Creates plots for VIO results. Adds plots for poses, velocities, IMU bias values, +integrated velocities forming poses, and detectected +features counts. Optionally plots groundtruth poses along with other pose data if groundtruth is provided. +""" + +import argparse +import os +import sys + +import matplotlib + +import message_reader +import plot_conversions +import plotters +import results_savers + +matplotlib.use("pdf") +import csv +import math + +import geometry_msgs +import matplotlib.pyplot as plt +import rosbag +from matplotlib.backends.backend_pdf import PdfPages + + +def plot_vio_results( + pdf, + results_csv_file, + groundtruth_poses, + graph_vio_states, + imu_bias_extrapolated_poses, + depth_odometries, +): + poses_plotter = plotters.MultiPosePlotter( + "Time (s)", "Position (m)", "Graph vs. Groundtruth Position", True + ) + poses_plotter.add_poses( + "Groundtruth Poses", + groundtruth_poses, + linestyle="None", + marker="o", + markeredgewidth=0.1, + markersize=1.5, + ) + + graph_vio_poses = plot_conversions.adjusted_graph_vio_poses_from_graph_vio_states( + graph_vio_states, groundtruth_poses + ) + poses_plotter.add_poses("Graph VIO Poses", graph_vio_poses, linestyle="-") + poses_plotter.plot(pdf) + results_savers.save_rmse( + graph_vio_poses, groundtruth_poses, results_csv_file, pdf, "Graph VIO Poses" + ) + + velocities_plotter = plotters.MultiVector3dPlotter( + "Time (s)", "Velocity (m/s)", "Graph VIO Velocities", True + ) + graph_vio_velocity_plotter = ( + plot_conversions.velocity_plotter_from_graph_vio_states(graph_vio_states) + ) + velocities_plotter.add(graph_vio_velocity_plotter) + velocities_plotter.plot(pdf) + + accel_bias_plotters = plotters.MultiVector3dPlotter( + "Time (s)", "Accelerometer Bias (m/s^2)", "Graph VIO Accel. Biases", True + ) + graph_vio_accel_bias_plotter = ( + plot_conversions.accel_bias_plotter_from_graph_vio_states(graph_vio_states) + ) + accel_bias_plotters.add(graph_vio_accel_bias_plotter) + accel_bias_plotters.plot(pdf) + + gyro_bias_plotters = plotters.MultiVector3dPlotter( + "Time (s)", "Gyro Bias (rad/s^2)", "Graph VIO Gyro. Biases", True + ) + graph_vio_gyro_bias_plotter = ( + plot_conversions.gyro_bias_plotter_from_graph_vio_states(graph_vio_states) + ) + gyro_bias_plotters.add(graph_vio_gyro_bias_plotter) + gyro_bias_plotters.plot(pdf) + + of_count_plotter = ( + plot_conversions.optical_flow_feature_count_plotter_from_graph_vio_states( + graph_vio_states + ) + ) + of_count_plotter.plot(pdf) + + of_num_factors_plotter = ( + plot_conversions.optical_flow_factor_count_plotter_from_graph_vio_states( + graph_vio_states + ) + ) + of_num_factors_plotter.plot(pdf) + + depth_num_factors_plotter = ( + plot_conversions.depth_factor_count_plotter_from_graph_vio_states( + graph_vio_states + ) + ) + depth_num_factors_plotter.plot(pdf) + + integrated_velocity_poses = ( + plot_conversions.absolute_poses_from_integrated_graph_vio_state_velocities( + graph_vio_states, groundtruth_poses + ) + ) + integrated_velocity_poses_plotter = plotters.MultiPosePlotter( + "Time (s)", + "Position (m)", + "Integrated Velocities vs. Groundtruth Position", + True, + ) + integrated_velocity_poses_plotter.add_poses( + "Groundtruth Poses", + groundtruth_poses, + linestyle="None", + marker="o", + markeredgewidth=0.1, + markersize=1.5, + ) + integrated_velocity_poses_plotter.add_poses( + "Graph VIO Integrated Velocity Poses", integrated_velocity_poses, linestyle="-" + ) + integrated_velocity_poses_plotter.plot_positions(pdf) + results_savers.save_rmse( + integrated_velocity_poses, + groundtruth_poses, + results_csv_file, + pdf, + "Graph VIO Integrated Velocity Poses", + ) + + if len(imu_bias_extrapolated_poses) != 0: + absolute_imu_bias_extrapolated_poses = ( + plot_conversions.absolute_poses_from_imu_bias_extrapolated_poses( + imu_bias_extrapolated_poses, groundtruth_poses + ) + ) + imu_bias_extrapolated_poses_plotter = plotters.MultiPosePlotter( + "Time (s)", + "Position (m)", + "IMU Bias Extrapolated vs. Groundtruth Position", + True, + ) + imu_bias_extrapolated_poses_plotter.add_poses( + "Groundtruth Poses", + groundtruth_poses, + linestyle="None", + marker="o", + markeredgewidth=0.1, + markersize=1.5, + ) + imu_bias_extrapolated_poses_plotter.add_poses( + "IMU Bias Extrapolated Poses", + absolute_imu_bias_extrapolated_poses, + linestyle="-", + ) + imu_bias_extrapolated_poses_plotter.plot(pdf) + results_savers.save_rmse( + imu_bias_extrapolated_poses, + groundtruth_poses, + results_csv_file, + pdf, + "IMU Bias Extrapolated Poses", + ) + + if len(depth_odometries) != 0: + absolute_depth_odom_relative_poses = ( + plot_conversions.absolute_poses_from_relative_poses( + plot_conversions.poses_from_depth_odometries(depth_odometries), + groundtruth_poses, + ) + ) + depth_odom_relative_poses_plotter = plotters.MultiPosePlotter( + "Time (s)", "Position (m)", "Depth Odometry vs. Groundtruth Position", True + ) + depth_odom_relative_poses_plotter.add_poses( + "Groundtruth Poses", + groundtruth_poses, + linestyle="None", + marker="o", + markeredgewidth=0.1, + markersize=1.5, + ) + depth_odom_relative_poses_plotter.add_poses( + "Depth Odometry Poses", absolute_depth_odom_relative_poses, linestyle="-" + ) + depth_odom_relative_poses_plotter.plot(pdf) + + num_features_plotter = ( + plot_conversions.num_features_plotter_from_depth_odometries( + depth_odometries + ) + ) + num_features_plotter.plot(pdf) + + runtime_plotter = plot_conversions.runtime_plotter_from_depth_odometries( + depth_odometries + ) + runtime_plotter.plot(pdf) + + standstill_plotter = plot_conversions.standstill_plotter_from_states( + graph_vio_states + ) + standstill_plotter.plot(pdf) + + num_states_plotter = plot_conversions.num_states_plotter_from_states( + graph_vio_states + ) + num_states_plotter.plot(pdf) + + duration_plotter = plot_conversions.duration_plotter_from_states(graph_vio_states) + duration_plotter.plot(pdf) + + optimization_time_plotter = plot_conversions.optimization_time_plotter_from_states( + graph_vio_states + ) + optimization_time_plotter.plot(pdf) + + update_time_plotter = plot_conversions.update_time_plotter_from_states( + graph_vio_states + ) + update_time_plotter.plot(pdf) + + optimization_iterations_plotter = ( + plot_conversions.optimization_iterations_plotter_from_states(graph_vio_states) + ) + optimization_iterations_plotter.plot(pdf) + + +# Loads poses from the provided bagfile, generates plots, and saves results to a pdf and csv file. +# The csv file contains results in (TODO: define format). +# The RMSE rel start and end time define the time range for evaluating the RMSE, in case the bag starts or ends with little/uninteresting movement +# that shouldn't be included in the RMSE calculation. +# The groundtruth bag must have the same start time as other bagfile, otherwise RMSE calculations will be flawed +def load_data_and_create_vio_plots( + bagfile, + output_pdf_file, + results_csv_file="vio_results.csv", + groundtruth_bagfile=None, + rmse_rel_start_time=0, + rmse_rel_end_time=-1, +): + # Load bagfile with VIO results + bag = rosbag.Bag(bagfile) + # Load bagfile with groundtruth poses. Assume groundtruth is in the same results bag + # if no separate bagfile for groundtruth is provided. + groundtruth_bag = rosbag.Bag(groundtruth_bagfile) if groundtruth_bagfile else bag + bag_start_time = bag.get_start_time() + + # Load groundtruth poses + # Use sparse mapping poses as groundtruth. + groundtruth_poses = [] + message_reader.load_poses( + groundtruth_poses, "/sparse_mapping/pose", groundtruth_bag, bag_start_time + ) + + # Load graph VIO states + graph_vio_states = [] + message_reader.load_graph_vio_states( + graph_vio_states, "/graph_vio/state", bag, bag_start_time + ) + + # Load IMU bias extrapolated poses + imu_bias_extrapolated_poses = [] + message_reader.load_poses( + imu_bias_extrapolated_poses, "/imu_bias_extrapolator/pose", bag, bag_start_time + ) + + # Load Depth Odometries + depth_odometries = message_reader.load_depth_odometries( + "/loc/depth/odom", bag, bag_start_time + ) + bag.close() + + with PdfPages(output_pdf_file) as pdf: + plot_vio_results( + pdf, + results_csv_file, + groundtruth_poses, + graph_vio_states, + imu_bias_extrapolated_poses, + depth_odometries, + ) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument("bagfile", help="Input bagfile.") + parser.add_argument( + "--output-file", default="vio_output.pdf", help="Output pdf file." + ) + parser.add_argument( + "--results-csv-file", + default="vio_results.csv", + help="Output csv file containing results stats.", + ) + parser.add_argument( + "-g", + "--groundtruth-bagfile", + default=None, + help="Optional bagfile containing groundtruth poses to use as a comparison for poses in the input bagfile. If none provided, sparse mapping poses are used as groundtruth from the input bagfile if available.", + ) + parser.add_argument( + "--rmse-rel-start-time", + type=float, + default=0, + help="Optional start time for plots.", + ) + parser.add_argument( + "--rmse-rel-end-time", + type=float, + default=-1, + help="Optional end time for plots.", + ) + args = parser.parse_args() + if not os.path.isfile(args.bagfile): + print(("Bag file " + args.bagfile + " does not exist.")) + sys.exit() + if args.groundtruth_bagfile and not os.path.isfile(args.groundtruth_bagfile): + print(("Groundtruth Bag file " + args.groundtruth_bagfile + " does not exist.")) + sys.exit() + load_data_and_create_vio_plots( + args.bagfile, + args.output_file, + args.results_csv_file, + args.groundtruth_bagfile, + args.rmse_rel_start_time, + args.rmse_rel_end_time, + ) diff --git a/tools/localization_analysis/src/feature_track_image_adder.cc b/tools/localization_analysis/src/feature_track_image_adder.cc new file mode 100644 index 0000000000..f5e94dd53e --- /dev/null +++ b/tools/localization_analysis/src/feature_track_image_adder.cc @@ -0,0 +1,112 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_analysis/feature_track_image_adder.h> + +#include <cv_bridge/cv_bridge.h> + +namespace localization_analysis { +namespace vc = vision_common; + +void FeatureTrackImage(const vc::SpacedFeatureTracker& feature_tracker, const camera::CameraParameters& camera_params, + cv::Mat& feature_track_image) { + const auto& feature_tracks = feature_tracker.feature_tracks(); + for (const auto& feature_track_pair : feature_tracks) { + const auto& feature_track = feature_track_pair.second; + // Color using unique color for id + const int id = feature_track.set().crbegin()->second.feature_track_id; + const cv::Scalar color = cv::Scalar((id * 123) % 255, (id * 456) % 255, (id * 789) % 255); + + // Draw track history + if (feature_track.size() > 1) { + for (auto point_it = feature_track.set().begin(); point_it != std::prev(feature_track.set().end()); ++point_it) { + const auto& point1 = point_it->second.image_point; + const auto& point2 = std::next(point_it)->second.image_point; + const auto distorted_previous_point = Distort(point1, camera_params); + const auto distorted_current_point = Distort(point2, camera_params); + cv::circle(feature_track_image, distorted_current_point, 2, cv::Scalar(0, 255, 255), -1, 8); + cv::line(feature_track_image, distorted_current_point, distorted_previous_point, color, 2, 8, 0); + } + } else { + cv::circle(feature_track_image, Distort(feature_track.set().crbegin()->second.image_point, camera_params), 2, + color, -1, 8); + } + // Draw feature id at most recent point + cv::putText(feature_track_image, std::to_string(feature_track.set().crbegin()->second.feature_track_id), + Distort(feature_track.set().crbegin()->second.image_point, camera_params), cv::FONT_HERSHEY_PLAIN, 2, + cv::Scalar(255, 0, 0)); + } +} + +void MarkSmartFactorPoints(const std::vector<boost::shared_ptr<const factor_adders::RobustSmartFactor>> smart_factors, + const camera::CameraParameters& camera_params, const gtsam::Values& values, + cv::Mat& feature_track_image) { + for (const auto& smart_factor : smart_factors) { + const auto& points = smart_factor->measured(); + const auto& latest_point = points.front(); + const auto distorted_point = Distort(latest_point, camera_params); + const cv::Point2f rectangle_offset(25, 25); + cv::rectangle(feature_track_image, distorted_point + rectangle_offset, distorted_point - rectangle_offset, + cv::Scalar(200, 100, 0), 4, 8); + + // Draw track history using gray points + for (const auto& point : points) { + const auto distorted_point = Distort(point, camera_params); + cv::circle(feature_track_image, distorted_point, 6 /* Radius*/, cv::Scalar(100, 100, 100), -1 /*Filled*/, 8); + } + // Draw reprojected triangulated point in blue, draw red line to latest measurement showing projection error + const auto triangulated_point = smart_factor->serialized_point(values); + const auto cameras = smart_factor->cameras(values); + + // Cameras are in same order as keys + const auto& measurements = smart_factor->measured(); + if (triangulated_point) { + // Latest camera is first camera + const auto projected_point = cameras.front().project2(*triangulated_point); + const auto distorted_projected_point = localization_analysis::Distort(projected_point, camera_params); + cv::circle(feature_track_image, distorted_projected_point, 7 /* Radius*/, cv::Scalar(255, 0, 0), -1 /*Filled*/, + 8); + cv::line(feature_track_image, distorted_projected_point, distorted_point, cv::Scalar(0, 0, 255), 2, 8, 0); + } + } +} + +boost::optional<sensor_msgs::ImagePtr> CreateFeatureTrackImage( + const sensor_msgs::ImageConstPtr& image_msg, const vc::SpacedFeatureTracker& feature_tracker, + const camera::CameraParameters& camera_params, + const std::vector<boost::shared_ptr<const factor_adders::RobustSmartFactor>>& smart_factors, + const gtsam::Values& values) { + cv_bridge::CvImagePtr feature_track_image; + try { + feature_track_image = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::RGB8); + } catch (cv_bridge::Exception& e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return boost::none; + } + + FeatureTrackImage(feature_tracker, camera_params, feature_track_image->image); + MarkSmartFactorPoints(smart_factors, camera_params, values, feature_track_image->image); + return feature_track_image->toImageMsg(); +} + +cv::Point2f Distort(const Eigen::Vector2d& undistorted_point, const camera::CameraParameters& params) { + Eigen::Vector2d distorted_point; + params.Convert<camera::UNDISTORTED_C, camera::DISTORTED>(undistorted_point, &distorted_point); + return cv::Point2f(distorted_point.x(), distorted_point.y()); +} +} // namespace localization_analysis diff --git a/tools/localization_analysis/src/graph_bag.cc b/tools/localization_analysis/src/graph_bag.cc deleted file mode 100644 index 3963078b79..0000000000 --- a/tools/localization_analysis/src/graph_bag.cc +++ /dev/null @@ -1,197 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <ff_common/utils.h> -#include <ff_common/ff_names.h> -#include <localization_analysis/graph_bag.h> -#include <localization_analysis/parameter_reader.h> -#include <localization_analysis/utilities.h> -#include <graph_localizer/utilities.h> -#include <localization_common/logger.h> -#include <localization_common/utilities.h> -#include <msg_conversions/msg_conversions.h> - -#include <geometry_msgs/PoseStamped.h> -#include <geometry_msgs/Vector3Stamped.h> -#include <ros/time.h> -#include <sensor_msgs/Imu.h> - -#include <Eigen/Core> - -#include <chrono> -#include <cstdlib> -#include <vector> - -namespace localization_analysis { -namespace gl = graph_localizer; -namespace lc = localization_common; -namespace mc = msg_conversions; - -GraphBag::GraphBag(const std::string& bag_name, const std::string& map_file, const std::string& image_topic, - const std::string& results_bag, const std::string& output_stats_file, const bool use_image_features, - const std::string& graph_config_path_prefix) - : results_bag_(results_bag, rosbag::bagmode::Write), - imu_bias_tester_wrapper_(graph_config_path_prefix), - imu_augmentor_wrapper_(graph_config_path_prefix), - output_stats_file_(output_stats_file) { - config_reader::ConfigReader config; - config.AddFile("cameras.config"); - config.AddFile("geometry.config"); - config.AddFile("tools/graph_bag.config"); - lc::LoadGraphLocalizerConfig(config, graph_config_path_prefix); - - if (!config.ReadFiles()) { - LogFatal("Failed to read config files."); - } - - LiveMeasurementSimulatorParams params; - LoadLiveMeasurementSimulatorParams(config, bag_name, map_file, image_topic, params); - // Load this seperately so this can be set independently of config file, - // i.e. when running a bag sweep or param sweep - // TODO(rsoussan): clean this up - params.use_image_features = use_image_features; - live_measurement_simulator_.reset(new LiveMeasurementSimulator(params)); - - GraphLocalizerSimulatorParams graph_params; - LoadGraphLocalizerSimulatorParams(config, graph_params); - graph_localizer_simulator_.reset(new GraphLocalizerSimulator(graph_params, graph_config_path_prefix)); - - LoadGraphBagParams(config, params_); -} - -void GraphBag::SaveOpticalFlowTracksImage(const sensor_msgs::ImageConstPtr& image_msg, - const GraphLocalizerSimulator& graph_localizer) { - std::vector<const SmartFactor*> smart_factors; - if (graph_localizer.graph_localizer()) smart_factors = SmartFactors(*(graph_localizer.graph_localizer())); - const auto feature_track_image_msg = - CreateFeatureTrackImage(image_msg, *(graph_localizer.feature_tracks()), *params_.nav_cam_params, smart_factors); - if (!feature_track_image_msg) return; - SaveMsg(**feature_track_image_msg, kFeatureTracksImageTopic_, results_bag_); -} - -void GraphBag::Run() { - // Required to start bias estimation - graph_localizer_simulator_->ResetBiasesAndLocalizer(); - lc::Timer graph_bag_timer("Graph Bag Timer"); - graph_bag_timer.Start(); - const double start_time = live_measurement_simulator_->CurrentTime(); - while (live_measurement_simulator_->ProcessMessage()) { - const lc::Time current_time = live_measurement_simulator_->CurrentTime(); - if (params_.log_relative_time) LogInfo("Run: Rel t: " << current_time - start_time); - const auto flight_mode_msg = live_measurement_simulator_->GetFlightModeMessage(current_time); - if (flight_mode_msg) { - graph_localizer_simulator_->BufferFlightModeMsg(*flight_mode_msg); - imu_augmentor_wrapper_.FlightModeCallback(*flight_mode_msg); - } - const auto imu_msg = live_measurement_simulator_->GetImuMessage(current_time); - if (imu_msg) { - graph_localizer_simulator_->BufferImuMsg(*imu_msg); - imu_augmentor_wrapper_.ImuCallback(*imu_msg); - imu_bias_tester_wrapper_.ImuCallback(*imu_msg); - - // Save imu augmented loc msg if available - const auto imu_augmented_loc_msg = imu_augmentor_wrapper_.LatestImuAugmentedLocalizationMsg(); - if (!imu_augmented_loc_msg) { - LogWarningEveryN(50, "Run: Failed to get latest imu augmented loc msg."); - } else { - SaveMsg(*imu_augmented_loc_msg, TOPIC_GNC_EKF, results_bag_); - } - } - const auto depth_odometry_msg = live_measurement_simulator_->GetDepthOdometryMessage(current_time); - if (depth_odometry_msg) { - graph_localizer_simulator_->BufferDepthOdometryMsg(*depth_odometry_msg); - } - const auto of_msg = live_measurement_simulator_->GetOFMessage(current_time); - if (of_msg) { - graph_localizer_simulator_->BufferOpticalFlowMsg(*of_msg); - if (params_.save_optical_flow_images) { - const auto img_msg = live_measurement_simulator_->GetImageMessage(lc::TimeFromHeader(of_msg->header)); - if (img_msg && graph_localizer_simulator_->feature_tracks()) - SaveOpticalFlowTracksImage(*img_msg, *graph_localizer_simulator_); - } - } - const auto vl_msg = live_measurement_simulator_->GetVLMessage(current_time); - if (vl_msg) { - graph_localizer_simulator_->BufferVLVisualLandmarksMsg(*vl_msg); - SaveMsg(*vl_msg, TOPIC_LOCALIZATION_ML_FEATURES, - results_bag_); - if (gl::ValidVLMsg(*vl_msg, params_.sparse_mapping_min_num_landmarks)) { - const gtsam::Pose3 sparse_mapping_global_T_body = - lc::PoseFromMsgWithExtrinsics(vl_msg->pose, params_.body_T_nav_cam.inverse()); - const lc::Time timestamp = lc::TimeFromHeader(vl_msg->header); - SaveMsg(graph_localizer::PoseMsg(sparse_mapping_global_T_body, timestamp), TOPIC_SPARSE_MAPPING_POSE, - results_bag_); - } - } - const auto ar_msg = live_measurement_simulator_->GetARMessage(current_time); - if (ar_msg) { - static bool marked_world_T_dock_for_resetting_if_necessary = false; - // In lieu of doing this on a mode switch to AR_MODE, reset world_T_dock using loc if necessary when receive first - // ar msg - if (!marked_world_T_dock_for_resetting_if_necessary) { - graph_localizer_simulator_->MarkWorldTDockForResettingIfNecessary(); - marked_world_T_dock_for_resetting_if_necessary = true; - } - graph_localizer_simulator_->BufferARVisualLandmarksMsg(*ar_msg); - if (gl::ValidVLMsg(*ar_msg, params_.ar_min_num_landmarks)) { - const auto ar_tag_pose_msg = graph_localizer_simulator_->LatestARTagPoseMsg(); - if (!ar_tag_pose_msg) { - LogWarning("Run: Failed to get ar tag pose msg"); - } else { - static lc::Time last_added_timestamp = 0; - const auto timestamp = lc::TimeFromHeader(ar_tag_pose_msg->header); - // Prevent adding the same pose twice, since the pose is buffered before adding to the graph localizer - // wrapper in the graph localizer simulator and LatestARTagPoseMsg returns - // the last pose that has already been added to the graph localizer wrapper. - if (last_added_timestamp != timestamp) { - SaveMsg(*ar_tag_pose_msg, TOPIC_AR_TAG_POSE, results_bag_); - last_added_timestamp = timestamp; - } - } - } - } - - const bool updated_graph = graph_localizer_simulator_->AddMeasurementsAndUpdateIfReady(current_time); - if (updated_graph) { - // Save latest graph localization msg - // Pass latest loc state to imu augmentor if it is available. - const auto localization_msg = graph_localizer_simulator_->LatestLocalizationStateMsg(); - if (!localization_msg) { - LogWarningEveryN(50, "Run: Failed to get localization msg."); - } else { - imu_augmentor_wrapper_.LocalizationStateCallback(*localization_msg); - SaveMsg(*localization_msg, TOPIC_GRAPH_LOC_STATE, results_bag_); - const auto imu_bias_tester_predicted_states = - imu_bias_tester_wrapper_.LocalizationStateCallback(*localization_msg); - SaveImuBiasTesterPredictedStates(imu_bias_tester_predicted_states, results_bag_); - } - } - } - graph_bag_timer.Stop(); - graph_bag_timer.Log(); - const auto graph_stats = graph_localizer_simulator_->graph_localizer_stats(); - if (!graph_stats) { - LogError("Run: Failed to get graph stats"); - } else { - std::ofstream log_file; - log_file.open(output_stats_file_); - graph_stats->LogToCsv(log_file); - graph_bag_timer.LogToCsv(log_file); - } -} -} // namespace localization_analysis diff --git a/tools/localization_analysis/src/graph_localizer_simulator.cc b/tools/localization_analysis/src/graph_localizer_simulator.cc index b6f2f7829b..6a3638e438 100644 --- a/tools/localization_analysis/src/graph_localizer_simulator.cc +++ b/tools/localization_analysis/src/graph_localizer_simulator.cc @@ -22,14 +22,16 @@ namespace localization_analysis { namespace lc = localization_common; GraphLocalizerSimulator::GraphLocalizerSimulator(const GraphLocalizerSimulatorParams& params, const std::string& graph_config_path_prefix) - : GraphLocalizerWrapper(graph_config_path_prefix), params_(params) {} + : RosGraphLocalizerWrapper(graph_config_path_prefix), params_(params) {} -void GraphLocalizerSimulator::BufferOpticalFlowMsg(const ff_msgs::Feature2dArray& feature_array_msg) { - of_msg_buffer_.emplace_back(feature_array_msg); +void GraphLocalizerSimulator::BufferImuMsg(const sensor_msgs::Imu& imu_msg) { imu_msg_buffer_.emplace_back(imu_msg); } + +void GraphLocalizerSimulator::BufferFlightModeMsg(const ff_msgs::FlightMode& flight_mode_msg) { + flight_mode_msg_buffer_.emplace_back(flight_mode_msg); } -void GraphLocalizerSimulator::BufferDepthOdometryMsg(const ff_msgs::DepthOdometry& depth_odometry_msg) { - depth_odometry_msg_buffer_.emplace_back(depth_odometry_msg); +void GraphLocalizerSimulator::BufferGraphVIOStateMsg(const ff_msgs::GraphVIOState& graph_vio_state_msg) { + vio_msg_buffer_.emplace_back(graph_vio_state_msg); } void GraphLocalizerSimulator::BufferVLVisualLandmarksMsg(const ff_msgs::VisualLandmarks& visual_landmarks_msg) { @@ -40,12 +42,6 @@ void GraphLocalizerSimulator::BufferARVisualLandmarksMsg(const ff_msgs::VisualLa ar_msg_buffer_.emplace_back(visual_landmarks_msg); } -void GraphLocalizerSimulator::BufferImuMsg(const sensor_msgs::Imu& imu_msg) { imu_msg_buffer_.emplace_back(imu_msg); } - -void GraphLocalizerSimulator::BufferFlightModeMsg(const ff_msgs::FlightMode& flight_mode_msg) { - flight_mode_msg_buffer_.emplace_back(flight_mode_msg); -} - bool GraphLocalizerSimulator::AddMeasurementsAndUpdateIfReady(const lc::Time& current_time) { // If not initialized, add measurements as these are required for initialization. // Otherwise add measurements if enough time has passed since last optimization, simulating @@ -65,17 +61,14 @@ bool GraphLocalizerSimulator::AddMeasurementsAndUpdateIfReady(const lc::Time& cu ImuCallback(imu_msg); } imu_msg_buffer_.clear(); - for (const auto& depth_odometry_msg : depth_odometry_msg_buffer_) { - DepthOdometryCallback(depth_odometry_msg); - } - depth_odometry_msg_buffer_.clear(); - for (const auto& of_msg : of_msg_buffer_) { - OpticalFlowCallback(of_msg); + + for (const auto& vio_msg : vio_msg_buffer_) { + GraphVIOStateCallback(vio_msg); } - of_msg_buffer_.clear(); + vio_msg_buffer_.clear(); for (const auto& vl_msg : vl_msg_buffer_) { - VLVisualLandmarksCallback(vl_msg); + SparseMapVisualLandmarksCallback(vl_msg); } vl_msg_buffer_.clear(); diff --git a/tools/localization_analysis/src/graph_vio_simulator.cc b/tools/localization_analysis/src/graph_vio_simulator.cc new file mode 100644 index 0000000000..2ea6635230 --- /dev/null +++ b/tools/localization_analysis/src/graph_vio_simulator.cc @@ -0,0 +1,74 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_analysis/graph_vio_simulator.h> + +namespace localization_analysis { +namespace lc = localization_common; +GraphVIOSimulator::GraphVIOSimulator(const GraphVIOSimulatorParams& params, const std::string& graph_config_path_prefix) + : RosGraphVIOWrapper(graph_config_path_prefix), params_(params) {} + +void GraphVIOSimulator::BufferOpticalFlowMsg(const ff_msgs::Feature2dArray& feature_array_msg) { + of_msg_buffer_.emplace_back(feature_array_msg); +} + +void GraphVIOSimulator::BufferDepthOdometryMsg(const ff_msgs::DepthOdometry& depth_odometry_msg) { + depth_odometry_msg_buffer_.emplace_back(depth_odometry_msg); +} + +void GraphVIOSimulator::BufferImuMsg(const sensor_msgs::Imu& imu_msg) { imu_msg_buffer_.emplace_back(imu_msg); } + +void GraphVIOSimulator::BufferFlightModeMsg(const ff_msgs::FlightMode& flight_mode_msg) { + flight_mode_msg_buffer_.emplace_back(flight_mode_msg); +} + +bool GraphVIOSimulator::AddMeasurementsAndUpdateIfReady(const lc::Time& current_time) { + // If not initialized, add measurements as these are required for initialization. + // Otherwise add measurements if enough time has passed since last optimization, simulating + // optimization delay. + if (Initialized() && last_update_time_ && (current_time - *last_update_time_) < params_.optimization_time) { + return false; + } + + // Add measurements + // Add Flight Mode msgs before IMU so imu filters can be set + for (const auto& flight_mode_msg : flight_mode_msg_buffer_) { + FlightModeCallback(flight_mode_msg); + } + flight_mode_msg_buffer_.clear(); + + for (const auto& imu_msg : imu_msg_buffer_) { + ImuCallback(imu_msg); + } + imu_msg_buffer_.clear(); + + for (const auto& depth_odometry_msg : depth_odometry_msg_buffer_) { + DepthOdometryCallback(depth_odometry_msg); + } + depth_odometry_msg_buffer_.clear(); + + for (const auto& of_msg : of_msg_buffer_) { + FeaturePointsCallback(of_msg); + } + of_msg_buffer_.clear(); + + Update(); + last_update_time_ = current_time; + return true; +} +} // namespace localization_analysis diff --git a/tools/localization_analysis/src/imu_bias_extrapolator.cc b/tools/localization_analysis/src/imu_bias_extrapolator.cc new file mode 100644 index 0000000000..fa928eba2d --- /dev/null +++ b/tools/localization_analysis/src/imu_bias_extrapolator.cc @@ -0,0 +1,121 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <ff_common/ff_names.h> +#include <localization_analysis/imu_bias_extrapolator.h> +#include <localization_analysis/utilities.h> +#include <localization_common/utilities.h> +#include <localization_measurements/imu_measurement.h> +#include <msg_conversions/msg_conversions.h> +#include <parameter_reader/imu_integration.h> + +namespace ii = imu_integration; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace mc = msg_conversions; +namespace pr = parameter_reader; + +namespace localization_analysis { +void SaveExtrapolatedStates(const std::vector<lc::CombinedNavState>& extrapolated_states, rosbag::Bag& bag) { + for (const auto& state : extrapolated_states) { + geometry_msgs::PoseStamped pose_msg; + lc::PoseToMsg(state.pose(), pose_msg.pose); + lc::TimeToHeader(state.timestamp(), pose_msg.header); + const ros::Time timestamp = lc::RosTimeFromHeader(pose_msg.header); + bag.write("/" + std::string(TOPIC_IMU_BIAS_EXTRAPOLATOR_POSE), timestamp, pose_msg); + geometry_msgs::Vector3Stamped velocity_msg; + mc::VectorToMsg(state.velocity(), velocity_msg.vector); + lc::TimeToHeader(state.timestamp(), velocity_msg.header); + bag.write("/" + std::string(TOPIC_IMU_BIAS_EXTRAPOLATOR_VELOCITY), timestamp, velocity_msg); + } +} + +ImuBiasExtrapolator::ImuBiasExtrapolator(const std::string& input_bag_name, const std::string& output_bag_name) + : input_bag_(input_bag_name, rosbag::bagmode::Read), + output_bag_(output_bag_name, rosbag::bagmode::Write), + initialized_(false) { + config_reader::ConfigReader config; + config.AddFile("localization/imu_filter.config"); + config.AddFile("localization/imu_integrator.config"); + config.AddFile("transforms.config"); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + + ii::ImuIntegratorParams params; + pr::LoadImuIntegratorParams(config, params); + imu_integrator_.reset(new ii::ImuIntegrator(params)); +} + +bool ImuBiasExtrapolator::Initialized() { return initialized_; } + +void ImuBiasExtrapolator::Initialize(const lc::CombinedNavState& combined_nav_state) { + // Initialize first extrapolated state using pose/velocity/bias of first VIO state + latest_extrapolated_state_ = combined_nav_state; + initialized_ = true; +} + +std::vector<lc::CombinedNavState> ImuBiasExtrapolator::VIOStateCallback( + const ff_msgs::GraphVIOState& graph_vio_state_msg) { + const auto& latest_state_msg = graph_vio_state_msg.combined_nav_states.combined_nav_states.back(); + const auto combined_nav_state = lc::CombinedNavStateFromMsg(latest_state_msg); + combined_nav_states_.Add(combined_nav_state.timestamp(), combined_nav_state); + if (imu_integrator_->empty()) return {}; + // Remove old combined nav states + const auto oldest_imu_time = *(imu_integrator_->OldestTimestamp()); + combined_nav_states_.RemoveBelowLowerBoundValues(oldest_imu_time); + std::vector<lc::CombinedNavState> extrapolated_states; + // Create integrated imu states between successive combined nav states using the + // older combined nav state's IMU bias values. + // Make sure an upper bound combined nav state exists before generating imu states + // so each generated imu state uses the closest lower bound nav state biases. + // Wait until imu measurement timestamps exceed the upper bound state before removing + // combined nav states. + while (combined_nav_states_.size() >= 2) { + auto lower_bound_state_it = combined_nav_states_.set().begin(); + const auto upper_bound_timestamp = std::next(lower_bound_state_it)->first; + if (*(imu_integrator_->LatestTimestamp()) < upper_bound_timestamp) break; + if (!Initialized()) Initialize(lower_bound_state_it->second); + imu_integrator_->RemoveOldValues(lower_bound_state_it->second.timestamp()); + // Initialize starting state using latest extrapolated pose/velocity/timestamp + // but latest received nav state bias so the new bias is used for integration. + const lc::CombinedNavState starting_state(latest_extrapolated_state_.nav_state(), + lower_bound_state_it->second.bias(), + latest_extrapolated_state_.timestamp()); + latest_extrapolated_state_ = *(imu_integrator_->Extrapolate(starting_state, upper_bound_timestamp)); + extrapolated_states.emplace_back(latest_extrapolated_state_); + combined_nav_states_.set().erase(lower_bound_state_it); + } + return extrapolated_states; +} + +void ImuBiasExtrapolator::AddExtrapolatedStates() { + rosbag::View view(input_bag_); + for (const rosbag::MessageInstance msg : view) { + if (string_ends_with(msg.getTopic(), TOPIC_GRAPH_VIO_STATE)) { + const ff_msgs::GraphVIOState::ConstPtr vio_msg = msg.instantiate<ff_msgs::GraphVIOState>(); + const auto extrapolated_states = VIOStateCallback(*vio_msg); + SaveExtrapolatedStates(extrapolated_states, output_bag_); + } else if (string_ends_with(msg.getTopic(), TOPIC_HARDWARE_IMU)) { + sensor_msgs::ImuConstPtr imu_msg = msg.instantiate<sensor_msgs::Imu>(); + imu_integrator_->AddImuMeasurement(lm::ImuMeasurement(*imu_msg)); + } + output_bag_.write(msg.getTopic(), msg.getTime(), msg); + } +} +} // namespace localization_analysis diff --git a/tools/localization_analysis/src/imu_bias_tester_adder.cc b/tools/localization_analysis/src/imu_bias_tester_adder.cc deleted file mode 100644 index c141783e5a..0000000000 --- a/tools/localization_analysis/src/imu_bias_tester_adder.cc +++ /dev/null @@ -1,43 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include <ff_common/ff_names.h> -#include <localization_analysis/imu_bias_tester_adder.h> -#include <localization_analysis/utilities.h> -#include <imu_bias_tester/imu_bias_tester_wrapper.h> - -namespace localization_analysis { -ImuBiasTesterAdder::ImuBiasTesterAdder(const std::string& input_bag_name, const std::string& output_bag_name) - : input_bag_(input_bag_name, rosbag::bagmode::Read), output_bag_(output_bag_name, rosbag::bagmode::Write) {} - -void ImuBiasTesterAdder::AddPredictions() { - rosbag::View view(input_bag_); - for (const rosbag::MessageInstance msg : view) { - if (string_ends_with(msg.getTopic(), TOPIC_GRAPH_LOC_STATE)) { - const ff_msgs::GraphState::ConstPtr localization_msg = msg.instantiate<ff_msgs::GraphState>(); - const auto imu_bias_tester_predicted_states = - imu_bias_tester_wrapper_.LocalizationStateCallback(*localization_msg); - SaveImuBiasTesterPredictedStates(imu_bias_tester_predicted_states, output_bag_); - } else if (string_ends_with(msg.getTopic(), TOPIC_HARDWARE_IMU)) { - sensor_msgs::ImuConstPtr imu_msg = msg.instantiate<sensor_msgs::Imu>(); - imu_bias_tester_wrapper_.ImuCallback(*imu_msg); - } - output_bag_.write(msg.getTopic(), msg.getTime(), msg); - } -} -} // namespace localization_analysis diff --git a/tools/localization_analysis/src/live_measurement_simulator.cc b/tools/localization_analysis/src/live_measurement_simulator.cc index 85d23a82d9..e52ab2ad3f 100644 --- a/tools/localization_analysis/src/live_measurement_simulator.cc +++ b/tools/localization_analysis/src/live_measurement_simulator.cc @@ -56,7 +56,7 @@ LiveMeasurementSimulator::LiveMeasurementSimulator(const LiveMeasurementSimulato topics.push_back(TOPIC_HARDWARE_IMU); topics.push_back(std::string("/") + kImageTopic_); topics.push_back(kImageTopic_); - if (params_.use_image_features) { + if (params_.use_bag_image_feature_msgs) { topics.push_back(std::string("/") + TOPIC_LOCALIZATION_OF_FEATURES); topics.push_back(TOPIC_LOCALIZATION_OF_FEATURES); topics.push_back(std::string("/") + TOPIC_LOCALIZATION_ML_FEATURES); @@ -69,6 +69,19 @@ LiveMeasurementSimulator::LiveMeasurementSimulator(const LiveMeasurementSimulato topics.push_back(std::string("/") + TOPIC_LOCALIZATION_DEPTH_ODOM); topics.push_back(TOPIC_LOCALIZATION_DEPTH_ODOM); + topic_localization_depth_image_ = static_cast<std::string>(TOPIC_HARDWARE_PICOFLEXX_PREFIX) + + static_cast<std::string>(TOPIC_HARDWARE_NAME_HAZ_CAM) + + static_cast<std::string>(TOPIC_HARDWARE_PICOFLEXX_SUFFIX_EXTENDED) + + static_cast<std::string>(TOPIC_HARDWARE_PICOFLEXX_SUFFIX_AMPLITUDE_IMAGE); + topics.push_back(std::string("/") + topic_localization_depth_image_); + topics.push_back(topic_localization_depth_image_); + + topic_localization_depth_cloud_ = static_cast<std::string>(TOPIC_HARDWARE_PICOFLEXX_PREFIX) + + static_cast<std::string>(TOPIC_HARDWARE_NAME_HAZ_CAM) + + static_cast<std::string>(TOPIC_HARDWARE_PICOFLEXX_SUFFIX); + topics.push_back(std::string("/") + topic_localization_depth_cloud_); + topics.push_back(topic_localization_depth_cloud_); + topics.push_back(std::string("/") + TOPIC_MOBILITY_FLIGHT_MODE); topics.push_back(TOPIC_MOBILITY_FLIGHT_MODE); @@ -105,23 +118,36 @@ bool LiveMeasurementSimulator::ProcessMessage() { if (*view_it_ == view_->end()) return false; const auto& msg = **view_it_; current_time_ = lc::TimeFromRosTime(msg.getTime()); + /*if (string_ends_with(msg.getTopic(), TOPIC_MOBILITY_FLIGHT_MODE)) { + const ff_msgs::FlightModeConstPtr flight_mode = msg.instantiate<ff_msgs::FlightMode>(); + flight_mode_buffer_.BufferMessage(*flight_mode); + }*/ if (string_ends_with(msg.getTopic(), TOPIC_HARDWARE_IMU)) { sensor_msgs::ImuConstPtr imu_msg = msg.instantiate<sensor_msgs::Imu>(); imu_buffer_.BufferMessage(*imu_msg); - } else if (string_ends_with(msg.getTopic(), TOPIC_MOBILITY_FLIGHT_MODE)) { - const ff_msgs::FlightModeConstPtr flight_mode = msg.instantiate<ff_msgs::FlightMode>(); - flight_mode_buffer_.BufferMessage(*flight_mode); - } else if (string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_DEPTH_ODOM)) { + } else if (params_.use_bag_depth_odom_msgs && string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_DEPTH_ODOM)) { const ff_msgs::DepthOdometryConstPtr depth_odometry = msg.instantiate<ff_msgs::DepthOdometry>(); depth_odometry_buffer_.BufferMessage(*depth_odometry); + } else if (!params_.use_bag_depth_odom_msgs && string_ends_with(msg.getTopic(), topic_localization_depth_image_)) { + const sensor_msgs::ImageConstPtr depth_image = msg.instantiate<sensor_msgs::Image>(); + const auto depth_odometry_msgs = depth_odometry_wrapper_.ImageCallback(depth_image); + for (const auto& depth_odometry_msg : depth_odometry_msgs) { + depth_odometry_buffer_.BufferMessage(depth_odometry_msg); + } + } else if (!params_.use_bag_depth_odom_msgs && string_ends_with(msg.getTopic(), topic_localization_depth_cloud_)) { + const sensor_msgs::PointCloud2ConstPtr depth_cloud = msg.instantiate<sensor_msgs::PointCloud2>(); + const auto depth_odometry_msgs = depth_odometry_wrapper_.PointCloudCallback(depth_cloud); + for (const auto& depth_odometry_msg : depth_odometry_msgs) { + depth_odometry_buffer_.BufferMessage(depth_odometry_msg); + } } else if (string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_AR_FEATURES)) { // 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_image_features && string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_OF_FEATURES)) { + } 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_image_features && string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_ML_FEATURES)) { + } else if (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_)) { @@ -129,7 +155,7 @@ bool LiveMeasurementSimulator::ProcessMessage() { if (params_.save_optical_flow_images) { img_buffer_.emplace(localization_common::TimeFromHeader(image_msg->header), image_msg); } - if (!params_.use_image_features) { + if (!params_.use_bag_image_feature_msgs) { const ff_msgs::Feature2dArray of_features = GenerateOFFeatures(image_msg); of_buffer_.BufferMessage(of_features); diff --git a/tools/localization_analysis/src/map_matcher.cc b/tools/localization_analysis/src/map_matcher.cc index 8dfe37d11b..e66e2856f3 100644 --- a/tools/localization_analysis/src/map_matcher.cc +++ b/tools/localization_analysis/src/map_matcher.cc @@ -17,7 +17,6 @@ */ #include <ff_common/ff_names.h> -#include <graph_localizer/utilities.h> #include <localization_analysis/map_matcher.h> #include <localization_analysis/utilities.h> #include <localization_common/utilities.h> @@ -86,11 +85,10 @@ void MapMatcher::AddMapMatches() { feature_averager_.Update(vl_msg.landmarks.size()); const ros::Time timestamp = lc::RosTimeFromHeader(image_msg->header); output_bag_.write(std::string("/") + TOPIC_LOCALIZATION_ML_FEATURES, timestamp, vl_msg); - if (graph_localizer::ValidVLMsg(vl_msg, sparse_mapping_min_num_landmarks_)) { + if (static_cast<int>(vl_msg.landmarks.size()) >= 5) { const gtsam::Pose3 sparse_mapping_global_T_body = lc::PoseFromMsgWithExtrinsics(vl_msg.pose, body_T_nav_cam_.inverse()); - const auto pose_msg = - graph_localizer::PoseMsg(lc::EigenPose(sparse_mapping_global_T_body), lc::TimeFromHeader(vl_msg.header)); + const auto pose_msg = PoseMsg(lc::EigenPose(sparse_mapping_global_T_body), lc::TimeFromHeader(vl_msg.header)); output_bag_.write(std::string("/") + TOPIC_SPARSE_MAPPING_POSE, timestamp, pose_msg); } } else if (nonloc_bag_.isOpen()) { diff --git a/tools/localization_analysis/src/offline_replay.cc b/tools/localization_analysis/src/offline_replay.cc new file mode 100644 index 0000000000..1f315016a0 --- /dev/null +++ b/tools/localization_analysis/src/offline_replay.cc @@ -0,0 +1,209 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <ff_common/ff_names.h> +#include <ff_common/utils.h> +#include <localization_analysis/feature_track_image_adder.h> +#include <localization_analysis/offline_replay.h> +#include <localization_analysis/parameter_reader.h> +#include <localization_analysis/utilities.h> +#include <localization_common/logger.h> +#include <localization_common/utilities.h> +#include <msg_conversions/msg_conversions.h> + +#include <geometry_msgs/PoseStamped.h> +#include <geometry_msgs/Vector3Stamped.h> +#include <ros/time.h> +#include <sensor_msgs/Imu.h> + +#include <Eigen/Core> + +#include <chrono> +#include <cstdlib> +#include <vector> + +namespace localization_analysis { +namespace gl = graph_localizer; +namespace gv = graph_vio; +namespace lc = localization_common; +namespace mc = msg_conversions; + +OfflineReplay::OfflineReplay(const std::string& bag_name, const std::string& map_file, const std::string& image_topic, + const std::string& results_bag, const std::string& output_stats_file, + const bool use_bag_image_feature_msgs, const std::string& graph_config_path_prefix) + : results_bag_(results_bag, rosbag::bagmode::Write), + // imu_bias_tester_wrapper_(graph_config_path_prefix), + pose_extrapolator_wrapper_(graph_config_path_prefix), + output_stats_file_(output_stats_file) { + config_reader::ConfigReader config; + config.AddFile("cameras.config"); + config.AddFile("geometry.config"); + config.AddFile("tools/offline_replay.config"); + lc::LoadGraphLocalizerConfig(config, graph_config_path_prefix); + lc::LoadGraphVIOConfig(config, graph_config_path_prefix); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + + LiveMeasurementSimulatorParams params; + LoadLiveMeasurementSimulatorParams(config, bag_name, map_file, image_topic, params); + // Load this seperately so this can be set independently of config file, + // i.e. when running a bag sweep or param sweep + // TODO(rsoussan): clean this up + params.use_bag_image_feature_msgs = use_bag_image_feature_msgs; + live_measurement_simulator_.reset(new LiveMeasurementSimulator(params)); + + GraphLocalizerSimulatorParams graph_loc_params; + LoadGraphLocalizerSimulatorParams(config, graph_loc_params); + graph_localizer_simulator_.reset(new GraphLocalizerSimulator(graph_loc_params, graph_config_path_prefix)); + + GraphVIOSimulatorParams graph_vio_params; + LoadGraphVIOSimulatorParams(config, graph_vio_params); + graph_vio_simulator_.reset(new GraphVIOSimulator(graph_vio_params, graph_config_path_prefix)); + LoadOfflineReplayParams(config, params_); +} + +void OfflineReplay::Run() { + // Required to start bias estimation + graph_vio_simulator_->ResetBiasesAndVIO(); + graph_localizer_simulator_->ResetLocalizer(); + lc::Timer offline_replay_timer("Offline Replay Timer"); + offline_replay_timer.Start(); + const double start_time = live_measurement_simulator_->CurrentTime(); + while (live_measurement_simulator_->ProcessMessage()) { + const lc::Time current_time = live_measurement_simulator_->CurrentTime(); + if (params_.log_relative_time) LogInfo("Run: Rel t: " << current_time - start_time); + const auto flight_mode_msg = live_measurement_simulator_->GetFlightModeMessage(current_time); + if (flight_mode_msg) { + graph_vio_simulator_->BufferFlightModeMsg(*flight_mode_msg); + graph_localizer_simulator_->BufferFlightModeMsg(*flight_mode_msg); + pose_extrapolator_wrapper_.FlightModeCallback(*flight_mode_msg); + } + const auto imu_msg = live_measurement_simulator_->GetImuMessage(current_time); + if (imu_msg) { + graph_vio_simulator_->BufferImuMsg(*imu_msg); + graph_localizer_simulator_->BufferImuMsg(*imu_msg); + pose_extrapolator_wrapper_.ImuCallback(*imu_msg); + SaveMsg(*imu_msg, "hw/imu", results_bag_); + + // Save extrapolated loc msg if available + const auto extrapolated_loc_msg = pose_extrapolator_wrapper_.LatestExtrapolatedLocalizationMsg(); + if (!extrapolated_loc_msg) { + LogWarningEveryN(500, "Run: Failed to get latest extrapolated loc msg."); + } else { + SaveMsg(*extrapolated_loc_msg, TOPIC_GNC_EKF, results_bag_); + } + } + const auto depth_odometry_msg = live_measurement_simulator_->GetDepthOdometryMessage(current_time); + if (depth_odometry_msg) { + graph_vio_simulator_->BufferDepthOdometryMsg(*depth_odometry_msg); + SaveMsg(*depth_odometry_msg, TOPIC_LOCALIZATION_DEPTH_ODOM, results_bag_); + } + const auto of_msg = live_measurement_simulator_->GetOFMessage(current_time); + if (of_msg) { + const lc::Time timestamp = lc::TimeFromHeader(of_msg->header); + graph_vio_simulator_->BufferOpticalFlowMsg(*of_msg); + } + const auto vl_msg = live_measurement_simulator_->GetVLMessage(current_time); + if (vl_msg) { + const lc::Time timestamp = lc::TimeFromHeader(vl_msg->header); + graph_localizer_simulator_->BufferVLVisualLandmarksMsg(*vl_msg); + if (static_cast<int>(vl_msg->landmarks.size()) >= params_.sparse_mapping_min_num_landmarks) { + const gtsam::Pose3 sparse_mapping_global_T_body = + lc::PoseFromMsgWithExtrinsics(vl_msg->pose, params_.body_T_nav_cam.inverse()); + const lc::Time timestamp = lc::TimeFromHeader(vl_msg->header); + SaveMsg(PoseMsg(sparse_mapping_global_T_body, timestamp), TOPIC_SPARSE_MAPPING_POSE, results_bag_); + } + } + + const auto ar_msg = live_measurement_simulator_->GetARMessage(current_time); + if (ar_msg) { + graph_localizer_simulator_->BufferARVisualLandmarksMsg(*ar_msg); + latest_ar_msg_ = ar_msg; + } + + const bool updated_vio_graph = graph_vio_simulator_->AddMeasurementsAndUpdateIfReady(current_time); + if (updated_vio_graph) { + // Pass pose covariance interpolater used for relative factors + // from graph vio to graph localizer + if (graph_vio_simulator_->Initialized() && graph_localizer_simulator_->Initialized()) { + graph_localizer_simulator_->graph_localizer()->SetPoseCovarianceInterpolater( + graph_vio_simulator_->graph_vio()->MarginalsPoseCovarianceInterpolater()); + } + const auto vio_msg = graph_vio_simulator_->GraphVIOStateMsg(); + if (!vio_msg) { + LogWarningEveryN(200, "Run: Failed to get vio msg."); + } else { + pose_extrapolator_wrapper_.GraphVIOStateCallback(*vio_msg); + // TODO(rsoussan): Pass this to live measurement simulator? allow for simulated delay? + graph_localizer_simulator_->BufferGraphVIOStateMsg(*vio_msg); + SaveMsg(*vio_msg, TOPIC_GRAPH_VIO_STATE, results_bag_); + if (params_.save_optical_flow_images) { + const auto& graph_vio = graph_vio_simulator_->graph_vio(); + // Use spaced feature tracks so points only drawn when they are included in the localizer + const auto latest_time = graph_vio->feature_tracker().SpacedFeatureTracks().crbegin()->crbegin()->timestamp; + // const auto latest_time = + // *(graph_vio->feature_tracker().feature_tracks().crbegin()->second.LatestTimestamp()); + const auto img_msg = live_measurement_simulator_->GetImageMessage(latest_time); + if (img_msg && graph_vio) { + const auto smart_factors = graph_vio->Factors<factor_adders::RobustSmartFactor>(); + const auto feature_track_image_msg = + CreateFeatureTrackImage(*img_msg, graph_vio->feature_tracker(), *params_.nav_cam_params, smart_factors, + ((const graph_vio::GraphVIO*)graph_vio.get())->gtsam_values()); + if (!feature_track_image_msg) return; + SaveMsg(**feature_track_image_msg, kFeatureTracksImageTopic_, results_bag_); + } + } + } + } + const bool updated_localizer_graph = graph_localizer_simulator_->AddMeasurementsAndUpdateIfReady(current_time); + if (updated_localizer_graph) { + // Save latest graph localization msg + // Pass latest loc state to imu augmentor if it is available. + const auto localization_msg = graph_localizer_simulator_->GraphLocStateMsg(); + if (!localization_msg) { + LogWarningEveryN(1000, "Run: Failed to get localization msg."); + } else { + pose_extrapolator_wrapper_.LocalizationStateCallback(*localization_msg); + SaveMsg(*localization_msg, TOPIC_GRAPH_LOC_STATE, results_bag_); + } + if (latest_ar_msg_ && graph_localizer_simulator_->WorldTDock()) { + if (static_cast<int>(ar_msg->landmarks.size()) >= params_.ar_min_num_landmarks) { + const gtsam::Pose3 world_T_body = + (*graph_localizer_simulator_->WorldTDock()) * + lc::PoseFromMsgWithExtrinsics(ar_msg->pose, params_.body_T_dock_cam.inverse()); + const lc::Time timestamp = lc::TimeFromHeader(ar_msg->header); + SaveMsg(PoseMsg(world_T_body, timestamp), TOPIC_AR_TAG_POSE, results_bag_); + latest_ar_msg_ = boost::none; + } + } + } + } + offline_replay_timer.Stop(); + offline_replay_timer.Log(); + /*const auto graph_stats = graph_localizer_simulator_->graph_localizer_stats(); + if (!graph_stats) { + LogError("Run: Failed to get graph stats"); + } else { + std::ofstream log_file; + log_file.open(output_stats_file_); + graph_stats->LogToCsv(log_file); + offline_replay_timer.LogToCsv(log_file); + }*/ +} +} // namespace localization_analysis diff --git a/tools/localization_analysis/src/parameter_reader.cc b/tools/localization_analysis/src/parameter_reader.cc index 44be2ba3f5..166de26223 100644 --- a/tools/localization_analysis/src/parameter_reader.cc +++ b/tools/localization_analysis/src/parameter_reader.cc @@ -25,36 +25,44 @@ namespace lc = localization_common; namespace mc = msg_conversions; void LoadMessageBufferParams(const std::string& message_type, config_reader::ConfigReader& config, - MessageBufferParams& params) { - params.msg_delay = mc::LoadDouble(config, message_type + "_msg_delay"); - params.min_msg_spacing = mc::LoadDouble(config, message_type + "_min_msg_spacing"); + MessageBufferParams& params, const std::string& prefix) { + LOAD_PARAM(params.msg_delay, config, prefix + message_type + "_"); + LOAD_PARAM(params.min_msg_spacing, config, prefix + message_type + "_"); } void LoadLiveMeasurementSimulatorParams(config_reader::ConfigReader& config, const std::string& bag_name, const std::string& map_file, const std::string& image_topic, LiveMeasurementSimulatorParams& params) { - LoadMessageBufferParams("imu", config, params.imu); - LoadMessageBufferParams("flight_mode", config, params.flight_mode); - LoadMessageBufferParams("depth_odometry", config, params.depth_odometry); - LoadMessageBufferParams("of", config, params.of); - LoadMessageBufferParams("vl", config, params.vl); - LoadMessageBufferParams("ar", config, params.ar); - params.save_optical_flow_images = mc::LoadBool(config, "save_optical_flow_images"); + // Note using image features is set in the offline replay tool + LoadMessageBufferParams("imu", config, params.imu, "or_"); + LoadMessageBufferParams("flight_mode", config, params.flight_mode, "or_"); + LoadMessageBufferParams("depth_odometry", config, params.depth_odometry, "or_"); + LoadMessageBufferParams("of", config, params.of, "or_"); + LoadMessageBufferParams("vl", config, params.vl, "or_"); + LoadMessageBufferParams("ar", config, params.ar, "or_"); + LoadMessageBufferParams("vio", config, params.vio, "or_"); + LOAD_PARAM(params.save_optical_flow_images, config, "or_"); + LOAD_PARAM(params.use_bag_depth_odom_msgs, config, "or_"); params.bag_name = bag_name; params.map_file = map_file; params.image_topic = image_topic; } void LoadGraphLocalizerSimulatorParams(config_reader::ConfigReader& config, GraphLocalizerSimulatorParams& params) { - params.optimization_time = mc::LoadDouble(config, "optimization_time"); + LOAD_PARAM(params.optimization_time, config, "or_loc_"); } -void LoadGraphBagParams(config_reader::ConfigReader& config, GraphBagParams& params) { - params.save_optical_flow_images = mc::LoadBool(config, "save_optical_flow_images"); - params.log_relative_time = mc::LoadBool(config, "log_relative_time"); +void LoadGraphVIOSimulatorParams(config_reader::ConfigReader& config, GraphVIOSimulatorParams& params) { + LOAD_PARAM(params.optimization_time, config, "or_vio_"); +} + +void LoadOfflineReplayParams(config_reader::ConfigReader& config, OfflineReplayParams& params) { + LOAD_PARAM(params.save_optical_flow_images, config, "or_"); + LOAD_PARAM(params.log_relative_time, config, "or_"); + LOAD_PARAM(params.sparse_mapping_min_num_landmarks, config, "or_"); + LOAD_PARAM(params.ar_min_num_landmarks, config, "or_"); params.nav_cam_params.reset(new camera::CameraParameters(&config, "nav_cam")); params.body_T_nav_cam = lc::LoadTransform(config, "nav_cam_transform"); - params.sparse_mapping_min_num_landmarks = mc::LoadInt(config, "loc_adder_min_num_matches"); - params.ar_min_num_landmarks = mc::LoadInt(config, "ar_tag_loc_adder_min_num_matches"); + params.body_T_dock_cam = lc::LoadTransform(config, "dock_cam_transform"); } } // namespace localization_analysis diff --git a/tools/localization_analysis/src/sparse_mapping_pose_adder.cc b/tools/localization_analysis/src/sparse_mapping_pose_adder.cc index e0a9e12dd2..26c0648856 100644 --- a/tools/localization_analysis/src/sparse_mapping_pose_adder.cc +++ b/tools/localization_analysis/src/sparse_mapping_pose_adder.cc @@ -19,7 +19,6 @@ #include <ff_common/ff_names.h> #include <localization_analysis/sparse_mapping_pose_adder.h> #include <localization_analysis/utilities.h> -#include <graph_localizer/utilities.h> #include <localization_common/utilities.h> #include <ff_msgs/VisualLandmarks.h> @@ -42,8 +41,7 @@ void SparseMappingPoseAdder::AddPoses() { const auto world_T_nav_cam = lc::PoseFromMsg(vl_features->pose); const auto world_T_body = world_T_nav_cam * nav_cam_T_body_; // TODO(rsoussan): put this in loc common - const auto sparse_mapping_pose_msg = - graph_localizer::PoseMsg(world_T_body, lc::TimeFromHeader(vl_features->header)); + const auto sparse_mapping_pose_msg = PoseMsg(world_T_body, lc::TimeFromHeader(vl_features->header)); const ros::Time timestamp = lc::RosTimeFromHeader(vl_features->header); output_bag_.write("/" + std::string(TOPIC_SPARSE_MAPPING_POSE), timestamp, sparse_mapping_pose_msg); } diff --git a/tools/localization_analysis/src/utilities.cc b/tools/localization_analysis/src/utilities.cc index 3036d5ce65..740f6b8399 100644 --- a/tools/localization_analysis/src/utilities.cc +++ b/tools/localization_analysis/src/utilities.cc @@ -30,9 +30,10 @@ namespace localization_analysis { namespace lc = localization_common; +namespace lm = localization_measurements; namespace mc = msg_conversions; -void FeatureTrackImage(const graph_localizer::FeatureTrackIdMap& feature_tracks, +/*void FeatureTrackImage(const graph_localizer::FeatureTrackIdMap& feature_tracks, const camera::CameraParameters& camera_params, cv::Mat& feature_track_image) { for (const auto& feature_track : feature_tracks) { const auto& points = feature_track.second->points(); @@ -55,13 +56,13 @@ void FeatureTrackImage(const graph_localizer::FeatureTrackIdMap& feature_tracks, const auto& point2 = std::next(point_it)->second.image_point; const auto distorted_previous_point = Distort(point1, camera_params); const auto distorted_current_point = Distort(point2, camera_params); - cv::circle(feature_track_image, distorted_current_point, 2 /* Radius*/, cv::Scalar(0, 255, 255), -1 /*Filled*/, + cv::circle(feature_track_image, distorted_current_point, 2, cv::Scalar(0, 255, 255), -1, 8); cv::line(feature_track_image, distorted_current_point, distorted_previous_point, color, 2, 8, 0); } } else { - cv::circle(feature_track_image, Distort(points.cbegin()->second.image_point, camera_params), 2 /* Radius*/, color, - -1 /*Filled*/, 8); + cv::circle(feature_track_image, Distort(points.cbegin()->second.image_point, camera_params), 2, color, + -1, 8); } // Draw feature id at most recent point cv::putText(feature_track_image, std::to_string(points.crbegin()->second.feature_id), @@ -75,7 +76,7 @@ void MarkSmartFactorPoints(const std::vector<const SmartFactor*> smart_factors, for (const auto smart_factor : smart_factors) { const auto& point = smart_factor->measured().back(); const auto distorted_point = Distort(point, camera_params); - cv::circle(feature_track_image, distorted_point, 15 /* Radius*/, cv::Scalar(200, 100, 0), -1 /*Filled*/, 8); + cv::circle(feature_track_image, distorted_point, 15, cv::Scalar(200, 100, 0), -1, 8); } } @@ -111,7 +112,7 @@ std::vector<const SmartFactor*> SmartFactors(const graph_localizer::GraphLocaliz } } return smart_factors; -} +}*/ bool string_ends_with(const std::string& str, const std::string& ending) { if (str.length() >= ending.length()) { @@ -121,7 +122,29 @@ bool string_ends_with(const std::string& str, const std::string& ending) { } } -void SaveImuBiasTesterPredictedStates(const std::vector<lc::CombinedNavState>& imu_bias_tester_predicted_states, +geometry_msgs::PoseStamped PoseMsg(const Eigen::Isometry3d& global_T_body, const std_msgs::Header& header) { + geometry_msgs::PoseStamped pose_msg; + pose_msg.header = header; + mc::EigenPoseToMsg(global_T_body, pose_msg.pose); + return pose_msg; +} + +geometry_msgs::PoseStamped PoseMsg(const Eigen::Isometry3d& global_T_body, const lc::Time time) { + std_msgs::Header header; + lc::TimeToHeader(time, header); + header.frame_id = "world"; + return PoseMsg(global_T_body, header); +} + +geometry_msgs::PoseStamped PoseMsg(const gtsam::Pose3& global_T_body, const lc::Time time) { + return PoseMsg(lc::EigenPose(global_T_body), time); +} + +geometry_msgs::PoseStamped PoseMsg(const lm::TimestampedPose& timestamped_pose) { + return PoseMsg(timestamped_pose.pose, timestamped_pose.time); +} + +/*void SaveImuBiasTesterPredictedStates(const std::vector<lc::CombinedNavState>& imu_bias_tester_predicted_states, rosbag::Bag& bag) { for (const auto& state : imu_bias_tester_predicted_states) { geometry_msgs::PoseStamped pose_msg; @@ -133,5 +156,5 @@ void SaveImuBiasTesterPredictedStates(const std::vector<lc::CombinedNavState>& i lc::TimeToHeader(state.timestamp(), velocity_msg.header); SaveMsg(velocity_msg, TOPIC_IMU_BIAS_TESTER_VELOCITY, bag); } -} +}*/ } // namespace localization_analysis diff --git a/tools/localization_analysis/test/test_offline_replay_parameter_reader.cc b/tools/localization_analysis/test/test_offline_replay_parameter_reader.cc new file mode 100644 index 0000000000..77d75c8cca --- /dev/null +++ b/tools/localization_analysis/test/test_offline_replay_parameter_reader.cc @@ -0,0 +1,104 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include <localization_common/logger.h> +#include <localization_common/test_utilities.h> +#include <localization_common/utilities.h> +#include <localization_analysis/parameter_reader.h> + +#include <gtest/gtest.h> + +namespace la = localization_analysis; +namespace lc = localization_common; + +class OfflineReplayParameterReaderTest : public ::testing::Test { + public: + OfflineReplayParameterReaderTest() {} + + void SetUp() final { + lc::SetEnvironmentConfigs(); + config_reader::ConfigReader config; + config.AddFile("tools/offline_replay.config"); + lc::LoadGraphLocalizerConfig(config); + lc::LoadGraphVIOConfig(config); + + la::LoadLiveMeasurementSimulatorParams(config, "test.bag", "test.map", "test_topic", + live_measurement_simulator_params_); + live_measurement_simulator_params_.use_bag_image_feature_msgs = true; + la::LoadGraphLocalizerSimulatorParams(config, graph_localizer_simulator_params_); + la::LoadGraphVIOSimulatorParams(config, graph_vio_simulator_params_); + la::LoadOfflineReplayParams(config, offline_replay_params_); + } + + la::LiveMeasurementSimulatorParams live_measurement_simulator_params_; + la::GraphLocalizerSimulatorParams graph_localizer_simulator_params_; + la::GraphVIOSimulatorParams graph_vio_simulator_params_; + la::OfflineReplayParams offline_replay_params_; +}; + +TEST_F(OfflineReplayParameterReaderTest, LiveMeasurementSimulatorParams) { + const auto& params = live_measurement_simulator_params_; + EXPECT_NEAR(params.imu.msg_delay, 0, 1e-6); + EXPECT_NEAR(params.imu.min_msg_spacing, 0, 1e-6); + EXPECT_NEAR(params.flight_mode.msg_delay, 0, 1e-6); + EXPECT_NEAR(params.flight_mode.min_msg_spacing, 0, 1e-6); + EXPECT_NEAR(params.depth_odometry.msg_delay, 0, 1e-6); + EXPECT_NEAR(params.depth_odometry.min_msg_spacing, 0, 1e-6); + EXPECT_NEAR(params.of.msg_delay, 0, 1e-6); + EXPECT_NEAR(params.of.min_msg_spacing, 0, 1e-6); + EXPECT_NEAR(params.vl.msg_delay, 0, 1e-6); + EXPECT_NEAR(params.vl.min_msg_spacing, 0, 1e-6); + EXPECT_NEAR(params.ar.msg_delay, 0, 1e-6); + EXPECT_NEAR(params.ar.min_msg_spacing, 0, 1e-6); + EXPECT_NEAR(params.vio.msg_delay, 0, 1e-6); + EXPECT_NEAR(params.vio.min_msg_spacing, 0, 1e-6); + EXPECT_EQ(params.bag_name, "test.bag"); + EXPECT_EQ(params.map_file, "test.map"); + EXPECT_EQ(params.image_topic, "test_topic"); + EXPECT_EQ(params.use_bag_image_feature_msgs, true); + EXPECT_EQ(params.save_optical_flow_images, false); +} + +TEST_F(OfflineReplayParameterReaderTest, GraphLocalizerSimulatorParams) { + const auto& params = graph_localizer_simulator_params_; + EXPECT_NEAR(params.optimization_time, 0.05, 1e-6); +} + +TEST_F(OfflineReplayParameterReaderTest, GraphVIOSimulatorParams) { + const auto& params = graph_vio_simulator_params_; + EXPECT_NEAR(params.optimization_time, 0.3, 1e-6); +} + +TEST_F(OfflineReplayParameterReaderTest, OfflineReplayParams) { + const auto& params = offline_replay_params_; + EXPECT_EQ(params.save_optical_flow_images, false); + EXPECT_EQ(params.log_relative_time, false); + EXPECT_EQ(params.ar_min_num_landmarks, 5); + EXPECT_EQ(params.sparse_mapping_min_num_landmarks, 5); + // Taken using current nav cam extrinsics + const gtsam::Pose3 expected_body_T_cam(gtsam::Rot3(0.500, 0.500, 0.500, 0.500), + gtsam::Point3(0.1177, -0.0422, -0.0826)); + EXPECT_MATRIX_NEAR(params.body_T_nav_cam, expected_body_T_cam, 1e-6); + // TODO(rsoussan): Check nav_cam_params +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tools/localization_analysis/test/test_offline_replay_parameter_reader.test b/tools/localization_analysis/test/test_offline_replay_parameter_reader.test new file mode 100644 index 0000000000..f29fffda9f --- /dev/null +++ b/tools/localization_analysis/test/test_offline_replay_parameter_reader.test @@ -0,0 +1,20 @@ +<!-- Copyright (c) 2017, United States Government, as represented by the --> +<!-- Administrator of the National Aeronautics and Space Administration. --> +<!-- --> +<!-- All rights reserved. --> +<!-- --> +<!-- The Astrobee platform is licensed under the Apache License, Version 2.0 --> +<!-- (the "License"); you may not use this file except in compliance with --> +<!-- the License. You may obtain a copy of the License at --> +<!-- --> +<!-- http://www.apache.org/licenses/LICENSE-2.0 --> +<!-- --> +<!-- Unless required by applicable law or agreed to in writing, software --> +<!-- distributed under the License is distributed on an "AS IS" BASIS, --> +<!-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or --> +<!-- implied. See the License for the specific language governing --> +<!-- permissions and limitations under the License. --> + +<launch> + <test pkg="localization_analysis" type="test_offline_replay_parameter_reader" test-name="test_offline_replay_parameter_reader" /> +</launch> diff --git a/tools/localization_analysis/tools/run_depth_odometry_adder.cc b/tools/localization_analysis/tools/run_depth_odometry_adder.cc index 0b9d397baa..45f9fef429 100644 --- a/tools/localization_analysis/tools/run_depth_odometry_adder.cc +++ b/tools/localization_analysis/tools/run_depth_odometry_adder.cc @@ -37,8 +37,7 @@ int main(int argc, char** argv) { desc.add_options()("help,h", "produce help message")( "bagfile", po::value<std::string>()->required(), "Input bagfile containing point cloud and image messages for a depth camera.")( - "config-path,c", po::value<std::string>()->required(), "Path to config directory.")( - "robot-config-file,r", po::value<std::string>(&robot_config_file)->default_value("config/robots/bumble.config"), + "robot-config-file,r", po::value<std::string>(&robot_config_file)->default_value("bumble.config"), "Robot config file")("world,w", po::value<std::string>(&world)->default_value("iss"), "World name")( "output-bagfile,o", po::value<std::string>(&output_bagfile)->default_value(""), "Output bagfile, defaults to input_bag + with_depth_odometry.bag")( @@ -64,7 +63,6 @@ int main(int argc, char** argv) { } const std::string input_bag = vm["bagfile"].as<std::string>(); - const std::string config_path = vm["config-path"].as<std::string>(); // Only pass program name to free flyer so that boost command line options // are ignored when parsing gflags. @@ -82,7 +80,7 @@ int main(int argc, char** argv) { boost::filesystem::path(input_bag_path.stem().string() + "_with_depth_odometry.bag"); output_bagfile = output_bag_path.string(); } - lc::SetEnvironmentConfigs(config_path, world, robot_config_file); + lc::SetEnvironmentConfigs(world, robot_config_file); config_reader::ConfigReader config; config.AddFile("geometry.config"); if (!config.ReadFiles()) { diff --git a/tools/localization_analysis/tools/run_imu_bias_tester_adder.cc b/tools/localization_analysis/tools/run_imu_bias_extrapolator.cc similarity index 76% rename from tools/localization_analysis/tools/run_imu_bias_tester_adder.cc rename to tools/localization_analysis/tools/run_imu_bias_extrapolator.cc index cee28fa0f2..f0a2e0a18d 100644 --- a/tools/localization_analysis/tools/run_imu_bias_tester_adder.cc +++ b/tools/localization_analysis/tools/run_imu_bias_extrapolator.cc @@ -17,7 +17,7 @@ */ #include <ff_common/init.h> -#include <localization_analysis/imu_bias_tester_adder.h> +#include <localization_analysis/imu_bias_extrapolator.h> #include <localization_common/logger.h> #include <localization_common/utilities.h> @@ -31,15 +31,13 @@ int main(int argc, char** argv) { std::string robot_config_file; std::string world; po::options_description desc( - "Adds imu bias tester predictions to a new bag file using recorded localization states and imu msgs"); + "Adds imu bias extrapolated predictions to a new bag file using recorded vio states and imu msgs"); desc.add_options()("help,h", "produce help message")("bagfile", po::value<std::string>()->required(), "Input bagfile")( - "config-path,c", po::value<std::string>()->required(), "Config path")( - "robot-config-file,r", po::value<std::string>(&robot_config_file)->default_value("config/robots/bumble.config"), + "robot-config-file,r", po::value<std::string>(&robot_config_file)->default_value("bumble.config"), "Robot config file")("world,w", po::value<std::string>(&world)->default_value("iss"), "World name"); po::positional_options_description p; p.add("bagfile", 1); - p.add("config-path", 1); po::variables_map vm; try { po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm); @@ -54,7 +52,6 @@ int main(int argc, char** argv) { } const std::string input_bag = vm["bagfile"].as<std::string>(); - const std::string config_path = vm["config-path"].as<std::string>(); // Only pass program name to free flyer so that boost command line options // are ignored when parsing gflags. @@ -69,13 +66,7 @@ int main(int argc, char** argv) { boost::filesystem::path output_bag_path = input_bag_path.parent_path() / boost::filesystem::path(input_bag_path.stem().string() + "_with_imu_bias_tester_predictions.bag"); - lc::SetEnvironmentConfigs(config_path, world, robot_config_file); - /* config_reader::ConfigReader config; - config.AddFile("geometry.config"); - if (!config.ReadFiles()) { - LogFatal("Failed to read config files."); - }*/ - - localization_analysis::ImuBiasTesterAdder imu_bias_tester_adder(input_bag, output_bag_path.string()); - imu_bias_tester_adder.AddPredictions(); + lc::SetEnvironmentConfigs(world, robot_config_file); + localization_analysis::ImuBiasExtrapolator imu_bias_extrapolator(input_bag, output_bag_path.string()); + imu_bias_extrapolator.AddExtrapolatedStates(); } diff --git a/tools/localization_analysis/tools/run_map_matcher.cc b/tools/localization_analysis/tools/run_map_matcher.cc index f3fbacf788..059e973508 100644 --- a/tools/localization_analysis/tools/run_map_matcher.cc +++ b/tools/localization_analysis/tools/run_map_matcher.cc @@ -45,9 +45,9 @@ int main(int argc, char** argv) { "Robot config file")("world,w", po::value<std::string>(&world)->default_value("iss"), "World name")( "output-bagfile,o", po::value<std::string>(&output_bagfile)->default_value(""), "Output bagfile, defaults to input_bag + _map_matches.bag")( - "config-path-prefix,p", po::value<std::string>(&config_path_prefix)->default_value(""), "Config path prefix") - ("save-noloc-imgs,s", po::value<std::string>(&save_noloc_imgs)->default_value("")->implicit_value(""), - "Save non-localized images to a bag, defaults to input_bag + _nonloc_imgs.bag"); + "config-path-prefix,p", po::value<std::string>(&config_path_prefix)->default_value(""), "Config path prefix")( + "save-noloc-imgs,s", po::value<std::string>(&save_noloc_imgs)->default_value("")->implicit_value(""), + "Save non-localized images to a bag, defaults to input_bag + _nonloc_imgs.bag"); po::positional_options_description p; p.add("bagfile", 1); p.add("map-file", 1); @@ -93,7 +93,7 @@ int main(int argc, char** argv) { save_noloc_imgs = boost::filesystem::current_path().string() + "/" + input_bag_path.stem().string() + "_nonloc_imgs.bag"; } - lc::SetEnvironmentConfigs(config_path, world, robot_config_file); + lc::SetEnvironmentConfigs(world, robot_config_file); config_reader::ConfigReader config; localization_analysis::MapMatcher map_matcher(input_bag, map_file, image_topic, output_bagfile, config_path_prefix, save_noloc_imgs); diff --git a/tools/localization_analysis/tools/run_graph_bag.cc b/tools/localization_analysis/tools/run_offline_replay.cc similarity index 78% rename from tools/localization_analysis/tools/run_graph_bag.cc rename to tools/localization_analysis/tools/run_offline_replay.cc index 4e39da2134..922c5458a2 100644 --- a/tools/localization_analysis/tools/run_graph_bag.cc +++ b/tools/localization_analysis/tools/run_offline_replay.cc @@ -17,7 +17,7 @@ */ #include <ff_common/init.h> -#include <localization_analysis/graph_bag.h> +#include <localization_analysis/offline_replay.h> #include <localization_common/logger.h> #include <localization_common/utilities.h> @@ -35,9 +35,12 @@ int main(int argc, char** argv) { std::string image_topic; std::string output_bagfile; std::string output_stats_file; - bool use_image_features; + std::string robot_config_file; + std::string world; + bool use_bag_image_feature_msgs; std::string graph_config_path_prefix; - po::options_description desc("Runs graph localization on a bagfile and saves the results to a new bagfile"); + po::options_description desc( + "Runs graph localization and VIO using a bagfile and saves the results to a new bagfile"); desc.add_options()("help,h", "produce help message")( "bagfile", po::value<std::string>()->required(), "Input bagfile")("map-file", po::value<std::string>()->required(), "Map file")( @@ -46,7 +49,10 @@ int main(int argc, char** argv) { "Output bagfile")("output-stats-file,s", po::value<std::string>(&output_stats_file)->default_value("graph_stats.csv"), "Output stats file")( - "use-image-features,f", po::value<bool>(&use_image_features)->default_value(true), "Use image features")( + "robot-config-file,r", po::value<std::string>(&robot_config_file)->default_value("bumble.config"), + "Robot config file")("world,w", po::value<std::string>(&world)->default_value("iss"), "World name")( + "use-bag-image-feature-msgs,f", po::value<bool>(&use_bag_image_feature_msgs)->default_value(true), + "Use image feature msgs from the bagfile or regenerate them from images.")( "graph-config-path-prefix,g", po::value<std::string>(&graph_config_path_prefix)->default_value(""), "Graph config path prefix"); @@ -96,14 +102,17 @@ int main(int argc, char** argv) { output_stats_file = output_stats_full_path.string(); } + // Set environment configs + lc::SetEnvironmentConfigs(world, robot_config_file); config_reader::ConfigReader config; - localization_analysis::GraphBag localization_analysis( - input_bag, map_file, image_topic, output_bagfile, output_stats_file, use_image_features); + localization_analysis::OfflineReplay offline_replay(input_bag, map_file, image_topic, output_bagfile, + output_stats_file, use_bag_image_feature_msgs, + graph_config_path_prefix); #ifdef GOOGLE_PROFILER ProfilerStart(boost::filesystem::current_path() + "/localization_analysis_prof.txt"); #endif - localization_analysis.Run(); + offline_replay.Run(); #ifdef GOOLGE_PROFILER ProfilerFlush(); ProfilerStop(); diff --git a/tools/localization_analysis/tools/run_sparse_mapping_pose_adder.cc b/tools/localization_analysis/tools/run_sparse_mapping_pose_adder.cc index bec8977fc9..1664ff2ae5 100644 --- a/tools/localization_analysis/tools/run_sparse_mapping_pose_adder.cc +++ b/tools/localization_analysis/tools/run_sparse_mapping_pose_adder.cc @@ -36,12 +36,10 @@ int main(int argc, char** argv) { "Adds sparse mapping poses to a new bag file using sparse mapping feature messages and body_T_nav_cam extrinsics"); desc.add_options()("help,h", "produce help message")("bagfile", po::value<std::string>()->required(), "Input bagfile")( - "config-path,c", po::value<std::string>()->required(), "Config path")( - "robot-config-file,r", po::value<std::string>(&robot_config_file)->default_value("config/robots/bumble.config"), + "robot-config-file,r", po::value<std::string>(&robot_config_file)->default_value("bumble.config"), "Robot config file")("world,w", po::value<std::string>(&world)->default_value("iss"), "World name"); po::positional_options_description p; p.add("bagfile", 1); - p.add("config-path", 1); po::variables_map vm; try { po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm); @@ -56,7 +54,6 @@ int main(int argc, char** argv) { } const std::string input_bag = vm["bagfile"].as<std::string>(); - const std::string config_path = vm["config-path"].as<std::string>(); // Only pass program name to free flyer so that boost command line options // are ignored when parsing gflags. @@ -71,7 +68,7 @@ int main(int argc, char** argv) { boost::filesystem::path output_bag_path = input_bag_path.parent_path() / boost::filesystem::path(input_bag_path.stem().string() + "_with_sparse_mapping_poses.bag"); - lc::SetEnvironmentConfigs(config_path, world, robot_config_file); + lc::SetEnvironmentConfigs(world, robot_config_file); config_reader::ConfigReader config; config.AddFile("geometry.config"); if (!config.ReadFiles()) { diff --git a/tools/localization_rviz_plugins/CMakeLists.txt b/tools/localization_rviz_plugins/CMakeLists.txt index 53e568b65f..d93d1ff85b 100644 --- a/tools/localization_rviz_plugins/CMakeLists.txt +++ b/tools/localization_rviz_plugins/CMakeLists.txt @@ -18,7 +18,7 @@ cmake_minimum_required(VERSION 3.0) project(localization_rviz_plugins) -if (USE_ROS AND BUILD_LOC_RVIZ_PLUGINS) +if (FALSE AND BUILD_LOC_RVIZ_PLUGINS) ## Compile as C++14, supported in ROS Kinetic and newer add_compile_options(-std=c++14)