Adding global_calibration for finding the relative position of the camera inside the robot.
Change-Id: I98a9fb4544f17969c47b519114fb8ecf8e412329
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index 77adc83..1c1fb99 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -58,6 +58,7 @@
double r1 = extrinsics.r1;
double r2 = extrinsics.r2;
double rup = intrinsics.mount_angle;
+ double rbarrel = intrinsics.barrel_mount;
double fl = intrinsics.focal_length;
::Eigen::Matrix<double, 1, 3> pts{pt.x(), pt.y() + y, 0.0};
@@ -93,6 +94,11 @@
pts.transpose();
}
+ // TODO: Maybe barrel should be extrinsics to allow rocking?
+ // Also, in this case, barrel should go above the rotation above?
+ pts = ::Eigen::AngleAxis<double>(rbarrel, ::Eigen::Vector3d(0.0, 0.0, 1.0)) *
+ pts.transpose();
+
// TODO: Final image projection.
::Eigen::Matrix<double, 1, 3> res = pts;