From 608f6688c6f6088f1357fed9e68b45d68bc9ee2d Mon Sep 17 00:00:00 2001 From: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com> Date: Fri, 5 Feb 2021 10:27:33 -0800 Subject: [PATCH] Release 0.14.0 * Added graph_localizer package * Added supporting packages for graph localizer including imu_augmetor, imu_integation, localization_common, localization_measurements * Added tools for graph localizer including graph_bag and imu_bias_tester * Added rviz plugins for graph localizer and associated packages in localization_rviz_plugins * Added gtsam debian Please Note: Perching does not work in this release, please use 0.13.0 for this. --- CMakeLists.txt | 21 +- RELEASE.md | 9 + astrobee.doxyfile | 2 +- astrobee/commands/freeFlyerPlanSchema.json | 3 +- .../communications/dds_ros_bridge.config | 11 + astrobee/config/faults.config | 107 +- astrobee/config/graph_localizer.config | 150 ++ astrobee/config/localization.config | 2 +- astrobee/config/management/data_bagger.config | 2 + astrobee/config/management/fault_table.config | 169 +-- astrobee/config/management/sys_monitor.config | 5 +- astrobee/config/robots/bsharp2.config | 32 +- astrobee/config/tools/graph_bag.config | 33 + astrobee/config/worlds/granite.config | 2 + astrobee/config/worlds/iss.config | 2 + astrobee/launch/astrobee.launch | 37 +- .../offline_localization/loc_only.launch | 38 + .../offline_localization/loc_rviz.launch | 49 + .../localization_from_bag.launch | 96 ++ .../record_localization.launch | 22 + .../replay_localization.launch | 33 + astrobee/launch/replay.launch | 99 -- astrobee/launch/robot/LLP.launch | 81 +- astrobee/launch/robot/MLP.launch | 629 +++++---- astrobee/launch/sim.launch | 59 +- astrobee/launch/spawn.launch | 2 + astrobee/resources/rviz/loc_granite.rviz | 275 ++++ astrobee/resources/rviz/loc_iss.rviz | 273 ++++ .../dds_msgs/idl/AstrobeeCommandConstants.idl | 1 + .../dds_msgs/idl/TelemetryState.idl | 1 + .../include/dds_ros_bridge/dds_ros_bridge.h | 7 +- .../rapid_position_provider_ros_pose_helper.h | 48 + .../dds_ros_bridge/ros_pose_rapid_position.h | 65 + .../ros_telemetry_rapid_telemetry.h | 1 + .../dds_ros_bridge/src/dds_ros_bridge.cc | 79 ++ ...rapid_position_provider_ros_pose_helper.cc | 53 + .../src/ros_pose_rapid_position.cc | 80 ++ .../src/ros_telemetry_rapid_telemetry.cc | 10 + .../ff_msgs/msg/CommandConstants.msg | 1 + communications/ff_msgs/msg/GraphState.msg | 44 + .../ff_msgs/msg/LocalizationGraph.msg | 21 + communications/ff_msgs/srv/SetRate.srv | 15 +- debian/changelog | 11 + debian/control | 4 +- doc/general_documentation/flight_release.md | 15 +- gnc/ctl/CMakeLists.txt | 2 +- gnc/ctl/test/test_ctl.test | 1 + gnc/ekf/include/ekf/ekf.h | 4 +- gnc/ekf/src/ekf.cc | 2 + .../include/smart_dock/smart_dock.h | 4 + hardware/smart_dock/tools/smart_dock_tool.cc | 4 + localization/CMakeLists.txt | 6 + localization/graph_localizer/CMakeLists.txt | 57 + .../graph_localizer/calibration_params.h | 34 + .../graph_localizer/cumulative_factor_adder.h | 43 + .../include/graph_localizer/factor_adder.h | 43 + .../graph_localizer/factor_adder_params.h | 29 + .../include/graph_localizer/factor_params.h | 38 + .../include/graph_localizer/factor_to_add.h | 67 + .../include/graph_localizer/feature_counts.h | 35 + .../include/graph_localizer/feature_track.h | 48 + .../include/graph_localizer/feature_tracker.h | 61 + .../graph_localizer/feature_tracker_params.h | 28 + .../include/graph_localizer/graph_action.h | 32 + .../graph_initializer_params.h | 37 + .../include/graph_localizer/graph_localizer.h | 254 ++++ .../graph_localizer_initializer.h | 72 + .../graph_localizer/graph_localizer_nodelet.h | 121 ++ .../graph_localizer/graph_localizer_params.h | 56 + .../graph_localizer/graph_localizer_wrapper.h | 121 ++ .../include/graph_localizer/graph_stats.h | 100 ++ .../include/graph_localizer/graph_values.h | 169 +++ .../graph_localizer/graph_values_params.h | 34 + .../include/graph_localizer/key_info.h | 59 + .../graph_localizer/loc_factor_adder.h | 47 + .../graph_localizer/loc_factor_adder_params.h | 48 + .../include/graph_localizer/loc_pose_factor.h | 52 + .../graph_localizer/loc_projection_factor.h | 237 ++++ .../include/graph_localizer/noise_params.h | 34 + .../graph_localizer/parameter_reader.h | 57 + .../graph_localizer/pose_rotation_factor.h | 98 ++ .../graph_localizer/projection_factor_adder.h | 49 + .../projection_factor_adder_params.h | 45 + .../robust_smart_projection_pose_factor.h | 221 +++ .../graph_localizer/rotation_factor_adder.h | 44 + .../rotation_factor_adder_params.h | 37 + .../include/graph_localizer/sanity_checker.h | 41 + .../graph_localizer/sanity_checker_params.h | 35 + .../include/graph_localizer/serialization.h | 30 + ...smart_projection_cumulative_factor_adder.h | 48 + .../smart_projection_factor_adder_params.h | 51 + .../graph_localizer/standstill_factor_adder.h | 46 + .../standstill_factor_adder_params.h | 34 + .../include/graph_localizer/utilities.h | 88 ++ .../launch/graph_localizer.launch | 27 + .../graph_localizer/nodelet_plugins.xml | 11 + localization/graph_localizer/package.xml | 26 + localization/graph_localizer/readme.md | 33 + .../graph_localizer/src/feature_tracker.cc | 119 ++ .../graph_localizer/src/graph_localizer.cc | 1243 +++++++++++++++++ .../src/graph_localizer_intializer.cc | 185 +++ .../src/graph_localizer_nodelet.cc | 277 ++++ .../src/graph_localizer_wrapper.cc | 318 +++++ .../graph_localizer/src/graph_stats.cc | 169 +++ .../graph_localizer/src/graph_values.cc | 494 +++++++ .../graph_localizer/src/loc_factor_adder.cc | 103 ++ .../graph_localizer/src/parameter_reader.cc | 215 +++ .../src/projection_factor_adder.cc | 89 ++ .../src/rotation_factor_adder.cc | 103 ++ .../graph_localizer/src/sanity_checker.cc | 55 + .../graph_localizer/src/serialization.cc | 93 ++ ...mart_projection_cumulative_factor_adder.cc | 89 ++ .../src/standstill_factor_adder.cc | 80 ++ localization/graph_localizer/src/utilities.cc | 253 ++++ .../test/test_rotation_factor.cc | 53 + .../test/test_rotation_factor.test | 20 + .../test/test_rotation_factor_adder.cc | 153 ++ .../test/test_rotation_factor_adder.test | 20 + .../ground_truth_localizer/CMakeLists.txt | 38 + .../ground_truth_localizer_nodelet.h | 72 + .../include/ground_truth_localizer/twist.h | 30 + .../ground_truth_localizer/utilities.h | 37 + .../launch/ground_truth_localizer.launch | 27 + .../nodelet_plugins.xml | 11 + .../ground_truth_localizer/package.xml | 26 + localization/ground_truth_localizer/readme.md | 5 + .../src/ground_truth_localizer_nodelet.cc | 90 ++ .../ground_truth_localizer/src/utilities.cc | 52 + localization/imu_augmentor/CMakeLists.txt | 37 + .../include/imu_augmentor/imu_augmentor.h | 38 + .../imu_augmentor/imu_augmentor_nodelet.h | 70 + .../imu_augmentor/imu_augmentor_params.h | 29 + .../imu_augmentor/imu_augmentor_wrapper.h | 69 + .../imu_augmentor/launch/imu_augmentor.launch | 27 + .../imu_augmentor/nodelet_plugins.xml | 11 + localization/imu_augmentor/package.xml | 28 + localization/imu_augmentor/readme.md | 16 + .../imu_augmentor/src/imu_augmentor.cc | 53 + .../src/imu_augmentor_nodelet.cc | 118 ++ .../src/imu_augmentor_wrapper.cc | 151 ++ localization/imu_integration/CMakeLists.txt | 31 + .../butterworth_lowpass_filter.h | 43 + .../butterworth_lowpass_filter_3rd_order.h | 45 + .../butterworth_lowpass_filter_5th_order.h | 43 + .../butterworth_lowpass_filter_5th_order_05.h | 43 + .../butterworth_lowpass_filter_5th_order_1.h | 43 + .../include/imu_integration/filter.h | 31 + .../include/imu_integration/identity_filter.h | 33 + .../include/imu_integration/imu_filter.h | 51 + .../imu_integration/imu_filter_params.h | 30 + .../include/imu_integration/imu_integrator.h | 83 ++ .../imu_integration/imu_integrator_params.h | 43 + .../imu_integration/latest_imu_integrator.h | 48 + .../latest_imu_integrator_params.h | 32 + .../include/imu_integration/utilities.h | 58 + localization/imu_integration/package.xml | 17 + localization/imu_integration/readme.md | 14 + .../src/butterworth_lowpass_filter.cc | 63 + .../butterworth_lowpass_filter_3rd_order.cc | 62 + .../butterworth_lowpass_filter_5th_order.cc | 63 + ...butterworth_lowpass_filter_5th_order_05.cc | 65 + .../butterworth_lowpass_filter_5th_order_1.cc | 64 + .../imu_integration/src/identity_filter.cc | 26 + .../imu_integration/src/imu_filter.cc | 100 ++ .../imu_integration/src/imu_integrator.cc | 177 +++ .../src/latest_imu_integrator.cc | 60 + localization/imu_integration/src/utilities.cc | 95 ++ .../launch/lk_optical_flow.launch | 4 +- .../localization_common/CMakeLists.txt | 31 + .../include/localization_common/averager.h | 72 + .../localization_common/combined_nav_state.h | 58 + .../combined_nav_state_covariances.h | 63 + .../include/localization_common/logger.h | 164 +++ .../include/localization_common/rate_timer.h | 48 + .../include/localization_common/ros_timer.h | 49 + .../include/localization_common/time.h | 27 + .../include/localization_common/timer.h | 52 + .../include/localization_common/utilities.h | 128 ++ localization/localization_common/package.xml | 17 + localization/localization_common/readme.md | 11 + .../localization_common/src/averager.cc | 106 ++ .../src/combined_nav_state.cc | 29 + .../src/combined_nav_state_covariances.cc | 62 + .../localization_common/src/rate_timer.cc | 71 + .../localization_common/src/ros_timer.cc | 41 + localization/localization_common/src/time.cc | 23 + localization/localization_common/src/timer.cc | 60 + .../localization_common/src/utilities.cc | 158 +++ .../launch/localization_manager.launch | 2 + .../localization_measurements/CMakeLists.txt | 31 + .../localization_measurements/feature_point.h | 57 + .../feature_points_measurement.h | 34 + .../localization_measurements/image_point.h | 28 + .../imu_measurement.h | 50 + .../localization_measurements/map_point.h | 28 + .../matched_projection.h | 40 + .../matched_projections_measurement.h | 36 + .../localization_measurements/measurement.h | 33 + .../measurement_conversions.h | 42 + .../localization_measurements/package.xml | 19 + .../localization_measurements/readme.md | 5 + .../src/measurement_conversions.cc | 69 + .../launch/localization_node.launch | 2 + .../marker_tracking/ros/src/marker_tracker.cc | 5 +- localization/readme.md | 26 +- .../include/sparse_mapping/tensor.h | 2 +- localization/sparse_mapping/src/tensor.cc | 5 +- management/executive/src/executive.cc | 3 + management/sys_monitor/src/sys_monitor.cc | 6 +- .../docker/astrobee_base_kinetic.Dockerfile | 7 +- .../docker/astrobee_base_melodic.Dockerfile | 5 +- scripts/docker/astrobee_kinetic.Dockerfile | 3 +- scripts/docker/astrobee_melodic.Dockerfile | 3 +- scripts/docker/build.sh | 21 +- .../cross_compile/astrobee_cross.Dockerfile | 16 - .../cross_compile/astrobee_debian.Dockerfile | 39 + .../docker/cross_compile/debian_compile.sh | 55 + scripts/docker/readme.md | 32 +- .../docker/test_astrobee_kinetic.Dockerfile | 2 +- .../docker/test_astrobee_melodic.Dockerfile | 2 +- scripts/setup/debians/.gitignore | 2 + scripts/setup/debians/build_gtsam.sh | 32 + .../setup/debians/build_install_debians.sh | 6 + .../debians/build_install_debians_18_04.sh | 6 + scripts/setup/debians/gtsam/changelog | 5 + scripts/setup/debians/gtsam/compat | 1 + scripts/setup/debians/gtsam/control | 18 + scripts/setup/debians/gtsam/copyright | 31 + scripts/setup/debians/gtsam/libgtsam-dev.dirs | 2 + .../setup/debians/gtsam/libgtsam-dev.install | 2 + scripts/setup/debians/gtsam/libgtsam.dirs | 1 + scripts/setup/debians/gtsam/libgtsam.install | 1 + scripts/setup/debians/gtsam/rules | 26 + scripts/setup/debians/gtsam/source/format | 1 + scripts/setup/packages_base.lst | 2 + scripts/setup/packages_base_18.lst | 2 + scripts/teleop/run_estimate_bias.sh | 20 + scripts/teleop/run_reset_localization.sh | 20 + shared/ff_util/include/ff_util/ff_faults.h | 4 +- shared/ff_util/include/ff_util/ff_names.h | 13 +- shared/ff_util/launch/ff_nodelet.launch | 3 +- shared/jsonloader/src/command_repo.cc | 3 +- shared/msg_conversions/CMakeLists.txt | 5 + .../include/msg_conversions/msg_conversions.h | 127 +- shared/msg_conversions/src/msg_conversions.cc | 151 +- simulation/CMakeLists.txt | 1 + .../gazebo_sensor_plugin_ar_tags.cc | 173 +-- .../gazebo_sensor_plugin_optical_flow.cc | 3 +- .../gazebo_sensor_plugin_sci_cam.cc | 4 +- .../gazebo_sensor_plugin_sparse_map.cc | 3 +- submodules/platform | 2 +- tools/CMakeLists.txt | 3 + tools/ekf_bag/scripts/bag_sweep.py | 26 + tools/ekf_bag/scripts/ekf_graph.py | 18 +- tools/ekf_bag/scripts/test_ekf_graph.py | 30 +- tools/ekf_bag/src/ekf_bag.cc | 46 +- tools/ekf_bag/src/ekf_bag_csv.cc | 4 + .../scripts/communications/com_manager.py | 4 + .../scripts/communications/data_support.py | 6 + .../scripts/communications/dds_support.py | 12 +- .../dds_types/BaseDDSProfile.xml | 30 + .../dds_types/PositionSample.xml | 29 + tools/gnc_visualizer/scripts/plot_types.py | 19 +- tools/gnc_visualizer/scripts/visualizer.py | 18 +- tools/graph_bag/CMakeLists.txt | 39 + .../include/graph_bag/bag_imu_filterer.h | 44 + tools/graph_bag/include/graph_bag/graph_bag.h | 71 + .../include/graph_bag/graph_bag_params.h | 37 + .../graph_bag/graph_localizer_simulator.h | 58 + .../graph_localizer_simulator_params.h | 27 + .../graph_bag/live_measurement_simulator.h | 80 ++ .../live_measurement_simulator_params.h | 40 + .../include/graph_bag/message_buffer.h | 61 + .../include/graph_bag/message_buffer_params.h | 30 + .../include/graph_bag/parameter_reader.h | 38 + .../graph_bag/sparse_mapping_pose_adder.h | 45 + tools/graph_bag/include/graph_bag/utilities.h | 38 + tools/graph_bag/package.xml | 29 + tools/graph_bag/readme.md | 33 + tools/graph_bag/scripts/average_results.py | 59 + .../scripts/bag_and_parameter_sweep.py | 104 ++ tools/graph_bag/scripts/bag_sweep.py | 109 ++ tools/graph_bag/scripts/bag_sweep_main.py | 40 + .../graph_bag/scripts/check_bags_for_gaps.py | 38 + tools/graph_bag/scripts/config_creator.py | 50 + tools/graph_bag/scripts/imu_analyzer.py | 202 +++ tools/graph_bag/scripts/imu_analyzer_main.py | 41 + tools/graph_bag/scripts/imu_measurements.py | 33 + tools/graph_bag/scripts/loc_states.py | 57 + tools/graph_bag/scripts/merge_bags.py | 65 + .../scripts/multiprocessing_helpers.py | 32 + tools/graph_bag/scripts/orientations.py | 31 + tools/graph_bag/scripts/parameter_sweep.py | 170 +++ .../scripts/plot_bag_sweep_results.py | 130 ++ tools/graph_bag/scripts/plot_helpers.py | 80 ++ .../scripts/plot_parameter_sweep_results.py | 82 ++ tools/graph_bag/scripts/plot_results.py | 412 ++++++ tools/graph_bag/scripts/plot_results_main.py | 36 + tools/graph_bag/scripts/poses.py | 47 + tools/graph_bag/scripts/rmse_utilities.py | 72 + .../graph_bag/scripts/test_rmse_utilities.py | 172 +++ tools/graph_bag/scripts/utilities.py | 108 ++ tools/graph_bag/scripts/vector3d.py | 25 + tools/graph_bag/scripts/vector3ds.py | 45 + tools/graph_bag/src/bag_imu_filterer.cc | 74 + tools/graph_bag/src/graph_bag.cc | 187 +++ .../src/graph_localizer_simulator.cc | 74 + .../src/live_measurement_simulator.cc | 167 +++ tools/graph_bag/src/parameter_reader.cc | 57 + .../src/sparse_mapping_pose_adder.cc | 64 + tools/graph_bag/src/utilities.cc | 86 ++ tools/graph_bag/tools/run_bag_imu_filterer.cc | 69 + tools/graph_bag/tools/run_graph_bag.cc | 119 ++ .../tools/run_sparse_mapping_pose_adder.cc | 85 ++ tools/imu_bias_tester/CMakeLists.txt | 37 + .../include/imu_bias_tester/imu_bias_tester.h | 52 + .../imu_bias_tester/imu_bias_tester_nodelet.h | 56 + .../imu_bias_tester/imu_bias_tester_wrapper.h | 44 + .../launch/imu_bias_tester.launch | 27 + tools/imu_bias_tester/nodelet_plugins.xml | 11 + tools/imu_bias_tester/package.xml | 28 + tools/imu_bias_tester/readme.md | 11 + tools/imu_bias_tester/src/imu_bias_tester.cc | 82 ++ .../src/imu_bias_tester_nodelet.cc | 72 + .../src/imu_bias_tester_wrapper.cc | 54 + .../localization_rviz_plugins/CMakeLists.txt | 40 + .../nodelet_plugins.xml | 38 + tools/localization_rviz_plugins/package.xml | 22 + .../plugin_description.xml | 24 + tools/localization_rviz_plugins/readme.md | 18 + .../src/imu_augmentor_display.cc | 102 ++ .../src/imu_augmentor_display.h | 68 + .../src/localization_graph_display.cc | 516 +++++++ .../src/localization_graph_display.h | 117 ++ .../src/localization_graph_panel.cc | 380 +++++ .../src/localization_graph_panel.h | 82 ++ .../src/pose_display.cc | 71 + .../src/pose_display.h | 63 + .../src/slider_property.cc | 44 + .../src/slider_property.h | 54 + .../src/sparse_mapping_display.cc | 96 ++ .../src/sparse_mapping_display.h | 58 + .../src/utilities.cc | 100 ++ .../localization_rviz_plugins/src/utilities.h | 62 + tools/readme.md | 12 +- 345 files changed, 20519 insertions(+), 1039 deletions(-) create mode 100644 astrobee/config/graph_localizer.config create mode 100644 astrobee/config/tools/graph_bag.config create mode 100644 astrobee/launch/offline_localization/loc_only.launch create mode 100644 astrobee/launch/offline_localization/loc_rviz.launch create mode 100644 astrobee/launch/offline_localization/localization_from_bag.launch create mode 100644 astrobee/launch/offline_localization/record_localization.launch create mode 100644 astrobee/launch/offline_localization/replay_localization.launch delete mode 100644 astrobee/launch/replay.launch create mode 100644 astrobee/resources/rviz/loc_granite.rviz create mode 100644 astrobee/resources/rviz/loc_iss.rviz create mode 100644 communications/dds_ros_bridge/include/dds_ros_bridge/rapid_position_provider_ros_pose_helper.h create mode 100644 communications/dds_ros_bridge/include/dds_ros_bridge/ros_pose_rapid_position.h create mode 100644 communications/dds_ros_bridge/src/rapid_position_provider_ros_pose_helper.cc create mode 100644 communications/dds_ros_bridge/src/ros_pose_rapid_position.cc create mode 100644 communications/ff_msgs/msg/GraphState.msg create mode 100644 communications/ff_msgs/msg/LocalizationGraph.msg create mode 100644 localization/graph_localizer/CMakeLists.txt create mode 100644 localization/graph_localizer/include/graph_localizer/calibration_params.h create mode 100644 localization/graph_localizer/include/graph_localizer/cumulative_factor_adder.h create mode 100644 localization/graph_localizer/include/graph_localizer/factor_adder.h create mode 100644 localization/graph_localizer/include/graph_localizer/factor_adder_params.h create mode 100644 localization/graph_localizer/include/graph_localizer/factor_params.h create mode 100644 localization/graph_localizer/include/graph_localizer/factor_to_add.h create mode 100644 localization/graph_localizer/include/graph_localizer/feature_counts.h create mode 100644 localization/graph_localizer/include/graph_localizer/feature_track.h create mode 100644 localization/graph_localizer/include/graph_localizer/feature_tracker.h create mode 100644 localization/graph_localizer/include/graph_localizer/feature_tracker_params.h create mode 100644 localization/graph_localizer/include/graph_localizer/graph_action.h create mode 100644 localization/graph_localizer/include/graph_localizer/graph_initializer_params.h create mode 100644 localization/graph_localizer/include/graph_localizer/graph_localizer.h create mode 100644 localization/graph_localizer/include/graph_localizer/graph_localizer_initializer.h create mode 100644 localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet.h create mode 100644 localization/graph_localizer/include/graph_localizer/graph_localizer_params.h create mode 100644 localization/graph_localizer/include/graph_localizer/graph_localizer_wrapper.h create mode 100644 localization/graph_localizer/include/graph_localizer/graph_stats.h create mode 100644 localization/graph_localizer/include/graph_localizer/graph_values.h create mode 100644 localization/graph_localizer/include/graph_localizer/graph_values_params.h create mode 100644 localization/graph_localizer/include/graph_localizer/key_info.h create mode 100644 localization/graph_localizer/include/graph_localizer/loc_factor_adder.h create mode 100644 localization/graph_localizer/include/graph_localizer/loc_factor_adder_params.h create mode 100644 localization/graph_localizer/include/graph_localizer/loc_pose_factor.h create mode 100644 localization/graph_localizer/include/graph_localizer/loc_projection_factor.h create mode 100644 localization/graph_localizer/include/graph_localizer/noise_params.h create mode 100644 localization/graph_localizer/include/graph_localizer/parameter_reader.h create mode 100644 localization/graph_localizer/include/graph_localizer/pose_rotation_factor.h create mode 100644 localization/graph_localizer/include/graph_localizer/projection_factor_adder.h create mode 100644 localization/graph_localizer/include/graph_localizer/projection_factor_adder_params.h create mode 100644 localization/graph_localizer/include/graph_localizer/robust_smart_projection_pose_factor.h create mode 100644 localization/graph_localizer/include/graph_localizer/rotation_factor_adder.h create mode 100644 localization/graph_localizer/include/graph_localizer/rotation_factor_adder_params.h create mode 100644 localization/graph_localizer/include/graph_localizer/sanity_checker.h create mode 100644 localization/graph_localizer/include/graph_localizer/sanity_checker_params.h create mode 100644 localization/graph_localizer/include/graph_localizer/serialization.h create mode 100644 localization/graph_localizer/include/graph_localizer/smart_projection_cumulative_factor_adder.h create mode 100644 localization/graph_localizer/include/graph_localizer/smart_projection_factor_adder_params.h create mode 100644 localization/graph_localizer/include/graph_localizer/standstill_factor_adder.h create mode 100644 localization/graph_localizer/include/graph_localizer/standstill_factor_adder_params.h create mode 100644 localization/graph_localizer/include/graph_localizer/utilities.h create mode 100644 localization/graph_localizer/launch/graph_localizer.launch create mode 100644 localization/graph_localizer/nodelet_plugins.xml create mode 100644 localization/graph_localizer/package.xml create mode 100644 localization/graph_localizer/readme.md create mode 100644 localization/graph_localizer/src/feature_tracker.cc create mode 100644 localization/graph_localizer/src/graph_localizer.cc create mode 100644 localization/graph_localizer/src/graph_localizer_intializer.cc create mode 100644 localization/graph_localizer/src/graph_localizer_nodelet.cc create mode 100644 localization/graph_localizer/src/graph_localizer_wrapper.cc create mode 100644 localization/graph_localizer/src/graph_stats.cc create mode 100644 localization/graph_localizer/src/graph_values.cc create mode 100644 localization/graph_localizer/src/loc_factor_adder.cc create mode 100644 localization/graph_localizer/src/parameter_reader.cc create mode 100644 localization/graph_localizer/src/projection_factor_adder.cc create mode 100644 localization/graph_localizer/src/rotation_factor_adder.cc create mode 100644 localization/graph_localizer/src/sanity_checker.cc create mode 100644 localization/graph_localizer/src/serialization.cc create mode 100644 localization/graph_localizer/src/smart_projection_cumulative_factor_adder.cc create mode 100644 localization/graph_localizer/src/standstill_factor_adder.cc create mode 100644 localization/graph_localizer/src/utilities.cc create mode 100644 localization/graph_localizer/test/test_rotation_factor.cc create mode 100644 localization/graph_localizer/test/test_rotation_factor.test create mode 100644 localization/graph_localizer/test/test_rotation_factor_adder.cc create mode 100644 localization/graph_localizer/test/test_rotation_factor_adder.test create mode 100644 localization/ground_truth_localizer/CMakeLists.txt create mode 100644 localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h create mode 100644 localization/ground_truth_localizer/include/ground_truth_localizer/twist.h create mode 100644 localization/ground_truth_localizer/include/ground_truth_localizer/utilities.h create mode 100644 localization/ground_truth_localizer/launch/ground_truth_localizer.launch create mode 100644 localization/ground_truth_localizer/nodelet_plugins.xml create mode 100644 localization/ground_truth_localizer/package.xml create mode 100644 localization/ground_truth_localizer/readme.md create mode 100644 localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc create mode 100644 localization/ground_truth_localizer/src/utilities.cc create mode 100644 localization/imu_augmentor/CMakeLists.txt create mode 100644 localization/imu_augmentor/include/imu_augmentor/imu_augmentor.h create mode 100644 localization/imu_augmentor/include/imu_augmentor/imu_augmentor_nodelet.h create mode 100644 localization/imu_augmentor/include/imu_augmentor/imu_augmentor_params.h create mode 100644 localization/imu_augmentor/include/imu_augmentor/imu_augmentor_wrapper.h create mode 100644 localization/imu_augmentor/launch/imu_augmentor.launch create mode 100644 localization/imu_augmentor/nodelet_plugins.xml create mode 100644 localization/imu_augmentor/package.xml create mode 100644 localization/imu_augmentor/readme.md create mode 100644 localization/imu_augmentor/src/imu_augmentor.cc create mode 100644 localization/imu_augmentor/src/imu_augmentor_nodelet.cc create mode 100644 localization/imu_augmentor/src/imu_augmentor_wrapper.cc create mode 100644 localization/imu_integration/CMakeLists.txt create mode 100644 localization/imu_integration/include/imu_integration/butterworth_lowpass_filter.h create mode 100644 localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_3rd_order.h create mode 100644 localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_5th_order.h create mode 100644 localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_5th_order_05.h create mode 100644 localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_5th_order_1.h create mode 100644 localization/imu_integration/include/imu_integration/filter.h create mode 100644 localization/imu_integration/include/imu_integration/identity_filter.h create mode 100644 localization/imu_integration/include/imu_integration/imu_filter.h create mode 100644 localization/imu_integration/include/imu_integration/imu_filter_params.h create mode 100644 localization/imu_integration/include/imu_integration/imu_integrator.h create mode 100644 localization/imu_integration/include/imu_integration/imu_integrator_params.h create mode 100644 localization/imu_integration/include/imu_integration/latest_imu_integrator.h create mode 100644 localization/imu_integration/include/imu_integration/latest_imu_integrator_params.h create mode 100644 localization/imu_integration/include/imu_integration/utilities.h create mode 100644 localization/imu_integration/package.xml create mode 100644 localization/imu_integration/readme.md create mode 100644 localization/imu_integration/src/butterworth_lowpass_filter.cc create mode 100644 localization/imu_integration/src/butterworth_lowpass_filter_3rd_order.cc create mode 100644 localization/imu_integration/src/butterworth_lowpass_filter_5th_order.cc create mode 100644 localization/imu_integration/src/butterworth_lowpass_filter_5th_order_05.cc create mode 100644 localization/imu_integration/src/butterworth_lowpass_filter_5th_order_1.cc create mode 100644 localization/imu_integration/src/identity_filter.cc create mode 100644 localization/imu_integration/src/imu_filter.cc create mode 100644 localization/imu_integration/src/imu_integrator.cc create mode 100644 localization/imu_integration/src/latest_imu_integrator.cc create mode 100644 localization/imu_integration/src/utilities.cc create mode 100644 localization/localization_common/CMakeLists.txt create mode 100644 localization/localization_common/include/localization_common/averager.h create mode 100644 localization/localization_common/include/localization_common/combined_nav_state.h create mode 100644 localization/localization_common/include/localization_common/combined_nav_state_covariances.h create mode 100644 localization/localization_common/include/localization_common/logger.h create mode 100644 localization/localization_common/include/localization_common/rate_timer.h create mode 100644 localization/localization_common/include/localization_common/ros_timer.h create mode 100644 localization/localization_common/include/localization_common/time.h create mode 100644 localization/localization_common/include/localization_common/timer.h create mode 100644 localization/localization_common/include/localization_common/utilities.h create mode 100644 localization/localization_common/package.xml create mode 100644 localization/localization_common/readme.md create mode 100644 localization/localization_common/src/averager.cc create mode 100644 localization/localization_common/src/combined_nav_state.cc create mode 100644 localization/localization_common/src/combined_nav_state_covariances.cc create mode 100644 localization/localization_common/src/rate_timer.cc create mode 100644 localization/localization_common/src/ros_timer.cc create mode 100644 localization/localization_common/src/time.cc create mode 100644 localization/localization_common/src/timer.cc create mode 100644 localization/localization_common/src/utilities.cc create mode 100644 localization/localization_measurements/CMakeLists.txt create mode 100644 localization/localization_measurements/include/localization_measurements/feature_point.h create mode 100644 localization/localization_measurements/include/localization_measurements/feature_points_measurement.h create mode 100644 localization/localization_measurements/include/localization_measurements/image_point.h create mode 100644 localization/localization_measurements/include/localization_measurements/imu_measurement.h create mode 100644 localization/localization_measurements/include/localization_measurements/map_point.h create mode 100644 localization/localization_measurements/include/localization_measurements/matched_projection.h create mode 100644 localization/localization_measurements/include/localization_measurements/matched_projections_measurement.h create mode 100644 localization/localization_measurements/include/localization_measurements/measurement.h create mode 100644 localization/localization_measurements/include/localization_measurements/measurement_conversions.h create mode 100644 localization/localization_measurements/package.xml create mode 100644 localization/localization_measurements/readme.md create mode 100644 localization/localization_measurements/src/measurement_conversions.cc create mode 100644 scripts/docker/cross_compile/astrobee_debian.Dockerfile create mode 100755 scripts/docker/cross_compile/debian_compile.sh create mode 100755 scripts/setup/debians/build_gtsam.sh create mode 100644 scripts/setup/debians/gtsam/changelog create mode 100644 scripts/setup/debians/gtsam/compat create mode 100644 scripts/setup/debians/gtsam/control create mode 100644 scripts/setup/debians/gtsam/copyright create mode 100644 scripts/setup/debians/gtsam/libgtsam-dev.dirs create mode 100644 scripts/setup/debians/gtsam/libgtsam-dev.install create mode 100644 scripts/setup/debians/gtsam/libgtsam.dirs create mode 100644 scripts/setup/debians/gtsam/libgtsam.install create mode 100755 scripts/setup/debians/gtsam/rules create mode 100644 scripts/setup/debians/gtsam/source/format create mode 100755 scripts/teleop/run_estimate_bias.sh create mode 100755 scripts/teleop/run_reset_localization.sh create mode 100644 tools/gnc_visualizer/scripts/communications/dds_types/PositionSample.xml create mode 100644 tools/graph_bag/CMakeLists.txt create mode 100644 tools/graph_bag/include/graph_bag/bag_imu_filterer.h create mode 100644 tools/graph_bag/include/graph_bag/graph_bag.h create mode 100644 tools/graph_bag/include/graph_bag/graph_bag_params.h create mode 100644 tools/graph_bag/include/graph_bag/graph_localizer_simulator.h create mode 100644 tools/graph_bag/include/graph_bag/graph_localizer_simulator_params.h create mode 100644 tools/graph_bag/include/graph_bag/live_measurement_simulator.h create mode 100644 tools/graph_bag/include/graph_bag/live_measurement_simulator_params.h create mode 100644 tools/graph_bag/include/graph_bag/message_buffer.h create mode 100644 tools/graph_bag/include/graph_bag/message_buffer_params.h create mode 100644 tools/graph_bag/include/graph_bag/parameter_reader.h create mode 100644 tools/graph_bag/include/graph_bag/sparse_mapping_pose_adder.h create mode 100644 tools/graph_bag/include/graph_bag/utilities.h create mode 100644 tools/graph_bag/package.xml create mode 100644 tools/graph_bag/readme.md create mode 100755 tools/graph_bag/scripts/average_results.py create mode 100755 tools/graph_bag/scripts/bag_and_parameter_sweep.py create mode 100644 tools/graph_bag/scripts/bag_sweep.py create mode 100755 tools/graph_bag/scripts/bag_sweep_main.py create mode 100755 tools/graph_bag/scripts/check_bags_for_gaps.py create mode 100644 tools/graph_bag/scripts/config_creator.py create mode 100644 tools/graph_bag/scripts/imu_analyzer.py create mode 100755 tools/graph_bag/scripts/imu_analyzer_main.py create mode 100644 tools/graph_bag/scripts/imu_measurements.py create mode 100644 tools/graph_bag/scripts/loc_states.py create mode 100755 tools/graph_bag/scripts/merge_bags.py create mode 100644 tools/graph_bag/scripts/multiprocessing_helpers.py create mode 100644 tools/graph_bag/scripts/orientations.py create mode 100755 tools/graph_bag/scripts/parameter_sweep.py create mode 100755 tools/graph_bag/scripts/plot_bag_sweep_results.py create mode 100644 tools/graph_bag/scripts/plot_helpers.py create mode 100755 tools/graph_bag/scripts/plot_parameter_sweep_results.py create mode 100644 tools/graph_bag/scripts/plot_results.py create mode 100755 tools/graph_bag/scripts/plot_results_main.py create mode 100644 tools/graph_bag/scripts/poses.py create mode 100644 tools/graph_bag/scripts/rmse_utilities.py create mode 100644 tools/graph_bag/scripts/test_rmse_utilities.py create mode 100644 tools/graph_bag/scripts/utilities.py create mode 100644 tools/graph_bag/scripts/vector3d.py create mode 100644 tools/graph_bag/scripts/vector3ds.py create mode 100644 tools/graph_bag/src/bag_imu_filterer.cc create mode 100644 tools/graph_bag/src/graph_bag.cc create mode 100644 tools/graph_bag/src/graph_localizer_simulator.cc create mode 100644 tools/graph_bag/src/live_measurement_simulator.cc create mode 100644 tools/graph_bag/src/parameter_reader.cc create mode 100644 tools/graph_bag/src/sparse_mapping_pose_adder.cc create mode 100644 tools/graph_bag/src/utilities.cc create mode 100644 tools/graph_bag/tools/run_bag_imu_filterer.cc create mode 100644 tools/graph_bag/tools/run_graph_bag.cc create mode 100644 tools/graph_bag/tools/run_sparse_mapping_pose_adder.cc create mode 100644 tools/imu_bias_tester/CMakeLists.txt create mode 100644 tools/imu_bias_tester/include/imu_bias_tester/imu_bias_tester.h create mode 100644 tools/imu_bias_tester/include/imu_bias_tester/imu_bias_tester_nodelet.h create mode 100644 tools/imu_bias_tester/include/imu_bias_tester/imu_bias_tester_wrapper.h create mode 100644 tools/imu_bias_tester/launch/imu_bias_tester.launch create mode 100644 tools/imu_bias_tester/nodelet_plugins.xml create mode 100644 tools/imu_bias_tester/package.xml create mode 100644 tools/imu_bias_tester/readme.md create mode 100644 tools/imu_bias_tester/src/imu_bias_tester.cc create mode 100644 tools/imu_bias_tester/src/imu_bias_tester_nodelet.cc create mode 100644 tools/imu_bias_tester/src/imu_bias_tester_wrapper.cc create mode 100644 tools/localization_rviz_plugins/CMakeLists.txt create mode 100644 tools/localization_rviz_plugins/nodelet_plugins.xml create mode 100644 tools/localization_rviz_plugins/package.xml create mode 100644 tools/localization_rviz_plugins/plugin_description.xml create mode 100644 tools/localization_rviz_plugins/readme.md create mode 100644 tools/localization_rviz_plugins/src/imu_augmentor_display.cc create mode 100644 tools/localization_rviz_plugins/src/imu_augmentor_display.h create mode 100644 tools/localization_rviz_plugins/src/localization_graph_display.cc create mode 100644 tools/localization_rviz_plugins/src/localization_graph_display.h create mode 100644 tools/localization_rviz_plugins/src/localization_graph_panel.cc create mode 100644 tools/localization_rviz_plugins/src/localization_graph_panel.h create mode 100644 tools/localization_rviz_plugins/src/pose_display.cc create mode 100644 tools/localization_rviz_plugins/src/pose_display.h create mode 100644 tools/localization_rviz_plugins/src/slider_property.cc create mode 100644 tools/localization_rviz_plugins/src/slider_property.h create mode 100644 tools/localization_rviz_plugins/src/sparse_mapping_display.cc create mode 100644 tools/localization_rviz_plugins/src/sparse_mapping_display.h create mode 100644 tools/localization_rviz_plugins/src/utilities.cc create mode 100644 tools/localization_rviz_plugins/src/utilities.h diff --git a/CMakeLists.txt b/CMakeLists.txt index c287591fc9..ea7c9db632 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,7 @@ cmake_minimum_required(VERSION 3.0) project(Astrobee) -set(ASTROBEE_VERSION 0.13.0) +set(ASTROBEE_VERSION 0.14.0) # Define our options option(USE_CCACHE @@ -69,6 +69,10 @@ option(ENABLE_VIVE_SOLVER option(ENABLE_INTEGRATION_TESTING "Build the integration tests if tests are active." ON) +option(BUILD_LOC_RVIZ_PLUGINS + "Build the localization rviz plugins." + ON) + if ( "${CMAKE_VERSION}" VERSION_GREATER 3.0.0 ) cmake_policy(SET CMP0045 OLD) @@ -212,7 +216,7 @@ endif (NOT PROTOBUF_PROTOC_EXECUTABLE) find_package(Ceres REQUIRED) -find_package(Boost 1.54.0 QUIET REQUIRED COMPONENTS filesystem system iostreams thread program_options) +find_package(Boost 1.54.0 QUIET REQUIRED COMPONENTS filesystem system iostreams thread program_options timer) find_package(Eigen3 REQUIRED) find_package(ZLIB) find_package(Luajit20 REQUIRED) @@ -239,6 +243,7 @@ set(OpenCV_LIBRARIES ${OpenCV_LIBS}) find_package(dbow2 REQUIRED) find_package(Alvar REQUIRED) +find_package(GTSAM REQUIRED) find_package(OpenMVG QUIET REQUIRED) find_package(JsonCpp REQUIRED) find_package(FFMPEG QUIET) # Optional @@ -261,6 +266,15 @@ if (NOT USE_CTC) # pkg-config needs extra work to be used for cross compile endif (YAMLCPP_FOUND) endif (NOT USE_CTC) +if(ENABLE_GOOGLE_PROF) + SET(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--no-as-needed -lprofiler -Wl,--as-needed") + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DDEBUG") +endif() + +if(USE_CTC) + set(BUILD_LOC_RVIZ_PLUGINS OFF) +endif() + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-unused-local-typedefs -Wno-packed-bitfield-compat -std=c++11") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-unused-local-typedefs -Wno-packed-bitfield-compat -std=c++11") set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-undefined") @@ -294,7 +308,8 @@ if (USE_CTC) endif (USE_CTC) if (USE_ROS) set(ENV{ROS_LANG_DISABLE} "genlisp:gennodejs:geneus:$ENV{ROS_LANG_DISABLE}") - find_package(catkin2 COMPONENTS roscpp message_generation std_msgs geometry_msgs sensor_msgs cv_bridge image_transport tf tf2 tf2_ros rosbag nodelet) + find_package(catkin2 COMPONENTS roscpp message_generation std_msgs geometry_msgs sensor_msgs cv_bridge image_transport tf tf2 tf2_ros rosbag nodelet rviz) + find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) if (IS_BAMBOO_BUILD) include(BambooFix) diff --git a/RELEASE.md b/RELEASE.md index 9386a2e510..ca0197c468 100644 --- a/RELEASE.md +++ b/RELEASE.md @@ -1,5 +1,14 @@ # Releases +## Release 0.14.0 + + * Added graph_localizer package + * Added supporting packages for graph localizer including imu_augmetor, imu_integation, localization_common, localization_measurements + * Added tools for graph localizer including graph_bag and imu_bias_tester + * Added rviz plugins for graph localizer and associated packages in localization_rviz_plugins + * Added gtsam debian + Note: Perching does not work in this release, use another version for this. + ## Release 0.13.0 * Multiple updates to documentation, doxygen is exported to github pages diff --git a/astrobee.doxyfile b/astrobee.doxyfile index 6f80ecb5a9..4931b43e07 100644 --- a/astrobee.doxyfile +++ b/astrobee.doxyfile @@ -39,7 +39,7 @@ PROJECT_NAME = "NASA Astrobee Robot Software" # control system is used. -PROJECT_NUMBER = 0.13.0 +PROJECT_NUMBER = 0.14.0 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/astrobee/commands/freeFlyerPlanSchema.json b/astrobee/commands/freeFlyerPlanSchema.json index 9a5cc6452b..5097536bd0 100644 --- a/astrobee/commands/freeFlyerPlanSchema.json +++ b/astrobee/commands/freeFlyerPlanSchema.json @@ -112,7 +112,8 @@ ["EkfState", "EkfState"], ["GncState", "GncState"], ["PmcCmdState", "PmcCmdState"], - ["Position", "Position"] + ["Position", "Position"], + ["SparseMappingPose", "SparseMappingPose"] ] }, diff --git a/astrobee/config/communications/dds_ros_bridge.config b/astrobee/config/communications/dds_ros_bridge.config index 87bc29001b..ad2aa479d7 100644 --- a/astrobee/config/communications/dds_ros_bridge.config +++ b/astrobee/config/communications/dds_ros_bridge.config @@ -148,6 +148,16 @@ pub_topic_RLRLS = "" use_RORP = true pub_topic_RORP = "" +-- ros_sparse_mapping_pose_rapid_position => RSMPRP -- +use_RSMPRP = true +pub_topic_RSMPRP = "-sparse" +use_rate_RSMPRP = true + +-- ros_vive_pose_rapid_position => RVPRP -- +use_RVPRP = false +pub_topic_RVPRP = "-vive" +use_rate_RVPRP = false + -- ros_payload_power_state_rapid_payload_power_state => RPPSRPPS -- use_RPPSRPPS = true pub_topic_RPPSRPPS = "" @@ -175,6 +185,7 @@ ekf_state_rate = 0 gnc_state_rate = 0 pmc_command_rate = 0 position_rate = 30 +sparse_mapping_pose_rate = 0 battery_time_round_to_multiple = 10; diff --git a/astrobee/config/faults.config b/astrobee/config/faults.config index c1c5d23710..f78681e217 100644 --- a/astrobee/config/faults.config +++ b/astrobee/config/faults.config @@ -18,59 +18,59 @@ -- Autogenerated from FMECA! DO NOT CHANGE! arm = { - {id=61, key="INITIALIZATION_FAILED", description="Arm Initialization Failed"}, + {id=63, key="INITIALIZATION_FAILED", description="Arm Initialization Failed"}, } all = { - {id=85, key="UNKNOWN_FAULT_KEY", description="Unknown Fault Key"}, + {id=87, key="UNKNOWN_FAULT_KEY", description="Unknown Fault Key"}, } access_control = { - {id=70, key="INITIALIZATION_FAILED", description="Access Control Initialization Failed"}, + {id=72, key="INITIALIZATION_FAILED", description="Access Control Initialization Failed"}, } executive = { - {id=68, key="INITIALIZATION_FAILED", description="Executive Initialization Failed"}, + {id=70, key="INITIALIZATION_FAILED", description="Executive Initialization Failed"}, } data_bagger = { - {id=83, key="INITIALIZATION_FAILED", description="Data Bagger Initialization Failed"}, + {id=85, key="INITIALIZATION_FAILED", description="Data Bagger Initialization Failed"}, } llp_cpu_mem_monitor = { - {id=72, key="INITIALIZATION_FAILED", description="LLP CPU Monitor Initialization Failed"}, - {id=86, key="LOAD_TOO_HIGH", description="LLP CPU load too high"}, - {id=97, key="MEMORY_USAGE_TOO_HIGH", description="LLP Memory usage too high"}, - {id=100, key="TEMPERATURE_TOO_HIGH", description="LLP Temperature too high"}, + {id=74, key="INITIALIZATION_FAILED", description="LLP CPU Monitor Initialization Failed"}, + {id=88, key="LOAD_TOO_HIGH", description="LLP CPU load too high"}, + {id=99, key="MEMORY_USAGE_TOO_HIGH", description="LLP Memory usage too high"}, + {id=102, key="TEMPERATURE_TOO_HIGH", description="LLP Temperature too high"}, } llp_disk_monitor = { - {id=75, key="INITIALIZATION_FAILED", description="LLP Disk Monitor Initialization Failed"}, - {id=89, key="DISK_CONFIG_INVALID", description="LLP Disk conifg not valid"}, - {id=91, key="DISK_USAGE_HIGH", description="LLP Disk usage getting high"}, - {id=94, key="DISK_USAGE_TOO_HIGH", description="LLP Disk usage too high"}, + {id=77, key="INITIALIZATION_FAILED", description="LLP Disk Monitor Initialization Failed"}, + {id=91, key="DISK_CONFIG_INVALID", description="LLP Disk conifg not valid"}, + {id=93, key="DISK_USAGE_HIGH", description="LLP Disk usage getting high"}, + {id=96, key="DISK_USAGE_TOO_HIGH", description="LLP Disk usage too high"}, } image_sampler = { - {id=81, key="INITIALIZATION_FAILED", description="Image Sampler Initialization Failed"}, + {id=83, key="INITIALIZATION_FAILED", description="Image Sampler Initialization Failed"}, } mlp_disk_monitor = { - {id=76, key="INITIALIZATION_FAILED", description="MLP Disk Monitor Initialization Failed"}, - {id=90, key="DISK_CONFIG_INVALID", description="MLP Disk config not valid"}, - {id=92, key="DISK_USAGE_HIGH", description="MLP Disk usage getting high"}, - {id=95, key="DISK_USAGE_TOO_HIGH", description="MLP Disk usage too high"}, + {id=78, key="INITIALIZATION_FAILED", description="MLP Disk Monitor Initialization Failed"}, + {id=92, key="DISK_CONFIG_INVALID", description="MLP Disk config not valid"}, + {id=94, key="DISK_USAGE_HIGH", description="MLP Disk usage getting high"}, + {id=97, key="DISK_USAGE_TOO_HIGH", description="MLP Disk usage too high"}, } mlp_cpu_mem_monitor = { - {id=73, key="INITIALIZATION_FAILED", description="MLP CPU Monitor Initialization Failed"}, - {id=87, key="LOAD_TOO_HIGH", description="MLP CPU load too high"}, - {id=98, key="MEMORY_USAGE_TOO_HIGH", description="MLP Memory usage too high"}, - {id=101, key="TEMPERATURE_TOO_HIGH", description="MLP Temperature too high"}, + {id=75, key="INITIALIZATION_FAILED", description="MLP CPU Monitor Initialization Failed"}, + {id=89, key="LOAD_TOO_HIGH", description="MLP CPU load too high"}, + {id=100, key="MEMORY_USAGE_TOO_HIGH", description="MLP Memory usage too high"}, + {id=103, key="TEMPERATURE_TOO_HIGH", description="MLP Temperature too high"}, } sys_monitor = { - {id=105, key="TIME_DIFF_TOO_HIGH", description="Time difference between mlp and llp too high"}, + {id=107, key="TIME_DIFF_TOO_HIGH", description="Time difference between mlp and llp too high"}, } optical_flow_nodelet = { @@ -86,29 +86,29 @@ handrail_detect = { } localization_manager = { - {id=62, key="INITIALIZATION_FAILED", description="Localization Manager Initialization Failed"}, - {id=117, key="LOCALIZATION_PIPELINE_UNSTABLE", description="Localization pipeline is not producing registration or features."}, - {id=119, key="ESTIMATE_CONFIDENCE_TOO_LOW", description="Estimator confidence too low for too long"}, + {id=64, key="INITIALIZATION_FAILED", description="Localization Manager Initialization Failed"}, + {id=119, key="LOCALIZATION_PIPELINE_UNSTABLE", description="Localization pipeline is not producing registration or features."}, + {id=121, key="ESTIMATE_CONFIDENCE_TOO_LOW", description="Estimator confidence too low for too long"}, } dds_ros_bridge = { - {id=71, key="INITIALIZATION_FAILED", description="DDS ROS Bridge Initialization Failed"}, + {id=73, key="INITIALIZATION_FAILED", description="DDS ROS Bridge Initialization Failed"}, } mapper = { - {id=80, key="INITIALIZATION_FAILED", description="Mapper Initialization Failed"}, + {id=82, key="INITIALIZATION_FAILED", description="Mapper Initialization Failed"}, } planner_qp = { - {id=78, key="INITIALIZATION_FAILED", description="QP Planner Initialization Failed"}, + {id=80, key="INITIALIZATION_FAILED", description="QP Planner Initialization Failed"}, } planner_trapezoidal = { - {id=79, key="INITIALIZATION_FAILED", description="Trapezoidal Planner Initialization Failed"}, + {id=81, key="INITIALIZATION_FAILED", description="Trapezoidal Planner Initialization Failed"}, } choreographer = { - {id=67, key="INITIALIZATION_FAILED", description="Choreographer Initialization Failed"}, + {id=69, key="INITIALIZATION_FAILED", description="Choreographer Initialization Failed"}, } laser = { @@ -118,11 +118,11 @@ eps_driver = { } perching_arm = { - {id=60, key="INITIALIZATION_FAILED", description="Perching Arm Initialization Failed"}, + {id=62, key="INITIALIZATION_FAILED", description="Perching Arm Initialization Failed"}, } framestore = { - {id=82, key="INITIALIZATION_FAILED", description="Frame Store Initialization Failed"}, + {id=84, key="INITIALIZATION_FAILED", description="Frame Store Initialization Failed"}, } pmc_actuator = { @@ -138,45 +138,48 @@ flashlight_aft = { } pico_driver = { - {id=59, key="INITIALIZATION_FAILED", description="PicoFlexx Initialization Failed"}, - {id=124, key="NO_COMMUNICATION_HAZCAM", description="Unable to communicate with the Hazcam"}, + {id=61, key="INITIALIZATION_FAILED", description="PicoFlexx Initialization Failed"}, + {id=126, key="NO_COMMUNICATION_HAZCAM", description="Unable to communicate with the Hazcam"}, } dock_cam = { } ctl = { - {id=53, key="INITIALIZATION_FAILED", description="Control Initialization Failed"}, - {id=133, key="KINETIC_ENERGY_INCREASING", description="Kinetic energy increasing in stopping mode"}, + {id=54, key="INITIALIZATION_FAILED", description="Control Initialization Failed"}, + {id=135, key="KINETIC_ENERGY_INCREASING", description="Kinetic energy increasing in stopping mode"}, } -ekf = { - {id=54, key="INITIALIZATION_FAILED", description="EKF Initialization Failed"}, - {id=134, key="CONTROL_CYCLE_OVERRUN", description="Control Cycle Overrun"}, - {id=136, key="POSE_ESTIMATE_NON_PHYSICAL", description="Pose estimate non-physical"}, +imu_aug = { + {id=56, key="INITIALIZATION_FAILED", description="IMU Augmentor Initialization Failed"}, +} + +graph_loc = { + {id=55, key="INITIALIZATION_FAILED", description="Graph Localizer Initialization Failed"}, + {id=137, key="POSE_ESTIMATE_NON_PHYSICAL", description="Pose estimate non-physical"}, } fam = { - {id=55, key="INITIALIZATION_FAILED", description="FAM Initialization Failed"}, + {id=57, key="INITIALIZATION_FAILED", description="FAM Initialization Failed"}, } speed_cam = { {id=10, key="VELOCITY_TOO_HIGH", description="Velocity too high"}, - {id=56, key="INITIALIZATION_FAILED", description="Speedcam Initialization Failed"}, - {id=126, key="NO_COMMUNICATION_SPEEDCAM", description="Unable to communicate with the Speedcam"}, + {id=58, key="INITIALIZATION_FAILED", description="Speedcam Initialization Failed"}, + {id=128, key="NO_COMMUNICATION_SPEEDCAM", description="Unable to communicate with the Speedcam"}, } epson_imu = { - {id=112, key="IMU_ACCEL_ABOVE_LIMITS", description="IMU acceleration above limits"}, - {id=145, key="BAD_GYRO_DATA", description="IMU returns bad gyro data"}, - {id=146, key="IMU_OVERTEMP", description="IMU overtemp"}, - {id=147, key="SELF_TEST_FAULT_CODE", description="IMU Fault code - Self test"}, - {id=148, key="HARDWARE_ERROR_FAULT_CODE", description="IMU Fault code - Hardware error"}, - {id=150, key="BAD_ACCEL_DATA", description="IMU returns bad accel data"}, - {id=151, key="NO_IMU_DATA_PACKETS", description="Loss of data packets from IMU"}, + {id=114, key="IMU_ACCEL_ABOVE_LIMITS", description="IMU acceleration above limits"}, + {id=146, key="BAD_GYRO_DATA", description="IMU returns bad gyro data"}, + {id=147, key="IMU_OVERTEMP", description="IMU overtemp"}, + {id=148, key="SELF_TEST_FAULT_CODE", description="IMU Fault code - Self test"}, + {id=149, key="HARDWARE_ERROR_FAULT_CODE", description="IMU Fault code - Hardware error"}, + {id=151, key="BAD_ACCEL_DATA", description="IMU returns bad accel data"}, + {id=152, key="NO_IMU_DATA_PACKETS", description="Loss of data packets from IMU"}, } dock = { - {id=84, key="INITIALIZATION_FAILED", description="Docker Initialization Failed"}, + {id=86, key="INITIALIZATION_FAILED", description="Docker Initialization Failed"}, } diff --git a/astrobee/config/graph_localizer.config b/astrobee/config/graph_localizer.config new file mode 100644 index 0000000000..8820bab93b --- /dev/null +++ b/astrobee/config/graph_localizer.config @@ -0,0 +1,150 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- 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 = 2 +-- 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 +max_standstill_feature_track_avg_distance_from_mean = 0.075 +standstill_min_num_points_per_track = 5 +-- 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 +-- 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 +-- Lost Threshold +position_cov_log_det_lost_threshold = 0 +orientation_cov_log_det_lost_threshold = 0 +-- Feature Tracker +feature_tracker_sliding_window_duration = 0.5 +-- Standstill +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 = 10 +smart_projection_adder_min_num_points = 2 +smart_projection_adder_rotation_only_fallback = false +smart_projection_adder_splitting = true +smart_projection_adder_scale_noise_with_num_points = true +smart_projection_adder_noise_scale = 2 +-- 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.01 +loc_adder_prior_quaternion_stddev = 0.01 +loc_adder_min_num_matches = 5 +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_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 +-- Imu integration +-- butter, butter3, butter5, butter5_1, butter5_05, none +imu_filter = "butter" +imu_augmentor_standstill = true +-- 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 +gyro_sigma = 0.005 +-- 0.1 / 60; // 10 cm VRW +accel_sigma = 0.03 +accel_bias_sigma = 0.01 +gyro_bias_sigma = 0.001 +integration_variance = 0.05 +bias_acc_omega_int = 0.0001 +-- Other +verbose = false +fatal_failures = 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.config b/astrobee/config/localization.config index 16e538c03d..c8c6ba2266 100644 --- a/astrobee/config/localization.config +++ b/astrobee/config/localization.config @@ -23,7 +23,7 @@ histogram_equalization = 1 min_features = 200 max_features = 800 detection_retries = 1 -num_threads = 2 +num_threads = 1 min_brisk_threshold = 20.0 default_brisk_threshold = 90.0 max_brisk_threshold = 110.0 diff --git a/astrobee/config/management/data_bagger.config b/astrobee/config/management/data_bagger.config index d8319293dd..1f3bed440e 100644 --- a/astrobee/config/management/data_bagger.config +++ b/astrobee/config/management/data_bagger.config @@ -30,6 +30,8 @@ bag_size_bytes = 96*1024*1024 default_topics = {{topic="gnc/ctl/traj", downlink="immediate", frequency=-1}, {topic="gnc/ekf", downlink="immediate", frequency=-1}, + {topic="sparse_mapping/pose", downlink="immediate", frequency=-1}, + {topic="graph_loc/state", downlink="immediate", frequency=-1}, {topic="hw/eps/battery/bottom_left/state", downlink="immediate", frequency=-1}, {topic="hw/eps/battery/bottom_left/temp", downlink="immediate", frequency=-1}, {topic="hw/eps/battery/bottom_right/state", downlink="immediate", frequency=-1}, diff --git a/astrobee/config/management/fault_table.config b/astrobee/config/management/fault_table.config index 9a60f7e281..93e803efd1 100644 --- a/astrobee/config/management/fault_table.config +++ b/astrobee/config/management/fault_table.config @@ -22,185 +22,188 @@ require "management/fault_functions" subsystems={ {name="behaviors", nodes={ {name="arm", faults={ - {id=22, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Arm", heartbeat={timeout_sec=1.1, misses=2.0}}, - {id=61, warning=false, blocking=false, response=command("unloadNodelet", "arm", ""), key="INITIALIZATION_FAILED", description="Arm Initialization Failed"}, + {id=23, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Arm", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=63, warning=false, blocking=false, response=command("unloadNodelet", "arm", ""), key="INITIALIZATION_FAILED", description="Arm Initialization Failed"}, }}, }}, {name="management", nodes={ {name="all", faults={ - {id=85, warning=false, blocking=false, response=command("noOp"), key="UNKNOWN_FAULT_KEY", description="Unknown Fault Key"}, + {id=87, warning=false, blocking=false, response=command("noOp"), key="UNKNOWN_FAULT_KEY", description="Unknown Fault Key"}, }}, {name="access_control", faults={ - {id=31, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat from Access Control", heartbeat={timeout_sec=1.1, misses=3.0}}, - {id=70, warning=false, blocking=true, response=command("unloadNodelet", "access_control", ""), key="INITIALIZATION_FAILED", description="Access Control Initialization Failed"}, + {id=32, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat from Access Control", heartbeat={timeout_sec=1.1, misses=3.0}}, + {id=72, warning=false, blocking=true, response=command("unloadNodelet", "access_control", ""), key="INITIALIZATION_FAILED", description="Access Control Initialization Failed"}, }}, {name="executive", faults={ - {id=29, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Executive", heartbeat={timeout_sec=2.1, misses=2.0}}, - {id=68, warning=false, blocking=false, response=command("unloadNodelet", "executive", ""), key="INITIALIZATION_FAILED", description="Executive Initialization Failed"}, + {id=30, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Executive", heartbeat={timeout_sec=2.1, misses=2.0}}, + {id=70, warning=false, blocking=false, response=command("unloadNodelet", "executive", ""), key="INITIALIZATION_FAILED", description="Executive Initialization Failed"}, }}, {name="data_bagger", faults={ - {id=47, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Data Bagger", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=83, warning=false, blocking=false, response=command("unloadNodelet", "data_bagger", ""), key="INITIALIZATION_FAILED", description="Data Bagger Initialization Failed"}, + {id=48, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Data Bagger", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=85, warning=false, blocking=false, response=command("unloadNodelet", "data_bagger", ""), key="INITIALIZATION_FAILED", description="Data Bagger Initialization Failed"}, }}, {name="llp_cpu_mem_monitor", faults={ - {id=33, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from LLP CPU Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, - {id=72, warning=false, blocking=true, response=command("unloadNodelet", "llp_cpu_mem_monitor", ""), key="INITIALIZATION_FAILED", description="LLP CPU Monitor Initialization Failed"}, - {id=86, warning=true, blocking=true, response=command("stopAllMotion"), key="LOAD_TOO_HIGH", description="LLP CPU load too high"}, - {id=97, warning=true, blocking=false, response=command("noOp"), key="MEMORY_USAGE_TOO_HIGH", description="LLP Memory usage too high"}, - {id=100, warning=true, blocking=true, response=command("stopAllMotion"), key="TEMPERATURE_TOO_HIGH", description="LLP Temperature too high"}, + {id=34, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from LLP CPU Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=74, warning=false, blocking=true, response=command("unloadNodelet", "llp_cpu_mem_monitor", ""), key="INITIALIZATION_FAILED", description="LLP CPU Monitor Initialization Failed"}, + {id=88, warning=true, blocking=true, response=command("stopAllMotion"), key="LOAD_TOO_HIGH", description="LLP CPU load too high"}, + {id=99, warning=true, blocking=false, response=command("noOp"), key="MEMORY_USAGE_TOO_HIGH", description="LLP Memory usage too high"}, + {id=102, warning=true, blocking=true, response=command("stopAllMotion"), key="TEMPERATURE_TOO_HIGH", description="LLP Temperature too high"}, }}, {name="llp_disk_monitor", faults={ - {id=36, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat from LLP Disk Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, - {id=75, warning=false, blocking=true, response=command("unloadNodelet", "llp_disk_monitor", ""), key="INITIALIZATION_FAILED", description="LLP Disk Monitor Initialization Failed"}, - {id=89, warning=true, blocking=false, response=command("noOp"), key="DISK_CONFIG_INVALID", description="LLP Disk conifg not valid"}, - {id=91, warning=true, blocking=false, response=command("noOp"), key="DISK_USAGE_HIGH", description="LLP Disk usage getting high"}, - {id=94, warning=false, blocking=true, response=command("fault"), key="DISK_USAGE_TOO_HIGH", description="LLP Disk usage too high"}, + {id=37, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat from LLP Disk Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=77, warning=false, blocking=true, response=command("unloadNodelet", "llp_disk_monitor", ""), key="INITIALIZATION_FAILED", description="LLP Disk Monitor Initialization Failed"}, + {id=91, warning=true, blocking=false, response=command("noOp"), key="DISK_CONFIG_INVALID", description="LLP Disk conifg not valid"}, + {id=93, warning=true, blocking=false, response=command("noOp"), key="DISK_USAGE_HIGH", description="LLP Disk usage getting high"}, + {id=96, warning=false, blocking=true, response=command("fault"), key="DISK_USAGE_TOO_HIGH", description="LLP Disk usage too high"}, }}, {name="image_sampler", faults={ - {id=42, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Image Sampler", heartbeat={timeout_sec=1.1, misses=5.0}}, - {id=81, warning=false, blocking=false, response=command("unloadNodelet", "image_sampler", ""), key="INITIALIZATION_FAILED", description="Image Sampler Initialization Failed"}, + {id=43, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Image Sampler", heartbeat={timeout_sec=1.1, misses=5.0}}, + {id=83, warning=false, blocking=false, response=command("unloadNodelet", "image_sampler", ""), key="INITIALIZATION_FAILED", description="Image Sampler Initialization Failed"}, }}, {name="mlp_disk_monitor", faults={ - {id=37, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat from MLP Disk Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, - {id=76, warning=false, blocking=true, response=command("unloadNodelet", "mlp_disk_monitor", ""), key="INITIALIZATION_FAILED", description="MLP Disk Monitor Initialization Failed"}, - {id=90, warning=true, blocking=false, response=command("noOp"), key="DISK_CONFIG_INVALID", description="MLP Disk config not valid"}, - {id=92, warning=true, blocking=false, response=command("noOp"), key="DISK_USAGE_HIGH", description="MLP Disk usage getting high"}, - {id=95, warning=false, blocking=true, response=command("fault"), key="DISK_USAGE_TOO_HIGH", description="MLP Disk usage too high"}, + {id=38, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat from MLP Disk Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=78, warning=false, blocking=true, response=command("unloadNodelet", "mlp_disk_monitor", ""), key="INITIALIZATION_FAILED", description="MLP Disk Monitor Initialization Failed"}, + {id=92, warning=true, blocking=false, response=command("noOp"), key="DISK_CONFIG_INVALID", description="MLP Disk config not valid"}, + {id=94, warning=true, blocking=false, response=command("noOp"), key="DISK_USAGE_HIGH", description="MLP Disk usage getting high"}, + {id=97, warning=false, blocking=true, response=command("fault"), key="DISK_USAGE_TOO_HIGH", description="MLP Disk usage too high"}, }}, {name="mlp_cpu_mem_monitor", faults={ - {id=34, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from MLP CPU Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, - {id=73, warning=false, blocking=true, response=command("unloadNodelet", "mlp_cpu_monitor", ""), key="INITIALIZATION_FAILED", description="MLP CPU Monitor Initialization Failed"}, - {id=87, warning=true, blocking=true, response=command("stopAllMotion"), key="LOAD_TOO_HIGH", description="MLP CPU load too high"}, - {id=98, warning=true, blocking=false, response=command("noOp"), key="MEMORY_USAGE_TOO_HIGH", description="MLP Memory usage too high"}, - {id=101, warning=true, blocking=true, response=command("stopAllMotion"), key="TEMPERATURE_TOO_HIGH", description="MLP Temperature too high"}, + {id=35, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from MLP CPU Monitor", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=75, warning=false, blocking=true, response=command("unloadNodelet", "mlp_cpu_monitor", ""), key="INITIALIZATION_FAILED", description="MLP CPU Monitor Initialization Failed"}, + {id=89, warning=true, blocking=true, response=command("stopAllMotion"), key="LOAD_TOO_HIGH", description="MLP CPU load too high"}, + {id=100, warning=true, blocking=false, response=command("noOp"), key="MEMORY_USAGE_TOO_HIGH", description="MLP Memory usage too high"}, + {id=103, warning=true, blocking=true, response=command("stopAllMotion"), key="TEMPERATURE_TOO_HIGH", description="MLP Temperature too high"}, }}, }}, {name="avionics", nodes={ {name="sys_monitor", faults={ - {id=105, warning=true, blocking=false, response=command("noOp"), key="TIME_DIFF_TOO_HIGH", description="Time difference between mlp and llp too high"}, + {id=107, warning=true, blocking=false, response=command("noOp"), key="TIME_DIFF_TOO_HIGH", description="Time difference between mlp and llp too high"}, }}, }}, {name="localization", nodes={ {name="optical_flow_nodelet", faults={ - {id=27, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from VisualOdometer", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=28, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from VisualOdometer", heartbeat={timeout_sec=1.1, misses=2.0}}, }}, {name="marker_tracking", faults={ - {id=25, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from DockLoc (ar tags)", heartbeat={timeout_sec=1.1, misses=4.0}}, + {id=26, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from DockLoc (ar tags)", heartbeat={timeout_sec=1.1, misses=4.0}}, }}, {name="localization_node", faults={ - {id=24, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from NavLoc (sparse mapping)", heartbeat={timeout_sec=1.1, misses=4.0}}, + {id=25, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from NavLoc (sparse mapping)", heartbeat={timeout_sec=1.1, misses=4.0}}, }}, {name="handrail_detect", faults={ - {id=26, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from PerchLoc (handrail detect)", heartbeat={timeout_sec=1.1, misses=4.0}}, + {id=27, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from PerchLoc (handrail detect)", heartbeat={timeout_sec=1.1, misses=4.0}}, }}, {name="localization_manager", faults={ - {id=23, warning=false, blocking=true, response=command("idlePropulsion"), key="HEARTBEAT_MISSING", description="No Heartbeat from Localization Manager", heartbeat={timeout_sec=1.1, misses=2.0}}, - {id=62, warning=false, blocking=true, response=command("unloadNodelet", "localization_manager", ""), key="INITIALIZATION_FAILED", description="Localization Manager Initialization Failed"}, - {id=117, warning=false, blocking=false, response=command("stopAllMotion"), key="LOCALIZATION_PIPELINE_UNSTABLE", description="Localization pipeline is not producing registration or features."}, - {id=119, warning=false, blocking=true, response=command("idlePropulsion"), key="ESTIMATE_CONFIDENCE_TOO_LOW", description="Estimator confidence too low for too long"}, + {id=24, warning=false, blocking=true, response=command("idlePropulsion"), key="HEARTBEAT_MISSING", description="No Heartbeat from Localization Manager", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=64, warning=false, blocking=true, response=command("unloadNodelet", "localization_manager", ""), key="INITIALIZATION_FAILED", description="Localization Manager Initialization Failed"}, + {id=119, warning=false, blocking=false, response=command("noOp"), key="LOCALIZATION_PIPELINE_UNSTABLE", description="Localization pipeline is not producing registration or features."}, + {id=121, warning=false, blocking=true, response=command("idlePropulsion"), key="ESTIMATE_CONFIDENCE_TOO_LOW", description="Estimator confidence too low for too long"}, }}, }}, {name="communication", nodes={ {name="dds_ros_bridge", faults={ - {id=32, warning=false, blocking=false, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat form DDS ROS Bridge", heartbeat={timeout_sec=1.1, misses=5.0}}, - {id=71, warning=false, blocking=false, response=command("unloadNodelet", "dds_ros_bridge", ""), key="INITIALIZATION_FAILED", description="DDS ROS Bridge Initialization Failed"}, + {id=33, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat form DDS ROS Bridge", heartbeat={timeout_sec=1.1, misses=5.0}}, + {id=73, warning=false, blocking=true, response=command("unloadNodelet", "dds_ros_bridge", ""), key="INITIALIZATION_FAILED", description="DDS ROS Bridge Initialization Failed"}, }}, }}, {name="mobility", nodes={ {name="mapper", faults={ - {id=41, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Mapper", heartbeat={timeout_sec=1.1, misses=2.0}}, - {id=80, warning=false, blocking=false, response=command("unloadNodelet", "mapper", ""), key="INITIALIZATION_FAILED", description="Mapper Initialization Failed"}, + {id=42, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Mapper", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=82, warning=false, blocking=false, response=command("unloadNodelet", "mapper", ""), key="INITIALIZATION_FAILED", description="Mapper Initialization Failed"}, }}, {name="planner_qp", faults={ - {id=39, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from QP Planner", heartbeat={timeout_sec=1.1, misses=5.0}}, - {id=78, warning=false, blocking=false, response=command("unloadNodelet", "planner_qp", ""), key="INITIALIZATION_FAILED", description="QP Planner Initialization Failed"}, + {id=40, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from QP Planner", heartbeat={timeout_sec=1.1, misses=5.0}}, + {id=80, warning=false, blocking=false, response=command("unloadNodelet", "planner_qp", ""), key="INITIALIZATION_FAILED", description="QP Planner Initialization Failed"}, }}, {name="planner_trapezoidal", faults={ - {id=40, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Trapezoidal Planner", heartbeat={timeout_sec=1.1, misses=5.0}}, - {id=79, warning=false, blocking=false, response=command("unloadNodelet", "planner_trapezoidal", ""), key="INITIALIZATION_FAILED", description="Trapezoidal Planner Initialization Failed"}, + {id=41, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Trapezoidal Planner", heartbeat={timeout_sec=1.1, misses=5.0}}, + {id=81, warning=false, blocking=false, response=command("unloadNodelet", "planner_trapezoidal", ""), key="INITIALIZATION_FAILED", description="Trapezoidal Planner Initialization Failed"}, }}, {name="choreographer", faults={ - {id=28, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Choreographer", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=67, warning=false, blocking=false, response=command("unloadNodelet", "choreographer", ""), key="INITIALIZATION_FAILED", description="Choreographer Initialization Failed"}, + {id=29, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Choreographer", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=69, warning=false, blocking=false, response=command("unloadNodelet", "choreographer", ""), key="INITIALIZATION_FAILED", description="Choreographer Initialization Failed"}, }}, }}, {name="hardware", nodes={ {name="laser", faults={ - {id=44, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Laser", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=45, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Laser", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, {name="eps_driver", faults={ {id=11, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from EPS", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, {name="perching_arm", faults={ - {id=21, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Perching Arm", heartbeat={timeout_sec=1.1, misses=2.0}}, - {id=60, warning=false, blocking=false, response=command("unloadNodelet", "perching_arm", ""), key="INITIALIZATION_FAILED", description="Perching Arm Initialization Failed"}, + {id=22, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Perching Arm", heartbeat={timeout_sec=1.1, misses=2.0}}, + {id=62, warning=false, blocking=false, response=command("unloadNodelet", "perching_arm", ""), key="INITIALIZATION_FAILED", description="Perching Arm Initialization Failed"}, }}, {name="framestore", faults={ - {id=43, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from FrameStore", heartbeat={timeout_sec=1.1, misses=3.0}}, - {id=82, warning=false, blocking=false, response=command("unloadNodelet", "framestore", ""), key="INITIALIZATION_FAILED", description="Frame Store Initialization Failed"}, + {id=44, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from FrameStore", heartbeat={timeout_sec=1.1, misses=3.0}}, + {id=84, warning=false, blocking=false, response=command("unloadNodelet", "framestore", ""), key="INITIALIZATION_FAILED", description="Frame Store Initialization Failed"}, }}, {name="pmc_actuator", faults={ {id=13, warning=false, blocking=false, response=command("idlePropulsion"), key="HEARTBEAT_MISSING", description="No Heartbeat from PMC Actuator", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, {name="flashlight_front", faults={ - {id=45, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Front Flashlight", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=46, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Front Flashlight", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, {name="nav_cam", faults={ - {id=18, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Navcam", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=19, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Navcam", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, {name="flashlight_aft", faults={ - {id=46, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Aft Flashlight", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=47, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Aft Flashlight", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, {name="pico_driver", faults={ - {id=20, warning=false, blocking=false, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from PicoFlexx", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=59, warning=false, blocking=false, response=command("unloadNodelet", "pico_driver", ""), key="INITIALIZATION_FAILED", description="PicoFlexx Initialization Failed"}, - {id=124, warning=false, blocking=false, response=command("stop"), key="NO_COMMUNICATION_HAZCAM", description="Unable to communicate with the Hazcam"}, + {id=21, warning=false, blocking=false, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from PicoFlexx", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=61, warning=false, blocking=false, response=command("unloadNodelet", "pico_driver", ""), key="INITIALIZATION_FAILED", description="PicoFlexx Initialization Failed"}, + {id=126, warning=false, blocking=false, response=command("stop"), key="NO_COMMUNICATION_HAZCAM", description="Unable to communicate with the Hazcam"}, }}, {name="dock_cam", faults={ - {id=19, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Dockcam", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=20, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Dockcam", heartbeat={timeout_sec=1.1, misses=1.0}}, }}, }}, {name="gnc", nodes={ {name="ctl", faults={ {id=14, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Control", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=53, warning=false, blocking=false, response=command("unloadNodelet", "ctl", ""), key="INITIALIZATION_FAILED", description="Control Initialization Failed"}, - {id=133, warning=false, blocking=false, response=command("noOp"), key="KINETIC_ENERGY_INCREASING", description="Kinetic energy increasing in stopping mode"}, + {id=54, warning=false, blocking=false, response=command("unloadNodelet", "ctl", ""), key="INITIALIZATION_FAILED", description="Control Initialization Failed"}, + {id=135, warning=false, blocking=false, response=command("noOp"), key="KINETIC_ENERGY_INCREASING", description="Kinetic energy increasing in stopping mode"}, }}, - {name="ekf", faults={ - {id=15, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from EKF", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=54, warning=false, blocking=false, response=command("unloadNodelet", "ekf", ""), key="INITIALIZATION_FAILED", description="EKF Initialization Failed"}, - {id=134, warning=true, blocking=false, response=command("noOp"), key="CONTROL_CYCLE_OVERRUN", description="Control Cycle Overrun"}, - {id=136, warning=false, blocking=false, response=command("noOp"), key="POSE_ESTIMATE_NON_PHYSICAL", description="Pose estimate non-physical"}, + {name="imu_aug", 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=56, warning=false, blocking=true, response=command("unloadNodelet", "imu_aug", ""), key="INITIALIZATION_FAILED", description="IMU Augmentor 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}}, + {id=55, warning=false, blocking=true, response=command("unloadNodelet", "graph_loc", ""), key="INITIALIZATION_FAILED", description="Graph Localizer Initialization Failed"}, + {id=137, warning=false, blocking=false, response=command("noOp"), key="POSE_ESTIMATE_NON_PHYSICAL", description="Pose estimate non-physical"}, }}, {name="fam", faults={ - {id=16, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from FAM", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=55, warning=false, blocking=false, response=command("unloadNodelet", "fam", ""), key="INITIALIZATION_FAILED", description="FAM Initialization Failed"}, + {id=17, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from FAM", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=57, warning=false, blocking=false, response=command("unloadNodelet", "fam", ""), key="INITIALIZATION_FAILED", description="FAM Initialization Failed"}, }}, }}, {name="speed camera", nodes={ {name="speed_cam", faults={ {id=10, warning=false, blocking=false, response=command("idlePropulsion"), key="VELOCITY_TOO_HIGH", description="Velocity too high"}, - {id=17, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from Speedcam", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=56, warning=false, blocking=true, response=command("unloadNodelet", "speed_cam", ""), key="INITIALIZATION_FAILED", description="Speedcam Initialization Failed"}, - {id=126, warning=false, blocking=true, response=command("idlePropulsion"), key="NO_COMMUNICATION_SPEEDCAM", description="Unable to communicate with the Speedcam"}, + {id=18, warning=false, blocking=true, response=command("stopAllMotion"), key="HEARTBEAT_MISSING", description="No Heartbeat from Speedcam", heartbeat={timeout_sec=1.1, misses=1.0}}, + {id=58, warning=false, blocking=true, response=command("unloadNodelet", "speed_cam", ""), key="INITIALIZATION_FAILED", description="Speedcam Initialization Failed"}, + {id=128, warning=false, blocking=true, response=command("idlePropulsion"), key="NO_COMMUNICATION_SPEEDCAM", description="Unable to communicate with the Speedcam"}, }}, }}, {name="imu", nodes={ {name="epson_imu", faults={ {id=12, warning=false, blocking=false, response=command("idlePropulsion"), key="HEARTBEAT_MISSING", description="No Heartbeat from IMU", heartbeat={timeout_sec=1.1, misses=1.0}}, - {id=112, warning=false, blocking=true, response=command("idlePropulsion"), key="IMU_ACCEL_ABOVE_LIMITS", description="IMU acceleration above limits"}, - {id=145, warning=true, blocking=false, response=command("noOp"), key="BAD_GYRO_DATA", description="IMU returns bad gyro data"}, - {id=146, warning=false, blocking=true, response=command("idlePropulsion"), key="IMU_OVERTEMP", description="IMU overtemp"}, - {id=147, warning=false, blocking=true, response=command("fault"), key="SELF_TEST_FAULT_CODE", description="IMU Fault code - Self test"}, - {id=148, warning=false, blocking=true, response=command("fault"), key="HARDWARE_ERROR_FAULT_CODE", description="IMU Fault code - Hardware error"}, - {id=150, warning=true, blocking=false, response=command("noOp"), key="BAD_ACCEL_DATA", description="IMU returns bad accel data"}, - {id=151, warning=false, blocking=true, response=command("idlePropulsion"), key="NO_IMU_DATA_PACKETS", description="Loss of data packets from IMU"}, + {id=114, warning=false, blocking=true, response=command("idlePropulsion"), key="IMU_ACCEL_ABOVE_LIMITS", description="IMU acceleration above limits"}, + {id=146, warning=true, blocking=false, response=command("noOp"), key="BAD_GYRO_DATA", description="IMU returns bad gyro data"}, + {id=147, warning=false, blocking=true, response=command("idlePropulsion"), key="IMU_OVERTEMP", description="IMU overtemp"}, + {id=148, warning=false, blocking=true, response=command("fault"), key="SELF_TEST_FAULT_CODE", description="IMU Fault code - Self test"}, + {id=149, warning=false, blocking=true, response=command("fault"), key="HARDWARE_ERROR_FAULT_CODE", description="IMU Fault code - Hardware error"}, + {id=151, warning=true, blocking=false, response=command("noOp"), key="BAD_ACCEL_DATA", description="IMU returns bad accel data"}, + {id=152, warning=false, blocking=true, response=command("idlePropulsion"), key="NO_IMU_DATA_PACKETS", description="Loss of data packets from IMU"}, }}, }}, {name="procedures", nodes={ {name="dock", faults={ - {id=48, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Docker", heartbeat={timeout_sec=1.1, misses=4.0}}, - {id=84, warning=false, blocking=false, response=command("unloadNodelet", "dock", ""), key="INITIALIZATION_FAILED", description="Docker Initialization Failed"}, + {id=49, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Docker", heartbeat={timeout_sec=1.1, misses=4.0}}, + {id=86, warning=false, blocking=false, response=command("unloadNodelet", "dock", ""), key="INITIALIZATION_FAILED", description="Docker Initialization Failed"}, }}, }}, } diff --git a/astrobee/config/management/sys_monitor.config b/astrobee/config/management/sys_monitor.config index da56d3d563..7a7f7f26b4 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 = "ekf" +time_diff_node = "imu_aug" -- Threshold used to trigger time drift fault time_drift_thres_sec = 0.25 @@ -66,7 +66,8 @@ nodelet_types={ {name="speed_cam", type="speed_cam/SpeedCamNode"}, {name="pico_driver", type="pico_driver/PicoDriverNodelet"}, {name="ctl", type="ctl/CtlNodelet"}, - {name="ekf", type="ekf/EkfNodelet"}, + {name="graph_loc", type="graph_localizer/GraphLocalizerNodelet"}, + {name="imu_aug", type="imu_augmentor/ImuAugmentorNodelet"}, {name="fam", type="fam/FamNodelet"}, {name="arm", type="arm/ArmNodelet"}, {name="dock", type="dock/DockNodelet"}} diff --git a/astrobee/config/robots/bsharp2.config b/astrobee/config/robots/bsharp2.config index cda2bdee55..07f57ff99e 100644 --- a/astrobee/config/robots/bsharp2.config +++ b/astrobee/config/robots/bsharp2.config @@ -34,15 +34,15 @@ robot_geometry = { -- dock_cam_transform = transform(vec3(-0.1032-0.0029, -0.0540, -0.0064), quat4(0.54029868, -0.45297017, -0.47464308, 0.52688644)), -- imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.0043601096, -0.0066617904, 0.75077957, 0.66050535)), - hazcam_to_navcam_transform = transform(vec3(0.08017845188659993, 0.0015130070850712233, -0.0037397072731650606), quat4(-0.010795173083773568, -0.020686270289023867, 0.99972637753705862, -0.0016469718428887884)), - scicam_to_hazcam_transform = transform(vec3(0.039812511450892832, 0.017762378646844504, -0.0039477526753304197), quat4(-0.0017211102261497006, -0.023078433050076921, 0.99972939958575946, -0.0023561221189618764)), + hazcam_to_navcam_transform = transform(vec3(0.078032110432164, 0.0042875352601025, 0.014128477959323), quat4(-0.0027776443, -0.018759555, 0.9998178, -0.0021748829)), + scicam_to_hazcam_transform = transform(vec3(0.034551960811004, 0.022188043489199, -0.013680922388742), quat4(0.0017295218, -0.028156155, 0.99960204, 7.9377659e-06)), navcam_to_hazcam_timestamp_offset = -0.02, - scicam_to_hazcam_timestamp_offset = -0.31, + scicam_to_hazcam_timestamp_offset = -0.21, hazcam_depth_to_image_transform = { - 0.9672, 0.0017, 0.0111, -0.0060, - -0.0016, 0.9672, -0.0065, 0.0040, - -0.0112, 0.0065, 0.9672, -0.0133, - 0.0, 0.0, 0.0, 1.0}, + 0.94687664237117, -0.00042839578619649, -0.0009798676950181, 0.001373455007722, + 0.00042815681004901, 0.94687712204638, -0.00023113888106316, 0.00046083490888393, + 0.0009799721467424, 0.00023069565137983, 0.9468767108962, 0.00056469038710861, + 0, 0, 0, 1}, -- Engineering positions with idealized orientations perch_cam_transform = transform(vec3(-0.1331, 0.0509, -0.0166), quat4(0.000, -0.70710678118, 0.000, 0.70710678118)), -- placeholder, not valid! @@ -54,10 +54,10 @@ robot_geometry = { robot_camera_calibrations = { nav_cam = { - distortion_coeff = 0.982597, + distortion_coeff = 1.0033897, intrinsic_matrix = { - 607.592, 0.0, 589.65385, - 0.0, 607.72719, 498.22211, + 620.67509, 0.0, 580.25394, + 0.0, 620.67509, 494.45803, 0.0, 0.0, 1.0 }, gain=100, @@ -84,20 +84,20 @@ nav_cam = { exposure=150 }, haz_cam = { - distortion_coeff = {-0.266289, -0.0723862, 0.0026379, -0.0037575}, + distortion_coeff = {-0.36056524, 0.063303105, 0.0017514406, 3.3229736e-05}, intrinsic_matrix = { - 214.5571, 0.0, 110.90497, - 0.0, 213.63798, 90.388202, + 219.1313, 0.0, 110.9028, + 0.0, 219.1313, 90.52451, 0.0, 0.0, 1.0 }, gain=100, exposure=150 }, sci_cam = { - distortion_coeff = {0.0582312, -0.0757094, 0.00166962, -0.0025029}, + distortion_coeff = {0.00013126118, 0.011836873, 0.0014470664, -0.00011076796}, intrinsic_matrix = { - 1021.0608, 0.0, 661.64049, - 0.0, 1020.7, 520.01707, + 1040.0823, 0.0, 663.45304, + 0.0, 1040.0823, 523.50871, 0.0, 0.0, 1.0 }, gain=100, diff --git a/astrobee/config/tools/graph_bag.config b/astrobee/config/tools/graph_bag.config new file mode 100644 index 0000000000..f4a0b499fb --- /dev/null +++ b/astrobee/config/tools/graph_bag.config @@ -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. + +-- imu +imu_msg_delay = 0 +imu_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; diff --git a/astrobee/config/worlds/granite.config b/astrobee/config/worlds/granite.config index 76b2e55df1..08052b018d 100755 --- a/astrobee/config/worlds/granite.config +++ b/astrobee/config/worlds/granite.config @@ -46,6 +46,8 @@ world_vision_map_filename = resolve_resource("maps/granite.map") 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 -------------------------- -- PHYSICS -- diff --git a/astrobee/config/worlds/iss.config b/astrobee/config/worlds/iss.config index 368ddba01a..7000aaabc9 100755 --- a/astrobee/config/worlds/iss.config +++ b/astrobee/config/worlds/iss.config @@ -52,6 +52,8 @@ world_vision_map_filename = resolve_resource("maps/iss.map") world_vision_ar_tag_filename = "dock_markers_world.config" -- Distance of handrail from wall plane world_handrail_wall_min_gap = 0.06 +-- Huber loss function cutoff point to switch from quadratic to linear +world_huber_k = 1.345 -------------------------- -- PHYSICS -- diff --git a/astrobee/launch/astrobee.launch b/astrobee/launch/astrobee.launch index a32646da18..e00b14a02b 100644 --- a/astrobee/launch/astrobee.launch +++ b/astrobee/launch/astrobee.launch @@ -29,9 +29,8 @@ - - - + + @@ -63,9 +62,6 @@ - - - @@ -78,16 +74,14 @@ - - - - - - - - - + + + + + + + @@ -120,7 +114,7 @@ - + @@ -160,7 +154,7 @@ - + @@ -170,11 +164,4 @@ - - - > - - - - diff --git a/astrobee/launch/offline_localization/loc_only.launch b/astrobee/launch/offline_localization/loc_only.launch new file mode 100644 index 0000000000..95a468bd90 --- /dev/null +++ b/astrobee/launch/offline_localization/loc_only.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/astrobee/launch/offline_localization/loc_rviz.launch b/astrobee/launch/offline_localization/loc_rviz.launch new file mode 100644 index 0000000000..58001c1cf0 --- /dev/null +++ b/astrobee/launch/offline_localization/loc_rviz.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/astrobee/launch/offline_localization/localization_from_bag.launch b/astrobee/launch/offline_localization/localization_from_bag.launch new file mode 100644 index 0000000000..20305126c0 --- /dev/null +++ b/astrobee/launch/offline_localization/localization_from_bag.launch @@ -0,0 +1,96 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/astrobee/launch/offline_localization/record_localization.launch b/astrobee/launch/offline_localization/record_localization.launch new file mode 100644 index 0000000000..2df4d712bb --- /dev/null +++ b/astrobee/launch/offline_localization/record_localization.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/astrobee/launch/offline_localization/replay_localization.launch b/astrobee/launch/offline_localization/replay_localization.launch new file mode 100644 index 0000000000..7e9514cf78 --- /dev/null +++ b/astrobee/launch/offline_localization/replay_localization.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/astrobee/launch/replay.launch b/astrobee/launch/replay.launch deleted file mode 100644 index 0427f93167..0000000000 --- a/astrobee/launch/replay.launch +++ /dev/null @@ -1,99 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/astrobee/launch/robot/LLP.launch b/astrobee/launch/robot/LLP.launch index 62dba226fa..3ce187f857 100644 --- a/astrobee/launch/robot/LLP.launch +++ b/astrobee/launch/robot/LLP.launch @@ -22,57 +22,36 @@ - + - - + - - - - - - - - + + + + + + + - - - - - - - - - - - + - - - - - - - - - - - - - + + + @@ -80,6 +59,26 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/astrobee/launch/robot/MLP.launch b/astrobee/launch/robot/MLP.launch index f437deceea..7e5b4cd87a 100644 --- a/astrobee/launch/robot/MLP.launch +++ b/astrobee/launch/robot/MLP.launch @@ -25,38 +25,40 @@ - + + - - - + - - - - - - - - - @@ -81,340 +83,331 @@ - - + + - - - + + + - - + + + - - - + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - + + + + + + + + + + + - - + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - + + + + + + + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/astrobee/launch/sim.launch b/astrobee/launch/sim.launch index 1d8d907798..2ad5ebf0a2 100644 --- a/astrobee/launch/sim.launch +++ b/astrobee/launch/sim.launch @@ -33,6 +33,7 @@ + @@ -52,8 +53,8 @@ - + default="9.816 -9.806 4.293 0 0 0 1" /> + @@ -65,7 +66,6 @@ default="0 0 -0.7 0 0 0 1" /> - @@ -90,9 +90,6 @@ - - - @@ -111,8 +108,7 @@ - - + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + @@ -163,6 +157,7 @@ + diff --git a/astrobee/launch/spawn.launch b/astrobee/launch/spawn.launch index 94253eec17..4beaddacbc 100644 --- a/astrobee/launch/spawn.launch +++ b/astrobee/launch/spawn.launch @@ -31,6 +31,7 @@ + @@ -85,6 +86,7 @@ + diff --git a/astrobee/resources/rviz/loc_granite.rviz b/astrobee/resources/rviz/loc_granite.rviz new file mode 100644 index 0000000000..5317085a22 --- /dev/null +++ b/astrobee/resources/rviz/loc_granite.rviz @@ -0,0 +1,275 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Graph1 + - /Image1 + - /PoseDisplay1 + - /Granite1/Links1 + - /Granite1/Links1/body1 + Splitter Ratio: 0.5 + Tree Height: 178 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 + - Class: localization_rviz_plugins/GraphPanel + Name: GraphPanel +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: localization_rviz_plugins/Graph + Enabled: true + Imu Factor Arrows Diameter: 0.00999999978 + Name: Graph + Pose Axes Size: 0.100000001 + Show Imu Factor Arrows: true + Show Pose Axes: true + Topic: /graph_loc/graph + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /graph_localizer/optical_flow_feature_tracks + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: localization_rviz_plugins/PoseDisplay + Enabled: true + Name: PoseDisplay + Number of Poses: 10 + Pose Axes Size: 0.100000001 + Topic: /sparse_mapping/pose + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: false + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + carriage: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + inertial_link: + Alpha: 1 + Show Axes: false + Show Trail: false + top_aft: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + top_aft_arm_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + top_aft_arm_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + top_aft_gripper_left_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + top_aft_gripper_left_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + top_aft_gripper_right_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + top_aft_gripper_right_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: false + Name: Robot + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: Granite + Robot Description: /granite/robot_description + TF Prefix: granite + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: localization_rviz_plugins/SparseMappingDisplay + Enabled: true + Name: SparseMappingDisplay + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /sparse_mapping/map_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: rviz + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.82393694 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.106598854 + Y: 0.172128871 + Z: -0.198201209 + Focal Shape Fixed Size: false + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.445203364 + Target Frame: body + Value: Orbit (rviz) + Yaw: 3.74267197 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + GraphPanel: + collapsed: false + Height: 1041 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000020c00000372fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000000f3000000f300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000013a000001550000001600fffffffb000000140047007200610070006800500061006e0065006c01000002950000011e0000011e00ffffff000000010000010f00000372fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004100000372000000bb00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077c0000003efc0100000002fb0000000800540069006d006501000000000000077c0000027600fffffffb0000000800540069006d006501000000000000045000000000000000000000056a0000037200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1916 + X: 0 + Y: 18 diff --git a/astrobee/resources/rviz/loc_iss.rviz b/astrobee/resources/rviz/loc_iss.rviz new file mode 100644 index 0000000000..d4355a340e --- /dev/null +++ b/astrobee/resources/rviz/loc_iss.rviz @@ -0,0 +1,273 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Graph1 + - /Image1 + - /PoseDisplay1 + Splitter Ratio: 0.5 + Tree Height: 178 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 + - Class: localization_rviz_plugins/GraphPanel + Name: GraphPanel +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: localization_rviz_plugins/Graph + Enabled: true + Imu Factor Arrows Diameter: 0.00999999978 + Name: Graph + Pose Axes Size: 0.100000001 + Show Imu Factor Arrows: true + Show Pose Axes: true + Topic: /graph_loc/graph + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /graph_localizer/optical_flow_feature_tracks + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: localization_rviz_plugins/PoseDisplay + Enabled: true + Name: PoseDisplay + Number of Poses: 10 + Pose Axes Size: 0.100000001 + Topic: /sparse_mapping/pose + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + carriage: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + inertial_link: + Alpha: 1 + Show Axes: false + Show Trail: false + top_aft: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_aft_arm_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_aft_arm_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_aft_gripper_left_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_aft_gripper_left_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_aft_gripper_right_distal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_aft_gripper_right_proximal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: Robot + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: ISS + Robot Description: /iss/robot_description + TF Prefix: iss + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: localization_rviz_plugins/SparseMappingDisplay + Enabled: true + Name: SparseMappingDisplay + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /sparse_mapping/map_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: rviz + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.08937788 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.240398735 + Target Frame: body + Value: Orbit (rviz) + Yaw: 1.66722131 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + GraphPanel: + collapsed: false + Height: 1041 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000020c00000372fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000000f3000000f300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000013a000001550000001600fffffffb000000140047007200610070006800500061006e0065006c01000002950000011e0000011e00ffffff000000010000010f00000372fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004100000372000000bb00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003bc0000003efc0100000002fb0000000800540069006d00650100000000000003bc0000027600fffffffb0000000800540069006d00650100000000000004500000000000000000000001aa0000037200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 956 + X: 960 + Y: 18 diff --git a/communications/dds_msgs/idl/AstrobeeCommandConstants.idl b/communications/dds_msgs/idl/AstrobeeCommandConstants.idl index 005d92eeb9..912b3d88fa 100644 --- a/communications/dds_msgs/idl/AstrobeeCommandConstants.idl +++ b/communications/dds_msgs/idl/AstrobeeCommandConstants.idl @@ -497,6 +497,7 @@ module rapid { const rapid::String32 SETTINGS_TELEMETRY_TYPE_GNC_STATE = "GncState"; const rapid::String32 SETTINGS_TELEMETRY_TYPE_PMC_CMD_STATE = "PmcCmdState"; const rapid::String32 SETTINGS_TELEMETRY_TYPE_POSITION = "Position"; + const rapid::String32 SETTINGS_TELEMETRY_TYPE_SPARSE_MAPPING_POSE = "SparseMappingPose"; }; diff --git a/communications/dds_msgs/idl/TelemetryState.idl b/communications/dds_msgs/idl/TelemetryState.idl index 5e1856e860..b0ca30bbe6 100644 --- a/communications/dds_msgs/idl/TelemetryState.idl +++ b/communications/dds_msgs/idl/TelemetryState.idl @@ -42,6 +42,7 @@ module rapid { public float gncStateRate; public float pmcCmdStateRate; public float positionRate; + public float sparseMappingPoseRate; public CameraInfoSequence8 cameras; }; diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/dds_ros_bridge.h b/communications/dds_ros_bridge/include/dds_ros_bridge/dds_ros_bridge.h index 81e6951790..fb814c7483 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/dds_ros_bridge.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/dds_ros_bridge.h @@ -57,6 +57,7 @@ #include "dds_ros_bridge/ros_payload_state.h" #include "dds_ros_bridge/ros_plan_status_rapid_plan_status.h" #include "dds_ros_bridge/ros_pmc_cmd_state.h" +#include "dds_ros_bridge/ros_pose_rapid_position.h" #include "dds_ros_bridge/ros_string_rapid_text_message.h" #include "dds_ros_bridge/ros_sub_rapid_pub.h" #include "dds_ros_bridge/ros_telemetry_rapid_telemetry.h" @@ -92,7 +93,8 @@ enum RateType { EKF_STATE, GNC_STATE, PMC_CMD_STATE, - POSITION + POSITION, + SPARSE_MAPPING_POSE }; class RosSubRapidPub; @@ -167,6 +169,8 @@ class DdsRosBridge : public ff_util::FreeFlyerNodelet { const std::string& name); bool BuildPmcCmdStateToRapid(const std::string& sub_topic, const std::string& name); + bool BuildPoseToPosition(const std::string& sub_topic, + const std::string& name); bool BuildStringToTextMessage(const std::string& name); bool BuildTelemetryToRapid(const std::string& sub_topic, const std::string& name); @@ -197,6 +201,7 @@ class DdsRosBridge : public ff_util::FreeFlyerNodelet { bool SetEkfPositionRate(float rate, std::string &err_msg, RateType type); bool SetGncStateRate(float rate, std::string &err_msg); bool SetPmcStateRate(float rate, std::string &err_msg); + bool SetSparseMappingPoseRate(float rate, std::string &err_msg); bool SetTelemRateCallback(ff_msgs::SetRate::Request &req, ff_msgs::SetRate::Response &res); diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_position_provider_ros_pose_helper.h b/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_position_provider_ros_pose_helper.h new file mode 100644 index 0000000000..e909d41234 --- /dev/null +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/rapid_position_provider_ros_pose_helper.h @@ -0,0 +1,48 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 DDS_ROS_BRIDGE_RAPID_POSITION_PROVIDER_ROS_POSE_HELPER_H_ +#define DDS_ROS_BRIDGE_RAPID_POSITION_PROVIDER_ROS_POSE_HELPER_H_ + +#include + +#include + +#include "dds_ros_bridge/util.h" + +#include "geometry_msgs/PoseStamped.h" + +#include "rapidIo/PositionProvider.h" + +namespace rapid { + +/** + * Provide a publisher which does not convert Quaternion to Rotation Matrix + * instead publish quaternion directly + * This publisher only does one copy into the RAPID data types + */ +class PositionProviderRosPoseHelper : public PositionProvider { + public: + PositionProviderRosPoseHelper(PositionTopicPairParameters const& params, + const std::string& entity_name); + void Publish(const geometry_msgs::PoseStamped::ConstPtr& pose); +}; + +} // end namespace rapid + +#endif // DDS_ROS_BRIDGE_RAPID_POSITION_PROVIDER_ROS_POSE_HELPER_H_ diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_pose_rapid_position.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_pose_rapid_position.h new file mode 100644 index 0000000000..2eb2ac7a39 --- /dev/null +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_pose_rapid_position.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 DDS_ROS_BRIDGE_ROS_POSE_RAPID_POSITION_H_ +#define DDS_ROS_BRIDGE_ROS_POSE_RAPID_POSITION_H_ + +#include + +#include "dds_ros_bridge/ros_sub_rapid_pub.h" +#include "dds_ros_bridge/rapid_position_provider_ros_pose_helper.h" +#include "dds_ros_bridge/util.h" + +#include "geometry_msgs/PoseStamped.h" + +#include "knDds/DdsTypedSupplier.h" + +#include "rapidDds/RapidConstants.h" + +#include "rapidIo/RapidIoParameters.h" + +#include "rapidUtil/RapidHelper.h" + +namespace ff { + +class RosPoseRapidPosition : public RosSubRapidPub { + public: + RosPoseRapidPosition(const std::string& subscribe_topic, + const std::string& pub_topic, + bool use_rate, + const ros::NodeHandle& nh, + const unsigned int queue_size = 10); + + void DataCallback(geometry_msgs::PoseStampedConstPtr const& data); + void PubPosition(const ros::TimerEvent& event); + void SetPositionPublishRate(float rate); + + private: + rapid::PositionTopicPairParameters params_; + std::shared_ptr provider_; + + geometry_msgs::PoseStampedConstPtr pose_msg_; + + bool use_rate_; + + ros::Timer position_timer_; +}; + +} // end namespace ff + +#endif // DDS_ROS_BRIDGE_ROS_POSE_RAPID_POSITION_H_ diff --git a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_telemetry_rapid_telemetry.h b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_telemetry_rapid_telemetry.h index fb48484f86..09f3cf9e81 100644 --- a/communications/dds_ros_bridge/include/dds_ros_bridge/ros_telemetry_rapid_telemetry.h +++ b/communications/dds_ros_bridge/include/dds_ros_bridge/ros_telemetry_rapid_telemetry.h @@ -56,6 +56,7 @@ class RosTelemetryRapidTelemetry : public RosSubRapidPub { void SetGncStateRate(float rate); void SetPmcCmdStateRate(float rate); void SetPositionRate(float rate); + void SetSparseMappingPoseRate(float rate); protected: bool AssembleConfig(config_reader::ConfigReader& config_params); diff --git a/communications/dds_ros_bridge/src/dds_ros_bridge.cc b/communications/dds_ros_bridge/src/dds_ros_bridge.cc index 0067575136..45788de2d9 100644 --- a/communications/dds_ros_bridge/src/dds_ros_bridge.cc +++ b/communications/dds_ros_bridge/src/dds_ros_bridge.cc @@ -587,6 +587,34 @@ bool DdsRosBridge::BuildPmcCmdStateToRapid(const std::string& sub_topic, return true; } +bool DdsRosBridge::BuildPoseToPosition(const std::string& sub_topic, + const std::string& name) { + std::string pub_topic; + bool use, use_rate; + std::string use_exp = "use_rate_" + name; + + if (ReadTopicInfo(name, "pub", pub_topic, use)) { + if (use) { + if (!config_params_.GetBool(use_exp.c_str(), &use_rate)) { + ROS_FATAL("DDS Bridge: use rate %s not specified!", name.c_str()); + return false; + } + + ff::RosSubRapidPubPtr pose_to_position( + new ff::RosPoseRapidPosition(sub_topic, + pub_topic, + use_rate, + nh_)); + components_++; + ros_sub_rapid_pubs_[name] = pose_to_position; + } + } else { + return false; + } + + return true; +} + bool DdsRosBridge::BuildStringToTextMessage(const std::string& name) { std::string pub_topic, sub_topic; bool use; @@ -726,6 +754,9 @@ bool DdsRosBridge::SetTelem(float rate, std::string &err_msg, RateType type) { case POSITION: RTRT->SetPositionRate(rate); break; + case SPARSE_MAPPING_POSE: + RTRT->SetSparseMappingPoseRate(rate); + break; default: err_msg = "DDS Bridge: Unknown type when setting telemetry rate!"; return false; @@ -858,6 +889,23 @@ bool DdsRosBridge::SetPmcStateRate(float rate, std::string &err_msg) { return true; } +bool DdsRosBridge::SetSparseMappingPoseRate(float rate, std::string &err_msg) { + if (ros_sub_rapid_pubs_.count("RSMPRP") == 0) { + err_msg = "DDS Bridge: Sparse mapping pose message not add but needed."; + return false; + } + + if (!SetTelem(rate, err_msg, SPARSE_MAPPING_POSE)) { + return false; + } + + ff::RosPoseRapidPosition *RSMP = static_cast + (ros_sub_rapid_pubs_["RSMPRP"].get()); + + RSMP->SetPositionPublishRate(rate); + return true; +} + bool DdsRosBridge::SetTelemRateCallback(ff_msgs::SetRate::Request &req, ff_msgs::SetRate::Response &res) { std::string err_msg; @@ -883,6 +931,9 @@ bool DdsRosBridge::SetTelemRateCallback(ff_msgs::SetRate::Request &req, case ff_msgs::SetRate::Request::POSITION: res.success = SetEkfPositionRate(req.rate, err_msg, POSITION); break; + case ff_msgs::SetRate::Request::SPARSE_MAPPING_POSE: + res.success = SetSparseMappingPoseRate(req.rate, err_msg); + break; default: res.success = false; err_msg = "DDS Bridge: Unknown which in set telemetry rate service."; @@ -1291,6 +1342,16 @@ bool DdsRosBridge::ReadParams() { return false; } + // ros_sparse_mapping_pose_rapid_position => RSMPRP + if (!BuildPoseToPosition(TOPIC_SPARSE_MAPPING_POSE, "RSMPRP")) { + return false; + } + + // ros_vive_pose_rapid_position => RVPRP + if (!BuildPoseToPosition("loc/truth", "RVPRP")) { + return false; + } + // ros_string_rapid_text_message => RSRTM if (!BuildStringToTextMessage("RSRTM")) { return false; @@ -1334,6 +1395,7 @@ bool DdsRosBridge::ReadRateParams() { float temp_rate; std::string err_msg; + // Get comm status pub rate if (!config_params_.GetReal("comm_status_rate", &temp_rate)) { ROS_FATAL("DDS Bridge: comm state rate not specified!"); return false; @@ -1344,6 +1406,7 @@ bool DdsRosBridge::ReadRateParams() { return false; } + // Get cpu state pub rate if (!config_params_.GetReal("cpu_state_rate", &temp_rate)) { ROS_FATAL("DDS Bridge: cpu state rate not specified!"); return false; @@ -1354,6 +1417,7 @@ bool DdsRosBridge::ReadRateParams() { return false; } + // Get disk state pub rate if (!config_params_.GetReal("disk_state_rate", &temp_rate)) { ROS_FATAL("DDS Bridge: disk state rate not specified!"); return false; @@ -1364,6 +1428,7 @@ bool DdsRosBridge::ReadRateParams() { return false; } + // Get ekf state pub rate if (!config_params_.GetReal("ekf_state_rate", &temp_rate)) { ROS_FATAL("DDS Bridge: ekf state rate not specified!"); return false; @@ -1374,6 +1439,7 @@ bool DdsRosBridge::ReadRateParams() { return false; } + // Get gnc state pub rate if (!config_params_.GetReal("gnc_state_rate", &temp_rate)) { ROS_FATAL("DDS Bridge: gnc state rate not specified!"); return false; @@ -1384,6 +1450,7 @@ bool DdsRosBridge::ReadRateParams() { return false; } + // Get pmc command pub rate if (!config_params_.GetReal("pmc_command_rate", &temp_rate)) { ROS_FATAL("DDS Bridge: pmc command rate not specified!"); return false; @@ -1394,6 +1461,7 @@ bool DdsRosBridge::ReadRateParams() { return false; } + // Get position pub rate if (!config_params_.GetReal("position_rate", &temp_rate)) { ROS_FATAL("DDS Bridge: position rate not specified!"); return false; @@ -1404,6 +1472,17 @@ bool DdsRosBridge::ReadRateParams() { return false; } + // Get sparse mapping pose pub rate + if (!config_params_.GetReal("sparse_mapping_pose_rate", &temp_rate)) { + ROS_FATAL("DDS Bridge: sparse mapping pose rate not specified!"); + return false; + } + + if (!SetSparseMappingPoseRate(temp_rate, err_msg)) { + ROS_FATAL("%s", err_msg.c_str()); + return false; + } + return true; } diff --git a/communications/dds_ros_bridge/src/rapid_position_provider_ros_pose_helper.cc b/communications/dds_ros_bridge/src/rapid_position_provider_ros_pose_helper.cc new file mode 100644 index 0000000000..ad9c463ba9 --- /dev/null +++ b/communications/dds_ros_bridge/src/rapid_position_provider_ros_pose_helper.cc @@ -0,0 +1,53 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 "dds_ros_bridge/rapid_position_provider_ros_pose_helper.h" + +namespace rapid { + +PositionProviderRosPoseHelper::PositionProviderRosPoseHelper( + PositionTopicPairParameters const& params, + std::string const& entity_name) + : PositionProvider(params, entity_name) { +} + +void PositionProviderRosPoseHelper::Publish( + geometry_msgs::PoseStamped::ConstPtr const& pose) { + rapid::PositionSample& sample = m_dataSupplier.event(); + + sample.hdr.timeStamp = util::RosTime2RapidTime(pose->header.stamp); + sample.hdr.statusCode = 0; + + // pos + // double << float32 + sample.pose.xyz[0] = pose->pose.position.x; + sample.pose.xyz[1] = pose->pose.position.y; + sample.pose.xyz[2] = pose->pose.position.z; + + // orientation as rapid::RAPID_ROT_QUAT + // order specified in rapid::BaseTypesX Y Z W + // float << float3 + sample.pose.rot[0] = pose->pose.orientation.x; + sample.pose.rot[1] = pose->pose.orientation.y; + sample.pose.rot[2] = pose->pose.orientation.z; + sample.pose.rot[3] = pose->pose.orientation.w; + + m_dataSupplier.sendEvent(); +} + +} // end namespace rapid diff --git a/communications/dds_ros_bridge/src/ros_pose_rapid_position.cc b/communications/dds_ros_bridge/src/ros_pose_rapid_position.cc new file mode 100644 index 0000000000..0c58096b3d --- /dev/null +++ b/communications/dds_ros_bridge/src/ros_pose_rapid_position.cc @@ -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. + */ + +#include "dds_ros_bridge/ros_pose_rapid_position.h" + +namespace ff { + +RosPoseRapidPosition::RosPoseRapidPosition(const std::string& subscribe_topic, + const std::string& pub_topic, + bool use_rate, + const ros::NodeHandle& nh, + const unsigned int queue_size) : + RosSubRapidPub(subscribe_topic, pub_topic, nh, queue_size) { + params_.config.poseEncoding = rapid::RAPID_ROT_QUAT; + + params_.topicSuffix += pub_topic; + + // instantiate provider + provider_.reset( + new rapid::PositionProviderRosPoseHelper(params_, "RosPoseRapidPosition")); + + // start subscriber + sub_ = nh_.subscribe(subscribe_topic, + queue_size, + &RosPoseRapidPosition::DataCallback, + this); + + // Setup timers for publishing the position but don't start them until the + // rate is set. The bridge will set the rate at the end of its init if needed + position_timer_ = nh_.createTimer(ros::Rate(1.0), + &RosPoseRapidPosition::PubPosition, + this, + false, + false); + + // This is used for ground testing and in those cases, we don't care about + // limiting the rate of the messge. + use_rate_ = use_rate; +} + +void RosPoseRapidPosition::DataCallback( + geometry_msgs::PoseStampedConstPtr const& data) { + if (use_rate_) { + pose_msg_ = data; + } else { + provider_->Publish(data); + } +} + +void RosPoseRapidPosition::PubPosition(const ros::TimerEvent& event) { + // Make sure we have received a pose message before trying to send it + if (pose_msg_ != NULL) { + provider_->Publish(pose_msg_); + } +} + +void RosPoseRapidPosition::SetPositionPublishRate(float rate) { + if (rate == 0) { + position_timer_.stop(); + } else { + position_timer_.setPeriod(ros::Duration(ros::Rate(rate))); + position_timer_.start(); // Start in case it was never started + } +} +} // end namespace ff diff --git a/communications/dds_ros_bridge/src/ros_telemetry_rapid_telemetry.cc b/communications/dds_ros_bridge/src/ros_telemetry_rapid_telemetry.cc index 22362f034d..ba77b20fca 100644 --- a/communications/dds_ros_bridge/src/ros_telemetry_rapid_telemetry.cc +++ b/communications/dds_ros_bridge/src/ros_telemetry_rapid_telemetry.cc @@ -59,6 +59,7 @@ RosTelemetryRapidTelemetry::RosTelemetryRapidTelemetry( state_supplier_->event().ekfStateRate = 0; state_supplier_->event().gncStateRate = 0; state_supplier_->event().positionRate = 0; + state_supplier_->event().sparseMappingPoseRate = 0; if (AssembleConfig(config_params)) { config_supplier_->sendEvent(); @@ -165,6 +166,15 @@ void RosTelemetryRapidTelemetry::SetPositionRate(float rate) { state_supplier_->sendEvent(); } +void RosTelemetryRapidTelemetry::SetSparseMappingPoseRate(float rate) { + if (rate < 0) { + state_supplier_->event().sparseMappingPoseRate = 0; + } else { + state_supplier_->event().sparseMappingPoseRate = rate; + } + state_supplier_->sendEvent(); +} + bool RosTelemetryRapidTelemetry::AssembleConfig( config_reader::ConfigReader& config_params) { std::string temp_name, temp_resolution, temp_mode; diff --git a/communications/ff_msgs/msg/CommandConstants.msg b/communications/ff_msgs/msg/CommandConstants.msg index f970736e03..5e05585372 100644 --- a/communications/ff_msgs/msg/CommandConstants.msg +++ b/communications/ff_msgs/msg/CommandConstants.msg @@ -51,6 +51,7 @@ string PARAM_NAME_TELEMETRY_TYPE_EKF_STATE = EkfState string PARAM_NAME_TELEMETRY_TYPE_GNC_STATE = GncState string PARAM_NAME_TELEMETRY_TYPE_PMC_CMD_STATE = PmcCmdState string PARAM_NAME_TELEMETRY_TYPE_POSITION = Position +string PARAM_NAME_TELEMETRY_TYPE_SPARSE_MAPPING_POSE = SparseMappingPose string CMD_NAME_GRAB_CONTROL = grabControl string CMD_NAME_REQUEST_CONTROL = requestControl diff --git a/communications/ff_msgs/msg/GraphState.msg b/communications/ff_msgs/msg/GraphState.msg new file mode 100644 index 0000000000..782e5cb219 --- /dev/null +++ b/communications/ff_msgs/msg/GraphState.msg @@ -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. +# + +std_msgs/Header header # header with timestamp +string child_frame_id # frame ID +# State Estimates +geometry_msgs/Pose pose # world_T_body +geometry_msgs/Vector3 velocity # body velocity +geometry_msgs/Vector3 gyro_bias # estimated gyro bias +geometry_msgs/Vector3 accel_bias # estimated accel bias +# Covariances/Confidences +# covariance diagonal. 1-3 orientation, 4-6 gyro bias, 7-9 velocity, 10-12 accel bias, 13-15 position +float32[15] cov_diag +# confidence in estimate. 0 is good, 1 is a bit confused, 2 is lost +uint8 confidence +uint8 CONFIDENCE_GOOD = 0 # Tracking well +uint8 CONFIDENCE_POOR = 1 # Tracking poorly +uint8 CONFIDENCE_LOST = 2 # We are lost +# Stats +uint8 of_count # optical flow feature count +uint8 ml_count # ml feature count +uint32 iterations # Optimization iterations +float32 optimization_time +float32 update_time # Include optimization_time and other operations to add data to graph +uint32 num_factors +uint32 num_states +# Status +bool standstill +bool estimating_bias # Are we busy estimating the bias? diff --git a/communications/ff_msgs/msg/LocalizationGraph.msg b/communications/ff_msgs/msg/LocalizationGraph.msg new file mode 100644 index 0000000000..c40cb04674 --- /dev/null +++ b/communications/ff_msgs/msg/LocalizationGraph.msg @@ -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. +# +std_msgs/Header header # header with timestamp +string child_frame_id # frame ID + +string serialized_graph diff --git a/communications/ff_msgs/srv/SetRate.srv b/communications/ff_msgs/srv/SetRate.srv index 6f21a95466..39065839da 100644 --- a/communications/ff_msgs/srv/SetRate.srv +++ b/communications/ff_msgs/srv/SetRate.srv @@ -17,13 +17,14 @@ # # This message sets the rates at which certain messages are published. -uint8 COMM_STATUS = 0 # Set the communication status message rate -uint8 CPU_STATE = 1 # Set the cpu state message rate -uint8 DISK_STATE = 2 # Set the disk state message rate -uint8 EKF_STATE = 3 # Set the ekf state message rate -uint8 GNC_STATE = 4 # Set the gnc state message rate -uint8 PMC_CMD_STATE = 5 # Set the pmc command state message rate -uint8 POSITION = 6 # Set the position message rate +uint8 COMM_STATUS = 0 # Set the communication status message rate +uint8 CPU_STATE = 1 # Set the cpu state message rate +uint8 DISK_STATE = 2 # Set the disk state message rate +uint8 EKF_STATE = 3 # Set the ekf state message rate +uint8 GNC_STATE = 4 # Set the gnc state message rate +uint8 PMC_CMD_STATE = 5 # Set the pmc command state message rate +uint8 POSITION = 6 # Set the position message rate +uint8 SPARSE_MAPPING_POSE = 7 # Set the sparse mapping pose message rate uint8 which # which message we are setting the rate for float32 rate # the rate diff --git a/debian/changelog b/debian/changelog index f0cf332fe6..002c09e7c0 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,3 +1,14 @@ +astrobee (0.14.0) testing; urgency=medium + + * Added graph_localizer package + * Added supporting packages for graph localizer including imu_augmetor, imu_integation, localization_common, localization_measurements + * Added tools for graph localizer including graph_bag and imu_bias_tester + * Added rviz plugins for graph localizer and associated packages in localization_rviz_plugins + * Added gtsam debian + * Note, Perching does not work in this release, use another version for this. + + -- Astrobee Flight Software Wed, 27 Jan 2021 13:43:11 -0800 + astrobee (0.13.0) testing; urgency=medium * Multiple updates to documentation, doxygen is exported to github pages diff --git a/debian/control b/debian/control index 49fb891d02..5053260b57 100644 --- a/debian/control +++ b/debian/control @@ -16,7 +16,7 @@ Depends: astrobee0 (= ${binary:Version}), ${misc:Depends} libace-dev, libqt4-dev, protobuf-compiler, libv4l-dev, libeigen3-dev, libluajit-5.1-dev, libjsoncpp-dev, libtinyxml-dev, libyaml-cpp-dev, libusb-1.0-0-dev, - libalvar-dev, libdbow2-dev, libopenmvg-dev, libroyale-dev, libceres-dev, rti-dev, + libalvar-dev, libdbow2-dev, libgtsam-dev, libopenmvg-dev, libroyale-dev, libceres-dev, rti-dev, libsoracore-dev, libmiro-dev, libdecomputil-dev, libjps3d-dev Description: Astrobee flight software development files. The Astrobee flight software development files. @@ -32,7 +32,7 @@ Package: astrobee0 Architecture: any Depends: ${shlibs:Depends}, ${misc:Depends} astrobee-config (>= ${binary:Version}), - libalvar2 (>=2.0), libdbow21 (>=0.1), libopenmvg1 (>=1.0), libroyale1 (>=1.0), + libalvar2 (>=2.0), libdbow21 (>=0.1), libgtsam (>=4.0), libopenmvg1 (>=1.0), libroyale1 (>=1.0), libceres1 (>=1.0), rti (>=1.0), libmiro0 (>=0.1), libsoracore1 (>=1.0), libdecomputil0 (>=0.1), libjps3d0 (>=0.1), libluajit-5.1-2, diff --git a/doc/general_documentation/flight_release.md b/doc/general_documentation/flight_release.md index 56ae36cf8f..14d3c4d19a 100644 --- a/doc/general_documentation/flight_release.md +++ b/doc/general_documentation/flight_release.md @@ -75,16 +75,17 @@ fixes, recreate the debians, and repeat the release testing procedure. ## Finish Release Once the debians pass all the required testing, it is ready to become an -official flight release. Please make a pull request on astrobee develop and set -Brian, Katie, or Marina as the reviewer. They will review the request and merge -it into develop and master. You will also need to copy the debians to a -temporary location on volar. +official flight release. Please make two pull requests; one on astrobee develop +and one on astrobee master. Set Brian, Katie, or Marina as the reviewer. They +will review the request and do the merging into develop and master. You will +also need to copy the debians to a temporary location on volar. scp astrobee0_0.x.x_armhf.deb \ - @volar:/home/p-free-flyer/free-flyer/FSW/ars_debs/dists/xenial/main/release_candidate/ + @volar:/home/p-free-flyer/free-flyer/FSW/ars_debs/release_candidate/ scp astrobee-config_0.x.x_all.deb \ - @volar:/home/p-free-flyer/free-flyer/FSW/ars_debs/dists/xenial/main/release_candidate/ - + @volar:/home/p-free-flyer/free-flyer/FSW/ars_debs/release_candidate/ + scp astrobee-dev_0.x.x_armhf.deb \ + @volar:/home/p-free-flyer/free-flyer/FSW/ars_debs/release_candidate/ After the debians have been copied to volar, make sure the group permissions are set to read and right. Finally, please email Ruben Garcia Ruiz so that he can diff --git a/gnc/ctl/CMakeLists.txt b/gnc/ctl/CMakeLists.txt index e01a6a06f2..cd8dca1eff 100644 --- a/gnc/ctl/CMakeLists.txt +++ b/gnc/ctl/CMakeLists.txt @@ -28,7 +28,7 @@ create_library(TARGET ctl INC ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} DEPS ff_msgs ff_hw_msgs) -if(CATKIN_ENABLE_TESTING) +if(CATKIN_ENABLE_TESTING AND ENABLE_INTEGRATION_TESTING) find_package(rostest REQUIRED) add_rostest_gtest(test_ctl test/test_ctl.test test/test_ctl_nominal.cc diff --git a/gnc/ctl/test/test_ctl.test b/gnc/ctl/test/test_ctl.test index b5383985ef..d7391d150c 100644 --- a/gnc/ctl/test/test_ctl.test +++ b/gnc/ctl/test/test_ctl.test @@ -19,5 +19,6 @@ + diff --git a/gnc/ekf/include/ekf/ekf.h b/gnc/ekf/include/ekf/ekf.h index aae175a0f7..6283d66db0 100644 --- a/gnc/ekf/include/ekf/ekf.h +++ b/gnc/ekf/include/ekf/ekf.h @@ -99,6 +99,8 @@ class Ekf { Eigen::Affine3d GetNavCamToBody(void) const {return nav_cam_to_body_;} + bool HasPoseEstimate() const { return has_pose_estimate_; } + protected: /** Functions **/ /** @@ -159,7 +161,7 @@ class Ekf { // only save this for writing to a file later geometry_msgs::Pose last_estimate_pose_; - + bool has_pose_estimate_ = false; /** Configuration Constants **/ diff --git a/gnc/ekf/src/ekf.cc b/gnc/ekf/src/ekf.cc index 8fb8ca9db7..dcdb9efb4e 100644 --- a/gnc/ekf/src/ekf.cc +++ b/gnc/ekf/src/ekf.cc @@ -332,7 +332,9 @@ void Ekf::VisualLandmarksUpdate(const ff_msgs::VisualLandmarks & vl) { } if (vl.landmarks.size() < 5) return; + last_estimate_pose_= vl.pose; + has_pose_estimate_ = true; // find better way to choose limited landmarks to send? for (int i = 0; i < std::min(ml_max_features_, static_cast(vl.landmarks.size())); i++) { vis_.cvs_landmarks[i] = vl.landmarks[i].x; diff --git a/hardware/smart_dock/include/smart_dock/smart_dock.h b/hardware/smart_dock/include/smart_dock/smart_dock.h index 114becf1a9..d2a8713899 100644 --- a/hardware/smart_dock/include/smart_dock/smart_dock.h +++ b/hardware/smart_dock/include/smart_dock/smart_dock.h @@ -155,6 +155,10 @@ class SmartDock { FAULT_OT_CHARGER, FAULT_OT_ACTUATOR_1, FAULT_OT_ACTUATOR_2, + FAULT_WDT1_EPS_REBOOT, + FAULT_WDT2_DOCKCTL_REBOOT, + FAULT_WDT3_DOCKPC_REBOOT, + FAULT_WDT4_DOCKCTL_REBOOT, NUM_FAULTS }; diff --git a/hardware/smart_dock/tools/smart_dock_tool.cc b/hardware/smart_dock/tools/smart_dock_tool.cc index b9b544c25d..f45c77a3b6 100644 --- a/hardware/smart_dock/tools/smart_dock_tool.cc +++ b/hardware/smart_dock/tools/smart_dock_tool.cc @@ -814,6 +814,10 @@ int main(int argc, char** argv) { idxs[SD::FAULT_OT_CHARGER] = Value("Charger o/temp", {"t_chg"}); idxs[SD::FAULT_OT_ACTUATOR_1] = Value("Actuator 1 o/temp", {"t_act1"}); idxs[SD::FAULT_OT_ACTUATOR_2] = Value("Actuator 2 o/temp", {"t_act2"}); + idxs[SD::FAULT_WDT1_EPS_REBOOT] = Value("WatchDog Timer 1 has rebooted EPS", {"wdt1_eps"}); + idxs[SD::FAULT_WDT2_DOCKCTL_REBOOT] = Value("WatchDog Timer 2 has rebooted Dock Controller", {"wdt2_dctl"}); + idxs[SD::FAULT_WDT3_DOCKPC_REBOOT] = Value("WatchDog Timer 3 has rebooted Dock Processor", {"wdt3_dpc"}); + idxs[SD::FAULT_WDT4_DOCKCTL_REBOOT] = Value("WatchDog Timer 4 has rebooted Dock Controller", {"wdt4_dctl"}); if (FLAGS_list) return Print("Indexes", idxs, true); if (FLAGS_clear) { if (!sd.ClearFaults()) return Error("Could not clear the faults."); diff --git a/localization/CMakeLists.txt b/localization/CMakeLists.txt index 27a2c24a75..2c6b79981b 100644 --- a/localization/CMakeLists.txt +++ b/localization/CMakeLists.txt @@ -16,10 +16,16 @@ # under the License. add_subdirectory(camera) +add_subdirectory(imu_integration) add_subdirectory(interest_point) +add_subdirectory(localization_common) +add_subdirectory(localization_measurements) add_subdirectory(sparse_mapping) if (USE_ROS) + add_subdirectory(graph_localizer) + add_subdirectory(ground_truth_localizer) + add_subdirectory(imu_augmentor) add_subdirectory(marker_tracking) add_subdirectory(lk_optical_flow) add_subdirectory(handrail_detect) diff --git a/localization/graph_localizer/CMakeLists.txt b/localization/graph_localizer/CMakeLists.txt new file mode 100644 index 0000000000..15eebabd31 --- /dev/null +++ b/localization/graph_localizer/CMakeLists.txt @@ -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. + +project(graph_localizer) + +if (USE_ROS) + +# include ff_nodelet to get ff_util header files since these aren't exposed elsewhere +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} camera config_reader ff_nodelet imu_integration localization_common localization_measurements msg_conversions + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} + CATKIN_DEPENDS roscpp + DEPENDS gtsam ff_msgs +) + +create_library(TARGET ${PROJECT_NAME} + LIBS ${catkin_LIBRARIES} ${GLOG_LIBRARIES} gtsam camera config_reader ff_nodelet imu_integration localization_common localization_measurements msg_conversions + INC ${catkin_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} + DEPS ff_msgs +) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + 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 gtsam + ) + add_rostest_gtest(test_rotation_factor + test/test_rotation_factor.test + test/test_rotation_factor.cc + ) + target_link_libraries(test_rotation_factor + graph_localizer gtsam + ) + +endif() + +install_launch_files() + +endif (USE_ROS) diff --git a/localization/graph_localizer/include/graph_localizer/calibration_params.h b/localization/graph_localizer/include/graph_localizer/calibration_params.h new file mode 100644 index 0000000000..8adb9d44b3 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/calibration_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 GRAPH_LOCALIZER_CALIBRATION_PARAMS_H_ +#define GRAPH_LOCALIZER_CALIBRATION_PARAMS_H_ + +#include +#include + +namespace graph_localizer { +struct CalibrationParams { + gtsam::Pose3 body_T_dock_cam; + gtsam::Pose3 body_T_nav_cam; + gtsam::Pose3 world_T_dock; + boost::shared_ptr nav_cam_intrinsics; + boost::shared_ptr dock_cam_intrinsics; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_CALIBRATION_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/cumulative_factor_adder.h b/localization/graph_localizer/include/graph_localizer/cumulative_factor_adder.h new file mode 100644 index 0000000000..2a594eb1ca --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/cumulative_factor_adder.h @@ -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. + */ + +#ifndef GRAPH_LOCALIZER_CUMULATIVE_FACTOR_ADDER_H_ +#define GRAPH_LOCALIZER_CUMULATIVE_FACTOR_ADDER_H_ + +#include + +#include + +namespace graph_localizer { +template +class CumulativeFactorAdder { + public: + explicit CumulativeFactorAdder(const PARAMS& params) : params_(params) {} + + virtual ~CumulativeFactorAdder() {} + + virtual std::vector AddFactors() = 0; + + const PARAMS& params() const { return params_; } + + private: + PARAMS params_; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_CUMULATIVE_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/factor_adder.h b/localization/graph_localizer/include/graph_localizer/factor_adder.h new file mode 100644 index 0000000000..b8e8312ae8 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/factor_adder.h @@ -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. + */ + +#ifndef GRAPH_LOCALIZER_FACTOR_ADDER_H_ +#define GRAPH_LOCALIZER_FACTOR_ADDER_H_ + +#include + +#include + +namespace graph_localizer { +template +class FactorAdder { + public: + explicit FactorAdder(const PARAMS& params) : params_(params) {} + + virtual ~FactorAdder() {} + + virtual std::vector AddFactors(const MEASUREMENT& measurement) = 0; + + const PARAMS& params() const { return params_; } + + private: + PARAMS params_; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/factor_adder_params.h b/localization/graph_localizer/include/graph_localizer/factor_adder_params.h new file mode 100644 index 0000000000..92f4845406 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/factor_adder_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 GRAPH_LOCALIZER_FACTOR_ADDER_PARAMS_H_ +#define GRAPH_LOCALIZER_FACTOR_ADDER_PARAMS_H_ + +namespace graph_localizer { +struct FactorAdderParams { + bool enabled; + double huber_k; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/factor_params.h b/localization/graph_localizer/include/graph_localizer/factor_params.h new file mode 100644 index 0000000000..cdbbaffff1 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/factor_params.h @@ -0,0 +1,38 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include +#include + +namespace graph_localizer { +struct FactorParams { + 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/factor_to_add.h b/localization/graph_localizer/include/graph_localizer/factor_to_add.h new file mode 100644 index 0000000000..0c5547a339 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/factor_to_add.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 GRAPH_LOCALIZER_FACTOR_TO_ADD_H_ +#define GRAPH_LOCALIZER_FACTOR_TO_ADD_H_ + +#include +#include +#include + +#include + +#include + +namespace graph_localizer { +struct FactorToAdd { + FactorToAdd(const KeyInfos& key_infos, boost::shared_ptr factor) + : factor(factor), key_infos(key_infos) {} + + boost::shared_ptr factor; + KeyInfos key_infos; +}; + +class FactorsToAdd { + public: + FactorsToAdd(const localization_common::Time timestamp, const std::vector& factors_to_add, + const GraphAction graph_action = GraphAction::kNone) + : timestamp_(timestamp), factors_to_add_(factors_to_add), graph_action_(graph_action) {} + + explicit FactorsToAdd(const GraphAction graph_action = GraphAction::kNone) : graph_action_(graph_action) {} + + 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& Get() const { return factors_to_add_; } + std::vector& Get() { return factors_to_add_; } + GraphAction graph_action() const { return graph_action_; } + + private: + // Timestamp used to sort factors when adding to graph. + localization_common::Time timestamp_; + std::vector factors_to_add_; + GraphAction graph_action_; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_FACTOR_TO_ADD_H_ diff --git a/localization/graph_localizer/include/graph_localizer/feature_counts.h b/localization/graph_localizer/include/graph_localizer/feature_counts.h new file mode 100644 index 0000000000..3943dc1da4 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/feature_counts.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 GRAPH_LOCALIZER_FEATURE_COUNTS_H_ +#define GRAPH_LOCALIZER_FEATURE_COUNTS_H_ + +namespace graph_localizer { + +struct FeatureCounts { + void Reset() { + vl = 0; + of = 0; + } + + int vl = 0; + int of = 0; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_FEATURE_COUNTS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/feature_track.h b/localization/graph_localizer/include/graph_localizer/feature_track.h new file mode 100644 index 0000000000..39441aad52 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/feature_track.h @@ -0,0 +1,48 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 + +#include +#include + +namespace graph_localizer { + +struct FeatureTrack { + localization_measurements::FeatureId id; + std::deque points; + localization_measurements::ImageId latest_image_id; + + private: + // Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(id); + ar& BOOST_SERIALIZATION_NVP(latest_image_id); + ar& BOOST_SERIALIZATION_NVP(points); + } +}; + +using FeatureTracks = std::vector; +} // 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 new file mode 100644 index 0000000000..606aa31537 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/feature_tracker.h @@ -0,0 +1,61 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include + +#include + +#include +#include + +namespace graph_localizer { +using FeatureTrackMap = std::map; +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 FeatureTrackMap& feature_tracks() const { return feature_tracks_; } + void RemovePointsOutsideWindow(); + void RemoveOldFeaturePoints(localization_common::Time oldest_allowed_time); + boost::optional OldestTimestamp() const; + boost::optional latest_timestamp() const; + boost::optional PreviousTimestamp() const; + + private: + // Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(feature_tracks_); + } + + FeatureTrackMap feature_tracks_; + FeatureTrackerParams params_; + boost::optional latest_time_; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_FEATURE_TRACKER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/feature_tracker_params.h b/localization/graph_localizer/include/graph_localizer/feature_tracker_params.h new file mode 100644 index 0000000000..e0132786a0 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/feature_tracker_params.h @@ -0,0 +1,28 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_PARAMS_H_ +#define GRAPH_LOCALIZER_FEATURE_TRACKER_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; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_FEATURE_TRACKER_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_action.h b/localization/graph_localizer/include/graph_localizer/graph_action.h new file mode 100644 index 0000000000..3b586063b3 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/graph_action.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 GRAPH_LOCALIZER_GRAPH_ACTION_H_ +#define GRAPH_LOCALIZER_GRAPH_ACTION_H_ + +namespace graph_localizer { +enum class GraphAction { + kNone, + kDeleteExistingSmartFactors, + kLocProjectionNoiseScaling, + kARTagProjectionNoiseScaling, + kTriangulateNewPoint +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_GRAPH_ACTION_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_initializer_params.h b/localization/graph_localizer/include/graph_localizer/graph_initializer_params.h new file mode 100644 index 0000000000..08c33ee016 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/graph_initializer_params.h @@ -0,0 +1,37 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_INITIALIZER_PARAMS_H_ +#define GRAPH_LOCALIZER_GRAPH_INITIALIZER_PARAMS_H_ + +#include +#include + +#include + +#include + +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; + std::string imu_bias_filename; + int num_bias_estimation_measurements; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_GRAPH_INITIALIZER_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer.h b/localization/graph_localizer/include/graph_localizer/graph_localizer.h new file mode 100644 index 0000000000..b54c28f455 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer.h @@ -0,0 +1,254 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_H_ +#define GRAPH_LOCALIZER_GRAPH_LOCALIZER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace graph_localizer { +namespace sym = gtsam::symbol_shorthand; +using Calibration = gtsam::Cal3_S2; +using Camera = gtsam::PinholePose; +using RobustSmartFactor = gtsam::RobustSmartProjectionPoseFactor; +using SharedRobustSmartFactor = boost::shared_ptr; +using ProjectionFactor = gtsam::GenericProjectionFactor; + +class GraphLocalizer { + public: + explicit GraphLocalizer(const GraphLocalizerParams& params); + // For Serialization Only + GraphLocalizer() {} + ~GraphLocalizer(); + void AddImuMeasurement(const localization_measurements::ImuMeasurement& imu_measurement); + boost::optional LatestCombinedNavState() const; + boost::optional GetCombinedNavState( + const localization_common::Time time) const; + boost::optional> + LatestCombinedNavStateAndCovariances() const; + bool AddOpticalFlowMeasurement( + const localization_measurements::FeaturePointsMeasurement& optical_flow_feature_points_measurement); + bool TriangulateNewPoint(FactorsToAdd& factors_to_add); + bool LocProjectionNoiseScaling(FactorsToAdd& factors_to_add); + bool ARProjectionNoiseScaling(FactorsToAdd& factors_to_add); + bool MapProjectionNoiseScaling(const LocFactorAdderParams& params, FactorsToAdd& factors_to_add); + void CheckForStandstill(); + void AddARTagMeasurement( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement); + void AddSparseMappingMeasurement( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement); + // Attempts to remove most recent or oldest measurements to make and invalid smart factor valid + // TODO(rsoussan): Move this to SmartProjectionFactorAdder or utilities! + void SplitSmartFactorsIfNeeded(FactorsToAdd& factors_to_add); + + bool Update(); + const FeatureTrackMap& feature_tracks() const { return feature_tracker_->feature_tracks(); } + + boost::optional> LatestBiases() const; + + boost::optional LatestExtrapolatedPoseTime() const; + + int NumFeatures() const; + + int NumOFFactors(const bool check_valid = true) const; + + int NumSmartFactors(const bool check_valid = true) const; + + int NumProjectionFactors(const bool check_valid = true) const; + + const GraphValues& graph_values() const; + + const gtsam::NonlinearFactorGraph& factor_graph() const; + + void SaveGraphDotFile(const std::string& output_path = "graph.dot") const; + + bool standstill() const; + + const GraphLocalizerParams& params() const; + + template + int NumFactors() const { + int num_factors = 0; + for (const auto& factor : graph_) { + if (dynamic_cast(factor.get())) { + ++num_factors; + } + } + return num_factors; + } + + void LogOnDestruction(const bool log_on_destruction); + + const GraphStats& graph_stats() const; + + private: + gtsam::NonlinearFactorGraph MarginalFactors(const gtsam::NonlinearFactorGraph& old_factors, + const gtsam::KeyVector& old_keys, + const gtsam::GaussianFactorGraph::Eliminate& eliminate_function) const; + + // 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& marginals, + const localization_common::Time last_latest_time); + + void UpdatePointPriors(const gtsam::Marginals& marginals); + + void RemovePriors(const int key_index); + + // Integrates latest imu measurements up to timestamp and adds imu factor and + // new combined nav state + bool CreateAndAddLatestImuFactorAndCombinedNavState(const localization_common::Time timestamp); + + bool AddOrSplitImuFactorIfNeeded(const localization_common::Time timestamp); + + bool SplitOldImuFactorAndAddCombinedNavState(const localization_common::Time timestamp); + + void AddStartingPriors(const localization_common::CombinedNavState& global_N_body_start, const int key_index, + gtsam::NonlinearFactorGraph& graph); + + void AddPriors(const localization_common::CombinedNavState& global_N_body, + const localization_common::CombinedNavStateNoise& noise, const int key_index, + gtsam::NonlinearFactorGraph& graph); + + boost::optional> + LatestCombinedNavStateAndCovariances(const gtsam::Marginals& marginals) const; + + bool CreateAndAddImuFactorAndPredictedCombinedNavState(const localization_common::CombinedNavState& global_N_body, + const gtsam::PreintegratedCombinedMeasurements& pim); + + void BufferFactors(const std::vector& factors_to_add_vec); + + void BufferCumulativeFactors(); + + void RemoveOldMeasurementsFromCumulativeFactors(const gtsam::KeyVector& old_keys); + + int AddBufferedFactors(); + + bool DoGraphAction(FactorsToAdd& factors_to_add); + + bool Rekey(FactorToAdd& factor_to_add); + + bool ReadyToAddMeasurement(const localization_common::Time timestamp) const; + + bool MeasurementRecentEnough(const localization_common::Time timestamp) const; + + void RemoveOldBufferedFactors(const localization_common::Time oldest_allowed_timestamp); + + std::vector TimestampsToAdd(const localization_common::Time timestamp, + const localization_common::Time last_added_timestamp); + + template + void DeleteFactors() { + int num_removed_factors = 0; + for (auto factor_it = graph_.begin(); factor_it != graph_.end();) { + if (dynamic_cast(factor_it->get())) { + factor_it = graph_.erase(factor_it); + ++num_removed_factors; + continue; + } + ++factor_it; + } + LogDebug("DeleteFactors: Num removed factors: " << num_removed_factors); + } + + // TODO(rsoussan): make a static and dynamic key index? + static int GenerateKeyIndex() { + static int key_index = 0; + return key_index++; + } + + void PrintFactorDebugInfo() const; + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int file_version) { + ar& BOOST_SERIALIZATION_NVP(feature_tracker_); + ar& BOOST_SERIALIZATION_NVP(graph_); + ar& BOOST_SERIALIZATION_NVP(graph_values_); + } + + std::shared_ptr feature_tracker_; + imu_integration::LatestImuIntegrator latest_imu_integrator_; + std::shared_ptr graph_values_; + bool log_on_destruction_; + GraphLocalizerParams params_; + gtsam::LevenbergMarquardtParams levenberg_marquardt_params_; + gtsam::TriangulationParameters projection_triangulation_params_; + gtsam::SmartProjectionParams smart_projection_params_; + gtsam::NonlinearFactorGraph graph_; + boost::optional marginals_; + boost::optional last_optical_flow_measurement_; + std::multimap buffered_factors_to_add_; + + // Factor Adders + std::unique_ptr ar_tag_loc_factor_adder_; + std::unique_ptr loc_factor_adder_; + std::unique_ptr projection_factor_adder_; + std::unique_ptr rotation_factor_adder_; + std::unique_ptr smart_projection_cumulative_factor_adder_; + std::unique_ptr standstill_factor_adder_; + + gtsam::Marginals::Factorization marginals_factorization_; + boost::optional standstill_; + boost::optional last_latest_time_; + GraphStats graph_stats_; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_GRAPH_LOCALIZER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer_initializer.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_initializer.h new file mode 100644 index 0000000000..651164a7c3 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer_initializer.h @@ -0,0 +1,72 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace graph_localizer { +class GraphLocalizerInitializer { + public: + void SetBiases(const gtsam::imuBias::ConstantBias& imu_bias, const bool loaded_from_previous_estimate = false, + const bool save_to_file = false); + void SetStartPose(const gtsam::Pose3& global_T_body_start, const double timestamp); + 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 EstimateBiases() const; + const GraphLocalizerParams& params() const; + void LoadGraphLocalizerParams(config_reader::ConfigReader& config); + bool RemovedGravityFromBiasIfNecessary() const; + void EstimateAndSetImuBiases(const localization_measurements::ImuMeasurement& imu_measurement); + + 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_ = false; + bool has_start_pose_ = false; + bool has_params_ = false; + bool estimate_biases_ = false; + bool removed_gravity_from_bias_if_necessary_ = false; + graph_localizer::GraphLocalizerParams params_; + std::unique_ptr imu_bias_filter_; + std::vector 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 new file mode 100644 index 0000000000..a1ba6e1874 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet.h @@ -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. + */ +#ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_NODELET_H_ +#define GRAPH_LOCALIZER_GRAPH_LOCALIZER_NODELET_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +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 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 PublishWorldTDockTF(); + + 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 ImuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg); + + 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_; + ros::Publisher state_pub_, graph_pub_, ar_tag_pose_pub_, sparse_mapping_pose_pub_, reset_pub_, heartbeat_pub_; + ros::ServiceServer reset_srv_, bias_srv_, bias_from_file_srv_, input_mode_srv_; + tf2_ros::TransformBroadcaster transform_pub_; + std::string platform_name_; + ff_msgs::Heartbeat heartbeat_; + int ar_min_num_landmarks_; + int sparse_mapping_min_num_landmarks_; + int last_mode_ = -1; + + // 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 imu_timer_ = localization_common::RosTimer("Imu msg"); + localization_common::Timer callbacks_timer_ = localization_common::Timer("Callbacks"); +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_GRAPH_LOCALIZER_NODELET_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 new file mode 100644 index 0000000000..0ebe32f2dd --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer_params.h @@ -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. + */ +#ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_PARAMS_H_ +#define GRAPH_LOCALIZER_GRAPH_LOCALIZER_PARAMS_H_ + +#include +#include +#include +#include +#include +#include + +#include + +namespace graph_localizer { +struct GraphLocalizerParams { + CalibrationParams calibration; + FactorParams factor; + FeatureTrackerParams feature_tracker; + GraphValuesParams graph_values; + NoiseParams noise; + GraphInitializerParams graph_initializer; + bool verbose; + bool fatal_failures; + bool print_factor_info; + bool use_ceres_params; + int max_iterations; + std::string marginals_factorization; + bool limit_imu_factor_spacing; + double max_imu_factor_spacing; + bool add_priors; + bool add_marginal_factors; + double max_standstill_feature_track_avg_distance_from_mean; + int standstill_min_num_points_per_track; + double huber_k; + int log_rate; + bool estimate_world_T_dock_using_loc; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_GRAPH_LOCALIZER_PARAMS_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 new file mode 100644 index 0000000000..8a1ee5f238 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer_wrapper.h @@ -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. + */ +#ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_WRAPPER_H_ +#define GRAPH_LOCALIZER_GRAPH_LOCALIZER_WRAPPER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +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 LatestSparseMappingPoseMsg() const; + + boost::optional LatestARTagPoseMsg() const; + + boost::optional LatestCombinedNavState() const; + + boost::optional LatestLocalizationStateMsg(); + + boost::optional 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 ImuCallback(const sensor_msgs::Imu& imu_msg); + + boost::optional feature_tracks() const; + + void MarkWorldTDockForResettingIfNecessary(); + + void ResetWorldTDockUsingLoc(const ff_msgs::VisualLandmarks& visual_landmarks_msg); + + gtsam::Pose3 estimated_world_T_dock() const; + + void SaveLocalizationGraphDotFile() const; + + boost::optional graph_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 graph_localizer_; + // TODO(rsoussan): Make graph localizer wrapper params + bool publish_localization_graph_; + bool save_localization_graph_dot_file_; + boost::optional> latest_biases_; + GraphLocalizerInitializer graph_localizer_initializer_; + FeatureCounts feature_counts_; + boost::optional> sparse_mapping_pose_; + boost::optional> ar_tag_pose_; + std::unique_ptr sanity_checker_; + double position_cov_log_det_lost_threshold_; + double orientation_cov_log_det_lost_threshold_; + gtsam::Pose3 estimated_world_T_dock_; + bool reset_world_T_dock_; + bool estimate_world_T_dock_using_loc_; + int ar_min_num_landmarks_; + int sparse_mapping_min_num_landmarks_; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_GRAPH_LOCALIZER_WRAPPER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_stats.h b/localization/graph_localizer/include/graph_localizer/graph_stats.h new file mode 100644 index 0000000000..b668ffae58 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/graph_stats.h @@ -0,0 +1,100 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_STATS_H_ +#define GRAPH_LOCALIZER_GRAPH_STATS_H_ + +#include +#include + +#include + +namespace graph_localizer { +// Forward declaration since GraphLocalizer will have a GraphStats object +class GraphLocalizer; + +class GraphStats { + public: + GraphStats(); + void UpdateErrors(const GraphLocalizer& graph); + void UpdateStats(const GraphLocalizer& graph); + void Log() const; + void LogToFile(std::ofstream& ofstream) const; + void LogToCsv(std::ofstream& ofstream) const; + + std::vector> timers_; + std::vector> stats_averagers_; + std::vector> 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"); + localization_common::Averager num_states_averager_ = localization_common::Averager("Num States"); + localization_common::Averager duration_averager_ = localization_common::Averager("Duration"); + 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"); + 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"); + // Factor Error Averagers + localization_common::Averager total_error_averager_ = localization_common::Averager("Total 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: + template + void LogToFile(const std::vector>& loggers, std::ofstream& ofstream) const { + for (const auto& logger : loggers) logger.get().LogToFile(ofstream); + } + + template + void LogToCsv(const std::vector>& loggers, std::ofstream& ofstream) const { + for (const auto& logger : loggers) logger.get().LogToCsv(ofstream); + } + + template + void Log(const std::vector>& loggers) const { + for (const auto& logger : loggers) logger.get().Log(); + } +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_GRAPH_STATS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_values.h b/localization/graph_localizer/include/graph_localizer/graph_values.h new file mode 100644 index 0000000000..7bc94ec2d2 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/graph_values.h @@ -0,0 +1,169 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_VALUES_H_ +#define GRAPH_LOCALIZER_GRAPH_VALUES_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace graph_localizer { +namespace sym = gtsam::symbol_shorthand; +class GraphValues { + public: + explicit GraphValues(const GraphValuesParams& params = GraphValuesParams()); + + // 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 LatestCombinedNavState() const; + + boost::optional OldestCombinedNavState() const; + + boost::optional OldestCombinedNavStateKeyIndex() const; + + boost::optional LatestCombinedNavStateKeyIndex() const; + + boost::optional> LatestBias() const; + + // Returns the oldest time that will be in graph values once the window is slid using params + boost::optional SlideWindowNewOldestTime() const; + + boost::optional KeyIndex(const localization_common::Time timestamp) const; + + void UpdateValues(const gtsam::Values& new_values); + + // TODO(rsoussan): Put this somewhere else? + static gtsam::NonlinearFactorGraph RemoveOldFactors(const gtsam::KeyVector& old_keys, + gtsam::NonlinearFactorGraph& graph); + + gtsam::KeyVector OldFeatureKeys(const gtsam::NonlinearFactorGraph& factors) const; + + void RemoveOldFeatures(const gtsam::KeyVector& old_feature_keys); + + int RemoveOldCombinedNavStates(const localization_common::Time oldest_allowed_time); + + gtsam::KeyVector OldKeys(const localization_common::Time oldest_allowed_time) const; + + const gtsam::Values& values() const { return values_; } + + boost::optional PoseKey(const localization_common::Time timestamp) const; + + boost::optional GetKey(KeyCreatorFunction key_creator_function, + const localization_common::Time timestamp) const; + + boost::optional OldestTimestamp() const; + + boost::optional LatestTimestamp() const; + + boost::optional ClosestPoseTimestamp(const localization_common::Time timestamp) const; + + // Assumes timestamp is within bounds of graph values timestamps. + std::pair, boost::optional> + LowerAndUpperBoundTimestamp(const localization_common::Time timestamp) const; + + boost::optional LowerBoundOrEqualCombinedNavState( + const localization_common::Time timestamp) const; + + bool HasKey(const localization_common::Time timestamp) const; + + template + 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 GetCombinedNavState( + const localization_common::Time timestamp) const; + + double Duration() const; + + int NumStates() const; + + template + boost::optional at(const gtsam::Key& key) const { + if (!values_.exists(key)) { + LogError("at: Key not present in values."); + return boost::none; + } + + return values_.at(key); + } + + boost::optional Timestamp(const int key_index) const; + + bool HasFeature(const localization_measurements::FeatureId id) const; + + boost::optional 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; + + bool AddFeature(const localization_measurements::FeatureId id, const gtsam::Point3& feature_point, + const gtsam::Key& key); + + gtsam::KeyVector FeatureKeys() const; + + int NumFeatures() const; + + private: + // Removes keys from timestamp_key_index_map, values from values + bool RemoveCombinedNavState(const localization_common::Time timestamp); + + bool Empty() const; + + boost::optional LowerBoundOrEqualTimestamp( + const localization_common::Time timestamp) const; + + // Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(values_); + ar& BOOST_SERIALIZATION_NVP(timestamp_key_index_map_); + ar& BOOST_SERIALIZATION_NVP(feature_id_key_map_); + ar& BOOST_SERIALIZATION_NVP(feature_key_index_); + } + + GraphValuesParams params_; + gtsam::Values values_; + std::map timestamp_key_index_map_; + std::unordered_map 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_GRAPH_VALUES_H_ diff --git a/localization/graph_localizer/include/graph_localizer/graph_values_params.h b/localization/graph_localizer/include/graph_localizer/graph_values_params.h new file mode 100644 index 0000000000..62d82c984a --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/graph_values_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 GRAPH_LOCALIZER_GRAPH_VALUES_PARAMS_H_ +#define GRAPH_LOCALIZER_GRAPH_VALUES_PARAMS_H_ + +#include + +namespace graph_localizer { +struct GraphValuesParams { + // Only kept if there are at least min_num_states and not more than max_num_states + double ideal_duration; + int min_num_states; + int max_num_states; + // If storing 3d features as states (i.e. bundle adjustment type problems) + int min_num_factors_per_feature; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_GRAPH_VALUES_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/key_info.h b/localization/graph_localizer/include/graph_localizer/key_info.h new file mode 100644 index 0000000000..a341e9b86f --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/key_info.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_LOCALIZER_KEY_INFO_H_ +#define GRAPH_LOCALIZER_KEY_INFO_H_ + +#include + +#include + +#include +#include + +namespace graph_localizer { +using KeyCreatorFunction = std::function; +class KeyInfo { + public: + KeyInfo(KeyCreatorFunction key_creator_function, const localization_common::Time timestamp) + : key_creator_function_(key_creator_function), timestamp_(timestamp), static_(false) {} + // Use for static keys + // TODO(rsoussan): Clean up this interface, use inheritance instead? + KeyInfo(KeyCreatorFunction key_creator_function, const int id) + : key_creator_function_(key_creator_function), 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_; } + + private: + KeyCreatorFunction key_creator_function_; + localization_common::Time timestamp_; + int id_; + // Static (not changing with time) key + bool static_; +}; + +using KeyInfos = std::vector; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_KEY_INFO_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 new file mode 100644 index 0000000000..1e10952124 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/loc_factor_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 GRAPH_LOCALIZER_LOC_FACTOR_ADDER_H_ +#define GRAPH_LOCALIZER_LOC_FACTOR_ADDER_H_ + +#include +#include +#include +#include +#include + +#include + +namespace graph_localizer { +class LocFactorAdder + : public FactorAdder { + using Base = FactorAdder; + + public: + LocFactorAdder(const LocFactorAdderParams& params, const GraphAction projection_graph_action); + + std::vector AddFactors( + const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement) final; + + private: + localization_common::Averager num_landmarks_averager_ = localization_common::Averager("Num Landmarks"); + GraphAction projection_graph_action_; +}; +} // 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 new file mode 100644 index 0000000000..a299d58c01 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/loc_factor_adder_params.h @@ -0,0 +1,48 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 + +#include +#include +#include + +namespace graph_localizer { +struct LocFactorAdderParams : public FactorAdderParams { + bool add_pose_priors; + bool add_projections; + double prior_translation_stddev; + double prior_quaternion_stddev; + 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 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_pose_factor.h b/localization/graph_localizer/include/graph_localizer/loc_pose_factor.h new file mode 100644 index 0000000000..8160165576 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/loc_pose_factor.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 GRAPH_LOCALIZER_LOC_POSE_FACTOR_H_ +#define GRAPH_LOCALIZER_LOC_POSE_FACTOR_H_ + +#include + +namespace gtsam { +// Allows for differentiation between loc pose factors and other pose priors, perhaps +// added as starting priors or marginalization factors +class LocPoseFactor : public PriorFactor { + private: + typedef PriorFactor Base; + + public: + typedef typename boost::shared_ptr shared_ptr; + + LocPoseFactor() {} + + virtual ~LocPoseFactor() {} + + LocPoseFactor(Key key, const Pose3& prior, const SharedNoiseModel& model = nullptr) : Base(key, prior, model) {} + + LocPoseFactor(Key key, const Pose3& prior, const Matrix& covariance) : Base(key, prior, covariance) {} + + private: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; +} // namespace gtsam + +#endif // GRAPH_LOCALIZER_LOC_POSE_FACTOR_H_ diff --git a/localization/graph_localizer/include/graph_localizer/loc_projection_factor.h b/localization/graph_localizer/include/graph_localizer/loc_projection_factor.h new file mode 100644 index 0000000000..8590d59324 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/loc_projection_factor.h @@ -0,0 +1,237 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_PROJECTION_FACTOR_H_ +#define GRAPH_LOCALIZER_LOC_PROJECTION_FACTOR_H_ + +#include +#include +#include +#include +#include + +#include + +namespace gtsam { + +/** + * Non-linear factor for a constraint derived from a 2D measurement. The + * calibration and landmark point are known here. Adapted from gtsam + * GenericProjectionFactor. + */ +template +class LocProjectionFactor : public NoiseModelFactor1 { + protected: + // 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 K_; ///< shared pointer to calibration object + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + // verbosity handling for Cheirality Exceptions + bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: + ///< false) + bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions + ///< (default: false) + + public: + /// shorthand for base class type + typedef NoiseModelFactor1 Base; + + /// shorthand for this class + typedef LocProjectionFactor This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor + LocProjectionFactor() + : measured_(0, 0), landmark_point_(0, 0, 0), throwCheirality_(false), verboseCheirality_(false) {} + + /** + * Constructor + * @param measured is the 2 dimensional location of point in image (the + * measurement) + * @param model is the standard deviation + * @param poseKey is the index of the camera + * @param pointKey is the index of the landmark + * @param K shared pointer to the constant calibration + * @param body_P_sensor is the transform from body to sensor frame (default + * identity) + */ + LocProjectionFactor(const Point2& measured, const LANDMARK& landmark_point, const SharedNoiseModel& model, + Key poseKey, const boost::shared_ptr& K, + boost::optional body_P_sensor = boost::none) + : Base(model, poseKey), + measured_(measured), + landmark_point_(landmark_point), + K_(K), + body_P_sensor_(body_P_sensor), + throwCheirality_(false), + verboseCheirality_(false) {} + + /** + * Constructor with exception-handling flags + * @param measured is the 2 dimensional location of point in image (the + * measurement) + * @param model is the standard deviation + * @param poseKey is the index of the camera + * @param pointKey is the index of the landmark + * @param K shared pointer to the constant calibration + * @param throwCheirality determines whether Cheirality exceptions are + * rethrown + * @param verboseCheirality determines whether exceptions are printed for + * Cheirality + * @param body_P_sensor is the transform from body to sensor frame (default + * identity) + */ + LocProjectionFactor(const Point2& measured, const LANDMARK& landmark_point, const SharedNoiseModel& model, + Key poseKey, const boost::shared_ptr& K, bool throwCheirality, + bool verboseCheirality, boost::optional body_P_sensor = boost::none) + : Base(model, poseKey), + measured_(measured), + landmark_point_(landmark_point), + K_(K), + body_P_sensor_(body_P_sensor), + throwCheirality_(throwCheirality), + verboseCheirality_(verboseCheirality) {} + + /** Virtual destructor */ + virtual ~LocProjectionFactor() {} + + /// @return a deep copy of this factor + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast(NonlinearFactor::shared_ptr(new This(*this))); + } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "LocProjectionFactor, z = "; + traits::Print(measured_); + std::cout << " landmark_point = "; + traits::Print(landmark_point_); + if (this->body_P_sensor_) this->body_P_sensor_->print(" sensor pose in body frame: "); + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol) && + traits::Equals(this->landmark_point_, e->landmark_point_, tol) && this->K_->equals(*e->K_, tol) && + ((!body_P_sensor_ && !e->body_P_sensor_) || + (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } + + /// Evaluate error h(x)-z and optionally derivatives + // TODO(rsoussan): Replace PinholeCamera with PinholePose? + Vector evaluateError(const Pose3& pose, boost::optional H1 = boost::none) const { + try { + if (body_P_sensor_) { + if (H1) { + Matrix H0; + PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); + Point2 reprojectionError(camera.project(landmark_point_, H1, boost::none, boost::none) - measured_); + *H1 = *H1 * H0; + return reprojectionError; + } else { + PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); + return camera.project(landmark_point_, H1, boost::none, boost::none) - measured_; + } + } else { + PinholeCamera camera(pose, *K_); + return camera.project(landmark_point_, H1, boost::none, boost::none) - measured_; + } + } catch (CheiralityException& e) { + if (H1) *H1 = Matrix::Zero(2, 6); + if (verboseCheirality_) + std::cout << e.what() << ": Landmark moved behind camera " << DefaultKeyFormatter(this->key()) << std::endl; + if (throwCheirality_) throw CheiralityException(this->key()); + } + return Vector2::Constant(0.0); + } + + bool cheiralityError(const Pose3& pose) const { + try { + if (body_P_sensor_) { + PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); + camera.project(landmark_point_) - measured_; + } else { + PinholeCamera camera(pose, *K_); + camera.project(landmark_point_) - measured_; + } + } catch (CheiralityException& e) { + return true; + } + 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 */ + inline const boost::shared_ptr calibration() const { return K_; } + + /** return verbosity */ + inline bool verboseCheirality() const { return verboseCheirality_; } + + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { return throwCheirality_; } + + boost::optional body_P_sensor() const { return body_P_sensor_; } + + private: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + 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_); + ar& BOOST_SERIALIZATION_NVP(verboseCheirality_); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +/// traits +template +struct traits> + : public Testable> {}; + +} // namespace gtsam + +#endif // GRAPH_LOCALIZER_LOC_PROJECTION_FACTOR_H_ diff --git a/localization/graph_localizer/include/graph_localizer/noise_params.h b/localization/graph_localizer/include/graph_localizer/noise_params.h new file mode 100644 index 0000000000..55c919cfe5 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/noise_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 GRAPH_LOCALIZER_NOISE_PARAMS_H_ +#define GRAPH_LOCALIZER_NOISE_PARAMS_H_ + +#include + +namespace graph_localizer { +struct NoiseParams { + 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 point_prior_translation_stddev; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_NOISE_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/parameter_reader.h b/localization/graph_localizer/include/graph_localizer/parameter_reader.h new file mode 100644 index 0000000000..c2b18b4ade --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/parameter_reader.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 GRAPH_LOCALIZER_PARAMETER_READER_H_ +#define GRAPH_LOCALIZER_PARAMETER_READER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace graph_localizer { +void LoadCalibrationParams(config_reader::ConfigReader& config, CalibrationParams& params); +void LoadFactorParams(config_reader::ConfigReader& config, FactorParams& 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 LoadGraphValuesParams(config_reader::ConfigReader& config, GraphValuesParams& params); +void LoadImuIntegrationParams(config_reader::ConfigReader& config, GraphInitializerParams& params); +void LoadNoiseParams(config_reader::ConfigReader& config, NoiseParams& 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 LoadGraphLocalizerParams(config_reader::ConfigReader& config, GraphLocalizerParams& params); +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_PARAMETER_READER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/pose_rotation_factor.h b/localization/graph_localizer/include/graph_localizer/pose_rotation_factor.h new file mode 100644 index 0000000000..46479cf437 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/pose_rotation_factor.h @@ -0,0 +1,98 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_POSE_ROTATION_FACTOR_H_ +#define GRAPH_LOCALIZER_POSE_ROTATION_FACTOR_H_ + +#include +#include +#include + +#include + +namespace gtsam { +class PoseRotationFactor : public NoiseModelFactor2 { + typedef NoiseModelFactor2 Base; + typedef PoseRotationFactor This; + + public: + PoseRotationFactor() {} + + PoseRotationFactor(const Rot3& rotation, const SharedNoiseModel& model, Key pose_key_1, Key pose_key_2) + : Base(model, pose_key_1, pose_key_2), rotation_(rotation) {} + + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast(gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << s << "PoseRotationFactor, z = "; + traits::Print(rotation_); + Base::print("", keyFormatter); + } + + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && traits::Equals(this->rotation_, e->rotation(), tol); + } + + Vector evaluateError(const Pose3& pose1, const Pose3& pose2, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + const auto& rot1 = pose1.rotation(H1); + const auto& rot2 = pose2.rotation(H2); + // TODO(rsoussan): How to use ref to block of H1 and H2 instead of making new matrices and copying? + Matrix H1_rot_matrix; + Matrix H2_rot_matrix; + boost::optional H1_rot = H1_rot_matrix; + boost::optional H2_rot = H2_rot_matrix; + if (!H1) H1_rot = boost::none; + if (!H2) H2_rot = boost::none; + // Adapted from BetweenFactor.h + const auto relative_rotation = traits::Between(rot1, rot2, H1_rot, H2_rot); + // manifold equivalent of h(x)-z -> log(z,h(x)) + traits::ChartJacobian::Jacobian Hlocal; + Vector error = traits::Local(rotation_, relative_rotation, boost::none, (H1_rot || H2_rot) ? &Hlocal : 0); + if (H1_rot) { + *H1_rot = Hlocal * (*H1_rot); + H1->block<3, 3>(0, 0) = H1_rot_matrix; + } + if (H2_rot) { + *H2_rot = Hlocal * (*H2_rot); + H2->block<3, 3>(0, 0) = H2_rot_matrix; + } + return error; + } + + const Rot3& rotation() const { return rotation_; } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(rotation_); + } + + Rot3 rotation_; + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace gtsam + +#endif // GRAPH_LOCALIZER_POSE_ROTATION_FACTOR_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 new file mode 100644 index 0000000000..402802afd1 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/projection_factor_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 GRAPH_LOCALIZER_PROJECTION_FACTOR_ADDER_H_ +#define GRAPH_LOCALIZER_PROJECTION_FACTOR_ADDER_H_ + +#include +#include +#include +#include +#include + +#include + +namespace graph_localizer { +class ProjectionFactorAdder + : public FactorAdder { + using Base = FactorAdder; + + public: + ProjectionFactorAdder(const ProjectionFactorAdderParams& params, + std::shared_ptr feature_tracker, + std::shared_ptr graph_values); + + std::vector AddFactors( + const localization_measurements::FeaturePointsMeasurement& feature_points_measurement) final; + + private: + std::shared_ptr feature_tracker_; + std::shared_ptr 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 new file mode 100644 index 0000000000..04b1bb989d --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/projection_factor_adder_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 GRAPH_LOCALIZER_PROJECTION_FACTOR_ADDER_PARAMS_H_ +#define GRAPH_LOCALIZER_PROJECTION_FACTOR_ADDER_PARAMS_H_ + +#include + +#include +#include +#include + +#include + +namespace graph_localizer { +struct ProjectionFactorAdderParams : public 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 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/robust_smart_projection_pose_factor.h b/localization/graph_localizer/include/graph_localizer/robust_smart_projection_pose_factor.h new file mode 100644 index 0000000000..4f52300027 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/robust_smart_projection_pose_factor.h @@ -0,0 +1,221 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_ROBUST_SMART_PROJECTION_POSE_FACTOR_H_ +#define GRAPH_LOCALIZER_ROBUST_SMART_PROJECTION_POSE_FACTOR_H_ + +#include + +#include +#include + +namespace gtsam { +template +class RobustSmartProjectionPoseFactor : public SmartProjectionPoseFactor { + typedef PinholePose Camera; + typedef SmartFactorBase Base; + typedef typename Camera::Measurement Z; + static const int Dim = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension + + public: + // For serialization + RobustSmartProjectionPoseFactor() {} + + /** + * Constructor + * @param sharedNoiseModel isotropic noise model for the 2D feature measurements + * @param K (fixed) calibration, assumed to be the same for all cameras + * @param params parameters for the smart projection factors + */ + RobustSmartProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, const boost::shared_ptr K, + const boost::optional body_P_sensor, + const SmartProjectionParams& params = SmartProjectionParams(), + const bool rotation_only_fallback = false, const bool robust = true, + const double huber_k = 1.0) + : SmartProjectionPoseFactor(sharedNoiseModel, K, body_P_sensor, params), + rotation_only_fallback_(rotation_only_fallback), + robust_(robust), + huber_k_(huber_k) { + // From SmartFactorBase + if (!sharedNoiseModel) throw std::runtime_error("RobustSmartProjectionPoseFactor: sharedNoiseModel is required"); + SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast(sharedNoiseModel); + if (!sharedIsotropic) throw std::runtime_error("RobustSmartProjectionPoseFactor: needs isotropic"); + noise_inv_sigma_ = 1.0 / sharedIsotropic->sigma(); + triangulation_params_ = params.triangulation; + } + + boost::shared_ptr linearize(const Values& values) const override { + typename Base::Cameras cameras = this->cameras(values); + // if (!this->triangulateForLinearize(cameras)) return boost::make_shared>(this->keys()); + const auto result = this->triangulateSafe(cameras); + // Adapted from SmartFactorBase::CreateJacobianSVDFactor + size_t m = this->keys().size(); + typename Base::FBlocks F; + Vector b; + const size_t M = ZDim * m; + Matrix E0(M, M - 3); + + // Handle behind camera result with rotation only factors (see paper) + // Degenerate result tends to lead to solve failures, so return empty factor in this case + if (result.valid()) { + this->computeJacobiansSVD(F, E0, b, cameras, *(this->point())); + // TODO(rsoussan): This should be here but leads to a degeneracy. It is done like this in gtsam + // for the jacobiansvd factorization but not the hessian factorization -> why? + } // else if (useForRotationOnly(result)) { // Rotation only factor + // Unit3 backProjected = cameras[0].backprojectPointAtInfinity(this->measured().at(0)); + // this->computeJacobiansSVD(F, E0, b, cameras, backProjected); + // } + else { // Empty factor // NOLINT + return boost::make_shared>(this->keys()); + } + return createRegularJacobianFactorSVD(this->keys(), F, E0, b); + } + + bool useForRotationOnly(const gtsam::TriangulationResult& result) const { + if (!rotation_only_fallback_) return false; + // Enable some 'invalid' results as these can still be useful for rotation errors + // return (result.degenerate()); + return (result.behindCamera()); + } + + double error(const Values& values) const override { + if (this->active(values)) { + try { + const double total_reprojection_loss = this->totalReprojectionError(this->cameras(values)); + const auto result = this->point(); + // TODO(rsoussan): This should be here but leads to a degeneracy (see comment in linearize) + // if (!result.valid() && !useForRotationOnly(result)) return 0.0; + // Multiply by 2 since totalReporjectionError divides mahal distance by 2, and robust_model_->loss + // expects mahal distance + const double loss = robust_ ? robustLoss(2.0 * total_reprojection_loss) : total_reprojection_loss; + return loss; + } catch (...) { + // Catch cheirality and other errors, zero on errors + return 0.0; + } + } else { // Inactive + return 0.0; + } + } + + // These "serialized" functions are only needed due to an error in gtsam for serializing result_. + // Call these instead of error() and point() for a smart factor that has been serialized. + // TODO(rsoussan): Remove these when gtsam bug fixed + double serialized_error(const Values& values) const { + if (this->active(values)) { + try { + const auto point = serialized_point(values); + const double total_reprojection_loss = this->totalReprojectionError(this->cameras(values), point); + // TODO(rsoussan): This should be here but leads to a degeneracy (see comment in linearize) + // if (!result.valid() && !useForRotationOnly(result)) return 0.0; + // Multiply by 2 since totalReporjectionError divides mahal distance by 2, and robust_model_->loss + // expects mahal distance + const double loss = robust_ ? robustLoss(2.0 * total_reprojection_loss) : total_reprojection_loss; + return loss; + } catch (...) { + // Catch cheirality and other errors, zero on errors + return 0.0; + } + } else { // Inactive + return 0.0; + } + } + + TriangulationResult serialized_point(const Values& values) const { + return gtsam::triangulateSafe(this->cameras(values), this->measured(), triangulation_params_); + } + + bool robust() const { return robust_; } + + double noise_inv_sigma() const { return noise_inv_sigma_; } + + // More efficient implementation of robust loss (also avoids inheritance calls) + double robustLoss(const double mahal_distance) const { + const double sqrt_mahal_distance = std::sqrt(mahal_distance); + const double absError = std::abs(sqrt_mahal_distance); + if (absError <= huber_k_) { // |x| <= k + return mahal_distance / 2; + } else { // |x| > k + return huber_k_ * (absError - (huber_k_ / 2)); + } + } + + private: + template + boost::shared_ptr> createRegularJacobianFactorSVD( + const KeyVector& keys, + const std::vector, Eigen::aligned_allocator>>& + Fblocks, + const Matrix& Enull, const Vector& b) const { + typedef std::pair KeyMatrix; + + Vector reduced_error = Enull.transpose() * b; + // Apply noise whitening and robust weighting manually to more efficiently robustify + // error vector and jacobians. Equivalent to calling whitenSystem with a robust noise model. + // Assumes noise is diagonal (required for this factor anyway). + reduced_error *= noise_inv_sigma_; + double reduced_matrix_weight = noise_inv_sigma_; + if (robust_) { + const double robust_weight = robustWeight(reduced_error.norm()); + reduced_error *= robust_weight; + reduced_matrix_weight *= robust_weight; + } + + size_t numKeys = Enull.rows() / ZDim; + size_t m2 = ZDim * numKeys - 3; // TODO(gtsam): is this not just Enull.rows()? + std::vector reduced_matrices; + reduced_matrices.reserve(numKeys); + for (size_t k = 0; k < Fblocks.size(); ++k) { + Key key = keys[k]; + reduced_matrices.emplace_back( + KeyMatrix(key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k] * reduced_matrix_weight)); + } + + return boost::make_shared>(reduced_matrices, reduced_error); + } + + // TODO(rsoussan): profile these calls vs. gtsam better, is there a significant improvement? + // More efficient implementation of robust weight (also avoids inheritance calls) + double robustWeight(const double error_norm) const { + const double squared_weight = (error_norm <= huber_k_) ? (1.0) : (huber_k_ / error_norm); + return std::sqrt(squared_weight); + } + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SmartProjectionPoseFactor); + ar& BOOST_SERIALIZATION_NVP(noise_inv_sigma_); + ar& BOOST_SERIALIZATION_NVP(huber_k_); + ar& BOOST_SERIALIZATION_NVP(robust_); + ar& BOOST_SERIALIZATION_NVP(rotation_only_fallback_); + ar& BOOST_SERIALIZATION_NVP(triangulation_params_); + } + + bool rotation_only_fallback_; + double robust_; + double huber_k_; + double noise_inv_sigma_; + // TODO(rsoussan): Remove once result_ serialization bug in gtsam fixed + TriangulationParameters triangulation_params_; +}; +} // namespace gtsam + +#endif // GRAPH_LOCALIZER_ROBUST_SMART_PROJECTION_POSE_FACTOR_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 new file mode 100644 index 0000000000..b33312b27b --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/rotation_factor_adder.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 GRAPH_LOCALIZER_ROTATION_FACTOR_ADDER_H_ +#define GRAPH_LOCALIZER_ROTATION_FACTOR_ADDER_H_ + +#include +#include +#include +#include + +#include + +namespace graph_localizer { +class RotationFactorAdder + : public FactorAdder { + using Base = FactorAdder; + + public: + RotationFactorAdder(const RotationFactorAdderParams& params, std::shared_ptr feature_tracker); + + std::vector AddFactors(const localization_measurements::FeaturePointsMeasurement& measurement) final; + + private: + std::shared_ptr feature_tracker_; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_ROTATION_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/rotation_factor_adder_params.h b/localization/graph_localizer/include/graph_localizer/rotation_factor_adder_params.h new file mode 100644 index 0000000000..375b63944b --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/rotation_factor_adder_params.h @@ -0,0 +1,37 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_PARAMS_H_ +#define GRAPH_LOCALIZER_ROTATION_FACTOR_ADDER_PARAMS_H_ + +#include + +#include +#include + +namespace graph_localizer { +struct RotationFactorAdderParams : public 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 + +#endif // GRAPH_LOCALIZER_ROTATION_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/sanity_checker.h b/localization/graph_localizer/include/graph_localizer/sanity_checker.h new file mode 100644 index 0000000000..702029881e --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/sanity_checker.h @@ -0,0 +1,41 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include + +#include + +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 new file mode 100644 index 0000000000..3052cb1ba6 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/sanity_checker_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 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/serialization.h b/localization/graph_localizer/include/graph_localizer/serialization.h new file mode 100644 index 0000000000..ba3b5c25c1 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/serialization.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 GRAPH_LOCALIZER_SERIALIZATION_H_ +#define GRAPH_LOCALIZER_SERIALIZATION_H_ + +#include + +#include + +namespace graph_localizer { +std::string SerializeBinary(const GraphLocalizer& graph_localizer); +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_SERIALIZATION_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 new file mode 100644 index 0000000000..f34c97bcee --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/smart_projection_cumulative_factor_adder.h @@ -0,0 +1,48 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include + +#include + +#include + +namespace graph_localizer { +class SmartProjectionCumulativeFactorAdder : public CumulativeFactorAdder { + using Base = CumulativeFactorAdder; + + public: + SmartProjectionCumulativeFactorAdder(const SmartProjectionFactorAdderParams& params, + std::shared_ptr feature_tracker); + + std::vector AddFactors() final; + + private: + void AddSmartFactor(const FeatureTrack& feature_track, FactorsToAdd& smart_factors_to_add) const; + + std::shared_ptr 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 new file mode 100644 index 0000000000..99f9d50f9b --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/smart_projection_factor_adder_params.h @@ -0,0 +1,51 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 + +#include +#include +#include + +#include + +namespace graph_localizer { +struct SmartProjectionFactorAdderParams : public 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; + bool rotation_only_fallback; + bool splitting; + bool scale_noise_with_num_points; + double noise_scale; + gtsam::Pose3 body_T_cam; + boost::shared_ptr 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/standstill_factor_adder.h b/localization/graph_localizer/include/graph_localizer/standstill_factor_adder.h new file mode 100644 index 0000000000..37a85f6b10 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/standstill_factor_adder.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 GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_H_ +#define GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_H_ + +#include +#include +#include +#include + +#include + +namespace graph_localizer { +class StandstillFactorAdder + : public FactorAdder { + using Base = FactorAdder; + + public: + explicit StandstillFactorAdder(const StandstillFactorAdderParams& params, + std::shared_ptr feature_tracker); + + std::vector AddFactors( + const localization_measurements::FeaturePointsMeasurement& feature_points_measurement) final; + + private: + std::shared_ptr feature_tracker_; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/standstill_factor_adder_params.h b/localization/graph_localizer/include/graph_localizer/standstill_factor_adder_params.h new file mode 100644 index 0000000000..2128af5b97 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/standstill_factor_adder_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 GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_PARAMS_H_ +#define GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_PARAMS_H_ + +#include + +namespace graph_localizer { +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 + +#endif // GRAPH_LOCALIZER_STANDSTILL_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/utilities.h b/localization/graph_localizer/include/graph_localizer/utilities.h new file mode 100644 index 0000000000..cb379bf4a4 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/utilities.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 GRAPH_LOCALIZER_UTILITIES_H_ +#define GRAPH_LOCALIZER_UTILITIES_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include +#include + +#include +#include +#include +#include + +namespace graph_localizer { +bool ValidPointSet(const std::deque& points, + const double average_distance_from_mean, const double min_avg_distance_from_mean, + const int min_num_points); + +double AverageDistanceFromMean(const std::deque& points); + +bool ValidVLMsg(const ff_msgs::VisualLandmarks& visual_landmarks_msg, const int min_num_landmarks); + +ff_msgs::GraphState GraphStateMsg(const localization_common::CombinedNavState& combined_nav_state, + const localization_common::CombinedNavStateCovariances& covariances, + const int num_optical_flow_features_in_last_measurement, + const int num_sparse_mapping_features_in_last_measurement, const bool estimating_bias, + const double position_log_det_threshold, const double orientation_log_det_threshold, + const bool standstill, const GraphStats& graph_stats); + +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); + +gtsam::noiseModel::Robust::shared_ptr Robust(const gtsam::SharedNoiseModel& noise, const double huber_k); + +boost::optional FixSmartFactorByRemovingIndividualMeasurements( + const GraphLocalizerParams& params, const RobustSmartFactor& smart_factor, + const gtsam::SmartProjectionParams& smart_projection_params, const GraphValues& graph_values); + +boost::optional FixSmartFactorByRemovingMeasurementSequence( + const GraphLocalizerParams& params, const RobustSmartFactor& smart_factor, + const gtsam::SmartProjectionParams& smart_projection_params, const GraphValues& graph_values); + +SharedRobustSmartFactor RemoveSmartFactorMeasurements(const RobustSmartFactor& smart_factor, + const std::unordered_set& factor_key_indices_to_remove, + const SmartProjectionFactorAdderParams& params, + const gtsam::SmartProjectionParams& smart_projection_params); +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_UTILITIES_H_ diff --git a/localization/graph_localizer/launch/graph_localizer.launch b/localization/graph_localizer/launch/graph_localizer.launch new file mode 100644 index 0000000000..c208161605 --- /dev/null +++ b/localization/graph_localizer/launch/graph_localizer.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/graph_localizer/nodelet_plugins.xml b/localization/graph_localizer/nodelet_plugins.xml new file mode 100644 index 0000000000..5dfc21ed51 --- /dev/null +++ b/localization/graph_localizer/nodelet_plugins.xml @@ -0,0 +1,11 @@ + + + + This nodelet wraps graph localizer + + + + + diff --git a/localization/graph_localizer/package.xml b/localization/graph_localizer/package.xml new file mode 100644 index 0000000000..784d51ba15 --- /dev/null +++ b/localization/graph_localizer/package.xml @@ -0,0 +1,26 @@ + + graph_localizer + 1.0.0 + + The graph localizer package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + ff_msgs + nodelet + roscpp + ff_msgs + nodelet + roscpp + + + + diff --git a/localization/graph_localizer/readme.md b/localization/graph_localizer/readme.md new file mode 100644 index 0000000000..73416c644f --- /dev/null +++ b/localization/graph_localizer/readme.md @@ -0,0 +1,33 @@ +\page graphlocalizer Graph Localizer + +# Package Overview +The graph localizer fuses imu measurements, optical flow features, localization image projection measurements, and ar tag measurements to estimate a history of pose, velocity, and imu biases for desired timestamps. The graph localizer uses the gtsam package (_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). It relies on 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. + +## Ros Node +The ros node subscribes to the measurement topics and publishes a graph localization state message containing state estimates and covariances. Optionally, the node also publishes a serialized graph localizer that can be used to save the graph localizer and for visualization in rviz using one of the available localization plugins (see localization\_rviz\_plugins). + +# Important Classes + +## Graph Localizer +Fuses measurements to estimate pose, velocity, and biases for a window of time (and optionally for a max number of states, where a state is a pose, velocity and bias estimate at a timestamp). Creates error functions (factors) for each measurement and minimizes these errors by adjusting the history of state estimates using a nonlinear optimization procedure provided by gtsam. + +## Graph Values +While the graph localizer contains a factor graph to store factors, state estimates are stored in the GraphValues object. This provides the option to `slide_window` which maintains the correct duration and number of state parameters for the graph. + + +## Feature Tracker +Optical flow feature tracks are stored in the FeatureTracker class which also support removing old measurements. + + +## Sanity Checker +The sanity checker optionally checks localization estimates using either the current localization pose compared to some other source (sparse mapping) or the localization pose covariance. + +# Inputs +* `/loc/of/features` +* `/loc/ml/features` +* `/loc/ar/features` +* `/hw/imu` + +# Outputs +* `graph_loc/graph` +* `graph_loc/state` diff --git a/localization/graph_localizer/src/feature_tracker.cc b/localization/graph_localizer/src/feature_tracker.cc new file mode 100644 index 0000000000..4a18ea46d3 --- /dev/null +++ b/localization/graph_localizer/src/feature_tracker.cc @@ -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. + */ + +#include +#include + +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) { + // Update latest time if a new time is available. + // Feature points don't necessarily arrive in time order. + if (!feature_points.empty()) { + latest_time_ = + latest_time_ ? std::max(*latest_time_, feature_points.front().timestamp) : feature_points.front().timestamp; + } + + const int starting_num_feature_tracks = feature_tracks_.size(); + + LogDebug("UpdateFeatureTracks: Starting num feature tracks: " << starting_num_feature_tracks); + // Update existing features or add new one + for (const auto& feature_point : feature_points) { + feature_tracks_[feature_point.feature_id].latest_image_id = feature_point.image_id; + feature_tracks_[feature_point.feature_id].id = feature_point.feature_id; + feature_tracks_[feature_point.feature_id].points.emplace_back(feature_point); + } + + const int post_add_num_feature_tracks = 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 image_id = feature_points.empty() ? 0 : feature_points.front().image_id; + const bool remove_all_features = feature_points.empty(); + for (auto feature_it = feature_tracks_.cbegin(); feature_it != feature_tracks_.cend();) { + if (feature_it->second.latest_image_id != image_id || remove_all_features) { + feature_it = feature_tracks_.erase(feature_it); + } else { + ++feature_it; + } + } + + const int removed_num_feature_tracks = post_add_num_feature_tracks - feature_tracks_.size(); + LogDebug("UpdateFeatureTracks: Removed feature tracks: " << removed_num_feature_tracks); + LogDebug("UpdateFeatureTracks: Final total num feature tracks: " << feature_tracks_.size()); +} + +void FeatureTracker::RemovePointsOutsideWindow() { + if (!latest_time_) return; + const lc::Time oldest_allowed_time = *latest_time_ - params_.sliding_window_duration; + if (oldest_allowed_time <= 0) return; + RemoveOldFeaturePoints(oldest_allowed_time); +} + +void FeatureTracker::RemoveOldFeaturePoints(lc::Time oldest_allowed_time) { + // Remove any timestamp before oldest_allowed_time and before start of time window + oldest_allowed_time = + latest_time_ ? std::max(*latest_time_ - params_.sliding_window_duration, oldest_allowed_time) : oldest_allowed_time; + + // Handle any out of order tracks that are too old. Split into two for loops + // so ordered points can be removed in sub linear time (most measurements are ordered). + // TODO(rsoussan): Do this more efficiently + for (auto& feature : feature_tracks_) { + auto point_it = feature.second.points.cbegin(); + while (point_it != feature.second.points.cend() && point_it->timestamp < oldest_allowed_time) { + ++point_it; + } + feature.second.points.erase(feature.second.points.begin(), point_it); + } + + for (auto& feature : feature_tracks_) { + auto point_it = feature.second.points.cbegin(); + while (point_it != feature.second.points.cend()) { + if (point_it->timestamp < oldest_allowed_time) + point_it = feature.second.points.erase(point_it); + else + ++point_it; + } + } +} + +boost::optional FeatureTracker::latest_timestamp() const { return latest_time_; } + +boost::optional FeatureTracker::PreviousTimestamp() const { + for (const auto& feature_track_pair : feature_tracks_) { + const auto& points = feature_track_pair.second.points; + if (points.size() < 2) continue; + return points[points.size() - 2].timestamp; + } + return boost::none; +} + +// TODO(rsoussan): Store points in sorted order to make this and PreviousTimestamp more efficient +boost::optional FeatureTracker::OldestTimestamp() const { + boost::optional oldest_timestamp; + for (const auto& feature_track_pair : feature_tracks_) { + for (const auto& point : feature_track_pair.second.points) { + if (!oldest_timestamp || point.timestamp < *oldest_timestamp) oldest_timestamp = point.timestamp; + } + } + return oldest_timestamp; +} + +} // namespace graph_localizer diff --git a/localization/graph_localizer/src/graph_localizer.cc b/localization/graph_localizer/src/graph_localizer.cc new file mode 100644 index 0000000000..274e607af4 --- /dev/null +++ b/localization/graph_localizer/src/graph_localizer.cc @@ -0,0 +1,1243 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +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_localizer { +namespace ii = imu_integration; +namespace lc = localization_common; +namespace lm = localization_measurements; + +GraphLocalizer::GraphLocalizer(const GraphLocalizerParams& params) + : feature_tracker_(new FeatureTracker(params.feature_tracker)), + latest_imu_integrator_(params.graph_initializer), + graph_values_(new GraphValues(params.graph_values)), + log_on_destruction_(true), + params_(params) { + // Assumes zero initial velocity + const lc::CombinedNavState global_N_body_start(params_.graph_initializer.global_T_body_start, + gtsam::Velocity3::Zero(), params_.graph_initializer.initial_imu_bias, + params_.graph_initializer.start_time); + + // Add first nav state and priors to graph + const int key_index = GenerateKeyIndex(); + graph_values_->AddCombinedNavState(global_N_body_start, key_index); + AddStartingPriors(global_N_body_start, key_index, graph_); + + // Initialize smart projection factor params + // TODO(rsoussan): Remove this once splitting function is moved, remove smart_projection_params_ from graph_localizer + smart_projection_params_.verboseCheirality = params_.factor.smart_projection_adder.verbose_cheirality; + smart_projection_params_.setRankTolerance(1e-9); + smart_projection_params_.setLandmarkDistanceThreshold( + params_.factor.smart_projection_adder.landmark_distance_threshold); + smart_projection_params_.setDynamicOutlierRejectionThreshold( + params_.factor.smart_projection_adder.dynamic_outlier_rejection_threshold); + smart_projection_params_.setRetriangulationThreshold(params_.factor.smart_projection_adder.retriangulation_threshold); + smart_projection_params_.setEnableEPI(params_.factor.smart_projection_adder.enable_EPI); + + // Initialize projection triangulation params + projection_triangulation_params_.rankTolerance = 1e-9; + projection_triangulation_params_.enableEPI = params_.factor.projection_adder.enable_EPI; + projection_triangulation_params_.landmarkDistanceThreshold = + params_.factor.projection_adder.landmark_distance_threshold; + projection_triangulation_params_.dynamicOutlierRejectionThreshold = + params_.factor.projection_adder.dynamic_outlier_rejection_threshold; + + // 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("GraphLocalizer: No marginals factorization entered, defaulting to qr."); + marginals_factorization_ = gtsam::Marginals::Factorization::QR; + } + + // Initialize Factor Adders + ar_tag_loc_factor_adder_.reset( + new LocFactorAdder(params_.factor.ar_tag_loc_adder, GraphAction::kARTagProjectionNoiseScaling)); + loc_factor_adder_.reset(new LocFactorAdder(params_.factor.loc_adder, GraphAction::kLocProjectionNoiseScaling)); + projection_factor_adder_.reset( + new ProjectionFactorAdder(params_.factor.projection_adder, feature_tracker_, 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_)); +} + +GraphLocalizer::~GraphLocalizer() { + if (log_on_destruction_) graph_stats_.Log(); +} + +void GraphLocalizer::AddStartingPriors(const lc::CombinedNavState& global_N_body_start, const int key_index, + gtsam::NonlinearFactorGraph& graph) { + const gtsam::Vector6 pose_prior_noise_sigmas( + (gtsam::Vector(6) << params_.noise.starting_prior_translation_stddev, + params_.noise.starting_prior_translation_stddev, params_.noise.starting_prior_translation_stddev, + params_.noise.starting_prior_quaternion_stddev, params_.noise.starting_prior_quaternion_stddev, + params_.noise.starting_prior_quaternion_stddev) + .finished()); + const gtsam::Vector3 velocity_prior_noise_sigmas((gtsam::Vector(3) << params_.noise.starting_prior_velocity_stddev, + params_.noise.starting_prior_velocity_stddev, + params_.noise.starting_prior_velocity_stddev) + .finished()); + const gtsam::Vector6 bias_prior_noise_sigmas( + (gtsam::Vector(6) << params_.noise.starting_prior_accel_bias_stddev, params_.noise.starting_prior_accel_bias_stddev, + params_.noise.starting_prior_accel_bias_stddev, params_.noise.starting_prior_gyro_bias_stddev, + params_.noise.starting_prior_gyro_bias_stddev, params_.noise.starting_prior_gyro_bias_stddev) + .finished()); + lc::CombinedNavStateNoise noise; + noise.pose_noise = Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(pose_prior_noise_sigmas)), params_.huber_k); + noise.velocity_noise = + Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(velocity_prior_noise_sigmas)), + params_.huber_k); + noise.bias_noise = Robust( + gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(bias_prior_noise_sigmas)), params_.huber_k); + AddPriors(global_N_body_start, noise, key_index, graph); +} + +void GraphLocalizer::AddPriors(const lc::CombinedNavState& global_N_body, const lc::CombinedNavStateNoise& noise, + const int key_index, gtsam::NonlinearFactorGraph& graph) { + gtsam::PriorFactor pose_prior_factor(sym::P(key_index), global_N_body.pose(), noise.pose_noise); + graph.push_back(pose_prior_factor); + gtsam::PriorFactor velocity_prior_factor(sym::V(key_index), global_N_body.velocity(), + noise.velocity_noise); + graph.push_back(velocity_prior_factor); + gtsam::PriorFactor bias_prior_factor(sym::B(key_index), global_N_body.bias(), + noise.bias_noise); + graph.push_back(bias_prior_factor); +} + +boost::optional> +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> +GraphLocalizer::LatestCombinedNavStateAndCovariances(const gtsam::Marginals& marginals) const { + const auto global_N_body_latest = 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 = 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{*global_N_body_latest, + latest_combined_nav_state_covariances}; + } catch (...) { + LogError("LatestCombinedNavStateAndCovariances: Failed to get marginal covariances."); + return boost::none; + } +} + +boost::optional GraphLocalizer::LatestCombinedNavState() const { + const auto global_N_body_latest = 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 GraphLocalizer::GetCombinedNavState(const lc::Time time) const { + const auto lower_bound_or_equal_combined_nav_state = 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) { + LogError("GetCombinedNavState: Failed to create integrated pim."); + return boost::none; + } + + return ii::PimPredict(*lower_bound_or_equal_combined_nav_state, *integrated_pim); +} + +boost::optional> GraphLocalizer::LatestBiases() const { + const auto latest_bias = 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 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.points); + // Only consider long enough feature tracks for standstill candidates + if (static_cast(feature_track.second.points.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."); +} + +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(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::AddSparseMappingMeasurement( + 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)); + } +} + +// TODO(rsoussan): Clean this function up (duplicate code), address other todo's +void GraphLocalizer::SplitSmartFactorsIfNeeded(FactorsToAdd& factors_to_add) { + for (auto& factor_to_add : factors_to_add.Get()) { + auto smart_factor = dynamic_cast(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"); + } +} + +bool GraphLocalizer::TriangulateNewPoint(FactorsToAdd& factors_to_add) { + gtsam::CameraSet 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(factor.get()); + if (!projection_factor) { + LogError("TriangulateNewPoint: Failed to cast to projection factor."); + return false; + } + const auto world_T_body = graph_values_->at(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_.calibration.body_T_nav_cam; + camera_set.emplace_back(world_T_camera, params_.calibration.nav_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(); + graph_values_->AddFeature(feature_id, *world_t_triangulated_point, point_key); + if (params_.factor.projection_adder.add_point_priors) { + const gtsam::Vector3 point_prior_noise_sigmas((gtsam::Vector(3) << params_.noise.point_prior_translation_stddev, + params_.noise.point_prior_translation_stddev, + params_.noise.point_prior_translation_stddev) + .finished()); + const auto point_noise = + Robust(gtsam::noiseModel::Diagonal::Sigmas(Eigen::Ref(point_prior_noise_sigmas)), + params_.huber_k); + gtsam::PriorFactor point_prior_factor(point_key, *world_t_triangulated_point, point_noise); + graph_.push_back(point_prior_factor); + } + return true; +} + +bool GraphLocalizer::LocProjectionNoiseScaling(FactorsToAdd& factors_to_add) { + return MapProjectionNoiseScaling(params_.factor.loc_adder, factors_to_add); +} + +bool GraphLocalizer::ARProjectionNoiseScaling(FactorsToAdd& factors_to_add) { + return MapProjectionNoiseScaling(params_.factor.ar_tag_loc_adder, factors_to_add); +} + +bool GraphLocalizer::MapProjectionNoiseScaling(const LocFactorAdderParams& params, FactorsToAdd& factors_to_add) { + auto& factors = factors_to_add.Get(); + boost::optional pose_key; + boost::optional world_T_cam; + for (auto factor_it = factors.begin(); factor_it != factors.end();) { + auto& factor = factor_it->factor; + auto projection_factor = dynamic_cast*>(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(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(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(FactorToAdd({KeyInfo(&sym::P, factors_to_add.timestamp())}, pose_prior_factor)); + } + return true; +} + +bool GraphLocalizer::AddOrSplitImuFactorIfNeeded(const lc::Time timestamp) { + 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."); + const auto timestamps_to_add = TimestampsToAdd(timestamp, *latest_timestamp); + if (timestamps_to_add.size() > 1) + LogDebug("AddOrSplitImuFactorIfNeeded: Adding extra imu factors and nav states due to large time difference."); + bool added_timestamps = true; + for (const auto timestamp_to_add : timestamps_to_add) { + added_timestamps &= CreateAndAddLatestImuFactorAndCombinedNavState(timestamp_to_add); + } + return added_timestamps; + } else { + LogDebug("AddOrSplitImuFactorIfNeeded: Splitting old imu factor."); + return SplitOldImuFactorAndAddCombinedNavState(timestamp); + } +} + +std::vector GraphLocalizer::TimestampsToAdd(const lc::Time timestamp, const lc::Time last_added_timestamp) { + if (!params_.limit_imu_factor_spacing) return {timestamp}; + const double timestamp_difference = timestamp - last_added_timestamp; + if (timestamp_difference < params_.max_imu_factor_spacing) return {timestamp}; + // Evenly distribute timestamps so that the min number is added such that each spacing is <= max_imu_factor_spacing + const int num_timestamps_to_add = std::ceil(timestamp_difference / params_.max_imu_factor_spacing); + const double timestamps_to_add_spacing = timestamp_difference / num_timestamps_to_add; + std::vector timestamps_to_add; + // Add up to final timestamp, insert timestamp argument as final timestamp to ensure no floating point errors occur, + // since the final timestamp should exactly match the given timestamp + for (int i = 1; i < num_timestamps_to_add; ++i) { + const double timestamp_to_add = last_added_timestamp + i * timestamps_to_add_spacing; + // TODO(rsoussan): Account for recent enough when calculating spacing? + if (MeasurementRecentEnough(timestamp_to_add)) timestamps_to_add.emplace_back(timestamp_to_add); + } + timestamps_to_add.emplace_back(timestamp); + + return timestamps_to_add; +} + +bool GraphLocalizer::SplitOldImuFactorAndAddCombinedNavState(const lc::Time timestamp) { + 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 = graph_.begin(); factor_it != graph_.end();) { + if (dynamic_cast(factor_it->get()) && + graph_values_->ContainsCombinedNavStateKey(**factor_it, *lower_bound_key_index) && + graph_values_->ContainsCombinedNavStateKey(**factor_it, *upper_bound_key_index)) { + graph_.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(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)) { + 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); + graph_.push_back(combined_imu_factor); + return true; +} + +bool GraphLocalizer::CreateAndAddLatestImuFactorAndCombinedNavState(const lc::Time timestamp) { + 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())) { + 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 GraphLocalizer::CreateAndAddImuFactorAndPredictedCombinedNavState( + const lc::CombinedNavState& global_N_body, const gtsam::PreintegratedCombinedMeasurements& pim) { + 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); + graph_.push_back(combined_imu_factor); + graph_values_->AddCombinedNavState(global_N_body_predicted, key_index_1); + return true; +} + +// Adapted from gtsam::BatchFixedLagSmoother +gtsam::NonlinearFactorGraph GraphLocalizer::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(graph_values_->values()); + const auto linear_marginal_factors = + *(linearized_graph->eliminatePartialMultifrontal(old_keys, eliminate_function).second); + return gtsam::LinearContainerFactor::ConvertLinearGraph(linear_marginal_factors, graph_values_->values()); +} + +bool GraphLocalizer::SlideWindow(const boost::optional& marginals, const lc::Time last_latest_time) { + const auto graph_values_ideal_new_oldest_time = graph_values_->SlideWindowNewOldestTime(); + if (!graph_values_ideal_new_oldest_time) { + LogDebug("SlideWindow: No states removed. "); + return true; + } + // 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 < *graph_values_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, *graph_values_ideal_new_oldest_time); + + // Add marginal factors for marginalized values + auto old_keys = graph_values_->OldKeys(new_oldest_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); + auto old_factors = graph_values_->RemoveOldFactors(old_keys, graph_); + gtsam::KeyVector old_feature_keys; + if (params_.factor.projection_adder.enabled) { + // Call remove old factors before old feature keys, since old feature keys depend on + // number of factors per key remaining + old_feature_keys = graph_values_->OldFeatureKeys(graph_); + auto old_feature_factors = graph_values_->RemoveOldFactors(old_feature_keys, graph_); + old_keys.insert(old_keys.end(), old_feature_keys.begin(), old_feature_keys.end()); + old_factors.push_back(old_feature_factors); + } + if (params_.add_marginal_factors) { + const auto marginal_factors = MarginalFactors(old_factors, old_keys, gtsam::EliminateQR); + for (const auto& marginal_factor : marginal_factors) { + graph_.push_back(marginal_factor); + } + } + + graph_values_->RemoveOldCombinedNavStates(new_oldest_time); + if (params_.factor.projection_adder.enabled) graph_values_->RemoveOldFeatures(old_feature_keys); + + // Remove old data from other containers + // TODO(rsoussan): Just use new_oldest_time and don't bother getting oldest timestamp here? + const auto oldest_timestamp = graph_values_->OldestTimestamp(); + if (!oldest_timestamp || *oldest_timestamp != new_oldest_time) { + LogError("SlideWindow: Failed to get oldest timestamp."); + return false; + } + + feature_tracker_->RemoveOldFeaturePoints(*oldest_timestamp); + latest_imu_integrator_.RemoveOldMeasurements(*oldest_timestamp); + RemoveOldBufferedFactors(*oldest_timestamp); + + if (params_.factor.projection_adder.enabled && params_.factor.projection_adder.add_point_priors && marginals_) { + UpdatePointPriors(*marginals_); + } + + 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); + if (marginals) { + lc::CombinedNavStateNoise noise; + noise.pose_noise = Robust( + gtsam::noiseModel::Gaussian::Covariance(marginals->marginalCovariance(sym::P(*key_index))), params_.huber_k); + noise.velocity_noise = Robust( + gtsam::noiseModel::Gaussian::Covariance(marginals->marginalCovariance(sym::V(*key_index))), params_.huber_k); + noise.bias_noise = Robust( + gtsam::noiseModel::Gaussian::Covariance(marginals->marginalCovariance(sym::B(*key_index))), params_.huber_k); + AddPriors(*global_N_body_oldest, noise, *key_index, graph_); + } else { + // TODO(rsoussan): Add seperate marginal fallback sigmas instead of relying on starting prior sigmas + AddStartingPriors(*global_N_body_oldest, *key_index, graph_); + } + } + + return true; +} + +void GraphLocalizer::UpdatePointPriors(const gtsam::Marginals& marginals) { + const auto feature_keys = graph_values_->FeatureKeys(); + for (const auto& feature_key : feature_keys) { + const auto world_t_point = graph_values_->at(feature_key); + if (!world_t_point) { + LogError("UpdatePointPriors: Failed to get world_t_point."); + continue; + } + for (auto factor_it = graph_.begin(); factor_it != graph_.end();) { + const auto point_prior_factor = dynamic_cast*>(factor_it->get()); + if (point_prior_factor && (point_prior_factor->key() == feature_key)) { + // Erase old prior + factor_it = graph_.erase(factor_it); + // Add updated one + const auto point_prior_noise = + Robust(gtsam::noiseModel::Gaussian::Covariance(marginals.marginalCovariance(feature_key)), params_.huber_k); + const gtsam::PriorFactor point_prior_factor(feature_key, *world_t_point, point_prior_noise); + graph_.push_back(point_prior_factor); + // Only one point prior per feature + break; + } else { + ++factor_it; + } + } + } +} + +void GraphLocalizer::RemovePriors(const int key_index) { + int removed_factors = 0; + for (auto factor_it = graph_.begin(); factor_it != graph_.end();) { + bool erase_factor = false; + const auto pose_prior_factor = dynamic_cast*>(factor_it->get()); + const auto loc_pose_factor = dynamic_cast(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*>(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*>(factor_it->get()); + if (bias_prior_factor && bias_prior_factor->key() == sym::B(key_index)) { + erase_factor = true; + } + + if (erase_factor) { + factor_it = graph_.erase(factor_it); + ++removed_factors; + } else { + ++factor_it; + continue; + } + } + LogDebug("RemovePriors: Erase " << removed_factors << " factors."); +} + +void GraphLocalizer::BufferCumulativeFactors() { + // Remove measurements here so they are more likely to fit in sliding window duration when optimized + feature_tracker_->RemovePointsOutsideWindow(); + 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_.begin(); factor_it != graph_.end();) { + const auto smart_factor = dynamic_cast(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 factor_key_indices_to_remove; + for (int i = 0; i < static_cast(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(factor_keys.size() - factor_key_indices_to_remove.size()) < + params_.factor.smart_projection_adder.min_num_points) { + factor_it = graph_.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_params_); + *factor_it = new_smart_factor; + continue; + } + } + } +} + +void GraphLocalizer::BufferFactors(const std::vector& 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); +} + +void GraphLocalizer::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; + } + } +} + +int GraphLocalizer::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() && latest_imu_integrator_.LatestTime() && + factors_to_add_it->first <= *(latest_imu_integrator_.LatestTime());) { + auto& factors_to_add = factors_to_add_it->second; + for (auto& factor_to_add : factors_to_add.Get()) { + // Add combined nav states and connecting imu factors for each key in factor if necessary + // TODO(rsoussan): make this more efficient for factors with multiple keys with the same timestamp? + for (const auto& key_info : factor_to_add.key_infos) { + // Ignore static keys + if (key_info.is_static()) continue; + if (!AddOrSplitImuFactorIfNeeded(key_info.timestamp())) { + LogError("AddBufferedFactor: Failed to add or split imu factors necessary for adding factor."); + continue; + } + } + + if (!Rekey(factor_to_add)) { + LogError("AddBufferedMeasurements: Failed to rekey factor to add."); + continue; + } + } + + // Do graph action after adding necessary imu factors and nav states so these are available + 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; + } + + 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); + } + + LogDebug("AddBufferedFactors: Added " << num_added_factors << " factors."); + return num_added_factors; +} + +bool GraphLocalizer::DoGraphAction(FactorsToAdd& factors_to_add) { + switch (factors_to_add.graph_action()) { + case GraphAction::kNone: + return true; + case GraphAction::kDeleteExistingSmartFactors: + LogDebug("DoGraphAction: Deleting smart factors."); + DeleteFactors(); + // TODO(rsoussan): rename this graph action to handle smart factors + if (params_.factor.smart_projection_adder.splitting) SplitSmartFactorsIfNeeded(factors_to_add); + return true; + case GraphAction::kTriangulateNewPoint: + return TriangulateNewPoint(factors_to_add); + case GraphAction::kLocProjectionNoiseScaling: + return LocProjectionNoiseScaling(factors_to_add); + case GraphAction::kARTagProjectionNoiseScaling: + return ARProjectionNoiseScaling(factors_to_add); + } + + // Shouldn't occur + return true; +} + +bool GraphLocalizer::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(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 = graph_values_->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); + } + } + factor_to_add.factor->keys() = new_keys; + return true; +} + +bool GraphLocalizer::ReadyToAddMeasurement(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); +} + +bool GraphLocalizer::MeasurementRecentEnough(const lc::Time timestamp) const { + if (!latest_imu_integrator_.OldestTime()) { + LogDebug("MeasurementRecentEnough: Waiting until imu measurements have been received."); + return false; + } + if (timestamp < graph_values_->OldestTimestamp()) return false; + if (timestamp < latest_imu_integrator_.OldestTime()) return false; + return true; +} + +void GraphLocalizer::PrintFactorDebugInfo() const { + for (const auto& factor : graph_) { + const auto smart_factor = dynamic_cast(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(factor.get()); + if (imu_factor) { + LogDebug("PrintFactorDebugInfo: CombinedImuFactor: " << *imu_factor); + LogDebug("PrintFactorDebugInfo: CombinedImuFactor PIM: " << imu_factor->preintegratedMeasurements()); + } + } +} + +const GraphLocalizerParams& GraphLocalizer::params() const { return params_; } + +int GraphLocalizer::NumFeatures() const { return graph_values_->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(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_) { + const auto projection_factor = dynamic_cast(factor.get()); + if (projection_factor) { + if (check_valid) { + const auto world_t_point = graph_values_->at(projection_factor->key2()); + if (!world_t_point) { + LogError("NumProjectionFactors: Failed to get point."); + continue; + } + const auto world_T_body = graph_values_->at(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; +} + +int GraphLocalizer::NumSmartFactors(const bool check_valid) const { + int num_of_factors = 0; + for (const auto& factor : graph_) { + const auto smart_factor = dynamic_cast(factor.get()); + if (smart_factor) { + if (check_valid) { + if (smart_factor->isValid()) ++num_of_factors; + } else { + ++num_of_factors; + } + } + } + return num_of_factors; +} + +const GraphValues& GraphLocalizer::graph_values() const { return *graph_values_; } + +const gtsam::NonlinearFactorGraph& GraphLocalizer::factor_graph() const { return graph_; } + +void GraphLocalizer::SaveGraphDotFile(const std::string& output_path) const { + std::ofstream of(output_path.c_str()); + graph_.saveGraph(of, graph_values_->values()); +} + +const GraphStats& GraphLocalizer::graph_stats() const { return graph_stats_; } + +void GraphLocalizer::LogOnDestruction(const bool log_on_destruction) { log_on_destruction_ = log_on_destruction; } + +bool GraphLocalizer::standstill() const { + // If uninitialized, return not at standstill + // TODO(rsoussan): Is this the appropriate behavior? + if (!standstill_) return false; + return *standstill_; +} + +bool GraphLocalizer::Update() { + LogDebug("Update: Updating."); + graph_stats_.update_timer_.Start(); + + 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_, graph_values_->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(); + + 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 = graph_values_->SlideWindowNewOldestTime(); + if (new_oldest_time) { + const auto old_keys = graph_values_->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; + } + } + + // Optimize + gtsam::LevenbergMarquardtOptimizer optimizer(graph_, graph_values_->values(), levenberg_marquardt_params_); + + graph_stats_.optimization_timer_.Start(); + // TODO(rsoussan): Indicate if failure occurs in state msg, perhaps using confidence value for localizer + try { + graph_values_->UpdateValues(optimizer.optimize()); + } catch (gtsam::IndeterminantLinearSystemException) { + log(params_.fatal_failures, "Update: Indeterminant linear system error during optimization, 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_, graph_values_->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_ = graph_values_->LatestTimestamp(); + graph_stats_.iterations_averager_.Update(optimizer.iterations()); + graph_stats_.UpdateStats(*this); + graph_stats_.UpdateErrors(*this); + + if (params_.print_factor_info) PrintFactorDebugInfo(); + + // Update imu integrator bias + const auto latest_bias = graph_values_->LatestBias(); + if (!latest_bias) { + LogError("Update: Failed to get latest bias."); + return false; + } + + latest_imu_integrator_.ResetPimIntegrationAndSetBias(latest_bias->first); + graph_stats_.update_timer_.Stop(); + return true; +} +} // namespace graph_localizer diff --git a/localization/graph_localizer/src/graph_localizer_intializer.cc b/localization/graph_localizer/src/graph_localizer_intializer.cc new file mode 100644 index 0000000000..5979a80f0b --- /dev/null +++ b/localization/graph_localizer/src/graph_localizer_intializer.cc @@ -0,0 +1,185 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include + +#include +#include + +namespace graph_localizer { +namespace lc = localization_common; +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 gtsam::Pose3& global_T_body_start, const double timestamp) { + params_.graph_initializer.start_time = timestamp; + params_.graph_initializer.global_T_body_start = global_T_body_start; + has_start_pose_ = true; + RemoveGravityFromBiasIfPossibleAndNecessary(); +} + +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::ImuFilter(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()) { + LogError("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 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(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::ImuFilter(params_.graph_initializer.filter)); + 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() && 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::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 new file mode 100644 index 0000000000..fdcf9284be --- /dev/null +++ b/localization/graph_localizer/src/graph_localizer_nodelet.cc @@ -0,0 +1,277 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace graph_localizer { +namespace lc = localization_common; +namespace mc = msg_conversions; + +GraphLocalizerNodelet::GraphLocalizerNodelet() + : ff_util::FreeFlyerNodelet(NODE_GRAPH_LOC, true), platform_name_(GetPlatform()) { + 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."); + } + 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"); +} + +void GraphLocalizerNodelet::Initialize(ros::NodeHandle* nh) { + ff_common::InitFreeFlyerApplication(getMyArgv()); + SubscribeAndAdvertise(nh); + Run(); +} + +void GraphLocalizerNodelet::SubscribeAndAdvertise(ros::NodeHandle* nh) { + state_pub_ = nh->advertise(TOPIC_GRAPH_LOC_STATE, 10); + sparse_mapping_pose_pub_ = nh->advertise(TOPIC_SPARSE_MAPPING_POSE, 10); + ar_tag_pose_pub_ = nh->advertise(TOPIC_AR_TAG_POSE, 10); + graph_pub_ = nh->advertise(TOPIC_GRAPH_LOC, 10); + reset_pub_ = nh->advertise(TOPIC_GNC_EKF_RESET, 10); + heartbeat_pub_ = nh->advertise(TOPIC_HEARTBEAT, 5, true); + + // Buffer max should be at least 10 and a max of 2 seconds of data + // Imu freq: ~62.5 + constexpr int kMaxNumImuMsgs = 125; + // AR freq: ~1 + constexpr int kMaxNumARMsgs = 10; + // VL freq: ~1 + constexpr int kMaxNumVLMsgs = 10; + // OF freq: ~10 + constexpr int kMaxNumOFMsgs = 20; + + imu_sub_ = private_nh_.subscribe(TOPIC_HARDWARE_IMU, kMaxNumImuMsgs, &GraphLocalizerNodelet::ImuCallback, this, + ros::TransportHints().tcpNoDelay()); + ar_sub_ = + private_nh_.subscribe(TOPIC_LOCALIZATION_AR_FEATURES, kMaxNumARMsgs, + &GraphLocalizerNodelet::ARVisualLandmarksCallback, this, ros::TransportHints().tcpNoDelay()); + of_sub_ = + private_nh_.subscribe(TOPIC_LOCALIZATION_OF_FEATURES, kMaxNumOFMsgs, &GraphLocalizerNodelet::OpticalFlowCallback, + this, ros::TransportHints().tcpNoDelay()); + vl_sub_ = + private_nh_.subscribe(TOPIC_LOCALIZATION_ML_FEATURES, kMaxNumVLMsgs, + &GraphLocalizerNodelet::VLVisualLandmarksCallback, this, ros::TransportHints().tcpNoDelay()); + 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_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(); + } + 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) { + graph_localizer_wrapper_.ResetBiasesAndLocalizer(); + PublishReset(); + EnableLocalizer(); + return true; +} + +bool GraphLocalizerNodelet::ResetBiasesFromFileAndResetLocalizer(std_srvs::Empty::Request& req, + std_srvs::Empty::Response& res) { + return ResetBiasesFromFileAndResetLocalizer(); +} + +bool GraphLocalizerNodelet::ResetBiasesFromFileAndResetLocalizer() { + 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() { + 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, sparse_mapping_min_num_landmarks_)) 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, ar_min_num_landmarks_)) PublishARTagPose(); +} + +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::PublishLocalizationState() { + const 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; + } + 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) { + LogWarning("PublishARTagPose: Failed to get latest ar tag pose msg."); + return; + } + ar_tag_pose_pub_.publish(*latest_ar_tag_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_); + transform_pub_.sendTransform(world_T_dock_tf); +} + +void GraphLocalizerNodelet::PublishReset() const { + std_msgs::Empty msg; + reset_pub_.publish(msg); +} + +void GraphLocalizerNodelet::PublishHeartbeat() { + heartbeat_.header.stamp = ros::Time::now(); + heartbeat_pub_.publish(heartbeat_); +} + +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()) { + callbacks_timer_.Start(); + private_queue_.callAvailable(); + callbacks_timer_.StopAndVlogEveryN(100, 2); + graph_localizer_wrapper_.Update(); + PublishGraphMessages(); + PublishHeartbeat(); + rate.sleep(); + } +} +} // namespace graph_localizer + +PLUGINLIB_EXPORT_CLASS(graph_localizer::GraphLocalizerNodelet, nodelet::Nodelet); diff --git a/localization/graph_localizer/src/graph_localizer_wrapper.cc b/localization/graph_localizer/src/graph_localizer_wrapper.cc new file mode 100644 index 0000000000..f9a505f09d --- /dev/null +++ b/localization/graph_localizer/src/graph_localizer_wrapper.cc @@ -0,0 +1,318 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace graph_localizer { +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) { + config_reader::ConfigReader config; + lc::LoadGraphLocalizerConfig(config, graph_config_path_prefix); + config.AddFile("transforms.config"); + config.AddFile("cameras.config"); + config.AddFile("geometry.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) { + if (graph_localizer_) { + if (graph_localizer_->AddOpticalFlowMeasurement(lm::MakeFeaturePointsMeasurement(feature_array_msg))) { + feature_counts_.of = graph_localizer_->NumOFFactors(); + } + } +} + +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_->first, 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(); + graph_localizer_.reset(); + sanity_checker_->Reset(); +} + +void GraphLocalizerWrapper::VLVisualLandmarksCallback(const ff_msgs::VisualLandmarks& visual_landmarks_msg) { + if (!ValidVLMsg(visual_landmarks_msg, sparse_mapping_min_num_landmarks_)) return; + if (graph_localizer_) { + graph_localizer_->AddSparseMappingMeasurement(lm::MakeMatchedProjectionsMeasurement(visual_landmarks_msg)); + feature_counts_.vl = visual_landmarks_msg.landmarks.size(); + } + + const gtsam::Pose3 sparse_mapping_global_T_body = + lc::GtPose(visual_landmarks_msg, graph_localizer_initializer_.params().calibration.body_T_nav_cam.inverse()); + const lc::Time timestamp = lc::TimeFromHeader(visual_landmarks_msg.header); + sparse_mapping_pose_ = std::make_pair(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_->first, sparse_mapping_pose_->second); + } +} + +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) { + 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_ = std::make_pair(frame_changed_ar_measurements.global_T_cam * + graph_localizer_initializer_.params().calibration.body_T_dock_cam.inverse(), + frame_changed_ar_measurements.timestamp); + // TODO(rsoussan): Make seperate ar count, update GraphState and EkfState + feature_counts_.vl = visual_landmarks_msg.landmarks.size(); + } +} + +void GraphLocalizerWrapper::ImuCallback(const sensor_msgs::Imu& imu_msg) { + if (graph_localizer_) { + graph_localizer_->AddImuMeasurement(lm::ImuMeasurement(imu_msg)); + latest_biases_ = graph_localizer_->LatestBiases(); + if (!latest_biases_) { + LogError("ImuCallback: Failed to get latest biases."); + } + } else if (graph_localizer_initializer_.EstimateBiases()) { + graph_localizer_initializer_.EstimateAndSetImuBiases(lm::ImuMeasurement(imu_msg)); + } + + if (!graph_localizer_ && graph_localizer_initializer_.ReadyToInitialize()) { + InitializeGraph(); + LogDebug("ImuCallback: Initialized Graph."); + } +} + +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 GraphLocalizerWrapper::feature_tracks() const { + if (!graph_localizer_) return boost::none; + return graph_localizer_->feature_tracks(); +} + +void GraphLocalizerWrapper::MarkWorldTDockForResettingIfNecessary() { + if (estimate_world_T_dock_using_loc_) reset_world_T_dock_ = 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::GtPose(visual_landmarks_msg, graph_localizer_initializer_.params().calibration.body_T_dock_cam.inverse()); + estimated_world_T_dock_ = latest_combined_nav_state->pose() * dock_T_body.inverse(); +} + +gtsam::Pose3 GraphLocalizerWrapper::estimated_world_T_dock() const { return estimated_world_T_dock_; } + +boost::optional 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_->first, sparse_mapping_pose_->second); +} + +boost::optional 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_->first, ar_tag_pose_->second); +} + +boost::optional 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 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_.of, + feature_counts_.vl, graph_localizer_initializer_.EstimateBiases(), position_cov_log_det_lost_threshold_, + orientation_cov_log_det_lost_threshold_, graph_localizer_->standstill(), graph_localizer_->graph_stats()); + feature_counts_.Reset(); + return graph_state_msg; +} + +boost::optional 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 GraphLocalizerWrapper::graph_stats() const { + if (!graph_localizer_) { + LogDebug("GraphStats: Failed to get graph stats."); + return boost::none; + } + return graph_localizer_->graph_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/graph_stats.cc b/localization/graph_localizer/src/graph_stats.cc new file mode 100644 index 0000000000..5cbae24555 --- /dev/null +++ b/localization/graph_localizer/src/graph_stats.cc @@ -0,0 +1,169 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include +#include + +#include +#include + +namespace graph_localizer { +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_); + + stats_averagers_.emplace_back(iterations_averager_); + stats_averagers_.emplace_back(num_states_averager_); + stats_averagers_.emplace_back(duration_averager_); + stats_averagers_.emplace_back(num_optical_flow_factors_averager_); + stats_averagers_.emplace_back(num_loc_pose_factors_averager_); + stats_averagers_.emplace_back(num_loc_proj_factors_averager_); + stats_averagers_.emplace_back(num_imu_factors_averager_); + stats_averagers_.emplace_back(num_rotation_factors_averager_); + stats_averagers_.emplace_back(num_standstill_between_factors_averager_); + stats_averagers_.emplace_back(num_vel_prior_factors_averager_); + stats_averagers_.emplace_back(num_marginal_factors_averager_); + stats_averagers_.emplace_back(num_factors_averager_); + stats_averagers_.emplace_back(num_features_averager_); + + error_averagers_.emplace_back(total_error_averager_); + error_averagers_.emplace_back(of_error_averager_); + error_averagers_.emplace_back(loc_proj_error_averager_); + error_averagers_.emplace_back(loc_pose_error_averager_); + error_averagers_.emplace_back(imu_error_averager_); + error_averagers_.emplace_back(rotation_error_averager_); + error_averagers_.emplace_back(standstill_between_error_averager_); + error_averagers_.emplace_back(pose_prior_error_averager_); + error_averagers_.emplace_back(velocity_prior_error_averager_); + error_averagers_.emplace_back(bias_prior_error_averager_); +} + +void GraphStats::UpdateErrors(const GraphLocalizer& graph) { + log_error_timer_.Start(); + double total_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.factor_graph()) { + const double error = factor->error(graph.graph_values().values()); + total_error += error; + const auto smart_factor = dynamic_cast(factor.get()); + if (smart_factor) { + optical_flow_factor_error += error; + } + const auto projection_factor = dynamic_cast(factor.get()); + if (projection_factor) { + optical_flow_factor_error += error; + } + const auto imu_factor = dynamic_cast(factor.get()); + if (imu_factor) { + imu_factor_error += error; + } + const auto loc_factor = dynamic_cast*>(factor.get()); + if (loc_factor) { + loc_proj_error += error; + } + const auto loc_pose_factor = dynamic_cast(factor.get()); + if (loc_pose_factor) { + loc_pose_error += error; + } + const auto rotation_factor = dynamic_cast(factor.get()); + if (rotation_factor) { + rotation_factor_error += error; + } + const auto standstill_between_factor = dynamic_cast*>(factor.get()); + if (standstill_between_factor) { + standstill_between_factor_error += error; + } + + // Prior Factors + const auto pose_prior_factor = dynamic_cast*>(factor.get()); + if (pose_prior_factor && !loc_pose_factor) { + pose_prior_error += error; + } + const auto velocity_prior_factor = dynamic_cast*>(factor.get()); + if (velocity_prior_factor) { + velocity_prior_error += error; + } + const auto bias_prior_factor = dynamic_cast*>(factor.get()); + if (bias_prior_factor) { + bias_prior_error += error; + } + } + total_error_averager_.Update(total_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); + log_error_timer_.Stop(); +} + +void GraphStats::UpdateStats(const GraphLocalizer& graph) { + log_stats_timer_.Start(); + num_states_averager_.Update(graph.graph_values().NumStates()); + duration_averager_.Update(graph.graph_values().Duration()); + num_optical_flow_factors_averager_.Update(graph.NumOFFactors()); + num_loc_pose_factors_averager_.Update(graph.NumFactors()); + num_loc_proj_factors_averager_.Update(graph.NumFactors>()); + num_imu_factors_averager_.Update(graph.NumFactors()); + num_rotation_factors_averager_.Update(graph.NumFactors()); + num_standstill_between_factors_averager_.Update(graph.NumFactors>()); + num_vel_prior_factors_averager_.Update(graph.NumFactors>()); + num_marginal_factors_averager_.Update(graph.NumFactors()); + if (graph.params().factor.projection_adder.enabled) num_features_averager_.Update(graph.NumFeatures()); + num_factors_averager_.Update(graph.factor_graph().size()); + log_stats_timer_.Stop(); +} + +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_localizer diff --git a/localization/graph_localizer/src/graph_values.cc b/localization/graph_localizer/src/graph_values.cc new file mode 100644 index 0000000000..18fdca5625 --- /dev/null +++ b/localization/graph_localizer/src/graph_values.cc @@ -0,0 +1,494 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace graph_localizer { +namespace lc = localization_common; +namespace lm = localization_measurements; +GraphValues::GraphValues(const GraphValuesParams& params) : params_(params), feature_key_index_(0) { + LogDebug("GraphValues: Window duration: " << params_.ideal_duration); + LogDebug("GraphValues: Window min num states: " << params_.min_num_states); +} + +boost::optional GraphValues::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 GraphValues::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 GraphValues::OldestTimestamp() const { + if (Empty()) { + LogError("OldestTimestamp: No states available."); + return boost::none; + } + return timestamp_key_index_map_.cbegin()->first; +} + +boost::optional GraphValues::LatestTimestamp() const { + if (Empty()) { + LogError("LatestTimestamp: No states available."); + return boost::none; + } + return timestamp_key_index_map_.crbegin()->first; +} + +boost::optional GraphValues::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> GraphValues::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(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(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 GraphValues::GetKey(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 GraphValues::HasKey(const lc::Time timestamp) const { return (timestamp_key_index_map_.count(timestamp) != 0); } + +bool GraphValues::Empty() const { return timestamp_key_index_map_.empty(); } + +boost::optional GraphValues::PoseKey(const lc::Time timestamp) const { return GetKey(&sym::P, timestamp); } + +boost::optional GraphValues::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(sym::P(*key_index)); + if (!pose) { + LogError("GetCombinedNavState: Failed to get pose for key index."); + return boost::none; + } + + const auto velocity = at(sym::V(*key_index)); + if (!velocity) { + LogError("GetCombinedNavState: Failed to get velocity for key index."); + return boost::none; + } + + const auto bias = at(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 GraphValues::Duration() const { + if (Empty()) return 0; + return (*LatestTimestamp() - *OldestTimestamp()); +} + +int GraphValues::NumStates() const { return timestamp_key_index_map_.size(); } + +boost::optional GraphValues::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; +} + +bool GraphValues::HasFeature(const lm::FeatureId id) const { return (feature_id_key_map_.count(id) > 0); } + +boost::optional GraphValues::FeatureKey(const lm::FeatureId id) const { + if (!HasFeature(id)) return boost::none; + return feature_id_key_map_.at(id); +} + +gtsam::Key GraphValues::CreateFeatureKey() const { return sym::F(++feature_key_index_); } + +gtsam::KeyVector GraphValues::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 GraphValues::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; +} + +boost::optional GraphValues::LatestCombinedNavStateKeyIndex() const { + if (Empty()) { + LogError("LatestCombinedNavStateKeyIndex: No combined nav states available."); + return boost::none; + } + return timestamp_key_index_map_.crbegin()->second; +} + +int GraphValues::NumFeatures() const { return feature_id_key_map_.size(); } + +boost::optional GraphValues::OldestCombinedNavStateKeyIndex() const { + if (Empty()) { + LogError("OldestCombinedNavStateKeyIndex: No combined nav states available."); + return boost::none; + } + return timestamp_key_index_map_.cbegin()->second; +} + +boost::optional> GraphValues::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(sym::B(key_index)); + if (!bias) { + LogError("LatestBias: Failed to get bias at key index."); + return boost::none; + } + + return std::pair{*bias, timestamp}; +} + +boost::optional GraphValues::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 GraphValues::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 GraphValues::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 GraphValues::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 GraphValues::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); +} + +void GraphValues::UpdateValues(const gtsam::Values& new_values) { values_ = new_values; } + +gtsam::NonlinearFactorGraph GraphValues::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; +} + +int GraphValues::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 GraphValues::OldKeys(const lc::Time oldest_allowed_time) 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 GraphValues::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; +} + +gtsam::KeyVector GraphValues::OldFeatureKeys(const gtsam::NonlinearFactorGraph& factors) const { + using ProjectionFactor = gtsam::GenericProjectionFactor; + 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(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) { + old_features.emplace_back(key); + } + } + return old_features; +} + +void GraphValues::RemoveOldFeatures(const gtsam::KeyVector& old_keys) { + for (const auto& key : old_keys) { + 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/loc_factor_adder.cc b/localization/graph_localizer/src/loc_factor_adder.cc new file mode 100644 index 0000000000..0cfb7b6498 --- /dev/null +++ b/localization/graph_localizer/src/loc_factor_adder.cc @@ -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. + */ + +#include +#include +#include +#include +#include +#include + +#include + +namespace graph_localizer { +namespace lm = localization_measurements; +namespace sym = gtsam::symbol_shorthand; +LocFactorAdder::LocFactorAdder(const LocFactorAdderParams& params, const GraphAction projection_graph_action) + : LocFactorAdder::Base(params), projection_graph_action_(projection_graph_action) {} + +std::vector LocFactorAdder::AddFactors( + const lm::MatchedProjectionsMeasurement& matched_projections_measurement) { + if (matched_projections_measurement.matched_projections.empty()) { + LogDebug("AddFactors: Empty measurement."); + return {}; + } + + if (static_cast(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 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(num_landmarks)), 2); + } + + 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(noise_scale * pose_prior_noise_sigmas)), + params().huber_k); + + const KeyInfo key_info(&sym::P, 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(num_landmarks)), 2); + } + + int num_loc_projection_factors = 0; + FactorsToAdd projection_factors_to_add(projection_graph_action_); + projection_factors_to_add.reserve(matched_projections_measurement.matched_projections.size()); + for (const auto& matched_projection : matched_projections_measurement.matched_projections) { + const KeyInfo key_info(&sym::P, 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; + } + 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; +} +} // namespace graph_localizer diff --git a/localization/graph_localizer/src/parameter_reader.cc b/localization/graph_localizer/src/parameter_reader.cc new file mode 100644 index 0000000000..8bc60f7a22 --- /dev/null +++ b/localization/graph_localizer/src/parameter_reader.cc @@ -0,0 +1,215 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include +#include + +namespace graph_localizer { +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.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) { + 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 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.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 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.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.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.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"); +} + +void LoadGraphValuesParams(config_reader::ConfigReader& config, GraphValuesParams& 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"); + params.min_num_factors_per_feature = mc::LoadInt(config, "min_num_factors_per_feature"); +} + +void LoadNoiseParams(config_reader::ConfigReader& config, NoiseParams& 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"); +} + +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 LoadGraphLocalizerParams(config_reader::ConfigReader& config, GraphLocalizerParams& params) { + LoadCalibrationParams(config, params.calibration); + LoadGraphInitializerParams(config, params.graph_initializer); + LoadFactorParams(config, params.factor); + LoadFeatureTrackerParams(config, params.feature_tracker); + LoadGraphValuesParams(config, params.graph_values); + LoadNoiseParams(config, params.noise); + params.verbose = mc::LoadBool(config, "verbose"); + params.fatal_failures = mc::LoadBool(config, "fatal_failures"); + 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.limit_imu_factor_spacing = mc::LoadBool(config, "limit_imu_factor_spacing"); + params.max_imu_factor_spacing = mc::LoadDouble(config, "max_imu_factor_spacing"); + params.add_priors = mc::LoadBool(config, "add_priors"); + params.add_marginal_factors = mc::LoadBool(config, "add_marginal_factors"); + 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.log_rate = mc::LoadInt(config, "log_rate"); + params.estimate_world_T_dock_using_loc = mc::LoadBool(config, "estimate_world_T_dock_using_loc"); +} +} // namespace graph_localizer diff --git a/localization/graph_localizer/src/projection_factor_adder.cc b/localization/graph_localizer/src/projection_factor_adder.cc new file mode 100644 index 0000000000..fa3ca76b66 --- /dev/null +++ b/localization/graph_localizer/src/projection_factor_adder.cc @@ -0,0 +1,89 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include + +#include +#include + +namespace graph_localizer { +namespace lm = localization_measurements; +namespace sym = gtsam::symbol_shorthand; +ProjectionFactorAdder::ProjectionFactorAdder(const ProjectionFactorAdderParams& params, + std::shared_ptr feature_tracker, + std::shared_ptr graph_values) + : ProjectionFactorAdder::Base(params), feature_tracker_(feature_tracker), graph_values_(graph_values) {} + +std::vector ProjectionFactorAdder::AddFactors( + const lm::FeaturePointsMeasurement& feature_points_measurement) { + std::vector factors_to_add_vec; + // Add projection factors for new measurements of already existing features + FactorsToAdd projection_factors_to_add; + for (const auto& feature_point : feature_points_measurement.feature_points) { + if (graph_values_->HasFeature(feature_point.feature_id)) { + const KeyInfo pose_key_info(&sym::P, feature_point.timestamp); + const KeyInfo static_point_key_info(&sym::F, feature_point.feature_id); + const auto point_key = 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( + 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(feature_track.points.size()) >= params().min_num_measurements_for_triangulation && + !graph_values_->HasFeature(feature_track.id) && + (new_features + 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 + FactorsToAdd projection_factors_with_new_point_to_add(GraphAction::kTriangulateNewPoint); + const auto point_key = graph_values_->CreateFeatureKey(); + for (const auto& feature_point : feature_track.points) { + const KeyInfo pose_key_info(&sym::P, feature_point.timestamp); + const KeyInfo static_point_key_info(&sym::F, feature_point.feature_id); + const auto projection_factor = boost::make_shared( + 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/rotation_factor_adder.cc b/localization/graph_localizer/src/rotation_factor_adder.cc new file mode 100644 index 0000000000..e3a95fb8b3 --- /dev/null +++ b/localization/graph_localizer/src/rotation_factor_adder.cc @@ -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. + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace graph_localizer { +namespace lm = localization_measurements; +namespace sym = gtsam::symbol_shorthand; +RotationFactorAdder::RotationFactorAdder(const RotationFactorAdderParams& params, + std::shared_ptr feature_tracker) + : RotationFactorAdder::Base(params), feature_tracker_(feature_tracker) {} + +std::vector RotationFactorAdder::AddFactors(const lm::FeaturePointsMeasurement& measurement) { + std::vector points_1; + std::vector 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.points.size() < 2) continue; + // Get points for most recent and second to most recent images + const auto& point_1 = feature_track.points[feature_track.points.size() - 2].image_point; + const auto& point_2 = feature_track.points.back().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(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(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 KeyInfo pose_1_key_info(&sym::P, *(feature_tracker_->PreviousTimestamp())); + const KeyInfo pose_2_key_info(&sym::P, 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(rotation_noise_sigmas)), params().huber_k); + const auto rotation_factor = boost::make_shared( + body_1_R_body_2, rotation_noise, pose_1_key_info.UninitializedKey(), pose_2_key_info.UninitializedKey()); + 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 new file mode 100644 index 0000000000..de9d4b9097 --- /dev/null +++ b/localization/graph_localizer/src/sanity_checker.cc @@ -0,0 +1,55 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include + +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 { + bool sane = true; + if (params_.check_position_covariance) { + const double log_det_position_cov = covariances.LogDeterminantPositionCovariance(); + sane &= (log_det_position_cov <= params_.position_covariance_threshold); + } + + if (params_.check_orientation_covariance) { + const double log_det_orientation_cov = covariances.LogDeterminantOrientationCovariance(); + sane &= (log_det_orientation_cov <= params_.orientation_covariance_threshold); + } + + return sane; +} + +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 new file mode 100644 index 0000000000..44c046acdf --- /dev/null +++ b/localization/graph_localizer/src/serialization.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 +#include + +#include + +#include + +// Value types used in GraphValues +#include +#include +#include +#include +#include +#include +#include + +// Factors used in NonlinearFactorGraph +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// 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_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; +BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor, "gtsam::PriorFactorPose3"); +BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor, "gtsam::PriorFactorVector3"); +BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor, "gtsam::PriorFactorConstantBias"); +BOOST_CLASS_EXPORT_GUID(LocProjectionFactor, "gtsam::LocProjectionFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::LocPoseFactor, "gtsam::LocPoseFactor"); +using ProjectionFactor = gtsam::GenericProjectionFactor; +BOOST_CLASS_EXPORT_GUID(ProjectionFactor, "gtsam::GenericProjectionFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::RobustSmartProjectionPoseFactor, + "gtsam::RobustSmartProjectionPoseFactorCal3_S2"); +BOOST_CLASS_EXPORT_GUID(gtsam::SmartProjectionPoseFactor, "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::BetweenFactorPose3"); +BOOST_CLASS_EXPORT_GUID(gtsam::PoseRotationFactor, "gtsam::PoseRotationFactor"); + +// 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 new file mode 100644 index 0000000000..63fad52bbe --- /dev/null +++ b/localization/graph_localizer/src/smart_projection_cumulative_factor_adder.cc @@ -0,0 +1,89 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include + +#include +#include + +namespace graph_localizer { +namespace lm = localization_measurements; +namespace sym = gtsam::symbol_shorthand; +SmartProjectionCumulativeFactorAdder::SmartProjectionCumulativeFactorAdder( + const SmartProjectionFactorAdderParams& params, std::shared_ptr 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); + smart_projection_params_.setEnableEPI(params.enable_EPI); +} + +std::vector SmartProjectionCumulativeFactorAdder::AddFactors() { + // Add smart factor for each valid feature track + FactorsToAdd smart_factors_to_add(GraphAction::kDeleteExistingSmartFactors); + int num_added_smart_factors = 0; + for (const auto& feature_track : feature_tracker_->feature_tracks()) { + const double average_distance_from_mean = AverageDistanceFromMean(feature_track.second.points); + if (ValidPointSet(feature_track.second.points, average_distance_from_mean, params().min_avg_distance_from_mean, + params().min_num_points) && + num_added_smart_factors < params().max_num_factors) { + AddSmartFactor(feature_track.second, smart_factors_to_add); + ++num_added_smart_factors; + } + } + + if (smart_factors_to_add.empty()) return {}; + const auto latest_timestamp = feature_tracker_->latest_timestamp(); + 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 FeatureTrack& feature_track, + FactorsToAdd& smart_factors_to_add) const { + SharedRobustSmartFactor smart_factor; + const int num_feature_track_points = feature_track.points.size(); + const auto noise = params().scale_noise_with_num_points + ? gtsam::noiseModel::Isotropic::Sigma( + 2, params().noise_scale * num_feature_track_points * params().cam_noise->sigma()) + : params().cam_noise; + smart_factor = + boost::make_shared(noise, params().cam_intrinsics, params().body_T_cam, smart_projection_params_, + params().rotation_only_fallback, params().robust, params().huber_k); + + 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 (const auto& feature_point : feature_track.points) { + const KeyInfo key_info(&sym::P, 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}); +} +} // namespace graph_localizer diff --git a/localization/graph_localizer/src/standstill_factor_adder.cc b/localization/graph_localizer/src/standstill_factor_adder.cc new file mode 100644 index 0000000000..9f837d74f9 --- /dev/null +++ b/localization/graph_localizer/src/standstill_factor_adder.cc @@ -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. + */ + +#include +#include +#include + +#include +#include +#include + +namespace graph_localizer { +namespace lm = localization_measurements; +namespace sym = gtsam::symbol_shorthand; +StandstillFactorAdder::StandstillFactorAdder(const StandstillFactorAdderParams& params, + std::shared_ptr feature_tracker) + : StandstillFactorAdder::Base(params), feature_tracker_(feature_tracker) {} + +std::vector StandstillFactorAdder::AddFactors( + const lm::FeaturePointsMeasurement& feature_points_measurement) { + std::vector factors_to_add; + if (params().add_velocity_prior) { + 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(velocity_prior_noise_sigmas)), + params().huber_k); + + const KeyInfo velocity_key_info(&sym::V, feature_points_measurement.timestamp); + gtsam::PriorFactor::shared_ptr velocity_prior_factor(new gtsam::PriorFactor( + 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) { + 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(pose_between_noise_sigmas)), + params().huber_k); + const KeyInfo previous_between_key_info(&sym::P, *previous_timestamp); + const KeyInfo current_between_key_info(&sym::P, feature_points_measurement.timestamp); + gtsam::BetweenFactor::shared_ptr pose_between_factor(new gtsam::BetweenFactor( + 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/utilities.cc b/localization/graph_localizer/src/utilities.cc new file mode 100644 index 0000000000..41f39a413c --- /dev/null +++ b/localization/graph_localizer/src/utilities.cc @@ -0,0 +1,253 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include +#include +#include + +#include +#include + +namespace graph_localizer { +namespace ii = imu_integration; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace mc = msg_conversions; + +bool ValidPointSet(const std::deque& points, const double average_distance_from_mean, + const double min_avg_distance_from_mean, const int min_num_points) { + if (static_cast(points.size()) < min_num_points) return false; + return (average_distance_from_mean >= min_avg_distance_from_mean); +} + +double AverageDistanceFromMean(const std::deque& 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(visual_landmarks_msg.landmarks.size()) >= min_num_landmarks); +} + +ff_msgs::GraphState GraphStateMsg(const lc::CombinedNavState& combined_nav_state, + const lc::CombinedNavStateCovariances& covariances, + const int num_optical_flow_features_in_last_measurement, + const int num_sparse_mapping_features_in_last_measurement, const bool estimating_bias, + const double position_log_det_threshold, const double orientation_log_det_threshold, + const bool standstill, const GraphStats& graph_stats) { + 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 + loc_msg.confidence = covariances.PoseConfidence(position_log_det_threshold, orientation_log_det_threshold); + + // Set Graph Feature Counts/Information + // TODO(rsoussan): Change msg to use int instead of uint8_t to avoid this overflow issue + loc_msg.of_count = + num_optical_flow_features_in_last_measurement <= 255 ? num_optical_flow_features_in_last_measurement : 255; + loc_msg.ml_count = + num_sparse_mapping_features_in_last_measurement <= 255 ? num_sparse_mapping_features_in_last_measurement : 255; + 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_states = graph_stats.num_states_averager_.last_value(); + loc_msg.standstill = standstill; + 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); +} + +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 FixSmartFactorByRemovingIndividualMeasurements( + const GraphLocalizerParams& params, const RobustSmartFactor& smart_factor, + const gtsam::SmartProjectionParams& smart_projection_params, const GraphValues& 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::MeasurementVector measurements_to_add; + gtsam::KeyVector keys_to_add; + for (int i = 0; i < static_cast(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( + params.factor.smart_projection_adder.cam_noise, params.factor.smart_projection_adder.cam_intrinsics, + params.factor.smart_projection_adder.body_T_cam, smart_projection_params, + params.factor.smart_projection_adder.rotation_only_fallback, params.factor.smart_projection_adder.robust, + params.factor.smart_projection_adder.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 FixSmartFactorByRemovingMeasurementSequence( + const GraphLocalizerParams& params, const RobustSmartFactor& smart_factor, + const gtsam::SmartProjectionParams& smart_projection_params, const GraphValues& 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::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( + params.factor.smart_projection_adder.cam_noise, params.factor.smart_projection_adder.cam_intrinsics, + params.factor.smart_projection_adder.body_T_cam, smart_projection_params, + params.factor.smart_projection_adder.rotation_only_fallback, params.factor.smart_projection_adder.robust, + params.factor.smart_projection_adder.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::MeasurementVector measurements_to_add; + gtsam::KeyVector keys_to_add; + for (int i = num_measurements_to_add; + i >= static_cast(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( + params.factor.smart_projection_adder.cam_noise, params.factor.smart_projection_adder.cam_intrinsics, + params.factor.smart_projection_adder.body_T_cam, smart_projection_params, + params.factor.smart_projection_adder.rotation_only_fallback, params.factor.smart_projection_adder.robust, + params.factor.smart_projection_adder.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& factor_key_indices_to_remove, + const SmartProjectionFactorAdderParams& params, + const gtsam::SmartProjectionParams& smart_projection_params) { + auto new_smart_factor = boost::make_shared( + 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(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; +} +} // namespace graph_localizer diff --git a/localization/graph_localizer/test/test_rotation_factor.cc b/localization/graph_localizer/test/test_rotation_factor.cc new file mode 100644 index 0000000000..98abb72b3e --- /dev/null +++ b/localization/graph_localizer/test/test_rotation_factor.cc @@ -0,0 +1,53 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include + +#include +#include +#include +#include + +#include + +namespace sym = gtsam::symbol_shorthand; +TEST(RotationFactorTester, Jacobian) { + const gtsam::Pose3 world_T_body_1; + const gtsam::Point3 body_1_t_body_2(0.1, 0.1, 0.1); + const gtsam::Rot3 body_1_R_body_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 body_1_T_body_2(body_1_R_body_2, body_1_t_body_2); + const gtsam::Pose3 world_T_body_2 = world_T_body_1 * body_1_T_body_2; + // Use body_1_T_body_2 as a pertubation + const gtsam::Pose3 world_T_perturbed_body_2 = world_T_body_2 * body_1_T_body_2; + const auto noise = gtsam::noiseModel::Unit::Create(3); + const gtsam::PoseRotationFactor rotation_factor(body_1_R_body_2, noise, sym::P(0), sym::P(1)); + gtsam::Matrix factor_H1, factor_H2; + const auto factor_error = + rotation_factor.evaluateError(world_T_body_1, world_T_perturbed_body_2, factor_H1, factor_H2); + const auto numerical_H1 = gtsam::numericalDerivative21( + boost::function( + boost::bind(>sam::PoseRotationFactor::evaluateError, rotation_factor, _1, _2, boost::none, boost::none)), + world_T_body_1, world_T_perturbed_body_2, 1e-5); + ASSERT_TRUE(numerical_H1.isApprox(factor_H1.matrix(), 1e-6)); + const auto numerical_H2 = gtsam::numericalDerivative22( + boost::function( + boost::bind(>sam::PoseRotationFactor::evaluateError, rotation_factor, _1, _2, boost::none, boost::none)), + world_T_body_1, world_T_perturbed_body_2, 1e-5); + ASSERT_TRUE(numerical_H2.isApprox(factor_H2.matrix(), 1e-6)); +} diff --git a/localization/graph_localizer/test/test_rotation_factor.test b/localization/graph_localizer/test/test_rotation_factor.test new file mode 100644 index 0000000000..1535f0f22e --- /dev/null +++ b/localization/graph_localizer/test/test_rotation_factor.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/graph_localizer/test/test_rotation_factor_adder.cc b/localization/graph_localizer/test/test_rotation_factor_adder.cc new file mode 100644 index 0000000000..740c9de49c --- /dev/null +++ b/localization/graph_localizer/test/test_rotation_factor_adder.cc @@ -0,0 +1,153 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +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 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 cam_1( + world_T_cam_1, boost::make_shared(rotation_factor_adder_params_.nav_cam_intrinsics)); + const gtsam::PinholePose cam_2( + world_T_cam_2, boost::make_shared(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 feature_tracker_; + std::unique_ptr 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(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(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 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 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(sym::P(1)); + const auto optimized_world_T_body_2 = optimized_values.at(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)); +} diff --git a/localization/graph_localizer/test/test_rotation_factor_adder.test b/localization/graph_localizer/test/test_rotation_factor_adder.test new file mode 100644 index 0000000000..2ff3ec849b --- /dev/null +++ b/localization/graph_localizer/test/test_rotation_factor_adder.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/ground_truth_localizer/CMakeLists.txt b/localization/ground_truth_localizer/CMakeLists.txt new file mode 100644 index 0000000000..9d1d878271 --- /dev/null +++ b/localization/ground_truth_localizer/CMakeLists.txt @@ -0,0 +1,38 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +project(ground_truth_localizer) + +if (USE_ROS) + +# include ff_nodelet to get ff_util header files since these aren't exposed elsewhere +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} config_reader ff_nodelet localization_common + INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS roscpp + DEPENDS ff_msgs +) + +create_library(TARGET ${PROJECT_NAME} + LIBS ${catkin_LIBRARIES} ${GLOG_LIBRARIES} config_reader ff_nodelet localization_common + INC ${catkin_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} + DEPS ff_msgs +) + +install_launch_files() + +endif (USE_ROS) diff --git a/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h b/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h new file mode 100644 index 0000000000..eec5741255 --- /dev/null +++ b/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h @@ -0,0 +1,72 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 GROUND_TRUTH_LOCALIZER_GROUND_TRUTH_LOCALIZER_NODELET_H_ +#define GROUND_TRUTH_LOCALIZER_GROUND_TRUTH_LOCALIZER_NODELET_H_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +namespace ground_truth_localizer { +class GroundTruthLocalizerNodelet : public ff_util::FreeFlyerNodelet { + public: + GroundTruthLocalizerNodelet(); + + private: + void Initialize(ros::NodeHandle* nh); + + void SubscribeAndAdvertise(ros::NodeHandle* nh); + + bool SetMode(ff_msgs::SetEkfInput::Request& req, ff_msgs::SetEkfInput::Response& res); + + void PoseCallback(geometry_msgs::PoseStamped::ConstPtr const& pose); + + void TwistCallback(geometry_msgs::TwistStamped::ConstPtr const& twist); + + void PublishLocState(const localization_common::Time& timestamp); + + std::string platform_name_; + boost::optional pose_; + boost::optional twist_; + int input_mode_ = ff_msgs::SetEkfInputRequest::MODE_TRUTH; + ros::Subscriber pose_sub_, twist_sub_; + ros::Publisher state_pub_, pose_pub_, twist_pub_, heartbeat_pub_; + ff_msgs::Heartbeat heartbeat_; + tf2_ros::TransformBroadcaster transform_pub_; + ros::ServiceServer input_mode_srv_; +}; +} // namespace ground_truth_localizer + +#endif // GROUND_TRUTH_LOCALIZER_GROUND_TRUTH_LOCALIZER_NODELET_H_ diff --git a/localization/ground_truth_localizer/include/ground_truth_localizer/twist.h b/localization/ground_truth_localizer/include/ground_truth_localizer/twist.h new file mode 100644 index 0000000000..91f06b29af --- /dev/null +++ b/localization/ground_truth_localizer/include/ground_truth_localizer/twist.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 GROUND_TRUTH_LOCALIZER_TWIST_H_ +#define GROUND_TRUTH_LOCALIZER_TWIST_H_ + +#include + +namespace ground_truth_localizer { +struct Twist { + Eigen::Vector3d linear_velocity; + Eigen::Vector3d angular_velocity; +}; +} // namespace ground_truth_localizer + +#endif // GROUND_TRUTH_LOCALIZER_TWIST_H_ diff --git a/localization/ground_truth_localizer/include/ground_truth_localizer/utilities.h b/localization/ground_truth_localizer/include/ground_truth_localizer/utilities.h new file mode 100644 index 0000000000..2e5dd75558 --- /dev/null +++ b/localization/ground_truth_localizer/include/ground_truth_localizer/utilities.h @@ -0,0 +1,37 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 GROUND_TRUTH_LOCALIZER_UTILITIES_H_ +#define GROUND_TRUTH_LOCALIZER_UTILITIES_H_ + +#include +#include +#include + +#include +#include +#include + +#include + +namespace ground_truth_localizer { +Eigen::Isometry3d PoseFromMsg(const geometry_msgs::PoseStamped& pose_msg); +Twist TwistFromMsg(const geometry_msgs::TwistStamped& twist_msg); +ff_msgs::EkfState LocStateMsg(const Eigen::Isometry3d& pose, const Twist& twist, const localization_common::Time& time); +} // namespace ground_truth_localizer + +#endif // GROUND_TRUTH_LOCALIZER_UTILITIES_H_ diff --git a/localization/ground_truth_localizer/launch/ground_truth_localizer.launch b/localization/ground_truth_localizer/launch/ground_truth_localizer.launch new file mode 100644 index 0000000000..469707ae7f --- /dev/null +++ b/localization/ground_truth_localizer/launch/ground_truth_localizer.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/ground_truth_localizer/nodelet_plugins.xml b/localization/ground_truth_localizer/nodelet_plugins.xml new file mode 100644 index 0000000000..c43d1234dc --- /dev/null +++ b/localization/ground_truth_localizer/nodelet_plugins.xml @@ -0,0 +1,11 @@ + + + + This nodelet wraps ground truth localizer + + + + + diff --git a/localization/ground_truth_localizer/package.xml b/localization/ground_truth_localizer/package.xml new file mode 100644 index 0000000000..842b4c5d22 --- /dev/null +++ b/localization/ground_truth_localizer/package.xml @@ -0,0 +1,26 @@ + + ground_truth_localizer + 1.0.0 + + The ground_truth localizer package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + ff_msgs + nodelet + roscpp + ff_msgs + nodelet + roscpp + + + + diff --git a/localization/ground_truth_localizer/readme.md b/localization/ground_truth_localizer/readme.md new file mode 100644 index 0000000000..c57a676be9 --- /dev/null +++ b/localization/ground_truth_localizer/readme.md @@ -0,0 +1,5 @@ +\page groundtruthlocalizer Ground Truth Localizer + +# Package overview +Echos ground truth poses and twists. + diff --git a/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc b/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc new file mode 100644 index 0000000000..befe94c8a0 --- /dev/null +++ b/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc @@ -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. + */ + +#include +#include +#include +#include +#include + +#include + +namespace ground_truth_localizer { +namespace lc = localization_common; +GroundTruthLocalizerNodelet::GroundTruthLocalizerNodelet() + : ff_util::FreeFlyerNodelet(NODE_SIM_LOC, true), platform_name_(GetPlatform()) {} + +void GroundTruthLocalizerNodelet::Initialize(ros::NodeHandle* nh) { + ff_common::InitFreeFlyerApplication(getMyArgv()); + SubscribeAndAdvertise(nh); +} + +void GroundTruthLocalizerNodelet::SubscribeAndAdvertise(ros::NodeHandle* nh) { + pose_pub_ = nh->advertise(TOPIC_LOCALIZATION_POSE, 1); + twist_pub_ = nh->advertise(TOPIC_LOCALIZATION_TWIST, 1); + state_pub_ = nh->advertise(TOPIC_GNC_EKF, 1); + heartbeat_pub_ = nh->advertise(TOPIC_HEARTBEAT, 5, true); + + pose_sub_ = nh->subscribe(TOPIC_LOCALIZATION_TRUTH, 1, &GroundTruthLocalizerNodelet::PoseCallback, this, + ros::TransportHints().tcpNoDelay()); + twist_sub_ = nh->subscribe(TOPIC_LOCALIZATION_TRUTH_TWIST, 1, &GroundTruthLocalizerNodelet::TwistCallback, this, + ros::TransportHints().tcpNoDelay()); + + input_mode_srv_ = nh->advertiseService(SERVICE_GNC_EKF_SET_INPUT, &GroundTruthLocalizerNodelet::SetMode, this); +} + +bool GroundTruthLocalizerNodelet::SetMode(ff_msgs::SetEkfInput::Request& req, ff_msgs::SetEkfInput::Response& res) { + input_mode_ = req.mode; + return true; +} + +void GroundTruthLocalizerNodelet::PoseCallback(geometry_msgs::PoseStamped::ConstPtr const& pose) { + assert(pose->header.frame_id == "world"); + pose_ = PoseFromMsg(*pose); + pose_pub_.publish(pose); + 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 + // and this runs in place of them + heartbeat_.node = NODE_GRAPH_LOC; + heartbeat_pub_.publish(heartbeat_); + heartbeat_.node = NODE_IMU_AUG; + heartbeat_pub_.publish(heartbeat_); +} + +void GroundTruthLocalizerNodelet::TwistCallback(geometry_msgs::TwistStamped::ConstPtr const& twist) { + assert(twist->header.frame_id == "world"); + twist_ = TwistFromMsg(*twist); + twist_pub_.publish(twist); + const lc::Time timestamp = lc::TimeFromHeader(twist->header); + PublishLocState(timestamp); +} + +void GroundTruthLocalizerNodelet::PublishLocState(const lc::Time& timestamp) { + if (!twist_ || !pose_) return; + const auto loc_state_msg = LocStateMsg(*pose_, *twist_, timestamp); + state_pub_.publish(loc_state_msg); + + // Also publish world_T_body TF + const auto world_T_body_tf = lc::PoseToTF(*pose_, "world", "body", timestamp, platform_name_); + transform_pub_.sendTransform(world_T_body_tf); +} +} // namespace ground_truth_localizer + +PLUGINLIB_EXPORT_CLASS(ground_truth_localizer::GroundTruthLocalizerNodelet, nodelet::Nodelet); diff --git a/localization/ground_truth_localizer/src/utilities.cc b/localization/ground_truth_localizer/src/utilities.cc new file mode 100644 index 0000000000..a58e5703f3 --- /dev/null +++ b/localization/ground_truth_localizer/src/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 +#include +#include + +namespace ground_truth_localizer { +namespace lc = localization_common; +namespace mc = msg_conversions; + +Eigen::Isometry3d PoseFromMsg(const geometry_msgs::PoseStamped& pose_msg) { + Eigen::Isometry3d pose = Eigen::Isometry3d::Identity(); + pose.translation() = mc::VectorFromMsg(pose_msg.pose.position); + pose.linear() = + mc::RotationFromMsg(pose_msg.pose.orientation).toRotationMatrix(); + return pose; +} + +Twist TwistFromMsg(const geometry_msgs::TwistStamped& twist_msg) { + Twist twist; + twist.linear_velocity = mc::VectorFromMsg(twist_msg.twist.linear); + twist.angular_velocity = mc::VectorFromMsg(twist_msg.twist.angular); + return twist; +} + +ff_msgs::EkfState LocStateMsg(const Eigen::Isometry3d& pose, const Twist& twist, + const localization_common::Time& timestamp) { + ff_msgs::EkfState loc_msg; + mc::EigenPoseToMsg(pose, loc_msg.pose); + mc::VectorToMsg(twist.linear_velocity, loc_msg.velocity); + mc::VectorToMsg(twist.angular_velocity, loc_msg.omega); + lc::TimeToHeader(timestamp, loc_msg.header); + loc_msg.confidence = lc::Confidence::kGood; + return loc_msg; +} +} // namespace ground_truth_localizer diff --git a/localization/imu_augmentor/CMakeLists.txt b/localization/imu_augmentor/CMakeLists.txt new file mode 100644 index 0000000000..2039fa6dee --- /dev/null +++ b/localization/imu_augmentor/CMakeLists.txt @@ -0,0 +1,37 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +project(imu_augmentor) + +if (USE_ROS) + +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} ff_nodelet imu_integration localization_common localization_measurements + INCLUDE_DIRS include ${GTSAM_INCLUDE_DIR} ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS + DEPENDS gtsam +) + +create_library(TARGET ${PROJECT_NAME} + LIBS ${catkin_LIBRARIES} ${GLOG_LIBRARIES} ff_nodelet imu_integration localization_common localization_measurements + INC ${catkin_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} + DEPS gtsam ff_msgs +) + +install_launch_files() + +endif (USE_ROS) diff --git a/localization/imu_augmentor/include/imu_augmentor/imu_augmentor.h b/localization/imu_augmentor/include/imu_augmentor/imu_augmentor.h new file mode 100644 index 0000000000..eefb45b064 --- /dev/null +++ b/localization/imu_augmentor/include/imu_augmentor/imu_augmentor.h @@ -0,0 +1,38 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_H_ +#define IMU_AUGMENTOR_IMU_AUGMENTOR_H_ + +#include +#include +#include + +#include + +namespace imu_augmentor { +class ImuAugmentor : public imu_integration::ImuIntegrator { + public: + explicit ImuAugmentor(const ImuAugmentorParams& params); + + boost::optional PimPredict( + const localization_common::CombinedNavState& combined_nav_state); +}; +} // namespace imu_augmentor + +#endif // IMU_AUGMENTOR_IMU_AUGMENTOR_H_ diff --git a/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_nodelet.h b/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_nodelet.h new file mode 100644 index 0000000000..ca6861d5ff --- /dev/null +++ b/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_nodelet.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 IMU_AUGMENTOR_IMU_AUGMENTOR_NODELET_H_ +#define IMU_AUGMENTOR_IMU_AUGMENTOR_NODELET_H_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include + +namespace imu_augmentor { +class ImuAugmentorNodelet : public ff_util::FreeFlyerNodelet { + public: + ImuAugmentorNodelet(); + + 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); + + boost::optional PublishLatestImuAugmentedLocalizationState(); + + void PublishPoseAndTwistAndTransform(const ff_msgs::EkfState& loc_msg); + + void Run(); + + void PublishHeartbeat(); + + imu_augmentor::ImuAugmentorWrapper imu_augmentor_wrapper_; + std::string platform_name_; + ros::NodeHandle imu_nh_, loc_nh_; + ros::CallbackQueue imu_queue_, loc_queue_; + ros::Subscriber imu_sub_, state_sub_; + ros::Publisher state_pub_, pose_pub_, twist_pub_, heartbeat_pub_; + ff_msgs::Heartbeat heartbeat_; + tf2_ros::TransformBroadcaster transform_pub_; +}; +} // namespace imu_augmentor + +#endif // IMU_AUGMENTOR_IMU_AUGMENTOR_NODELET_H_ diff --git a/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_params.h b/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_params.h new file mode 100644 index 0000000000..1af9af07e9 --- /dev/null +++ b/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_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 IMU_AUGMENTOR_IMU_AUGMENTOR_PARAMS_H_ +#define IMU_AUGMENTOR_IMU_AUGMENTOR_PARAMS_H_ + +#include + +namespace imu_augmentor { +struct ImuAugmentorParams : imu_integration::ImuIntegratorParams { + bool standstill_enabled; +}; +} // namespace imu_augmentor + +#endif // IMU_AUGMENTOR_IMU_AUGMENTOR_PARAMS_H_ diff --git a/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_wrapper.h b/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_wrapper.h new file mode 100644 index 0000000000..6fcac482ba --- /dev/null +++ b/localization/imu_augmentor/include/imu_augmentor/imu_augmentor_wrapper.h @@ -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. + */ +#ifndef IMU_AUGMENTOR_IMU_AUGMENTOR_WRAPPER_H_ +#define IMU_AUGMENTOR_IMU_AUGMENTOR_WRAPPER_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace imu_augmentor { +class ImuAugmentorWrapper { + public: + explicit ImuAugmentorWrapper(const std::string& graph_config_path_prefix = ""); + + void LocalizationStateCallback(const ff_msgs::GraphState& loc_msg); + + void ImuCallback(const sensor_msgs::Imu& imu_msg); + + boost::optional> + LatestImuAugmentedCombinedNavStateAndCovariances(); + + boost::optional LatestImuAugmentedLocalizationMsg(); + + private: + 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 imu_augmentor_; + boost::optional latest_combined_nav_state_; + boost::optional latest_covariances_; + boost::optional latest_loc_msg_; + std::unique_ptr preintegration_helper_; + ImuAugmentorParams params_; + boost::optional 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/launch/imu_augmentor.launch b/localization/imu_augmentor/launch/imu_augmentor.launch new file mode 100644 index 0000000000..fd7fabc34f --- /dev/null +++ b/localization/imu_augmentor/launch/imu_augmentor.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/imu_augmentor/nodelet_plugins.xml b/localization/imu_augmentor/nodelet_plugins.xml new file mode 100644 index 0000000000..8839d4225b --- /dev/null +++ b/localization/imu_augmentor/nodelet_plugins.xml @@ -0,0 +1,11 @@ + + + + This nodelet wraps the imu_augmentor + + + + + diff --git a/localization/imu_augmentor/package.xml b/localization/imu_augmentor/package.xml new file mode 100644 index 0000000000..0e0156dca1 --- /dev/null +++ b/localization/imu_augmentor/package.xml @@ -0,0 +1,28 @@ + + imu_augmentor + 1.0.0 + + Imu augmentor package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + catkin + ff_msgs + nodelet + roscpp + ff_msgs + nodelet + roscpp + + + + + diff --git a/localization/imu_augmentor/readme.md b/localization/imu_augmentor/readme.md new file mode 100644 index 0000000000..085c0cd436 --- /dev/null +++ b/localization/imu_augmentor/readme.md @@ -0,0 +1,16 @@ +\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 new file mode 100644 index 0000000000..abefad9fdb --- /dev/null +++ b/localization/imu_augmentor/src/imu_augmentor.cc @@ -0,0 +1,53 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include + +namespace imu_augmentor { +namespace ii = imu_integration; +namespace lc = localization_common; +namespace lm = localization_measurements; +ImuAugmentor::ImuAugmentor(const ImuAugmentorParams& params) : ii::ImuIntegrator(params) {} + +boost::optional ImuAugmentor::PimPredict(const lc::CombinedNavState& combined_nav_state) { + if (Empty()) return combined_nav_state; + // 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(combined_nav_state.timestamp()); + if (measurement_it == measurements().cend()) return combined_nav_state; + auto last_predicted_combined_nav_state = combined_nav_state; + auto pim = ii::Pim(last_predicted_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(last_predicted_combined_nav_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); + ++num_measurements_added; + } + + RemoveOldMeasurements(combined_nav_state.timestamp()); + LogDebug("PimPredict: Added " << num_measurements_added << " measurements."); + return last_predicted_combined_nav_state; +} +} // namespace imu_augmentor diff --git a/localization/imu_augmentor/src/imu_augmentor_nodelet.cc b/localization/imu_augmentor/src/imu_augmentor_nodelet.cc new file mode 100644 index 0000000000..c7e10a6f0f --- /dev/null +++ b/localization/imu_augmentor/src/imu_augmentor_nodelet.cc @@ -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. + */ + +#include +#include +#include +#include + +#include +#include + +namespace imu_augmentor { +namespace lc = localization_common; + +ImuAugmentorNodelet::ImuAugmentorNodelet() + : ff_util::FreeFlyerNodelet(NODE_IMU_AUG, true), platform_name_(GetPlatform()) { + imu_nh_.setCallbackQueue(&imu_queue_); + loc_nh_.setCallbackQueue(&loc_queue_); + heartbeat_.node = GetName(); + heartbeat_.nodelet_manager = ros::this_node::getName(); +} + +void ImuAugmentorNodelet::Initialize(ros::NodeHandle* nh) { + SubscribeAndAdvertise(nh); + Run(); +} + +void ImuAugmentorNodelet::SubscribeAndAdvertise(ros::NodeHandle* nh) { + state_pub_ = nh->advertise(TOPIC_GNC_EKF, 1); + pose_pub_ = nh->advertise(TOPIC_LOCALIZATION_POSE, 1); + twist_pub_ = nh->advertise(TOPIC_LOCALIZATION_TWIST, 1); + heartbeat_pub_ = nh->advertise(TOPIC_HEARTBEAT, 5, true); + + imu_sub_ = imu_nh_.subscribe(TOPIC_HARDWARE_IMU, 100, &ImuAugmentorNodelet::ImuCallback, this, + ros::TransportHints().tcpNoDelay()); + state_sub_ = loc_nh_.subscribe(TOPIC_GRAPH_LOC_STATE, 1, &ImuAugmentorNodelet::LocalizationStateCallback, this, + ros::TransportHints().tcpNoDelay()); +} + +void ImuAugmentorNodelet::ImuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg) { + imu_augmentor_wrapper_.ImuCallback(*imu_msg); +} + +void ImuAugmentorNodelet::LocalizationStateCallback(const ff_msgs::GraphState::ConstPtr& loc_msg) { + imu_augmentor_wrapper_.LocalizationStateCallback(*loc_msg); +} + +boost::optional 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."); + return boost::none; + } + state_pub_.publish(*latest_imu_augmented_loc_msg); + return latest_imu_augmented_loc_msg; +} + +void ImuAugmentorNodelet::PublishPoseAndTwistAndTransform(const ff_msgs::EkfState& loc_msg) { + // Publish pose + geometry_msgs::PoseStamped pose_msg; + pose_msg.header = loc_msg.header; + pose_msg.pose = loc_msg.pose; + pose_pub_.publish(pose_msg); + + // Publish twist + geometry_msgs::TwistStamped twist_msg; + twist_msg.header = loc_msg.header; + twist_msg.twist.linear = loc_msg.velocity; + twist_msg.twist.angular = loc_msg.omega; + twist_pub_.publish(twist_msg); + + // Publish TF + geometry_msgs::TransformStamped transform_msg; + transform_msg.header = loc_msg.header; + transform_msg.child_frame_id = platform_name_ + "body"; + transform_msg.transform.translation.x = loc_msg.pose.position.x; + transform_msg.transform.translation.y = loc_msg.pose.position.y; + transform_msg.transform.translation.z = loc_msg.pose.position.z; + transform_msg.transform.rotation = loc_msg.pose.orientation; + transform_pub_.sendTransform(transform_msg); +} + +void ImuAugmentorNodelet::PublishHeartbeat() { + heartbeat_.header.stamp = ros::Time::now(); + heartbeat_pub_.publish(heartbeat_); +} + +void ImuAugmentorNodelet::Run() { + ros::Rate rate(100); + while (ros::ok()) { + imu_queue_.callAvailable(); + loc_queue_.callAvailable(); + const auto loc_msg = PublishLatestImuAugmentedLocalizationState(); + if (loc_msg) { + PublishPoseAndTwistAndTransform(*loc_msg); + } + PublishHeartbeat(); + rate.sleep(); + } +} +} // namespace imu_augmentor + +PLUGINLIB_EXPORT_CLASS(imu_augmentor::ImuAugmentorNodelet, nodelet::Nodelet); diff --git a/localization/imu_augmentor/src/imu_augmentor_wrapper.cc b/localization/imu_augmentor/src/imu_augmentor_wrapper.cc new file mode 100644 index 0000000000..0ca8919006 --- /dev/null +++ b/localization/imu_augmentor/src/imu_augmentor_wrapper.cc @@ -0,0 +1,151 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include +#include +#include +#include + +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."); + } + + ii::LoadImuIntegratorParams(config, params_); + params_.standstill_enabled = mc::LoadBool(config, "imu_augmentor_standstill"); + 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 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); + + latest_combined_nav_state_ = lc::CombinedNavStateFromMsg(loc_msg); + latest_covariances_ = lc::CombinedNavStateCovariancesFromMsg(loc_msg); + 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)); +} + +boost::optional> +ImuAugmentorWrapper::LatestImuAugmentedCombinedNavStateAndCovariances() { + if (!latest_combined_nav_state_ || !latest_covariances_ || !imu_augmentor_) { + LogError( + "LatestImuAugmentedCombinedNavStateAndCovariances: Not enough information available to create desired data."); + return boost::none; + } + + if (standstill()) { + LogDebugEveryN(100, "LatestImuAugmentedCombinedNavStateAndCovariances: Standstill."); + return std::pair{*latest_combined_nav_state_, + *latest_covariances_}; + } + + const auto latest_imu_augmented_combined_nav_state = imu_augmentor_->PimPredict(*latest_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{*latest_imu_augmented_combined_nav_state, + *latest_covariances_}; +} + +boost::optional 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; + latest_imu_augmented_loc_msg.of_count = latest_loc_msg_->of_count; + latest_imu_augmented_loc_msg.ml_count = latest_loc_msg_->ml_count; + 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_integration/CMakeLists.txt b/localization/imu_integration/CMakeLists.txt new file mode 100644 index 0000000000..7d07b77bed --- /dev/null +++ b/localization/imu_integration/CMakeLists.txt @@ -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. + +project(imu_integration) + +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} config_reader localization_common localization_measurements + INCLUDE_DIRS include ${GTSAM_INCLUDE_DIR} ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS + DEPENDS gtsam +) + +create_library(TARGET ${PROJECT_NAME} + LIBS ${catkin_LIBRARIES} ${GLOG_LIBRARIES} config_reader localization_common localization_measurements + INC ${catkin_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} + DEPS gtsam +) diff --git a/localization/imu_integration/include/imu_integration/butterworth_lowpass_filter.h b/localization/imu_integration/include/imu_integration/butterworth_lowpass_filter.h new file mode 100644 index 0000000000..5283586ebb --- /dev/null +++ b/localization/imu_integration/include/imu_integration/butterworth_lowpass_filter.h @@ -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. + */ + +#ifndef IMU_INTEGRATION_BUTTERWORTH_LOWPASS_FILTER_H_ +#define IMU_INTEGRATION_BUTTERWORTH_LOWPASS_FILTER_H_ + +#include + +#include + +namespace imu_integration { +class ButterworthLowpassFilter : public Filter { + public: + ButterworthLowpassFilter(); + // Returns filtered value and timestamp + double AddValue(const double value) final; + + private: + void Initialize(const double first_value, const double gain); + // Notation taken from mkfilter site + // /www/usr/fisher/helpers/mkfilter + std::array xv_; + std::array yv_; + bool initialized_; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_BUTTERWORTH_LOWPASS_FILTER_H_ diff --git a/localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_3rd_order.h b/localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_3rd_order.h new file mode 100644 index 0000000000..ad6dc6ea2f --- /dev/null +++ b/localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_3rd_order.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_BUTTERWORTH_LOWPASS_FILTER_3RD_ORDER_H_ +#define IMU_INTEGRATION_BUTTERWORTH_LOWPASS_FILTER_3RD_ORDER_H_ + +#include + +#include + +#include + +namespace imu_integration { +class ButterworthLowpassFilter3rdOrder : public Filter { + public: + ButterworthLowpassFilter3rdOrder(); + // Returns filtered value and timestamp + double AddValue(const double value) final; + + private: + void Initialize(const double first_value, const double gain); + // Notation taken from mkfilter site + // /www/usr/fisher/helpers/mkfilter + std::array xv_; + std::array yv_; + bool initialized_; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_BUTTERWORTH_LOWPASS_FILTER_3RD_ORDER_H_ diff --git a/localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_5th_order.h b/localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_5th_order.h new file mode 100644 index 0000000000..cf6df9bdd1 --- /dev/null +++ b/localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_5th_order.h @@ -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. + */ + +#ifndef IMU_INTEGRATION_BUTTERWORTH_LOWPASS_FILTER_5TH_ORDER_H_ +#define IMU_INTEGRATION_BUTTERWORTH_LOWPASS_FILTER_5TH_ORDER_H_ + +#include + +#include + +namespace imu_integration { +class ButterworthLowpassFilter5thOrder : public Filter { + public: + ButterworthLowpassFilter5thOrder(); + // Returns filtered value and timestamp + double AddValue(const double value) final; + + private: + void Initialize(const double first_value, const double gain); + // Notation taken from mkfilter site + // /www/usr/fisher/helpers/mkfilter + std::array xv_; + std::array yv_; + bool initialized_; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_BUTTERWORTH_LOWPASS_FILTER_5TH_ORDER_H_ diff --git a/localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_5th_order_05.h b/localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_5th_order_05.h new file mode 100644 index 0000000000..1e37857295 --- /dev/null +++ b/localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_5th_order_05.h @@ -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. + */ + +#ifndef IMU_INTEGRATION_BUTTERWORTH_LOWPASS_FILTER_5TH_ORDER_05_H_ +#define IMU_INTEGRATION_BUTTERWORTH_LOWPASS_FILTER_5TH_ORDER_05_H_ + +#include + +#include + +namespace imu_integration { +class ButterworthLowpassFilter5thOrder05 : public Filter { + public: + ButterworthLowpassFilter5thOrder05(); + // Returns filtered value and timestamp + double AddValue(const double value) final; + + private: + void Initialize(const double first_value, const double gain); + // Notation taken from mkfilter site + // /www/usr/fisher/helpers/mkfilter + std::array xv_; + std::array yv_; + bool initialized_; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_BUTTERWORTH_LOWPASS_FILTER_5TH_ORDER_05_H_ diff --git a/localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_5th_order_1.h b/localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_5th_order_1.h new file mode 100644 index 0000000000..4197813c72 --- /dev/null +++ b/localization/imu_integration/include/imu_integration/butterworth_lowpass_filter_5th_order_1.h @@ -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. + */ + +#ifndef IMU_INTEGRATION_BUTTERWORTH_LOWPASS_FILTER_5TH_ORDER_1_H_ +#define IMU_INTEGRATION_BUTTERWORTH_LOWPASS_FILTER_5TH_ORDER_1_H_ + +#include + +#include + +namespace imu_integration { +class ButterworthLowpassFilter5thOrder1 : public Filter { + public: + ButterworthLowpassFilter5thOrder1(); + // Returns filtered value and timestamp + double AddValue(const double value) final; + + private: + void Initialize(const double first_value, const double gain); + // Notation taken from mkfilter site + // /www/usr/fisher/helpers/mkfilter + std::array xv_; + std::array yv_; + bool initialized_; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_BUTTERWORTH_LOWPASS_FILTER_5TH_ORDER_1_H_ diff --git a/localization/imu_integration/include/imu_integration/filter.h b/localization/imu_integration/include/imu_integration/filter.h new file mode 100644 index 0000000000..dc0fc42a0c --- /dev/null +++ b/localization/imu_integration/include/imu_integration/filter.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 IMU_INTEGRATION_FILTER_H_ +#define IMU_INTEGRATION_FILTER_H_ + +namespace imu_integration { +class Filter { + public: + virtual ~Filter() = default; + // Returns filtered value and timestamp + virtual double AddValue(const double value) = 0; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_FILTER_H_ diff --git a/localization/imu_integration/include/imu_integration/identity_filter.h b/localization/imu_integration/include/imu_integration/identity_filter.h new file mode 100644 index 0000000000..2a3725a3fb --- /dev/null +++ b/localization/imu_integration/include/imu_integration/identity_filter.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 IMU_INTEGRATION_IDENTITY_FILTER_H_ +#define IMU_INTEGRATION_IDENTITY_FILTER_H_ + +#include + +namespace imu_integration { +class IdentityFilter : public Filter { + public: + IdentityFilter(); + // Returns filtered value and timestamp + double AddValue(const double value) final; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_IDENTITY_FILTER_H_ diff --git a/localization/imu_integration/include/imu_integration/imu_filter.h b/localization/imu_integration/include/imu_integration/imu_filter.h new file mode 100644 index 0000000000..108835dd91 --- /dev/null +++ b/localization/imu_integration/include/imu_integration/imu_filter.h @@ -0,0 +1,51 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_IMU_FILTER_H_ +#define IMU_INTEGRATION_IMU_FILTER_H_ + +#include +#include +#include +#include + +#include + +#include + +namespace imu_integration { +class ImuFilter { + public: + explicit ImuFilter(const ImuFilterParams& params); + // Returns filtered measurement if one is available + boost::optional AddMeasurement( + const localization_measurements::ImuMeasurement& imu_measurement); + + private: + // Acceleration Filters + std::unique_ptr acceleration_x_filter_; + std::unique_ptr acceleration_y_filter_; + std::unique_ptr acceleration_z_filter_; + // Angular Velocity Filters + std::unique_ptr angular_velocity_x_filter_; + std::unique_ptr angular_velocity_y_filter_; + std::unique_ptr angular_velocity_z_filter_; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_IMU_FILTER_H_ diff --git a/localization/imu_integration/include/imu_integration/imu_filter_params.h b/localization/imu_integration/include/imu_integration/imu_filter_params.h new file mode 100644 index 0000000000..8ce2a072da --- /dev/null +++ b/localization/imu_integration/include/imu_integration/imu_filter_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 IMU_INTEGRATION_IMU_FILTER_PARAMS_H_ +#define IMU_INTEGRATION_IMU_FILTER_PARAMS_H_ + +#include + +namespace imu_integration { +struct ImuFilterParams { + // none, butter, butter3, butter5, butter5_1, butter5_05 + std::string type = "none"; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_IMU_FILTER_PARAMS_H_ diff --git a/localization/imu_integration/include/imu_integration/imu_integrator.h b/localization/imu_integration/include/imu_integration/imu_integrator.h new file mode 100644 index 0000000000..6c2773d176 --- /dev/null +++ b/localization/imu_integration/include/imu_integration/imu_integrator.h @@ -0,0 +1,83 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_IMU_INTEGRATOR_H_ +#define IMU_INTEGRATION_IMU_INTEGRATOR_H_ + +#include +#include + +#include +#include +#include + +#include +#include + +#include + +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 { + 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 IntegrateImuMeasurements( + const localization_common::Time start_time, const localization_common::Time end_time, + gtsam::PreintegratedCombinedMeasurements& pim) const; + + boost::optional IntegratedPim( + const gtsam::imuBias::ConstantBias& bias, const localization_common::Time start_time, + const localization_common::Time end_time, + boost::shared_ptr params) const; + + void RemoveOldMeasurements(const localization_common::Time new_start_time); + + boost::optional OldestTime() const; + + boost::optional LatestTime() const; + + boost::optional LatestMeasurement() const; + + bool Empty() const; + + int Size() const; + + boost::shared_ptr pim_params() const; + + bool WithinBounds(const localization_common::Time timestamp); + + const std::map& measurements() const; + + private: + ImuIntegratorParams params_; + boost::shared_ptr pim_params_; + std::map measurements_; + std::unique_ptr imu_filter_; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_IMU_INTEGRATOR_H_ diff --git a/localization/imu_integration/include/imu_integration/imu_integrator_params.h b/localization/imu_integration/include/imu_integration/imu_integrator_params.h new file mode 100644 index 0000000000..03e2d542ab --- /dev/null +++ b/localization/imu_integration/include/imu_integration/imu_integrator_params.h @@ -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. + */ +#ifndef IMU_INTEGRATION_IMU_INTEGRATOR_PARAMS_H_ +#define IMU_INTEGRATION_IMU_INTEGRATOR_PARAMS_H_ + +#include + +#include +#include + +#include + +namespace imu_integration { +struct ImuIntegratorParams { + gtsam::Vector3 gravity; + gtsam::Pose3 body_T_imu; + ImuFilterParams filter; + // From gtsam: Angular and velocity random walk expressed in degrees respectively m/s per sqrt(hr). + double gyro_sigma; + double accel_sigma; + double accel_bias_sigma; + double gyro_bias_sigma; + double integration_variance; + double bias_acc_omega_int; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_IMU_INTEGRATOR_PARAMS_H_ diff --git a/localization/imu_integration/include/imu_integration/latest_imu_integrator.h b/localization/imu_integration/include/imu_integration/latest_imu_integrator.h new file mode 100644 index 0000000000..9881b990db --- /dev/null +++ b/localization/imu_integration/include/imu_integration/latest_imu_integrator.h @@ -0,0 +1,48 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include + +#include + +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 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/latest_imu_integrator_params.h new file mode 100644 index 0000000000..7ba5e4630b --- /dev/null +++ b/localization/imu_integration/include/imu_integration/latest_imu_integrator_params.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 IMU_INTEGRATION_LATEST_IMU_INTEGRATOR_PARAMS_H_ +#define IMU_INTEGRATION_LATEST_IMU_INTEGRATOR_PARAMS_H_ + +#include + +#include + +namespace imu_integration { +struct LatestImuIntegratorParams : public ImuIntegratorParams { + double start_time; + gtsam::imuBias::ConstantBias initial_imu_bias; +}; +} // namespace imu_integration + +#endif // IMU_INTEGRATION_LATEST_IMU_INTEGRATOR_PARAMS_H_ diff --git a/localization/imu_integration/include/imu_integration/utilities.h b/localization/imu_integration/include/imu_integration/utilities.h new file mode 100644 index 0000000000..5ae5feb454 --- /dev/null +++ b/localization/imu_integration/include/imu_integration/utilities.h @@ -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. + */ + +#ifndef IMU_INTEGRATION_UTILITIES_H_ +#define IMU_INTEGRATION_UTILITIES_H_ + +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include + +namespace imu_integration { +namespace sym = gtsam::symbol_shorthand; +boost::optional Interpolate( + const localization_measurements::ImuMeasurement& imu_measurement_a, + const localization_measurements::ImuMeasurement& imu_measurement_b, const localization_common::Time timestamp); + +gtsam::PreintegratedCombinedMeasurements Pim( + const gtsam::imuBias::ConstantBias& bias, + const boost::shared_ptr& params); + +void AddMeasurement(const localization_measurements::ImuMeasurement& imu_measurement, + localization_common::Time& last_added_imu_measurement_time, + gtsam::PreintegratedCombinedMeasurements& pim); + +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); +} // namespace imu_integration + +#endif // IMU_INTEGRATION_UTILITIES_H_ diff --git a/localization/imu_integration/package.xml b/localization/imu_integration/package.xml new file mode 100644 index 0000000000..dde9b9886c --- /dev/null +++ b/localization/imu_integration/package.xml @@ -0,0 +1,17 @@ + + imu_integration + 1.0.0 + + Imu integration package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + diff --git a/localization/imu_integration/readme.md b/localization/imu_integration/readme.md new file mode 100644 index 0000000000..ddd4c800f9 --- /dev/null +++ b/localization/imu_integration/readme.md @@ -0,0 +1,14 @@ +\page imuintegration Imu Integration + +# Package Overview +The imu integration package integrates imu measurements using the gtsam PreintegratedCombinedMeasurements (pim) class. As measurements are added to the pim, their acceleration and angular velocities are integrated after removing imu biases and uncertainties are propogated accordingly (_Dellaert, The new Imu Factor, https://github.com/borglab/gtsam/blob/develop/doc/ImuFactor.pdf_). For efficieny and particulary for use in an optimization procedure, the pim uses the initial biases and velocity estimate and assumes these are constant for the integration procedure. The pim also assumes gravity is constantly oriented for the integration procedure, which can lead to accumulated errors if there are large orientation changes before reinitializing the pim. Therefore there is a tradeoff of efficiency and accuracy, where the most accurate integration would reset the pim after each measurement integration. + +## Important Classes +# ImuIntegrator +Maintains a history of measurements and allows for the create of pims using different sets of measurements. + +# LatestImuIntegrator +Adds on to the ImuIntegrator by keeping track of the latest integrated measurement and allowing for the integration of the latest measurements up to a certain time. + +# Filters +There are various filters provided that enable filtering of imu data before it is added to an imu integrator. Filter params are provided in their header files, and vary by filter order and cutoff frequency. Each butterworth filter is a lowpass filter. diff --git a/localization/imu_integration/src/butterworth_lowpass_filter.cc b/localization/imu_integration/src/butterworth_lowpass_filter.cc new file mode 100644 index 0000000000..ccc1ba3a58 --- /dev/null +++ b/localization/imu_integration/src/butterworth_lowpass_filter.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 +#include + +namespace imu_integration { +ButterworthLowpassFilter::ButterworthLowpassFilter() : initialized_(false) {} + +void ButterworthLowpassFilter::Initialize(const double first_value, const double gain) { + // TODO(rsoussan): Look into filtfilt (forward and backward filtering to eliminate phase shift) + // Initialize values so filter output starts at first value + for (auto& val : xv_) { + val = first_value / gain; + } + for (auto& val : yv_) { + val = first_value; + } + + initialized_ = true; +} + +double ButterworthLowpassFilter::AddValue(const double value) { + // Corner frequency at 3Hz and notch filter at 29.16 Hz, 1st order + /* Digital filter designed by mkfilter/mkshape/gencode A.J. Fisher + Command line: /www/usr/fisher/helpers/mkfilter -Bu -Lp -o 1 -a 4.8000000000e-02 0.0000000000e+00 + -Z 4.6656000000e-01 -l */ + static constexpr double GAIN = 2.999100930e+01; + + if (!initialized_) Initialize(value, GAIN); + + // Shift input vals + xv_[0] = xv_[1]; + xv_[1] = xv_[2]; + xv_[2] = xv_[3]; + // Add new value + xv_[3] = value / GAIN; + // Shift output vals + yv_[0] = yv_[1]; + yv_[1] = yv_[2]; + yv_[2] = yv_[3]; + // Generate new output + yv_[3] = (xv_[0] + xv_[3]) + 2.9560160746 * (xv_[1] + xv_[2]) + (-0.0000000000 * yv_[0]) + (-0.0000000000 * yv_[1]) + + (0.7361865327 * yv_[2]); + // Return most recent output + return yv_[3]; +} +} // namespace imu_integration diff --git a/localization/imu_integration/src/butterworth_lowpass_filter_3rd_order.cc b/localization/imu_integration/src/butterworth_lowpass_filter_3rd_order.cc new file mode 100644 index 0000000000..07e8b17c97 --- /dev/null +++ b/localization/imu_integration/src/butterworth_lowpass_filter_3rd_order.cc @@ -0,0 +1,62 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include + +namespace imu_integration { +ButterworthLowpassFilter3rdOrder::ButterworthLowpassFilter3rdOrder() : initialized_(false) {} + +void ButterworthLowpassFilter3rdOrder::Initialize(const double first_value, const double gain) { + // TODO(rsoussan): Look into filtfilt (forward and backward filtering to eliminate phase shift) + // Initialize values so filter output starts at first value + for (auto& val : xv_) { + val = first_value / gain; + } + for (auto& val : yv_) { + val = first_value; + } + + initialized_ = true; +} + +double ButterworthLowpassFilter3rdOrder::AddValue(const double value) { + // Corner frequency at 3Hz and notch filter at 29.16 Hz, 3rd order + /* Digital filter designed by mkfilter/mkshape/gencode A.J. Fisher + Command line: /www/usr/fisher/helpers/mkfilter -Bu -Lp -o 3 -a 4.8000000000e-02 0.0000000000e+00 -Z 4.6656000000e-01 + -l */ + static constexpr double GAIN = 1.526307756e+03; + + if (!initialized_) Initialize(value, GAIN); + + const int last_index = xv_.size() - 1; + // Shift input and output vals + for (int i = 0; i < last_index; ++i) { + xv_[i] = xv_[i + 1]; + yv_[i] = yv_[i + 1]; + } + // Add new values + xv_[last_index] = value / GAIN; + // Generate new output + yv_[last_index] = (xv_[0] + xv_[5]) + 4.9560160746 * (xv_[1] + xv_[4]) + 9.8680482239 * (xv_[2] + xv_[3]) + + (-0.0000000000 * yv_[0]) + (-0.0000000000 * yv_[1]) + (0.5457868345 * yv_[2]) + + (-1.9654812209 * yv_[3]) + (2.3989592965 * yv_[4]); + // Return most recent output + return yv_[last_index]; +} +} // namespace imu_integration diff --git a/localization/imu_integration/src/butterworth_lowpass_filter_5th_order.cc b/localization/imu_integration/src/butterworth_lowpass_filter_5th_order.cc new file mode 100644 index 0000000000..4d61203c67 --- /dev/null +++ b/localization/imu_integration/src/butterworth_lowpass_filter_5th_order.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 +#include + +namespace imu_integration { +ButterworthLowpassFilter5thOrder::ButterworthLowpassFilter5thOrder() : initialized_(false) {} + +void ButterworthLowpassFilter5thOrder::Initialize(const double first_value, const double gain) { + // TODO(rsoussan): Look into filtfilt (forward and backward filtering to eliminate phase shift) + // Initialize values so filter output starts at first value + for (auto& val : xv_) { + val = first_value / gain; + } + for (auto& val : yv_) { + val = first_value; + } + + initialized_ = true; +} + +double ButterworthLowpassFilter5thOrder::AddValue(const double value) { + // Corner frequency at 3Hz and notch filter at 29.16 Hz, 5th order + /* Digital filter designed by mkfilter/mkshape/gencode A.J. Fisher + Command line: /www/usr/fisher/helpers/mkfilter -Bu -Lp -o 5 -a 4.8000000000e-02 0.0000000000e+00 -Z 4.6656000000e-01 + -l */ + static constexpr double GAIN = 7.974174280e+04; + + if (!initialized_) Initialize(value, GAIN); + + const int last_index = xv_.size() - 1; + // Shift input and output vals + for (int i = 0; i < last_index; ++i) { + xv_[i] = xv_[i + 1]; + yv_[i] = yv_[i + 1]; + } + // Add new values + xv_[last_index] = value / GAIN; + // Generate new output + yv_[last_index] = (xv_[0] + xv_[7]) + 6.9560160746 * (xv_[1] + xv_[6]) + 20.7800803730 * (xv_[2] + xv_[5]) + + 34.5601607460 * (xv_[3] + xv_[4]) + (-0.0000000000 * yv_[0]) + (-0.0000000000 * yv_[1]) + + (0.3750929134 * yv_[2]) + (-2.2411521809 * yv_[3]) + (5.3982113474 * yv_[4]) + + (-6.5588143289 * yv_[5]) + (4.0250747177 * yv_[6]); + // Return most recent output + return yv_[last_index]; +} +} // namespace imu_integration diff --git a/localization/imu_integration/src/butterworth_lowpass_filter_5th_order_05.cc b/localization/imu_integration/src/butterworth_lowpass_filter_5th_order_05.cc new file mode 100644 index 0000000000..9c13e13936 --- /dev/null +++ b/localization/imu_integration/src/butterworth_lowpass_filter_5th_order_05.cc @@ -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. + */ + +#include +#include + +namespace imu_integration { +ButterworthLowpassFilter5thOrder05::ButterworthLowpassFilter5thOrder05() : initialized_(false) {} + +void ButterworthLowpassFilter5thOrder05::Initialize(const double first_value, const double gain) { + // TODO(rsoussan): Look into filtfilt (forward and backward filtering to eliminate phase shift) + // Initialize values so filter output starts at first value + for (auto& val : xv_) { + val = first_value / gain; + } + for (auto& val : yv_) { + val = first_value; + } + + initialized_ = true; +} + +double ButterworthLowpassFilter5thOrder05::AddValue(const double value) { + // Corner frequency at 0.5Hz and notch filter at 29.16 Hz, 5th order + /* Digital filter designed by mkfilter/mkshape/gencode A.J. Fisher + Command line: /www/usr/fisher/helpers/mkfilter -Bu -Lp -o 5 -a 8.0000000000e-03 0.0000000000e+00 + -Z 4.6656000000e-01 -l */ + + static constexpr double GAIN = 4.274918013e+08; + + if (!initialized_) Initialize(value, GAIN); + + const int last_index = xv_.size() - 1; + // Shift input and output vals + for (int i = 0; i < last_index; ++i) { + xv_[i] = xv_[i + 1]; + yv_[i] = yv_[i + 1]; + } + // Add new values + xv_[last_index] = value / GAIN; + // Generate new output + yv_[last_index] = (xv_[0] + xv_[7]) + 6.9560160746 * (xv_[1] + xv_[6]) + 20.7800803730 * (xv_[2] + xv_[5]) + + 34.5601607460 * (xv_[3] + xv_[4]) + (-0.0000000000 * yv_[0]) + (-0.0000000000 * yv_[1]) + + (0.8498599655 * yv_[2]) + (-4.3875359464 * yv_[3]) + (9.0628533836 * yv_[4]) + + (-9.3625201736 * yv_[5]) + (4.8373424748 * yv_[6]); + + // Return most recent output + return yv_[last_index]; +} +} // namespace imu_integration diff --git a/localization/imu_integration/src/butterworth_lowpass_filter_5th_order_1.cc b/localization/imu_integration/src/butterworth_lowpass_filter_5th_order_1.cc new file mode 100644 index 0000000000..6d50b8d460 --- /dev/null +++ b/localization/imu_integration/src/butterworth_lowpass_filter_5th_order_1.cc @@ -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. + */ + +#include +#include + +namespace imu_integration { +ButterworthLowpassFilter5thOrder1::ButterworthLowpassFilter5thOrder1() : initialized_(false) {} + +void ButterworthLowpassFilter5thOrder1::Initialize(const double first_value, const double gain) { + // TODO(rsoussan): Look into filtfilt (forward and backward filtering to eliminate phase shift) + // Initialize values so filter output starts at first value + for (auto& val : xv_) { + val = first_value / gain; + } + for (auto& val : yv_) { + val = first_value; + } + + initialized_ = true; +} + +double ButterworthLowpassFilter5thOrder1::AddValue(const double value) { + // Corner frequency at 1Hz and notch filter at 29.16 Hz, 5th order + /* Digital filter designed by mkfilter/mkshape/gencode A.J. Fisher + Command line: /www/usr/fisher/helpers/mkfilter -Bu -Lp -o 5 -a 1.6000000000e-02 0.0000000000e+00 + -Z 4.6656000000e-01 -l */ + + static constexpr double GAIN = 1.444638217e+07; + + if (!initialized_) Initialize(value, GAIN); + + const int last_index = xv_.size() - 1; + // Shift input and output vals + for (int i = 0; i < last_index; ++i) { + xv_[i] = xv_[i + 1]; + yv_[i] = yv_[i + 1]; + } + // Add new values + xv_[last_index] = value / GAIN; + // Generate new output + yv_[last_index] = (xv_[0] + xv_[7]) + 6.9560160746 * (xv_[1] + xv_[6]) + 20.7800803730 * (xv_[2] + xv_[5]) + + 34.5601607460 * (xv_[3] + xv_[4]) + (-0.0000000000 * yv_[0]) + (-0.0000000000 * yv_[1]) + + (0.7221701429 * yv_[2]) + (-3.8457619644 * yv_[3]) + (8.2000057707 * yv_[4]) + + (-8.7511375257 * yv_[5]) + (4.6747148135 * yv_[6]); + // Return most recent output + return yv_[last_index]; +} +} // namespace imu_integration diff --git a/localization/imu_integration/src/identity_filter.cc b/localization/imu_integration/src/identity_filter.cc new file mode 100644 index 0000000000..02bc892d1c --- /dev/null +++ b/localization/imu_integration/src/identity_filter.cc @@ -0,0 +1,26 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include + +namespace imu_integration { +IdentityFilter::IdentityFilter() {} + +double IdentityFilter::AddValue(const double value) { return value; } +} // namespace imu_integration diff --git a/localization/imu_integration/src/imu_filter.cc b/localization/imu_integration/src/imu_filter.cc new file mode 100644 index 0000000000..a9472cdce7 --- /dev/null +++ b/localization/imu_integration/src/imu_filter.cc @@ -0,0 +1,100 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include +#include +#include +#include +#include + +namespace imu_integration { +namespace lm = localization_measurements; +ImuFilter::ImuFilter(const ImuFilterParams& params) { + // TODO(rsoussan): Do this more efficiently + if (params.type == "butter") { + LogDebug("ImuFilter: Using Butterworth lowpass filter."); + acceleration_x_filter_.reset(new ButterworthLowpassFilter()); + acceleration_y_filter_.reset(new ButterworthLowpassFilter()); + acceleration_z_filter_.reset(new ButterworthLowpassFilter()); + angular_velocity_x_filter_.reset(new ButterworthLowpassFilter()); + angular_velocity_y_filter_.reset(new ButterworthLowpassFilter()); + angular_velocity_z_filter_.reset(new ButterworthLowpassFilter()); + } else if (params.type == "butter3") { + LogDebug("ImuFilter: Using Butterworth lowpass 3rd order filter."); + acceleration_x_filter_.reset(new ButterworthLowpassFilter3rdOrder()); + acceleration_y_filter_.reset(new ButterworthLowpassFilter3rdOrder()); + acceleration_z_filter_.reset(new ButterworthLowpassFilter3rdOrder()); + angular_velocity_x_filter_.reset(new ButterworthLowpassFilter3rdOrder()); + angular_velocity_y_filter_.reset(new ButterworthLowpassFilter3rdOrder()); + angular_velocity_z_filter_.reset(new ButterworthLowpassFilter3rdOrder()); + } else if (params.type == "butter5") { + LogDebug("ImuFilter: Using Butterworth lowpass 5th order filter."); + acceleration_x_filter_.reset(new ButterworthLowpassFilter5thOrder()); + acceleration_y_filter_.reset(new ButterworthLowpassFilter5thOrder()); + acceleration_z_filter_.reset(new ButterworthLowpassFilter5thOrder()); + angular_velocity_x_filter_.reset(new ButterworthLowpassFilter5thOrder()); + angular_velocity_y_filter_.reset(new ButterworthLowpassFilter5thOrder()); + angular_velocity_z_filter_.reset(new ButterworthLowpassFilter5thOrder()); + } else if (params.type == "butter5_1") { + LogDebug("ImuFilter: Using Butterworth lowpass 5th order 1Hz cutoff filter."); + acceleration_x_filter_.reset(new ButterworthLowpassFilter5thOrder1()); + acceleration_y_filter_.reset(new ButterworthLowpassFilter5thOrder1()); + acceleration_z_filter_.reset(new ButterworthLowpassFilter5thOrder1()); + angular_velocity_x_filter_.reset(new ButterworthLowpassFilter5thOrder1()); + angular_velocity_y_filter_.reset(new ButterworthLowpassFilter5thOrder1()); + angular_velocity_z_filter_.reset(new ButterworthLowpassFilter5thOrder1()); + } else if (params.type == "butter5_05") { + LogDebug("ImuFilter: Using Butterworth lowpass 5th order 0.5Hz cutoff filter."); + acceleration_x_filter_.reset(new ButterworthLowpassFilter5thOrder05()); + acceleration_y_filter_.reset(new ButterworthLowpassFilter5thOrder05()); + acceleration_z_filter_.reset(new ButterworthLowpassFilter5thOrder05()); + angular_velocity_x_filter_.reset(new ButterworthLowpassFilter5thOrder05()); + angular_velocity_y_filter_.reset(new ButterworthLowpassFilter5thOrder05()); + angular_velocity_z_filter_.reset(new ButterworthLowpassFilter5thOrder05()); + } else if (params.type == "none") { + LogDebug("ImuFilter: No filter."); + acceleration_x_filter_.reset(new IdentityFilter()); + acceleration_y_filter_.reset(new IdentityFilter()); + acceleration_z_filter_.reset(new IdentityFilter()); + angular_velocity_x_filter_.reset(new IdentityFilter()); + angular_velocity_y_filter_.reset(new IdentityFilter()); + angular_velocity_z_filter_.reset(new IdentityFilter()); + } else { + LogFatal("ImuFilter: Invalid filter selection."); + } +} + +boost::optional ImuFilter::AddMeasurement(const lm::ImuMeasurement& imu_measurement) { + const double filtered_acceleration_x = acceleration_x_filter_->AddValue(imu_measurement.acceleration.x()); + const double filtered_acceleration_y = acceleration_y_filter_->AddValue(imu_measurement.acceleration.y()); + const double filtered_acceleration_z = acceleration_z_filter_->AddValue(imu_measurement.acceleration.z()); + const double filtered_angular_velocity_x = angular_velocity_x_filter_->AddValue(imu_measurement.angular_velocity.x()); + const double filtered_angular_velocity_y = angular_velocity_y_filter_->AddValue(imu_measurement.angular_velocity.y()); + 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, + filtered_angular_velocity_z; + return filtered_imu_measurement; +} +} // namespace imu_integration diff --git a/localization/imu_integration/src/imu_integrator.cc b/localization/imu_integration/src/imu_integrator.cc new file mode 100644 index 0000000000..2cfbfea018 --- /dev/null +++ b/localization/imu_integration/src/imu_integrator.cc @@ -0,0 +1,177 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include + +namespace imu_integration { +namespace lc = localization_common; +namespace lm = localization_measurements; +ImuIntegrator::ImuIntegrator(const ImuIntegratorParams& params) + : params_(params), imu_filter_(new ImuFilter(params.filter)) { + LogDebug("ImuIntegrator: Gravity vector: " << std::endl << params_.gravity.matrix()); + pim_params_.reset(new gtsam::PreintegratedCombinedMeasurements::Params(params_.gravity)); + // Set sensor covariances + pim_params_->gyroscopeCovariance = params_.gyro_sigma * params_.gyro_sigma * gtsam::I_3x3; + pim_params_->accelerometerCovariance = params_.accel_sigma * params_.accel_sigma * gtsam::I_3x3; + pim_params_->integrationCovariance = params_.integration_variance * gtsam::I_3x3; + // Set bias random walk covariances + pim_params_->biasAccCovariance = params_.accel_bias_sigma * params_.accel_bias_sigma * gtsam::I_3x3; + pim_params_->biasOmegaCovariance = params_.gyro_bias_sigma * params_.gyro_bias_sigma * gtsam::I_3x3; + // Set bias covariance used for pim integration + pim_params_->biasAccOmegaInt = params_.bias_acc_omega_int * gtsam::I_6x6; + // Set imu calibration relative pose + pim_params_->setBodyPSensor(params_.body_T_imu); +} + +void ImuIntegrator::BufferImuMeasurement(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); + } +} + +boost::optional ImuIntegrator::IntegrateImuMeasurements(const lc::Time start_time, const lc::Time end_time, + gtsam::PreintegratedCombinedMeasurements& pim) const { + if (measurements_.size() < 2) { + LogError("IntegrateImuMeasurements: Less than 2 measurements available."); + return boost::none; + } + if (end_time < measurements_.cbegin()->first) { + LogError("IntegrateImuMeasurements: End time occurs before first measurement."); + return boost::none; + } + if (end_time > measurements_.crbegin()->first) { + LogError("IntegrateImuMeasurements: End time occurs after last measurement."); + return boost::none; + } + if (start_time > end_time) { + LogError("IntegrateImuMeasurements: Start time occurs after end time."); + return boost::none; + } + + // 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); + 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) { + AddMeasurement(measurement_it->second, last_added_imu_measurement_time, pim); + ++num_measurements_added; + } + + // Add final interpolated measurement if necessary + if (last_added_imu_measurement_time != end_time) { + const auto interpolated_measurement = + Interpolate(std::prev(measurement_it)->second, measurement_it->second, end_time); + if (!interpolated_measurement) { + LogError("IntegrateImuMeasurements: Failed to interpolate final measurement."); + return boost::none; + } + AddMeasurement(*interpolated_measurement, last_added_imu_measurement_time, pim); + ++num_measurements_added; + } + + if (last_added_imu_measurement_time != end_time) { + LogError("IntegrateImuMeasurements: Last added time not equal to end time."); + return boost::none; + } + + LogDebug("IntegrateImuMeasurements: Num imu measurements integrated: " << num_measurements_added); + LogDebug( + "IntegrateImuMeasurements: Total Num Imu Measurements after " + "integrating: " + << measurements_.size()); + return last_added_imu_measurement_time; +} + +void ImuIntegrator::RemoveOldMeasurements(const lc::Time new_start_time) { + for (auto measurement_it = measurements_.cbegin(); + measurement_it != measurements_.cend() && measurement_it->first < new_start_time;) { + measurement_it = measurements_.erase(measurement_it); + } +} + +boost::optional ImuIntegrator::IntegratedPim( + const gtsam::imuBias::ConstantBias& bias, const lc::Time start_time, const lc::Time end_time, + boost::shared_ptr params) const { + auto pim = Pim(bias, params); + const auto last_integrated_measurement_time = IntegrateImuMeasurements(start_time, end_time, pim); + if (!last_integrated_measurement_time) { + LogError("IntegratedPim: Failed to integrate imu measurments."); + return boost::none; + } + return pim; +} + +boost::shared_ptr 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 copied_params( + new gtsam::PreintegratedCombinedMeasurements::Params(*pim_params_)); + return copied_params; +} + +boost::optional ImuIntegrator::OldestTime() const { + if (Empty()) { + LogError("OldestTime: No measurements available."); + return boost::none; + } + return measurements_.cbegin()->first; +} + +boost::optional ImuIntegrator::LatestTime() const { + if (Empty()) { + LogError("LatestTime: No measurements available."); + return boost::none; + } + return measurements_.crbegin()->first; +} + +boost::optional ImuIntegrator::LatestMeasurement() const { + if (Empty()) { + LogError("LatestTime: No measurements available."); + 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); +} + +const std::map& ImuIntegrator::measurements() + const { + return measurements_; +} + +} // namespace imu_integration diff --git a/localization/imu_integration/src/latest_imu_integrator.cc b/localization/imu_integration/src/latest_imu_integrator.cc new file mode 100644 index 0000000000..4450f9af53 --- /dev/null +++ b/localization/imu_integration/src/latest_imu_integrator.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 +#include + +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/utilities.cc b/localization/imu_integration/src/utilities.cc new file mode 100644 index 0000000000..1098780f33 --- /dev/null +++ b/localization/imu_integration/src/utilities.cc @@ -0,0 +1,95 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include + +namespace imu_integration { +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace mc = msg_conversions; + +boost::optional Interpolate(const lm::ImuMeasurement& imu_measurement_a, + const lm::ImuMeasurement& imu_measurement_b, const lc::Time timestamp) { + if (timestamp < imu_measurement_a.timestamp || timestamp > imu_measurement_b.timestamp) { + LogError( + "Interpolate: Interpolation timestamp out of range of imu " + "measurements."); + return boost::none; + } + + const double alpha = + (timestamp - imu_measurement_a.timestamp) / (imu_measurement_b.timestamp - imu_measurement_a.timestamp); + const Eigen::Vector3d interpolated_acceleration = + (1.0 - alpha) * imu_measurement_a.acceleration + alpha * imu_measurement_b.acceleration; + const Eigen::Vector3d interpolated_angular_velocity = + (1.0 - alpha) * imu_measurement_a.angular_velocity + alpha * imu_measurement_b.angular_velocity; + + return lm::ImuMeasurement(interpolated_acceleration, interpolated_angular_velocity, timestamp); +} + +gtsam::PreintegratedCombinedMeasurements Pim( + const gtsam::imuBias::ConstantBias& bias, + const boost::shared_ptr& params) { + gtsam::PreintegratedCombinedMeasurements pim(params); + pim.resetIntegrationAndSetBias(bias); + return pim; +} + +void AddMeasurement(const lm::ImuMeasurement& imu_measurement, lc::Time& last_added_imu_measurement_time, + gtsam::PreintegratedCombinedMeasurements& pim) { + const double dt = imu_measurement.timestamp - last_added_imu_measurement_time; + // TODO(rsoussan): check if dt too large? Pass threshold param? + if (dt == 0) { + LogError("AddMeasurement: Timestamp difference 0, failed to add measurement."); + return; + } + pim.integrateMeasurement(imu_measurement.acceleration, imu_measurement.angular_velocity, dt); + last_added_imu_measurement_time = imu_measurement.timestamp; +} + +lc::CombinedNavState PimPredict(const lc::CombinedNavState& combined_nav_state, + const gtsam::PreintegratedCombinedMeasurements& pim) { + const gtsam::NavState predicted_nav_state = pim.predict(combined_nav_state.nav_state(), pim.biasHat()); + 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"); + params.filter.type = mc::LoadString(config, "imu_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"); +} +} // namespace imu_integration diff --git a/localization/lk_optical_flow/launch/lk_optical_flow.launch b/localization/lk_optical_flow/launch/lk_optical_flow.launch index 1ffcd6cac6..d05dce51f5 100644 --- a/localization/lk_optical_flow/launch/lk_optical_flow.launch +++ b/localization/lk_optical_flow/launch/lk_optical_flow.launch @@ -17,9 +17,11 @@ + + - \ No newline at end of file + diff --git a/localization/localization_common/CMakeLists.txt b/localization/localization_common/CMakeLists.txt new file mode 100644 index 0000000000..953e1db213 --- /dev/null +++ b/localization/localization_common/CMakeLists.txt @@ -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. + +project(localization_common) + +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GTSAM_LIBRARIES} camera config_reader msg_conversions + INCLUDE_DIRS include ${GTSAM_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} + CATKIN_DEPENDS + DEPENDS gtsam +) + +create_library(TARGET ${PROJECT_NAME} + LIBS ${catkin_LIBRARIES} camera config_reader gtsam msg_conversions + INC ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} + DEPS +) diff --git a/localization/localization_common/include/localization_common/averager.h b/localization/localization_common/include/localization_common/averager.h new file mode 100644 index 0000000000..e7165acb2b --- /dev/null +++ b/localization/localization_common/include/localization_common/averager.h @@ -0,0 +1,72 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_AVERAGER_H_ +#define LOCALIZATION_COMMON_AVERAGER_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace localization_common { +class Averager { + public: + explicit Averager(const std::string& averager_name = "", const std::string& type_name = "", + const std::string& units = "", const bool log_on_destruction = false); + ~Averager(); + void Update(const double value); + double average() const; + int count() const; + double last_value() const; + void Log() const; + void LogToFile(std::ofstream& ofstream) const; + void LogToCsv(std::ofstream& ofstream) const; + void LogEveryN(const int num_events_per_log) const; + void UpdateAndLog(const double value); + void UpdateAndLogEveryN(const double value, const int num_events_per_log); + void Vlog(const int level = 2) const; + void VlogEveryN(const int num_events_per_log, const int level) const; + + private: + std::string StatsString() const; + std::string CsvStatsString() const; + std::string LastValueString() const; + + std::string name_; + std::string type_name_; + std::string units_; + std::string spacer_; + double last_value_; + bool log_on_destruction_; + boost::accumulators::accumulator_set< + double, boost::accumulators::stats> + accumulator_; +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_AVERAGER_H_ diff --git a/localization/localization_common/include/localization_common/combined_nav_state.h b/localization/localization_common/include/localization_common/combined_nav_state.h new file mode 100644 index 0000000000..12e167de6c --- /dev/null +++ b/localization/localization_common/include/localization_common/combined_nav_state.h @@ -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. + */ + +#ifndef LOCALIZATION_COMMON_COMBINED_NAV_STATE_H_ +#define LOCALIZATION_COMMON_COMBINED_NAV_STATE_H_ + +#include + +#include +#include +#include +#include +#include +#include + +namespace localization_common { +struct CombinedNavStateNoise { + gtsam::SharedNoiseModel pose_noise; + gtsam::SharedNoiseModel velocity_noise; + gtsam::SharedNoiseModel bias_noise; +}; + +class CombinedNavState { + public: + CombinedNavState(const gtsam::NavState& nav_state, const gtsam::imuBias::ConstantBias& bias, const Time timestamp); + CombinedNavState(const gtsam::Pose3& pose, const gtsam::Velocity3& velocity, const gtsam::imuBias::ConstantBias& bias, + const Time timestamp); + CombinedNavState() = default; + Time timestamp() const { return timestamp_; } + const gtsam::NavState& nav_state() const { return nav_state_; } + gtsam::Pose3 pose() const { return nav_state().pose(); } + const gtsam::Velocity3& velocity() const { return nav_state().velocity(); } + const gtsam::imuBias::ConstantBias& bias() const { return bias_; } + + private: + gtsam::NavState nav_state_; + gtsam::imuBias::ConstantBias bias_; + Time timestamp_; +}; + +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_COMBINED_NAV_STATE_H_ diff --git a/localization/localization_common/include/localization_common/combined_nav_state_covariances.h b/localization/localization_common/include/localization_common/combined_nav_state_covariances.h new file mode 100644 index 0000000000..4db6a6570a --- /dev/null +++ b/localization/localization_common/include/localization_common/combined_nav_state_covariances.h @@ -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. + */ + +#ifndef LOCALIZATION_COMMON_COMBINED_NAV_STATE_COVARIANCES_H_ +#define LOCALIZATION_COMMON_COMBINED_NAV_STATE_COVARIANCES_H_ + +#include +#include + +namespace localization_common { +enum Confidence { kGood, kPoor, kLost }; +class CombinedNavStateCovariances { + public: + CombinedNavStateCovariances(const Eigen::Matrix& pose_covariance, + const Eigen::Matrix3d& velocity_covariance, + const Eigen::Matrix& bias_covariance); + CombinedNavStateCovariances(const Eigen::Vector3d& position_variances, const Eigen::Vector3d& orientation_variances, + const Eigen::Vector3d& velocity_variances, + const Eigen::Vector3d& accelerometer_bias_variances, + const Eigen::Vector3d& gyro_bias_variances); + CombinedNavStateCovariances() = default; + // create constructor from marginals! -> put this in + // localization_common! + Confidence PoseConfidence(const double position_log_det_threshold, const double orientation_log_det_threshold) const; + + const Eigen::Matrix& pose_covariance() const { return pose_covariance_; } + const Eigen::Matrix3d& velocity_covariance() const { return velocity_covariance_; } + const Eigen::Matrix& bias_covariance() const { return bias_covariance_; } + + // Diagonal Accessors for Loc msg + Eigen::Vector3d position_variances() const { return pose_covariance_.block<3, 3>(0, 0).diagonal(); } + Eigen::Vector3d orientation_variances() const { return pose_covariance_.block<3, 3>(3, 3).diagonal(); } + Eigen::Vector3d velocity_variances() const { return velocity_covariance_.diagonal(); } + Eigen::Vector3d gyro_bias_variances() const { return bias_covariance_.block<3, 3>(3, 3).diagonal(); } + Eigen::Vector3d accel_bias_variances() const { return bias_covariance_.block<3, 3>(0, 0).diagonal(); } + + double LogDeterminantPositionCovariance() const; + + double LogDeterminantOrientationCovariance() const; + + private: + Eigen::Matrix pose_covariance_; + Eigen::Matrix3d velocity_covariance_; + Eigen::Matrix bias_covariance_; +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_COMBINED_NAV_STATE_COVARIANCES_H_ diff --git a/localization/localization_common/include/localization_common/logger.h b/localization/localization_common/include/localization_common/logger.h new file mode 100644 index 0000000000..bb9673eb1b --- /dev/null +++ b/localization/localization_common/include/localization_common/logger.h @@ -0,0 +1,164 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_LOGGER_H_ +#define LOCALIZATION_COMMON_LOGGER_H_ + +#include + +#include + +#include +#include + +// Select logger here +// TODO(rsoussan): Add compile definition for this? +#define USE_ROS_LOGGING +// #define USE_GLOG_LOGGING + +#ifdef USE_ROS_LOGGING +#define __FILENAME__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__) + +// clang-format off +#define LogInfo(msg) \ + do { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_INFO_STREAM(ss.str()); \ + } while (0) + +#define LogWarning(msg) \ + do { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_WARN_STREAM(ss.str()); \ + } while (0) + +#define LogError(msg) \ + do { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_ERROR_STREAM(ss.str()); \ + } while (0) + +#define LogFatal(msg) \ + do { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_FATAL_STREAM(ss.str()); \ + } while (0) + +#define LogDebug(msg) \ + do { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_DEBUG_STREAM(ss.str()); \ + } while (0) + +#define LogInfoEveryN(n, msg) \ + do { \ + static int count = 0; \ + ++count; \ + if (count % n == 0) { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_INFO_STREAM(ss.str()); \ + } \ + } while (0) + +#define LogWarningEveryN(n, msg) \ + do { \ + static int count = 0; \ + ++count; \ + if (count % n == 0) { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_WARN_STREAM(ss.str()); \ + } \ + } while (0) + +#define LogErrorEveryN(n, msg) \ + do { \ + static int count = 0; \ + ++count; \ + if (count % n == 0) { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_ERROR_STREAM(ss.str()); \ + } \ + } while (0) + +#define LogDebugEveryN(n, msg) \ + do { \ + static int count = 0; \ + ++count; \ + if (count % n == 0) { \ + std::stringstream ss; \ + ss << __FILENAME__ << ":" << __LINE__ << ": " << msg << std::endl; \ + ROS_DEBUG_STREAM(ss.str()); \ + } \ + } while (0) + +#elif defined(USE_GLOG_LOGGING) +#define LogInfo(msg) \ + do { \ + LOG(INFO) << msg; \ + } while (0) +#define LogWarning(msg) \ + do { \ + LOG(WARNING) << msg; \ + } while (0) + +#define LogError(msg) \ + do { \ + LOG(ERROR) << msg; \ + } while (0) + +#define LogFatal(msg) \ + do { \ + LOG(FATAL) << msg; \ + } while (0) + +#define LogDebug(msg) \ + do { \ + VLOG(2) << msg; \ + } while (0) + +#define LogInfoEveryN(n, msg) \ + do { \ + LOG_EVERY_N(INFO, n) << msg; \ + } while (0) + +#define LogWarningEveryN(n, msg) \ + do { \ + LOG_EVERY_N(WARNING, n) << msg; \ + } while (0) + +#define LogErrorEveryN(n, msg) \ + do { \ + LOG_EVERY_N(ERROR, n) << msg; \ + } while (0) + +#define LogDebugEveryN(n, msg) \ + do { \ + DLOG_EVERY_N(INFO, n) << msg; \ + } while (0) + +#endif +// clang-format on +#endif // LOCALIZATION_COMMON_LOGGER_H_ diff --git a/localization/localization_common/include/localization_common/rate_timer.h b/localization/localization_common/include/localization_common/rate_timer.h new file mode 100644 index 0000000000..7e8cf3ccd4 --- /dev/null +++ b/localization/localization_common/include/localization_common/rate_timer.h @@ -0,0 +1,48 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_RATE_TIMER_H_ +#define LOCALIZATION_COMMON_RATE_TIMER_H_ + +#include +#include + +#include +#include + +namespace localization_common { +class RateTimer { + public: + explicit RateTimer(const std::string& timer_name = ""); + void Record(); + void Log() const; + void LogEveryN(const int num_events_per_log) const; + void VlogEveryN(const int num_events_per_log, const int level) const; + void RecordAndLog(); + void RecordAndLogEveryN(const int num_events_per_log); + void RecordAndVlogEveryN(const int num_events_per_log, const int level); + void Vlog(const int level = 2) const; + + private: + std::chrono::time_point start_time_; + Averager averager_; + int num_events_; +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_RATE_TIMER_H_ diff --git a/localization/localization_common/include/localization_common/ros_timer.h b/localization/localization_common/include/localization_common/ros_timer.h new file mode 100644 index 0000000000..105dba12c6 --- /dev/null +++ b/localization/localization_common/include/localization_common/ros_timer.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 LOCALIZATION_COMMON_ROS_TIMER_H_ +#define LOCALIZATION_COMMON_ROS_TIMER_H_ + +#include +#include + +#include +#include + +#include + +namespace localization_common { +class RosTimer { + public: + explicit RosTimer(const std::string& timer_name); + void Start(); + // Uses header time as start time and ros::Time::now as stop time + void HeaderDiff(const std_msgs::Header& header); + void Stop(); + void Log() const; + void Vlog(const int level = 2) const; + void LogEveryN(const int num_timing_events_per_log) const; + void VlogEveryN(const int num_timing_events_per_log, const int level = 2) const; + + private: + Averager averager_; + ros::Time start_time_; +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_ROS_TIMER_H_ diff --git a/localization/localization_common/include/localization_common/time.h b/localization/localization_common/include/localization_common/time.h new file mode 100644 index 0000000000..6e1c62d83e --- /dev/null +++ b/localization/localization_common/include/localization_common/time.h @@ -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. + */ + +#ifndef LOCALIZATION_COMMON_TIME_H_ +#define LOCALIZATION_COMMON_TIME_H_ + +namespace localization_common { +using Time = double; +Time GetTime(const int seconds, const int nanoseconds); +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_TIME_H_ diff --git a/localization/localization_common/include/localization_common/timer.h b/localization/localization_common/include/localization_common/timer.h new file mode 100644 index 0000000000..66deea7307 --- /dev/null +++ b/localization/localization_common/include/localization_common/timer.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_COMMON_TIMER_H_ +#define LOCALIZATION_COMMON_TIMER_H_ + +#include +#include + +#include +#include +#include + +namespace localization_common { +class Timer { + public: + explicit Timer(const std::string& timer_name = ""); + void Start(); + void Stop(); + void Log() const; + void LogToFile(std::ofstream& ofstream) const; + void LogToCsv(std::ofstream& ofstream) const; + void LogEveryN(const int num_events_per_log) const; + void VlogEveryN(const int num_events_per_log, const int level) const; + void StopAndLog(); + void StopAndLogEveryN(const int num_events_per_log); + void StopAndVlogEveryN(const int num_events_per_log, const int level); + void Vlog(const int level = 2) const; + double last_value() const; + + private: + std::chrono::time_point start_time_; + Averager averager_; +}; +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_TIMER_H_ diff --git a/localization/localization_common/include/localization_common/utilities.h b/localization/localization_common/include/localization_common/utilities.h new file mode 100644 index 0000000000..82ebd964b6 --- /dev/null +++ b/localization/localization_common/include/localization_common/utilities.h @@ -0,0 +1,128 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_UTILITIES_H_ +#define LOCALIZATION_COMMON_UTILITIES_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include + +namespace localization_common { +gtsam::Pose3 LoadTransform(config_reader::ConfigReader& config, const std::string& transform_config_name); + +gtsam::Vector3 LoadVector3(config_reader::ConfigReader& config, const std::string& config_name); + +gtsam::Cal3_S2 LoadCameraIntrinsics(config_reader::ConfigReader& config, const std::string& intrinsics_config_name); + +gtsam::Pose3 GtPose(const Eigen::Isometry3d& eigen_pose); + +Eigen::Isometry3d EigenPose(const CombinedNavState& combined_nav_state); + +Eigen::Isometry3d EigenPose(const gtsam::Pose3& pose); + +// Returns pose in body frame +gtsam::Pose3 GtPose(const ff_msgs::VisualLandmarks& vl_features, const gtsam::Pose3& sensor_T_body); + +// Returns pose in sensor frame +gtsam::Pose3 GtPose(const ff_msgs::VisualLandmarks& vl_features); + +// 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); + +ros::Time RosTimeFromHeader(const std_msgs::Header& header); + +Time TimeFromHeader(const std_msgs::Header& header); + +Time TimeFromRosTime(const ros::Time& time); + +void TimeToHeader(const Time timestamp, std_msgs::Header& header); + +gtsam::Pose3 PoseFromMsg(const geometry_msgs::PoseStamped& msg); + +gtsam::Pose3 PoseFromMsg(const geometry_msgs::Pose& msg_pose); + +void PoseToMsg(const gtsam::Pose3& pose, geometry_msgs::Pose& msg_pose); + +geometry_msgs::TransformStamped PoseToTF(const Eigen::Isometry3d& pose, const std::string& parent_frame, + const std::string& child_frame, const Time timestamp, + const std::string& platform_name = ""); + +geometry_msgs::TransformStamped PoseToTF(const gtsam::Pose3& pose, const std::string& parent_frame, + const std::string& child_frame, const Time timestamp, + const std::string& platform_name = ""); + +CombinedNavState CombinedNavStateFromMsg(const ff_msgs::GraphState& loc_msg); + +CombinedNavStateCovariances CombinedNavStateCovariancesFromMsg(const ff_msgs::GraphState& loc_msg); + +// Returns gravity corrected accelerometer measurement +gtsam::Vector3 RemoveGravityFromAccelerometerMeasurement(const gtsam::Vector3& global_F_gravity, + const gtsam::Pose3& body_T_imu, + const gtsam::Pose3& global_T_body, + const gtsam::Vector3& uncorrected_accelerometer_measurement); + +template +void CombinedNavStateToMsg(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); + msg_conversions::VectorToMsg(combined_nav_state.bias().gyroscope(), loc_msg.gyro_bias); + TimeToHeader(combined_nav_state.timestamp(), loc_msg.header); +} + +template +void CombinedNavStateCovariancesToMsg(const CombinedNavStateCovariances& covariances, LocMsgType& loc_msg) { + // Orientation (0-2) + msg_conversions::VariancesToCovDiag(covariances.orientation_variances(), &loc_msg.cov_diag[0]); + + // Gyro Bias (3-5) + msg_conversions::VariancesToCovDiag(covariances.gyro_bias_variances(), &loc_msg.cov_diag[3]); + + // Velocity (6-8) + msg_conversions::VariancesToCovDiag(covariances.velocity_variances(), &loc_msg.cov_diag[6]); + + // Accel Bias (9-11) + msg_conversions::VariancesToCovDiag(covariances.accel_bias_variances(), &loc_msg.cov_diag[9]); + + // Position (12-14) + msg_conversions::VariancesToCovDiag(covariances.position_variances(), &loc_msg.cov_diag[12]); +} + +} // namespace localization_common + +#endif // LOCALIZATION_COMMON_UTILITIES_H_ diff --git a/localization/localization_common/package.xml b/localization/localization_common/package.xml new file mode 100644 index 0000000000..defe706423 --- /dev/null +++ b/localization/localization_common/package.xml @@ -0,0 +1,17 @@ + + localization_common + 1.0.0 + + The localization common package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + diff --git a/localization/localization_common/readme.md b/localization/localization_common/readme.md new file mode 100644 index 0000000000..59cb151366 --- /dev/null +++ b/localization/localization_common/readme.md @@ -0,0 +1,11 @@ +\page localizationcommon Localization Common + +# Package Overview +Localization common provides several objects that are quite useful for various localization functions, along with various helper functions. Many of these helper functions make converting between gtsam and ros types required for ros messages easier, along with wrapping some parameter loading functions to provide a cleaner parameter loading interface. + +# Important Classes +## CombinedNavState +CombinedNavState extends the gtsam::NavState to include imu biases and a timestamp. This is the state that is estimated by the localizer. + +## Timer and Averager +The timer and averager classes provide easy ways to profile data and save statistics. Each maintains a rolling average to avoid overflow and optionally logs data to a log file. diff --git a/localization/localization_common/src/averager.cc b/localization/localization_common/src/averager.cc new file mode 100644 index 0000000000..e8b890e2f4 --- /dev/null +++ b/localization/localization_common/src/averager.cc @@ -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. + */ + +#include +#include + +namespace localization_common { +Averager::Averager(const std::string& name, const std::string& type_name, const std::string& units, + const bool log_on_destruction) + : name_(name), type_name_(type_name), units_(units), last_value_(0), log_on_destruction_(log_on_destruction) { + // Don't add a space if type name isn't specified + spacer_ = type_name_ == "" ? "" : " "; +} + +Averager::~Averager() { + if (log_on_destruction_) Log(); +} + +void Averager::Update(const double value) { + accumulator_(value); + last_value_ = value; +} + +double Averager::average() const { return boost::accumulators::mean(accumulator_); } + +int Averager::count() const { return boost::accumulators::count(accumulator_); } + +double Averager::last_value() const { return last_value_; } + +std::string Averager::LastValueString() const { + std::stringstream ss; + ss << name_ + spacer_ + type_name_ + ": " << last_value_ << spacer_ + units_; + return ss.str(); +} + +std::string Averager::StatsString() const { + std::stringstream ss; + ss << "Average " + name_ + spacer_ + type_name_ + ": " << average() << spacer_ + units_ + << ", min: " << boost::accumulators::min(accumulator_) << ", max: " << boost::accumulators::max(accumulator_) + << ", stddev: " << std::sqrt(boost::accumulators::variance(accumulator_)); + return ss.str(); +} + +std::string Averager::CsvStatsString() const { + std::stringstream ss; + // Format is average, min, max, stddev + // Spaces removed from name and replaced with underscores + std::string name = name_ + spacer_ + type_name_; + std::replace(name.begin(), name.end(), ' ', '_'); + ss << name << "," << average() << "," << boost::accumulators::min(accumulator_) << "," + << boost::accumulators::max(accumulator_) << "," << std::sqrt(boost::accumulators::variance(accumulator_)); + return ss.str(); +} + +void Averager::Log() const { + LogInfo(LastValueString()); + LogInfo(StatsString()); +} + +void Averager::LogToFile(std::ofstream& ofstream) const { ofstream << StatsString() << std::endl; } + +void Averager::LogToCsv(std::ofstream& ofstream) const { ofstream << CsvStatsString() << std::endl; } + +void Averager::UpdateAndLog(const double value) { + Update(value); + Log(); +} + +void Averager::UpdateAndLogEveryN(const double value, const int num_events_per_log) { + Update(value); + LogEveryN(num_events_per_log); +} + +void Averager::Vlog(const int level) const { + VLOG(level) << LastValueString(); + VLOG(level) << StatsString(); +} + +void Averager::LogEveryN(const int num_events_per_log) const { + if (count() % num_events_per_log == 0) { + Log(); + } +} + +void Averager::VlogEveryN(const int num_events_per_log, const int level) const { + if (count() % num_events_per_log == 0) { + Vlog(level); + } +} + +} // namespace localization_common diff --git a/localization/localization_common/src/combined_nav_state.cc b/localization/localization_common/src/combined_nav_state.cc new file mode 100644 index 0000000000..6c9cbe92f7 --- /dev/null +++ b/localization/localization_common/src/combined_nav_state.cc @@ -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. + */ + +#include + +namespace localization_common { +CombinedNavState::CombinedNavState(const gtsam::NavState& nav_state, const gtsam::imuBias::ConstantBias& bias, + const Time timestamp) + : nav_state_(nav_state), bias_(bias), timestamp_(timestamp) {} + +CombinedNavState::CombinedNavState(const gtsam::Pose3& pose, const gtsam::Velocity3& velocity, + const gtsam::imuBias::ConstantBias& bias, const Time timestamp) + : CombinedNavState(gtsam::NavState(pose, velocity), bias, timestamp) {} +} // namespace localization_common diff --git a/localization/localization_common/src/combined_nav_state_covariances.cc b/localization/localization_common/src/combined_nav_state_covariances.cc new file mode 100644 index 0000000000..2688dc5ce6 --- /dev/null +++ b/localization/localization_common/src/combined_nav_state_covariances.cc @@ -0,0 +1,62 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 + +namespace localization_common { +CombinedNavStateCovariances::CombinedNavStateCovariances(const Eigen::Matrix& pose_covariance, + const Eigen::Matrix3d& velocity_covariance, + const Eigen::Matrix& bias_covariance) + : pose_covariance_(pose_covariance), velocity_covariance_(velocity_covariance), bias_covariance_(bias_covariance) {} + +CombinedNavStateCovariances::CombinedNavStateCovariances(const Eigen::Vector3d& position_variances, + const Eigen::Vector3d& orientation_variances, + const Eigen::Vector3d& velocity_variances, + const Eigen::Vector3d& accelerometer_bias_variances, + const Eigen::Vector3d& gyro_bias_variances) { + pose_covariance_.block<3, 3>(0, 0) = position_variances.asDiagonal(); + pose_covariance_.block<3, 3>(3, 3) = orientation_variances.asDiagonal(); + velocity_covariance_ = velocity_variances.asDiagonal(); + bias_covariance_.block<3, 3>(0, 0) = accelerometer_bias_variances.asDiagonal(); + bias_covariance_.block<3, 3>(3, 3) = gyro_bias_variances.asDiagonal(); +} + +double CombinedNavStateCovariances::LogDeterminantPositionCovariance() const { + return std::log10(pose_covariance().block<3, 3>(0, 0).determinant()); +} + +double CombinedNavStateCovariances::LogDeterminantOrientationCovariance() const { + return std::log10(pose_covariance().block<3, 3>(3, 3).determinant()); +} + +Confidence CombinedNavStateCovariances::PoseConfidence(const double position_log_det_threshold, + const double orientation_log_det_threshold) const { + const double position_log_det = LogDeterminantPositionCovariance(); + const double orientation_log_det = std::log10(pose_covariance().block<3, 3>(3, 3).determinant()); + + const Confidence position_confidence = + position_log_det < position_log_det_threshold ? Confidence::kGood : Confidence::kPoor; + const Confidence orientation_confidence = + orientation_log_det < orientation_log_det_threshold ? Confidence::kGood : Confidence::kPoor; + + if (position_confidence == Confidence::kGood && orientation_confidence == Confidence::kGood) return Confidence::kGood; + // Lost if both confidences are bad, poor if only 1 is bad + if (position_confidence == Confidence::kPoor && orientation_confidence == Confidence::kPoor) return Confidence::kLost; + return Confidence::kPoor; +} +} // namespace localization_common diff --git a/localization/localization_common/src/rate_timer.cc b/localization/localization_common/src/rate_timer.cc new file mode 100644 index 0000000000..e9000e9ee1 --- /dev/null +++ b/localization/localization_common/src/rate_timer.cc @@ -0,0 +1,71 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 + +namespace localization_common { +RateTimer::RateTimer(const std::string& timer_name) : averager_(timer_name, "rate", "seconds"), num_events_(0) {} +void RateTimer::Record() { + const auto end_time = std::chrono::steady_clock::now(); + ++num_events_; + // First Recording, nothing to compare to yet + if (num_events_ == 1) { + start_time_ = end_time; + return; + } + + const double elapsed_time = std::chrono::duration(end_time - start_time_).count(); + averager_.Update(elapsed_time); + start_time_ = end_time; +} + +void RateTimer::RecordAndLog() { + Record(); + Log(); +} + +void RateTimer::RecordAndLogEveryN(const int num_events_per_log) { + Record(); + LogEveryN(num_events_per_log); +} + +void RateTimer::RecordAndVlogEveryN(const int num_events_per_log, const int level) { + Record(); + VlogEveryN(num_events_per_log, level); +} + +void RateTimer::Log() const { + if (num_events_ <= 1) return; + averager_.Log(); +} + +void RateTimer::LogEveryN(const int num_events_per_log) const { + if (num_events_ <= 1) return; + averager_.LogEveryN(num_events_per_log); +} + +void RateTimer::VlogEveryN(const int num_events_per_log, const int level) const { + if (num_events_ <= 1) return; + averager_.VlogEveryN(num_events_per_log, level); +} + +void RateTimer::Vlog(const int level) const { + if (num_events_ <= 1) return; + averager_.Vlog(level); +} +} // namespace localization_common diff --git a/localization/localization_common/src/ros_timer.cc b/localization/localization_common/src/ros_timer.cc new file mode 100644 index 0000000000..9e56aaf4e0 --- /dev/null +++ b/localization/localization_common/src/ros_timer.cc @@ -0,0 +1,41 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include + +namespace localization_common { +RosTimer::RosTimer(const std::string& timer_name) : averager_(timer_name, "time", "seconds"), start_time_(0) {} +void RosTimer::Start() { start_time_ = ros::Time::now(); } +void RosTimer::HeaderDiff(const std_msgs::Header& header) { + start_time_ = RosTimeFromHeader(header); + Stop(); +} + +void RosTimer::Stop() { + const auto end_time = ros::Time::now(); + const double elapsed_time = (end_time - start_time_).toSec(); + averager_.Update(elapsed_time); +} +void RosTimer::Log() const { averager_.Log(); } +void RosTimer::Vlog(const int level) const { averager_.Vlog(level); } +void RosTimer::LogEveryN(const int num_timing_events_per_log) const { averager_.LogEveryN(num_timing_events_per_log); } +void RosTimer::VlogEveryN(const int num_timing_events_per_log, const int level) const { + averager_.VlogEveryN(num_timing_events_per_log, level); +} +} // namespace localization_common diff --git a/localization/localization_common/src/time.cc b/localization/localization_common/src/time.cc new file mode 100644 index 0000000000..faae1fab89 --- /dev/null +++ b/localization/localization_common/src/time.cc @@ -0,0 +1,23 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 + +namespace localization_common { +Time GetTime(const int seconds, const int nanoseconds) { return seconds + 1e-9 * nanoseconds; } +} // namespace localization_common diff --git a/localization/localization_common/src/timer.cc b/localization/localization_common/src/timer.cc new file mode 100644 index 0000000000..e779971598 --- /dev/null +++ b/localization/localization_common/src/timer.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 + +namespace localization_common { +Timer::Timer(const std::string& timer_name) : averager_(timer_name, "time", "seconds") {} +void Timer::Start() { start_time_ = std::chrono::steady_clock::now(); } +void Timer::Stop() { + const auto end_time = std::chrono::steady_clock::now(); + const double elapsed_time = std::chrono::duration(end_time - start_time_).count(); + averager_.Update(elapsed_time); +} + +void Timer::Log() const { averager_.Log(); } + +void Timer::LogToFile(std::ofstream& ofstream) const { averager_.LogToFile(ofstream); } + +void Timer::LogToCsv(std::ofstream& ofstream) const { averager_.LogToCsv(ofstream); } + +void Timer::LogEveryN(const int num_events_per_log) const { averager_.LogEveryN(num_events_per_log); } + +void Timer::VlogEveryN(const int num_events_per_log, const int level) const { + averager_.VlogEveryN(num_events_per_log, level); +} + +void Timer::StopAndLog() { + Stop(); + Log(); +} + +void Timer::StopAndLogEveryN(const int num_events_per_log) { + Stop(); + LogEveryN(num_events_per_log); +} + +void Timer::StopAndVlogEveryN(const int num_events_per_log, const int level) { + Stop(); + VlogEveryN(num_events_per_log, level); +} + +void Timer::Vlog(const int level) const { averager_.Vlog(level); } + +double Timer::last_value() const { return averager_.last_value(); } +} // namespace localization_common diff --git a/localization/localization_common/src/utilities.cc b/localization/localization_common/src/utilities.cc new file mode 100644 index 0000000000..deeb7a01ab --- /dev/null +++ b/localization/localization_common/src/utilities.cc @@ -0,0 +1,158 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include +#include + +#include +#include + +#include +#include + +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); + return GtPose(body_T_sensor); +} + +gtsam::Vector3 LoadVector3(config_reader::ConfigReader& config, const std::string& config_name) { + Eigen::Vector3d vec; + mc::config_read_vector(&config, 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(); + // Assumes zero skew + return gtsam::Cal3_S2(intrinsics(0, 0), intrinsics(1, 1), 0, intrinsics(0, 2), intrinsics(1, 2)); +} + +gtsam::Pose3 GtPose(const Eigen::Isometry3d& eigen_pose) { + return gtsam::Pose3(Eigen::Ref(eigen_pose.matrix())); +} + +Eigen::Isometry3d EigenPose(const CombinedNavState& combined_nav_state) { return EigenPose(combined_nav_state.pose()); } + +Eigen::Isometry3d EigenPose(const gtsam::Pose3& pose) { return Eigen::Isometry3d(pose.matrix()); } + +gtsam::Pose3 GtPose(const ff_msgs::VisualLandmarks& vl_features, const gtsam::Pose3& sensor_T_body) { + const gtsam::Pose3 vl_global_T_sensor = GtPose(vl_features); + const gtsam::Pose3 vl_global_T_body = vl_global_T_sensor * sensor_T_body; + return vl_global_T_body; +} + +gtsam::Pose3 GtPose(const ff_msgs::VisualLandmarks& vl_features) { + gtsam::Point3 vl_global_t_sensor(vl_features.pose.position.x, vl_features.pose.position.y, + vl_features.pose.position.z); + const gtsam::Quaternion vl_global_Q_sensor(vl_features.pose.orientation.w, vl_features.pose.orientation.x, + vl_features.pose.orientation.y, vl_features.pose.orientation.z); + const gtsam::Rot3 vl_global_R_sensor(vl_global_Q_sensor); + return gtsam::Pose3(vl_global_R_sensor, vl_global_t_sensor); +} + +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."); +} + +void SetEnvironmentConfigs(const std::string& astrobee_configs_path, const std::string& world, + const std::string& 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_WORLD", world.c_str(), true); +} + +ros::Time RosTimeFromHeader(const std_msgs::Header& header) { return ros::Time(header.stamp.sec, header.stamp.nsec); } + +Time TimeFromHeader(const std_msgs::Header& header) { return GetTime(header.stamp.sec, header.stamp.nsec); } + +Time TimeFromRosTime(const ros::Time& time) { return GetTime(time.sec, time.nsec); } + +void TimeToHeader(const Time timestamp, std_msgs::Header& header) { + ros::Time ros_time(timestamp); + header.stamp.sec = ros_time.sec; + header.stamp.nsec = ros_time.nsec; +} + +gtsam::Pose3 PoseFromMsg(const geometry_msgs::PoseStamped& msg) { return PoseFromMsg(msg.pose); } + +gtsam::Pose3 PoseFromMsg(const geometry_msgs::Pose& msg_pose) { + return gtsam::Pose3(mc::RotationFromMsg(msg_pose.orientation), + mc::VectorFromMsg(msg_pose.position)); +} + +void PoseToMsg(const gtsam::Pose3& pose, geometry_msgs::Pose& msg_pose) { + const gtsam::Quaternion quaternion = pose.rotation().toQuaternion(); + mc::RotationToMsg(quaternion, msg_pose.orientation); + mc::VectorToMsg(pose.translation(), msg_pose.position); +} + +geometry_msgs::TransformStamped PoseToTF(const gtsam::Pose3& pose, const std::string& parent_frame, + const std::string& child_frame, const Time timestamp, + const std::string& platform_name) { + return PoseToTF(EigenPose(pose), parent_frame, child_frame, timestamp, platform_name); +} + +geometry_msgs::TransformStamped PoseToTF(const Eigen::Isometry3d& pose, const std::string& parent_frame, + const std::string& child_frame, const Time timestamp, + const std::string& platform_name) { + geometry_msgs::TransformStamped transform; + TimeToHeader(timestamp, transform.header); + transform.header.frame_id = parent_frame; + transform.child_frame_id = platform_name + child_frame; + mc::EigenPoseToMsg(pose, transform.transform); + return transform; +} + +CombinedNavState CombinedNavStateFromMsg(const ff_msgs::GraphState& loc_msg) { + const auto pose = PoseFromMsg(loc_msg.pose); + const auto velocity = mc::VectorFromMsg(loc_msg.velocity); + const auto accel_bias = mc::VectorFromMsg(loc_msg.accel_bias); + const auto gyro_bias = mc::VectorFromMsg(loc_msg.gyro_bias); + const auto timestamp = TimeFromHeader(loc_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); +} + +gtsam::Vector3 RemoveGravityFromAccelerometerMeasurement(const gtsam::Vector3& global_F_gravity, + const gtsam::Pose3& body_T_imu, + const gtsam::Pose3& global_T_body, + const gtsam::Vector3& uncorrected_accelerometer_measurement) { + const gtsam::Rot3 global_R_imu = global_T_body.rotation() * body_T_imu.rotation(); + const gtsam::Vector3 imu_F_gravity = global_R_imu.inverse() * global_F_gravity; + // Add gravity correction to measurement to offset negatively measured gravity + return (uncorrected_accelerometer_measurement + imu_F_gravity); +} +} // namespace localization_common diff --git a/localization/localization_manager/launch/localization_manager.launch b/localization/localization_manager/launch/localization_manager.launch index f5c7001030..2d342bb262 100644 --- a/localization/localization_manager/launch/localization_manager.launch +++ b/localization/localization_manager/launch/localization_manager.launch @@ -17,9 +17,11 @@ + + diff --git a/localization/localization_measurements/CMakeLists.txt b/localization/localization_measurements/CMakeLists.txt new file mode 100644 index 0000000000..479e3665f3 --- /dev/null +++ b/localization/localization_measurements/CMakeLists.txt @@ -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. + +project(localization_measurements) + +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GTSAM_LIBRARIES} localization_common + INCLUDE_DIRS include ${GTSAM_INCLUDE_DIR} + CATKIN_DEPENDS + DEPENDS gtsam ff_msgs +) + +create_library(TARGET ${PROJECT_NAME} + LIBS ${catkin_LIBRARIES} gtsam localization_common + INC ${catkin_INCLUDE_DIRS} + DEPS ff_msgs +) diff --git a/localization/localization_measurements/include/localization_measurements/feature_point.h b/localization/localization_measurements/include/localization_measurements/feature_point.h new file mode 100644 index 0000000000..8de06d54a7 --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/feature_point.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_FEATURE_POINT_H_ +#define LOCALIZATION_MEASUREMENTS_FEATURE_POINT_H_ + +#include + +#include + +#include + +namespace localization_measurements { +using FeatureId = int; +using ImageId = int; + +struct FeaturePoint { + FeaturePoint(const double u, const double v, const ImageId image_id, const FeatureId feature_id, + const localization_common::Time timestamp) + : image_point(u, v), image_id(image_id), feature_id(feature_id), timestamp(timestamp) {} + FeaturePoint() {} + gtsam::Point2 image_point; + ImageId image_id; + FeatureId feature_id; + localization_common::Time timestamp; + + private: + // Serialization function + friend class boost::serialization::access; + template + 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(timestamp); + } +}; + +using FeaturePoints = std::vector; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_FEATURE_POINT_H_ diff --git a/localization/localization_measurements/include/localization_measurements/feature_points_measurement.h b/localization/localization_measurements/include/localization_measurements/feature_points_measurement.h new file mode 100644 index 0000000000..d93d407c7e --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/feature_points_measurement.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 LOCALIZATION_MEASUREMENTS_FEATURE_POINTS_MEASUREMENT_H_ +#define LOCALIZATION_MEASUREMENTS_FEATURE_POINTS_MEASUREMENT_H_ + +#include +#include +#include + +#include + +namespace localization_measurements { +struct FeaturePointsMeasurement : public Measurement { + FeaturePoints feature_points; +}; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_FEATURE_POINTS_MEASUREMENT_H_ diff --git a/localization/localization_measurements/include/localization_measurements/image_point.h b/localization/localization_measurements/include/localization_measurements/image_point.h new file mode 100644 index 0000000000..669e1372f2 --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/image_point.h @@ -0,0 +1,28 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_IMAGE_POINT_H_ +#define LOCALIZATION_MEASUREMENTS_IMAGE_POINT_H_ + +#include + +namespace localization_measurements { +using ImagePoint = gtsam::Point2; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_IMAGE_POINT_H_ diff --git a/localization/localization_measurements/include/localization_measurements/imu_measurement.h b/localization/localization_measurements/include/localization_measurements/imu_measurement.h new file mode 100644 index 0000000000..60075a803e --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/imu_measurement.h @@ -0,0 +1,50 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_IMU_MEASUREMENT_H_ +#define LOCALIZATION_MEASUREMENTS_IMU_MEASUREMENT_H_ + +#include +#include + +#include + +#include + +namespace localization_measurements { +struct ImuMeasurement : public Measurement { + explicit ImuMeasurement(const sensor_msgs::Imu& imu_msg) { + acceleration.x() = imu_msg.linear_acceleration.x; + acceleration.y() = imu_msg.linear_acceleration.y; + acceleration.z() = imu_msg.linear_acceleration.z; + angular_velocity.x() = imu_msg.angular_velocity.x; + angular_velocity.y() = imu_msg.angular_velocity.y; + angular_velocity.z() = imu_msg.angular_velocity.z; + // Ros headers are stored as seconds and nanoseconds + timestamp = imu_msg.header.stamp.sec + 1e-9 * imu_msg.header.stamp.nsec; + } + ImuMeasurement(const Eigen::Vector3d& acceleration, const Eigen::Vector3d& angular_velocity, + const localization_common::Time timestamp) + : Measurement(timestamp), acceleration(acceleration), angular_velocity(angular_velocity) {} + + Eigen::Vector3d acceleration; + Eigen::Vector3d angular_velocity; +}; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_IMU_MEASUREMENT_H_ diff --git a/localization/localization_measurements/include/localization_measurements/map_point.h b/localization/localization_measurements/include/localization_measurements/map_point.h new file mode 100644 index 0000000000..42e072e550 --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/map_point.h @@ -0,0 +1,28 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_MAP_POINT_H_ +#define LOCALIZATION_MEASUREMENTS_MAP_POINT_H_ + +#include + +namespace localization_measurements { +using MapPoint = gtsam::Point3; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_MAP_POINT_H_ diff --git a/localization/localization_measurements/include/localization_measurements/matched_projection.h b/localization/localization_measurements/include/localization_measurements/matched_projection.h new file mode 100644 index 0000000000..963d250d5b --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/matched_projection.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 LOCALIZATION_MEASUREMENTS_MATCHED_PROJECTION_H_ +#define LOCALIZATION_MEASUREMENTS_MATCHED_PROJECTION_H_ + +#include +#include +#include + +#include + +namespace localization_measurements { +struct MatchedProjection { + MatchedProjection(const ImagePoint& image_point, const MapPoint& map_point, const localization_common::Time timestamp) + : image_point(image_point), map_point(map_point), timestamp(timestamp) {} + ImagePoint image_point; + MapPoint map_point; + localization_common::Time timestamp; +}; + +using MatchedProjections = std::vector; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_MATCHED_PROJECTION_H_ diff --git a/localization/localization_measurements/include/localization_measurements/matched_projections_measurement.h b/localization/localization_measurements/include/localization_measurements/matched_projections_measurement.h new file mode 100644 index 0000000000..38f2bc675b --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/matched_projections_measurement.h @@ -0,0 +1,36 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_MATCHED_PROJECTIONS_MEASUREMENT_H_ +#define LOCALIZATION_MEASUREMENTS_MATCHED_PROJECTIONS_MEASUREMENT_H_ + +#include +#include +#include + +#include + +namespace localization_measurements { +struct MatchedProjectionsMeasurement : public Measurement { + MatchedProjections matched_projections; + // TODO(soussan): put this somewhere else? + gtsam::Pose3 global_T_cam; +}; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_MATCHED_PROJECTIONS_MEASUREMENT_H_ diff --git a/localization/localization_measurements/include/localization_measurements/measurement.h b/localization/localization_measurements/include/localization_measurements/measurement.h new file mode 100644 index 0000000000..e871d48f1a --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/measurement.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 LOCALIZATION_MEASUREMENTS_MEASUREMENT_H_ +#define LOCALIZATION_MEASUREMENTS_MEASUREMENT_H_ + +#include + +namespace localization_measurements { +struct Measurement { + explicit Measurement(localization_common::Time timestamp) : timestamp(timestamp) {} + Measurement() = default; + virtual ~Measurement() = default; + localization_common::Time timestamp; +}; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_MEASUREMENT_H_ diff --git a/localization/localization_measurements/include/localization_measurements/measurement_conversions.h b/localization/localization_measurements/include/localization_measurements/measurement_conversions.h new file mode 100644 index 0000000000..d6ab2d4867 --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/measurement_conversions.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 LOCALIZATION_MEASUREMENTS_MEASUREMENT_CONVERSIONS_H_ +#define LOCALIZATION_MEASUREMENTS_MEASUREMENT_CONVERSIONS_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace localization_measurements { +MatchedProjectionsMeasurement MakeMatchedProjectionsMeasurement(const ff_msgs::VisualLandmarks& visual_landmarks); + +MatchedProjectionsMeasurement FrameChangeMatchedProjectionsMeasurement( + const MatchedProjectionsMeasurement& matched_projections_measurement, + const gtsam::Pose3& new_frame_T_measurement_origin); + +FeaturePointsMeasurement MakeFeaturePointsMeasurement(const ff_msgs::Feature2dArray& optical_flow_tracks); +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_MEASUREMENT_CONVERSIONS_H_ diff --git a/localization/localization_measurements/package.xml b/localization/localization_measurements/package.xml new file mode 100644 index 0000000000..d1ec591cc3 --- /dev/null +++ b/localization/localization_measurements/package.xml @@ -0,0 +1,19 @@ + + localization_measurements + 1.0.0 + + The localization measurements package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + ff_msgs + ff_msgs + diff --git a/localization/localization_measurements/readme.md b/localization/localization_measurements/readme.md new file mode 100644 index 0000000000..778840b10c --- /dev/null +++ b/localization/localization_measurements/readme.md @@ -0,0 +1,5 @@ +\page localizationmeasurements Localization Measurements + +# Package Overview +Localization measurements provides an inheritance class for sensor data used by the localizer. Each measurement has a timestamp, and a measurement fully encapsulates the data provided by a sensor message. Information used by multiple measurements are given their own objects for modularity. Whenever new sensor information will be used by the localizer, it is recommended to add a xxxx_measurement object if one does not already exist. +Measurements are converted from ros msgs to measurement objects in the measurements\_conversion file. Since this may happen in multiple places (for instance the localizer and also an offline localization tool), it is recommened to add conversion here to be used by other packages. diff --git a/localization/localization_measurements/src/measurement_conversions.cc b/localization/localization_measurements/src/measurement_conversions.cc new file mode 100644 index 0000000000..ddc7d6e62c --- /dev/null +++ b/localization/localization_measurements/src/measurement_conversions.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 +#include + +namespace localization_measurements { +namespace lc = localization_common; +MatchedProjectionsMeasurement MakeMatchedProjectionsMeasurement(const ff_msgs::VisualLandmarks& visual_landmarks) { + MatchedProjectionsMeasurement matched_projections_measurement; + matched_projections_measurement.matched_projections.reserve(visual_landmarks.landmarks.size()); + const lc::Time timestamp = lc::GetTime(visual_landmarks.header.stamp.sec, visual_landmarks.header.stamp.nsec); + matched_projections_measurement.timestamp = timestamp; + + for (const auto& landmark : visual_landmarks.landmarks) { + const ImagePoint image_point(landmark.u, landmark.v); + const MapPoint map_point(landmark.x, landmark.y, landmark.z); + matched_projections_measurement.matched_projections.emplace_back(image_point, map_point, timestamp); + } + + matched_projections_measurement.global_T_cam = lc::GtPose(visual_landmarks); + + return matched_projections_measurement; +} + +MatchedProjectionsMeasurement FrameChangeMatchedProjectionsMeasurement( + const MatchedProjectionsMeasurement& matched_projections_measurement, + const gtsam::Pose3& new_frame_T_measurement_origin) { + auto frame_changed_measurement = matched_projections_measurement; + for (auto& matched_projection : frame_changed_measurement.matched_projections) { + matched_projection.map_point = new_frame_T_measurement_origin * matched_projection.map_point; + } + frame_changed_measurement.global_T_cam = new_frame_T_measurement_origin * frame_changed_measurement.global_T_cam; + return frame_changed_measurement; +} + +FeaturePointsMeasurement MakeFeaturePointsMeasurement(const ff_msgs::Feature2dArray& optical_flow_feature_points) { + FeaturePointsMeasurement feature_points_measurement; + feature_points_measurement.feature_points.reserve(optical_flow_feature_points.feature_array.size()); + lc::Time timestamp = + lc::GetTime(optical_flow_feature_points.header.stamp.sec, optical_flow_feature_points.header.stamp.nsec); + feature_points_measurement.timestamp = timestamp; + // TODO(rsoussan): put this somewhere else? + static int image_id = 0; + ++image_id; + + 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)); + } + + return feature_points_measurement; +} +} // namespace localization_measurements diff --git a/localization/localization_node/launch/localization_node.launch b/localization/localization_node/launch/localization_node.launch index 1a93de5cc4..2dcef26881 100644 --- a/localization/localization_node/launch/localization_node.launch +++ b/localization/localization_node/launch/localization_node.launch @@ -17,9 +17,11 @@ + + diff --git a/localization/marker_tracking/ros/src/marker_tracker.cc b/localization/marker_tracking/ros/src/marker_tracker.cc index c22a8ca6ba..9340dd0ccd 100644 --- a/localization/marker_tracking/ros/src/marker_tracker.cc +++ b/localization/marker_tracking/ros/src/marker_tracker.cc @@ -195,9 +195,8 @@ bool MarkerTracker::IsMarkerValid(const alvar::MarkerData & marker) { */ void MarkerTracker::VideoCallback(const sensor_msgs::ImageConstPtr& image_msg) { ff_msgs::CameraRegistration r; - ros::Time timestamp = ros::Time::now(); r.header = std_msgs::Header(); - r.header.stamp = timestamp; + r.header.stamp = image_msg->header.stamp; r.camera_id = 0; reg_publisher_.publish(r); ros::spinOnce(); @@ -225,7 +224,7 @@ void MarkerTracker::VideoCallback(const sensor_msgs::ImageConstPtr& image_msg) { // Identify the AR tags that we have seen and transmit their positions. ff_msgs::VisualLandmarks msg; msg.header = std_msgs::Header(); - msg.header.stamp = ros::Time::now(); + msg.header.stamp = image_msg->header.stamp; msg.header.frame_id = "dock/body"; msg.camera_id = 0; msg.landmarks.resize(4 * valid_marker_count); diff --git a/localization/readme.md b/localization/readme.md index 908638662d..51650526a6 100644 --- a/localization/readme.md +++ b/localization/readme.md @@ -1,21 +1,31 @@ \page localization Localization -This folder consists of various visual feature detection algorithms, -which are then integrated into the EKF, part of \ref gnc. +This folder consists of various sensing algorithms which serve as input to localization, along with the graph-based localizer and helper libraries for components of its functionality. It also contains libraries for other localization functionality, such as imu augmentation, a ground truth localizer primarily for use with simulation, and a vive localization package. +\subpage camera -\subpage localization_manager - -\subpage localization_node - -\subpage opticalflow +\subpage graphlocalizer -\subpage camera +\subpage groundtruthlocalizer \subpage handrail +\subpage imuaugmentor + +\subpage imuintegration + \subpage interestpoint +\subpage localizationcommon + +\subpage localization_manager + +\subpage localizationmeasurements + +\subpage localization_node + +\subpage opticalflow + \subpage markertracking \subpage sparsemapping diff --git a/localization/sparse_mapping/include/sparse_mapping/tensor.h b/localization/sparse_mapping/include/sparse_mapping/tensor.h index be869fb456..348c669380 100644 --- a/localization/sparse_mapping/include/sparse_mapping/tensor.h +++ b/localization/sparse_mapping/include/sparse_mapping/tensor.h @@ -122,7 +122,7 @@ namespace sparse_mapping { * Register the map to the world coordinate system or verify * how well registration did. **/ - void RegistrationOrVerification(std::vector const& data_files, + double RegistrationOrVerification(std::vector const& data_files, bool verification, sparse_mapping::SparseMap * s); diff --git a/localization/sparse_mapping/src/tensor.cc b/localization/sparse_mapping/src/tensor.cc index ff27002954..8efc3ef245 100644 --- a/localization/sparse_mapping/src/tensor.cc +++ b/localization/sparse_mapping/src/tensor.cc @@ -1529,7 +1529,7 @@ void ExtractSubmap(std::vector * keep_ptr, // Register a map to world coordinates from user-supplied data, or simply // verify how well the map performs with this data. -void RegistrationOrVerification(std::vector const& data_files, +double RegistrationOrVerification(std::vector const& data_files, bool verification, sparse_mapping::SparseMap * map) { // Get the interest points in the images, and their positions in @@ -1725,7 +1725,7 @@ void RegistrationOrVerification(std::vector const& data_files, } if (verification) - return; + return 0; // Find the transform from the computed map coordinate system // to the world coordinate system. @@ -1771,6 +1771,7 @@ void RegistrationOrVerification(std::vector const& data_files, << images[id1] << ' ' << images[id2] << std::endl; } + return scale; } void PrintTrackStats(std::vector >const& pid_to_cid_fid, diff --git a/management/executive/src/executive.cc b/management/executive/src/executive.cc index c08a222f83..2b2c69e3f5 100644 --- a/management/executive/src/executive.cc +++ b/management/executive/src/executive.cc @@ -2761,6 +2761,9 @@ bool Executive::SetTelemetryRate(ff_msgs::CommandStampedPtr const& cmd) { } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_TELEMETRY_TYPE_POSITION) { set_rate_srv.request.which = ff_msgs::SetRate::Request::POSITION; + } else if (cmd->args[0].s == + CommandConstants::PARAM_NAME_TELEMETRY_TYPE_SPARSE_MAPPING_POSE) { + set_rate_srv.request.which = ff_msgs::SetRate::Request::SPARSE_MAPPING_POSE; } else { state_->AckCmd(cmd->cmd_id, ff_msgs::AckCompletedStatus::BAD_SYNTAX, diff --git a/management/sys_monitor/src/sys_monitor.cc b/management/sys_monitor/src/sys_monitor.cc index 5b2fe1b1e5..afc3e3f6f0 100644 --- a/management/sys_monitor/src/sys_monitor.cc +++ b/management/sys_monitor/src/sys_monitor.cc @@ -21,7 +21,7 @@ namespace sys_monitor { SysMonitor::SysMonitor() : ff_util::FreeFlyerNodelet(NODE_SYS_MONITOR, false), - time_diff_node_("ekf"), + time_diff_node_("imu_aug"), time_diff_fault_triggered_(false), pub_queue_size_(10), sub_queue_size_(100), @@ -170,7 +170,7 @@ void SysMonitor::HeartbeatCallback(ff_msgs::HeartbeatConstPtr const& hb) { RemoveFault(wd->fault_id()); } - // Check time drift, use time in ekf heartbeat + // Check time drift, use time in imu_aug heartbeat if (hb->node == time_diff_node_) { float time_diff_sec = (ros::Time::now() - hb->header.stamp).toSec(); PublishTimeDiff(time_diff_sec); @@ -544,7 +544,7 @@ bool SysMonitor::ReadParams() { if (!config_params_.GetStr("time_diff_node", &time_diff_node_)) { NODELET_WARN("Unable to read time diff node name."); - time_diff_node_ = "ekf"; + time_diff_node_ = "imu_aug"; } if (!config_params_.GetPosReal("time_drift_thres_sec", diff --git a/scripts/docker/astrobee_base_kinetic.Dockerfile b/scripts/docker/astrobee_base_kinetic.Dockerfile index 0df88613c3..db45423629 100644 --- a/scripts/docker/astrobee_base_kinetic.Dockerfile +++ b/scripts/docker/astrobee_base_kinetic.Dockerfile @@ -1,4 +1,9 @@ -# This will set up an Astrobee docker container using the non-NASA install instructions. +# This will set up an Astrobee kinetic docker container using the non-NASA install +# instructions. +# This image is the base, meaning that it contains all the installation context, +# but it doesn't copy or build the entire code. +# This image builds on top of the base kinetic image building the code. + # You must set the docker context to be the repository root directory FROM nvidia/opengl:1.0-glvnd-runtime-ubuntu16.04 diff --git a/scripts/docker/astrobee_base_melodic.Dockerfile b/scripts/docker/astrobee_base_melodic.Dockerfile index eae8912380..ad43aaec92 100755 --- a/scripts/docker/astrobee_base_melodic.Dockerfile +++ b/scripts/docker/astrobee_base_melodic.Dockerfile @@ -1,4 +1,7 @@ -# This will set up an Astrobee docker container using the non-NASA install instructions. +# This will set up an Astrobee melodic docker container using the non-NASA install +# instructions. +# This image is the base, meaning that it contains all the installation context, +# but it doesn't copy or build the entire code. # You must set the docker context to be the repository root directory FROM nvidia/opengl:1.0-glvnd-runtime-ubuntu18.04 diff --git a/scripts/docker/astrobee_kinetic.Dockerfile b/scripts/docker/astrobee_kinetic.Dockerfile index 286d5caa57..6b12ead42c 100644 --- a/scripts/docker/astrobee_kinetic.Dockerfile +++ b/scripts/docker/astrobee_kinetic.Dockerfile @@ -1,4 +1,5 @@ -# This will set up an Astrobee docker container using the non-NASA install instructions. +# This will set up an Astrobee kinetic docker container using the non-NASA install instructions. +# This image builds on top of the base kinetic image building the code. # You must set the docker context to be the repository root directory FROM astrobee/astrobee:base-latest-kinetic diff --git a/scripts/docker/astrobee_melodic.Dockerfile b/scripts/docker/astrobee_melodic.Dockerfile index 54f361eb44..71a451551b 100755 --- a/scripts/docker/astrobee_melodic.Dockerfile +++ b/scripts/docker/astrobee_melodic.Dockerfile @@ -1,4 +1,5 @@ -# This will set up an Astrobee docker container using the non-NASA install instructions. +# This will set up an Astrobee melodic docker container using the non-NASA install instructions. +# This image builds on top of the base melodic image building the code. # You must set the docker context to be the repository root directory FROM astrobee/astrobee:base-latest-melodic diff --git a/scripts/docker/build.sh b/scripts/docker/build.sh index fb769a7e84..0fb3f07858 100755 --- a/scripts/docker/build.sh +++ b/scripts/docker/build.sh @@ -29,9 +29,6 @@ ubuntu18=0 while [ "$1" != "" ]; do case $1 in - -a | --astrobee_source_dir ) shift - astrobee_source=$1 - ;; -n | --ubuntu18 ) ubuntu18=1 ;; -h | --help ) usage @@ -46,20 +43,20 @@ done thisdir=$(dirname "$(readlink -f "$0")") rootdir=${thisdir}/../.. -echo "Astrobee path: "${astrobee_source:-${rootdir}/} +echo "Astrobee path: "${${rootdir}/} if [ $ubuntu18 == 0 ]; then - docker build ${astrobee_source:-${rootdir}/} \ - -f ${astrobee_source:-${rootdir}/}scripts/docker/astrobee_base_kinetic.Dockerfile \ + docker build ${${rootdir}/} \ + -f ${${rootdir}/}scripts/docker/astrobee_base_kinetic.Dockerfile \ -t astrobee/astrobee:base-latest-kinetic - docker build ${astrobee_source:-${rootdir}/} \ - -f ${astrobee_source:-${rootdir}/}scripts/docker/astrobee_kinetic.Dockerfile \ + docker build ${${rootdir}/} \ + -f ${${rootdir}/}scripts/docker/astrobee_kinetic.Dockerfile \ -t astrobee/astrobee:latest-kinetic else - docker build ${astrobee_source:-${rootdir}/} \ - -f ${astrobee_source:-${rootdir}/}scripts/docker/astrobee_base_melodic.Dockerfile \ + docker build ${${rootdir}/} \ + -f ${${rootdir}/}scripts/docker/astrobee_base_melodic.Dockerfile \ -t astrobee/astrobee:base-latest-melodic - docker build ${astrobee_source:-${rootdir}/} \ - -f ${astrobee_source:-${rootdir}/}scripts/docker/astrobee_melodic.Dockerfile \ + docker build ${${rootdir}/} \ + -f ${${rootdir}/}scripts/docker/astrobee_melodic.Dockerfile \ -t astrobee/astrobee:latest-melodic fi diff --git a/scripts/docker/cross_compile/astrobee_cross.Dockerfile b/scripts/docker/cross_compile/astrobee_cross.Dockerfile index e2f7691972..eda0e081e0 100644 --- a/scripts/docker/cross_compile/astrobee_cross.Dockerfile +++ b/scripts/docker/cross_compile/astrobee_cross.Dockerfile @@ -30,19 +30,3 @@ ARG ARMHF_TOOLCHAIN=/arm_cross/toolchain/gcc # Cross-compile RUN ./src/astrobee/scripts/configure.sh -a -p /opt/astrobee -b /build RUN cd /build && make -j4 install - -#Add new sudo user -ENV USERNAME astrobee -RUN useradd -m $USERNAME && \ - echo "$USERNAME:$USERNAME" | chpasswd && \ - usermod --shell /bin/bash $USERNAME && \ - usermod -aG sudo $USERNAME && \ - echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers.d/$USERNAME && \ - chmod 0440 /etc/sudoers.d/$USERNAME && \ - # Replace 1000 with your user/group id - usermod --uid 1000 $USERNAME && \ - groupmod --gid 1000 $USERNAME - -#Add the entrypoint for docker -RUN echo "#!/bin/bash\nset -e\n\nsource \"/opt/ros/kinetic/setup.bash\"\nsource \"/build/astrobee/devel/setup.bash\"\nexport ASTROBEE_CONFIG_DIR=\"/src/astrobee/astrobee/config\"\nexec \"\$@\"" > /astrobee_init.sh && \ - chmod +x /astrobee_init.sh diff --git a/scripts/docker/cross_compile/astrobee_debian.Dockerfile b/scripts/docker/cross_compile/astrobee_debian.Dockerfile new file mode 100644 index 0000000000..ad78ca05f1 --- /dev/null +++ b/scripts/docker/cross_compile/astrobee_debian.Dockerfile @@ -0,0 +1,39 @@ +# This will set up an Astrobee docker container using the non-NASA install instructions. +# You must set the docker context to be the repository root directory + +FROM astrobee/astrobee:base-cross + +# Copy the setup scripts +COPY ./scripts/setup/*.sh /setup/astrobee/ + +# this command is expected to have output on stderr, so redirect to suppress warning +RUN /setup/astrobee/add_ros_repository.sh >/dev/null 2>&1 + +RUN apt-get update && apt-get install -y \ + protobuf-compiler \ + python-catkin-tools \ + python2.7 \ + python-pip \ + python2.7-dev \ + python2.7-empy \ + python-nose \ + qt4-default \ + devscripts \ + debhelper \ + && rm -rf /var/lib/apt/lists/* + +# Copy astrobee code +COPY . /home/astrobee/astrobee + +# Define the appropriate environment variables +ARG ARMHF_CHROOT_DIR=/arm_cross/rootfs +ARG ARMHF_TOOLCHAIN=/arm_cross/toolchain/gcc +ARG FF_SOURCE=/home/astrobee/astrobee + +# Cross-compile +RUN ln -s /arm_cross/toolchain/gcc/bin/arm-linux-gnueabihf-strip "/usr/bin/arm-linux-gnueabihf-strip" \ + && cd /home/astrobee/astrobee && ./scripts/build/build_debian.sh + +# Move resulting files to a folder +RUN mkdir /home/astrobee/debians \ + && mv -t /home/astrobee/debians /home/astrobee/*.deb /home/astrobee/*.build /home/astrobee/*.changes diff --git a/scripts/docker/cross_compile/debian_compile.sh b/scripts/docker/cross_compile/debian_compile.sh new file mode 100755 index 0000000000..0b2393e0d2 --- /dev/null +++ b/scripts/docker/cross_compile/debian_compile.sh @@ -0,0 +1,55 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +# This image is split into two, because we need to copy over the toolchain +# folder and rootfs folder. Because docker can only access files in the +# build context, and we don't want it to scan the entire computer by +# chosing a bigger build context, this is the easiest solution. This also +# allows the user to place the folders freely. + +# Base docker image copies over the toolchain and rootfs, resulting from +# the NASA_INSTALL.md instructions. Also installs minimum dependencies + +# Check that the paths are defined +DIR=$(dirname "$(readlink -f "$0")") + +echo "Build context for toolchain base: "${ARMHF_TOOLCHAIN} +if [ -z "${ARMHF_TOOLCHAIN}" ]; then echo ARMHF_TOOLCHAIN is not set, please check armhf instructions; exit -1; fi +echo "Build context for armhf base: "${ARMHF_CHROOT_DIR} +if [ -z "${ARMHF_CHROOT_DIR}" ]; then echo ARMHF_CHROOT_DIR is not set, please check armhf instructions; exit -1; fi +echo "Build context for cross: "${DIR}/../../.. + +if [ -z "${DEBIAN_PATH}" ] +then + if [ -d "${DIR}/../../../../astrobee_install/armhf/" ] # Checks if this folder is defined otherwise defaults to standard + then export DEBIAN_PATH=${DIR}/../../../../ + else export DEBIAN_PATH=$HOME/ + fi +fi +echo "Saving the cross-compiled code in: "${INSTALL_PATH} + +# Setup the build context +docker build $ARMHF_TOOLCHAIN -f scripts/docker/cross_compile/astrobee_base_toolchain.Dockerfile -t astrobee/astrobee:base-toolchain +docker build $ARMHF_CHROOT_DIR -f scripts/docker/cross_compile/astrobee_base_rootfs.Dockerfile -t astrobee/astrobee:base-cross + +# Cross-compiles the code +docker build ${DIR}/../../.. -f scripts/docker/cross_compile/astrobee_debian.Dockerfile -t astrobee/astrobee:debian + +# Save the code +docker cp $(docker create --rm astrobee/astrobee:debian):/home/astrobee/debians/. $DEBIAN_PATH diff --git a/scripts/docker/readme.md b/scripts/docker/readme.md index 722a3cc946..07e7cf02ab 100644 --- a/scripts/docker/readme.md +++ b/scripts/docker/readme.md @@ -1,6 +1,6 @@ \page install-docker Docker build -# Usage instructions for NASA users +# Usage instructions for Docker Here there are instructions on how to build and run the FSW using Docker. @@ -23,14 +23,8 @@ Available docker files: To build the docker images, run: ./build.sh -The option -a is used to select the location of the astrobee source code, otherwise it is assumed we are in the scripts/docker folder. The option -n is used for ubuntu 18 docker images -To rebuild the docker image, use: - - ./rebuild-and-test.sh -The extra options are the same as the build command. The resulting image will overwrite the previous image and can be used for testing purposes. Do not upload this image to the docker hub, instead use the build.sh command for size purposes, no need to have an extra layer. - ## Run the container To run the docker container: @@ -70,8 +64,28 @@ Next, download the cross toolchain and install the chroot: $SOURCE_PATH/submodules/platform/fetch_toolchain.sh $SOURCE_PATH/submodules/platform/rootfs/make_xenial.sh dev $ARMHF_CHROOT_DIR -From the source of the repository, run: +From the root of the repository, run: ./scripts/docker/cross_compile/cross_compile.sh -The code will be cross-compiles inside the docker container in /opt/astrobee, and can be copied to the robot. \ No newline at end of file +The code will be cross-compiles inside the docker container in /opt/astrobee, and +can be copied to the robot. + +After the debian files are generated inside the docker image, they are copied to +in order of priority: +1) To the INSTALL_PATH, if defined +2) To ${DIR}/../../../../astrobee_install/armhf/, if the directoy already exists, +where $DIR is the directory where the cross_compile.sh script is located. +3) To $HOME/astrobee_build/armhf otherwise + +## Building an Astrobee Debian (NASA users only) + +This step assumes that the cross compile setup was successful and that ARMHF_CHROOT_DIR +and ARMHF_TOOLCHAIN are defined. If not, please follow the cross-compile instructions. + +To generate the debians, from the root of the repository, run: + + ./scripts/docker/cross_compile/debian_compile.sh + +After the debian files are generated inside the docker image, they are copied to the +folder before the root of the repository, ${DIR}/../../../../, where $DIR is the directory where the debian_compile.sh script is located. \ No newline at end of file diff --git a/scripts/docker/test_astrobee_kinetic.Dockerfile b/scripts/docker/test_astrobee_kinetic.Dockerfile index cb402704f9..3bb573ffdc 100644 --- a/scripts/docker/test_astrobee_kinetic.Dockerfile +++ b/scripts/docker/test_astrobee_kinetic.Dockerfile @@ -1,4 +1,4 @@ -# This will set up an Astrobee docker container using the non-NASA install instructions. +# This will test an Astrobee kinetic docker container. # You must set the docker context to be the repository root directory FROM astrobee/astrobee:latest-kinetic diff --git a/scripts/docker/test_astrobee_melodic.Dockerfile b/scripts/docker/test_astrobee_melodic.Dockerfile index 20f1622baa..4714f9e222 100644 --- a/scripts/docker/test_astrobee_melodic.Dockerfile +++ b/scripts/docker/test_astrobee_melodic.Dockerfile @@ -1,4 +1,4 @@ -# This will set up an Astrobee docker container using the non-NASA install instructions. +# This will test an Astrobee melodic docker container. # You must set the docker context to be the repository root directory FROM astrobee/astrobee:latest-melodic diff --git a/scripts/setup/debians/.gitignore b/scripts/setup/debians/.gitignore index f8d83c9d00..583ec2c799 100644 --- a/scripts/setup/debians/.gitignore +++ b/scripts/setup/debians/.gitignore @@ -9,6 +9,8 @@ libalvar libdeepdive libdbow2 +libgtsam +libgtsamlib libdbowdlib libopenmvg libdecomputil diff --git a/scripts/setup/debians/build_gtsam.sh b/scripts/setup/debians/build_gtsam.sh new file mode 100755 index 0000000000..c56782fbf4 --- /dev/null +++ b/scripts/setup/debians/build_gtsam.sh @@ -0,0 +1,32 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +PACKAGE_NAME=libgtsam +ORIG_TAR=libgtsam_4.1.0.orig.tar.gz +DEB_DIR=gtsam + +if [ -d $PACKAGE_NAME ]; then + rm -rf $PACKAGE_NAME +fi +git clone --quiet https://github.com/borglab/gtsam.git $PACKAGE_NAME --branch 4.1rc 2>&1 || exit 1 +cd $PACKAGE_NAME +git archive --prefix=$PACKAGE_NAME/ --output=../$ORIG_TAR --format tar.gz HEAD || exit 1 +cp -r ../$DEB_DIR debian +debuild -us -uc || exit 1 +cd .. diff --git a/scripts/setup/debians/build_install_debians.sh b/scripts/setup/debians/build_install_debians.sh index 67f25b144b..dac56ba6e4 100755 --- a/scripts/setup/debians/build_install_debians.sh +++ b/scripts/setup/debians/build_install_debians.sh @@ -45,6 +45,12 @@ cd ${DEBIAN_LOC} ./build_dbow2.sh || exit 1 sudo dpkg -i libdbow*_amd64.deb || exit 1 +cd ${DEBIAN_LOC}/gtsam +sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control +cd ${DEBIAN_LOC} +./build_gtsam.sh || exit 1 +sudo dpkg -i libgtsam*_amd64.deb || exit 1 + cd ${DEBIAN_LOC}/decomputil sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control cd ${DEBIAN_LOC} diff --git a/scripts/setup/debians/build_install_debians_18_04.sh b/scripts/setup/debians/build_install_debians_18_04.sh index ed4a7e4c82..ac75038b32 100755 --- a/scripts/setup/debians/build_install_debians_18_04.sh +++ b/scripts/setup/debians/build_install_debians_18_04.sh @@ -51,6 +51,12 @@ cd ${DEBIAN_LOC} ./build_dbow2.sh || exit 1 sudo dpkg -i libdbow*_amd64.deb || exit 1 +cd ${DEBIAN_LOC}/gtsam +sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control +cd ${DEBIAN_LOC} +./build_gtsam.sh || exit 1 +sudo dpkg -i libgtsam*_amd64.deb || exit 1 + cd ${DEBIAN_LOC}/decomputil sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control cd ${DEBIAN_LOC} diff --git a/scripts/setup/debians/gtsam/changelog b/scripts/setup/debians/gtsam/changelog new file mode 100644 index 0000000000..ff2770caae --- /dev/null +++ b/scripts/setup/debians/gtsam/changelog @@ -0,0 +1,5 @@ +libgtsam (4.1.0-1) unstable; urgency=medium + + * Initial release. + + -- Ryan Soussan Mon, 31 Aug 2020 11:21:32 -0800 diff --git a/scripts/setup/debians/gtsam/compat b/scripts/setup/debians/gtsam/compat new file mode 100644 index 0000000000..ec635144f6 --- /dev/null +++ b/scripts/setup/debians/gtsam/compat @@ -0,0 +1 @@ +9 diff --git a/scripts/setup/debians/gtsam/control b/scripts/setup/debians/gtsam/control new file mode 100644 index 0000000000..0ac6b96352 --- /dev/null +++ b/scripts/setup/debians/gtsam/control @@ -0,0 +1,18 @@ +Source: libgtsam +Priority: optional +Maintainer: Ryan Soussan +Build-Depends: debhelper (>=9), cmake (>=3.5), libeigen3-dev (>=3.2), libboost-all-dev (>=1.58) +Standards-Version: 3.9.2 +Section: libs +Homepage: https://github.com/borglab/gtsam + +Package: libgtsam-dev +Section: libdevel +Architecture: any +Depends: libgtsam (= ${binary:Version}), ${misc:Depends} +Description: Smoothing and mapping library for robotics and vision. The development package. + +Package: libgtsam +Architecture: any +Depends: ${shlibs:Depends}, ${misc:Depends} +Description: Smoothing and mapping library for robotics and vision. diff --git a/scripts/setup/debians/gtsam/copyright b/scripts/setup/debians/gtsam/copyright new file mode 100644 index 0000000000..13b222d3e6 --- /dev/null +++ b/scripts/setup/debians/gtsam/copyright @@ -0,0 +1,31 @@ +Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ +Upstream-Name: libgtsam +Source: https://github.com/borglab/gtsam + +Files: * +Copyright: 2010 Georgia Tech Research Corporation +License: BSD +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +Files: debian/* +Copyright: 2017 NASA +License: Apache-2 + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + . + http://www.apache.org/licenses/LICENSE-2.0 + . + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/scripts/setup/debians/gtsam/libgtsam-dev.dirs b/scripts/setup/debians/gtsam/libgtsam-dev.dirs new file mode 100644 index 0000000000..44188162ec --- /dev/null +++ b/scripts/setup/debians/gtsam/libgtsam-dev.dirs @@ -0,0 +1,2 @@ +usr/lib +usr/include diff --git a/scripts/setup/debians/gtsam/libgtsam-dev.install b/scripts/setup/debians/gtsam/libgtsam-dev.install new file mode 100644 index 0000000000..35970b90ac --- /dev/null +++ b/scripts/setup/debians/gtsam/libgtsam-dev.install @@ -0,0 +1,2 @@ +usr/include/* +usr/lib/cmake/GTSAM/* usr/share/gtsam/cmake diff --git a/scripts/setup/debians/gtsam/libgtsam.dirs b/scripts/setup/debians/gtsam/libgtsam.dirs new file mode 100644 index 0000000000..68457717bd --- /dev/null +++ b/scripts/setup/debians/gtsam/libgtsam.dirs @@ -0,0 +1 @@ +usr/lib diff --git a/scripts/setup/debians/gtsam/libgtsam.install b/scripts/setup/debians/gtsam/libgtsam.install new file mode 100644 index 0000000000..703ad8b313 --- /dev/null +++ b/scripts/setup/debians/gtsam/libgtsam.install @@ -0,0 +1 @@ +usr/lib/*/lib*.so* diff --git a/scripts/setup/debians/gtsam/rules b/scripts/setup/debians/gtsam/rules new file mode 100755 index 0000000000..bf97609122 --- /dev/null +++ b/scripts/setup/debians/gtsam/rules @@ -0,0 +1,26 @@ +#!/usr/bin/make -f +# See debhelper(7) (uncomment to enable) +# output every command that modifies files on the build system. +#export DH_VERBOSE = 1 + + +# see FEATURE AREAS in dpkg-buildflags(1) +#export DEB_BUILD_MAINT_OPTIONS = hardening=+all + +# see ENVIRONMENT in dpkg-buildflags(1) +# package maintainers to append CFLAGS +#export DEB_CFLAGS_MAINT_APPEND = -Wall -pedantic +# package maintainers to append LDFLAGS +#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed + +%: + dh $@ --buildsystem=cmake + + +# dh_make generated override targets +# This is example for Cmake (See https://bugs.debian.org/641051 ) +#override_dh_auto_configure: +# dh_auto_configure -- # -DCMAKE_LIBRARY_PATH=$(DEB_HOST_MULTIARCH) +override_dh_auto_configure: + dh_auto_configure -- -DCMAKE_INSTALL_PREFIX=/usr -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_WITH_TBB=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_WRAP=OFF -DGTSAM_BUILD_DOCS=OFF -DGTSAM_INSTALL_CPPUNITLITE=OFF -DGTSAM_BUILD_UNSTABLE=OFF -DCMAKE_BUILD_TYPE=Release + diff --git a/scripts/setup/debians/gtsam/source/format b/scripts/setup/debians/gtsam/source/format new file mode 100644 index 0000000000..163aaf8d82 --- /dev/null +++ b/scripts/setup/debians/gtsam/source/format @@ -0,0 +1 @@ +3.0 (quilt) diff --git a/scripts/setup/packages_base.lst b/scripts/setup/packages_base.lst index 184ba07789..aae3d2f495 100644 --- a/scripts/setup/packages_base.lst +++ b/scripts/setup/packages_base.lst @@ -75,6 +75,7 @@ ros-kinetic-xacro ros-kinetic-octomap libalvar2 libdbow21 +libgtsam libopenmvg1 libroyale1 libceres1 @@ -111,6 +112,7 @@ libjson-c-dev libargtable2-dev libalvar-dev libdbow2-dev +libgtsam-dev libopenmvg-dev libroyale-dev libceres-dev diff --git a/scripts/setup/packages_base_18.lst b/scripts/setup/packages_base_18.lst index 37a118853c..6a0024f216 100644 --- a/scripts/setup/packages_base_18.lst +++ b/scripts/setup/packages_base_18.lst @@ -77,6 +77,7 @@ ros-melodic-xacro ros-melodic-octomap libalvar2 libdbow21 +libgtsam libopenmvg1 libroyale1 libceres1 @@ -113,6 +114,7 @@ libjson-c-dev libargtable2-dev libalvar-dev libdbow2-dev +libgtsam-dev libopenmvg-dev libroyale-dev libceres-dev diff --git a/scripts/teleop/run_estimate_bias.sh b/scripts/teleop/run_estimate_bias.sh new file mode 100755 index 0000000000..632a3ba07d --- /dev/null +++ b/scripts/teleop/run_estimate_bias.sh @@ -0,0 +1,20 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +rosservice call /gnc/ekf/init_bias diff --git a/scripts/teleop/run_reset_localization.sh b/scripts/teleop/run_reset_localization.sh new file mode 100755 index 0000000000..9ecaf7c917 --- /dev/null +++ b/scripts/teleop/run_reset_localization.sh @@ -0,0 +1,20 @@ +#!/bin/bash +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +rosservice call /gnc/ekf/reset diff --git a/shared/ff_util/include/ff_util/ff_faults.h b/shared/ff_util/include/ff_util/ff_faults.h index f028f48c88..9ef76da471 100644 --- a/shared/ff_util/include/ff_util/ff_faults.h +++ b/shared/ff_util/include/ff_util/ff_faults.h @@ -48,12 +48,11 @@ enum FaultKeys { DISK_USAGE_HIGH, HARDWARE_ERROR_FAULT_CODE, BAD_GYRO_DATA, - CONTROL_CYCLE_OVERRUN, IMU_OVERTEMP, IMU_ACCEL_ABOVE_LIMITS }; -constexpr int kFaultKeysSize = 25; +constexpr int kFaultKeysSize = 24; static std::string fault_keys[] = { "UNKNOWN_FAULT_KEY", @@ -78,7 +77,6 @@ static std::string fault_keys[] = { "DISK_USAGE_HIGH", "HARDWARE_ERROR_FAULT_CODE", "BAD_GYRO_DATA", - "CONTROL_CYCLE_OVERRUN", "IMU_OVERTEMP", "IMU_ACCEL_ABOVE_LIMITS" }; diff --git a/shared/ff_util/include/ff_util/ff_names.h b/shared/ff_util/include/ff_util/ff_names.h index beffd9a914..074e995ac5 100644 --- a/shared/ff_util/include/ff_util/ff_names.h +++ b/shared/ff_util/include/ff_util/ff_names.h @@ -113,9 +113,19 @@ #define NODE_CTL "ctl" #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_SIM_LOC "sim_loc" #define NODE_FAM "fam" #define NODE_SIM_WRAPPER "sim_wrapper" +#define TOPIC_GRAPH_LOC "graph_loc/graph" +#define TOPIC_GRAPH_LOC_STATE "graph_loc/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 ACTION_GNC_CTL_CONTROL "gnc/control" #define TOPIC_GNC_EKF "gnc/ekf" @@ -131,6 +141,7 @@ #define SERVICE_GNC_EKF_RESET "gnc/ekf/reset" #define SERVICE_GNC_EKF_RESET_HR "gnc/ekf/reset_hr" #define SERVICE_GNC_EKF_INIT_BIAS "gnc/ekf/init_bias" +#define SERVICE_GNC_EKF_INIT_BIAS_FROM_FILE "gnc/ekf/init_bias_from_file" #define SERVICE_GNC_EKF_SET_INPUT "gnc/ekf/set_input" #define SERVICE_GNC_CTL_ENABLE "gnc/ctl/enable" @@ -360,7 +371,7 @@ #define TOPIC_HARDWARE_IMU "hw/imu" #define TOPIC_HARDWARE_NAV_CAM "hw/cam_nav" #define TOPIC_HARDWARE_DOCK_CAM "hw/cam_dock" -#define TOPIC_HARDWARE_SCI_CAM "hw/cam_sci/compressed" +#define TOPIC_HARDWARE_SCI_CAM "hw/cam_sci" #define TOPIC_HARDWARE_LIGHT_FRONT "hw/light_front" #define TOPIC_HARDWARE_LIGHT_AFT "hw/light_aft" #define TOPIC_HARDWARE_LASER "hw/laser" diff --git a/shared/ff_util/launch/ff_nodelet.launch b/shared/ff_util/launch/ff_nodelet.launch index 2727d74832..595db21429 100644 --- a/shared/ff_util/launch/ff_nodelet.launch +++ b/shared/ff_util/launch/ff_nodelet.launch @@ -31,6 +31,7 @@ + @@ -62,7 +63,7 @@ + args="manager" launch-prefix="$(arg prefix) $(arg terminal)" output="$(arg output)"/> #include #include +#include #include +#include + namespace msg_conversions { - Eigen::Vector3d ros_point_to_eigen_vector(const geometry_msgs::Point & p); - Eigen::Vector3d ros_to_eigen_vector(const geometry_msgs::Vector3 & v); - geometry_msgs::Vector3 eigen_to_ros_vector(const Eigen::Vector3d & v); - void eigen_to_array_vector(const Eigen::Vector3d & v, float* array); - void ros_to_array_vector(const geometry_msgs::Vector3 & v, float* array); - geometry_msgs::Vector3 array_to_ros_vector(float* array); - - geometry_msgs::Point eigen_to_ros_point(const Eigen::Vector3d & v); - void ros_to_array_point(const geometry_msgs::Point & p, float* array); - geometry_msgs::Point array_to_ros_point(float* array); - - Eigen::Quaterniond ros_to_eigen_quat(const geometry_msgs::Quaternion & q); - geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Quaterniond & q); - geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Vector4d & v); - void eigen_to_array_quat(const Eigen::Quaterniond & q, float* array); - void ros_to_array_quat(const geometry_msgs::Quaternion & q, float* array); - geometry_msgs::Quaternion array_to_ros_quat(float* array); - Eigen::Affine3d ros_pose_to_eigen_transform(const geometry_msgs::Pose & p); - Eigen::Affine3d ros_to_eigen_transform(const geometry_msgs::Transform & p); - - // load from config file - bool config_read_quat(config_reader::ConfigReader* config, - const char* name, Eigen::Quaterniond* quat); - bool config_read_vector(config_reader::ConfigReader* config, - const char* name, Eigen::Vector3d* vec); - bool config_read_array(config_reader::ConfigReader* config, - const char* name, int length, float* dest); - bool config_read_matrix(config_reader::ConfigReader* config, const char* name, - int rows, int cols, float* dest); - bool config_read_transform(config_reader::ConfigReader* config, const char* name, - Eigen::Vector3d* vec, Eigen::Quaterniond* quat); - bool config_read_transform(config_reader::ConfigReader* config, const char* name, - geometry_msgs::Vector3* vec, geometry_msgs::Quaternion* quat); - bool config_read_transform(config_reader::ConfigReader* config, const char* name, - Eigen::Affine3d* transform); - bool config_read_transform(config_reader::ConfigReader* config, const char* name, - geometry_msgs::Transform* transform); - - bool config_read_quat(config_reader::ConfigReader::Table* t, Eigen::Quaterniond* quat); - bool config_read_vector(config_reader::ConfigReader::Table* t, Eigen::Vector3d* vec); - bool config_read_quat(config_reader::ConfigReader::Table* t, geometry_msgs::Quaternion* quat); - bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Vector3* vec); - bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Point* point); +Eigen::Vector3d ros_point_to_eigen_vector(const geometry_msgs::Point& p); +Eigen::Vector3d ros_to_eigen_vector(const geometry_msgs::Vector3& v); +geometry_msgs::Vector3 eigen_to_ros_vector(const Eigen::Vector3d& v); +void eigen_to_array_vector(const Eigen::Vector3d& v, float* array); +void ros_to_array_vector(const geometry_msgs::Vector3& v, float* array); +geometry_msgs::Vector3 array_to_ros_vector(float* array); + +geometry_msgs::Point eigen_to_ros_point(const Eigen::Vector3d& v); +void ros_to_array_point(const geometry_msgs::Point& p, float* array); +geometry_msgs::Point array_to_ros_point(float* array); + +Eigen::Quaterniond ros_to_eigen_quat(const geometry_msgs::Quaternion& q); +geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Quaterniond& q); +geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Vector4d& v); +void eigen_to_array_quat(const Eigen::Quaterniond& q, float* array); +void ros_to_array_quat(const geometry_msgs::Quaternion& q, float* array); +geometry_msgs::Quaternion array_to_ros_quat(float* array); +Eigen::Affine3d ros_pose_to_eigen_transform(const geometry_msgs::Pose& p); +Eigen::Affine3d ros_to_eigen_transform(const geometry_msgs::Transform& p); + +// load from config file +bool config_read_quat(config_reader::ConfigReader* config, const char* name, Eigen::Quaterniond* quat); +bool config_read_vector(config_reader::ConfigReader* config, const char* name, Eigen::Vector3d* vec); +bool config_read_array(config_reader::ConfigReader* config, const char* name, int length, float* dest); +bool config_read_matrix(config_reader::ConfigReader* config, const char* name, int rows, int cols, float* dest); +bool config_read_transform(config_reader::ConfigReader* config, const char* name, Eigen::Vector3d* vec, + Eigen::Quaterniond* quat); +bool config_read_transform(config_reader::ConfigReader* config, const char* name, geometry_msgs::Vector3* vec, + geometry_msgs::Quaternion* quat); +bool config_read_transform(config_reader::ConfigReader* config, const char* name, Eigen::Affine3d* transform); +bool config_read_transform(config_reader::ConfigReader* config, const char* name, geometry_msgs::Transform* transform); + +bool config_read_quat(config_reader::ConfigReader::Table* t, Eigen::Quaterniond* quat); +bool config_read_vector(config_reader::ConfigReader::Table* t, Eigen::Vector3d* vec); +bool config_read_quat(config_reader::ConfigReader::Table* t, geometry_msgs::Quaternion* quat); +bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Vector3* vec); +bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Point* point); + +// 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); +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); +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); +Eigen::Vector3d CovDiagToVariances(const float* const cov_diag); + +template +VectorType VectorFromMsg(const MsgVectorType& msg_vector) { + return VectorType(msg_vector.x, msg_vector.y, msg_vector.z); +} + +template +void VectorToMsg(const VectorType& vector, MsgVectorType& msg_vector) { + msg_vector.x = vector.x(); + msg_vector.y = vector.y(); + msg_vector.z = vector.z(); +} + +template +RotationType RotationFromMsg(const MsgRotationType& msg_rotation) { + return RotationType(msg_rotation.w, msg_rotation.x, msg_rotation.y, msg_rotation.z); +} + +template +void RotationToMsg(const RotationType& rotation, MsgRotationType& msg_rotation) { + msg_rotation.w = rotation.w(); + msg_rotation.x = rotation.x(); + msg_rotation.y = rotation.y(); + msg_rotation.z = rotation.z(); +} } // namespace msg_conversions #endif // MSG_CONVERSIONS_MSG_CONVERSIONS_H_ diff --git a/shared/msg_conversions/src/msg_conversions.cc b/shared/msg_conversions/src/msg_conversions.cc index cd6604a84a..34aa0d5fd6 100644 --- a/shared/msg_conversions/src/msg_conversions.cc +++ b/shared/msg_conversions/src/msg_conversions.cc @@ -1,14 +1,14 @@ /* Copyright (c) 2017, United States Government, as represented by the * Administrator of the National Aeronautics and Space Administration. - * + * * All rights reserved. - * + * * The Astrobee platform is licensed under the Apache License, Version 2.0 * (the "License"); you may not use this file except in compliance with the * License. You may obtain a copy of the License at - * + * * http://www.apache.org/licenses/LICENSE-2.0 - * + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the @@ -18,18 +18,15 @@ #include -namespace msg_conversions { +#include -Eigen::Vector3d ros_point_to_eigen_vector(const geometry_msgs::Point & p) { - return Eigen::Vector3d(p.x, p.y, p.z); -} +namespace msg_conversions { +Eigen::Vector3d ros_point_to_eigen_vector(const geometry_msgs::Point& p) { return Eigen::Vector3d(p.x, p.y, p.z); } -Eigen::Vector3d ros_to_eigen_vector(const geometry_msgs::Vector3 & v) { - return Eigen::Vector3d(v.x, v.y, v.z); -} +Eigen::Vector3d ros_to_eigen_vector(const geometry_msgs::Vector3& v) { return Eigen::Vector3d(v.x, v.y, v.z); } -geometry_msgs::Vector3 eigen_to_ros_vector(const Eigen::Vector3d & v) { +geometry_msgs::Vector3 eigen_to_ros_vector(const Eigen::Vector3d& v) { geometry_msgs::Vector3 n; n.x = v[0]; n.y = v[1]; @@ -37,7 +34,7 @@ geometry_msgs::Vector3 eigen_to_ros_vector(const Eigen::Vector3d & v) { return n; } -geometry_msgs::Point eigen_to_ros_point(const Eigen::Vector3d & v) { +geometry_msgs::Point eigen_to_ros_point(const Eigen::Vector3d& v) { geometry_msgs::Point n; n.x = v[0]; n.y = v[1]; @@ -45,11 +42,11 @@ geometry_msgs::Point eigen_to_ros_point(const Eigen::Vector3d & v) { return n; } -Eigen::Quaterniond ros_to_eigen_quat(const geometry_msgs::Quaternion & q) { +Eigen::Quaterniond ros_to_eigen_quat(const geometry_msgs::Quaternion& q) { return Eigen::Quaterniond(q.w, q.x, q.y, q.z); } -geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Quaterniond & q) { +geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Quaterniond& q) { geometry_msgs::Quaternion out; out.x = q.x(); out.y = q.y(); @@ -58,7 +55,7 @@ geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Quaterniond & q) { return out; } -geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Vector4d & v) { +geometry_msgs::Quaternion eigen_to_ros_quat(const Eigen::Vector4d& v) { geometry_msgs::Quaternion out; out.x = v.x(); out.y = v.y(); @@ -92,63 +89,59 @@ geometry_msgs::Quaternion array_to_ros_quat(float* array) { return q; } -void ros_to_array_vector(const geometry_msgs::Vector3 & v, float* array) { +void ros_to_array_vector(const geometry_msgs::Vector3& v, float* array) { array[0] = v.x; array[1] = v.y; array[2] = v.z; } -void ros_to_array_point(const geometry_msgs::Point & p, float* array) { +void ros_to_array_point(const geometry_msgs::Point& p, float* array) { array[0] = p.x; array[1] = p.y; array[2] = p.z; } -void ros_to_array_quat(const geometry_msgs::Quaternion & q, float* array) { +void ros_to_array_quat(const geometry_msgs::Quaternion& q, float* array) { array[0] = q.x; array[1] = q.y; array[2] = q.z; array[3] = q.w; } -void eigen_to_array_vector(const Eigen::Vector3d & v, float* array) { +void eigen_to_array_vector(const Eigen::Vector3d& v, float* array) { array[0] = v.x(); array[1] = v.y(); array[2] = v.z(); } -void eigen_to_array_quat(const Eigen::Quaterniond & q, float* array) { +void eigen_to_array_quat(const Eigen::Quaterniond& q, float* array) { array[0] = q.x(); array[1] = q.y(); array[2] = q.z(); array[3] = q.w(); } -bool config_read_quat(config_reader::ConfigReader* config, - const char* name, Eigen::Quaterniond* quat) { +bool config_read_quat(config_reader::ConfigReader* config, const char* name, Eigen::Quaterniond* quat) { config_reader::ConfigReader::Table t(config, name); return config_read_quat(&t, quat); } -bool config_read_vector(config_reader::ConfigReader* config, - const char* name, Eigen::Vector3d* vec) { +bool config_read_vector(config_reader::ConfigReader* config, const char* name, Eigen::Vector3d* vec) { config_reader::ConfigReader::Table t(config, name); return config_read_vector(&t, vec); } -bool config_read_array(config_reader::ConfigReader* config, - const char* name, int length, float* dest) { +bool config_read_array(config_reader::ConfigReader* config, const char* name, int length, float* dest) { config_reader::ConfigReader::Table array(config, name); for (int i = 0; i < length; i++) { - if (!array.GetReal(i+1, &dest[i])) { + if (!array.GetReal(i + 1, &dest[i])) { return false; } } return true; } -bool config_read_matrix(config_reader::ConfigReader* config, const char* name, - int rows, int cols, float* dest) { +bool config_read_matrix(config_reader::ConfigReader* config, const char* name, int rows, int cols, float* dest) { config_reader::ConfigReader::Table matrix(config, name); int index = 0; for (int i = 1; i <= rows; i++) { @@ -163,8 +156,8 @@ bool config_read_matrix(config_reader::ConfigReader* config, const char* name, return true; } -bool config_read_transform(config_reader::ConfigReader* config, const char* name, - Eigen::Vector3d* vec, Eigen::Quaterniond* quat) { +bool config_read_transform(config_reader::ConfigReader* config, const char* name, Eigen::Vector3d* vec, + Eigen::Quaterniond* quat) { config_reader::ConfigReader::Table t(config, name); config_reader::ConfigReader::Table v(&t, "trans"); config_reader::ConfigReader::Table r(&t, "rot"); @@ -173,8 +166,8 @@ bool config_read_transform(config_reader::ConfigReader* config, const char* name return success; } -bool config_read_transform(config_reader::ConfigReader* config, const char* name, - geometry_msgs::Vector3* vec, geometry_msgs::Quaternion* quat) { +bool config_read_transform(config_reader::ConfigReader* config, const char* name, geometry_msgs::Vector3* vec, + geometry_msgs::Quaternion* quat) { config_reader::ConfigReader::Table t(config, name); config_reader::ConfigReader::Table v(&t, "trans"); config_reader::ConfigReader::Table r(&t, "rot"); @@ -183,30 +176,25 @@ bool config_read_transform(config_reader::ConfigReader* config, const char* name return success; } -bool config_read_transform(config_reader::ConfigReader* config, const char* name, - Eigen::Affine3d* transform) { +bool config_read_transform(config_reader::ConfigReader* config, const char* name, Eigen::Affine3d* transform) { Eigen::Vector3d trans; Eigen::Quaterniond rot; - if (!config_read_transform(config, name, &trans, &rot)) - return false; - *transform = Eigen::Affine3d(Eigen::Translation3d(trans.x(), trans.y(), trans.z())) * - Eigen::Affine3d(rot); + if (!config_read_transform(config, name, &trans, &rot)) return false; + *transform = Eigen::Affine3d(Eigen::Translation3d(trans.x(), trans.y(), trans.z())) * Eigen::Affine3d(rot); return true; } -bool config_read_transform(config_reader::ConfigReader* config, const char* name, - geometry_msgs::Transform* transform) { +bool config_read_transform(config_reader::ConfigReader* config, const char* name, geometry_msgs::Transform* transform) { geometry_msgs::Vector3 trans; geometry_msgs::Quaternion rot; - if (!config_read_transform(config, name, &trans, &rot)) - return false; + if (!config_read_transform(config, name, &trans, &rot)) return false; transform->translation = trans; transform->rotation = rot; return true; } bool config_read_quat(config_reader::ConfigReader::Table* t, Eigen::Quaterniond* quat) { - bool success = t->GetReal("x", &quat->x()); + bool success = t->GetReal("x", &quat->x()); success = success && t->GetReal("y", &quat->y()); success = success && t->GetReal("z", &quat->z()); success = success && t->GetReal("w", &quat->w()); @@ -214,14 +202,14 @@ bool config_read_quat(config_reader::ConfigReader::Table* t, Eigen::Quaterniond* } bool config_read_vector(config_reader::ConfigReader::Table* t, Eigen::Vector3d* vec) { - bool success = t->GetReal(1, &vec->x()); + bool success = t->GetReal(1, &vec->x()); success = success && t->GetReal(2, &vec->y()); success = success && t->GetReal(3, &vec->z()); return success; } bool config_read_quat(config_reader::ConfigReader::Table* t, geometry_msgs::Quaternion* quat) { - bool success = t->GetReal("x", &quat->x); + bool success = t->GetReal("x", &quat->x); success = success && t->GetReal("y", &quat->y); success = success && t->GetReal("z", &quat->z); success = success && t->GetReal("w", &quat->w); @@ -229,33 +217,86 @@ bool config_read_quat(config_reader::ConfigReader::Table* t, geometry_msgs::Quat } bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Vector3* vec) { - bool success = t->GetReal(1, &vec->x); + bool success = t->GetReal(1, &vec->x); success = success && t->GetReal(2, &vec->y); success = success && t->GetReal(3, &vec->z); return success; } bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Point* point) { - bool success = t->GetReal(1, &point->x); + bool success = t->GetReal(1, &point->x); success = success && t->GetReal(2, &point->y); success = success && t->GetReal(3, &point->z); return success; } -Eigen::Affine3d ros_pose_to_eigen_transform(const geometry_msgs::Pose & p) { +Eigen::Affine3d ros_pose_to_eigen_transform(const geometry_msgs::Pose& p) { Eigen::Affine3d transform; transform.translation() = Eigen::Vector3d(p.position.x, p.position.y, p.position.z); - transform.linear() = Eigen::Quaterniond( - p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z).toRotationMatrix(); + transform.linear() = + Eigen::Quaterniond(p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z).toRotationMatrix(); return transform; } -Eigen::Affine3d ros_to_eigen_transform(const geometry_msgs::Transform & p) { +Eigen::Affine3d ros_to_eigen_transform(const geometry_msgs::Transform& p) { Eigen::Affine3d transform; transform.translation() = Eigen::Vector3d(p.translation.x, p.translation.y, p.translation.z); - transform.linear() = Eigen::Quaterniond( - p.rotation.w, p.rotation.x, p.rotation.y, p.rotation.z).toRotationMatrix(); + transform.linear() = Eigen::Quaterniond(p.rotation.w, p.rotation.x, p.rotation.y, p.rotation.z).toRotationMatrix(); return transform; } +Eigen::Isometry3d LoadEigenTransform(config_reader::ConfigReader& config, const std::string& transform_config_name) { + 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); + 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; +} + +double LoadDouble(config_reader::ConfigReader& config, const std::string& config_name) { + double val; + if (!config.GetReal(config_name.c_str(), &val)) ROS_FATAL_STREAM("Failed to load " << config_name); + return val; +} + +int LoadInt(config_reader::ConfigReader& config, const std::string& config_name) { + int val; + if (!config.GetInt(config_name.c_str(), &val)) ROS_FATAL_STREAM("Failed to load " << config_name); + return val; +} + +bool LoadBool(config_reader::ConfigReader& config, const std::string& config_name) { + bool val; + if (!config.GetBool(config_name.c_str(), &val)) ROS_FATAL_STREAM("Failed to load " << config_name); + return val; +} + +std::string LoadString(config_reader::ConfigReader& config, const std::string& config_name) { + std::string val; + if (!config.GetStr(config_name.c_str(), &val)) ROS_FATAL_STREAM("Failed to load " << config_name); + return val; +} + +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); +} + +void EigenPoseToMsg(const Eigen::Isometry3d& pose, geometry_msgs::Transform& msg_transform) { + RotationToMsg(Eigen::Quaterniond(pose.linear()), msg_transform.rotation); + VectorToMsg(pose.translation(), msg_transform.translation); +} + +void VariancesToCovDiag(const Eigen::Vector3d& variances, float* const cov_diag) { + cov_diag[0] = variances.x(); + cov_diag[1] = variances.y(); + cov_diag[2] = variances.z(); +} + +Eigen::Vector3d CovDiagToVariances(const float* const cov_diag) { + return Eigen::Vector3d(cov_diag[0], cov_diag[1], cov_diag[2]); +} } // end namespace msg_conversions diff --git a/simulation/CMakeLists.txt b/simulation/CMakeLists.txt index 40deaf433f..aaf26b2aea 100644 --- a/simulation/CMakeLists.txt +++ b/simulation/CMakeLists.txt @@ -507,6 +507,7 @@ create_library( ${catkin_LIBRARIES} astrobee_gazebo camera + msg_conversions INC ${GAZEBO_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} diff --git a/simulation/src/gazebo_sensor_plugin_ar_tags/gazebo_sensor_plugin_ar_tags.cc b/simulation/src/gazebo_sensor_plugin_ar_tags/gazebo_sensor_plugin_ar_tags.cc index 93fed365ba..738503c9eb 100644 --- a/simulation/src/gazebo_sensor_plugin_ar_tags/gazebo_sensor_plugin_ar_tags.cc +++ b/simulation/src/gazebo_sensor_plugin_ar_tags/gazebo_sensor_plugin_ar_tags.cc @@ -24,6 +24,7 @@ // FSW includes #include +#include // FSW messages #include @@ -41,16 +42,13 @@ namespace gazebo { class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { public: - GazeboSensorPluginARTags() : - FreeFlyerSensorPlugin("marker_tracking", "dock_cam", true), - active_(true) {} + GazeboSensorPluginARTags() : FreeFlyerSensorPlugin("marker_tracking", "dock_cam", true), active_(true) {} ~GazeboSensorPluginARTags() {} protected: // Called when plugin is loaded into gazebo - void LoadCallback(ros::NodeHandle *nh, - sensors::SensorPtr sensor, sdf::ElementPtr sdf) { + void LoadCallback(ros::NodeHandle* nh, sensors::SensorPtr sensor, sdf::ElementPtr sdf) { // Get a link to the parent sensor sensor_ = std::dynamic_pointer_cast(sensor); if (!sensor_) { @@ -59,42 +57,43 @@ class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { } // Check that we have a mono camera - if (sensor_->Camera()->ImageFormat() != "L8") - ROS_FATAL_STREAM("Camera format must be L8"); + if (sensor_->Camera()->ImageFormat() != "L8") ROS_FATAL_STREAM("Camera format must be L8"); // Build the nav cam Camera Model config_.AddFile("cameras.config"); + config_.AddFile("geometry.config"); config_.AddFile("dock_markers_specs.config"); config_.AddFile("dock_markers_world.config"); config_.AddFile("simulation/ar_tags.config"); if (!config_.ReadFiles()) { - ROS_ERROR("Failed to read config files."); - return; + ROS_ERROR("Failed to read config files."); + return; } - if (!config_.GetReal("drawing_width", &width_)) - NODELET_ERROR("Could not read the drawing_width parameter."); + if (!config_.GetReal("drawing_width", &width_)) NODELET_ERROR("Could not read the drawing_width parameter."); - if (!config_.GetReal("drawing_height", &height_)) - NODELET_ERROR("Could not read the drawing_height parameter."); + if (!config_.GetReal("drawing_height", &height_)) NODELET_ERROR("Could not read the drawing_height parameter."); - if (!config_.GetReal("rate", &rate_)) - NODELET_ERROR("Could not read the rate parameter."); + if (!config_.GetReal("rate", &rate_)) NODELET_ERROR("Could not read the rate parameter."); - if (!config_.GetReal("delay_camera", &delay_camera_)) - NODELET_ERROR("Could not read the delay_camera parameter."); + if (!config_.GetReal("delay_camera", &delay_camera_)) NODELET_ERROR("Could not read the delay_camera parameter."); if (!config_.GetReal("delay_features", &delay_features_)) NODELET_ERROR("Could not read the delay_features parameter."); - if (!config_.GetUInt("num_samp", &num_samp_)) - NODELET_ERROR("Could not read the num_samp parameter."); + if (!config_.GetUInt("num_samp", &num_samp_)) NODELET_ERROR("Could not read the num_samp parameter."); + + if (!config_.GetUInt("num_features", &num_features_)) NODELET_ERROR("Could not read the num_features parameter."); - if (!config_.GetUInt("num_features", &num_features_)) - NODELET_ERROR("Could not read the num_features parameter."); + if (!config_.GetTable("markers_world", &markers_)) NODELET_ERROR("Could not read the markers_world parameter."); - if (!config_.GetTable("markers_world", &markers_)) - NODELET_ERROR("Could not read the markers_world parameter."); + Eigen::Vector3d world_t_dock; + Eigen::Quaterniond world_Q_dock; + if (!msg_conversions::config_read_transform(&config_, "world_dock_transform", &world_t_dock, &world_Q_dock)) + NODELET_ERROR("Could not read world_T_dock transform."); + world_T_dock_ = Eigen::Isometry3d::Identity(); + world_T_dock_.translation() = world_t_dock; + world_T_dock_.linear() = world_Q_dock.toRotationMatrix(); for (int marker_i = 0; marker_i < markers_.GetSize(); marker_i++) { config_reader::ConfigReader::Table current_marker; @@ -112,12 +111,10 @@ class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { } // Create a publisher for the registration messages - pub_reg_ = nh->advertise( - TOPIC_LOCALIZATION_AR_REGISTRATION, 1); + pub_reg_ = nh->advertise(TOPIC_LOCALIZATION_AR_REGISTRATION, 1); // Create a publisher for the feature messages - pub_feat_ = nh->advertise( - TOPIC_LOCALIZATION_AR_FEATURES, 1); + pub_feat_ = nh->advertise(TOPIC_LOCALIZATION_AR_FEATURES, 1); // Only do this once msg_feat_.header.frame_id = std::string(FRAME_NAME_DOCK); @@ -125,55 +122,26 @@ class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { } // Only send measurements when extrinsics are available - void OnExtrinsicsReceived(ros::NodeHandle *nh) { + void OnExtrinsicsReceived(ros::NodeHandle* nh) { // Enable mapped landmarks - srv_enable_ = nh->advertiseService(SERVICE_LOCALIZATION_AR_ENABLE, - &GazeboSensorPluginARTags::EnableService, this); + srv_enable_ = nh->advertiseService(SERVICE_LOCALIZATION_AR_ENABLE, &GazeboSensorPluginARTags::EnableService, this); // Timer triggers registration - timer_registration_ = nh->createTimer(ros::Duration(ros::Rate(rate_)), - &GazeboSensorPluginARTags::SendRegistration, this, false, true); + timer_registration_ = + nh->createTimer(ros::Duration(ros::Rate(rate_)), &GazeboSensorPluginARTags::SendRegistration, this, false, true); // Timer triggers features - timer_features_ = nh->createTimer(ros::Duration(0.8 / rate_), - &GazeboSensorPluginARTags::SendFeatures, this, true, false); + timer_features_ = + nh->createTimer(ros::Duration(0.8 / rate_), &GazeboSensorPluginARTags::SendFeatures, this, true, false); } // Enable or disable the feature timer - bool EnableService(ff_msgs::SetBool::Request & req, - ff_msgs::SetBool::Response & res) { + bool EnableService(ff_msgs::SetBool::Request& req, ff_msgs::SetBool::Response& res) { active_ = req.enable; res.success = true; return true; } - // Manage the extrinsics based on the sensor type - bool GetDockLocation(Eigen::Affine3d & wTd) { - // Create a buffer and listener for TF2 transforms - static tf2_ros::Buffer buffer; - static tf2_ros::TransformListener listener(buffer); - // Get extrinsics from framestore - try { - // Lookup the transform for this sensor - geometry_msgs::TransformStamped tf = buffer.lookupTransform( - "world", "dock/body", ros::Time(0)); - // Handle the transform for all sensor types - wTd = ( - Eigen::Translation3d( - tf.transform.translation.x, - tf.transform.translation.y, - tf.transform.translation.z) * - Eigen::Quaterniond( - tf.transform.rotation.w, - tf.transform.rotation.x, - tf.transform.rotation.y, - tf.transform.rotation.z)); - } catch (tf2::TransformException &ex) { - return false; - } - return true; - } - // Called when featured must be sent void SendRegistration(ros::TimerEvent const& event) { if (!active_) return; @@ -191,58 +159,36 @@ class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { // Copy over the camera id to the feature message msg_feat_.camera_id = msg_reg_.camera_id; - // In order to get meaningful data we need a dock location - Eigen::Affine3d wTd; - if (!GetDockLocation(wTd)) { - timer_features_.stop(); - return; - } - // Handle the transform for all sensor types - Eigen::Affine3d wTb = ( - #if GAZEBO_MAJOR_VERSION > 7 - Eigen::Translation3d( - GetModel()->WorldPose().Pos().X(), - GetModel()->WorldPose().Pos().Y(), - GetModel()->WorldPose().Pos().Z()) * - Eigen::Quaterniond( - GetModel()->WorldPose().Rot().W(), - GetModel()->WorldPose().Rot().X(), - GetModel()->WorldPose().Rot().Y(), - GetModel()->WorldPose().Rot().Z())); - #else - Eigen::Translation3d( - GetModel()->GetWorldPose().pos.x, - GetModel()->GetWorldPose().pos.y, - GetModel()->GetWorldPose().pos.z) * - Eigen::Quaterniond( - GetModel()->GetWorldPose().rot.w, - GetModel()->GetWorldPose().rot.x, - GetModel()->GetWorldPose().rot.y, - GetModel()->GetWorldPose().rot.z)); - #endif - Eigen::Affine3d bTs = ( - Eigen::Translation3d( - sensor_->Pose().Pos().X(), - sensor_->Pose().Pos().Y(), - sensor_->Pose().Pos().Z()) * - Eigen::Quaterniond( - sensor_->Pose().Rot().W(), - sensor_->Pose().Rot().X(), - sensor_->Pose().Rot().Y(), - sensor_->Pose().Rot().Z())); - Eigen::Affine3d dTs = wTd.inverse() * wTb * bTs; + Eigen::Isometry3d world_T_body = ( +#if GAZEBO_MAJOR_VERSION > 7 + Eigen::Translation3d(GetModel()->WorldPose().Pos().X(), GetModel()->WorldPose().Pos().Y(), + GetModel()->WorldPose().Pos().Z()) * + Eigen::Quaterniond(GetModel()->WorldPose().Rot().W(), GetModel()->WorldPose().Rot().X(), + GetModel()->WorldPose().Rot().Y(), GetModel()->WorldPose().Rot().Z())); +#else + Eigen::Translation3d(GetModel()->GetWorldPose().pos.x, GetModel()->GetWorldPose().pos.y, + GetModel()->GetWorldPose().pos.z) * + Eigen::Quaterniond(GetModel()->GetWorldPose().rot.w, GetModel()->GetWorldPose().rot.x, + GetModel()->GetWorldPose().rot.y, GetModel()->GetWorldPose().rot.z)); +#endif + msg_feat_.header.stamp = ros::Time::now(); + + Eigen::Isometry3d body_T_dock_cam = + (Eigen::Translation3d(sensor_->Pose().Pos().X(), sensor_->Pose().Pos().Y(), sensor_->Pose().Pos().Z()) * + Eigen::Quaterniond(sensor_->Pose().Rot().W(), sensor_->Pose().Rot().X(), sensor_->Pose().Rot().Y(), + sensor_->Pose().Rot().Z())); + Eigen::Isometry3d dock_T_dock_cam = world_T_dock_.inverse() * world_T_body * body_T_dock_cam; // Initialize the camera paremeters static camera::CameraParameters cam_params(&config_, "dock_cam"); - static camera::CameraModel camera(Eigen::Vector3d(0, 0, 0), - Eigen::Matrix3d::Identity(), cam_params); + static camera::CameraModel camera(Eigen::Vector3d(0, 0, 0), Eigen::Matrix3d::Identity(), cam_params); // Assemble the feature message - msg_feat_.pose.position.x = dTs.translation().x(); - msg_feat_.pose.position.y = dTs.translation().y(); - msg_feat_.pose.position.z = dTs.translation().z(); - Eigen::Quaterniond q(dTs.rotation()); + msg_feat_.pose.position.x = dock_T_dock_cam.translation().x(); + msg_feat_.pose.position.y = dock_T_dock_cam.translation().y(); + msg_feat_.pose.position.z = dock_T_dock_cam.translation().z(); + Eigen::Quaterniond q(dock_T_dock_cam.rotation()); msg_feat_.pose.orientation.w = q.w(); msg_feat_.pose.orientation.x = q.x(); msg_feat_.pose.orientation.y = q.y(); @@ -256,11 +202,10 @@ class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { // using PnP on the points themselves. However, it's easier to just pull // this information from the simulation ground truth. Eigen::Vector3d pt_d = marker_positions_[i]; - Eigen::Vector3d pt_c = dTs.inverse() * pt_d; + Eigen::Vector3d pt_c = dock_T_dock_cam.inverse() * pt_d; // Check if the feature is in the field of view - if (!camera.IsInFov(pt_c)) - continue; + if (!camera.IsInFov(pt_c)) continue; // Get the image plane coordinates Eigen::Vector2d uv = camera.ImageCoordinates(pt_c); @@ -279,7 +224,6 @@ class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { // Send features void SendFeatures(ros::TimerEvent const& event) { if (!active_) return; - msg_feat_.header.stamp = ros::Time::now(); pub_feat_.publish(msg_feat_); } @@ -301,8 +245,9 @@ class GazeboSensorPluginARTags : public FreeFlyerSensorPlugin { unsigned int num_samp_; config_reader::ConfigReader::Table markers_; std::vector marker_positions_; + Eigen::Isometry3d world_T_dock_; }; GZ_REGISTER_SENSOR_PLUGIN(GazeboSensorPluginARTags) -} // namespace gazebo +} // namespace gazebo diff --git a/simulation/src/gazebo_sensor_plugin_optical_flow/gazebo_sensor_plugin_optical_flow.cc b/simulation/src/gazebo_sensor_plugin_optical_flow/gazebo_sensor_plugin_optical_flow.cc index bc3401e78e..def7683515 100644 --- a/simulation/src/gazebo_sensor_plugin_optical_flow/gazebo_sensor_plugin_optical_flow.cc +++ b/simulation/src/gazebo_sensor_plugin_optical_flow/gazebo_sensor_plugin_optical_flow.cc @@ -192,6 +192,8 @@ class GazeboSensorPluginOpticalFlow : public FreeFlyerSensorPlugin { sensor_->Pose().Rot().Z())); Eigen::Affine3d wTs = wTb * bTs; + msg_feat_.header.stamp = ros::Time::now(); + // Initialize the camera paremeters static camera::CameraParameters cam_params(&config_, "nav_cam"); static camera::CameraModel camera(Eigen::Vector3d(0, 0, 0), @@ -206,7 +208,6 @@ class GazeboSensorPluginOpticalFlow : public FreeFlyerSensorPlugin { // Send a registration pulse void SendFeatures(ros::TimerEvent const& event) { if (!active_) return; - msg_feat_.header.stamp = ros::Time::now(); pub_feat_.publish(msg_feat_); } diff --git a/simulation/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc b/simulation/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc index c1e9d4e23d..eefb14d667 100644 --- a/simulation/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc +++ b/simulation/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc @@ -102,7 +102,9 @@ class GazeboSensorPluginSciCam : public FreeFlyerSensorPlugin { pub_sci_cam_ack_ = nh->advertise(TOPIC_GUEST_SCIENCE_MANAGER_ACK, 10); // Create publishers for sci cam image, pose, and camera info - pub_sci_cam_image_ = nh->advertise(TOPIC_HARDWARE_SCI_CAM, 2, + std::string compressed_topic = TOPIC_HARDWARE_SCI_CAM; + compressed_topic += "/compressed"; + pub_sci_cam_image_ = nh->advertise(compressed_topic, 2, boost::bind(&GazeboSensorPluginSciCam::ToggleCallback, this), boost::bind(&GazeboSensorPluginSciCam::ToggleCallback, this)); pub_sci_cam_pose_ = nh->advertise(TOPIC_SCI_CAM_SIM_POSE, 10); diff --git a/simulation/src/gazebo_sensor_plugin_sparse_map/gazebo_sensor_plugin_sparse_map.cc b/simulation/src/gazebo_sensor_plugin_sparse_map/gazebo_sensor_plugin_sparse_map.cc index f51b7dd849..69086eb316 100644 --- a/simulation/src/gazebo_sensor_plugin_sparse_map/gazebo_sensor_plugin_sparse_map.cc +++ b/simulation/src/gazebo_sensor_plugin_sparse_map/gazebo_sensor_plugin_sparse_map.cc @@ -163,6 +163,8 @@ class GazeboSensorPluginSparseMap : public FreeFlyerSensorPlugin { Eigen::Affine3d sensor_to_world = SensorToWorld(GetModel()->GetWorldPose(), sensor_->Pose()); #endif + msg_feat_.header.stamp = ros::Time::now(); + // Initialize the camera parameters static camera::CameraParameters cam_params(&config_, "nav_cam"); static camera::CameraModel camera(Eigen::Vector3d(0, 0, 0), @@ -241,7 +243,6 @@ class GazeboSensorPluginSparseMap : public FreeFlyerSensorPlugin { // Send off the features void SendFeatures(ros::TimerEvent const& event) { if (!active_) return; - msg_feat_.header.stamp = ros::Time::now(); pub_feat_.publish(msg_feat_); } diff --git a/submodules/platform b/submodules/platform index f409a1934e..bea86eb611 160000 --- a/submodules/platform +++ b/submodules/platform @@ -1 +1 @@ -Subproject commit f409a1934e51cfff1914ff4274284179b9fab0f5 +Subproject commit bea86eb611d3b872c62cffee07dcc4922a7135df diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index c570e9f306..f64c1178c2 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -16,6 +16,9 @@ # under the License. add_subdirectory(ekf_bag) +add_subdirectory(graph_bag) +add_subdirectory(imu_bias_tester) +add_subdirectory(localization_rviz_plugins) add_subdirectory(rviz_visualizer) add_subdirectory(visualeyez) add_subdirectory(simulator) diff --git a/tools/ekf_bag/scripts/bag_sweep.py b/tools/ekf_bag/scripts/bag_sweep.py index f4f489236b..fcbdf78a53 100755 --- a/tools/ekf_bag/scripts/bag_sweep.py +++ b/tools/ekf_bag/scripts/bag_sweep.py @@ -21,6 +21,7 @@ import itertools import multiprocessing import os +import pandas as pd import ekf_graph import utilities @@ -57,6 +58,30 @@ def bag_sweep(bag_files, output_dir, args): pool.map(test_on_bag_helper, itertools.izip(bag_files, itertools.repeat(output_dir), itertools.repeat(args))) +def combined_results(csv_files): + dataframes = [pd.read_csv(file) for file in csv_files] + if not dataframes: + print('Failed to create dataframes') + exit() + names = dataframes[0].columns + combined_dataframes = pd.DataFrame(None, None, names) + for dataframe in dataframes: + trimmed_dataframe = pd.DataFrame(dataframe.values[0:1], columns=names) + combined_dataframes = combined_dataframes.append(trimmed_dataframe, ignore_index=True) + return combined_dataframes + + +def combine_results_in_csv_file(bag_files, 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 = [] + for bag_file in bag_files: + bag_name = os.path.splitext(os.path.basename(bag_file))[0] + output_csv_files.append(os.path.join(output_dir, bag_name + '_results.csv')) + combined_dataframe = combined_results(output_csv_files) + combined_dataframe.to_csv(combined_results_csv_file, index=False) + + if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('map_file', help='Full path to map file.') @@ -91,3 +116,4 @@ def bag_sweep(bag_files, output_dir, args): print('Output directory for results is {}'.format(output_dir)) bag_sweep(bag_files, output_dir, args) + combine_results_in_csv_file(bag_files, output_dir) diff --git a/tools/ekf_bag/scripts/ekf_graph.py b/tools/ekf_bag/scripts/ekf_graph.py index d960521460..f364fe7717 100755 --- a/tools/ekf_bag/scripts/ekf_graph.py +++ b/tools/ekf_bag/scripts/ekf_graph.py @@ -56,8 +56,8 @@ def quat_to_euler(q): return (x, y, z) # RMSE of matrix interpreted as n (dim(rows) vectors of dim(col) -def rmse_matrix(x): - return np.mean(np.sqrt(np.sum(np.square(x), axis=1))) +def rmse_matrix(error_matrix): + return np.sqrt(np.mean(np.sum(np.square(error_matrix), axis=1))) # Removes the ith entry of a_xs, a_ys, and a_zs if a_times[i] is # not in b_times[i]. Assumes timestamp vectors are sorted @@ -65,8 +65,10 @@ def prune_missing_timestamps(a_xs, a_ys, a_zs, a_times, b_times): a_index = 0 pruned_a_matrix = np.empty(shape=(len(b_times), 3)) for b_index, b_time in enumerate(b_times): - while not np.isclose(a_times[a_index], b_time) and a_times[a_index] < b_time and a_index < len(a_times): + while not np.isclose(a_times[a_index], b_time, rtol=0) and a_times[a_index] < b_time and a_index < len(a_times): a_index += 1 + if not np.isclose(a_times[a_index], b_time, rtol=0, atol=0.02): + print('Failed to find a time close to b time.') pruned_a_matrix[b_index] = np.array([a_xs[a_index], a_ys[a_index], a_zs[a_index]]) return pruned_a_matrix @@ -330,10 +332,16 @@ def get_stats(self, job_id): return stats, stat_names - def write_results_to_csv(self, job_id, results_csv_output_file): + def write_results_to_csv(self, job_id, results_csv_output_file, bagfile): with open(results_csv_output_file, 'w') as results_file: writer = csv.writer(results_file) stats, stat_names = self.get_stats(job_id) + bag_name = os.path.splitext(os.path.basename(bagfile))[0] + # TODO(rsoussan): better way to do this + stats.append(stats[-2]) + stat_names.append('rmse') + stats.append(bag_name) + stat_names.append('Bag') writer.writerow(stat_names) writer.writerow(stats) @@ -605,7 +613,7 @@ def run_ekf_and_save_stats(options): log = EkfLog(options.ekf_output_file, options.start_time, options.end_time) if options.save_stats: - log.write_results_to_csv(options.job_id, options.results_csv_output_file) + log.write_results_to_csv(options.job_id, options.results_csv_output_file, astrobee_bag) print("Printed csv reults for job_id: " + str(options.job_id)) if options.make_plots: diff --git a/tools/ekf_bag/scripts/test_ekf_graph.py b/tools/ekf_bag/scripts/test_ekf_graph.py index fc97c1bff7..605995a71c 100644 --- a/tools/ekf_bag/scripts/test_ekf_graph.py +++ b/tools/ekf_bag/scripts/test_ekf_graph.py @@ -22,7 +22,7 @@ import numpy as np import unittest -import ekf_graph +import ekf_graph class TestRMSESequence(unittest.TestCase): @@ -35,9 +35,9 @@ def test_prune_missing_timestamps_beginning_set(self): b_times = np.arange(5.0) pruned_a = ekf_graph.prune_missing_timestamps(a_xs, a_ys, a_zs, a_times, b_times) self.assertEqual(len(pruned_a), len(b_times)) - self.assertTrue(np.allclose(pruned_a[:, 0], b_times)) - self.assertTrue(np.allclose(pruned_a[:, 1], b_times + 1.0)) - self.assertTrue(np.allclose(pruned_a[:, 2], b_times + 2.0)) + self.assertTrue(np.allclose(pruned_a[:, 0], b_times, rtol=0)) + self.assertTrue(np.allclose(pruned_a[:, 1], b_times + 1.0, rtol=0)) + self.assertTrue(np.allclose(pruned_a[:, 2], b_times + 2.0, rtol=0)) def test_prune_missing_timestamps_middle_set(self): a_times = np.arange(10.0) @@ -47,9 +47,9 @@ def test_prune_missing_timestamps_middle_set(self): b_times = np.arange(3.0, 7.0) pruned_a = ekf_graph.prune_missing_timestamps(a_xs, a_ys, a_zs, a_times, b_times) self.assertEqual(len(pruned_a), len(b_times)) - self.assertTrue(np.allclose(pruned_a[:, 0], b_times)) - self.assertTrue(np.allclose(pruned_a[:, 1], b_times + 1.0)) - self.assertTrue(np.allclose(pruned_a[:, 2], b_times + 2.0)) + self.assertTrue(np.allclose(pruned_a[:, 0], b_times, rtol=0)) + self.assertTrue(np.allclose(pruned_a[:, 1], b_times + 1.0, rtol=0)) + self.assertTrue(np.allclose(pruned_a[:, 2], b_times + 2.0, rtol=0)) def test_prune_missing_timestamps_end_set(self): a_times = np.arange(10.0) @@ -59,9 +59,9 @@ def test_prune_missing_timestamps_end_set(self): b_times = np.arange(7.0, 10.0) pruned_a = ekf_graph.prune_missing_timestamps(a_xs, a_ys, a_zs, a_times, b_times) self.assertEqual(len(pruned_a), len(b_times)) - self.assertTrue(np.allclose(pruned_a[:, 0], b_times)) - self.assertTrue(np.allclose(pruned_a[:, 1], b_times + 1.0)) - self.assertTrue(np.allclose(pruned_a[:, 2], b_times + 2.0)) + self.assertTrue(np.allclose(pruned_a[:, 0], b_times, rtol=0)) + self.assertTrue(np.allclose(pruned_a[:, 1], b_times + 1.0, rtol=0)) + self.assertTrue(np.allclose(pruned_a[:, 2], b_times + 2.0, rtol=0)) def test_prune_missing_timestamps_scattered_set(self): a_times = np.arange(10.0) @@ -71,20 +71,20 @@ def test_prune_missing_timestamps_scattered_set(self): b_times = np.array([1.0, 5.0, 6.0, 9.0]) pruned_a = ekf_graph.prune_missing_timestamps(a_xs, a_ys, a_zs, a_times, b_times) self.assertEqual(len(pruned_a), len(b_times)) - self.assertTrue(np.allclose(pruned_a[:, 0], b_times)) - self.assertTrue(np.allclose(pruned_a[:, 1], b_times + 1.0)) - self.assertTrue(np.allclose(pruned_a[:, 2], b_times + 2.0)) + self.assertTrue(np.allclose(pruned_a[:, 0], b_times, rtol=0)) + self.assertTrue(np.allclose(pruned_a[:, 1], b_times + 1.0, rtol=0)) + self.assertTrue(np.allclose(pruned_a[:, 2], b_times + 2.0, rtol=0)) def test_rmse_matrix_ones(self): ones_mat = np.ones(shape=(5, 3)) - self.assertTrue(np.isclose(ekf_graph.rmse_matrix(ones_mat), math.sqrt(3.0))) + self.assertTrue(np.isclose(ekf_graph.rmse_matrix(ones_mat), math.sqrt(3.0), rtol=0)) def test_rmse_matrix_1_2_2(self): col_0 = np.ones(5) col_1 = np.full(5,2.0) col_2 = np.full(5,2.0) mat = np.column_stack((col_0, col_1, col_2)) - self.assertTrue(np.isclose(ekf_graph.rmse_matrix(mat), 3.0)) + self.assertTrue(np.isclose(ekf_graph.rmse_matrix(mat), 3.0, rtol=0)) if __name__ == '__main__': unittest.main() diff --git a/tools/ekf_bag/src/ekf_bag.cc b/tools/ekf_bag/src/ekf_bag.cc index 76f799dd22..688af2aa05 100644 --- a/tools/ekf_bag/src/ekf_bag.cc +++ b/tools/ekf_bag/src/ekf_bag.cc @@ -28,8 +28,8 @@ namespace ekf_bag { -EkfBag::EkfBag(const char* bagfile, const char* mapfile, bool run_ekf, - bool gen_features, const char* biasfile, std::string image_topic, const std::string& gnc_config) +EkfBag::EkfBag(const char* bagfile, const char* mapfile, bool run_ekf, bool gen_features, const char* biasfile, + std::string image_topic, const std::string& gnc_config) : map_(mapfile, true), loc_(&map_), run_ekf_(run_ekf), @@ -55,10 +55,8 @@ void EkfBag::ReadParams(config_reader::ConfigReader* config) { return; } - if (!config->GetReal("sparse_map_delay", &sparse_map_delay_)) - ROS_FATAL("sparse_map_delay not specified."); - if (!config->GetReal("of_delay", &of_delay_)) - ROS_FATAL("of_delay not specified."); + if (!config->GetReal("sparse_map_delay", &sparse_map_delay_)) ROS_FATAL("sparse_map_delay not specified."); + if (!config->GetReal("of_delay", &of_delay_)) ROS_FATAL("of_delay not specified."); ekf_.ReadParams(config); of_.ReadParams(config); @@ -96,8 +94,7 @@ void EkfBag::EstimateBias(void) { ros::Time start = temp.getBeginTime(); // look in only the first few seconds - rosbag::View early_imu(bag_, rosbag::TopicQuery(topics), start, - start + ros::Duration(5.0)); + rosbag::View early_imu(bag_, rosbag::TopicQuery(topics), start, start + ros::Duration(5.0)); int count = 0; for (rosbag::MessageInstance const m : early_imu) { @@ -141,8 +138,7 @@ void EkfBag::UpdateImu(const ros::Time& time, const sensor_msgs::Imu& imu) { if (ret) UpdateEKF(state); } -void EkfBag::UpdateImage(const ros::Time& time, - const sensor_msgs::ImageConstPtr& image_msg) { +void EkfBag::UpdateImage(const ros::Time& time, const sensor_msgs::ImageConstPtr& image_msg) { if (!run_ekf_) return; if (!gen_features_) return; if (!processing_of_) { @@ -170,8 +166,7 @@ void EkfBag::UpdateImage(const ros::Time& time, // do processing now, but send it later cv_bridge::CvImageConstPtr image; try { - image = - cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::MONO8); + image = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::MONO8); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); return; @@ -206,8 +201,7 @@ void EkfBag::UpdateSparseMapReg(const ff_msgs::CameraRegistration& reg) { bool string_ends_with(const std::string& str, const std::string& ending) { if (str.length() >= ending.length()) { - return (0 == str.compare(str.length() - ending.length(), ending.length(), - ending)); + return (0 == str.compare(str.length() - ending.length(), ending.length(), ending)); } else { return false; } @@ -253,8 +247,7 @@ void EkfBag::Run(void) { progress++; if (string_ends_with(m.getTopic(), image_topic_)) { - sensor_msgs::ImageConstPtr image_msg = - m.instantiate(); + sensor_msgs::ImageConstPtr image_msg = m.instantiate(); UpdateImage(m.getTime(), image_msg); // TODO(rsoussan): making print this optional /*ff_common::PrintProgressBar(stdout, @@ -263,26 +256,19 @@ void EkfBag::Run(void) { sensor_msgs::ImuConstPtr imu_msg = m.instantiate(); UpdateImu(m.getTime(), *imu_msg.get()); } else if (string_ends_with(m.getTopic(), TOPIC_LOCALIZATION_TRUTH)) { - geometry_msgs::PoseStampedConstPtr gt_msg = - m.instantiate(); + geometry_msgs::PoseStampedConstPtr gt_msg = m.instantiate(); UpdateGroundTruth(*gt_msg.get()); } else if (string_ends_with(m.getTopic(), TOPIC_LOCALIZATION_ML_FEATURES)) { - ff_msgs::VisualLandmarksConstPtr vl_msg = - m.instantiate(); + ff_msgs::VisualLandmarksConstPtr vl_msg = m.instantiate(); UpdateSparseMap(*vl_msg.get()); - } else if (string_ends_with(m.getTopic(), - TOPIC_LOCALIZATION_ML_REGISTRATION)) { - ff_msgs::CameraRegistrationConstPtr reg_msg = - m.instantiate(); + } else if (string_ends_with(m.getTopic(), TOPIC_LOCALIZATION_ML_REGISTRATION)) { + ff_msgs::CameraRegistrationConstPtr reg_msg = m.instantiate(); UpdateSparseMapReg(*reg_msg.get()); } else if (string_ends_with(m.getTopic(), TOPIC_LOCALIZATION_OF_FEATURES)) { - ff_msgs::Feature2dArrayConstPtr of_msg = - m.instantiate(); + ff_msgs::Feature2dArrayConstPtr of_msg = m.instantiate(); UpdateOpticalFlow(*of_msg.get()); - } else if (string_ends_with(m.getTopic(), - TOPIC_LOCALIZATION_OF_REGISTRATION)) { - ff_msgs::CameraRegistrationConstPtr reg_msg = - m.instantiate(); + } else if (string_ends_with(m.getTopic(), TOPIC_LOCALIZATION_OF_REGISTRATION)) { + ff_msgs::CameraRegistrationConstPtr reg_msg = m.instantiate(); UpdateOpticalFlowReg(*reg_msg.get()); } else if (string_ends_with(m.getTopic(), TOPIC_GNC_EKF)) { ff_msgs::EkfStateConstPtr ekf_msg = m.instantiate(); diff --git a/tools/ekf_bag/src/ekf_bag_csv.cc b/tools/ekf_bag/src/ekf_bag_csv.cc index 1f7b26d007..643cbfa6f0 100644 --- a/tools/ekf_bag/src/ekf_bag_csv.cc +++ b/tools/ekf_bag/src/ekf_bag_csv.cc @@ -77,6 +77,10 @@ void EkfBagCsv::UpdateEKF(const ff_msgs::EkfState & s) { start_time_ = s.header.stamp; start_time_set_ = true; } + + // Only print ekf results if it has been initialized with a sparse mapping measurement + if (!ekf_.HasPoseEstimate()) return; + fprintf(f_, "EKF %g ", (s.header.stamp - start_time_).toSec()); fprintf(f_, "%g %g %g ", s.pose.position.x, s.pose.position.y, s.pose.position.z); Eigen::Vector3f euler = QuatToEuler(s.pose.orientation); diff --git a/tools/gnc_visualizer/scripts/communications/com_manager.py b/tools/gnc_visualizer/scripts/communications/com_manager.py index e62f95f395..b69cd9dbc2 100644 --- a/tools/gnc_visualizer/scripts/communications/com_manager.py +++ b/tools/gnc_visualizer/scripts/communications/com_manager.py @@ -108,6 +108,7 @@ def __start_ros_com(self): self.subs_manager = com.RosSubscriberManager('gnc_visualizer', self.viz.print_to_log) self.subs_manager.add_subscriber('rosout', "/rosout", self.viz.log_callback, com.Log) self.subs_manager.add_subscriber('truth', "/loc/truth", self.viz.ground_truth_callback, com.PoseStamped) + self.subs_manager.add_subscriber('ml_pose', "/sparse_mapping/pose", self.viz.ml_pose_callback, com.PoseStamped) self.subs_manager.add_subscriber('ekf', "/gnc/ekf", self.viz.ekf_callback, com.EkfState) self.subs_manager.add_subscriber('ctl_cmd', "/gnc/ctl/command", self.viz.command_callback, com.FamCommand) self.subs_manager.add_subscriber('ctl_traj', "/gnc/ctl/traj", self.viz.traj_callback, com.ControlState) @@ -119,6 +120,9 @@ def __start_ros_com(self): def __start_dds_com(self): self.subs_manager = com.DdsSubscriberManager() + self.subs_manager.add_subscriber( + "sparse_mapping_pose_sub", com.DdsSubscriber( + "SparseMappingPoseStampedSubscriber::SparseMappingPoseStampedReader", self.viz.ml_pose_callback, com.PoseStamped), True) self.subs_manager.add_subscriber( "ekf_sub", com.DdsSubscriber( "EkfSubscriber::EkfReader", self.viz.ekf_callback, com.EkfState), True) diff --git a/tools/gnc_visualizer/scripts/communications/data_support.py b/tools/gnc_visualizer/scripts/communications/data_support.py index 6273bdfb17..2f5156ca30 100644 --- a/tools/gnc_visualizer/scripts/communications/data_support.py +++ b/tools/gnc_visualizer/scripts/communications/data_support.py @@ -74,6 +74,11 @@ def __init__(self, linear = Vector3(), angular = Vector3()): self.linear = linear self.angular = angular +class PoseStamped(Common): + def __init__(self): + self.header = Header() + self.pose = Pose() + class EkfState(Common): def __init__(self): self.header = Header() @@ -95,6 +100,7 @@ def __init__(self): #self.ml_mahal_dists = [0.0] * 50 self.ml_mahal_dists = np.zeros(50) + class PmcCommand(Common): def __init__(self, header = Header()): self.header = header diff --git a/tools/gnc_visualizer/scripts/communications/dds_support.py b/tools/gnc_visualizer/scripts/communications/dds_support.py index c6b2773dbf..7fae7e33c5 100644 --- a/tools/gnc_visualizer/scripts/communications/dds_support.py +++ b/tools/gnc_visualizer/scripts/communications/dds_support.py @@ -17,7 +17,7 @@ # License for the specific language governing permissions and limitations # under the License. -from data_support import PmcCommand, EkfState, FamCommand, ControlState, Quaternion, Vector3, Log +from data_support import PmcCommand, PoseStamped, EkfState, FamCommand, ControlState, Quaternion, Vector3, Log from sys import path as sysPath from os import path as osPath @@ -127,6 +127,7 @@ def stop(self): class Dict2RosMsgTranslator: supported_ros_types = { + PoseStamped: lambda self, data: self.__dictionary_to_pose_stamped_msg(data), EkfState: lambda self, data: self.__dictionary_to_ekf_msg(data), FamCommand: lambda self, data: self.__dictionary_to_fam_msg(data), ControlState: lambda self, data: self.__dictionary_to_control_msg(data), @@ -195,6 +196,15 @@ def __dictionary_to_fam_msg(self, dic): #print msg.asDict() return msg + + def __dictionary_to_pose_stamped_msg(self, dic): + msg = PoseStamped() + msg.header.stamp = dic['hdr']['timeStamp'] + msg.pose.position = self.__array_to_vector3d(dic['pose']['xyz']) + msg.pose.orientation = self.__array_to_quaternion(dic['pose']['rot']) + return msg + + def __dictionary_to_ekf_msg(self, dic): #print dic msg = EkfState() diff --git a/tools/gnc_visualizer/scripts/communications/dds_types/BaseDDSProfile.xml b/tools/gnc_visualizer/scripts/communications/dds_types/BaseDDSProfile.xml index 0a5a4e3b43..aa4e9d8fbf 100644 --- a/tools/gnc_visualizer/scripts/communications/dds_types/BaseDDSProfile.xml +++ b/tools/gnc_visualizer/scripts/communications/dds_types/BaseDDSProfile.xml @@ -586,6 +586,8 @@ + + @@ -931,16 +933,20 @@ + + + + @@ -952,6 +958,30 @@ + + + + + + + + Bumble + + + + + + + + + + + + Bumble + + + + diff --git a/tools/gnc_visualizer/scripts/communications/dds_types/PositionSample.xml b/tools/gnc_visualizer/scripts/communications/dds_types/PositionSample.xml new file mode 100644 index 0000000000..752cc0ba9d --- /dev/null +++ b/tools/gnc_visualizer/scripts/communications/dds_types/PositionSample.xml @@ -0,0 +1,29 @@ + + + + +class PositionSampleTypeSupport; +class PositionSampleDataWriter; +class PositionSampleDataReader; +struct PositionSampleSeq; +/** + * PositionSample message delivers the position of the Agent. Corresponding PositionConfig sets up the + * coordinate frame and specifies how the Transform3D.rot field is to be interpreted for pose and velocity. + */ + +#if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) +typedef PositionSampleTypeSupport TypeSupport; +typedef PositionSampleDataWriter DataWriter; +typedef PositionSampleDataReader DataReader; +typedef PositionSampleSeq Seq; +#endif +typedef PositionSample Type; +/** 3D pose of the agent. PositionConfig.poseEncoding specifies how to interpret the rotation. */ + +/** 3D velocity of the agent. PositionConfig.velocityEncoding specifies how to interpret the rotation. */ + +/** Any Agent-specific information needed. */ + + + + diff --git a/tools/gnc_visualizer/scripts/plot_types.py b/tools/gnc_visualizer/scripts/plot_types.py index 8e3cf838a6..04c325f417 100644 --- a/tools/gnc_visualizer/scripts/plot_types.py +++ b/tools/gnc_visualizer/scripts/plot_types.py @@ -115,6 +115,10 @@ def __init__(self): self.truth_x = self.plot(symbol='d', pen=None, symbolPen=None, symbolBrush=colors[0], symbolSize=5) self.truth_y = self.plot(symbol='d', pen=None, symbolPen=None, symbolBrush=colors[1], symbolSize=5) self.truth_z = self.plot(symbol='d', pen=None, symbolPen=None, symbolBrush=colors[2], symbolSize=5) + self.ml_pose_x = self.plot(symbol='d', pen=None, symbolPen=None, symbolBrush=colors[0], symbolSize=5) + self.ml_pose_y = self.plot(symbol='d', pen=None, symbolPen=None, symbolBrush=colors[1], symbolSize=5) + self.ml_pose_z = self.plot(symbol='d', pen=None, symbolPen=None, symbolBrush=colors[2], symbolSize=5) + def update_plot(self, data): super(PositionPlot, self).update_plot(data) @@ -126,6 +130,10 @@ def update_plot(self, data): self.truth_x.setData(data[0]['truth_time'][:s], data[0]['truth_position_x'][:s]) self.truth_y.setData(data[0]['truth_time'][:s], data[0]['truth_position_y'][:s]) self.truth_z.setData(data[0]['truth_time'][:s], data[0]['truth_position_z'][:s]) + s = data[1]['ml_pose_time'] + self.ml_pose_x.setData(data[0]['ml_pose_time'][:s], data[0]['ml_pose_position_x'][:s]) + self.ml_pose_y.setData(data[0]['ml_pose_time'][:s], data[0]['ml_pose_position_y'][:s]) + self.ml_pose_z.setData(data[0]['ml_pose_time'][:s], data[0]['ml_pose_position_z'][:s]) class VelocityPlot(VectorPlot): def __init__(self): @@ -151,6 +159,10 @@ def __init__(self): self.truth_x = self.plot(symbol='d', pen=None, symbolPen=None, symbolBrush=colors[0], symbolSize=5) self.truth_y = self.plot(symbol='d', pen=None, symbolPen=None, symbolBrush=colors[1], symbolSize=5) self.truth_z = self.plot(symbol='d', pen=None, symbolPen=None, symbolBrush=colors[2], symbolSize=5) + self.ml_pose_x = self.plot(symbol='d', pen=None, symbolPen=None, symbolBrush=colors[0], symbolSize=5) + self.ml_pose_y = self.plot(symbol='d', pen=None, symbolPen=None, symbolBrush=colors[1], symbolSize=5) + self.ml_pose_z = self.plot(symbol='d', pen=None, symbolPen=None, symbolBrush=colors[2], symbolSize=5) + def update_plot(self, data): super(OrientationPlot, self).update_plot(data) @@ -162,6 +174,11 @@ def update_plot(self, data): self.truth_x.setData(data[0]['truth_time'][:s], data[0]['truth_rot_x'][:s]) self.truth_y.setData(data[0]['truth_time'][:s], data[0]['truth_rot_y'][:s]) self.truth_z.setData(data[0]['truth_time'][:s], data[0]['truth_rot_z'][:s]) + s = data[1]['ml_pose_time'] + self.ml_pose_x.setData(data[0]['ml_pose_time'][:s], data[0]['ml_pose_rot_x'][:s]) + self.ml_pose_y.setData(data[0]['ml_pose_time'][:s], data[0]['ml_pose_rot_y'][:s]) + self.ml_pose_z.setData(data[0]['ml_pose_time'][:s], data[0]['ml_pose_rot_z'][:s]) + class OmegaPlot(VectorPlot): def __init__(self): @@ -312,8 +329,6 @@ def update_plot(self, data): cur_ml -= 1 else: cur_of -= 1 - self.disableAutoRange() - self.setYRange(0, 50) class MLMahalPlot(GraphPlot): def __init__(self): diff --git a/tools/gnc_visualizer/scripts/visualizer.py b/tools/gnc_visualizer/scripts/visualizer.py index 38ee421638..e659a010fe 100755 --- a/tools/gnc_visualizer/scripts/visualizer.py +++ b/tools/gnc_visualizer/scripts/visualizer.py @@ -134,6 +134,15 @@ def mahal_filter(dists): 'truth_rot_z': lambda x: quat_to_eulers(x.pose.orientation)[2] * 180 / pi} truth_callbacks = VisualizerCallback('truth_time', truth_data) +ml_pose_data = {'ml_pose_time': lambda x: time.time() - start_time, + 'ml_pose_position_x': lambda x: x.pose.position.x, + 'ml_pose_position_y': lambda x: x.pose.position.y, + 'ml_pose_position_z': lambda x: x.pose.position.z, + 'ml_pose_rot_x': lambda x: quat_to_eulers(x.pose.orientation)[0] * 180 / pi, + 'ml_pose_rot_y': lambda x: quat_to_eulers(x.pose.orientation)[1] * 180 / pi, + 'ml_pose_rot_z': lambda x: quat_to_eulers(x.pose.orientation)[2] * 180 / pi} +ml_pose_callbacks = VisualizerCallback('ml_pose_time', ml_pose_data) + command_data = {'command_time': lambda x: time.time() - start_time, 'command_status': lambda x: x.status, 'command_control_mode': lambda x: x.control_mode, @@ -192,7 +201,7 @@ def mahal_filter(dists): 'pmc_2_nozzle_6': lambda x: ord(x.goals[1].nozzle_positions[5]) } pmc_callbacks = VisualizerCallback('pmc_time', pmc_data) -callbacks_list = [ekf_callbacks, ml_callbacks, of_callbacks, truth_callbacks, command_callbacks, traj_callbacks, shaper_callbacks, pmc_callbacks] +callbacks_list = [ekf_callbacks, ml_callbacks, of_callbacks, truth_callbacks, ml_pose_callbacks, command_callbacks, traj_callbacks, shaper_callbacks, pmc_callbacks] class TerminalView(QtGui.QGraphicsTextItem): def __init__(self, graphics_view): @@ -253,8 +262,8 @@ def __init__(self, launch_command = None, plan = None, com_method = com.DDS_COM) for k in d.callbacks.keys(): self.data[k] = np.full(ARRAY_SIZE, 1e-10) - self.columns = [[plot_types.CtlPosPlot, plot_types.FeatureCountPlot, plot_types.ConfidencePlot, \ - plot_types.CommandStatusPlot], [plot_types.CtlRotPlot, plot_types.CovPlot]] + self.columns = [[plot_types.CtlPosPlot, plot_types.PositionPlot, plot_types.FeatureCountPlot, plot_types.ConfidencePlot, \ + plot_types.CommandStatusPlot], [plot_types.CtlRotPlot, plot_types.OrientationPlot, plot_types.CovPlot]] self.graphics_view = ParentGraphicsView() self.terminal_graphics_view = TerminalGraphicsView(self.graphics_view) @@ -567,6 +576,9 @@ def ekf_callback(self, data): def ground_truth_callback(self, data): self.add_data(truth_callbacks, data) + def ml_pose_callback(self, data): + self.add_data(ml_pose_callbacks, data) + def command_callback(self, data): self.add_data(command_callbacks, data) diff --git a/tools/graph_bag/CMakeLists.txt b/tools/graph_bag/CMakeLists.txt new file mode 100644 index 0000000000..c4982c61b3 --- /dev/null +++ b/tools/graph_bag/CMakeLists.txt @@ -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. + +cmake_minimum_required(VERSION 2.8.3) + +project(graph_bag) + +if (USE_ROS) + +catkin_package( + CATKIN_DEPENDS roscpp rosbag +) + +# include ff_nodelet to get ff_util header files since these aren't exposed elsewhere +create_library(TARGET ${PROJECT_NAME} + LIBS ff_common ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} camera ff_nodelet graph_localizer imu_augmentor imu_bias_tester imu_integration lk_optical_flow localization_common localization_measurements localization_node sparse_mapping msg_conversions + INC ${catkin_INCLUDE_DIRS} +) + +create_tool_targets(DIR tools + LIBS ${PROJECT_NAME} # profiler + INC ${catkin_INCLUDE_DIRS} +) + +endif (USE_ROS) diff --git a/tools/graph_bag/include/graph_bag/bag_imu_filterer.h b/tools/graph_bag/include/graph_bag/bag_imu_filterer.h new file mode 100644 index 0000000000..6d2f88e13f --- /dev/null +++ b/tools/graph_bag/include/graph_bag/bag_imu_filterer.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 GRAPH_BAG_BAG_IMU_FILTERER_H_ +#define GRAPH_BAG_BAG_IMU_FILTERER_H_ + +#include + +#include +#include + +#include + +namespace graph_bag { +// Reads through a bag file and filters imu measurements, replacing the old imu measurements with new filtered ones. +// Saves output to a new bagfile. +class BagImuFilterer { + public: + BagImuFilterer(const std::string& bag_name, const std::string& filtered_bag, const std::string& filter_name); + void Convert(); + + private: + std::unique_ptr imu_filter_; + rosbag::Bag bag_; + rosbag::Bag filtered_bag_; +}; +} // end namespace graph_bag + +#endif // GRAPH_BAG_BAG_IMU_FILTERER_H_ diff --git a/tools/graph_bag/include/graph_bag/graph_bag.h b/tools/graph_bag/include/graph_bag/graph_bag.h new file mode 100644 index 0000000000..fe864daeea --- /dev/null +++ b/tools/graph_bag/include/graph_bag/graph_bag.h @@ -0,0 +1,71 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_BAG_GRAPH_BAG_H_ +#define GRAPH_BAG_GRAPH_BAG_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace graph_bag { +// 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 { + 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 = ""); + void Run(); + + private: + void InitializeGraph(); + void SaveOpticalFlowTracksImage(const sensor_msgs::ImageConstPtr& image_msg, + const graph_localizer::FeatureTrackMap& feature_tracks); + void SaveImuBiasTesterPredictedStates( + const std::vector& imu_bias_tester_predicted_states); + template + void SaveMsg(const MsgType& msg, const std::string& topic) { + const ros::Time timestamp = localization_common::RosTimeFromHeader(msg.header); + results_bag_.write("/" + topic, timestamp, msg); + } + + std::unique_ptr graph_localizer_simulator_; + std::unique_ptr live_measurement_simulator_; + rosbag::Bag results_bag_; + imu_bias_tester::ImuBiasTesterWrapper imu_bias_tester_wrapper_; + imu_augmentor::ImuAugmentorWrapper imu_augmentor_wrapper_; + std::string output_stats_file_; + const std::string kFeatureTracksImageTopic_ = "feature_track_image"; + GraphBagParams params_; +}; +} // end namespace graph_bag + +#endif // GRAPH_BAG_GRAPH_BAG_H_ diff --git a/tools/graph_bag/include/graph_bag/graph_bag_params.h b/tools/graph_bag/include/graph_bag/graph_bag_params.h new file mode 100644 index 0000000000..32598d9a13 --- /dev/null +++ b/tools/graph_bag/include/graph_bag/graph_bag_params.h @@ -0,0 +1,37 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_BAG_GRAPH_BAG_PARAMS_H_ +#define GRAPH_BAG_GRAPH_BAG_PARAMS_H_ + +#include + +#include + +#include + +namespace graph_bag { +struct GraphBagParams { + bool save_optical_flow_images; + std::unique_ptr nav_cam_params; + gtsam::Pose3 body_T_nav_cam; + int ar_min_num_landmarks; + int sparse_mapping_min_num_landmarks; +}; +} // namespace graph_bag + +#endif // GRAPH_BAG_GRAPH_BAG_PARAMS_H_ diff --git a/tools/graph_bag/include/graph_bag/graph_localizer_simulator.h b/tools/graph_bag/include/graph_bag/graph_localizer_simulator.h new file mode 100644 index 0000000000..dba6789574 --- /dev/null +++ b/tools/graph_bag/include/graph_bag/graph_localizer_simulator.h @@ -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. + */ + +#ifndef GRAPH_BAG_GRAPH_LOCALIZER_SIMULATOR_H_ +#define GRAPH_BAG_GRAPH_LOCALIZER_SIMULATOR_H_ + +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace graph_bag { +class GraphLocalizerSimulator : public graph_localizer::GraphLocalizerWrapper { + 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); + + bool AddMeasurementsAndUpdateIfReady(const localization_common::Time& current_time); + + private: + std::vector of_msg_buffer_; + std::vector vl_msg_buffer_; + std::vector ar_msg_buffer_; + std::vector imu_msg_buffer_; + boost::optional last_update_time_; + GraphLocalizerSimulatorParams params_; +}; +} // namespace graph_bag + +#endif // GRAPH_BAG_GRAPH_LOCALIZER_SIMULATOR_H_ diff --git a/tools/graph_bag/include/graph_bag/graph_localizer_simulator_params.h b/tools/graph_bag/include/graph_bag/graph_localizer_simulator_params.h new file mode 100644 index 0000000000..d4057e0c9b --- /dev/null +++ b/tools/graph_bag/include/graph_bag/graph_localizer_simulator_params.h @@ -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. + */ +#ifndef GRAPH_BAG_GRAPH_LOCALIZER_SIMULATOR_PARAMS_H_ +#define GRAPH_BAG_GRAPH_LOCALIZER_SIMULATOR_PARAMS_H_ + +namespace graph_bag { +struct GraphLocalizerSimulatorParams { + double optimization_time; +}; +} // namespace graph_bag + +#endif // GRAPH_BAG_GRAPH_LOCALIZER_SIMULATOR_PARAMS_H_ diff --git a/tools/graph_bag/include/graph_bag/live_measurement_simulator.h b/tools/graph_bag/include/graph_bag/live_measurement_simulator.h new file mode 100644 index 0000000000..d496c27011 --- /dev/null +++ b/tools/graph_bag/include/graph_bag/live_measurement_simulator.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 GRAPH_BAG_LIVE_MEASUREMENT_SIMULATOR_H_ +#define GRAPH_BAG_LIVE_MEASUREMENT_SIMULATOR_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace graph_bag { +class LiveMeasurementSimulator { + public: + explicit LiveMeasurementSimulator(const LiveMeasurementSimulatorParams& params); + + bool ProcessMessage(); + + localization_common::Time CurrentTime(); + + boost::optional GetImageMessage(const localization_common::Time current_time); + boost::optional GetImuMessage(const localization_common::Time current_time); + boost::optional GetOFMessage(const localization_common::Time current_time); + boost::optional GetVLMessage(const localization_common::Time current_time); + boost::optional GetARMessage(const localization_common::Time current_time); + + private: + bool string_ends_with(const std::string& str, const std::string& ending); + + ff_msgs::Feature2dArray GenerateOFFeatures(const sensor_msgs::ImageConstPtr& image_msg); + + bool GenerateVLFeatures(const sensor_msgs::ImageConstPtr& image_msg, ff_msgs::VisualLandmarks& vl_features); + + rosbag::Bag bag_; + sparse_mapping::SparseMap map_; + localization_node::Localizer map_feature_matcher_; + LiveMeasurementSimulatorParams params_; + lk_optical_flow::LKOpticalFlow optical_flow_tracker_; + const std::string kImageTopic_; + std::unique_ptr view_; + boost::optional view_it_; + std::map img_buffer_; + MessageBuffer imu_buffer_; + MessageBuffer of_buffer_; + MessageBuffer vl_buffer_; + MessageBuffer ar_buffer_; + localization_common::Time current_time_; +}; +} // namespace graph_bag + +#endif // GRAPH_BAG_LIVE_MEASUREMENT_SIMULATOR_H_ diff --git a/tools/graph_bag/include/graph_bag/live_measurement_simulator_params.h b/tools/graph_bag/include/graph_bag/live_measurement_simulator_params.h new file mode 100644 index 0000000000..5a2e16cc4f --- /dev/null +++ b/tools/graph_bag/include/graph_bag/live_measurement_simulator_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 GRAPH_BAG_LIVE_MEASUREMENT_SIMULATOR_PARAMS_H_ +#define GRAPH_BAG_LIVE_MEASUREMENT_SIMULATOR_PARAMS_H_ + +#include + +#include + +namespace graph_bag { +struct LiveMeasurementSimulatorParams { + MessageBufferParams imu; + MessageBufferParams of; + MessageBufferParams vl; + MessageBufferParams ar; + MessageBufferParams img; + std::string bag_name; + std::string map_file; + std::string image_topic; + bool use_image_features; + bool save_optical_flow_images; +}; +} // namespace graph_bag + +#endif // GRAPH_BAG_LIVE_MEASUREMENT_SIMULATOR_PARAMS_H_ diff --git a/tools/graph_bag/include/graph_bag/message_buffer.h b/tools/graph_bag/include/graph_bag/message_buffer.h new file mode 100644 index 0000000000..6bf7059ef5 --- /dev/null +++ b/tools/graph_bag/include/graph_bag/message_buffer.h @@ -0,0 +1,61 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_BAG_MESSAGE_BUFFER_H_ +#define GRAPH_BAG_MESSAGE_BUFFER_H_ + +#include +#include +#include + +#include + +namespace graph_bag { +template +class MessageBuffer { + public: + explicit MessageBuffer(const MessageBufferParams& params) : params_(params) {} + // Assumes messages are buffered in time order + void BufferMessage(const MessageType& msg) { + const localization_common::Time timestamp = localization_common::TimeFromHeader(msg.header); + if (last_measurement_time_ && std::abs(*last_measurement_time_ - timestamp) < params_.min_msg_spacing) { + LOG(WARNING) << "BufferMessage: Dropping message that arrived too close to previous message."; + return; + } + msg_buffer_.emplace(timestamp, msg); + last_measurement_time_ = timestamp; + } + + boost::optional GetMessage(const localization_common::Time current_time) { + if (msg_buffer_.empty()) return boost::none; + if (current_time - msg_buffer_.cbegin()->first < params_.msg_delay) { + VLOG(2) << "GetMessage: Current time too close to message time, no message available."; + return boost::none; + } + const auto msg = msg_buffer_.cbegin()->second; + msg_buffer_.erase(msg_buffer_.begin()); + return msg; + } + + private: + MessageBufferParams params_; + std::map msg_buffer_; + boost::optional last_measurement_time_; +}; +} // namespace graph_bag + +#endif // GRAPH_BAG_MESSAGE_BUFFER_H_ diff --git a/tools/graph_bag/include/graph_bag/message_buffer_params.h b/tools/graph_bag/include/graph_bag/message_buffer_params.h new file mode 100644 index 0000000000..2f32851acf --- /dev/null +++ b/tools/graph_bag/include/graph_bag/message_buffer_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 GRAPH_BAG_MESSAGE_BUFFER_PARAMS_H_ +#define GRAPH_BAG_MESSAGE_BUFFER_PARAMS_H_ + +namespace graph_bag { +struct MessageBufferParams { + double msg_delay; + // Some message providers drop messages too close together in time. Drop all messages < min_msg_spacing from each + // other. + double min_msg_spacing; +}; +} // namespace graph_bag + +#endif // GRAPH_BAG_MESSAGE_BUFFER_PARAMS_H_ diff --git a/tools/graph_bag/include/graph_bag/parameter_reader.h b/tools/graph_bag/include/graph_bag/parameter_reader.h new file mode 100644 index 0000000000..28bfb44009 --- /dev/null +++ b/tools/graph_bag/include/graph_bag/parameter_reader.h @@ -0,0 +1,38 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_BAG_PARAMETER_READER_H_ +#define GRAPH_BAG_PARAMETER_READER_H_ + +#include +#include +#include +#include +#include + +#include + +namespace graph_bag { +void LoadMessageBufferParams(config_reader::ConfigReader& config, MessageBufferParams& params); +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); +} // namespace graph_bag + +#endif // GRAPH_BAG_PARAMETER_READER_H_ diff --git a/tools/graph_bag/include/graph_bag/sparse_mapping_pose_adder.h b/tools/graph_bag/include/graph_bag/sparse_mapping_pose_adder.h new file mode 100644 index 0000000000..a42a97eee2 --- /dev/null +++ b/tools/graph_bag/include/graph_bag/sparse_mapping_pose_adder.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 GRAPH_BAG_SPARSE_MAPPING_POSE_ADDER_H_ +#define GRAPH_BAG_SPARSE_MAPPING_POSE_ADDER_H_ + +#include + +#include +#include + +#include + +namespace graph_bag { +// Reads through a bag file and adds sparse mapping poses using ml/features/pose messages +// and body_T_nav_cam extrinsics +class SparseMappingPoseAdder { + public: + SparseMappingPoseAdder(const std::string& input_bag_name, const std::string& output_bag_name, + const gtsam::Pose3& nav_cam_T_body); + void AddPoses(); + + private: + rosbag::Bag input_bag_; + rosbag::Bag output_bag_; + gtsam::Pose3 nav_cam_T_body_; +}; +} // end namespace graph_bag + +#endif // GRAPH_BAG_SPARSE_MAPPING_POSE_ADDER_H_ diff --git a/tools/graph_bag/include/graph_bag/utilities.h b/tools/graph_bag/include/graph_bag/utilities.h new file mode 100644 index 0000000000..7360f7c1e5 --- /dev/null +++ b/tools/graph_bag/include/graph_bag/utilities.h @@ -0,0 +1,38 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_BAG_UTILITIES_H_ +#define GRAPH_BAG_UTILITIES_H_ + +#include +#include + +#include + +#include + +namespace graph_bag { +void FeatureTrackImage(const graph_localizer::FeatureTrackMap& feature_tracks, + const camera::CameraParameters& camera_params, cv::Mat& feature_track_image); +boost::optional CreateFeatureTrackImage(const sensor_msgs::ImageConstPtr& image_msg, + const graph_localizer::FeatureTrackMap& feature_tracks, + const camera::CameraParameters& camera_params); + +cv::Point Distort(const Eigen::Vector2d& undistorted_point, const camera::CameraParameters& params); +} // namespace graph_bag + +#endif // GRAPH_BAG_UTILITIES_H_ diff --git a/tools/graph_bag/package.xml b/tools/graph_bag/package.xml new file mode 100644 index 0000000000..bf259d87db --- /dev/null +++ b/tools/graph_bag/package.xml @@ -0,0 +1,29 @@ + + graph_bag + 1.0.0 + + The graph bag package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + eigen_conversions + roscpp + rosbag + graph_localizer + ff_util + tf + eigen_conversions + graph_localizer + roscpp + rosbag + ff_util + tf + diff --git a/tools/graph_bag/readme.md b/tools/graph_bag/readme.md new file mode 100644 index 0000000000..50380f251a --- /dev/null +++ b/tools/graph_bag/readme.md @@ -0,0 +1,33 @@ +\page graphbag Graph Bag + +# Package Overview +The graph bag package provides several tools for measuring localization performance as described below. + +# Tools +## GraphBag +Graph bag simulates localization using a saved bagfile. Rather than relying on rosbag play, it loads measurements directly and greatly decreases runtime. To accurately simulate measurement delays and drops, the LiveMeasurementSimulator class is provided along with a config file to provide delays and minimum spacing between measurements. Graph bag saves results to a new bag file that can be processed by the plot\_results\_main.py script into a pdf showing information such as poses estiamtes, velocity estimates, bias estiates, covariances, and more. + +## BagImuFilterer +The bag imu filterer enables the testing of imu filters written in c++. It parses a bag file and loads a c++ imu filter, then saves the filtered data to a new bag file. The filtered data can be plotted and analyzed by the imu\_analyzer script. + +# Scripts +## bag\_sweep +The bag sweep tool runs the graph bag tool in parallel on multiple bag files. It takes config file with bagnames, map names, and robot configs and produces pdfs and result bagfiles for each entry. +Example config file: +``` +/home/bag_name.bag /home/map_name.map /mgt/img_sampler/nav_cam/image_record /home/astrobee/astrobee config/robots/bumble.config iss false +/home/bag_name_2.bag /home/map_name.map /mgt/img_sampler/nav_cam/image_record /home/astrobee/astrobee config/robots/bumble.config iss false +``` +Example bag sweep command: +``` +rosrun graph_bag bag_sweep.py /home/bag_sweep_config.csv /home/output_dir +``` + +## check\_bags\_for\_gaps +This is a simple tool to check for large gaps in imu or image data in a bagfile. + +## imu\_analyzer\_main +The imu analyzer tool plots imu data and also filtered imu data. It can use python filtering tools to lowpass filter imu data at a certain cutoff frequency, or it can use a provided bag file with filtered imu data and compare this against a bag file with unfiltered data. Data is plotted in its raw form and also after being passed through an FFT to show its frequency response. + +## merge\_bags +Merge bags looks for bag files in the current directory with a provided name prefix and merges these into a single bag file. Bags are assumed to be numbered and are merged in numerical order. diff --git a/tools/graph_bag/scripts/average_results.py b/tools/graph_bag/scripts/average_results.py new file mode 100755 index 0000000000..772b789252 --- /dev/null +++ b/tools/graph_bag/scripts/average_results.py @@ -0,0 +1,59 @@ +#!/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 pandas as pd +import os + +import utilities + + +def combined_results(csv_files): + dataframes = [pd.read_csv(file) for file in csv_files] + if not dataframes: + print('Failed to create dataframes') + 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) + return combined_dataframes + + +def average_results(directory, csv_files): + combined_dataframes = combined_results(csv_files) + names = combined_dataframes.columns + mean_dataframe = pd.DataFrame() + for name in names: + mean_dataframe[name] = [combined_dataframes[name].mean()] + averaged_results_file = os.path.join(directory, 'averaged_results.csv') + mean_dataframe.to_csv(averaged_results_file, index=False) + + +# Averages results from all *stats.csv files in a directory (including subdirectories). +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('directory', help='Full path to directory where results files are.') + args = parser.parse_args() + results_csv_files = utilities.get_files(args.directory, '*stats.csv') + if not results_csv_files: + print('Failed to find stats.csv files') + exit() + average_results(args.directory, results_csv_files) diff --git a/tools/graph_bag/scripts/bag_and_parameter_sweep.py b/tools/graph_bag/scripts/bag_and_parameter_sweep.py new file mode 100755 index 0000000000..ff779fd939 --- /dev/null +++ b/tools/graph_bag/scripts/bag_and_parameter_sweep.py @@ -0,0 +1,104 @@ +#!/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 bag_sweep +import parameter_sweep +import plot_parameter_sweep_results +import utilities + +import argparse +import pandas as pd + +import os +import shutil +import sys + + +def save_ranges(param_range_directory, output_directory): + individual_ranges_file = os.path.join(param_range_directory, 'individual_value_ranges.csv') + copied_individual_ranges_file = os.path.join(output_directory, 'individual_value_ranges.csv') + shutil.copy(individual_ranges_file, copied_individual_ranges_file) + all_values_file = os.path.join(param_range_directory, 'all_value_combos.csv') + copied_all_values_file = os.path.join(output_directory, 'all_value_combos.csv') + shutil.copy(all_values_file, copied_all_values_file) + + +def add_job_id_mean_row(job_id, dataframes, mean_dataframe, names): + combined_dataframe = pd.DataFrame(None, None, names) + for dataframe in dataframes: + trimmed_dataframe = pd.DataFrame(dataframe.values[job_id:job_id + 1], columns=names) + combined_dataframe = combined_dataframe.append(trimmed_dataframe, ignore_index=True) + job_id_mean_dataframe = pd.DataFrame(None, None, names) + for name in names: + job_id_mean_dataframe[name] = [combined_dataframe[name].mean()] + return mean_dataframe.append(job_id_mean_dataframe, ignore_index=True) + + +def average_parameter_sweep_results(combined_results_csv_files, directory): + dataframes = [pd.read_csv(file) for file in combined_results_csv_files] + if not dataframes: + print('Failed to create dataframes') + exit() + names = dataframes[0].columns + mean_dataframe = pd.DataFrame(None, None, names) + jobs = dataframes[0].shape[0] + # 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.to_csv(mean_dataframe_file, index=False) + + +def bag_and_parameter_sweep(graph_bag_params_list, output_dir): + combined_results_csv_files = [] + param_range_directory = None + for graph_bag_params in graph_bag_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_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) + 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')) + average_parameter_sweep_results(combined_results_csv_files, output_dir) + save_ranges(param_range_directory, output_dir) + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('config_file') + parser.add_argument('output_dir') + 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() + output_dir = utilities.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_plot(results_pdf_file, combined_results_file, value_combos_file) diff --git a/tools/graph_bag/scripts/bag_sweep.py b/tools/graph_bag/scripts/bag_sweep.py new file mode 100644 index 0000000000..9deff9a663 --- /dev/null +++ b/tools/graph_bag/scripts/bag_sweep.py @@ -0,0 +1,109 @@ +#!/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 average_results +import multiprocessing_helpers + +import csv +import itertools +import multiprocessing +import os +import plot_bag_sweep_results +import sys + + +class GraphBagParams(object): + + def __init__(self, bagfile, map_file, image_topic, config_path, robot_config_file, world, use_image_features): + self.bagfile = bagfile + self.map_file = map_file + self.image_topic = image_topic + self.config_path = config_path + self.robot_config_file = robot_config_file + self.world = world + self.use_image_features = use_image_features + + +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() + + +# 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 graph_bag run_graph_bag ' + params.bagfile + ' ' + params.map_file + ' ' + params.config_path + ' -i ' + params.image_topic + ' -o ' + output_bag_path + ' -r ' + params.robot_config_file + ' -w ' + params.world + ' -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 graph_bag plot_results_main.py ' + output_bag_path + ' --output-file ' + output_pdf_file + ' --output-csv-file ' + output_csv_file + 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 = 6 + pool = multiprocessing.Pool(num_processes) + # izip arguments so we can pass as one argument to pool worker + pool.map(run_graph_bag_helper, itertools.izip(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) diff --git a/tools/graph_bag/scripts/bag_sweep_main.py b/tools/graph_bag/scripts/bag_sweep_main.py new file mode 100755 index 0000000000..cb017e2c1c --- /dev/null +++ b/tools/graph_bag/scripts/bag_sweep_main.py @@ -0,0 +1,40 @@ +#!/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 bag_sweep + +import argparse + +import os +import sys + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('config_file') + parser.add_argument('output_dir') + 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.bag_sweep(args.config_file, args.output_dir) diff --git a/tools/graph_bag/scripts/check_bags_for_gaps.py b/tools/graph_bag/scripts/check_bags_for_gaps.py new file mode 100755 index 0000000000..fd09c41b74 --- /dev/null +++ b/tools/graph_bag/scripts/check_bags_for_gaps.py @@ -0,0 +1,38 @@ +#!/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. +import utilities + +import argparse +import os +import sys + +import rosbag + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('bagfile') + parser.add_argument('topic') + parser.add_argument('-m', '--max-time-diff', type=float, default=0.5) + # Use header or received time + parser.add_argument('-r', '--use-receive-time', action='store_true') + args = parser.parse_args() + if not os.path.isfile(args.bagfile): + print('Bag file ' + args.bagfile + ' does not exist.') + sys.exit() + + utilities.get_topic_rates(args.bagfile, args.topic, args.max_time_diff, not args.use_receive_time, True) diff --git a/tools/graph_bag/scripts/config_creator.py b/tools/graph_bag/scripts/config_creator.py new file mode 100644 index 0000000000..3d94412a63 --- /dev/null +++ b/tools/graph_bag/scripts/config_creator.py @@ -0,0 +1,50 @@ +#!/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. + + +def check_and_fill_line(value_map, config_file_line): + line_strings = config_file_line.split() + # Overwrite val if config variable is in value map + if len(line_strings) > 0 and line_strings[0] in value_map: + return line_strings[0] + ' = ' + str(value_map[line_strings[0]]) + '\n' + return config_file_line + + +def fill_in_values(original_config, value_map, new_config): + original_config_file = open(original_config, 'r') + new_config_file = open(new_config, 'w') + for config_file_line in original_config_file: + new_config_file.write(check_and_fill_line(value_map, config_file_line)) + + +def make_value_map(values, value_names): + value_map = {} + if len(values) != len(value_names): + print("values and value_names not same length!") + exit() + + for index, value_name in enumerate(value_names): + value_map[value_name] = values[index] + + return value_map + + +def make_config(values, value_names, original_config, new_config): + value_map = make_value_map(values, value_names) + fill_in_values(original_config, value_map, new_config) diff --git a/tools/graph_bag/scripts/imu_analyzer.py b/tools/graph_bag/scripts/imu_analyzer.py new file mode 100644 index 0000000000..5a7bf1324d --- /dev/null +++ b/tools/graph_bag/scripts/imu_analyzer.py @@ -0,0 +1,202 @@ +#!/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 imu_measurements +import plot_helpers + +import matplotlib +matplotlib.use('pdf') +import matplotlib.pyplot as plt +from matplotlib.backends.backend_pdf import PdfPages +import numpy as np +import scipy.signal + +import rosbag +import geometry_msgs + + +def get_fft(data, times, sample_spacing): + magnitudes = np.fft.rfft(data) + frequencies = np.fft.rfftfreq(len(times), sample_spacing) + return magnitudes, frequencies + + +def butter_lowpass_sos_representation(cutoff_frequency, sample_rate, order=5): + nyquist_rate = 0.5 * sample_rate + # From Python Cookbook + # TODO(rsoussan): Why is this necessary? + critical_frequency = cutoff_frequency / nyquist_rate + sos = scipy.signal.butter(order, critical_frequency, btype='low', output='sos', analog=False) + return sos + + +def lowpass_filter(data, cutoff_frequency, sample_rate, order=5): + sos = butter_lowpass_sos_representation(cutoff_frequency, sample_rate, order=order) + filtered_data = scipy.signal.sosfilt(sos, data) + return filtered_data + + +def filter_imu_measurements(imu_measurements, filtered_imu_measurements, cutoff_frequency, sample_rate): + filtered_imu_measurements.accelerations.xs = lowpass_filter(imu_measurements.accelerations.xs, cutoff_frequency, + sample_rate) + filtered_imu_measurements.accelerations.ys = lowpass_filter(imu_measurements.accelerations.ys, cutoff_frequency, + sample_rate) + filtered_imu_measurements.accelerations.zs = lowpass_filter(imu_measurements.accelerations.zs, cutoff_frequency, + sample_rate) + filtered_imu_measurements.angular_velocities.xs = lowpass_filter(imu_measurements.angular_velocities.xs, + cutoff_frequency, sample_rate) + filtered_imu_measurements.angular_velocities.ys = lowpass_filter(imu_measurements.angular_velocities.ys, + cutoff_frequency, sample_rate) + filtered_imu_measurements.angular_velocities.zs = lowpass_filter(imu_measurements.angular_velocities.zs, + cutoff_frequency, sample_rate) + filtered_imu_measurements.times = imu_measurements.times + + +def plot_imu_measurements(pdf, imu_measurements, prefix=''): + # Acceleration + plt.figure() + plot_helpers.plot_vector3ds(imu_measurements.accelerations, imu_measurements.times, 'Acc.') + plt.xlabel('Time (s)') + plt.ylabel('Acceleration (m/s^2)') + plt.title(prefix + 'Acceleration') + plt.legend(prop={'size': 6}) + pdf.savefig() + plt.close() + + # Angular Velocity + plt.figure() + plot_helpers.plot_vector3ds(imu_measurements.angular_velocities, imu_measurements.times, 'Ang. Vel.') + plt.xlabel('Time (s)') + plt.ylabel('Angular Velocities') + plt.title(prefix + 'Angular Velocities') + plt.legend(prop={'size': 6}) + pdf.savefig() + plt.close() + + +def plot_fft(pdf, magnitude, frequency, prefix=''): + plt.figure() + plt.ylim(top=1.5) + plt.plot(frequency, np.absolute(magnitude), lw=1) + plt.xlabel('Frequency') + plt.ylabel('Magnitude') + plt.title(prefix + ' FFT') + pdf.savefig() + plt.close() + + +def plot_filtered_data(pdf, filtered_data, data, filtered_times, times, title): + plt.figure() + plt.plot(filtered_times, filtered_data, 'r', alpha=0.5, lw=1, label='Filtered') + plt.plot(times, data, 'g', alpha=0.5, lw=1, label='Raw') + plt.xlabel('Time (s)') + plt.ylabel('Acceleration (m/s^2)') + plt.title(title) + plt.legend(prop={'size': 6}) + pdf.savefig() + plt.close() + + +def load_imu_msgs(imu_measurements, imu_topic, bag): + for topic, msg, t in bag.read_messages(imu_topic): + imu_measurements.add_imu_measurement(msg) + + +def create_plots(bagfile, filtered_bagfile, output_file, cutoff_frequency): + bag = rosbag.Bag(bagfile) + measurements = imu_measurements.ImuMeasurements() + load_imu_msgs(measurements, '/hw/imu', bag) + bag.close() + + # Assumes samples are evenly spaced in time + # This is a fairly safe assumption for imu measurements + sample_spacing = np.diff(measurements.times)[0] + sample_rate = 1.0 / sample_spacing + + # Use second bagfile as filtered measurements, otherwise filter with python filter + filtered_measurements = imu_measurements.ImuMeasurements() + if (filtered_bagfile): + filtered_bag = rosbag.Bag(filtered_bagfile) + load_imu_msgs(filtered_measurements, '/hw/imu', filtered_bag) + filtered_bag.close() + else: + filter_imu_measurements(measurements, filtered_measurements, cutoff_frequency, sample_rate) + + # FFTs + acceleration_x_fft_magnitudes, acceleration_x_fft_frequencies = get_fft(measurements.accelerations.xs, + measurements.times, sample_spacing) + acceleration_y_fft_magnitudes, acceleration_y_fft_frequencies = get_fft(measurements.accelerations.ys, + measurements.times, sample_spacing) + acceleration_z_fft_magnitudes, acceleration_z_fft_frequencies = get_fft(measurements.accelerations.zs, + measurements.times, sample_spacing) + angular_velocity_x_fft_magnitudes, angular_velocity_x_fft_frequencies = get_fft(measurements.angular_velocities.xs, + measurements.times, sample_spacing) + angular_velocity_y_fft_magnitudes, angular_velocity_y_fft_frequencies = get_fft(measurements.angular_velocities.ys, + measurements.times, sample_spacing) + angular_velocity_z_fft_magnitudes, angular_velocity_z_fft_frequencies = get_fft(measurements.angular_velocities.zs, + measurements.times, sample_spacing) + + filtered_acceleration_x_fft_magnitudes, filtered_acceleration_x_fft_frequencies = get_fft( + filtered_measurements.accelerations.xs, filtered_measurements.times, sample_spacing) + filtered_acceleration_y_fft_magnitudes, filtered_acceleration_y_fft_frequencies = get_fft( + filtered_measurements.accelerations.ys, filtered_measurements.times, sample_spacing) + filtered_acceleration_z_fft_magnitudes, filtered_acceleration_z_fft_frequencies = get_fft( + filtered_measurements.accelerations.zs, filtered_measurements.times, sample_spacing) + filtered_angular_velocity_x_fft_magnitudes, filtered_angular_velocity_x_fft_frequencies = get_fft( + filtered_measurements.angular_velocities.xs, filtered_measurements.times, sample_spacing) + filtered_angular_velocity_y_fft_magnitudes, filtered_angular_velocity_y_fft_frequencies = get_fft( + filtered_measurements.angular_velocities.ys, filtered_measurements.times, sample_spacing) + filtered_angular_velocity_z_fft_magnitudes, filtered_angular_velocity_z_fft_frequencies = get_fft( + filtered_measurements.angular_velocities.zs, filtered_measurements.times, sample_spacing) + + with PdfPages(output_file) as pdf: + plot_imu_measurements(pdf, measurements, 'Raw Imu ') + + plot_fft(pdf, acceleration_x_fft_magnitudes, acceleration_x_fft_frequencies, 'Raw Imu FFT Accel x ') + plot_fft(pdf, acceleration_y_fft_magnitudes, acceleration_y_fft_frequencies, 'Raw Imu FFT Accel y ') + plot_fft(pdf, acceleration_z_fft_magnitudes, acceleration_z_fft_frequencies, 'Raw Imu FFT Accel z ') + plot_fft(pdf, angular_velocity_x_fft_magnitudes, angular_velocity_x_fft_frequencies, 'Raw Imu FFT Ang Vel x ') + plot_fft(pdf, angular_velocity_y_fft_magnitudes, angular_velocity_y_fft_frequencies, 'Raw Imu FFT Ang Vel y ') + plot_fft(pdf, angular_velocity_z_fft_magnitudes, angular_velocity_z_fft_frequencies, 'Raw Imu FFT Ang Vel z ') + + plot_filtered_data(pdf, filtered_measurements.accelerations.xs, measurements.accelerations.xs, + filtered_measurements.times, measurements.times, 'Filtered Accel x') + plot_filtered_data(pdf, filtered_measurements.accelerations.ys, measurements.accelerations.ys, + filtered_measurements.times, measurements.times, 'Filtered Accel y') + plot_filtered_data(pdf, filtered_measurements.accelerations.zs, measurements.accelerations.zs, + filtered_measurements.times, measurements.times, 'Filtered Accel z') + plot_filtered_data(pdf, filtered_measurements.angular_velocities.xs, measurements.angular_velocities.xs, + filtered_measurements.times, measurements.times, 'Filtered Ang Vel x') + plot_filtered_data(pdf, filtered_measurements.angular_velocities.ys, measurements.angular_velocities.ys, + filtered_measurements.times, measurements.times, 'Filtered Ang Vel y') + plot_filtered_data(pdf, filtered_measurements.angular_velocities.zs, measurements.angular_velocities.zs, + filtered_measurements.times, measurements.times, 'Filtered Ang Vel z') + + plot_fft(pdf, filtered_acceleration_x_fft_magnitudes, filtered_acceleration_x_fft_frequencies, + 'Filtered Imu FFT Accel x ') + plot_fft(pdf, filtered_acceleration_y_fft_magnitudes, filtered_acceleration_y_fft_frequencies, + 'Filtered Imu FFT Accel y ') + plot_fft(pdf, filtered_acceleration_z_fft_magnitudes, filtered_acceleration_z_fft_frequencies, + 'Filtered Imu FFT Accel z ') + plot_fft(pdf, filtered_angular_velocity_x_fft_magnitudes, filtered_angular_velocity_x_fft_frequencies, + 'Filtered Imu FFT Ang Vel x ') + plot_fft(pdf, filtered_angular_velocity_y_fft_magnitudes, filtered_angular_velocity_y_fft_frequencies, + 'Filtered Imu FFT Ang Vel y ') + plot_fft(pdf, filtered_angular_velocity_z_fft_magnitudes, filtered_angular_velocity_z_fft_frequencies, + 'Filtered Imu FFT Ang Vel z ') diff --git a/tools/graph_bag/scripts/imu_analyzer_main.py b/tools/graph_bag/scripts/imu_analyzer_main.py new file mode 100755 index 0000000000..a40b7dbc0a --- /dev/null +++ b/tools/graph_bag/scripts/imu_analyzer_main.py @@ -0,0 +1,41 @@ +#!/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 imu_analyzer + +import argparse + +import os +import sys + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('bagfile') + parser.add_argument('--output-file', default='imu_analyzer_output.pdf') + parser.add_argument('-f', '--filtered-bagfile', default='') + # Only applicable if filtered_bagfile not provided, uses python filters + parser.add_argument('-c', '--cutoff-frequency', type=float, default=5.0) + args = parser.parse_args() + if not os.path.isfile(args.bagfile): + print('Bag file ' + args.bagfile + ' does not exist.') + sys.exit() + if args.filtered_bagfile and not os.path.isfile(args.filtered_bagfile): + print('Bag file ' + args.filtered_bagfile + ' does not exist.') + sys.exit() + imu_analyzer.create_plots(args.bagfile, args.filtered_bagfile, args.output_file, args.cutoff_frequency) diff --git a/tools/graph_bag/scripts/imu_measurements.py b/tools/graph_bag/scripts/imu_measurements.py new file mode 100644 index 0000000000..1d5dc04bf0 --- /dev/null +++ b/tools/graph_bag/scripts/imu_measurements.py @@ -0,0 +1,33 @@ +#!/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 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) diff --git a/tools/graph_bag/scripts/loc_states.py b/tools/graph_bag/scripts/loc_states.py new file mode 100644 index 0000000000..e4f73a7ddb --- /dev/null +++ b/tools/graph_bag/scripts/loc_states.py @@ -0,0 +1,57 @@ +#!/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.of_counts = [] + self.vl_counts = [] + 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): + self.add_pose(msg.pose, msg.header.stamp) + self.of_counts.append(msg.of_count) + self.vl_counts.append(msg.ml_count) + 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/graph_bag/scripts/merge_bags.py b/tools/graph_bag/scripts/merge_bags.py new file mode 100755 index 0000000000..3c304fde27 --- /dev/null +++ b/tools/graph_bag/scripts/merge_bags.py @@ -0,0 +1,65 @@ +#!/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. + +import argparse +import os +import re +import sys + +import rosbag + + +# https://stackoverflow.com/a/4836734 +def natural_sort(l): + convert = lambda text: int(text) if text.isdigit() else text.lower() + alphanum_key = lambda key: [convert(c) for c in re.split('([0-9]+)', key)] + return sorted(l, key=alphanum_key) + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('input_bag_prefix') + parser.add_argument('--merged-bag', default='') + 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.startswith(args.input_bag_prefix) and bag.endswith('.bag') + ] + if (len(bag_names) == 0): + print('No bag files found') + sys.exit() + else: + print('Found ' + str(len(bag_names)) + ' bag files.') + + merged_bag_name = '' + if not args.merged_bag: + merged_bag_name = 'merged_' + args.input_bag_prefix + '.bag' + + sorted_bag_names = natural_sort(bag_names) + + topics = [ + '/hw/imu', '/loc/of/features', '/loc/ml/features', '/loc/ar/features', '/mgt/img_sampler/nav_cam/image_record' + ] + + with rosbag.Bag(merged_bag_name, 'w') as merged_bag: + for sorted_bag_name in sorted_bag_names: + with rosbag.Bag(sorted_bag_name, 'r') as sorted_bag: + for topic, msg, t in sorted_bag.read_messages(topics): + merged_bag.write(topic, msg, t) diff --git a/tools/graph_bag/scripts/multiprocessing_helpers.py b/tools/graph_bag/scripts/multiprocessing_helpers.py new file mode 100644 index 0000000000..14d2486479 --- /dev/null +++ b/tools/graph_bag/scripts/multiprocessing_helpers.py @@ -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. + +# Forward errors so we can recover failures +# even when running commands through multiprocessing +# pooling +def full_traceback(func): + import traceback, functools + + @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 diff --git a/tools/graph_bag/scripts/orientations.py b/tools/graph_bag/scripts/orientations.py new file mode 100644 index 0000000000..23453194f2 --- /dev/null +++ b/tools/graph_bag/scripts/orientations.py @@ -0,0 +1,31 @@ +#!/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. + + +class Orientations: + + def __init__(self): + self.yaws = [] + self.pitches = [] + self.rolls = [] + + def add(self, yaw, pitch, roll): + self.yaws.append(yaw) + self.pitches.append(pitch) + self.rolls.append(roll) diff --git a/tools/graph_bag/scripts/parameter_sweep.py b/tools/graph_bag/scripts/parameter_sweep.py new file mode 100755 index 0000000000..113f7c1faf --- /dev/null +++ b/tools/graph_bag/scripts/parameter_sweep.py @@ -0,0 +1,170 @@ +#!/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 csv +import itertools +import math +import multiprocessing +import numpy as np +import os + +import average_results +import config_creator +import plot_parameter_sweep_results +import utilities + + +# Run graph localizer with values. +# Add traceback so errors are forwarded, otherwise +# some errors are suppressed due to the multiprocessing +# library call +@utilities.full_traceback +def test_values(values, job_id, value_names, output_dir, bag_file, map_file, image_topic, config_path, robot_config, + world, use_image_features): + 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_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) + 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 + '/' + + run_command = 'rosrun graph_bag run_graph_bag ' + bag_file + ' ' + map_file + ' ' + config_path + ' -o ' + output_bag + ' -s ' + output_stats_file + ' -r ' + robot_config + ' -w ' + world + ' -g ' + graph_config_prefix + ' -f ' + str( + use_image_features) + 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') + plot_command = 'rosrun graph_bag plot_results_main.py ' + output_bag + ' --output-file ' + output_pdf_file + ' --output-csv-file ' + output_csv_file + 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 test_values_helper(zipped_vals): + return test_values(*zipped_vals) + + +def concat_results(job_ids, directory): + results_csv_files = [] + for job_id in job_ids: + results_csv_files.append(os.path.join(directory, str(job_id), 'graph_stats.csv')) + # 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.to_csv(combined_results_file, index=False) + + +def parameter_sweep(all_value_combos, value_names, output_dir, bag_file, map_file, image_topic, config_path, + robot_config, world, use_image_features): + job_ids = list(range(len(all_value_combos))) + num_processes = 6 + pool = multiprocessing.Pool(num_processes) + # izip arguments so we can pass as one argument to pool worker + pool.map( + test_values_helper, + itertools.izip(all_value_combos, job_ids, itertools.repeat(value_names), itertools.repeat(output_dir), + 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))) + concat_results(job_ids, output_dir) + + +def make_all_value_combinations(value_ranges): + return list(itertools.product(*value_ranges)) + + +def make_value_ranges(): + value_ranges = [] + value_names = [] + steps = 6 + + # tune num smart factors + value_ranges.append(np.logspace(-1, -6, steps, endpoint=True)) + value_names.append('accel_sigma') + value_ranges.append(np.logspace(-1, -6, steps, endpoint=True)) + value_names.append('accel_bias_sigma') + value_ranges.append(np.logspace(-1, -6, steps, endpoint=True)) + value_names.append('integration_variance') + value_ranges.append(np.logspace(-1, -6, steps, endpoint=True)) + value_names.append('bias_acc_omega_int') + + #q_gyro + # .001 -> 2 deg + #q_gyro_degrees_range = np.logspace(-3, .3, steps, endpoint=True) + #q_gyro_squared_rads_range = [math.radians(deg)**2 for deg in q_gyro_degrees_range] + #value_ranges.append(q_gyro_squared_rads_range) + #value_names.append('q_gyro') + + return value_ranges, value_names + + +def save_values(value_names, values, filename, output_dir): + with open(os.path.join(output_dir, filename), 'w') as values_file: + writer = csv.writer(values_file) + writer.writerow(value_names) + writer.writerows(values) + + +def make_values_and_parameter_sweep(output_dir, bag_file, map_file, image_topic, config_path, robot_config, world, + use_image_features): + output_dir = utilities.create_directory(output_dir) + print('Output directory for results is {}'.format(output_dir)) + + value_ranges, value_names = make_value_ranges() + save_values(value_names, value_ranges, 'individual_value_ranges.csv', output_dir) + + all_value_combos = make_all_value_combinations(value_ranges) + save_values(value_names, all_value_combos, 'all_value_combos.csv', output_dir) + + parameter_sweep(all_value_combos, value_names, output_dir, bag_file, map_file, image_topic, config_path, robot_config, + world, use_image_features) + 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_plot(results_pdf_file, combined_results_file, value_combos_file) + return output_dir + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + 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('use_image_features', + type=bool, + help='Use image features msgs from bagfile or generate features from images.') + + parser.add_argument( + '--directory', + 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() + + 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) diff --git a/tools/graph_bag/scripts/plot_bag_sweep_results.py b/tools/graph_bag/scripts/plot_bag_sweep_results.py new file mode 100755 index 0000000000..ec624fd96f --- /dev/null +++ b/tools/graph_bag/scripts/plot_bag_sweep_results.py @@ -0,0 +1,130 @@ +#!/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 matplotlib.pyplot as plt +from matplotlib.backends.backend_pdf import PdfPages +import pandas as pd + +import os +import sys + + +def save_rmse_results_to_csv(rmses, rmses_2=None, label_1=None, label_2=None): + mean_rmses_dataframe = pd.DataFrame() + labels = [] + if label_1 and label_2: + 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_rmse_%'] = relative_rmses + mean_rmses_dataframe['rel_rmse_delta_%'] = relative_change_in_rmses + mean_rmses_dataframe['mean_rmse'] = mean_rmses_list + mean_rmses_csv_file = 'mean_rmses.csv' + mean_rmses_dataframe.to_csv(mean_rmses_csv_file, index=False) + return mean_rmses_list, labels, relative_rmses, relative_change_in_rmses + + +def create_plot(output_file, csv_file, label_1='', csv_file_2=None, label_2=''): + dataframe = pd.read_csv(csv_file) + dataframe.sort_values(by=['Bag'], inplace=True) + rmses = dataframe['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 = range(len(shortened_bag_names)) + rmses_2 = None + if (csv_file_2): + dataframe_2 = pd.read_csv(csv_file_2) + dataframe_2.sort_values(by=['Bag'], inplace=True) + rmses_2 = dataframe_2['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: + plt.figure() + plt.plot(x_axis_vals, rmses, 'b', label=label_1, linestyle='None', marker='o', markeredgewidth=0.1, markersize=10.5) + if (csv_file_2): + 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('RMSE') + plt.title('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() + + # Plot mean rmses + mean_rmses, labels, relative_rmses, relative_change_in_rmses = save_rmse_results_to_csv( + rmses, rmses_2, label_1, label_2) + mean_rmses_1_string = 'rmse: ' + str(mean_rmses[0]) + if labels: + mean_rmses_1_string += ', label: ' + labels[0] + plt.figure() + plt.axis('off') + plt.text(0.0, 0.5, mean_rmses_1_string) + if len(mean_rmses) > 1: + mean_rmses_2_string = 'rmse: ' + str(mean_rmses[1]) + if labels: + mean_rmses_2_string += ', label: ' + labels[1] + plt.text(0.0, 0.4, mean_rmses_2_string) + relative_rmses_string = 'rel rmse %: ' + str(relative_rmses[0]) + plt.text(0.0, 0.3, relative_rmses_string) + relative_rmses_change_string = 'rel change in rmse %: ' + str(relative_change_in_rmses[0]) + plt.text(0.0, 0.2, relative_rmses_change_string) + pdf.savefig() + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + # Combined csv results, where each row is the result from a bag file + parser.add_argument('csv_file') + parser.add_argument('--output-file', default='bag_sweep_results.pdf') + parser.add_argument('--csv-file2', help='Optional second csv file to plot') + parser.add_argument('--label1', default='', help='Optional label for first csv file') + parser.add_argument('--label2', default='', help='Optional label for second csv file') + args = parser.parse_args() + create_plot(args.output_file, args.csv_file, args.label1, args.csv_file2, args.label2) diff --git a/tools/graph_bag/scripts/plot_helpers.py b/tools/graph_bag/scripts/plot_helpers.py new file mode 100644 index 0000000000..d9cebed518 --- /dev/null +++ b/tools/graph_bag/scripts/plot_helpers.py @@ -0,0 +1,80 @@ +#!/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 + +import matplotlib +matplotlib.use('pdf') +import matplotlib.pyplot as plt + + +def plot_vals(x_axis_vals, + vec_of_y_axis_vals, + labels, + colors, + linewidth=1, + linestyle='-', + marker=None, + markeredgewidth=None, + markersize=1): + for index, _ in enumerate(vec_of_y_axis_vals): + plt.plot(x_axis_vals, + vec_of_y_axis_vals[index], + colors[index], + linestyle=linestyle, + linewidth=linewidth, + marker=marker, + markeredgewidth=markeredgewidth, + markersize=markersize, + label=labels[index]) + + +def plot_vector3ds(vector3ds, + times, + label, + colors=['r', 'g', 'b'], + linewidth=1, + linestyle='-', + marker=None, + markeredgewidth=None, + markersize=1): + labels = [label + ' (X)', label + ' (Y)', label + ' (Z)'] + plot_vals(times, [vector3ds.xs, vector3ds.ys, vector3ds.zs], labels, colors, linewidth, linestyle, marker, + markeredgewidth, markersize) + + +def plot_positions(poses, colors, linewidth=1, linestyle='-', marker=None, markeredgewidth=None, markersize=1): + plot_vector3ds(poses.positions, + poses.times, + poses.pose_type + ' Pos.', + linewidth=linewidth, + linestyle=linestyle, + marker=marker, + markeredgewidth=markeredgewidth, + markersize=markersize) + + +def plot_orientations(poses, colors, linewidth=1, linestyle='-', marker=None, markeredgewidth=None, markersize=1): + labels = [ + poses.pose_type + ' Orientation (Yaw)', poses.pose_type + ' Orientation (Roll)', + poses.pose_type + ' Orienation (Pitch)' + ] + plot_vals(poses.times, [poses.orientations.yaws, poses.orientations.rolls, poses.orientations.pitches], labels, + colors, linewidth, linestyle, marker, markeredgewidth, markersize) diff --git a/tools/graph_bag/scripts/plot_parameter_sweep_results.py b/tools/graph_bag/scripts/plot_parameter_sweep_results.py new file mode 100755 index 0000000000..669d6c4282 --- /dev/null +++ b/tools/graph_bag/scripts/plot_parameter_sweep_results.py @@ -0,0 +1,82 @@ +#!/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 matplotlib.pyplot as plt +from matplotlib.backends.backend_pdf import PdfPages +import pandas as pd + +import math +import os +import sys + + +def create_plot(output_file, csv_file, value_combos_file): + dataframe = pd.read_csv(csv_file) + rmses = dataframe['rmse'] + x_axis_vals = [] + x_axis_label = '' + if value_combos_file: + value_combos_dataframe = pd.read_csv(value_combos_file) + if (len(value_combos_dataframe.columns) > 1): + print('Value combos include more than one parameter, cannot use for x axis of plot') + exit() + x_axis_label = value_combos_dataframe.columns[0] + x_axis_vals = value_combos_dataframe[x_axis_label] + else: + job_count = dataframe.shape[0] + x_axis_vals = range(job_count) + x_axis_label = 'Job Id' + with PdfPages(output_file) as pdf: + plt.figure() + plt.plot(x_axis_vals, rmses, linestyle='None', marker='o', markeredgewidth=0.1, markersize=10.5) + plt.ylabel('RMSE') + plt.title('RMSE vs. ' + x_axis_label) + x_range = x_axis_vals[len(x_axis_vals) - 1] - x_axis_vals[0] + # Use log scale if min and max x vals are more than 3 orders of magnitude apart + if (abs(math.log10(x_axis_vals[len(x_axis_vals) - 1]) - math.log10(x_axis_vals[0])) > 3): + plt.xscale('log', base=10) + # Extend x axis on either side using a log scale to make data more visible + first_val = x_axis_vals[0] + last_val = x_axis_vals[len(x_axis_vals) - 1] + if (first_val < last_val): + plt.xlim([first_val * 0.1, last_val * 10.0]) + else: + plt.xlim([last_val * 0.1, first_val * 10.0]) + else: + # Extend x axis on either side to make data more visible + x_buffer = x_range * 0.1 + plt.xlim([x_axis_vals[0] - x_buffer, x_axis_vals[len(x_axis_vals) - 1] + x_buffer]) + plt.ticklabel_format(useOffset=False) + plt.tight_layout() + plt.xlabel(x_axis_label) + pdf.savefig() + plt.close() + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + # Csv file with combined results (or mean results if using bag_and_param_sweep) for each job + parser.add_argument('csv_file') + parser.add_argument('--output-file', default='param_sweep_results.pdf') + parser.add_argument('--value-combos-file') + args = parser.parse_args() + create_plot(args.output_file, args.csv_file, args.value_combos_file) diff --git a/tools/graph_bag/scripts/plot_results.py b/tools/graph_bag/scripts/plot_results.py new file mode 100644 index 0000000000..8485a40b20 --- /dev/null +++ b/tools/graph_bag/scripts/plot_results.py @@ -0,0 +1,412 @@ +#!/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 loc_states +import plot_helpers +import poses +import rmse_utilities + +import matplotlib +matplotlib.use('pdf') +import matplotlib.pyplot as plt +from matplotlib.backends.backend_pdf import PdfPages + +import geometry_msgs +import math +import rosbag + +import csv + +def l2_map(vector3ds): + return map(lambda (x, y, z): math.sqrt(x + y + z), zip(vector3ds.xs, vector3ds.ys, vector3ds.zs)) + + +def add_pose_plots(pdf, sparse_mapping_poses, ar_tag_poses, graph_localization_poses, imu_augmented_graph_localization_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) + if ar_tag_poses.times: + plot_helpers.plot_positions(ar_tag_poses, + colors, + linestyle='None', + marker='x', + markeredgewidth=0.1, + markersize=1.5) + plot_helpers.plot_positions(graph_localization_poses, colors, linewidth=0.5) + plt.xlabel('Time (s)') + plt.ylabel('Position (m)') + plt.title('Graph 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) + if ar_tag_poses.times: + plot_helpers.plot_orientations(ar_tag_poses, + colors, + linestyle='None', + marker='x', + markeredgewidth=0.1, + markersize=1.5) + plot_helpers.plot_orientations(graph_localization_poses, colors, linewidth=0.5) + plt.xlabel('Time (s)') + plt.ylabel('Orienation (deg)') + plt.title('Graph vs. Sparse Mapping Orientation') + plt.legend(prop={'size': 6}) + pdf.savefig() + plt.close() + + # Imu Augmented Loc vs. Loc + plt.figure() + plot_helpers.plot_positions(graph_localization_poses, colors, linestyle='None', marker='o', markeredgewidth=0.1, markersize=1.5) + plot_helpers.plot_positions(imu_augmented_graph_localization_poses, colors, linewidth=0.5) + plt.xlabel('Time (s)') + plt.ylabel('Position (m)') + plt.title('Graph vs. Imu Augmented Graph Position') + plt.legend(prop={'size': 6}) + pdf.savefig() + plt.close() + + # orientations + plt.figure() + plot_helpers.plot_orientations(graph_localization_poses, colors, marker='o', markeredgewidth=0.1, markersize=1.5) + plot_helpers.plot_orientations(imu_augmented_graph_localization_poses, colors, linewidth=0.5) + plt.xlabel('Time (s)') + plt.ylabel('Orienation (deg)') + plt.title('Graph vs. Imu Augmented Graph Orientation') + 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.of_counts, + graph_localization_states.times, + 'OF', + 'r', + marker='x', + markeredgewidth=0.1, + markersize=1.5) + plot_features(graph_localization_states.vl_counts, + graph_localization_states.times, + 'VL', + 'b', + marker='o', + markeredgewidth=0.1, + markersize=1.5) + plt.xlabel('Time (s)') + plt.ylabel('Feature Counts (num features in graph)') + plt.title('Feature Counts') + plt.legend(prop={'size': 6}) + pdf.savefig() + plt.close() + + +def add_other_vector3d_plots(pdf, imu_augmented_graph_localization_states): + # 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('Velocities') + 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_stats(pdf, graph_localization_states, sparse_mapping_poses, output_csv_file): + stats = '' + rmse = rmse_utilities.rmse_timestamped_poses(graph_localization_states, sparse_mapping_poses) + stats += 'rmse: ' + str(rmse) + with open(output_csv_file, 'a') as output_csv: + csv_writer = csv.writer(output_csv, lineterminator='\n') + csv_writer.writerow(['rmse', str(rmse)]) + 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_other_loc_plots(pdf, graph_localization_states, imu_augmented_graph_localization_states): + add_feature_count_plots(pdf, graph_localization_states) + add_other_vector3d_plots(pdf, imu_augmented_graph_localization_states) + + +def load_pose_msgs(vec_of_poses, bag): + 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) + break + + +def load_loc_state_msgs(vec_of_loc_states, bag): + 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) + break + + +def has_topic(bag, topic): + topics = bag.get_type_and_topic_info().topics + return topic in topics + + +def create_plots(bagfile, output_pdf_file, output_csv_file='results.csv'): + bag = rosbag.Bag(bagfile) + + 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 = [sparse_mapping_poses, ar_tag_poses, imu_bias_tester_poses] + load_pose_msgs(vec_of_poses, bag) + + 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.close() + + with PdfPages(output_pdf_file) as pdf: + add_pose_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) + if has_imu_augmented_graph_localization_state: + add_other_loc_plots(pdf, graph_localization_states, imu_augmented_graph_localization_states) + else: + add_other_loc_plots(pdf, graph_localization_states, graph_localization_states) + plot_stats(pdf, graph_localization_states, sparse_mapping_poses, output_csv_file) diff --git a/tools/graph_bag/scripts/plot_results_main.py b/tools/graph_bag/scripts/plot_results_main.py new file mode 100755 index 0000000000..b9478899ab --- /dev/null +++ b/tools/graph_bag/scripts/plot_results_main.py @@ -0,0 +1,36 @@ +#!/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 plot_results + +import argparse + +import os +import sys + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('bagfile') + parser.add_argument('--output-file', default='output.pdf') + parser.add_argument('--output-csv-file', default='results.csv') + args = parser.parse_args() + if not os.path.isfile(args.bagfile): + print('Bag file ' + args.bagfile + ' does not exist.') + sys.exit() + plot_results.create_plots(args.bagfile, args.output_file, args.output_csv_file) diff --git a/tools/graph_bag/scripts/poses.py b/tools/graph_bag/scripts/poses.py new file mode 100644 index 0000000000..b899e7faef --- /dev/null +++ b/tools/graph_bag/scripts/poses.py @@ -0,0 +1,47 @@ +#!/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 +import orientations + +import scipy.spatial.transform + + +class Poses(object): + + def __init__(self, pose_type, topic): + self.positions = vector3ds.Vector3ds() + self.orientations = orientations.Orientations() + self.times = [] + self.pose_type = pose_type + self.topic = topic + + def add_pose(self, pose_msg, timestamp): + self.positions.add(pose_msg.position.x, pose_msg.position.y, pose_msg.position.z) + euler_angles = scipy.spatial.transform.Rotation.from_quat( + [pose_msg.orientation.x, pose_msg.orientation.y, pose_msg.orientation.z, + pose_msg.orientation.w]).as_euler('ZYX', degrees=True) + self.orientations.add(euler_angles[0], euler_angles[1], euler_angles[2]) + self.times.append(timestamp.secs + 1e-9 * timestamp.nsecs) + + def add_msg(self, msg, timestamp): + self.add_pose(msg.pose, timestamp) + + def position_vector(self, index): + return [self.positions.xs[index], self.positions.ys[index], self.positions.zs[index]] diff --git a/tools/graph_bag/scripts/rmse_utilities.py b/tools/graph_bag/scripts/rmse_utilities.py new file mode 100644 index 0000000000..fa22fa4e6c --- /dev/null +++ b/tools/graph_bag/scripts/rmse_utilities.py @@ -0,0 +1,72 @@ +#!/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 numpy as np + +import math + +# Assumes poses_a and poses_b are sorted in time +# TODO(rsoussan): Add copying of orientations +def get_same_timestamp_poses(poses_a, poses_b): + 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] + + if (a_time == b_time): + trimmed_poses_a.positions.add_vector3d(poses_a.positions.get_vector3d(a_index)) + # trimmed_poses_a.orientations.add_vector3d(poses_a.orientations.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.orientations.add_vector3d(poses_b.orientations.get_vector3d(b_index)) + trimmed_poses_b.times.append(poses_b.times[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 + + +# RMSE between two sequences of poses (position only). Only uses poses with the same timestamp +def rmse_timestamped_poses(poses_a, poses_b): + trimmed_poses_a, trimmed_poses_b = get_same_timestamp_poses(poses_a, poses_b) + assert len(trimmed_poses_a.times) == len(trimmed_poses_b.times), 'Length mismatch of poses' + num_poses = len(trimmed_poses_a.times) + mean_squared_error = 0 + for index in range(num_poses): + a_vec = trimmed_poses_a.positions.get_numpy_vector(index) + b_vec = trimmed_poses_b.positions.get_numpy_vector(index) + difference_vec = np.subtract(a_vec, b_vec) + squared_error = np.inner(difference_vec, difference_vec) + # Use rolling mean to avoid overflow + mean_squared_error += (squared_error - mean_squared_error)/(index + 1) + rmse = math.sqrt(mean_squared_error) + return rmse + + diff --git a/tools/graph_bag/scripts/test_rmse_utilities.py b/tools/graph_bag/scripts/test_rmse_utilities.py new file mode 100644 index 0000000000..f116d7b08d --- /dev/null +++ b/tools/graph_bag/scripts/test_rmse_utilities.py @@ -0,0 +1,172 @@ +#!/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 rmse_utilities + +import math +import numpy as np +import unittest + +def make_poses(times, xs, ys, zs): + new_poses = poses.Poses('', '') + new_poses.times = times + new_poses.positions.xs = xs + new_poses.positions.ys = ys + new_poses.positions.zs = zs + return new_poses + +class TestRMSESequence(unittest.TestCase): + + def test_prune_missing_timestamps_beginning_set(self): + a_times = np.arange(10.0) + xs = np.arange(10.0) + ys = np.arange(10.0) + 1.0 + zs = np.arange(10.0) + 2.0 + b_times = np.arange(5.0) + poses_a = make_poses(a_times, xs, ys, zs) + poses_b = make_poses(b_times, xs, ys, zs) + + trimmed_a, trimmed_b = rmse_utilities.get_same_timestamp_poses(poses_a, poses_b) + self.assertEqual(len(trimmed_a.times), len(trimmed_b.times)) + self.assertEqual(len(trimmed_a.times), 5) + self.assertTrue(np.allclose(trimmed_a.times, b_times, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.positions.xs, b_times, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.positions.ys, b_times + 1, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.positions.zs, b_times + 2, rtol=0)) + + def test_prune_missing_timestamps_middle_set(self): + a_times = np.arange(10.0) + xs = np.arange(10.0) + ys = np.arange(10.0) + 1.0 + zs = np.arange(10.0) + 2.0 + b_times = np.arange(3.0, 7.0) + poses_a = make_poses(a_times, xs, ys, zs) + poses_b = make_poses(b_times, xs, ys, zs) + + trimmed_a, trimmed_b = rmse_utilities.get_same_timestamp_poses(poses_a, poses_b) + self.assertEqual(len(trimmed_a.times), len(trimmed_b.times)) + self.assertEqual(len(trimmed_a.times), 4) + self.assertTrue(np.allclose(trimmed_a.times, b_times, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.positions.xs, b_times, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.positions.ys, b_times + 1, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.positions.zs, b_times + 2, rtol=0)) + + + def test_prune_missing_timestamps_end_set(self): + a_times = np.arange(10.0) + xs = np.arange(10.0) + ys = np.arange(10.0) + 1.0 + zs = np.arange(10.0) + 2.0 + b_times = np.arange(7.0, 10.0) + poses_a = make_poses(a_times, xs, ys, zs) + poses_b = make_poses(b_times, xs, ys, zs) + + trimmed_a, trimmed_b = rmse_utilities.get_same_timestamp_poses(poses_a, poses_b) + self.assertEqual(len(trimmed_a.times), len(trimmed_b.times)) + self.assertEqual(len(trimmed_a.times), 3) + self.assertTrue(np.allclose(trimmed_a.times, b_times, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.positions.xs, b_times, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.positions.ys, b_times + 1, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.positions.zs, b_times + 2, rtol=0)) + + def test_prune_missing_timestamps_scattered_set(self): + a_times = np.arange(10.0) + xs = np.arange(10.0) + ys = np.arange(10.0) + 1.0 + zs = np.arange(10.0) + 2.0 + b_times = np.array([1.0, 5.0, 6.0, 9.0]) + poses_a = make_poses(a_times, xs, ys, zs) + poses_b = make_poses(b_times, xs, ys, zs) + + trimmed_a, trimmed_b = rmse_utilities.get_same_timestamp_poses(poses_a, poses_b) + self.assertEqual(len(trimmed_a.times), len(trimmed_b.times)) + self.assertTrue(np.allclose(trimmed_a.times, b_times, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.positions.xs, b_times, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.positions.ys, b_times + 1, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.positions.zs, b_times + 2, rtol=0)) + + def test_prune_missing_timestamps_disjoint_set(self): + a_times = np.arange(10.0) + xs = np.arange(10.0) + ys = np.arange(10.0) + 1.0 + zs = np.arange(10.0) + 2.0 + b_times = np.arange(11, 20) + poses_a = make_poses(a_times, xs, ys, zs) + poses_b = make_poses(b_times, xs, ys, zs) + + trimmed_a, trimmed_b = rmse_utilities.get_same_timestamp_poses(poses_a, poses_b) + self.assertEqual(len(trimmed_a.times), 0) + self.assertEqual(len(trimmed_b.times), 0) + + + def test_prune_missing_timestamps_some_overlap(self): + a_times = np.arange(10.0) + xs = np.arange(10.0) + ys = np.arange(10.0) + 1.0 + zs = np.arange(10.0) + 2.0 + b_times = np.arange(8.0, 20.0) + poses_a = make_poses(a_times, xs, ys, zs) + poses_b = make_poses(b_times, xs, ys, zs) + + expected_time_range = np.arange(8.0, 10.0) + trimmed_a, trimmed_b = rmse_utilities.get_same_timestamp_poses(poses_a, poses_b) + self.assertEqual(len(trimmed_a.times), len(trimmed_b.times)) + self.assertTrue(np.allclose(trimmed_a.times, trimmed_b.times, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.times, expected_time_range, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.positions.xs, expected_time_range, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.positions.ys, expected_time_range + 1, rtol=0)) + self.assertTrue(np.allclose(trimmed_a.positions.zs, expected_time_range + 2, rtol=0)) + + + def test_rmse_same_poses(self): + a_times = np.arange(10.0) + xs = np.arange(10.0) + ys = np.arange(10.0) + 1.0 + zs = np.arange(10.0) + 2.0 + poses_a = make_poses(a_times, xs, ys, zs) + poses_b = make_poses(a_times, xs, ys, zs) + + rmse = rmse_utilities.rmse_timestamped_poses(poses_a, poses_b) + self.assertTrue(np.isclose(rmse, 0, rtol=0)) + + def test_rmse_off_by_one(self): + a_times = np.arange(10.0) + xs = np.arange(10.0) + ys = np.arange(10.0) + 1.0 + zs = np.arange(10.0) + 2.0 + poses_a = make_poses(a_times, xs, ys, zs) + poses_b = make_poses(a_times, xs + 1, ys, zs) + + rmse = rmse_utilities.rmse_timestamped_poses(poses_a, poses_b) + self.assertTrue(np.isclose(rmse, 1.0, rtol=0)) + + def test_rmse_all_off_by_one(self): + a_times = np.arange(10.0) + xs = np.arange(10.0) + ys = np.arange(10.0) + 1.0 + zs = np.arange(10.0) + 2.0 + poses_a = make_poses(a_times, xs, ys, zs) + poses_b = make_poses(a_times, xs + 1, ys + 1, zs + 1) + + rmse = rmse_utilities.rmse_timestamped_poses(poses_a, poses_b) + self.assertTrue(np.isclose(rmse, math.sqrt(3.0), rtol=0)) + +if __name__ == '__main__': + unittest.main() diff --git a/tools/graph_bag/scripts/utilities.py b/tools/graph_bag/scripts/utilities.py new file mode 100644 index 0000000000..a0d11938a3 --- /dev/null +++ b/tools/graph_bag/scripts/utilities.py @@ -0,0 +1,108 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# 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 datetime +import glob +import numpy as np +import os +import pandas as pd + +import rosbag + + +# Forward errors so we can recover failures +# even when running commands through multiprocessing +# pooling +def full_traceback(func): + import traceback, functools + + @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 + + +def get_files(directory, file_string): + return glob.glob(os.path.join(directory, file_string)) + + +def get_files_recursive(directory, file_string): + subdirectory_csv_files = [] + _, subdirectories, _ = os.walk(directory).next() + for subdirectory in subdirectories: + subdirectory_path = os.path.join(directory, subdirectory) + for subdirectory_csv_file in get_files(subdirectory_path, file_string): + subdirectory_csv_files.append(subdirectory_csv_file) + return subdirectory_csv_files + + +def create_directory(directory=None): + if directory == None: + directory = os.path.join(os.getcwd(), datetime.datetime.now().strftime('%Y-%m-%d_%H-%M-%S')) + if os.path.exists(directory): + print(directory + " already exists!") + exit() + os.makedirs(directory) + return directory + + +def load_dataframe(files): + dataframes = [pd.read_csv(file) for file in files] + dataframe = pd.concat(dataframes) + return dataframe + + +def get_topic_rates(bag_name, + topic, + min_time_diff_for_gap, + use_header_time=True, + verbose=False, + ignore_zero_time_diffs=True): + with rosbag.Bag(bag_name, 'r') as bag: + last_time = 0.0 + gaps = 0 + time_diffs = [] + for _, msg, t in bag.read_messages([topic]): + time = msg.header.stamp.secs + msg.header.stamp.nsecs * 1.0e-9 if use_header_time else t.secs + t.nsecs * 1.0e-9 + time_diff = time - last_time + if last_time != 0 and time_diff >= min_time_diff_for_gap: + if verbose: + print(topic + ' Gap: time: ' + str(time) + ', last_time: ' + str(last_time) + ', diff: ' + str(time_diff)) + gaps += 1 + if (last_time != 0 and (time_diff != 0 or not ignore_zero_time_diffs)): + time_diffs.append(time_diff) + last_time = time + + mean_time_diff = np.mean(time_diffs) + min_time_diff = np.min(time_diffs) + max_time_diff = np.max(time_diffs) + stddev_time_diff = np.std(time_diffs) + if verbose: + if use_header_time: + print('Using Header time.') + else: + print('Using Receive time.') + print('Found ' + str(gaps) + ' time diffs >= ' + str(min_time_diff_for_gap) + ' secs.') + print('Mean time diff: ' + str(mean_time_diff)) + print('Min time diff: ' + str(min_time_diff)) + print('Max time diff: ' + str(max_time_diff)) + print('Stddev time diff: ' + str(stddev_time_diff)) diff --git a/tools/graph_bag/scripts/vector3d.py b/tools/graph_bag/scripts/vector3d.py new file mode 100644 index 0000000000..90ef41495d --- /dev/null +++ b/tools/graph_bag/scripts/vector3d.py @@ -0,0 +1,25 @@ +#!/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. + +class Vector3d: + + def __init__(self, x, y, z): + self.x = x + self.y = y + self.z = z diff --git a/tools/graph_bag/scripts/vector3ds.py b/tools/graph_bag/scripts/vector3ds.py new file mode 100644 index 0000000000..411f25c7bc --- /dev/null +++ b/tools/graph_bag/scripts/vector3ds.py @@ -0,0 +1,45 @@ +#!/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 vector3d + +import numpy as np + +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/graph_bag/src/bag_imu_filterer.cc b/tools/graph_bag/src/bag_imu_filterer.cc new file mode 100644 index 0000000000..75ac0b793b --- /dev/null +++ b/tools/graph_bag/src/bag_imu_filterer.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 +#include +#include +#include +#include +#include + +#include + +namespace { +// TODO(rsoussan): Unify this with live measurement simulator, put in utilities +bool string_ends_with(const std::string& str, const std::string& ending) { + if (str.length() >= ending.length()) { + return (0 == str.compare(str.length() - ending.length(), ending.length(), ending)); + } else { + return false; + } +} +} // namespace + +namespace graph_bag { +namespace ii = imu_integration; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace mc = msg_conversions; + +BagImuFilterer::BagImuFilterer(const std::string& bag_name, const std::string& filtered_bag, + const std::string& filter_name) + : bag_(bag_name, rosbag::bagmode::Read), filtered_bag_(filtered_bag, rosbag::bagmode::Write) { + ii::ImuFilterParams params; + params.type = filter_name; + imu_filter_.reset(new ii::ImuFilter(params)); +} + +void BagImuFilterer::Convert() { + rosbag::View view(bag_); + for (const rosbag::MessageInstance m : view) { + // Convert imu message to filtered imu message + if (string_ends_with(m.getTopic(), TOPIC_HARDWARE_IMU)) { + sensor_msgs::ImuConstPtr imu_msg = m.instantiate(); + const lm::ImuMeasurement imu_measurement(*imu_msg); + const auto filtered_imu_measurement = imu_filter_->AddMeasurement(imu_measurement); + if (filtered_imu_measurement) { + sensor_msgs::Imu filtered_imu_msg; + mc::VectorToMsg(filtered_imu_measurement->acceleration, filtered_imu_msg.linear_acceleration); + mc::VectorToMsg(filtered_imu_measurement->angular_velocity, filtered_imu_msg.angular_velocity); + lc::TimeToHeader(filtered_imu_measurement->timestamp, filtered_imu_msg.header); + // TODO(rsoussan): Change receive timestamp to account for filter delay? + filtered_bag_.write(m.getTopic(), m.getTime(), filtered_imu_msg); + } + } else { // Don't change other msgs, write to filtered_bag + filtered_bag_.write(m.getTopic(), m.getTime(), m); + } + } +} +} // namespace graph_bag diff --git a/tools/graph_bag/src/graph_bag.cc b/tools/graph_bag/src/graph_bag.cc new file mode 100644 index 0000000000..041e88806b --- /dev/null +++ b/tools/graph_bag/src/graph_bag.cc @@ -0,0 +1,187 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +namespace graph_bag { +namespace gl = graph_localizer; +namespace lc = localization_common; + +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 graph_localizer::FeatureTrackMap& feature_tracks) { + const auto feature_track_image_msg = CreateFeatureTrackImage(image_msg, feature_tracks, *params_.nav_cam_params); + if (!feature_track_image_msg) return; + SaveMsg(*image_msg, kFeatureTracksImageTopic_); +} + +void GraphBag::SaveImuBiasTesterPredictedStates( + const std::vector& imu_bias_tester_predicted_states) { + for (const auto& state : imu_bias_tester_predicted_states) { + geometry_msgs::PoseStamped pose_msg; + lc::PoseToMsg(state.pose(), pose_msg.pose); + lc::TimeToHeader(state.timestamp(), pose_msg.header); + SaveMsg(pose_msg, TOPIC_IMU_BIAS_TESTER_POSE); + } +} + +void GraphBag::Run() { + // Required to start bias estimation + graph_localizer_simulator_->ResetBiasesAndLocalizer(); + lc::Timer graph_bag_timer("Graph Bag Timer"); + graph_bag_timer.Start(); + while (live_measurement_simulator_->ProcessMessage()) { + const lc::Time current_time = live_measurement_simulator_->CurrentTime(); + 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); + } + } + 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_->feature_tracks())); + } + } + const auto vl_msg = live_measurement_simulator_->GetVLMessage(current_time); + if (vl_msg) { + graph_localizer_simulator_->BufferVLVisualLandmarksMsg(*vl_msg); + if (gl::ValidVLMsg(*vl_msg, params_.sparse_mapping_min_num_landmarks)) { + const gtsam::Pose3 sparse_mapping_global_T_body = lc::GtPose(*vl_msg, 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); + } + } + 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); + 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); + const auto imu_bias_tester_predicted_states = + imu_bias_tester_wrapper_.LocalizationStateCallback(*localization_msg); + SaveImuBiasTesterPredictedStates(imu_bias_tester_predicted_states); + } + } + } + graph_bag_timer.Stop(); + graph_bag_timer.Log(); + const auto graph_stats = graph_localizer_simulator_->graph_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 graph_bag diff --git a/tools/graph_bag/src/graph_localizer_simulator.cc b/tools/graph_bag/src/graph_localizer_simulator.cc new file mode 100644 index 0000000000..e3e6d65bb1 --- /dev/null +++ b/tools/graph_bag/src/graph_localizer_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 + +namespace graph_bag { +namespace lc = localization_common; +GraphLocalizerSimulator::GraphLocalizerSimulator(const GraphLocalizerSimulatorParams& params, + const std::string& graph_config_path_prefix) + : GraphLocalizerWrapper(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::BufferVLVisualLandmarksMsg(const ff_msgs::VisualLandmarks& visual_landmarks_msg) { + vl_msg_buffer_.emplace_back(visual_landmarks_msg); +} + +void GraphLocalizerSimulator::BufferARVisualLandmarksMsg(const ff_msgs::VisualLandmarks& visual_landmarks_msg) { + ar_msg_buffer_.emplace_back(visual_landmarks_msg); +} + +void GraphLocalizerSimulator::BufferImuMsg(const sensor_msgs::Imu& imu_msg) { imu_msg_buffer_.emplace_back(imu_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 + // optimization delay. + if (Initialized() && last_update_time_ && (current_time - *last_update_time_) < params_.optimization_time) { + return false; + } + + // Add measurements + for (const auto& imu_msg : imu_msg_buffer_) { + ImuCallback(imu_msg); + } + imu_msg_buffer_.clear(); + + for (const auto& of_msg : of_msg_buffer_) { + OpticalFlowCallback(of_msg); + } + of_msg_buffer_.clear(); + + for (const auto& vl_msg : vl_msg_buffer_) { + VLVisualLandmarksCallback(vl_msg); + } + vl_msg_buffer_.clear(); + + for (const auto& ar_msg : ar_msg_buffer_) { + ARVisualLandmarksCallback(ar_msg); + } + ar_msg_buffer_.clear(); + + Update(); + last_update_time_ = current_time; + return true; +} +} // namespace graph_bag diff --git a/tools/graph_bag/src/live_measurement_simulator.cc b/tools/graph_bag/src/live_measurement_simulator.cc new file mode 100644 index 0000000000..2b32151d67 --- /dev/null +++ b/tools/graph_bag/src/live_measurement_simulator.cc @@ -0,0 +1,167 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include + +#include + +#include + +namespace graph_bag { +namespace lc = localization_common; +LiveMeasurementSimulator::LiveMeasurementSimulator(const LiveMeasurementSimulatorParams& params) + : bag_(params.bag_name, rosbag::bagmode::Read), + map_(params.map_file, true), + map_feature_matcher_(&map_), + params_(params), + kImageTopic_(params.image_topic), + imu_buffer_(params.imu), + of_buffer_(params.of), + vl_buffer_(params.vl), + ar_buffer_(params.ar) { + config_reader::ConfigReader config; + config.AddFile("cameras.config"); + config.AddFile("geometry.config"); + config.AddFile("localization.config"); + config.AddFile("optical_flow.config"); + + if (!config.ReadFiles()) { + LogError("Failed to read config files."); + exit(0); + } + + map_feature_matcher_.ReadParams(&config); + optical_flow_tracker_.ReadParams(&config); + std::vector topics; + topics.push_back(std::string("/") + TOPIC_HARDWARE_IMU); + topics.push_back(TOPIC_HARDWARE_IMU); + if (params_.use_image_features) { + 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); + topics.push_back(TOPIC_LOCALIZATION_ML_FEATURES); + } else { + topics.push_back(std::string("/") + kImageTopic_); + topics.push_back(kImageTopic_); + } + // Only use recorded ar features + topics.push_back(std::string("/") + TOPIC_LOCALIZATION_AR_FEATURES); + topics.push_back(TOPIC_LOCALIZATION_AR_FEATURES); + + view_.reset(new rosbag::View(bag_, rosbag::TopicQuery(topics))); + current_time_ = lc::TimeFromRosTime(view_->getBeginTime()); +} + +bool LiveMeasurementSimulator::string_ends_with(const std::string& str, const std::string& ending) { + if (str.length() >= ending.length()) { + return (0 == str.compare(str.length() - ending.length(), ending.length(), ending)); + } else { + return false; + } +} + +ff_msgs::Feature2dArray LiveMeasurementSimulator::GenerateOFFeatures(const sensor_msgs::ImageConstPtr& image_msg) { + ff_msgs::Feature2dArray of_features; + optical_flow_tracker_.OpticalFlow(image_msg, &of_features); + return of_features; +} + +bool LiveMeasurementSimulator::GenerateVLFeatures(const sensor_msgs::ImageConstPtr& image_msg, + ff_msgs::VisualLandmarks& vl_features) { + // Convert image to cv image + cv_bridge::CvImageConstPtr image; + try { + image = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::MONO8); + } catch (cv_bridge::Exception& e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return false; + } + + if (!map_feature_matcher_.Localize(image, &vl_features)) return false; + return true; +} + +bool LiveMeasurementSimulator::ProcessMessage() { + if (!view_it_) + view_it_ = view_->begin(); + else + ++(*view_it_); + 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_HARDWARE_IMU)) { + sensor_msgs::ImuConstPtr imu_msg = msg.instantiate(); + imu_buffer_.BufferMessage(*imu_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(); + ar_buffer_.BufferMessage(*ar_features); + } else if (params_.use_image_features) { + if (string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_OF_FEATURES)) { + const ff_msgs::Feature2dArrayConstPtr of_features = msg.instantiate(); + of_buffer_.BufferMessage(*of_features); + } else if (string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_ML_FEATURES)) { + const ff_msgs::VisualLandmarksConstPtr vl_features = msg.instantiate(); + vl_buffer_.BufferMessage(*vl_features); + } + } else if (string_ends_with(msg.getTopic(), kImageTopic_)) { + sensor_msgs::ImageConstPtr image_msg = msg.instantiate(); + if (params_.save_optical_flow_images) { + img_buffer_.emplace(localization_common::TimeFromHeader(image_msg->header), image_msg); + } + if (!params_.use_image_features) { + const ff_msgs::Feature2dArray of_features = GenerateOFFeatures(image_msg); + of_buffer_.BufferMessage(of_features); + + ff_msgs::VisualLandmarks vl_features; + if (GenerateVLFeatures(image_msg, vl_features)) { + vl_buffer_.BufferMessage(vl_features); + } + } + } else { + return true; + } + return true; +} + +lc::Time LiveMeasurementSimulator::CurrentTime() { return current_time_; } + +boost::optional LiveMeasurementSimulator::GetImuMessage(const lc::Time current_time) { + return imu_buffer_.GetMessage(current_time); +} +boost::optional LiveMeasurementSimulator::GetOFMessage(const lc::Time current_time) { + return of_buffer_.GetMessage(current_time); +} +boost::optional LiveMeasurementSimulator::GetVLMessage(const lc::Time current_time) { + return vl_buffer_.GetMessage(current_time); +} +boost::optional LiveMeasurementSimulator::GetARMessage(const lc::Time current_time) { + return ar_buffer_.GetMessage(current_time); +} +boost::optional LiveMeasurementSimulator::GetImageMessage(const lc::Time current_time) { + const auto img_it = img_buffer_.find(current_time); + if (img_it == img_buffer_.end()) return boost::none; + const auto current_img = img_it->second; + // Clear buffer up to current time + img_buffer_.erase(img_buffer_.begin(), img_it); + + return current_img; +} +} // namespace graph_bag diff --git a/tools/graph_bag/src/parameter_reader.cc b/tools/graph_bag/src/parameter_reader.cc new file mode 100644 index 0000000000..5b0fd282c2 --- /dev/null +++ b/tools/graph_bag/src/parameter_reader.cc @@ -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. + */ + +#include +#include +#include + +namespace graph_bag { +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"); +} + +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("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"); + 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"); +} + +void LoadGraphBagParams(config_reader::ConfigReader& config, GraphBagParams& params) { + params.save_optical_flow_images = mc::LoadBool(config, "save_optical_flow_images"); + 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"); +} +} // namespace graph_bag diff --git a/tools/graph_bag/src/sparse_mapping_pose_adder.cc b/tools/graph_bag/src/sparse_mapping_pose_adder.cc new file mode 100644 index 0000000000..94dc4aadd6 --- /dev/null +++ b/tools/graph_bag/src/sparse_mapping_pose_adder.cc @@ -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. + */ + +#include +#include +#include +#include + +#include + +namespace { +// TODO(rsoussan): Unify this with live measurement simulator, put in utilities +bool string_ends_with(const std::string& str, const std::string& ending) { + if (str.length() >= ending.length()) { + return (0 == str.compare(str.length() - ending.length(), ending.length(), ending)); + } else { + return false; + } +} +} // namespace + +namespace graph_bag { +namespace lc = localization_common; +namespace lm = localization_measurements; +SparseMappingPoseAdder::SparseMappingPoseAdder(const std::string& input_bag_name, const std::string& output_bag_name, + const gtsam::Pose3& nav_cam_T_body) + : input_bag_(input_bag_name, rosbag::bagmode::Read), + output_bag_(output_bag_name, rosbag::bagmode::Write), + nav_cam_T_body_(nav_cam_T_body) {} + +void SparseMappingPoseAdder::AddPoses() { + rosbag::View view(input_bag_); + for (const rosbag::MessageInstance msg : view) { + if (string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_ML_FEATURES)) { + const ff_msgs::VisualLandmarksConstPtr vl_features = msg.instantiate(); + if (vl_features->landmarks.size() >= 5) { + const auto world_T_nav_cam = lc::GtPose(*vl_features); + 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 ros::Time timestamp = lc::RosTimeFromHeader(vl_features->header); + output_bag_.write("/" + std::string(TOPIC_SPARSE_MAPPING_POSE), timestamp, sparse_mapping_pose_msg); + } + } + output_bag_.write(msg.getTopic(), msg.getTime(), msg); + } +} +} // namespace graph_bag diff --git a/tools/graph_bag/src/utilities.cc b/tools/graph_bag/src/utilities.cc new file mode 100644 index 0000000000..be50a857a7 --- /dev/null +++ b/tools/graph_bag/src/utilities.cc @@ -0,0 +1,86 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include + +#include +#include +#include + +#include +#include + +namespace graph_bag { +void FeatureTrackImage(const graph_localizer::FeatureTrackMap& 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; + cv::Scalar color; + if (points.size() <= 1) { + // Red for single point tracks + color = cv::Scalar(50, 255, 50, 1); + } else if (points.size() < 3) { + // Yellow for medium length tracks + color = cv::Scalar(255, 255, 0, 1); + } else { + // Green for long tracks + color = cv::Scalar(50, 255, 50, 1); + } + + // Draw track history + for (int i = 0; i < static_cast(points.size()) - 1; ++i) { + const auto distorted_previous_point = Distort(points[i].image_point, camera_params); + const auto distorted_current_point = Distort(points[i + 1].image_point, camera_params); + cv::circle(feature_track_image, distorted_current_point, 2 /* Radius*/, cv::Scalar(0, 255, 255), -1 /*Filled*/, + 8); + cv::line(feature_track_image, distorted_current_point, distorted_previous_point, color, 2, 8, 0); + } + // Account for single point tracks + if (points.size() == 1) { + cv::circle(feature_track_image, Distort(points[0].image_point, camera_params), 2 /* Radius*/, color, + -1 /*Filled*/, 8); + } + // Draw feature id at most recent point + cv::putText(feature_track_image, std::to_string(points[points.size() - 1].feature_id), + Distort(points[points.size() - 1].image_point, camera_params), CV_FONT_NORMAL, 0.4, + cv::Scalar(255, 0, 0)); + } +} + +boost::optional CreateFeatureTrackImage(const sensor_msgs::ImageConstPtr& image_msg, + const graph_localizer::FeatureTrackMap& feature_tracks, + const camera::CameraParameters& camera_params) { + 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_tracks, camera_params, feature_track_image->image); + return feature_track_image->toImageMsg(); +} + +cv::Point Distort(const Eigen::Vector2d& undistorted_point, const camera::CameraParameters& params) { + Eigen::Vector2d distorted_point; + params.Convert(undistorted_point, &distorted_point); + return cv::Point(distorted_point.x(), distorted_point.y()); +} +} // namespace graph_bag diff --git a/tools/graph_bag/tools/run_bag_imu_filterer.cc b/tools/graph_bag/tools/run_bag_imu_filterer.cc new file mode 100644 index 0000000000..b72e9299fb --- /dev/null +++ b/tools/graph_bag/tools/run_bag_imu_filterer.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 +#include +#include + +#include +#include + +namespace po = boost::program_options; +namespace lc = localization_common; + +int main(int argc, char** argv) { + std::string output_bagfile; + std::string filter_name; + po::options_description desc( + "Reads through a bag file and filters imu measurements, replacing the old imu measurements with new filtered " + "ones. Saves output to a new bagfile."); + desc.add_options()("help", "produce help message")("bagfile", po::value()->required(), "Input bagfile")( + "output-bagfile,o", po::value(&output_bagfile)->default_value("filtered_imu.bag"), "Output bagfile")( + "filter-name,f", po::value(&filter_name)->default_value("none"), "Imu filter name"); + po::positional_options_description p; + p.add("bagfile", 1); + po::variables_map vm; + try { + po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm); + po::notify(vm); + } catch (std::exception& e) { + std::cerr << "Error: " << e.what() << "\n"; + return 1; + } + + if (vm.count("help")) { + std::cout << desc << "\n"; + return 1; + } + + const std::string input_bag = vm["bagfile"].as(); + + if (!boost::filesystem::exists(input_bag)) { + LogFatal("Bagfile " << input_bag << " not found."); + } + + if (vm["output-bagfile"].defaulted()) { + const auto current_path = boost::filesystem::current_path(); + boost::filesystem::path output_bagfile_path(output_bagfile); + const auto output_bagfile_full_path = current_path / output_bagfile_path; + output_bagfile = output_bagfile_full_path.string(); + } + + graph_bag::BagImuFilterer bag_imu_filterer(input_bag, output_bagfile, filter_name); + bag_imu_filterer.Convert(); +} diff --git a/tools/graph_bag/tools/run_graph_bag.cc b/tools/graph_bag/tools/run_graph_bag.cc new file mode 100644 index 0000000000..0543a3900b --- /dev/null +++ b/tools/graph_bag/tools/run_graph_bag.cc @@ -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. + */ + +#include +#include +#include +#include + +#include +#include + +#ifdef GOOGLE_PROFILER +#include +#endif + +namespace po = boost::program_options; +namespace lc = localization_common; + +int main(int argc, char** argv) { + std::string image_topic; + std::string output_bagfile; + std::string output_stats_file; + std::string robot_config_file; + std::string world; + bool use_image_features; + std::string graph_config_path_prefix; + po::options_description desc("Runs graph localization on a bagfile and saves the results to a new bagfile."); + desc.add_options()("help", "produce help message")("bagfile", po::value()->required(), "Input bagfile")( + "map-file", po::value()->required(), "Map file")("config-path,c", po::value()->required(), + "Config path")( + "image-topic,i", po::value(&image_topic)->default_value("mgt/img_sampler/nav_cam/image_record"), + "Image topic")("output-bagfile,o", po::value(&output_bagfile)->default_value("results.bag"), + "Output bagfile")("output-stats-file,s", + po::value(&output_stats_file)->default_value("graph_stats.csv"), + "Output stats file")( + "robot-config-file,r", po::value(&robot_config_file)->default_value("config/robots/bumble.config"), + "Robot config file")("world,w", po::value(&world)->default_value("iss"), "World name")( + "use-image-features,f", po::value(&use_image_features)->default_value(true), "Use image features")( + "graph-config-path-prefix,g", po::value(&graph_config_path_prefix)->default_value(""), + "Graph config path prefix"); + po::positional_options_description p; + p.add("bagfile", 1); + p.add("map-file", 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); + po::notify(vm); + } catch (std::exception& e) { + std::cerr << "Error: " << e.what() << "\n"; + return 1; + } + + if (vm.count("help")) { + std::cout << desc << "\n"; + return 1; + } + + const std::string input_bag = vm["bagfile"].as(); + const std::string map_file = vm["map-file"].as(); + const std::string config_path = vm["config-path"].as(); + + // 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); + + if (!boost::filesystem::exists(input_bag)) { + LogFatal("Bagfile " << input_bag << " not found."); + } + + if (!boost::filesystem::exists(map_file)) { + LogFatal("Map file " << map_file << " not found."); + } + + if (vm["output-bagfile"].defaulted()) { + const auto current_path = boost::filesystem::current_path(); + boost::filesystem::path output_bagfile_path(output_bagfile); + const auto output_bagfile_full_path = current_path / output_bagfile_path; + output_bagfile = output_bagfile_full_path.string(); + } + + if (vm["output-stats-file"].defaulted()) { + const auto current_path = boost::filesystem::current_path(); + boost::filesystem::path output_stats_path(output_stats_file); + const auto output_stats_full_path = current_path / output_stats_path; + output_stats_file = output_stats_full_path.string(); + } + + // Set environment configs + lc::SetEnvironmentConfigs(config_path, world, robot_config_file); + config_reader::ConfigReader config; + + graph_bag::GraphBag graph_bag(input_bag, map_file, image_topic, output_bagfile, output_stats_file, use_image_features, + graph_config_path_prefix); +#ifdef GOOGLE_PROFILER + ProfilerStart(boost::filesystem::current_path() + "/graph_bag_prof.txt"); +#endif + graph_bag.Run(); +#ifdef GOOLGE_PROFILER + ProfilerFlush(); + ProfilerStop(); +#endif +} diff --git a/tools/graph_bag/tools/run_sparse_mapping_pose_adder.cc b/tools/graph_bag/tools/run_sparse_mapping_pose_adder.cc new file mode 100644 index 0000000000..ba9fddac88 --- /dev/null +++ b/tools/graph_bag/tools/run_sparse_mapping_pose_adder.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 +#include +#include +#include + +#include +#include + +#include + +namespace po = boost::program_options; +namespace lc = localization_common; + +int main(int argc, char** argv) { + std::string robot_config_file; + std::string world; + po::options_description desc( + "Adds sparse mapping poses to a new bag file using sparse mapping feature messages and body_T_nav_cam extrinsics."); + desc.add_options()("help", "produce help message")("bagfile", po::value()->required(), "Bagfile")( + "config-path,c", po::value()->required(), "Config path")( + "robot-config-file,r", po::value(&robot_config_file)->default_value("config/robots/bumble.config"), + "Robot config file")("world,w", po::value(&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); + po::notify(vm); + } catch (std::exception& e) { + std::cerr << "Error: " << e.what() << "\n"; + return 1; + } + + if (vm.count("help")) { + std::cout << desc << "\n"; + return 1; + } + + const std::string input_bag = vm["bagfile"].as(); + const std::string config_path = vm["config-path"].as(); + + // 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); + + if (!boost::filesystem::exists(input_bag)) { + LogFatal("Bagfile " << input_bag << " not found."); + } + + boost::filesystem::path input_bag_path(input_bag); + 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); + config_reader::ConfigReader config; + config.AddFile("geometry.config"); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + + const gtsam::Pose3 body_T_nav_cam = lc::LoadTransform(config, "nav_cam_transform"); + graph_bag::SparseMappingPoseAdder sparse_mapping_pose_adder(input_bag, output_bag_path.string(), + body_T_nav_cam.inverse()); + sparse_mapping_pose_adder.AddPoses(); +} diff --git a/tools/imu_bias_tester/CMakeLists.txt b/tools/imu_bias_tester/CMakeLists.txt new file mode 100644 index 0000000000..23e44b5bca --- /dev/null +++ b/tools/imu_bias_tester/CMakeLists.txt @@ -0,0 +1,37 @@ +#Copyright(c) 2017, United States Government, as represented by the +#Administrator of the National Aeronautics and Space Administration. +# +#All rights reserved. +# +#The Astrobee platform is licensed under the Apache License, Version 2.0 +#(the "License"); you may not use this file except in compliance with the +#License.You may obtain a copy of the License at +# +#http: // www.apache.org/licenses/LICENSE-2.0 +# +#Unless required by applicable law or agreed to in writing, software +#distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +#WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.See the +#License for the specific language governing permissions and limitations +#under the License. + +project(imu_bias_tester) + +if (USE_ROS) + +catkin_package( + LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} ff_nodelet imu_integration localization_common localization_measurements + INCLUDE_DIRS include ${GTSAM_INCLUDE_DIR} ${GLOG_INCLUDE_DIRS} + CATKIN_DEPENDS + DEPENDS gtsam +) + +create_library(TARGET ${PROJECT_NAME} + LIBS ${catkin_LIBRARIES} ${GLOG_LIBRARIES} ff_nodelet imu_integration localization_common localization_measurements + INC ${catkin_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} + DEPS gtsam ff_msgs +) + +install_launch_files() + +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 new file mode 100644 index 0000000000..3027b6d4de --- /dev/null +++ b/tools/imu_bias_tester/include/imu_bias_tester/imu_bias_tester.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 IMU_BIAS_TESTER_IMU_BIAS_TESTER_H_ +#define IMU_BIAS_TESTER_IMU_BIAS_TESTER_H_ + +#include +#include +#include + +#include +#include + +namespace imu_bias_tester { +class ImuBiasTester : public imu_integration::ImuIntegrator { + public: + explicit ImuBiasTester(const imu_integration::ImuIntegratorParams& params); + + std::vector 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& predicted_states); + void RemoveOldCombinedNavStatesIfNeeded(); + bool Initialized(); + void Initialize(const localization_common::CombinedNavState& combined_nav_state); + + std::map combined_nav_states_; + bool initialized_; + boost::optional 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 new file mode 100644 index 0000000000..972bd30b24 --- /dev/null +++ b/tools/imu_bias_tester/include/imu_bias_tester/imu_bias_tester_nodelet.h @@ -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. + */ +#ifndef IMU_BIAS_TESTER_IMU_BIAS_TESTER_NODELET_H_ +#define IMU_BIAS_TESTER_IMU_BIAS_TESTER_NODELET_H_ + +#include +#include +#include + +#include +#include +#include +#include + +#include + +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_; +}; +} // 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 new file mode 100644 index 0000000000..f314da70cb --- /dev/null +++ b/tools/imu_bias_tester/include/imu_bias_tester/imu_bias_tester_wrapper.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 IMU_BIAS_TESTER_IMU_BIAS_TESTER_WRAPPER_H_ +#define IMU_BIAS_TESTER_IMU_BIAS_TESTER_WRAPPER_H_ + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace imu_bias_tester { +class ImuBiasTesterWrapper { + public: + explicit ImuBiasTesterWrapper(const std::string& graph_config_path_prefix = ""); + // Returns predicted nav states + std::vector LocalizationStateCallback(const ff_msgs::GraphState& loc_msg); + void ImuCallback(const sensor_msgs::Imu& imu_msg); + + private: + std::unique_ptr 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 new file mode 100644 index 0000000000..fd50c6a9f8 --- /dev/null +++ b/tools/imu_bias_tester/launch/imu_bias_tester.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tools/imu_bias_tester/nodelet_plugins.xml b/tools/imu_bias_tester/nodelet_plugins.xml new file mode 100644 index 0000000000..d507253f52 --- /dev/null +++ b/tools/imu_bias_tester/nodelet_plugins.xml @@ -0,0 +1,11 @@ + + + + This nodelet wraps the imu_bias_tester + + + + + diff --git a/tools/imu_bias_tester/package.xml b/tools/imu_bias_tester/package.xml new file mode 100644 index 0000000000..6a7d64ec0b --- /dev/null +++ b/tools/imu_bias_tester/package.xml @@ -0,0 +1,28 @@ + + imu_bias_tester + 1.0.0 + + Imu bias tester package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + catkin + ff_msgs + nodelet + roscpp + ff_msgs + nodelet + roscpp + + + + + diff --git a/tools/imu_bias_tester/readme.md b/tools/imu_bias_tester/readme.md new file mode 100644 index 0000000000..0d4586b4c6 --- /dev/null +++ b/tools/imu_bias_tester/readme.md @@ -0,0 +1,11 @@ +\page imubiastester Imu Bias Tester + +## ImuBiasTester +The imu bias tester provides a way to evaluate the accuracy of imu biases without having bias groundtruth. It works by relying on some sort of localization groundtruth instead. The bias tester integrates imu measurements but updates biases as localization estimates are provided. Thus, if the biases are perfectly estimated by the localizer, the integrated imu measurements should perfectly match localization groundtruth. The nodelet subscribes to the imu and localization state and published a imu bias pose. + +# 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 new file mode 100644 index 0000000000..fa94de9d2f --- /dev/null +++ b/tools/imu_bias_tester/src/imu_bias_tester.cc @@ -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. + */ + +#include +#include + +#include + +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 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 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& 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().cbegin(); + 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_nodelet.cc b/tools/imu_bias_tester/src/imu_bias_tester_nodelet.cc new file mode 100644 index 0000000000..afb896e46f --- /dev/null +++ b/tools/imu_bias_tester/src/imu_bias_tester_nodelet.cc @@ -0,0 +1,72 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include + +#include + +namespace imu_bias_tester { +namespace lc = localization_common; + +ImuBiasTesterNodelet::ImuBiasTesterNodelet() : ff_util::FreeFlyerNodelet(NODE_IMU_AUG, true) { + imu_nh_.setCallbackQueue(&imu_queue_); + loc_nh_.setCallbackQueue(&loc_queue_); +} + +void ImuBiasTesterNodelet::Initialize(ros::NodeHandle* nh) { + SubscribeAndAdvertise(nh); + Run(); +} + +void ImuBiasTesterNodelet::SubscribeAndAdvertise(ros::NodeHandle* nh) { + pose_pub_ = nh->advertise(TOPIC_IMU_BIAS_TESTER_POSE, 1); + + imu_sub_ = imu_nh_.subscribe(TOPIC_HARDWARE_IMU, 100, &ImuBiasTesterNodelet::ImuCallback, this, + ros::TransportHints().tcpNoDelay()); + state_sub_ = loc_nh_.subscribe(TOPIC_GRAPH_LOC_STATE, 10, &ImuBiasTesterNodelet::LocalizationStateCallback, this, + ros::TransportHints().tcpNoDelay()); +} + +void ImuBiasTesterNodelet::ImuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg) { + imu_bias_tester_wrapper_.ImuCallback(*imu_msg); +} + +void ImuBiasTesterNodelet::LocalizationStateCallback(const ff_msgs::GraphState::ConstPtr& loc_msg) { + const auto imu_integration_states = imu_bias_tester_wrapper_.LocalizationStateCallback(*loc_msg); + for (const auto& state : imu_integration_states) { + geometry_msgs::PoseStamped pose_msg; + lc::PoseToMsg(state.pose(), pose_msg.pose); + lc::TimeToHeader(state.timestamp(), pose_msg.header); + pose_pub_.publish(pose_msg); + } +} + +void ImuBiasTesterNodelet::Run() { + ros::Rate rate(100); + while (ros::ok()) { + imu_queue_.callAvailable(); + loc_queue_.callAvailable(); + rate.sleep(); + } +} +} // namespace imu_bias_tester + +PLUGINLIB_EXPORT_CLASS(imu_bias_tester::ImuBiasTesterNodelet, nodelet::Nodelet); diff --git a/tools/imu_bias_tester/src/imu_bias_tester_wrapper.cc b/tools/imu_bias_tester/src/imu_bias_tester_wrapper.cc new file mode 100644 index 0000000000..c3f95caf68 --- /dev/null +++ b/tools/imu_bias_tester/src/imu_bias_tester_wrapper.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 +#include +#include +#include +#include + +#include + +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 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_rviz_plugins/CMakeLists.txt b/tools/localization_rviz_plugins/CMakeLists.txt new file mode 100644 index 0000000000..de1d5dec8e --- /dev/null +++ b/tools/localization_rviz_plugins/CMakeLists.txt @@ -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. + +project(localization_rviz_plugins) + +if (USE_ROS AND BUILD_LOC_RVIZ_PLUGINS) + + # For Qt + set(CMAKE_AUTOMOC ON) + set(QT_LIBRARIES Qt5::Widgets) + + catkin_package( + LIBRARIES ${PROJECT_NAME} ${catkin_LIBRARIES} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} ${QT_LIBRARIES} camera config_reader graph_bag graph_localizer imu_integration localization_common localization_measurements + INCLUDE_DIRS ${GTSAM_INCLUDE_DIR} ${GLOG_INCLUDE_DIRS} ${Qt5Core_INCLUDE_DIRS} ${Qt5Gui_INCLUDE_DIRS} + CATKIN_DEPENDS rviz + DEPENDS gtsam + ) + + create_library(TARGET ${PROJECT_NAME} + LIBS ${catkin_LIBRARIES} ${GLOG_LIBRARIES} ${QT_LIBRARIES} camera config_reader graph_bag graph_localizer imu_integration localization_common localization_measurements + INC ${catkin_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} ${Qt5Core_INCLUDE_DIRS} ${Qt5Gui_INCLUDE_DIRS} + ADD_SRCS src/localization_graph_display.cc src/localization_graph_panel.cc src/pose_display.cc src/utilities.cc src/imu_augmentor_display.cc src/sparse_mapping_display.cc ${MOC_FILES} + DEPS gtsam rviz + ) + +endif (USE_ROS AND BUILD_LOC_RVIZ_PLUGINS) diff --git a/tools/localization_rviz_plugins/nodelet_plugins.xml b/tools/localization_rviz_plugins/nodelet_plugins.xml new file mode 100644 index 0000000000..e10511248d --- /dev/null +++ b/tools/localization_rviz_plugins/nodelet_plugins.xml @@ -0,0 +1,38 @@ + + + + Localization Graph Display + + + + + Localization Graph Panel + + + + + Sparse Mapping Pose Display + + + + + Imu Augmentor Display + + + + + Sparse Mapping Display + + + + diff --git a/tools/localization_rviz_plugins/package.xml b/tools/localization_rviz_plugins/package.xml new file mode 100644 index 0000000000..29af693ce5 --- /dev/null +++ b/tools/localization_rviz_plugins/package.xml @@ -0,0 +1,22 @@ + + localization_rviz_plugins + 1.0.0 + + Localization rviz plugins package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + roscpp + rviz + + + + diff --git a/tools/localization_rviz_plugins/plugin_description.xml b/tools/localization_rviz_plugins/plugin_description.xml new file mode 100644 index 0000000000..685e2b5442 --- /dev/null +++ b/tools/localization_rviz_plugins/plugin_description.xml @@ -0,0 +1,24 @@ + + + + A panel widget allowing simple diff-drive style robot base control. + + + + + Displays direction and scale of accelerations from sensor_msgs/Imu messages. + + sensor_msgs/Imu + + + + Tool for planting flags on the ground plane in rviz. + + + diff --git a/tools/localization_rviz_plugins/readme.md b/tools/localization_rviz_plugins/readme.md new file mode 100644 index 0000000000..bb32981587 --- /dev/null +++ b/tools/localization_rviz_plugins/readme.md @@ -0,0 +1,18 @@ +\page localizationrvizplugins Localization Rviz Plugins + +# Package Overview +The localization rviz plugins package provides various plugins for use to display information on various localization components such as the graph localizer, imu augmentor, and sparse mapping results. Plugins have the advantage of allowing many customizations to visualize information. Additionally, whereas rviz provides visualization\_msgs, plugins can subscibe to any message and prevent the creation of many visualization only messages that may live in functional code. +The plugins in this package are spit between displays and panels, where displays provide 3D visualizations and panels display color coded text and numerical information. + +## Plugins +## Localization Graph Display +The localization graph display draws the full history of poses in the latest graph localization message. It also draws imu estimates between poses as arrows, and publishes optical flow feature track images using the feature tracks in the localizer. + +## Localization Graph Panel +The localization graph panel displays lots of information about the most recent graph localizer, including factor counts, factor information, timing, imu information, and more. + +## Imu Augmentor Display +The imu augmentor display draws imu augmentor poses. This is useful when comparing with graph localizer poses and sparse mapping poses, as ideally these are all alligned. + +## Pose Display +The pose display draws a history of poses. This is useful when comparing a pose topic such as sparse mapping poses or groundtruth poses with localization. diff --git a/tools/localization_rviz_plugins/src/imu_augmentor_display.cc b/tools/localization_rviz_plugins/src/imu_augmentor_display.cc new file mode 100644 index 0000000000..0f9fe00c1c --- /dev/null +++ b/tools/localization_rviz_plugins/src/imu_augmentor_display.cc @@ -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. + */ + +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include "imu_augmentor_display.h" // NOLINT +#include "utilities.h" // NOLINT + +namespace localization_rviz_plugins { +namespace lc = localization_common; +namespace mc = msg_conversions; + +ImuAugmentorDisplay::ImuAugmentorDisplay() { + show_pose_axes_.reset(new rviz::BoolProperty("Show Pose Axes", true, "Show graph poses as axes.", this)); + pose_axes_size_.reset(new rviz::FloatProperty("Pose Axes Size", 0.1, "Pose axes size.", this)); + number_of_poses_.reset(new rviz::IntProperty("Number of Poses", 10, "Number of Poses.", this)); + show_imu_acceleration_arrow_.reset( + new rviz::BoolProperty("Show Imu Acceleration Arrow", true, "Show imu acceleration arrow.", this)); + imu_acceleration_arrow_scale_.reset(new rviz::FloatProperty("Imu Arrow Scale", 0.1, "Imu arrow scale.", this)); +} + +void ImuAugmentorDisplay::onInitialize() { MFDClass::onInitialize(); } + +void ImuAugmentorDisplay::reset() { + MFDClass::reset(); + clearDisplay(); +} + +void ImuAugmentorDisplay::clearDisplay() { + imu_augmentor_pose_axes_.clear(); + imu_acceleration_arrow_.reset(); +} + +void ImuAugmentorDisplay::processMessage(const ff_msgs::EkfState::ConstPtr& msg) { + const auto world_T_body = lc::PoseFromMsg(msg->pose); + const auto timestamp = lc::RosTimeFromHeader(msg->header); + const auto current_frame_T_world = currentFrameTFrame("world", timestamp, *context_); + if (!current_frame_T_world) { + LogError("processMessage: Failed to get current_frame_T_world."); + return; + } + const gtsam::Pose3 current_frame_T_body = *current_frame_T_world * world_T_body; + + if (show_pose_axes_->getBool()) { + imu_augmentor_pose_axes_.set_capacity(number_of_poses_->getInt()); + const float scale = pose_axes_size_->getFloat(); + auto axis = axisFromPose(current_frame_T_body, scale, context_->getSceneManager(), scene_node_); + axis->setXColor(Ogre::ColourValue(0.5, 0, 0, 0.3)); + axis->setYColor(Ogre::ColourValue(0, 0.5, 0, 0.3)); + axis->setZColor(Ogre::ColourValue(0, 0, 0.5, 0.3)); + imu_augmentor_pose_axes_.push_back(std::move(axis)); + } else { + imu_augmentor_pose_axes_.clear(); + } + + if (show_imu_acceleration_arrow_->getBool()) { + imu_acceleration_arrow_.reset(new rviz::Arrow(context_->getSceneManager(), scene_node_)); + imu_acceleration_arrow_->setPosition(ogrePosition(current_frame_T_body)); + const gtsam::Vector3 body_F_acceleration = mc::VectorFromMsg(msg->accel); + const gtsam::Vector3 current_frame_F_acceleration = current_frame_T_body.rotation() * body_F_acceleration; + const float scale = imu_acceleration_arrow_scale_->getFloat(); + const auto orientation_and_length = + getOrientationAndLength(gtsam::Vector3::Zero(), scale * current_frame_F_acceleration); + const float diameter = scale / 50.0; + imu_acceleration_arrow_->setOrientation(orientation_and_length.first); + imu_acceleration_arrow_->set(3.0 * orientation_and_length.second / 4.0, 0.5 * diameter, + orientation_and_length.second / 4.0, diameter); + imu_acceleration_arrow_->setColor(1, 1, 0, 1); + } else { + imu_acceleration_arrow_.reset(); + } + // TODO(rsoussan): add ang vel visual? +} +} // namespace localization_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(localization_rviz_plugins::ImuAugmentorDisplay, rviz::Display) diff --git a/tools/localization_rviz_plugins/src/imu_augmentor_display.h b/tools/localization_rviz_plugins/src/imu_augmentor_display.h new file mode 100644 index 0000000000..311f61af85 --- /dev/null +++ b/tools/localization_rviz_plugins/src/imu_augmentor_display.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. + */ + +// Header file must go in src directory for Qt/Rviz plugin +#ifndef LOCALIZATION_RVIZ_PLUGINS_IMU_AUGMENTOR_DISPLAY_H_ // NOLINT +#define LOCALIZATION_RVIZ_PLUGINS_IMU_AUGMENTOR_DISPLAY_H_ // NOLINT + +// Required for Qt +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif + +namespace Ogre { +class SceneNode; +} + +namespace localization_rviz_plugins { + +class ImuAugmentorDisplay : public rviz::MessageFilterDisplay { + Q_OBJECT // NOLINT + public : // NOLINT + ImuAugmentorDisplay(); + ~ImuAugmentorDisplay() = default; + + // private: + protected: + void onInitialize() final; + void reset() final; + + private Q_SLOTS: // NOLINT + + private: + void processMessage(const ff_msgs::EkfState::ConstPtr& imu_augmentor_msg); + void clearDisplay(); + + boost::circular_buffer> imu_augmentor_pose_axes_; + std::unique_ptr imu_acceleration_arrow_; + std::unique_ptr show_pose_axes_; + std::unique_ptr pose_axes_size_; + std::unique_ptr number_of_poses_; + std::unique_ptr show_imu_acceleration_arrow_; + std::unique_ptr imu_acceleration_arrow_scale_; +}; +} // namespace localization_rviz_plugins +#endif // LOCALIZATION_RVIZ_PLUGINS_IMU_AUGMENTOR_DISPLAY_H_ NOLINT diff --git a/tools/localization_rviz_plugins/src/localization_graph_display.cc b/tools/localization_rviz_plugins/src/localization_graph_display.cc new file mode 100644 index 0000000000..ec197fe8b4 --- /dev/null +++ b/tools/localization_rviz_plugins/src/localization_graph_display.cc @@ -0,0 +1,516 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "localization_graph_display.h" // NOLINT +#include "utilities.h" // NOLINT + +namespace localization_rviz_plugins { +namespace lc = localization_common; + +LocalizationGraphDisplay::LocalizationGraphDisplay() { + show_pose_axes_.reset(new rviz::BoolProperty("Show Pose Axes", true, "Show graph poses as axes.", this)); + pose_axes_size_.reset(new rviz::FloatProperty("Pose Axes Size", 0.1, "Pose axes size.", this)); + show_imu_factor_arrows_.reset( + new rviz::BoolProperty("Show Imu Factor Arrows", true, "Show imu factors as arrows.", this)); + imu_factor_arrows_diameter_.reset( + new rviz::FloatProperty("Imu Factor Arrows Diameter", 0.01, "Imu factor arrows diameter.", this)); + publish_optical_flow_images_.reset( + new rviz::BoolProperty("Publish Optical Flow Images", true, "Publish Optical Flow feature tracks image.", this)); + publish_smart_factor_images_.reset( + new rviz::BoolProperty("Publish Smart Factor Images", true, "Publish Smart factor projection image.", this)); + publish_loc_projection_factor_images_.reset( + new rviz::BoolProperty("Publish Loc Projection Factor Images", true, "Publish loc projection factor image.", this)); + publish_projection_factor_images_.reset( + new rviz::BoolProperty("Publish Projection Factor Images", true, "Publish projection factor image.", this)); + show_projection_factor_visual_.reset( + new rviz::BoolProperty("Show Projection Visual", true, "Show projection factor visual.", this)); + projection_factor_slider_.reset(new rviz::SliderProperty( + "Show Projection Visual", 0, "Show projection factor visual.", this, SLOT(addSmartFactorsProjectionVisual()))); + + image_transport::ImageTransport image_transport(nh_); + image_sub_ = image_transport.subscribe(TOPIC_HARDWARE_NAV_CAM, 10, &LocalizationGraphDisplay::imageCallback, this); + optical_flow_image_pub_ = image_transport.advertise("/graph_localizer/optical_flow_feature_tracks", 1); + smart_factor_projection_image_pub_ = image_transport.advertise("/graph_localizer/smart_factor_projections", 1); + projection_image_pub_ = image_transport.advertise("/graph_localizer/visual_odometry_projections", 1); + loc_projection_factor_image_pub_ = image_transport.advertise("/graph_localizer/loc_projection_factor", 1); + + // Only pass program name to free flyer so that boost command line options + // are ignored when parsing gflags. + int ff_argc = 1; + char argv[] = "localization_graph_display"; + char* argv_ptr = &argv[0]; + char** argv_ptr_ptr = &argv_ptr; + ff_common::InitFreeFlyerApplication(&ff_argc, &argv_ptr_ptr); + config_reader::ConfigReader config; + config.AddFile("cameras.config"); + + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + // Needed for feature tracks visualization + nav_cam_params_.reset(new camera::CameraParameters(&config, "nav_cam")); +} + +void LocalizationGraphDisplay::onInitialize() { MFDClass::onInitialize(); } + +void LocalizationGraphDisplay::reset() { + MFDClass::reset(); + clearDisplay(); +} + +void LocalizationGraphDisplay::clearDisplay() { + graph_pose_axes_.clear(); + imu_factor_arrows_.clear(); +} + +void LocalizationGraphDisplay::imageCallback(const sensor_msgs::ImageConstPtr& image_msg) { + img_buffer_.emplace(lc::TimeFromHeader(image_msg->header), image_msg); +} + +void LocalizationGraphDisplay::addOpticalFlowVisual(const graph_localizer::FeatureTrackMap& feature_tracks, + const localization_common::Time latest_graph_time) { + if (!publish_optical_flow_images_->getBool()) return; + const auto img = getImage(latest_graph_time); + if (!img) return; + const auto feature_track_image = graph_bag::CreateFeatureTrackImage(img, feature_tracks, *nav_cam_params_); + if (!feature_track_image) return; + optical_flow_image_pub_.publish(*feature_track_image); +} + +sensor_msgs::ImageConstPtr LocalizationGraphDisplay::getImage(const localization_common::Time time) { + const auto img_it = img_buffer_.find(time); + if (img_it == img_buffer_.end()) return nullptr; + return img_it->second; +} + +void LocalizationGraphDisplay::clearImageBuffer(const localization_common::Time oldest_graph_time) { + const auto img_it = img_buffer_.find(oldest_graph_time); + if (img_it == img_buffer_.end()) return; + img_buffer_.erase(img_buffer_.begin(), img_it); +} + +cv::Scalar LocalizationGraphDisplay::textColor(const double val, const double green_threshold, + const double yellow_threshold) { + if (val < green_threshold) + return cv::Scalar(0, 255, 0); + else if (val < yellow_threshold) + return cv::Scalar(255, 255, 0); + else + return cv::Scalar(255, 0, 0); +} + +void LocalizationGraphDisplay::addProjectionVisual(const gtsam::CameraSet& cameras, + const Camera::MeasurementVector& measurements, + const gtsam::Point3& world_t_landmark, + std::vector& images) { + // TODO(rsoussan): get timestamp from somewhere else? + const auto current_frame_T_world = currentFrameTFrame("world", ros::Time::now(), *context_); + if (!current_frame_T_world) { + LogError("addProjectionVisual: Failed to get current_frame_T_world."); + return; + } + + std::unique_ptr landmark_point( + new rviz::Shape(rviz::Shape::Type::Sphere, context_->getSceneManager(), scene_node_)); + landmark_point->setColor(Ogre::ColourValue(1, 0, 0, 1)); + landmark_point->setPosition(ogrePosition(*current_frame_T_world * world_t_landmark)); + landmark_point->setScale(Ogre::Vector3(0.1, 0.1, 0.1)); + landmark_points_.push_back(std::move(landmark_point)); + + for (const auto& camera : cameras) { + auto axis = axisFromPose(*current_frame_T_world * camera.pose(), 0.1, context_->getSceneManager(), scene_node_); + axis->setXColor(Ogre::ColourValue(0.5, 0, 0, 0.3)); + axis->setYColor(Ogre::ColourValue(0, 0.5, 0, 0.3)); + axis->setZColor(Ogre::ColourValue(0, 0, 0.5, 0.3)); + camera_pose_axes_.push_back(std::move(axis)); + + std::unique_ptr camera_t_landmark_line(new rviz::Line(context_->getSceneManager(), scene_node_)); + camera_t_landmark_line->setPoints(ogrePosition(*current_frame_T_world * camera.pose()), + ogrePosition(*current_frame_T_world * world_t_landmark)); + camera_t_landmark_line->setColor(1.0, 0, 0, 1.0); + camera_t_landmark_lines_.emplace_back(std::move(camera_t_landmark_line)); + } + + // Create Projection Image + // TODO(rsoussan): unify this with smart projection visual + const auto& image = images.front(); + cv_bridge::CvImage projection_image; + projection_image.encoding = sensor_msgs::image_encodings::RGB8; + projection_image.image = cv::Mat(image.rows, image.cols, CV_8UC3, cv::Scalar(0, 0, 0)); + + const int num_images = images.size(); + const int rows = image.rows; + const int cols = image.cols; + int divisor; + // Always create square images (2x2, 3x3, or 4x4) + // Draw max 16 images + if (num_images <= 4) { + divisor = 2; + } else if (num_images <= 9) { + divisor = 3; + } else { + divisor = 4; + } + + const int width = cols / divisor; + const int height = rows / divisor; + + int destination_row = 0; + int destination_column = 0; + // Cameras are in same order as images and measurements + for (int i = 0; i < num_images && i < 16; ++i) { + const auto distorted_measurement = graph_bag::Distort(measurements[i], *nav_cam_params_); + cv::circle(images[i], distorted_measurement, 20 /* Radius*/, cv::Scalar(0, 255, 0), -1 /*Filled*/, 8); + const cv::Point rectangle_offset(40, 40); + cv::rectangle(images[i], distorted_measurement - rectangle_offset, distorted_measurement + rectangle_offset, + cv::Scalar(0, 255, 0), 8); + // TODO(rsoussan): account for case where triangulation fails + if (true) { + const auto projection = cameras[i].project2(world_t_landmark); + const auto distorted_projection = graph_bag::Distort(projection, *nav_cam_params_); + cv::circle(images[i], distorted_projection, 15 /* Radius*/, cv::Scalar(255, 0, 0), -1 /*Filled*/, 8); + } + cv::resize(images[i], images[i], cv::Size(width, height)); + images[i].copyTo(projection_image.image(cv::Rect(destination_column, destination_row, width, height))); + destination_column += width; + // Move down a row when a row of images in the destination image is filled. + // Account for integer division (farthest column might not be equal to destination.cols) + if (std::abs(destination_column - projection_image.image.cols) < 10) { + destination_column = 0; + destination_row += height; + } + } + projection_image_pub_.publish(projection_image.toImageMsg()); +} + +void LocalizationGraphDisplay::addLocProjectionVisual( + const std::vector*> loc_projection_factors, + const graph_localizer::GraphValues& graph_values) { + if (!publish_loc_projection_factor_images_->getBool()) return; + lc::Time latest_timestamp = std::numeric_limits::lowest(); + for (const auto loc_projection_factor : loc_projection_factors) { + const auto timestamp = graph_values.Timestamp(loc_projection_factor->key()); + if (!timestamp) continue; + if (*timestamp > latest_timestamp) latest_timestamp = *timestamp; + } + + std::vector*> latest_loc_projection_factors; + for (const auto loc_projection_factor : loc_projection_factors) { + const auto timestamp = graph_values.Timestamp(loc_projection_factor->key()); + if (!timestamp) continue; + if (*timestamp == latest_timestamp) latest_loc_projection_factors.emplace_back(loc_projection_factor); + } + + const auto image_msg = getImage(latest_timestamp); + if (!image_msg) return; + cv_bridge::CvImagePtr cv_image; + try { + cv_image = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::RGB8); + } catch (cv_bridge::Exception& e) { + LogError("cv_bridge exception: " << e.what()); + return; + } + const cv::Mat image = cv_image->image; + + cv_bridge::CvImage loc_projection_factor_image; + loc_projection_factor_image.encoding = sensor_msgs::image_encodings::RGB8; + loc_projection_factor_image.image = image.clone(); // cv::Mat(image.rows, image.cols, CV_8UC3, cv::Scalar(0, 0, 0)); + const auto world_T_body = graph_values.at(latest_loc_projection_factors.front()->key()); + if (!world_T_body) { + LogError("addLocProjectionVisual: Failed to get world_T_body."); + return; + } + const gtsam::PinholeCamera camera( + world_T_body->compose(*(latest_loc_projection_factors.front()->body_P_sensor())), + *(latest_loc_projection_factors.front()->calibration())); + + for (const auto loc_projection_factor : latest_loc_projection_factors) { + const auto projected_point = camera.project(loc_projection_factor->landmark_point()); + const auto distorted_measurement = graph_bag::Distort(loc_projection_factor->measured(), *nav_cam_params_); + cv::circle(loc_projection_factor_image.image, distorted_measurement, 13 /* Radius*/, cv::Scalar(0, 255, 0), + -1 /*Filled*/, 8); + const auto distorted_projected_point = graph_bag::Distort(projected_point, *nav_cam_params_); + cv::circle(loc_projection_factor_image.image, distorted_projected_point, 7 /* Radius*/, cv::Scalar(255, 0, 0), + -1 /*Filled*/, 8); + } + loc_projection_factor_image_pub_.publish(loc_projection_factor_image.toImageMsg()); +} + +void LocalizationGraphDisplay::addSmartFactorsProjectionVisual() { + landmark_points_.clear(); + camera_pose_axes_.clear(); + camera_t_landmark_lines_.clear(); + + if (!publish_projection_factor_images_->getBool() && !show_projection_factor_visual_->getBool()) return; + if (latest_smart_factors_.empty()) return; + + const int smart_factor_index = projection_factor_slider_->getInt(); + if (smart_factor_index >= static_cast(latest_smart_factors_.size())) { + LogError("addSmartFactorsProjectionVisual: Invalid smart factor index."); + return; + } + if (!latest_graph_localizer_) { + LogError("addSmartFactorsProjectionVisual: No latest graph localizer available."); + return; + } + const auto smart_factor = latest_smart_factors_[smart_factor_index]; + const auto landmark_point = smart_factor->serialized_point(latest_graph_localizer_->graph_values().values()); + // TODO(rsoussan): Draw failed landmark points, indicate with point and line colors there was a failure (color red on + // failure, green on success) + if (!landmark_point) return; + const auto cameras = smart_factor->cameras(latest_graph_localizer_->graph_values().values()); + std::vector images; + for (const auto& key : smart_factor->keys()) { + const auto timestamp = latest_graph_localizer_->graph_values().Timestamp(key); + if (!timestamp) { + LogError("addSmartFactorsProjectionVisual: Failed to get timestamp."); + return; + } + const auto image = getImage(*timestamp); + if (!image) { + LogError("addSmartFactorsProjectionVisual: Failed to get image."); + return; + } + cv_bridge::CvImagePtr cv_image; + try { + cv_image = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::RGB8); + } catch (cv_bridge::Exception& e) { + LogError("cv_bridge exception: " << e.what()); + return; + } + + images.emplace_back(cv_image->image); + } + addProjectionVisual(cameras, smart_factor->measured(), *landmark_point, images); +} + +void LocalizationGraphDisplay::addSmartFactorProjectionVisual(const SmartFactor& smart_factor, + const graph_localizer::GraphValues& graph_values) { + if (!publish_smart_factor_images_->getBool()) return; + std::vector images; + for (const auto& key : smart_factor.keys()) { + const auto timestamp = graph_values.Timestamp(key); + if (!timestamp) return; + const auto image = getImage(*timestamp); + if (!image) { + LogError("addSmartFactorProjectionVisual: Failed to get image."); + return; + } + cv_bridge::CvImagePtr cv_image; + try { + cv_image = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::RGB8); + } catch (cv_bridge::Exception& e) { + LogError("cv_bridge exception: " << e.what()); + return; + } + + images.emplace_back(cv_image->image); + } + + if (images.empty()) return; + const auto& image = images.front(); + cv_bridge::CvImage smart_factor_projection_image; + // smart_factor_projection_image.header = + smart_factor_projection_image.encoding = sensor_msgs::image_encodings::RGB8; + smart_factor_projection_image.image = cv::Mat(image.rows, image.cols, CV_8UC3, cv::Scalar(0, 0, 0)); + + const int num_images = images.size(); + const int rows = image.rows; + const int cols = image.cols; + int divisor; + // Always create square images (2x2, 3x3, or 4x4) + // Draw max 16 images + if (num_images <= 4) { + divisor = 2; + } else if (num_images <= 9) { + divisor = 3; + } else { + divisor = 4; + } + + const int width = cols / divisor; + const int height = rows / divisor; + + int destination_row = 0; + int destination_column = 0; + const auto point = smart_factor.serialized_point(graph_values.values()); + const auto cameras = smart_factor.cameras(graph_values.values()); + + // Cameras are in same order as keys + const auto& measurements = smart_factor.measured(); + double summed_error = 0; + for (int i = 0; i < num_images && i < 16; ++i) { + const auto distorted_measurement = graph_bag::Distort(measurements[i], *nav_cam_params_); + cv::circle(images[i], distorted_measurement, 13 /* Radius*/, cv::Scalar(0, 255, 0), -1 /*Filled*/, 8); + const cv::Point rectangle_offset(40, 40); + cv::rectangle(images[i], distorted_measurement - rectangle_offset, distorted_measurement + rectangle_offset, + cv::Scalar(0, 255, 0), 8); + if (point) { + const auto projection = cameras[i].project2(*point); + const auto distorted_projection = graph_bag::Distort(projection, *nav_cam_params_); + cv::circle(images[i], distorted_projection, 7 /* Radius*/, cv::Scalar(255, 0, 0), -1 /*Filled*/, 8); + const double error = 0.5 * (measurements[i] - projection).squaredNorm(); + const auto text_color = textColor(error, 1.0, 1.5); + cv::putText(images[i], std::to_string(error), cv::Point(cols / 2 - 100, rows - 20), CV_FONT_NORMAL, 3, text_color, + 4, cv::LINE_AA); + summed_error += error; + } + cv::resize(images[i], images[i], cv::Size(width, height)); + images[i].copyTo(smart_factor_projection_image.image(cv::Rect(destination_column, destination_row, width, height))); + destination_column += width; + // Move down a row when a row of images in the destination image is filled. + // Account for integer division (farthest column might not be equal to destination.cols) + if (std::abs(destination_column - smart_factor_projection_image.image.cols) < 10) { + destination_column = 0; + destination_row += height; + } + } + + std::string text; + if (!point) { + text = "Invalid Point"; + } else { + text = std::to_string(summed_error); + const double whitened_error = std::pow(smart_factor.noise_inv_sigma(), 2) * summed_error; + text += ", " + std::to_string(whitened_error); + if (smart_factor.robust()) text += ", " + std::to_string(smart_factor.robustLoss(2.0 * whitened_error)); + } + const auto text_color = textColor(summed_error, 1.0, 1.5); + cv::putText(smart_factor_projection_image.image, text, cv::Point(cols / 2 - 100, rows - 20), CV_FONT_NORMAL, 1, + text_color, 3, cv::LINE_AA); + smart_factor_projection_image_pub_.publish(smart_factor_projection_image.toImageMsg()); +} + +void LocalizationGraphDisplay::addImuVisual(const graph_localizer::GraphLocalizer& graph_localizer, + const gtsam::CombinedImuFactor* const imu_factor) { + const auto world_T_body = graph_localizer.graph_values().at(imu_factor->key1()); + if (!world_T_body) { + LogError("addImuVisual: Failed to get world_T_body."); + return; + } + + const auto timestamp = graph_localizer.graph_values().Timestamp(imu_factor->key1()); + if (!timestamp) { + LogError("addImuVisual: Failed to get timestamp."); + } + + const auto current_frame_T_world = currentFrameTFrame("world", ros::Time(*timestamp), *context_); + if (!current_frame_T_world) { + LogError("addImuVisual: Failed to get current_frame_T_world."); + return; + } + + const gtsam::Pose3 current_frame_T_body = *current_frame_T_world * *world_T_body; + + if (show_pose_axes_->getBool()) { + const float scale = pose_axes_size_->getFloat(); + auto axis = axisFromPose(current_frame_T_body, scale, context_->getSceneManager(), scene_node_); + graph_pose_axes_.emplace_back(std::move(axis)); + } + + if (show_imu_factor_arrows_->getBool()) { + const auto imu_predicted_combined_nav_state = pimPredict(graph_localizer, imu_factor); + if (!imu_predicted_combined_nav_state) { + LogError("AddImuVisual: Failed to get pim predicted nav state."); + return; + } + auto imu_factor_arrow = std::unique_ptr(new rviz::Arrow(context_->getSceneManager(), scene_node_)); + imu_factor_arrow->setPosition(ogrePosition(current_frame_T_body)); + const auto orientation_and_length = + getOrientationAndLength(world_T_body->translation(), imu_predicted_combined_nav_state->pose().translation()); + imu_factor_arrow->setOrientation(orientation_and_length.first); + const float diameter = imu_factor_arrows_diameter_->getFloat(); + imu_factor_arrow->set(3.0 * orientation_and_length.second / 4.0, 0.5 * diameter, + orientation_and_length.second / 4.0, diameter); + imu_factor_arrow->setColor(1, 1, 0, 1); + imu_factor_arrows_.emplace_back(std::move(imu_factor_arrow)); + } +} + +void LocalizationGraphDisplay::processMessage(const ff_msgs::LocalizationGraph::ConstPtr& msg) { + clearDisplay(); + latest_graph_localizer_.reset(new graph_localizer::GraphLocalizer()); + gtsam::deserializeBinary(msg->serialized_graph, *latest_graph_localizer_); + latest_graph_localizer_->LogOnDestruction(false); + std::vector*> loc_projection_factors; + latest_smart_factors_.clear(); + SmartFactor* largest_error_smart_factor = nullptr; + double largest_smart_factor_error = -1; + if (latest_graph_localizer_->graph_values().LatestTimestamp()) + addOpticalFlowVisual(latest_graph_localizer_->feature_tracks(), + *(latest_graph_localizer_->graph_values().LatestTimestamp())); + for (const auto factor : latest_graph_localizer_->factor_graph()) { + const auto smart_factor = dynamic_cast(factor.get()); + if (smart_factor) { + latest_smart_factors_.emplace_back(smart_factor); + const double smart_factor_error = + smart_factor->serialized_error(latest_graph_localizer_->graph_values().values()); + if (smart_factor_error > largest_smart_factor_error) { + largest_smart_factor_error = smart_factor_error; + largest_error_smart_factor = smart_factor; + } + } + const auto imu_factor = dynamic_cast(factor.get()); + if (imu_factor) { + addImuVisual(*latest_graph_localizer_, imu_factor); + } + const auto loc_projection_factor = dynamic_cast*>(factor.get()); + if (loc_projection_factor) { + loc_projection_factors.emplace_back(loc_projection_factor); + } + } + if (largest_error_smart_factor) + addSmartFactorProjectionVisual(*largest_error_smart_factor, latest_graph_localizer_->graph_values()); + + if (!loc_projection_factors.empty()) + addLocProjectionVisual(loc_projection_factors, latest_graph_localizer_->graph_values()); + + const auto oldest_timestamp = latest_graph_localizer_->graph_values().OldestTimestamp(); + if (oldest_timestamp) clearImageBuffer(*oldest_timestamp); + + projection_factor_slider_->setMaximum(latest_smart_factors_.size() - 1); + addSmartFactorsProjectionVisual(); +} + +} // namespace localization_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(localization_rviz_plugins::LocalizationGraphDisplay, rviz::Display) diff --git a/tools/localization_rviz_plugins/src/localization_graph_display.h b/tools/localization_rviz_plugins/src/localization_graph_display.h new file mode 100644 index 0000000000..ee108116d5 --- /dev/null +++ b/tools/localization_rviz_plugins/src/localization_graph_display.h @@ -0,0 +1,117 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 file must go in src directory for Qt/Rviz plugin +#ifndef LOCALIZATION_RVIZ_PLUGINS_LOCALIZATION_GRAPH_DISPLAY_H_ // NOLINT +#define LOCALIZATION_RVIZ_PLUGINS_LOCALIZATION_GRAPH_DISPLAY_H_ // NOLINT + +// Required for Qt +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "slider_property.h" // NOLINT +#endif + +// Forward declarations for ogre and rviz +namespace Ogre { +class SceneNode; +} + +namespace localization_rviz_plugins { +// TODO(rsoussan): put these somewhere else! +using Calibration = gtsam::Cal3_S2; +using Camera = gtsam::PinholePose; +using SmartFactor = gtsam::RobustSmartProjectionPoseFactor; + +class LocalizationGraphDisplay : public rviz::MessageFilterDisplay { + Q_OBJECT // NOLINT + public : // NOLINT + LocalizationGraphDisplay(); + ~LocalizationGraphDisplay() = default; + + // private: + protected: + void onInitialize() final; + void reset() final; + + private Q_SLOTS: // NOLINT + void addSmartFactorsProjectionVisual(); + + private: + void processMessage(const ff_msgs::LocalizationGraph::ConstPtr& graph_msg); + void imageCallback(const sensor_msgs::ImageConstPtr& image_msg); + void clearDisplay(); + void addImuVisual(const graph_localizer::GraphLocalizer& graph_localizer, + const gtsam::CombinedImuFactor* const imu_factor); + void addProjectionVisual(const gtsam::CameraSet& cameras, const Camera::MeasurementVector& measurements, + const gtsam::Point3& world_t_landmark, std::vector& images); + void addLocProjectionVisual(const std::vector*> loc_projection_factors, + const graph_localizer::GraphValues& graph_values); + void addOpticalFlowVisual(const graph_localizer::FeatureTrackMap& feature_tracks, + const localization_common::Time latest_graph_time); + void clearImageBuffer(const localization_common::Time oldest_graph_time); + sensor_msgs::ImageConstPtr getImage(const localization_common::Time time); + void addSmartFactorProjectionVisual(const SmartFactor& smart_factor, + const graph_localizer::GraphValues& graph_values); + cv::Scalar textColor(const double val, const double green_threshold, const double yellow_threshold); + + std::vector> graph_pose_axes_; + std::vector> imu_factor_arrows_; + std::unique_ptr show_pose_axes_; + std::unique_ptr pose_axes_size_; + std::unique_ptr show_imu_factor_arrows_; + std::unique_ptr imu_factor_arrows_diameter_; + std::unique_ptr publish_optical_flow_images_; + std::unique_ptr publish_smart_factor_images_; + std::unique_ptr publish_loc_projection_factor_images_; + std::unique_ptr publish_projection_factor_images_; + std::unique_ptr show_projection_factor_visual_; + std::unique_ptr projection_factor_slider_; + image_transport::Publisher optical_flow_image_pub_; + image_transport::Publisher smart_factor_projection_image_pub_; + image_transport::Publisher projection_image_pub_; + image_transport::Publisher loc_projection_factor_image_pub_; + image_transport::Subscriber image_sub_; + ros::NodeHandle nh_; + std::map img_buffer_; + std::unique_ptr nav_cam_params_; + std::vector> landmark_points_; + std::vector> camera_pose_axes_; + std::vector> camera_t_landmark_lines_; + std::unique_ptr latest_graph_localizer_; + std::vector latest_smart_factors_; +}; +} // namespace localization_rviz_plugins +#endif // LOCALIZATION_RVIZ_PLUGINS_LOCALIZATION_GRAPH_DISPLAY_H_ NOLINT diff --git a/tools/localization_rviz_plugins/src/localization_graph_panel.cc b/tools/localization_rviz_plugins/src/localization_graph_panel.cc new file mode 100644 index 0000000000..e8765c69ef --- /dev/null +++ b/tools/localization_rviz_plugins/src/localization_graph_panel.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 +#include + +#include +#include +#include + +#include + +#include "localization_graph_panel.h" // NOLINT +#include "utilities.h" // NOLINT + +namespace { +template +// Pass Comparator so green/yellow/red can be sorted in ascending or descending order +void highlightLabel(const double val, const double green_threshold, const double yellow_threshold, QLabel& label, + const Comparator comparator = Comparator()) { + if (comparator(val, green_threshold)) + label.setStyleSheet("QLabel { background-color : green; color : white; }"); + else if (comparator(val, yellow_threshold)) + label.setStyleSheet("QLabel { background-color : yellow; color : black; }"); + else + label.setStyleSheet("QLabel { background-color : red; color : white; }"); +} + +void addVectorToLabel(const gtsam::Vector3& vec, const QString& description, QLabel& label, bool add_norm = false, + const int precision = 2) { + QString text = description + " "; + if (add_norm) { + QString vec_norm_string; + vec_norm_string.setNum(vec.norm(), 'g', precision); + text += "norm: " + vec_norm_string + ", "; + } + QString vec_x_string; + QString vec_y_string; + QString vec_z_string; + vec_x_string.setNum(vec.x(), 'g', precision); + vec_y_string.setNum(vec.y(), 'g', precision); + vec_z_string.setNum(vec.z(), 'g', precision); + text += "vec (" + vec_x_string + ", " + vec_y_string + ", " + vec_z_string + ")"; + label.setText(text); +} + +// TODO(rsoussan): unify this with version from graph utilities +double AverageDistanceFromMean(const gtsam::Point2Vector& 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; + } + 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 - 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 + +namespace localization_rviz_plugins { +namespace gl = graph_localizer; +namespace lc = localization_common; + +LocalizationGraphPanel::LocalizationGraphPanel(QWidget* parent) : rviz::Panel(parent) { + QHBoxLayout* feature_count_layout = new QHBoxLayout; + of_count_label_ = new QLabel("OF Factors:"); + imu_count_label_ = new QLabel("Imu Factors:"); + loc_count_label_ = new QLabel("Loc Factors:"); + feature_count_layout->addWidget(of_count_label_); + feature_count_layout->addWidget(imu_count_label_); + feature_count_layout->addWidget(loc_count_label_); + + QHBoxLayout* prior_count_layout = new QHBoxLayout; + pose_prior_count_label_ = new QLabel("Pose Priors:"); + velocity_prior_count_label_ = new QLabel("Vel Priors:"); + bias_prior_count_label_ = new QLabel("Bias Priors:"); + prior_count_layout->addWidget(pose_prior_count_label_); + prior_count_layout->addWidget(velocity_prior_count_label_); + prior_count_layout->addWidget(bias_prior_count_label_); + + QHBoxLayout* error_layout = new QHBoxLayout; + total_error_label_ = new QLabel("Total Error:"); + of_error_label_ = new QLabel("OF Error:"); + imu_error_label_ = new QLabel("IMU Error:"); + loc_error_label_ = new QLabel("Loc Error:"); + error_layout->addWidget(total_error_label_); + error_layout->addWidget(of_error_label_); + error_layout->addWidget(imu_error_label_); + error_layout->addWidget(loc_error_label_); + + QHBoxLayout* prior_error_layout = new QHBoxLayout; + pose_prior_error_label_ = new QLabel("Pose Prior Error:"); + velocity_prior_error_label_ = new QLabel("Vel Prior Error:"); + bias_prior_error_label_ = new QLabel("Bias Prior Error:"); + prior_error_layout->addWidget(pose_prior_error_label_); + prior_error_layout->addWidget(velocity_prior_error_label_); + prior_error_layout->addWidget(bias_prior_error_label_); + + QHBoxLayout* of_result_layout = new QHBoxLayout; + of_valid_label_ = new QLabel("OF Valid:"); + of_degenerate_label_ = new QLabel("OF Degenerate:"); + of_result_layout->addWidget(of_valid_label_); + of_result_layout->addWidget(of_degenerate_label_); + + QHBoxLayout* of_result2_layout = new QHBoxLayout; + of_behind_camera_label_ = new QLabel("OF Behind Camera:"); + of_outlier_label_ = new QLabel("OF Outlier:"); + of_far_point_label_ = new QLabel("OF Far Point:"); + of_result2_layout->addWidget(of_behind_camera_label_); + of_result2_layout->addWidget(of_outlier_label_); + of_result2_layout->addWidget(of_far_point_label_); + + QHBoxLayout* of_info_layout = new QHBoxLayout; + of_avg_num_measurements_label_ = new QLabel("OF Avg # Measurements: "); + of_avg_dist_from_mean_label_ = new QLabel("OF Avg Dist From Mean: "); + of_info_layout->addWidget(of_avg_num_measurements_label_); + of_info_layout->addWidget(of_avg_dist_from_mean_label_); + + QHBoxLayout* imu_info_layout = new QHBoxLayout; + imu_avg_dt_label_ = new QLabel("Avg IMU dt: "); + imu_info_layout->addWidget(imu_avg_dt_label_); + + QHBoxLayout* imu_dp_dt_layout = new QHBoxLayout; + imu_avg_dp_dt_label_ = new QLabel("Avg IMU dp/dt: "); + imu_dp_dt_layout->addWidget(imu_avg_dp_dt_label_); + + QHBoxLayout* imu_dv_dt_layout = new QHBoxLayout; + imu_avg_dv_dt_label_ = new QLabel("Avg IMU dv/dt: "); + imu_dv_dt_layout->addWidget(imu_avg_dv_dt_label_); + + QHBoxLayout* graph_latest_layout = new QHBoxLayout; + time_since_latest_label_ = new QLabel("Time since latest: "); + graph_latest_layout->addWidget(time_since_latest_label_); + + QHBoxLayout* latest_velocity_layout = new QHBoxLayout; + latest_velocity_label_ = new QLabel("Latest Vel: "); + latest_velocity_layout->addWidget(latest_velocity_label_); + + QVBoxLayout* layout = new QVBoxLayout; + layout->addLayout(feature_count_layout); + layout->addLayout(prior_count_layout); + layout->addLayout(error_layout); + layout->addLayout(prior_error_layout); + layout->addLayout(of_result_layout); + layout->addLayout(of_result2_layout); + layout->addLayout(of_info_layout); + layout->addLayout(imu_info_layout); + layout->addLayout(imu_dp_dt_layout); + layout->addLayout(imu_dv_dt_layout); + layout->addLayout(graph_latest_layout); + layout->addLayout(latest_velocity_layout); + setLayout(layout); + + graph_sub_ = nh_.subscribe(TOPIC_GRAPH_LOC, 1, &LocalizationGraphPanel::LocalizationGraphCallback, this, + ros::TransportHints().tcpNoDelay()); +} + +void LocalizationGraphPanel::LocalizationGraphCallback(const ff_msgs::LocalizationGraph::ConstPtr& loc_msg) { + // TODO(rsoussan): put these somewhere else! + using Calibration = gtsam::Cal3_S2; + using Camera = gtsam::PinholeCamera; + using SmartFactor = gtsam::RobustSmartProjectionPoseFactor; + + gl::GraphLocalizer graph_localizer; + gtsam::deserializeBinary(loc_msg->serialized_graph, graph_localizer); + graph_localizer.LogOnDestruction(false); + int of_factors = 0; + int imu_factors = 0; + int loc_factors = 0; + int pose_prior_factors = 0; + int velocity_prior_factors = 0; + int bias_prior_factors = 0; + int of_valid = 0; + int of_degenerate = 0; + int of_behind_camera = 0; + int of_outlier = 0; + int of_far_point = 0; + int of_total_num_measurements = 0; + double of_total_avg_dist_from_mean = 0; + double total_imu_dt = 0; + gtsam::Vector3 total_imu_dp_dt = gtsam::Vector3::Zero(); + gtsam::Vector3 total_imu_dv_dt = gtsam::Vector3::Zero(); + double total_error = 0; + double smart_factor_error = 0; + double loc_proj_error = 0; + double imu_factor_error = 0; + double pose_prior_error = 0; + double velocity_prior_error = 0; + double bias_prior_error = 0; + for (const auto factor : graph_localizer.factor_graph()) { + const double error = factor->error(graph_localizer.graph_values().values()); + total_error += error; + const auto smart_factor = dynamic_cast(factor.get()); + if (smart_factor) { + // Using serialized error instead of error due to bug in gtsam serialization. + // TODO(rsoussan): Remove this when gtsam bug is fixed + const double serialized_error = smart_factor->serialized_error(graph_localizer.graph_values().values()); + total_error -= error; + total_error += serialized_error; + smart_factor_error += serialized_error; + ++of_factors; + of_total_num_measurements += smart_factor->measured().size(); + if (smart_factor->isValid()) ++of_valid; + if (smart_factor->isDegenerate()) ++of_degenerate; + if (smart_factor->isPointBehindCamera()) ++of_behind_camera; + if (smart_factor->isOutlier()) ++of_outlier; + if (smart_factor->isFarPoint()) ++of_far_point; + of_total_avg_dist_from_mean += AverageDistanceFromMean(smart_factor->measured()); + } + const auto imu_factor = dynamic_cast(factor.get()); + if (imu_factor) { + imu_factor_error += error; + ++imu_factors; + const double dt = imu_factor->preintegratedMeasurements().deltaTij(); + total_imu_dt += dt; + const auto imu_combined_nav_state = firstCombinedNavState(graph_localizer, imu_factor); + const auto imu_predicted_combined_nav_state = pimPredict(graph_localizer, imu_factor); + if (!imu_predicted_combined_nav_state || !imu_combined_nav_state) { + LogError("LocalizationGraphCallback: Failed to get imu nav state and pim predicted nav state."); + } else { + const gtsam::Vector3 dp = + imu_predicted_combined_nav_state->pose().translation() - imu_combined_nav_state->pose().translation(); + total_imu_dp_dt += dp / dt; + const gtsam::Vector3 dv = imu_predicted_combined_nav_state->velocity() - imu_combined_nav_state->velocity(); + total_imu_dv_dt += dv / dt; + } + } + const auto loc_factor = dynamic_cast*>(factor.get()); + if (loc_factor) { + loc_proj_error += error; + ++loc_factors; + } + // Prior Factors + const auto pose_prior_factor = dynamic_cast*>(factor.get()); + if (pose_prior_factor) { + pose_prior_error += error; + ++pose_prior_factors; + } + const auto velocity_prior_factor = dynamic_cast*>(factor.get()); + if (velocity_prior_factor) { + velocity_prior_error += error; + ++velocity_prior_factors; + } + const auto bias_prior_factor = dynamic_cast*>(factor.get()); + if (bias_prior_factor) { + bias_prior_error += error; + ++bias_prior_factors; + } + } + const auto latest_combined_nav_state = graph_localizer.graph_values().LatestCombinedNavState(); + if (latest_combined_nav_state) { + addVectorToLabel(latest_combined_nav_state->velocity(), "Latest Vel", *latest_velocity_label_, true); + + QString time_since_latest_string; + const auto current_time = lc::TimeFromRosTime(ros::Time::now()); + const double time_since_latest = current_time - latest_combined_nav_state->timestamp(); + time_since_latest_string.setNum(time_since_latest, 'g', 2); + highlightLabel>(time_since_latest, 0.25, 0.35, *time_since_latest_label_); + time_since_latest_label_->setText("Time since latest: " + time_since_latest_string); + } + + // Factor Counts + QString of_count; + of_count.setNum(of_factors); + of_count_label_->setText("OF Factors: " + of_count); + QString imu_count; + imu_count.setNum(imu_factors); + imu_count_label_->setText("IMU Factors: " + imu_count); + QString loc_count; + loc_count.setNum(loc_factors); + loc_count_label_->setText("Loc Factors: " + loc_count); + QString pose_prior_count; + pose_prior_count.setNum(pose_prior_factors); + pose_prior_count_label_->setText("Pose Priors: " + pose_prior_count); + QString velocity_prior_count; + velocity_prior_count.setNum(velocity_prior_factors); + velocity_prior_count_label_->setText("Vel Priors: " + velocity_prior_count); + QString bias_prior_count; + bias_prior_count.setNum(bias_prior_factors); + bias_prior_count_label_->setText("Bias Priors: " + bias_prior_count); + + // Errors + QString total_error_num; + total_error_num.setNum(total_error, 'g', 2); + total_error_label_->setText("Total Error: " + total_error_num); + highlightLabel>(total_error, 10, 50, *total_error_label_); + QString of_error; + of_error.setNum(smart_factor_error, 'g', 2); + of_error_label_->setText("OF Error: " + of_error); + highlightLabel>(smart_factor_error, 10, 50, *of_error_label_); + QString imu_error; + imu_error.setNum(imu_factor_error, 'g', 2); + imu_error_label_->setText("IMU Error: " + imu_error); + QString loc_error; + loc_error.setNum(loc_proj_error, 'g', 2); + loc_error_label_->setText("Loc Error: " + loc_error); + QString pose_prior_error_num; + pose_prior_error_num.setNum(pose_prior_error, 'g', 2); + pose_prior_error_label_->setText("Pose Priors Error: " + pose_prior_error_num); + QString velocity_prior_error_num; + velocity_prior_error_num.setNum(velocity_prior_error, 'g', 2); + velocity_prior_error_label_->setText("Vel Priors Error: " + velocity_prior_error_num); + QString bias_prior_error_num; + bias_prior_error_num.setNum(bias_prior_error, 'g', 2); + bias_prior_error_label_->setText("Bias Priors Error: " + bias_prior_error_num); + + // OF status + if (of_factors > 0) { + QString of_valid_percent; + of_valid_percent.setNum(static_cast(100.0 * of_valid) / of_factors, 'g', 2); + of_valid_label_->setText("OF Valid: " + of_valid_percent + "%"); + const double of_valid_percentage = 100.0 * static_cast(of_valid) / of_factors; + // Green if >= 50% valid, yellow if < 50% and > 0%, red if 0% valid + highlightLabel>(of_valid_percentage, 50, 0.1, *of_valid_label_); + + QString of_degenerate_percent; + of_degenerate_percent.setNum(static_cast(100.0 * of_degenerate) / of_factors, 'g', 2); + of_degenerate_label_->setText("OF Degenerate: " + of_degenerate_percent + "%"); + + QString of_behind_camera_percent; + of_behind_camera_percent.setNum(static_cast(100.0 * of_behind_camera) / of_factors, 'g', 2); + of_behind_camera_label_->setText("OF Behind Camera: " + of_behind_camera_percent + "%"); + + QString of_outlier_percent; + of_outlier_percent.setNum(static_cast(100.0 * of_outlier) / of_factors, 'g', 2); + of_outlier_label_->setText("OF Outlier: " + of_outlier_percent + "%"); + + QString of_far_point_percent; + of_far_point_percent.setNum(static_cast(100.0 * of_far_point) / of_factors, 'g', 2); + of_far_point_label_->setText("OF Far Point: " + of_far_point_percent + "%"); + + QString of_average_num_measurements; + of_average_num_measurements.setNum(static_cast(of_total_num_measurements) / of_factors, 'g', 2); + of_avg_num_measurements_label_->setText("OF Avg # Measurements: " + of_average_num_measurements); + + QString of_average_dist_from_mean; + of_average_dist_from_mean.setNum(static_cast(of_total_avg_dist_from_mean) / of_factors, 'g', 2); + of_avg_dist_from_mean_label_->setText("OF Avg Dist From Mean: " + of_average_dist_from_mean); + } + + // IMU status + if (imu_factors > 0) { + const double imu_avg_dt = total_imu_dt / imu_factors; + QString imu_avg_dt_string; + imu_avg_dt_string.setNum(imu_avg_dt, 'g', 2); + imu_avg_dt_label_->setText("Avg IMU dt: " + imu_avg_dt_string); + // Green if <= 0.1, yellow if <= .3 and > .1, red if > 0.3 + highlightLabel>(imu_avg_dt, 0.1, 0.3, *imu_avg_dt_label_); + const auto imu_avg_dp_dt = total_imu_dp_dt / imu_factors; + addVectorToLabel(imu_avg_dp_dt, "Avg IMU dp/dt", *imu_avg_dp_dt_label_, true); + + const auto imu_avg_dv_dt = total_imu_dv_dt / imu_factors; + addVectorToLabel(imu_avg_dv_dt, "Avg IMU dv/dt", *imu_avg_dv_dt_label_, true); + } +} +} // namespace localization_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(localization_rviz_plugins::LocalizationGraphPanel, rviz::Panel) diff --git a/tools/localization_rviz_plugins/src/localization_graph_panel.h b/tools/localization_rviz_plugins/src/localization_graph_panel.h new file mode 100644 index 0000000000..e80bfc9f84 --- /dev/null +++ b/tools/localization_rviz_plugins/src/localization_graph_panel.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 LOCALIZATION_RVIZ_PLUGINS_LOCALIZATION_GRAPH_PANEL_H // NOLINT +#define LOCALIZATION_RVIZ_PLUGINS_LOCALIZATION_GRAPH_PANEL_H // NOLINT + +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include +#include +#include +#endif + +class QLineEdit; + +namespace localization_rviz_plugins { + +class LocalizationGraphPanel : public rviz::Panel { + Q_OBJECT + public: // NOLINT + explicit LocalizationGraphPanel(QWidget* parent = 0); + public Q_SLOTS: // NOLINT + protected Q_SLOTS: // NOLINT + protected: // NOLINT + void LocalizationGraphCallback(const ff_msgs::LocalizationGraph::ConstPtr& loc_msg); + // Factor Counts + QLabel* of_count_label_; + QLabel* imu_count_label_; + QLabel* loc_count_label_; + QLabel* pose_prior_count_label_; + QLabel* velocity_prior_count_label_; + QLabel* bias_prior_count_label_; + // Errors + QLabel* total_error_label_; + QLabel* of_error_label_; + QLabel* imu_error_label_; + QLabel* loc_error_label_; + QLabel* pose_prior_error_label_; + QLabel* velocity_prior_error_label_; + QLabel* bias_prior_error_label_; + + // OF status + QLabel* of_valid_label_; + QLabel* of_degenerate_label_; + QLabel* of_behind_camera_label_; + QLabel* of_outlier_label_; + QLabel* of_far_point_label_; + // OF info + QLabel* of_avg_num_measurements_label_; + QLabel* of_avg_dist_from_mean_label_; + // IMU info + QLabel* imu_avg_dt_label_; + QLabel* imu_avg_dp_dt_label_; + QLabel* imu_avg_dv_dt_label_; + // Graph Latest + QLabel* latest_velocity_label_; + QLabel* time_since_latest_label_; + + ros::NodeHandle nh_; + ros::Subscriber graph_sub_; +}; +} // namespace localization_rviz_plugins + +#endif // LOCALIZATION_RVIZ_PLUGINS_LOCALIZATION_GRAPH_PANEL_H NOLINT diff --git a/tools/localization_rviz_plugins/src/pose_display.cc b/tools/localization_rviz_plugins/src/pose_display.cc new file mode 100644 index 0000000000..d0c0bb617a --- /dev/null +++ b/tools/localization_rviz_plugins/src/pose_display.cc @@ -0,0 +1,71 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 +#include + +#include + +#include +#include + +#include +#include + +#include + +#include "pose_display.h" // NOLINT +#include "utilities.h" // NOLINT + +namespace localization_rviz_plugins { +namespace lc = localization_common; + +PoseDisplay::PoseDisplay() { + pose_axes_size_.reset(new rviz::FloatProperty("Pose Axes Size", 0.1, "Pose axes size.", this)); + number_of_poses_.reset(new rviz::IntProperty("Number of Poses", 10, "Number of poses to display.", this)); +} + +void PoseDisplay::onInitialize() { MFDClass::onInitialize(); } + +void PoseDisplay::reset() { + MFDClass::reset(); + clearDisplay(); +} + +void PoseDisplay::clearDisplay() { pose_axes_.clear(); } + +void PoseDisplay::processMessage(const geometry_msgs::PoseStamped::ConstPtr& msg) { + pose_axes_.set_capacity(number_of_poses_->getInt()); + const float scale = pose_axes_size_->getFloat(); + const gtsam::Pose3 world_T_body = lc::PoseFromMsg(*msg); + const auto timestamp = lc::RosTimeFromHeader(msg->header); + const auto current_frame_T_world = currentFrameTFrame("world", timestamp, *context_); + if (!current_frame_T_world) { + LogError("processMessage: Failed to get current_frame_T_world."); + return; + } + auto axis = axisFromPose((*current_frame_T_world) * world_T_body, scale, context_->getSceneManager(), scene_node_); + axis->setXColor(Ogre::ColourValue(0.5, 0, 0, 0.3)); + axis->setYColor(Ogre::ColourValue(0, 0.5, 0, 0.3)); + axis->setZColor(Ogre::ColourValue(0, 0, 0.5, 0.3)); + pose_axes_.push_back(std::move(axis)); +} +} // namespace localization_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(localization_rviz_plugins::PoseDisplay, rviz::Display) diff --git a/tools/localization_rviz_plugins/src/pose_display.h b/tools/localization_rviz_plugins/src/pose_display.h new file mode 100644 index 0000000000..f3239459df --- /dev/null +++ b/tools/localization_rviz_plugins/src/pose_display.h @@ -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. + */ + +// Header file must go in src directory for Qt/Rviz plugin +#ifndef LOCALIZATION_RVIZ_PLUGINS_POSE_DISPLAY_H_ // NOLINT +#define LOCALIZATION_RVIZ_PLUGINS_POSE_DISPLAY_H_ // NOLINT + +// Required for Qt +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include +#include +#endif + +#include + +// Forward declarations for ogre and rviz +namespace Ogre { +class SceneNode; +} + +namespace localization_rviz_plugins { + +class PoseDisplay : public rviz::MessageFilterDisplay { + Q_OBJECT // NOLINT + public : // NOLINT + PoseDisplay(); + ~PoseDisplay() = default; + + protected: + void onInitialize() final; + void reset() final; + + private Q_SLOTS: // NOLINT + + private: + void processMessage(const geometry_msgs::PoseStamped::ConstPtr& msg); + void clearDisplay(); + + boost::circular_buffer> pose_axes_; + std::unique_ptr pose_axes_size_; + std::unique_ptr number_of_poses_; +}; +} // namespace localization_rviz_plugins +#endif // LOCALIZATION_RVIZ_PLUGINS_POSE_DISPLAY_H_ NOLINT diff --git a/tools/localization_rviz_plugins/src/slider_property.cc b/tools/localization_rviz_plugins/src/slider_property.cc new file mode 100644 index 0000000000..350deb55bc --- /dev/null +++ b/tools/localization_rviz_plugins/src/slider_property.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 "slider_property.h" // NOLINT + +namespace rviz { +SliderProperty::SliderProperty(const QString& name, const int default_value, const QString& description, + Property* parent, const char* changed_slot, QObject* receiver) + : Property(name, default_value, description, parent, changed_slot, receiver), max_(0), min_(0) {} + +QWidget* SliderProperty::createEditor(QWidget* parent, const QStyleOptionViewItem& /*option*/) { + slider_ = new QSlider(Qt::Horizontal, parent); + slider_->setMaximum(max_); + slider_->setMinimum(min_); + connect(slider_, SIGNAL(valueChanged(int)), this, SLOT(setValue(int))); + return slider_; +} + +void SliderProperty::setValue(const int value) { slider_->setValue(value); } + +void SliderProperty::setMaximum(const int max) { max_ = max; } + +void SliderProperty::setMinimum(const int min) { min_ = min; } + +int SliderProperty::getInt() const { + // TODO(rsoussan): Better way to handle this? Create slider in constructor? + if (slider_ == nullptr) return 0; + return getValue().toInt(); +} +} // end namespace rviz diff --git a/tools/localization_rviz_plugins/src/slider_property.h b/tools/localization_rviz_plugins/src/slider_property.h new file mode 100644 index 0000000000..a2fd2bf9bb --- /dev/null +++ b/tools/localization_rviz_plugins/src/slider_property.h @@ -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. + */ +#ifndef LOCALIZATION_RVIZ_PLUGINS_SLIDER_PROPERTY_H // NOLINT +#define LOCALIZATION_RVIZ_PLUGINS_SLIDER_PROPERTY_H // NOLINT + +#include + +#include + +namespace rviz { +class SliderProperty : public Property { + Q_OBJECT + public: + SliderProperty(const QString& name = QString(), const int default_value = 0, const QString& description = QString(), + Property* parent = nullptr, const char* changed_slot = nullptr, QObject* receiver = nullptr); + + QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option) override; + + void setMaximum(const int max); + + void setMinimum(const int min); + + int getInt() const; + + public Q_SLOTS: // NOLINT + void setValue(const int value); + + Q_SIGNALS: + + protected: + QSlider* slider_; + int max_; + int min_; + int value_; +}; + +} // end namespace rviz + +#endif // LOCALIZATION_RVIZ_PLUGINS_SLIDER_PROPERTY_H // NOLINT diff --git a/tools/localization_rviz_plugins/src/sparse_mapping_display.cc b/tools/localization_rviz_plugins/src/sparse_mapping_display.cc new file mode 100644 index 0000000000..356e80d0f8 --- /dev/null +++ b/tools/localization_rviz_plugins/src/sparse_mapping_display.cc @@ -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. + */ + +#include + +#include +#include + +#include +#include +#include + +#include + +#include "sparse_mapping_display.h" // NOLINT + +namespace localization_rviz_plugins { +SparseMappingDisplay::SparseMappingDisplay() { + int ff_argc = 1; + char argv[] = "sparse_mapping_display"; + char* argv_ptr = &argv[0]; + char** argv_ptr_ptr = &argv_ptr; + ff_common::InitFreeFlyerApplication(&ff_argc, &argv_ptr_ptr); + config_reader::ConfigReader config; + // TODO(rsoussan): Config reader fails to load map string unless a file is added (doesn't matter which one). + config.AddFile("geometry.config"); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + std::string map_file; + if (!config.GetStr("world_vision_map_filename", &map_file)) + LogFatal("SparseMappingDisplay: Failed to load map file."); + map_.reset(new sparse_mapping::SparseMap(map_file, true)); + map_cloud_publisher_ = nh_.advertise("sparse_mapping/map_cloud", 1, true); +} + +void SparseMappingDisplay::onInitialize() { drawMap(); } + +void SparseMappingDisplay::drawMap() { + // TODO(rsoussan): Use pcl point cloud when pcl_ros dependency added + sensor_msgs::PointCloud2 map_cloud; + map_cloud.header = std_msgs::Header(); + map_cloud.header.frame_id = "world"; + map_cloud.height = 1; + map_cloud.width = (map_.get())->GetNumLandmarks(); + map_cloud.fields.resize(3); + map_cloud.fields[0].name = "x"; + map_cloud.fields[0].offset = 0; + map_cloud.fields[0].datatype = 7; + map_cloud.fields[0].count = 1; + map_cloud.fields[1].name = "y"; + map_cloud.fields[1].offset = 4; + map_cloud.fields[1].datatype = 7; + map_cloud.fields[1].count = 1; + map_cloud.fields[2].name = "z"; + map_cloud.fields[2].offset = 8; + map_cloud.fields[2].datatype = 7; + map_cloud.fields[2].count = 1; + map_cloud.is_bigendian = false; + map_cloud.point_step = 12; + map_cloud.row_step = map_cloud.point_step * map_cloud.width; + map_cloud.is_dense = true; + map_cloud.data.resize(map_cloud.row_step); + + for (int i = 0; i < static_cast(map_cloud.width); ++i) { + const Eigen::Vector3f point = ((map_.get())->GetLandmarkPosition(i).cast()); + memcpy(&map_cloud.data[map_cloud.point_step * i + 0], &point.x(), 4); + memcpy(&map_cloud.data[map_cloud.point_step * i + 4], &point.y(), 4); + memcpy(&map_cloud.data[map_cloud.point_step * i + 8], &point.z(), 4); + } + + map_cloud.header.stamp = ros::Time::now(); + // TODO(rsoussan): Use ros point_cloud_common instead of publishing when rviz_default_plugin linker issue fixed + map_cloud_publisher_.publish(map_cloud); +} + +void SparseMappingDisplay::reset() {} +} // namespace localization_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(localization_rviz_plugins::SparseMappingDisplay, rviz::Display) diff --git a/tools/localization_rviz_plugins/src/sparse_mapping_display.h b/tools/localization_rviz_plugins/src/sparse_mapping_display.h new file mode 100644 index 0000000000..874c3bc73c --- /dev/null +++ b/tools/localization_rviz_plugins/src/sparse_mapping_display.h @@ -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. + */ + +// Header file must go in src directory for Qt/Rviz plugin +#ifndef LOCALIZATION_RVIZ_PLUGINS_SPARSE_MAPPING_DISPLAY_H_ // NOLINT +#define LOCALIZATION_RVIZ_PLUGINS_SPARSE_MAPPING_DISPLAY_H_ // NOLINT + +// Required for Qt +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#endif + +// Forward declarations for ogre and rviz +namespace Ogre { +class SceneNode; +} + +namespace localization_rviz_plugins { + +class SparseMappingDisplay : public rviz::Display { + Q_OBJECT // NOLINT + public : // NOLINT + SparseMappingDisplay(); + ~SparseMappingDisplay() = default; + + protected: + void onInitialize() final; + void reset() final; + + private: + void drawMap(); + + std::unique_ptr map_; + // TODO(rosussan): Remove publishing and use point_cloud_common from rviz/default_plugins + // when linking error is fixed + ros::Publisher map_cloud_publisher_; + ros::NodeHandle nh_; +}; +} // namespace localization_rviz_plugins +#endif // LOCALIZATION_RVIZ_PLUGINS_SPARSE_MAPPING_DISPLAY_H_ NOLINT diff --git a/tools/localization_rviz_plugins/src/utilities.cc b/tools/localization_rviz_plugins/src/utilities.cc new file mode 100644 index 0000000000..5060f2aa00 --- /dev/null +++ b/tools/localization_rviz_plugins/src/utilities.cc @@ -0,0 +1,100 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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 + +#include "utilities.h" // NOLINT + +namespace localization_rviz_plugins { +namespace lc = localization_common; +namespace ii = imu_integration; + +Ogre::Vector3 ogrePosition(const gtsam::Pose3& pose) { + return Ogre::Vector3(pose.translation().x(), pose.translation().y(), pose.translation().z()); +} + +Ogre::Vector3 ogrePosition(const gtsam::Point3& point) { return Ogre::Vector3(point.x(), point.y(), point.z()); } + +Ogre::Quaternion ogreQuaternion(const gtsam::Pose3& pose) { + const auto quaternion = pose.rotation().toQuaternion(); + return Ogre::Quaternion(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z()); +} + +boost::optional currentFrameTFrame(const std::string& frame_id, const ros::Time timestamp, + const rviz::DisplayContext& context) { + Ogre::Quaternion orientation; + Ogre::Vector3 position; + if (!context.getFrameManager()->getTransform(frame_id, timestamp, position, orientation)) { + LogError("currentFrameTFrame: Failed to get transform for " << frame_id); + return boost::none; + } + return gtsam::Pose3(gtsam::Rot3(orientation.w, orientation.x, orientation.y, orientation.z), + gtsam::Point3(position.x, position.y, position.z)); +} + +std::unique_ptr axisFromPose(const gtsam::Pose3& pose, const double scale, + Ogre::SceneManager* scene_manager, Ogre::SceneNode* scene_node) { + auto axis = std::unique_ptr(new rviz::Axes(scene_manager, scene_node)); + axis->setPosition(ogrePosition(pose)); + axis->setOrientation(ogreQuaternion(pose)); + axis->setScale(Ogre::Vector3(scale, scale, scale)); + return axis; +} + +boost::optional firstCombinedNavState(const graph_localizer::GraphLocalizer& graph_localizer, + const gtsam::CombinedImuFactor* const imu_factor) { + const auto pose = graph_localizer.graph_values().at(imu_factor->key1()); + if (!pose) { + LogError("pimPredict: Failed to get pose."); + return boost::none; + } + + const auto velocity = graph_localizer.graph_values().at(imu_factor->key2()); + if (!velocity) { + LogError("pimPredict: Failed to get velocity."); + return boost::none; + } + + // TODO(rsoussan): is this correct bias to use??? + const auto bias = graph_localizer.graph_values().at(imu_factor->key5()); + if (!bias) { + LogError("pimPredict: Failed to get bias."); + return boost::none; + } + + return lc::CombinedNavState(*pose, *velocity, *bias, 0 /*Dummy Timestamp*/); +} + +boost::optional pimPredict(const graph_localizer::GraphLocalizer& graph_localizer, + const gtsam::CombinedImuFactor* const imu_factor) { + const auto combined_nav_state(firstCombinedNavState(graph_localizer, imu_factor)); + if (!combined_nav_state) return boost::none; + const auto& pim = imu_factor->preintegratedMeasurements(); + return ii::PimPredict(*combined_nav_state, pim); +} + +std::pair getOrientationAndLength(const gtsam::Point3& point_a, + const gtsam::Point3& point_b) { + // Ogre identity vector is along negative z axis + const Eigen::Vector3d negative_z(0, 0, 1); + const auto normalized_difference_vector = (point_b - point_a).normalized(); + const double norm = (point_b - point_a).norm(); + const auto orientation = Eigen::Quaterniond().setFromTwoVectors(negative_z, normalized_difference_vector); + return {Ogre::Quaternion(orientation.w(), orientation.x(), orientation.y(), orientation.z()), norm}; +} +} // namespace localization_rviz_plugins diff --git a/tools/localization_rviz_plugins/src/utilities.h b/tools/localization_rviz_plugins/src/utilities.h new file mode 100644 index 0000000000..e693dd97b7 --- /dev/null +++ b/tools/localization_rviz_plugins/src/utilities.h @@ -0,0 +1,62 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * 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_RVIZ_PLUGINS_UTILITIES_H_ // NOLINT +#define LOCALIZATION_RVIZ_PLUGINS_UTILITIES_H_ // NOLINT + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace localization_rviz_plugins { +Ogre::Vector3 ogrePosition(const gtsam::Pose3& pose); + +Ogre::Vector3 ogrePosition(const gtsam::Point3& point); + +Ogre::Quaternion ogreQuaternion(const gtsam::Pose3& pose); + +boost::optional currentFrameTFrame(const std::string& frame_id, const ros::Time timestamp, + const rviz::DisplayContext& context); + +std::unique_ptr axisFromPose(const gtsam::Pose3& pose, const double scale, + Ogre::SceneManager* scene_manager, Ogre::SceneNode* scene_node); + +boost::optional firstCombinedNavState( + const graph_localizer::GraphLocalizer& graph_localizer, const gtsam::CombinedImuFactor* const imu_factor); + +boost::optional pimPredict( + const graph_localizer::GraphLocalizer& graph_localizer, const gtsam::CombinedImuFactor* const imu_factor); + +std::pair getOrientationAndLength(const gtsam::Point3& point_a, const gtsam::Point3& point_b); +} // namespace localization_rviz_plugins +#endif // LOCALIZATION_RVIZ_PLUGINS_UTILITIES_H_ NOLINT diff --git a/tools/readme.md b/tools/readme.md index 8ed1c11a45..0ce4200c7b 100644 --- a/tools/readme.md +++ b/tools/readme.md @@ -9,10 +9,16 @@ but are not run on the robot. \subpage gncvisualizer -\subpage rvizvisualizer +\subpage graphbag -\subpage visualeyez +\subpage imubiastester + +\subpage localizationrvizplugins + +\subpage rvizvisualizer \subpage simulator -\subpage dds_profile \ No newline at end of file +\subpage visualeyez + +\subpage dds_profile