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.h b/frc971/vision/calibration_accumulator.h
index 6d42708..e4f9c8a 100644
--- a/frc971/vision/calibration_accumulator.h
+++ b/frc971/vision/calibration_accumulator.h
@@ -26,6 +26,11 @@
// corresponding angular velocity and linear acceleration vectors wa.
virtual void UpdateIMU(aos::distributed_clock::time_point t,
std::pair<Eigen::Vector3d, Eigen::Vector3d> wa) = 0;
+
+ // Observes a turret sample at the corresponding time t, and with the
+ // corresponding state.
+ virtual void UpdateTurret(aos::distributed_clock::time_point t,
+ Eigen::Vector2d state) = 0;
};
// Class to both accumulate and replay camera and IMU data in time order.
@@ -40,12 +45,19 @@
void AddImu(aos::distributed_clock::time_point distributed_now,
Eigen::Vector3d gyro, Eigen::Vector3d accel);
+ // Adds a turret reading (position; velocity) to the list at the provided
+ // time.
+ void AddTurret(aos::distributed_clock::time_point distributed_now,
+ Eigen::Vector2d state);
+
// Processes the data points by calling UpdateCamera and UpdateIMU in time
// order.
void ReviewData(CalibrationDataObserver *observer) const;
size_t camera_samples_size() const { return rot_trans_points_.size(); }
+ size_t turret_samples() const { return turret_points_.size(); }
+
private:
std::vector<std::pair<aos::distributed_clock::time_point,
std::pair<Eigen::Vector3d, Eigen::Vector3d>>>
@@ -56,6 +68,10 @@
std::vector<std::pair<aos::distributed_clock::time_point,
std::pair<Eigen::Vector3d, Eigen::Vector3d>>>
rot_trans_points_;
+
+ // Turret state as a timestamp and [x, v].
+ std::vector<std::pair<aos::distributed_clock::time_point, Eigen::Vector2d>>
+ turret_points_;
};
// Class to register image and IMU callbacks in AOS and route them to the