Draw projected hub on final image in viewer
Before it only drew while animating the solving process
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I712bfb1d4dfc8ab327b447e61d55f868f4b250fb
diff --git a/y2022/vision/target_estimator.h b/y2022/vision/target_estimator.h
index b509a2e..174f774 100644
--- a/y2022/vision/target_estimator.h
+++ b/y2022/vision/target_estimator.h
@@ -47,9 +47,7 @@
inline double confidence() const { return confidence_; }
- // Draws the distance, angle, and rotation on the given image
- static void DrawEstimate(const TargetEstimate &target_estimate,
- cv::Mat view_image);
+ // Draws the distance, angle, rotation, and projected tape on the given image
void DrawEstimate(cv::Mat view_image) const;
private:
@@ -59,16 +57,24 @@
// clockwise around the rectangle
static const std::array<cv::Point3d, 4> kMiddleTapePiecePoints;
+ // Computes matrix of hub in camera's frame
+ template <typename S>
+ Eigen::Transform<S, 3, Eigen::Affine> ComputeHubCameraTransform(
+ S roll, S pitch, S yaw, S distance, S theta, S camera_height) const;
+
template <typename S>
cv::Point_<S> ProjectToImage(
cv::Point3d tape_point_hub,
- Eigen::Transform<S, 3, Eigen::Affine> &H_hub_camera) const;
+ const Eigen::Transform<S, 3, Eigen::Affine> &H_hub_camera) const;
template <typename S>
cv::Point_<S> DistanceFromTape(
size_t centroid_index,
const std::vector<cv::Point_<S>> &tape_points) const;
+ void DrawProjectedHub(const std::vector<cv::Point2d> &tape_points_proj,
+ cv::Mat view_image) const;
+
std::vector<BlobDetector::BlobStats> blob_stats_;
size_t middle_blob_index_;
std::optional<cv::Mat> image_;