Add support for solving for turrets
Optionally, use turrent positions to apply a rotation around Z to model
the turret. We then need to introduce a rotation from IMU frame to
turret frame (right before the bearing), and a translation as well.
This lets us represent the turret motion.
This is actually underconstrained. We can pick arbitrary turret pivot
points along the turret rotation axis without changing the solution, and
can arbitrarily rotate 0 on the turret the same way. Freeze Z in the
translation and apply a cost to the Z component of the rotation from IMU
to the turret frame to define these.
Change-Id: I3fed35a53e8e77e2c9feca9f5c6c3896b22c6277
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/vision/calibration_accumulator.cc b/frc971/vision/calibration_accumulator.cc
index 89945b0..ac1946c 100644
--- a/frc971/vision/calibration_accumulator.cc
+++ b/frc971/vision/calibration_accumulator.cc
@@ -38,6 +38,18 @@
imu_points_.emplace_back(distributed_now, std::make_pair(gyro, accel));
}
+void CalibrationData::AddTurret(
+ aos::distributed_clock::time_point distributed_now, Eigen::Vector2d state) {
+ // We want the turret to be known too when solving. But, we don't know if we
+ // are going to have a turret until we get the first reading. In that case,
+ // blow away any camera readings from before.
+ while (!rot_trans_points_.empty() &&
+ rot_trans_points_[0].first < distributed_now) {
+ rot_trans_points_.erase(rot_trans_points_.begin());
+ }
+ turret_points_.emplace_back(distributed_now, state);
+}
+
void CalibrationData::ReviewData(CalibrationDataObserver *observer) const {
size_t next_imu_point = 0;
size_t next_camera_point = 0;