Add a loss function to extrinsics calibration
Removes outliers.
Also added a flag for plotting
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ic522bae68b3d48507b58fd0cb0c3e79034240709
diff --git a/y2020/vision/extrinsics_calibration.cc b/y2020/vision/extrinsics_calibration.cc
index 3216c23..f2ac926 100644
--- a/y2020/vision/extrinsics_calibration.cc
+++ b/y2020/vision/extrinsics_calibration.cc
@@ -22,6 +22,7 @@
DEFINE_string(config, "config.json", "Path to the config file to use.");
DEFINE_string(pi, "pi-7971-2", "Pi name to calibrate.");
+DEFINE_bool(plot, false, "Whether to plot the resulting data.");
namespace frc971 {
namespace vision {
@@ -632,10 +633,11 @@
4, 3, 1, 3>(
new CostFunctor(&data), data.camera_samples_size() * 6);
problem.AddResidualBlock(
- cost_function, nullptr, initial_orientation.coeffs().data(),
- imu_to_camera.coeffs().data(), gyro_bias.data(), initial_state.data(),
- board_to_world.coeffs().data(), imu_to_camera_translation.data(),
- &gravity_scalar, accelerometer_bias.data());
+ cost_function, new ceres::HuberLoss(1.0),
+ initial_orientation.coeffs().data(), imu_to_camera.coeffs().data(),
+ gyro_bias.data(), initial_state.data(), board_to_world.coeffs().data(),
+ imu_to_camera_translation.data(), &gravity_scalar,
+ accelerometer_bias.data());
problem.SetParameterization(initial_orientation.coeffs().data(),
quaternion_local_parameterization);
problem.SetParameterization(imu_to_camera.coeffs().data(),
@@ -697,7 +699,9 @@
initial_state, board_to_world, imu_to_camera_translation,
gravity_scalar, accelerometer_bias);
data.ReviewData(&filter);
- filter.Plot();
+ if (FLAGS_plot) {
+ filter.Plot();
+ }
}
}