diff --git a/experimental/beacon_sim/make_belief_updater.hh b/experimental/beacon_sim/make_belief_updater.hh index 7c30aa88..0a9a884f 100644 --- a/experimental/beacon_sim/make_belief_updater.hh +++ b/experimental/beacon_sim/make_belief_updater.hh @@ -62,6 +62,18 @@ struct EdgeTransform { std::vector weight; TypedTransformVector transforms; }; +Eigen::Matrix3d compute_process_noise(const EkfSlamConfig &config, const double dt_s, const double arclength_m); +TypedTransform compute_process_transform(const Eigen::Matrix3d &process_noise, + const liegroups::SE2 &old_robot_from_new_robot, + const TransformType type); +TypedTransform compute_measurement_transform( + const liegroups::SE2 &local_from_robot, const EkfSlamConfig &ekf_config, + const EkfSlamEstimate &ekf_estimate, const std::optional> &available_beacons, + const double max_sensor_range_m, const TransformType type); + +Eigen::MatrixXd build_measurement_noise(const int num_observations, + const double range_measurement_noise_m, + const double bearing_measurement_noise_rad); std::tuple compute_edge_belief_transform( const liegroups::SE2 &local_from_robot, const Eigen::Vector2d &end_state_in_local, diff --git a/experimental/beacon_sim/make_eigen_bounds.cc b/experimental/beacon_sim/make_eigen_bounds.cc index 76856b31..7f1a5b6c 100644 --- a/experimental/beacon_sim/make_eigen_bounds.cc +++ b/experimental/beacon_sim/make_eigen_bounds.cc @@ -4,9 +4,21 @@ namespace robot::experimental::beacon_sim { + +double step_lower_eigen_bound( // returns a lower bound on min ev of \Omega_{t-1} + const double lower_eigen_value_information, // min ev of \Omega_t + const double upper_eigen_value_dynamics, // max ev of G_t^{-1} G_t^{-T} + const double upper_eigen_value_measurement, // max ev of M_t + const double lower_eigen_value_process_noise // min ev of R_t +) { + return 1. / (upper_eigen_value_dynamics * (1. / (lower_eigen_value_information - upper_eigen_value_measurement) - lower_eigen_value_process_noise)); +} + +/// Start state is the end of the edge +/// End state is the start of the edge (as we move backwards) double compute_backwards_edge_belief_transform( - const liegroups::SE2 &local_from_robot, // end state transform - const Eigen::Vector2d &start_state_in_local, // start state in local + const liegroups::SE2 &local_from_robot, // end node state transform + const Eigen::Vector2d &end_state_in_local, // start node state in local const EkfSlamConfig &ekf_config, const EkfSlamEstimate &ekf_estimate, const std::optional> &available_beacons, const double max_sensor_range_m, @@ -17,23 +29,26 @@ double compute_backwards_edge_belief_transform( liegroups::SE2 local_from_new_robot = local_from_robot; constexpr double TOL = 1e-6; - for (Eigen::Vector2d start_in_robot = local_from_robot.inverse() * start_state_in_local; + for (Eigen::Vector2d start_in_robot = local_from_robot.inverse() * end_state_in_local; start_in_robot.norm() > TOL; - start_in_robot = local_from_new_robot.inverse() * start_state_in_local) { + start_in_robot = local_from_new_robot.inverse() * end_state_in_local) { // Move towards the goal + std::cout << "Starting loop in walk along edge" << std::endl; + std::cout << " start in robot is " << std::endl << start_in_robot << std::endl; const liegroups::SE2 old_robot_from_new_robot = [&]() { - const double angle_to_goal_rad = std::atan2(start_in_robot.y(), start_in_robot.x()); + const double angle_to_goal_rad = std::atan2(start_in_robot.y(), -1.0 * start_in_robot.x()); //angle is 0 when goal is negative x axis + std::cout << " angle to goal is " << angle_to_goal_rad << std::endl; if (std::abs(angle_to_goal_rad) > TOL) { - // First turn to face the goal + // First turn until our back faces the goal constexpr double MAX_ANGLE_STEP_RAD = DT_S * ANGULAR_VELOCITY_RADPS; - const double angle_step_rad = std::copysign( + const double angle_step_rad = -1 * std::copysign( std::min(std::abs(angle_to_goal_rad), MAX_ANGLE_STEP_RAD), angle_to_goal_rad); return liegroups::SE2::rot(angle_step_rad); } else { - // Then drive towards the goal + // Then drive backwards towards the goal constexpr double MAX_DIST_STEP_M = DT_S * VELOCITY_MPS; - const double dist_step_m = std::min(start_in_robot.x(), MAX_DIST_STEP_M); + const double dist_step_m = -1.0 * std::min(std::abs(start_in_robot.x()), MAX_DIST_STEP_M); return liegroups::SE2::transX(dist_step_m); } }(); @@ -45,25 +60,22 @@ double compute_backwards_edge_belief_transform( const Eigen::Matrix3d process_noise_in_robot = compute_process_noise(ekf_config, DT_S, old_robot_from_new_robot.arclength()); - std::cout << "produced process noise " << process_noise_in_robot << std::endl; + std::cout << "produced process noise " << std::endl << process_noise_in_robot << std::endl; const TypedTransform process_transform = compute_process_transform(process_noise_in_robot, old_robot_from_new_robot, type); - std::cout << "produced process transform " << process_transform << std::endl; - + std::cout << "produced process transform " << std::endl << std::get>(process_transform) << std::endl; // Compute the measurement update for the covariance const TypedTransform measurement_transform = compute_measurement_transform(local_from_new_robot, ekf_config, ekf_estimate, available_beacons, max_sensor_range_m, type); - std::cout << "produced measurement transform " << measurement_transform << std::endl; + std::cout << "produced measurement transform " << std::endl << std::get>(measurement_transform) << std::endl; // scattering_transform = (scattering_transform * process_transform).value(); // scattering_transform = (scattering_transform * measurement_transform).value(); } - - return std::make_tuple(liegroups::SE2(local_from_new_robot.so2().log(), end_state_in_local), - scattering_transform); + return 0.0; } diff --git a/experimental/beacon_sim/make_eigen_bounds.hh b/experimental/beacon_sim/make_eigen_bounds.hh index 996e5559..9880215a 100644 --- a/experimental/beacon_sim/make_eigen_bounds.hh +++ b/experimental/beacon_sim/make_eigen_bounds.hh @@ -9,6 +9,14 @@ namespace robot::experimental::beacon_sim { +// https://www.desmos.com/calculator/rbt2nj14ec +double step_lower_eigen_bound( // returns a lower bound on min ev of \Omega_{t-1} + const double lower_eigen_value_information, // min ev of \Omega_t + const double upper_eigen_value_dynamics, // max ev of G_t^{-1} G_t^{-T} + const double upper_eigen_value_measurement, // max ev of M_t + const double lower_eigen_value_process_noise // min ev of R_t +); + double compute_backwards_edge_belief_transform( const liegroups::SE2 &local_from_robot, const Eigen::Vector2d &start_state_in_local, const EkfSlamConfig &ekf_config, const EkfSlamEstimate &ekf_estimate, diff --git a/experimental/beacon_sim/make_eigen_bounds_test.cc b/experimental/beacon_sim/make_eigen_bounds_test.cc index cd315762..8a7fec46 100644 --- a/experimental/beacon_sim/make_eigen_bounds_test.cc +++ b/experimental/beacon_sim/make_eigen_bounds_test.cc @@ -5,6 +5,35 @@ namespace robot::experimental::beacon_sim { +TEST(MakeEigenBoundsTest, step_eigen_bound_no_information) { + // Setup + constexpr double start_info_eigen_value = 1.0; + constexpr double ub_dynamics_eigen_value = 1.0; + constexpr double ub_measurement_eigen_value = 0.0; + constexpr double lower_process_noise_eigen_value = 1.0; + + // Action + double newbound = step_lower_eigen_bound(start_info_eigen_value, ub_dynamics_eigen_value, ub_measurement_eigen_value, lower_process_noise_eigen_value); + + // Verification + // lower bound should increase as more information is required travel the edge (as no info is gained and process noise is added) + ASSERT_GT(newbound, start_info_eigen_value); +} +TEST(MakeEigenBoundsTest, step_eigen_bound_information) { + // Setup + constexpr double start_info_eigen_value = 1.0; + constexpr double ub_dynamics_eigen_value = 1.0; + constexpr double ub_measurement_eigen_value = 1.0; + constexpr double lower_process_noise_eigen_value = 1.0; + + // Action + double newbound = step_lower_eigen_bound(start_info_eigen_value, ub_dynamics_eigen_value, ub_measurement_eigen_value, lower_process_noise_eigen_value); + + // Verification + // lower bound should decrease as information is added along the edge that outweighs the process noise + ASSERT_LT(newbound, start_info_eigen_value); +} + TEST(MakeEigenBoundsTest, compute_edge_transform_no_measurements) { // Setup const EkfSlamConfig ekf_config{ @@ -22,23 +51,24 @@ TEST(MakeEigenBoundsTest, compute_edge_transform_no_measurements) { .on_map_load_heading_uncertainty_rad = 0.5, }; constexpr double MAX_SENSOR_RANGE_M = 3.0; - constexpr int MAX_NUM_TRANSFORMS = 10; const auto &[road_map, ekf_slam, _] = create_grid_environment(ekf_config); constexpr int START_NODE_IDX = 6; constexpr int END_NODE_IDX = 3; const liegroups::SE2 local_from_robot = - liegroups::SE2::trans(road_map.points.at(START_NODE_IDX)); - const Eigen::Vector2d end_pos = road_map.points.at(END_NODE_IDX); + liegroups::SE2::trans(road_map.points.at(END_NODE_IDX)); + + std::cout << "End node position is " << local_from_robot.translation() << std::endl; + const Eigen::Vector2d start_pos = road_map.points.at(START_NODE_IDX); + std::cout << "Start node position is " << start_pos << std::endl; // Action - const auto edge_belief_transform = compute_edge_belief_transform( - local_from_robot, end_pos, ekf_slam.config(), ekf_slam.estimate(), {}, MAX_SENSOR_RANGE_M, - MAX_NUM_TRANSFORMS, TransformType::COVARIANCE); + const auto edge_belief_transform = compute_backwards_edge_belief_transform( + local_from_robot, start_pos, ekf_slam.config(), ekf_slam.estimate(), {}, MAX_SENSOR_RANGE_M, + TransformType::INFORMATION); // Verification - EXPECT_EQ(edge_belief_transform.local_from_robot.translation().x(), end_pos.x()); - EXPECT_EQ(edge_belief_transform.local_from_robot.translation().y(), end_pos.y()); + EXPECT_EQ(edge_belief_transform, 0.0); }