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/y2022/vision/calibrate_extrinsics.cc b/y2022/vision/calibrate_extrinsics.cc
index 05d3481..9b6c428 100644
--- a/y2022/vision/calibrate_extrinsics.cc
+++ b/y2022/vision/calibrate_extrinsics.cc
@@ -2,6 +2,7 @@
#include "Eigen/Geometry"
#include "absl/strings/str_format.h"
#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
#include "aos/init.h"
#include "aos/network/team_number.h"
#include "aos/time/time.h"
@@ -21,6 +22,8 @@
DEFINE_string(target_type, "charuco",
"Type of target: april_tag|aruco|charuco|charuco_diamond");
DEFINE_string(image_channel, "/camera", "Channel to listen for images on");
+DEFINE_string(output_logs, "/tmp/calibration/",
+ "Output folder for visualization logs.");
namespace frc971 {
namespace vision {
@@ -33,11 +36,24 @@
void Main(int argc, char **argv) {
CalibrationData data;
+ std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(FLAGS_pi);
+ CHECK(pi_number);
+ const std::string pi_name = absl::StrCat("pi", *pi_number);
+ LOG(INFO) << "Pi " << *pi_number;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config = [argc, argv,
+ pi_name]() {
+ aos::logger::LogReader reader(
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)));
+ return CalibrationFoxgloveVisualizer::AddVisualizationChannels(
+ reader.logged_configuration(),
+ aos::configuration::GetNode(reader.logged_configuration(), pi_name));
+ }();
{
// Now, accumulate all the data into the data object.
aos::logger::LogReader reader(
- aos::logger::SortParts(aos::logger::FindLogs(argc, argv)));
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)),
+ &config.message());
aos::SimulatedEventLoopFactory factory(reader.configuration());
reader.Register(&factory);
@@ -50,11 +66,8 @@
const aos::Node *const roborio_node =
aos::configuration::GetNode(factory.configuration(), "roborio");
- std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(FLAGS_pi);
- CHECK(pi_number);
- LOG(INFO) << "Pi " << *pi_number;
- const aos::Node *const pi_node = aos::configuration::GetNode(
- factory.configuration(), absl::StrCat("pi", *pi_number));
+ const aos::Node *const pi_node =
+ aos::configuration::GetNode(factory.configuration(), pi_name);
LOG(INFO) << "imu " << aos::FlatbufferToJson(imu_node);
LOG(INFO) << "roboRIO " << aos::FlatbufferToJson(roborio_node);
@@ -67,6 +80,11 @@
std::unique_ptr<aos::EventLoop> pi_event_loop =
factory.MakeEventLoop("calibration", pi_node);
+ std::unique_ptr<aos::EventLoop> logger_loop =
+ factory.MakeEventLoop("logger", pi_node);
+ aos::logger::Logger logger(logger_loop.get());
+ logger.StartLoggingOnRun(FLAGS_output_logs);
+
TargetType target_type = TargetType::kCharuco;
if (FLAGS_target_type == "april_tag") {
target_type = TargetType::kAprilTag;