Use a solver for hub estimation
Uses ceres to estimate the rotation and distance
from the hub to the camera.
Ready for reviewing
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: Idd4fb3334146a0587c713887626cb0abe43cdd4e
diff --git a/y2022/vision/camera_reader.h b/y2022/vision/camera_reader.h
index 949247f..3b1eca6 100644
--- a/y2022/vision/camera_reader.h
+++ b/y2022/vision/camera_reader.h
@@ -17,6 +17,7 @@
#include "y2022/vision/calibration_generated.h"
#include "y2022/vision/gpio.h"
#include "y2022/vision/target_estimate_generated.h"
+#include "y2022/vision/target_estimator.h"
namespace y2022 {
namespace vision {
@@ -36,6 +37,7 @@
camera_calibration_(FindCameraCalibration()),
reader_(reader),
image_sender_(event_loop->MakeSender<CameraImage>("/camera")),
+ target_estimator_(CameraIntrinsics(), CameraExtrinsics()),
target_estimate_sender_(
event_loop->MakeSender<TargetEstimate>("/camera")),
read_image_timer_(event_loop->AddTimer([this]() { ReadImage(); })),
@@ -94,6 +96,7 @@
const calibration::CameraCalibration *const camera_calibration_;
V4L2Reader *const reader_;
aos::Sender<CameraImage> image_sender_;
+ TargetEstimator target_estimator_;
aos::Sender<TargetEstimate> target_estimate_sender_;
// We schedule this immediately to read an image. Having it on a timer