Add skew to localizer visualization
Not using it for filtering yet, but this makes a filter easy to add.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ib0f052ee9cd709d1e5065e284830a9f6d94172f0
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index d0daa7b..32f5604 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -294,6 +294,12 @@
builder.add_implied_robot_y(Z(Corrector::kY));
builder.add_implied_robot_theta(Z(Corrector::kTheta));
+ Eigen::AngleAxisd rvec_camera_target(
+ Eigen::Affine3d(H_camera_target).rotation());
+ // Use y angle (around vertical axis) to compute skew
+ double skew = rvec_camera_target.axis().y() * rvec_camera_target.angle();
+ builder.add_skew(skew);
+
double distance_to_target =
Eigen::Affine3d(H_camera_target).translation().norm();