commit 8f4e43e5efaec4ce5a8d0b25bb365d357cfe8e8e
author milind-u <milind.upadhyay@gmail.com> Sun Apr 09 17:11:19 2023 -0700
committer milind-u <milind.upadhyay@gmail.com> Fri Apr 14 18:00:31 2023 -0700
tree 58f99194c109656da53c1a48793256b310b7ff04
parent 5ec00230799c0a89be3ebe18b9806ae7bb75dfda

Add secondary solver to target mapper

We first already solved for the relative constraints between targets.
Now fit this whole map onto where it should be in the world, by solving
for the transformation between the fixed target pose and where it
actually should be on the field.

Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ib93bc2d5f38a1e282826461a7cd33389f8566021

frc971/vision/target_mapper_test.cc

diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
index cd7d18b..1371f89 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -9,6 +9,9 @@
 #include "glog/logging.h"
 #include "gtest/gtest.h"
 
+DECLARE_int32(min_target_id);
+DECLARE_int32(max_target_id);
+
 namespace frc971::vision {
 
 namespace {
@@ -338,6 +341,9 @@
 }
 
 TEST(TargetMapperTest, TwoTargetsOneConstraint) {
+  FLAGS_min_target_id = 0;
+  FLAGS_max_target_id = 1;
+
   ceres::examples::MapOfPoses target_poses;
   target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
   target_poses[1] = Make2dPose(-5.0, 0.0, 0.0);
@@ -361,6 +367,9 @@
 }
 
 TEST(TargetMapperTest, TwoTargetsTwoConstraints) {
+  FLAGS_min_target_id = 0;
+  FLAGS_max_target_id = 1;
+
   ceres::examples::MapOfPoses target_poses;
   target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
   target_poses[1] = Make2dPose(-5.0, 0.0, -M_PI_2);
@@ -390,6 +399,9 @@
 }
 
 TEST(TargetMapperTest, TwoTargetsOneNoisyConstraint) {
+  FLAGS_min_target_id = 0;
+  FLAGS_max_target_id = 1;
+
   ceres::examples::MapOfPoses target_poses;
   target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
   target_poses[1] = Make2dPose(-5.0, 0.0, 0.0);
@@ -549,33 +561,6 @@
             .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 != 1) {
-      ceres::examples::Pose3d bad_pose{
-          Eigen::Vector3d::Zero(),
-          PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d::Zero())};
-      target_poses[target_id] = bad_pose;
-    }
-  }
-
-  frc971::vision::TargetMapper mapper_bad_poses(target_poses,
-                                                target_constraints);
-  mapper_bad_poses.Solve(kFieldName);
-
-  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