Pull common extrinsics calibration code out into //frc971/vision
This sets us up to have a generic solver interface, and year specific
data munging.
Change-Id: I5cba597aa263d5061b7c71cd617706460ddb5f93
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/vision/extrinsics_calibration.h b/frc971/vision/extrinsics_calibration.h
new file mode 100644
index 0000000..9d6c704
--- /dev/null
+++ b/frc971/vision/extrinsics_calibration.h
@@ -0,0 +1,39 @@
+#ifndef FRC971_VISION_EXTRINSICS_CALIBRATION_H_
+#define FRC971_VISION_EXTRINSICS_CALIBRATION_H_
+
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+#include "frc971/vision/calibration_accumulator.h"
+
+namespace frc971 {
+namespace vision {
+
+struct CalibrationParameters {
+ Eigen::Quaternion<double> initial_orientation =
+ Eigen::Quaternion<double>::Identity();
+ Eigen::Quaternion<double> imu_to_camera =
+ Eigen::Quaternion<double>::Identity();
+ Eigen::Quaternion<double> board_to_world =
+ Eigen::Quaternion<double>::Identity();
+
+ 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>::Zero();
+
+ double gravity_scalar = 1.0;
+ Eigen::Matrix<double, 3, 1> accelerometer_bias =
+ Eigen::Matrix<double, 3, 1>::Zero();
+};
+
+void Solve(const CalibrationData &data,
+ CalibrationParameters *calibration_parameters);
+
+void Plot(const CalibrationData &data,
+ const CalibrationParameters &calibration_parameters);
+
+} // namespace vision
+} // namespace frc971
+
+#endif // FRC971_VISION_EXTRINSICS_CALIBRATION_H_