Adding visualization tools for target_mapping and logging playback
Helps to see where the targets are being seen, and the respective localizations
Added ability to draw robot frame on visualizer
Change-Id: I8af7a6d84874fe626d8dc9452f77702741e72fbb
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 1cdbe36..0f75aba 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -117,6 +117,7 @@
"//frc971/vision:target_map_fbs",
"//frc971/vision:target_mapper",
"//frc971/vision:vision_fbs",
+ "//frc971/vision:visualize_robot",
"//third_party:opencv",
"//third_party/apriltag",
"//y2023/constants:constants_fbs",
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 95ad541..cf2244a 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -2,6 +2,8 @@
#include "y2023/vision/vision_util.h"
+#include <opencv2/highgui.hpp>
+
DEFINE_bool(
debug, false,
"If true, dump a ton of debug and crash on the first valid detection.");
@@ -26,13 +28,12 @@
: calibration_data_(event_loop),
image_size_(0, 0),
ftrace_(),
- image_callback_(
- event_loop, channel_name,
- [&](cv::Mat image_color_mat,
- const aos::monotonic_clock::time_point eof) {
- HandleImage(image_color_mat, eof);
- },
- chrono::milliseconds(5)),
+ image_callback_(event_loop, channel_name,
+ [&](cv::Mat image_color_mat,
+ const aos::monotonic_clock::time_point eof) {
+ HandleImage(image_color_mat, eof);
+ },
+ chrono::milliseconds(5)),
target_map_sender_(
event_loop->MakeSender<frc971::vision::TargetMap>("/camera")),
image_annotations_sender_(
@@ -54,7 +55,6 @@
calibration_data_.constants(), event_loop->node()->name()->string_view());
extrinsics_ = CameraExtrinsics(calibration_);
-
intrinsics_ = CameraIntrinsics(calibration_);
// Create an undistort projection matrix using the intrinsics
projection_matrix_ = cv::Mat::zeros(3, 4, CV_64F);
@@ -181,6 +181,8 @@
AprilRoboticsDetector::DetectionResult AprilRoboticsDetector::DetectTags(
cv::Mat image, aos::monotonic_clock::time_point eof) {
+ cv::Mat color_image;
+ cvtColor(image, color_image, cv::COLOR_GRAY2RGB);
const aos::monotonic_clock::time_point start_time =
aos::monotonic_clock::now();
@@ -275,10 +277,42 @@
.pose = pose,
.pose_error = pose_error,
.distortion_factor = distortion_factor});
+ if (FLAGS_visualize) {
+ // Draw raw (distorted) corner points in green
+ cv::line(color_image, orig_corner_points[0], orig_corner_points[1],
+ cv::Scalar(0, 255, 0), 2);
+ cv::line(color_image, orig_corner_points[1], orig_corner_points[2],
+ cv::Scalar(0, 255, 0), 2);
+ cv::line(color_image, orig_corner_points[2], orig_corner_points[3],
+ cv::Scalar(0, 255, 0), 2);
+ cv::line(color_image, orig_corner_points[3], orig_corner_points[0],
+ cv::Scalar(0, 255, 0), 2);
+
+ // Draw undistorted corner points in red
+ cv::line(color_image, corner_points[0], corner_points[1],
+ cv::Scalar(0, 0, 255), 2);
+ cv::line(color_image, corner_points[2], corner_points[1],
+ cv::Scalar(0, 0, 255), 2);
+ cv::line(color_image, corner_points[2], corner_points[3],
+ cv::Scalar(0, 0, 255), 2);
+ cv::line(color_image, corner_points[0], corner_points[3],
+ cv::Scalar(0, 0, 255), 2);
+ }
+
+ VLOG(1) << "Found tag number " << det->id << " hamming: " << det->hamming
+ << " margin: " << det->decision_margin;
+
} else {
rejections_++;
}
}
+ if (FLAGS_visualize) {
+ // Display the result
+ // Rotate by 180 degrees to make it upright
+ // TDOO<Jim>: Make this a flag, since we don't want it for box of pis
+ cv::rotate(color_image, color_image, 1);
+ cv::imshow("AprilRoboticsDetector Image", color_image);
+ }
const auto corners_offset = builder.fbb()->CreateVector(foxglove_corners);
foxglove::ImageAnnotations::Builder annotation_builder(*builder.fbb());
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index fab2d30..7caa848 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -11,6 +11,7 @@
#include "frc971/vision/target_map_generated.h"
#include "frc971/vision/target_mapper.h"
#include "frc971/vision/vision_generated.h"
+#include "frc971/vision/visualize_robot.h"
#include "opencv2/core/eigen.hpp"
#include "opencv2/imgproc.hpp"
#include "third_party/apriltag/apriltag.h"
@@ -83,8 +84,8 @@
frc971::vision::ImageCallback image_callback_;
aos::Sender<frc971::vision::TargetMap> target_map_sender_;
aos::Sender<foxglove::ImageAnnotations> image_annotations_sender_;
-
size_t rejections_;
+ frc971::vision::VisualizeRobot vis_robot_;
};
} // namespace vision
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 9352bdd..a110794 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -7,6 +7,7 @@
#include "frc971/vision/calibration_generated.h"
#include "frc971/vision/charuco_lib.h"
#include "frc971/vision/target_mapper.h"
+#include "frc971/vision/visualize_robot.h"
#include "opencv2/aruco.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/core/eigen.hpp"
@@ -28,8 +29,8 @@
"Directory to write solved target map to");
DEFINE_string(field_name, "charged_up",
"Field name, for the output json filename and flatbuffer field");
-DEFINE_int32(team_number, 7971,
- "Use the calibration for a node with this team number");
+DEFINE_int32(team_number, 0,
+ "Required: Use the calibration for a node with this team number");
DEFINE_string(mcap_output_path, "/tmp/log.mcap", "Log to output.");
DEFINE_string(pi, "pi1", "Pi name to generate mcap log for; defaults to pi1.");
DEFINE_double(max_pose_error, 1e-6,
@@ -52,16 +53,22 @@
return H_robot_target;
}
+frc971::vision::VisualizeRobot vis_robot_;
+const int kImageWidth = 1000;
+
// Add detected apriltag poses relative to the robot to
// timestamped_target_detections
void HandleAprilTags(const TargetMap &map,
aos::distributed_clock::time_point pi_distributed_time,
std::vector<DataAdapter::TimestampedDetection>
*timestamped_target_detections,
- Eigen::Affine3d extrinsics) {
+ Eigen::Affine3d extrinsics, std::string node_name,
+ frc971::vision::TargetMapper mapper) {
for (const auto *target_pose_fbs : *map.target_poses()) {
// Skip detections with high pose errors
if (target_pose_fbs->pose_error() > FLAGS_max_pose_error) {
+ LOG(INFO) << " Skipping tag " << target_pose_fbs->id()
+ << " due to pose error of " << target_pose_fbs->pose_error();
continue;
}
@@ -88,6 +95,27 @@
.distance_from_camera = distance_from_camera,
.distortion_factor = distortion_factor,
.id = static_cast<TargetMapper::TargetId>(target_pose.id)});
+
+ if (FLAGS_visualize) {
+ Eigen::Affine3d H_world_target = PoseUtils::Pose3dToAffine3d(
+ mapper.GetTargetPoseById(target_pose_fbs->id())->pose);
+ Eigen::Affine3d H_world_robot = H_world_target * H_robot_target.inverse();
+ Eigen::Affine3d H_world_camera =
+ H_world_target * H_camera_target.inverse();
+ LOG(INFO) << node_name << ", " << target_pose_fbs->id()
+ << ", t = " << pi_distributed_time
+ << ", pose_error = " << target_pose_fbs->pose_error()
+ << ", robot_pos (x,y,z) + "
+ << H_world_robot.translation().transpose();
+ vis_robot_.DrawRobotOutline(
+ H_world_robot, node_name + "-" +
+ std::to_string(target_pose_fbs->id()) + " " +
+ std::to_string(target_pose_fbs->pose_error() /
+ FLAGS_max_pose_error));
+ vis_robot_.DrawFrameAxes(H_world_camera, node_name);
+ vis_robot_.DrawFrameAxes(H_world_target,
+ std::to_string(target_pose_fbs->id()));
+ }
}
}
@@ -109,6 +137,10 @@
detectors->emplace_back(std::move(detector_ptr));
+ ceres::examples::VectorOfConstraints target_constraints;
+ frc971::vision::TargetMapper target_loc_mapper(FLAGS_json_path,
+ target_constraints);
+
mapping_event_loop->MakeWatcher("/camera", [=](const TargetMap &map) {
aos::distributed_clock::time_point pi_distributed_time =
reader->event_loop_factory()
@@ -116,8 +148,16 @@
->ToDistributedClock(aos::monotonic_clock::time_point(
aos::monotonic_clock::duration(map.monotonic_timestamp_ns())));
+ std::string node_name = detection_event_loop->node()->name()->str();
HandleAprilTags(map, pi_distributed_time, timestamped_target_detections,
- extrinsics);
+ extrinsics, node_name, target_loc_mapper);
+ if (FLAGS_visualize) {
+ cv::imshow("View", vis_robot_.image_);
+ cv::waitKey();
+ cv::Mat image_mat =
+ cv::Mat::zeros(cv::Size(kImageWidth, kImageWidth), CV_8UC3);
+ vis_robot_.SetImage(image_mat);
+ }
});
}
@@ -197,7 +237,6 @@
std::unique_ptr<aos::McapLogger> relogger;
if (!FLAGS_mcap_output_path.empty()) {
LOG(INFO) << "Writing out mcap file to " << FLAGS_mcap_output_path;
- // TODO: Should make this work for any pi
const aos::Node *node =
aos::configuration::GetNode(reader.configuration(), FLAGS_pi);
reader.event_loop_factory()->GetNodeEventLoopFactory(node)->OnStartup(
@@ -212,6 +251,14 @@
});
}
+ if (FLAGS_visualize) {
+ cv::Mat image_mat =
+ cv::Mat::zeros(cv::Size(kImageWidth, kImageWidth), CV_8UC3);
+ vis_robot_.SetImage(image_mat);
+ const double kFocalLength = 500.0;
+ vis_robot_.SetDefaultViewpoint(kImageWidth, kFocalLength);
+ }
+
reader.event_loop_factory()->Run();
auto target_constraints =