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.cc

diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index b4b1a87..ab4ee6d 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -163,6 +163,41 @@
   return {target_constraints, robot_delta_poses};
 }
 
+std::vector<ceres::examples::Constraint2d> DataAdapter::MatchTargetDetections(
+    const std::vector<DataAdapter::TimestampedDetection>
+        &timestamped_target_detections,
+    aos::distributed_clock::duration max_dt) {
+  CHECK_GE(timestamped_target_detections.size(), 2ul)
+      << "Must have at least 2 detections";
+
+  // Match consecutive detections
+  std::vector<ceres::examples::Constraint2d> target_constraints;
+  for (auto it = timestamped_target_detections.begin() + 1;
+       it < timestamped_target_detections.end(); it++) {
+    auto last_detection = *(it - 1);
+
+    // Skip two consecutive detections of the same target, because the solver
+    // doesn't allow this
+    if (it->id == last_detection.id) {
+      continue;
+    }
+
+    // Don't take into account constraints too far apart in time, because the
+    // recording device could have moved too much
+    if ((it->time - last_detection.time) > max_dt) {
+      continue;
+    }
+
+    // TODO(milind): better way to compute confidence since these detections are
+    // likely very close in time together
+    auto confidence = ComputeConfidence(last_detection.time, it->time);
+    target_constraints.emplace_back(
+        ComputeTargetConstraint(last_detection, *it, confidence));
+  }
+
+  return target_constraints;
+}
+
 Eigen::Matrix3d DataAdapter::ComputeConfidence(
     aos::distributed_clock::time_point start,
     aos::distributed_clock::time_point end) {
@@ -217,6 +252,15 @@
       target_constraint.yaw_radians, confidence};
 }
 
+ceres::examples::Constraint2d DataAdapter::ComputeTargetConstraint(
+    const TimestampedDetection &target_detection_start,
+    const TimestampedDetection &target_detection_end,
+    const Eigen::Matrix3d &confidence) {
+  return ComputeTargetConstraint(target_detection_start,
+                                 Eigen::Affine3d(Eigen::Matrix4d::Identity()),
+                                 target_detection_end, confidence);
+}
+
 TargetMapper::TargetMapper(
     std::string_view target_poses_path,
     std::vector<ceres::examples::Constraint2d> target_constraints)