Fix localizer when not in simulation
Not quite sure how this worked even somewhat...
Change-Id: I4e5f3fca0976512f2fd072bb619a997c85c2c714
diff --git a/y2019/control_loops/drivetrain/localizer.h b/y2019/control_loops/drivetrain/localizer.h
index f8849ce..b717837 100644
--- a/y2019/control_loops/drivetrain/localizer.h
+++ b/y2019/control_loops/drivetrain/localizer.h
@@ -237,7 +237,7 @@
// Compute the ckalman update for this step:
const TargetView &view = camera_views[jj];
const Eigen::Matrix<Scalar, kNOutputs, kNStates> H =
- HMatrix(*view.target, target_view);
+ HMatrix(*view.target, camera.pose());
const Eigen::Matrix<Scalar, kNStates, kNOutputs> PH = P * H.transpose();
const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> S = H * PH + R;
// Note: The inverse here should be very cheap so long as kNOutputs = 3.
@@ -354,11 +354,11 @@
}
Eigen::Matrix<Scalar, kNOutputs, kNStates> HMatrix(
- const Target &target, const TargetView &target_view) {
+ const Target &target, const Pose &camera_pose) {
// To calculate dheading/d{x,y,theta}:
// heading = arctan2(target_pos - camera_pos) - camera_theta
Eigen::Matrix<Scalar, 3, 1> target_pos = target.pose().abs_pos();
- Eigen::Matrix<Scalar, 3, 1> camera_pos = target_view.camera_pose.abs_pos();
+ Eigen::Matrix<Scalar, 3, 1> camera_pos = camera_pose.abs_pos();
Scalar diffx = target_pos.x() - camera_pos.x();
Scalar diffy = target_pos.y() - camera_pos.y();
Scalar norm2 = diffx * diffx + diffy * diffy;