Produce flatbuffers results from extrinsics cal
This makes it so that the extrinsics calibration process can, if it
wants, write out the resulting flatbuffer to a file or stdout or the
such.
Change-Id: Ib899d5ad910ecbdeada85f70a154d2542c9d28d7
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/vision/extrinsics_calibration.cc b/frc971/vision/extrinsics_calibration.cc
index cd6d183..d563fb0 100644
--- a/frc971/vision/extrinsics_calibration.cc
+++ b/frc971/vision/extrinsics_calibration.cc
@@ -859,8 +859,19 @@
}
};
-void Solve(const CalibrationData &data,
- CalibrationParameters *calibration_parameters) {
+std::vector<float> MatrixToVector(const Eigen::Matrix<double, 4, 4> &H) {
+ std::vector<float> data;
+ for (int row = 0; row < 4; ++row) {
+ for (int col = 0; col < 4; ++col) {
+ data.push_back(H(row, col));
+ }
+ }
+ return data;
+}
+
+aos::FlatbufferDetachedBuffer<calibration::CameraCalibration> Solve(
+ const CalibrationData &data,
+ CalibrationParameters *calibration_parameters) {
ceres::Problem problem;
ceres::EigenQuaternionParameterization *quaternion_local_parameterization =
@@ -949,6 +960,46 @@
LOG(INFO) << summary.FullReport();
LOG(INFO) << "Solution is " << (summary.IsSolutionUsable() ? "" : "NOT ")
<< "usable";
+
+ {
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::Offset<flatbuffers::Vector<float>> data_offset =
+ fbb.CreateVector(MatrixToVector(
+ (Eigen::Translation3d(
+ calibration_parameters->pivot_to_camera_translation) *
+ Eigen::Quaterniond(calibration_parameters->pivot_to_camera))
+ .inverse()
+ .matrix()));
+ calibration::TransformationMatrix::Builder matrix_builder(fbb);
+ matrix_builder.add_data(data_offset);
+ flatbuffers::Offset<calibration::TransformationMatrix>
+ camera_to_pivot_offset = matrix_builder.Finish();
+
+ flatbuffers::Offset<calibration::TransformationMatrix> pivot_to_imu_offset;
+ if (calibration_parameters->has_pivot) {
+ flatbuffers::Offset<flatbuffers::Vector<float>> data_offset =
+ fbb.CreateVector(MatrixToVector(
+ (Eigen::Translation3d(
+ calibration_parameters->pivot_to_imu_translation) *
+ Eigen::Quaterniond(calibration_parameters->pivot_to_imu))
+ .matrix()));
+ calibration::TransformationMatrix::Builder matrix_builder(fbb);
+ matrix_builder.add_data(data_offset);
+ pivot_to_imu_offset = matrix_builder.Finish();
+ }
+
+ calibration::CameraCalibration::Builder calibration_builder(fbb);
+ if (calibration_parameters->has_pivot) {
+ calibration_builder.add_fixed_extrinsics(pivot_to_imu_offset);
+ calibration_builder.add_turret_extrinsics(camera_to_pivot_offset);
+ } else {
+ calibration_builder.add_fixed_extrinsics(camera_to_pivot_offset);
+ }
+ fbb.Finish(calibration_builder.Finish());
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration> extrinsics =
+ fbb.Release();
+ return extrinsics;
+ }
}
void Plot(const CalibrationData &data,
diff --git a/frc971/vision/extrinsics_calibration.h b/frc971/vision/extrinsics_calibration.h
index eb35482..fb52ed9 100644
--- a/frc971/vision/extrinsics_calibration.h
+++ b/frc971/vision/extrinsics_calibration.h
@@ -33,10 +33,17 @@
bool has_pivot = false;
};
+// Copies an Eigen matrix into a row-major vector of the data. Used for
+// converting into the flatbuffer TransformationMatrix type.
+std::vector<float> MatrixToVector(const Eigen::Matrix<double, 4, 4> &H);
+
// 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);
+// Produces a CameraCalibration flatbuffer which will have the relevant
+// extrinsics fields populated, so that it can subsequently be merged with the
+// intrinsics portion of the calibration.
+aos::FlatbufferDetachedBuffer<calibration::CameraCalibration> Solve(
+ const CalibrationData &data, CalibrationParameters *calibration_parameters);
// Plots the calibrated results to help visualize the fit.
void Plot(const CalibrationData &data,