commit a30a4a1cf6a7c14ec166c35f44a13375ad7e1b80
author milind-u <milind.upadhyay@gmail.com> Fri Mar 24 20:49:41 2023 -0700
committer Milind Upadhyay <milind.upadhyay@gmail.com> Sat Mar 25 18:25:35 2023 -0700
tree ad81d775470367b51d5502043c3bf4e5f28bb853
parent 95b677b2ba7d43de79c81d37bde57b3b974b12c9

Allow visualizing detections from multiple pis

That way, we can see how much different pose estimates agree with each
other on where the robot is.

Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ibc5dbe86f5b836ad77b356fa74bc752cc652afab

frc971/vision/visualize_robot.cc

diff --git a/frc971/vision/visualize_robot.cc b/frc971/vision/visualize_robot.cc
index eb2da9a..55932ef 100644
--- a/frc971/vision/visualize_robot.cc
+++ b/frc971/vision/visualize_robot.cc
@@ -1,11 +1,12 @@
 #include "frc971/vision/visualize_robot.h"
-#include "glog/logging.h"
 
 #include <opencv2/calib3d.hpp>
 #include <opencv2/core/eigen.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc.hpp>
 
+#include "glog/logging.h"
+
 namespace frc971 {
 namespace vision {
 
@@ -42,15 +43,17 @@
   return projected_point;
 }
 
-void VisualizeRobot::DrawLine(Eigen::Vector3d start3d, Eigen::Vector3d end3d) {
+void VisualizeRobot::DrawLine(Eigen::Vector3d start3d, Eigen::Vector3d end3d,
+                              cv::Scalar color) {
   cv::Point start2d = ProjectPoint(start3d);
   cv::Point end2d = ProjectPoint(end3d);
 
-  cv::line(image_, start2d, end2d, cv::Scalar(0, 200, 0));
+  cv::line(image_, start2d, end2d, color);
 }
 
 void VisualizeRobot::DrawFrameAxes(Eigen::Affine3d H_world_target,
-                                   std::string label, double axis_scale) {
+                                   std::string label, cv::Scalar label_color,
+                                   double axis_scale) {
   // Map origin to display from global (world) frame to camera frame
   Eigen::Affine3d H_viewpoint_target =
       H_world_viewpoint_.inverse() * H_world_target;
@@ -79,13 +82,13 @@
     label_coord[0] = label_coord[0] / label_coord[2] + 5;
     label_coord[1] = label_coord[1] / label_coord[2];
     cv::putText(image_, label, cv::Point(label_coord[0], label_coord[1]),
-                cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(0, 0, 255));
+                cv::FONT_HERSHEY_PLAIN, 1.0, label_color);
   }
 }
 
 void VisualizeRobot::DrawRobotOutline(Eigen::Affine3d H_world_robot,
-                                      std::string label) {
-  DrawFrameAxes(H_world_robot, label);
+                                      std::string label, cv::Scalar color) {
+  DrawFrameAxes(H_world_robot, label, color);
   const double kBotHalfWidth = 0.75 / 2.0;
   const double kBotHalfLength = 1.0 / 2.0;
   // Compute coordinates for front/rear and right/left corners
@@ -98,10 +101,10 @@
   Eigen::Vector3d rr_corner =
       H_world_robot * Eigen::Vector3d(-kBotHalfLength, kBotHalfWidth, 0);
 
-  DrawLine(fr_corner, fl_corner);
-  DrawLine(fl_corner, rl_corner);
-  DrawLine(rl_corner, rr_corner);
-  DrawLine(rr_corner, fr_corner);
+  DrawLine(fr_corner, fl_corner, color);
+  DrawLine(fl_corner, rl_corner, color);
+  DrawLine(rl_corner, rr_corner, color);
+  DrawLine(rr_corner, fr_corner, color);
 }
 
 }  // namespace vision

frc971/vision/visualize_robot.h

diff --git a/frc971/vision/visualize_robot.h b/frc971/vision/visualize_robot.h
index f5e75af..cd8b4d0 100644
--- a/frc971/vision/visualize_robot.h
+++ b/frc971/vision/visualize_robot.h
@@ -24,6 +24,14 @@
   // Set image on which to draw
   void SetImage(cv::Mat image) { image_ = image; }
 
+  // Sets image to all black.
+  // Uses default_size if no image has been set yet, else image_.size()
+  void ClearImage(cv::Size default_size = cv::Size(1280, 720)) {
+    auto image_size = (image_.data == nullptr ? default_size : image_.size());
+    cv::Mat black_image_mat = cv::Mat::zeros(image_size, CV_8UC3);
+    SetImage(black_image_mat);
+  }
+
   // Set the viewpoint of the camera relative to a global origin
   void SetViewpoint(Eigen::Affine3d view_origin) {
     H_world_viewpoint_ = view_origin;
@@ -49,16 +57,19 @@
   cv::Point ProjectPoint(Eigen::Vector3d point3d);
 
   // Draw a line connecting two 3D points
-  void DrawLine(Eigen::Vector3d start, Eigen::Vector3d end);
+  void DrawLine(Eigen::Vector3d start, Eigen::Vector3d end,
+                cv::Scalar color = cv::Scalar(0, 200, 0));
 
   // Draw coordinate frame for a target frame relative to the world frame
   // Axes are drawn (x,y,z) -> (red, green, blue)
   void DrawFrameAxes(Eigen::Affine3d H_world_target, std::string label = "",
+                     cv::Scalar label_color = cv::Scalar(0, 0, 255),
                      double axis_scale = 0.25);
 
   // TODO<Jim>: Also implement DrawBoardOutline?  Maybe one function w/
   // parameters?
-  void DrawRobotOutline(Eigen::Affine3d H_world_robot, std::string label = "");
+  void DrawRobotOutline(Eigen::Affine3d H_world_robot, std::string label = "",
+                        cv::Scalar color = cv::Scalar(0, 200, 0));
 
   Eigen::Affine3d H_world_viewpoint_;  // Where to view the world from
   cv::Mat image_;                      // Image to draw on