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/extrinsics_calibration.h b/frc971/vision/extrinsics_calibration.h
index 9d6c704..b24f13f 100644
--- a/frc971/vision/extrinsics_calibration.h
+++ b/frc971/vision/extrinsics_calibration.h
@@ -11,7 +11,9 @@
struct CalibrationParameters {
Eigen::Quaternion<double> initial_orientation =
Eigen::Quaternion<double>::Identity();
- Eigen::Quaternion<double> imu_to_camera =
+ Eigen::Quaternion<double> pivot_to_camera =
+ Eigen::Quaternion<double>::Identity();
+ Eigen::Quaternion<double> pivot_to_imu =
Eigen::Quaternion<double>::Identity();
Eigen::Quaternion<double> board_to_world =
Eigen::Quaternion<double>::Identity();
@@ -19,17 +21,24 @@
Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero();
Eigen::Matrix<double, 6, 1> initial_state =
Eigen::Matrix<double, 6, 1>::Zero();
- Eigen::Matrix<double, 3, 1> imu_to_camera_translation =
+ Eigen::Matrix<double, 3, 1> pivot_to_camera_translation =
+ Eigen::Matrix<double, 3, 1>::Zero();
+ Eigen::Matrix<double, 3, 1> pivot_to_imu_translation =
Eigen::Matrix<double, 3, 1>::Zero();
double gravity_scalar = 1.0;
Eigen::Matrix<double, 3, 1> accelerometer_bias =
Eigen::Matrix<double, 3, 1>::Zero();
+
+ bool has_pivot = false;
};
+// Solves the mounting problem given the calibration data and parameters. The
+// parameters are used as the seed to the solver.
void Solve(const CalibrationData &data,
CalibrationParameters *calibration_parameters);
+// Plots the calibrated results to help visualize the fit.
void Plot(const CalibrationData &data,
const CalibrationParameters &calibration_parameters);