frc971/vision/target_mapper_test.cc

#include "frc971/vision/target_mapper.h"

#include <random>

#include "aos/events/simulated_event_loop.h"
#include "aos/testing/random_seed.h"
#include "glog/logging.h"
#include "gtest/gtest.h"

namespace frc971::vision {

namespace {
constexpr double kToleranceMeters = 0.05;
constexpr double kToleranceRadians = 0.05;
}  // namespace

#define EXPECT_POSE_NEAR(pose1, pose2)             \
  EXPECT_NEAR(pose1.x, pose2.x, kToleranceMeters); \
  EXPECT_NEAR(pose1.y, pose2.y, kToleranceMeters); \
  EXPECT_NEAR(pose1.yaw_radians, pose2.yaw_radians, kToleranceRadians);

#define EXPECT_POSE_EQ(pose1, pose2)  \
  EXPECT_DOUBLE_EQ(pose1.x, pose2.x); \
  EXPECT_DOUBLE_EQ(pose1.y, pose2.y); \
  EXPECT_DOUBLE_EQ(pose1.yaw_radians, pose2.yaw_radians);

#define EXPECT_BETWEEN_EXCLUSIVE(value, a, b) \
  {                                           \
    auto low = std::min(a, b);                \
    auto high = std::max(a, b);               \
    EXPECT_GT(value, low);                    \
    EXPECT_LT(value, high);                   \
  }

namespace {
// Expects angles to be normalized
double DeltaAngle(double a, double b) {
  double delta = std::abs(a - b);
  return std::min(delta, (2.0 * M_PI) - delta);
}
}  // namespace

// Expects angles to be normalized
#define EXPECT_ANGLE_BETWEEN_EXCLUSIVE(theta, a, b)  \
  EXPECT_LT(DeltaAngle(a, theta), DeltaAngle(a, b)); \
  EXPECT_LT(DeltaAngle(b, theta), DeltaAngle(a, b));

#define EXPECT_POSE_IN_RANGE(interpolated_pose, pose_start, pose_end)      \
  EXPECT_BETWEEN_EXCLUSIVE(interpolated_pose.x, pose_start.x, pose_end.x); \
  EXPECT_BETWEEN_EXCLUSIVE(interpolated_pose.y, pose_start.y, pose_end.y); \
  EXPECT_ANGLE_BETWEEN_EXCLUSIVE(interpolated_pose.yaw_radians,            \
                                 pose_start.yaw_radians,                   \
                                 pose_end.yaw_radians);

// Both confidence matrixes should have the same dimensions and be square
#define EXPECT_CONFIDENCE_GT(confidence1, confidence2) \
  {                                                    \
    ASSERT_EQ(confidence1.rows(), confidence2.rows()); \
    ASSERT_EQ(confidence1.rows(), confidence1.cols()); \
    ASSERT_EQ(confidence2.rows(), confidence2.cols()); \
    for (size_t i = 0; i < confidence1.rows(); i++) {  \
      EXPECT_GT(confidence1(i, i), confidence2(i, i)); \
    }                                                  \
  }

namespace {
ceres::examples::Pose2d MakePose(double x, double y, double yaw_radians) {
  return ceres::examples::Pose2d{x, y, yaw_radians};
}

bool TargetIsInView(TargetMapper::TargetPose target_detection) {
  // And check if it is within the fov of the robot /
  // camera, assuming camera is pointing in the
  // positive x-direction of the robot
  double angle_to_target =
      atan2(target_detection.pose.y, target_detection.pose.x);

  // Simulated camera field of view, in radians
  constexpr double kCameraFov = M_PI_2;
  if (fabs(angle_to_target) <= kCameraFov / 2.0) {
    VLOG(2) << "Found target in view, based on T = " << target_detection.pose.x
            << ", " << target_detection.pose.y << " with angle "
            << angle_to_target;
    return true;
  } else {
    return false;
  }
}

aos::distributed_clock::time_point TimeInMs(size_t ms) {
  return aos::distributed_clock::time_point(std::chrono::milliseconds(ms));
}

}  // namespace

TEST(DataAdapterTest, Interpolation) {
  std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
      {TimeInMs(0), ceres::examples::Pose2d{1.0, 2.0, 0.0}},
      {TimeInMs(5), ceres::examples::Pose2d{1.0, 2.0, 0.0}},
      {TimeInMs(10), ceres::examples::Pose2d{3.0, 1.0, M_PI_2}},
      {TimeInMs(15), ceres::examples::Pose2d{5.0, -2.0, -M_PI}},
      {TimeInMs(20), ceres::examples::Pose2d{5.0, -2.0, -M_PI}},
      {TimeInMs(25), ceres::examples::Pose2d{10.0, -32.0, M_PI_2}},
      {TimeInMs(30), ceres::examples::Pose2d{-15.0, 12.0, 0.0}},
      {TimeInMs(35), ceres::examples::Pose2d{-15.0, 12.0, 0.0}}};
  std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
      {{TimeInMs(5),
        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
        0},
       {TimeInMs(9),
        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
        1},
       {TimeInMs(9),
        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
        2},
       {TimeInMs(15),
        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
        0},
       {TimeInMs(16),
        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
        2},
       {TimeInMs(27),
        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
        1}};
  auto [target_constraints, robot_delta_poses] =
      DataAdapter::MatchTargetDetections(timestamped_robot_poses,
                                         timestamped_target_detections);

  // Check that target constraints got inserted in the
  // correct spots
  EXPECT_EQ(target_constraints.size(),
            timestamped_target_detections.size() - 1);
  for (auto it = target_constraints.begin(); it < target_constraints.end();
       it++) {
    auto timestamped_it = timestamped_target_detections.begin() +
                          (it - target_constraints.begin());
    EXPECT_EQ(it->id_begin, timestamped_it->id);
    EXPECT_EQ(it->id_end, (timestamped_it + 1)->id);
  }

  // Check that poses were interpolated correctly.
  // Keep track of the computed robot pose by adding the delta poses
  auto computed_robot_pose = timestamped_robot_poses[1].pose;

  computed_robot_pose =
      PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[0]);
  EXPECT_POSE_IN_RANGE(computed_robot_pose, timestamped_robot_poses[1].pose,
                       timestamped_robot_poses[2].pose);

  computed_robot_pose =
      PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[1]);
  EXPECT_POSE_IN_RANGE(computed_robot_pose, timestamped_robot_poses[1].pose,
                       timestamped_robot_poses[2].pose);
  EXPECT_POSE_EQ(robot_delta_poses[1], MakePose(0.0, 0.0, 0.0));

  computed_robot_pose =
      PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[2]);
  EXPECT_POSE_EQ(computed_robot_pose, timestamped_robot_poses[3].pose);

  computed_robot_pose =
      PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[3]);
  EXPECT_POSE_EQ(computed_robot_pose, timestamped_robot_poses[4].pose);

  computed_robot_pose =
      PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[4]);
  EXPECT_POSE_IN_RANGE(computed_robot_pose, timestamped_robot_poses[5].pose,
                       timestamped_robot_poses[6].pose);

  // Check the confidence matrices. Don't check the actual values
  // in case the constants change, just check the confidence of contraints
  // relative to each other, as constraints over longer time periods should have
  // lower confidence.
  const auto confidence_0ms =
      DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(0));
  const auto confidence_1ms =
      DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(1));
  const auto confidence_4ms =
      DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(4));
  const auto confidence_6ms =
      DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(6));
  const auto confidence_11ms =
      DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(11));

  // Check relative magnitude of different confidences.
  // Confidences for 0-5ms, 5-10ms, and 10-15ms periods are equal
  // because they fit within one control loop iteration.
  EXPECT_EQ(confidence_0ms, confidence_1ms);
  EXPECT_EQ(confidence_1ms, confidence_4ms);
  EXPECT_CONFIDENCE_GT(confidence_4ms, confidence_6ms);
  EXPECT_CONFIDENCE_GT(confidence_6ms, confidence_11ms);

  // Check that confidences (information) of actual constraints are correct
  EXPECT_EQ(target_constraints[0].information, confidence_4ms);
  EXPECT_EQ(target_constraints[1].information, confidence_0ms);
  EXPECT_EQ(target_constraints[2].information, confidence_6ms);
  EXPECT_EQ(target_constraints[3].information, confidence_1ms);
  EXPECT_EQ(target_constraints[4].information, confidence_11ms);
}

TEST(TargetMapperTest, TwoTargetsOneConstraint) {
  std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
  target_poses[0] = ceres::examples::Pose2d{5.0, 0.0, M_PI};
  target_poses[1] = ceres::examples::Pose2d{-5.0, 0.0, 0.0};

  std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
      {TimeInMs(5), ceres::examples::Pose2d{2.0, 0.0, 0.0}},
      {TimeInMs(10), ceres::examples::Pose2d{-1.0, 0.0, 0.0}},
      {TimeInMs(15), ceres::examples::Pose2d{-1.0, 0.0, 0.0}}};
  std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
      {{TimeInMs(5),
        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{3.0, 0.0, M_PI}),
        0},
       {TimeInMs(10),
        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{-4.0, 0.0, 0.0}),
        1}};
  auto target_constraints =
      DataAdapter::MatchTargetDetections(timestamped_robot_poses,
                                         timestamped_target_detections)
          .first;

  frc971::vision::TargetMapper mapper(target_poses, target_constraints);
  mapper.Solve();

  ASSERT_EQ(mapper.target_poses().size(), 2);
  EXPECT_POSE_NEAR(mapper.target_poses()[0], MakePose(5.0, 0.0, M_PI));
  EXPECT_POSE_NEAR(mapper.target_poses()[1], MakePose(-5.0, 0.0, 0.0));
}

TEST(TargetMapperTest, TwoTargetsTwoConstraints) {
  std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
  target_poses[0] = ceres::examples::Pose2d{5.0, 0.0, M_PI};
  target_poses[1] = ceres::examples::Pose2d{-5.0, 0.0, -M_PI_2};

  std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
      {TimeInMs(5), ceres::examples::Pose2d{-1.0, 0.0, 0.0}},
      {TimeInMs(10), ceres::examples::Pose2d{3.0, 0.0, 0.0}},
      {TimeInMs(15), ceres::examples::Pose2d{4.0, 0.0, 0.0}},
      {TimeInMs(20), ceres::examples::Pose2d{-1.0, 0.0, 0.0}}};
  std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
      {{TimeInMs(5),
        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{6.0, 0.0, M_PI}),
        0},
       {TimeInMs(10),
        PoseUtils::Pose2dToAffine3d(
            ceres::examples::Pose2d{-8.0, 0.0, -M_PI_2}),
        1},
       {TimeInMs(15),
        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{1.0, 0.0, M_PI}),
        0}};
  auto target_constraints =
      DataAdapter::MatchTargetDetections(timestamped_robot_poses,
                                         timestamped_target_detections)
          .first;

  frc971::vision::TargetMapper mapper(target_poses, target_constraints);
  mapper.Solve();

  ASSERT_EQ(mapper.target_poses().size(), 2);
  EXPECT_POSE_NEAR(mapper.target_poses()[0], MakePose(5.0, 0.0, M_PI));
  EXPECT_POSE_NEAR(mapper.target_poses()[1], MakePose(-5.0, 0.0, -M_PI_2));
}

TEST(TargetMapperTest, TwoTargetsOneNoisyConstraint) {
  std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
  target_poses[0] = ceres::examples::Pose2d{5.0, 0.0, M_PI};
  target_poses[1] = ceres::examples::Pose2d{-5.0, 0.0, 0.0};

  std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
      {TimeInMs(5), ceres::examples::Pose2d{1.99, 0.0, 0.0}},
      {TimeInMs(10), ceres::examples::Pose2d{-1.0, 0.0, 0.0}},
      {TimeInMs(15), ceres::examples::Pose2d{-1.01, -0.01, 0.004}}};
  std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
      {{TimeInMs(5),
        PoseUtils::Pose2dToAffine3d(
            ceres::examples::Pose2d{3.01, 0.001, M_PI - 0.001}),
        0},
       {TimeInMs(10),
        PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{-4.01, 0.0, 0.0}),
        1}};

  auto target_constraints =
      DataAdapter::MatchTargetDetections(timestamped_robot_poses,
                                         timestamped_target_detections)
          .first;

  frc971::vision::TargetMapper mapper(target_poses, target_constraints);
  mapper.Solve();

  ASSERT_EQ(mapper.target_poses().size(), 2);
  EXPECT_POSE_NEAR(mapper.target_poses()[0], MakePose(5.0, 0.0, M_PI));
  EXPECT_POSE_NEAR(mapper.target_poses()[1], MakePose(-5.0, 0.0, 0.0));
}

TEST(TargetMapperTest, MultiTargetCircleMotion) {
  // Build set of target locations wrt global origin
  // For simplicity, do this on a grid of the field
  double field_half_length = 7.5;  // half length of the field
  double field_half_width = 5.0;   // half width of the field
  std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
  std::vector<TargetMapper::TargetPose> actual_target_poses;
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      TargetMapper::TargetId target_id = i * 3 + j;
      TargetMapper::TargetPose target_pose{
          target_id, ceres::examples::Pose2d{field_half_length * (1 - i),
                                             field_half_width * (1 - j), 0.0}};
      actual_target_poses.emplace_back(target_pose);
      target_poses[target_id] = target_pose.pose;
      VLOG(2) << "VERTEX_SE2 " << target_id << " " << target_pose.pose.x << " "
              << target_pose.pose.y << " " << target_pose.pose.yaw_radians;
    }
  }

  // Now, create a bunch of robot poses and target
  // observations
  size_t dt = 1;

  std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses;
  std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections;

  constexpr size_t kTotalSteps = 100;
  for (size_t step_count = 0; step_count < kTotalSteps; step_count++) {
    size_t t = dt * step_count;
    // Circle clockwise around the center of the field
    double robot_theta = t;
    double robot_x = (field_half_length / 2.0) * cos(robot_theta);
    double robot_y = (-field_half_width / 2.0) * sin(robot_theta);

    ceres::examples::Pose2d robot_pose{robot_x, robot_y, robot_theta};
    for (TargetMapper::TargetPose target_pose : actual_target_poses) {
      TargetMapper::TargetPose target_detection = {
          .id = target_pose.id,
          .pose = PoseUtils::ComputeRelativePose(robot_pose, target_pose.pose)};
      if (TargetIsInView(target_detection)) {
        // Define random generator with Gaussian
        // distribution
        const double mean = 0.0;
        const double stddev = 1.0;
        // Can play with this to see how it impacts
        // randomness
        constexpr double kNoiseScale = 0.01;
        std::default_random_engine generator(aos::testing::RandomSeed());
        std::normal_distribution<double> dist(mean, stddev);

        target_detection.pose.x += dist(generator) * kNoiseScale;
        target_detection.pose.y += dist(generator) * kNoiseScale;
        robot_pose.x += dist(generator) * kNoiseScale;
        robot_pose.y += dist(generator) * kNoiseScale;

        auto time_point =
            aos::distributed_clock::time_point(std::chrono::milliseconds(t));
        timestamped_robot_poses.emplace_back(DataAdapter::TimestampedPose{
            .time = time_point, .pose = robot_pose});
        timestamped_target_detections.emplace_back(
            DataAdapter::TimestampedDetection{
                .time = time_point,
                .H_robot_target =
                    PoseUtils::Pose2dToAffine3d(target_detection.pose),
                .id = target_detection.id});
      }
    }
  }

  {
    // Add in a robot pose after all target poses
    auto final_robot_pose =
        timestamped_robot_poses[timestamped_robot_poses.size() - 1];
    timestamped_robot_poses.emplace_back(DataAdapter::TimestampedPose{
        .time = final_robot_pose.time + std::chrono::milliseconds(dt),
        .pose = final_robot_pose.pose});
  }

  auto target_constraints =
      DataAdapter::MatchTargetDetections(timestamped_robot_poses,
                                         timestamped_target_detections)
          .first;
  frc971::vision::TargetMapper mapper(target_poses, target_constraints);
  mapper.Solve();

  for (auto [target_pose_id, mapper_target_pose] : mapper.target_poses()) {
    TargetMapper::TargetPose actual_target_pose =
        TargetMapper::GetTargetPoseById(actual_target_poses, target_pose_id)
            .value();
    EXPECT_POSE_NEAR(mapper_target_pose, actual_target_pose.pose);
  }

  //
  // See what happens when we don't start with the
  // correct values
  //
  for (auto [target_id, target_pose] : target_poses) {
    // Skip first pose, since that needs to be correct
    // and is fixed in the solver
    if (target_id != 0) {
      ceres::examples::Pose2d bad_pose{0.0, 0.0, M_PI / 2.0};
      target_poses[target_id] = bad_pose;
    }
  }

  frc971::vision::TargetMapper mapper_bad_poses(target_poses,
                                                target_constraints);
  mapper_bad_poses.Solve();

  for (auto [target_pose_id, mapper_target_pose] :
       mapper_bad_poses.target_poses()) {
    TargetMapper::TargetPose actual_target_pose =
        TargetMapper::GetTargetPoseById(actual_target_poses, target_pose_id)
            .value();
    EXPECT_POSE_NEAR(mapper_target_pose, actual_target_pose.pose);
  }
}

}  // namespace frc971::vision