Small tweaks to get extrinsics to work and add some debugging info
Added workaround to support foxglove-- still need to figure out annotations
Remove fixed extrinsics from initial calibration file
Use charuco_diamond by default for extrinsic calibration
Change-Id: Ia2410b227cf3531a4a68e861128deb3c2fe4aa79
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/frc971/vision/extrinsics_calibration.cc b/frc971/vision/extrinsics_calibration.cc
index d563fb0..daa371e 100644
--- a/frc971/vision/extrinsics_calibration.cc
+++ b/frc971/vision/extrinsics_calibration.cc
@@ -488,38 +488,6 @@
vis_robot.SetCameraParameters(camera_mat);
vis_robot.SetDistortionCoefficients(dist_coeffs);
- /*
- // Draw an initial visualization
- Eigen::Vector3d T_world_imu_vec =
- calibration_parameters.initial_state.block<3, 1>(0, 0);
- Eigen::Translation3d T_world_imu(T_world_imu_vec);
- Eigen::Affine3d H_world_imu =
- T_world_imu * calibration_parameters.initial_orientation;
-
- vis_robot.DrawFrameAxes(H_world_imu, "imu");
-
- Eigen::Quaterniond R_imu_pivot(calibration_parameters.pivot_to_imu);
- Eigen::Translation3d T_imu_pivot(
- calibration_parameters.pivot_to_imu_translation);
- Eigen::Affine3d H_imu_pivot = T_imu_pivot * R_imu_pivot;
- Eigen::Affine3d H_world_pivot = H_world_imu * H_imu_pivot;
- vis_robot.DrawFrameAxes(H_world_pivot, "pivot");
-
- Eigen::Affine3d H_imupivot_camerapivot(
- Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitZ()));
- Eigen::Quaterniond R_camera_pivot(calibration_parameters.pivot_to_camera);
- Eigen::Translation3d T_camera_pivot(
- calibration_parameters.pivot_to_camera_translation);
- Eigen::Affine3d H_camera_pivot = T_camera_pivot * R_camera_pivot;
- Eigen::Affine3d H_world_camera = H_world_imu * H_imu_pivot *
- H_imupivot_camerapivot *
- H_camera_pivot.inverse();
- vis_robot.DrawFrameAxes(H_world_camera, "camera");
-
- cv::imshow("Original poses", image_mat);
- cv::waitKey();
- */
-
uint current_state_index = 0;
uint current_turret_index = 0;
for (uint i = 0; i < camera_times_.size() - 1; i++) {
@@ -623,11 +591,6 @@
cv::imshow("Live", image_mat);
cv::waitKey(50);
-
- if (i % 200 == 0) {
- LOG(INFO) << "Pausing at step " << i;
- cv::waitKey();
- }
}
LOG(INFO) << "Finished visualizing robot. Press any key to continue";
cv::waitKey();
@@ -849,11 +812,11 @@
trans_error_scale * filter.errorpz(i);
}
- LOG(INFO) << "Cost function calc took "
- << chrono::duration<double>(aos::monotonic_clock::now() -
- start_time)
- .count()
- << " seconds";
+ VLOG(2) << "Cost function calc took "
+ << chrono::duration<double>(aos::monotonic_clock::now() -
+ start_time)
+ .count()
+ << " seconds";
return true;
}
@@ -898,7 +861,7 @@
calibration_parameters->accelerometer_bias.data());
}
- {
+ if (calibration_parameters->has_pivot) {
// The turret's Z rotation is redundant with the camera's mounting z
// rotation since it's along the rotation axis.
ceres::CostFunction *turret_z_cost_function =
@@ -922,6 +885,17 @@
calibration_parameters->pivot_to_imu_translation.data());
}
+ {
+ // The board rotation in z is a bit arbitrary, so hoping to limit it to
+ // increase repeatability
+ ceres::CostFunction *board_z_cost_function =
+ new ceres::AutoDiffCostFunction<PenalizeQuaternionZ, 1, 4>(
+ new PenalizeQuaternionZ());
+ problem.AddResidualBlock(
+ board_z_cost_function, nullptr,
+ calibration_parameters->board_to_world.coeffs().data());
+ }
+
problem.SetParameterization(
calibration_parameters->initial_orientation.coeffs().data(),
quaternion_local_parameterization);
@@ -952,9 +926,9 @@
// Run the solver!
ceres::Solver::Options options;
options.minimizer_progress_to_stdout = true;
- options.gradient_tolerance = 1e-12;
+ options.gradient_tolerance = 1e-6;
options.function_tolerance = 1e-6;
- options.parameter_tolerance = 1e-12;
+ options.parameter_tolerance = 1e-6;
ceres::Solver::Summary summary;
Solve(options, &problem, &summary);
LOG(INFO) << summary.FullReport();