Log charuco corner annotations and images in calibration_accumulator
This makes it so that we can readily use foxglove to visualize at least
the charuco detections in a full log being used for extrinsics
calibration. For future changes we will pull in more.
Change-Id: I3416350698d455c7443c2a6f17867614d2e77c0c
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/frc971/vision/calibration_accumulator.h b/frc971/vision/calibration_accumulator.h
index d9f6065..5c435ad 100644
--- a/frc971/vision/calibration_accumulator.h
+++ b/frc971/vision/calibration_accumulator.h
@@ -8,6 +8,7 @@
#include "aos/time/time.h"
#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/foxglove_image_converter.h"
#include "frc971/wpilib/imu_batch_generated.h"
namespace frc971 {
@@ -76,6 +77,28 @@
turret_points_;
};
+class CalibrationFoxgloveVisualizer {
+ public:
+ CalibrationFoxgloveVisualizer(aos::EventLoop *event_loop);
+
+ static aos::FlatbufferDetachedBuffer<aos::Configuration>
+ AddVisualizationChannels(const aos::Configuration *config,
+ const aos::Node *node);
+
+ void HandleCharuco(const aos::monotonic_clock::time_point eof,
+ std::vector<std::vector<cv::Point2f>> charuco_corners) {
+ auto builder = annotations_sender_.MakeBuilder();
+ builder.CheckOk(
+ builder.Send(BuildAnnotations(eof, charuco_corners, builder.fbb())));
+ }
+
+ private:
+ aos::EventLoop *event_loop_;
+ FoxgloveImageConverter image_converter_;
+
+ aos::Sender<foxglove::ImageAnnotations> annotations_sender_;
+};
+
// Class to register image and IMU callbacks in AOS and route them to the
// corresponding CalibrationData class.
class Calibration {
@@ -110,6 +133,9 @@
CalibrationData *data_;
+ std::unique_ptr<aos::EventLoop> visualizer_event_loop_;
+ CalibrationFoxgloveVisualizer visualizer_;
+
frc971::IMUValuesT last_value_;
};