commit ec493910a586e1a73680163b6e8fc92a66cb151a
author Milind Upadhyay <milind.upadhyay@gmail.com> Sun Dec 18 21:33:15 2022 -0800
committer milind-u <milind.upadhyay@gmail.com> Mon Jan 09 18:21:37 2023 -0800
tree 0fe5cc811c9cab7c9c07f5a868f2d49e7ceade92
parent 92819872cd0108db8567ee91b3fe158d9db428d6

Add option to map without robot position

We currently don't have a reliable way of getting the box of pis
position (need to put down estimator on IMU pi, or come up with a
different solution). For testing, we will just assume that two
images captured within a short amount of time have no robot motion
between them.

Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: Iff86e22491411ad2593436b5395f25f401d11367

frc971/vision/target_mapper.h

diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index 4dbe52b..8f83278 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -116,6 +116,16 @@
       const std::vector<TimestampedPose> &timestamped_robot_poses,
       const std::vector<TimestampedDetection> &timestamped_target_detections);
 
+  // Pairs consecutive target detections that are not too far apart in time into
+  // constraints. Meant to be used on a system without a position measurement.
+  // Assumes timestamped_target_detections is in chronological order.
+  // max_dt is the maximum time between two target detections to match them up.
+  // If too much time passes, the recoding device (box of pis) could have moved
+  // too much
+  static std::vector<ceres::examples::Constraint2d> MatchTargetDetections(
+      const std::vector<TimestampedDetection> &timestamped_target_detections,
+      aos::distributed_clock::duration max_dt = std::chrono::milliseconds(5));
+
   // Computes inverse of covariance matrix, assuming there was a target
   // detection between robot movement over the given time period. Ceres calls
   // this matrix the "information"
@@ -137,6 +147,12 @@
       const Eigen::Affine3d &H_robotstart_robotend,
       const TimestampedDetection &target_detection_end,
       const Eigen::Matrix3d &confidence);
+  // Same as above function, but assumes no robot motion between the two
+  // detections
+  static ceres::examples::Constraint2d ComputeTargetConstraint(
+      const TimestampedDetection &target_detection_start,
+      const TimestampedDetection &target_detection_end,
+      const Eigen::Matrix3d &confidence);
 };
 
 }  // namespace frc971::vision