Print out robot relative xyz of target in debug_viewer
Change-Id: Ife6d6d3a31a5d519137b491d908b63b26ff089c5
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index 9f1d4dd..4f0e747 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -80,6 +80,7 @@
}
bool HandleBlobs(BlobList imgs, ImageFormat fmt) override {
+ const CameraGeometry camera_geometry = GetCamera(FLAGS_camera)->geometry;
imgs_last_ = imgs;
fmt_last_ = fmt;
// reset for next drawing cycle
@@ -174,8 +175,23 @@
// Check that our current results match possible solutions.
results = target_finder_.FilterResults(results, 0, draw_results_);
if (draw_results_) {
- for (const IntermediateResult &res : results) {
- DrawTarget(res, {0, 255, 0});
+ for (const IntermediateResult &result : results) {
+ ::std::cout << "Found target x: "
+ << camera_geometry.location[0] +
+ ::std::cos(camera_geometry.heading +
+ result.extrinsics.r2) *
+ result.extrinsics.z
+ << ::std::endl;
+ ::std::cout << "Found target y: "
+ << camera_geometry.location[1] +
+ ::std::sin(camera_geometry.heading +
+ result.extrinsics.r2) *
+ result.extrinsics.z
+ << ::std::endl;
+ ::std::cout << "Found target z: "
+ << camera_geometry.location[2] + result.extrinsics.y
+ << ::std::endl;
+ DrawTarget(result, {0, 255, 0});
}
}