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();
diff --git a/y2023/localizer/visualization.fbs b/y2023/localizer/visualization.fbs
index 9c9c648..8fa03f0 100644
--- a/y2023/localizer/visualization.fbs
+++ b/y2023/localizer/visualization.fbs
@@ -25,6 +25,8 @@
correction_x: double (id: 12);
correction_y: double (id: 13);
correction_theta: double (id: 14);
+ // The angle between the camera axis and target normal.
+ skew:double (id: 15);
}
table Visualization {