diff --git a/experimental/beacon_sim/BUILD b/experimental/beacon_sim/BUILD index 79a7674e..960629cd 100644 --- a/experimental/beacon_sim/BUILD +++ b/experimental/beacon_sim/BUILD @@ -516,6 +516,26 @@ cc_test( ] ) +cc_library( + name = "make_eigen_bounds", + srcs = ["make_eigen_bounds.cc"], + hdrs = ["make_eigen_bounds.hh"], + deps = [ + "@eigen", + ":make_belief_updater" + ] +) + +cc_test( + name = "make_eigen_bounds_test", + srcs = ["make_eigen_bounds_test.cc"], + deps = [ + ":make_eigen_bounds", + ":test_helpers", + "@com_google_googletest//:gtest_main", + ] +) + cc_library( name = "robot_belief", hdrs = ["robot_belief.hh"], diff --git a/experimental/beacon_sim/make_eigen_bounds.cc b/experimental/beacon_sim/make_eigen_bounds.cc new file mode 100644 index 00000000..76856b31 --- /dev/null +++ b/experimental/beacon_sim/make_eigen_bounds.cc @@ -0,0 +1,70 @@ +#include "experimental/beacon_sim/make_eigen_bounds.hh" +#include + + +namespace robot::experimental::beacon_sim { + +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 EkfSlamConfig &ekf_config, const EkfSlamEstimate &ekf_estimate, + const std::optional> &available_beacons, + const double max_sensor_range_m, + const TransformType type) { + constexpr double DT_S = 0.5; + constexpr double VELOCITY_MPS = 2.0; + constexpr double ANGULAR_VELOCITY_RADPS = 2.0; + + 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; + start_in_robot.norm() > TOL; + start_in_robot = local_from_new_robot.inverse() * start_state_in_local) { + // Move towards the goal + const liegroups::SE2 old_robot_from_new_robot = [&]() { + const double angle_to_goal_rad = std::atan2(start_in_robot.y(), start_in_robot.x()); + + if (std::abs(angle_to_goal_rad) > TOL) { + // First turn to face the goal + constexpr double MAX_ANGLE_STEP_RAD = DT_S * ANGULAR_VELOCITY_RADPS; + const double angle_step_rad = 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 + 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); + return liegroups::SE2::transX(dist_step_m); + } + }(); + + // Update the mean + local_from_new_robot = local_from_new_robot * old_robot_from_new_robot; + + // Compute the process update for the covariance + 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; + + 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; + + // 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; + + // 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); +} + + +} //namespace \ No newline at end of file diff --git a/experimental/beacon_sim/make_eigen_bounds.hh b/experimental/beacon_sim/make_eigen_bounds.hh new file mode 100644 index 00000000..996e5559 --- /dev/null +++ b/experimental/beacon_sim/make_eigen_bounds.hh @@ -0,0 +1,19 @@ +#pragma once + +#include +#include +#include + +#include "experimental/beacon_sim/make_belief_updater.hh" + +namespace robot::experimental::beacon_sim { + + +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, + const std::optional> &available_beacons, const double max_sensor_range_m, + const TransformType transform_type); + + +} // namespace robot::experimental::beacon_sim diff --git a/experimental/beacon_sim/make_eigen_bounds_test.cc b/experimental/beacon_sim/make_eigen_bounds_test.cc new file mode 100644 index 00000000..cd315762 --- /dev/null +++ b/experimental/beacon_sim/make_eigen_bounds_test.cc @@ -0,0 +1,45 @@ +#include "experimental/beacon_sim/make_eigen_bounds.hh" + +#include "experimental/beacon_sim/test_helpers.hh" +#include "gtest/gtest.h" + +namespace robot::experimental::beacon_sim { + +TEST(MakeEigenBoundsTest, compute_edge_transform_no_measurements) { + // Setup + const EkfSlamConfig ekf_config{ + .max_num_beacons = 1, + .initial_beacon_uncertainty_m = 100.0, + .along_track_process_noise_m_per_rt_meter = 0.05, + .cross_track_process_noise_m_per_rt_meter = 0.05, + .pos_process_noise_m_per_rt_s = 0.0, + .heading_process_noise_rad_per_rt_meter = 1e-3, + .heading_process_noise_rad_per_rt_s = 0.0, + .beacon_pos_process_noise_m_per_rt_s = 1e-6, + .range_measurement_noise_m = 1e-1, + .bearing_measurement_noise_rad = 1e-1, + .on_map_load_position_uncertainty_m = 2.0, + .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); + + // 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); + + // 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()); +} + + +} // namespace robot::experimental::beacon_sim