Make target mapping work with aprilrobotics
- Use a separate event loop for detecting and mapping (one makes a
sender and one makes a watcher)
- Remap target map channel so we can send those while replaying
- Improving debugging messages
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I52f2b234b7f53db77b45126fc2a4ea8e07baa0c8
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index b3049e4..01a8df0 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -189,11 +189,14 @@
ceres::examples::Pose3d target_constraint =
PoseUtils::Affine3dToPose3d(H_targetstart_targetend);
- return ceres::examples::Constraint3d{
- target_detection_start.id,
- target_detection_end.id,
- {target_constraint.p, target_constraint.q},
- confidence};
+ const auto constraint_3d =
+ ceres::examples::Constraint3d{target_detection_start.id,
+ target_detection_end.id,
+ {target_constraint.p, target_constraint.q},
+ confidence};
+
+ VLOG(2) << "Computed constraint: " << constraint_3d;
+ return constraint_3d;
}
TargetMapper::TargetMapper(