Solver with turret fixes and visualization
Converges generally well, but still has sensitivity to some parameters
In addition to turret and visualization, significant changes include:
-- Added quaternion parameterization for pivot_to_imu
-- Filtering out of bad gyro readings
-- Scaling of rotation error (residual)
Visualization tool is really just a set of helper routines to plot axes
in 3D space in a projected 2D virtual image
Change-Id: I03a6939b6b12df4b8116c30c617a1595781fe635
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/y2020/vision/extrinsics_calibration.cc b/y2020/vision/extrinsics_calibration.cc
index c561652..83d8760 100644
--- a/y2020/vision/extrinsics_calibration.cc
+++ b/y2020/vision/extrinsics_calibration.cc
@@ -40,6 +40,8 @@
CHECK(aos::configuration::MultiNode(reader.configuration()));
// Find the nodes we care about.
+ const aos::Node *const imu_node =
+ aos::configuration::GetNode(factory.configuration(), "imu");
const aos::Node *const roborio_node =
aos::configuration::GetNode(factory.configuration(), "roborio");
@@ -49,17 +51,20 @@
const aos::Node *const pi_node = aos::configuration::GetNode(
factory.configuration(), absl::StrCat("pi", *pi_number));
+ LOG(INFO) << "imu " << aos::FlatbufferToJson(imu_node);
LOG(INFO) << "roboRIO " << aos::FlatbufferToJson(roborio_node);
LOG(INFO) << "Pi " << aos::FlatbufferToJson(pi_node);
+ std::unique_ptr<aos::EventLoop> imu_event_loop =
+ factory.MakeEventLoop("calibration", imu_node);
std::unique_ptr<aos::EventLoop> roborio_event_loop =
factory.MakeEventLoop("calibration", roborio_node);
std::unique_ptr<aos::EventLoop> pi_event_loop =
factory.MakeEventLoop("calibration", pi_node);
// Now, hook Calibration up to everything.
- Calibration extractor(&factory, pi_event_loop.get(),
- roborio_event_loop.get(), FLAGS_pi, &data);
+ Calibration extractor(&factory, pi_event_loop.get(), imu_event_loop.get(),
+ FLAGS_pi, &data);
if (FLAGS_turret) {
aos::NodeEventLoopFactory *roborio_factory =
@@ -89,25 +94,42 @@
Eigen::Vector3d(0.0, 0.0, M_PI)));
const Eigen::Quaternion<double> nominal_pivot_to_camera(
Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitX()));
+ const Eigen::Quaternion<double> nominal_pivot_to_imu(
+ Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()));
const Eigen::Quaternion<double> nominal_board_to_world(
Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX()));
+ Eigen::Matrix<double, 6, 1> nominal_initial_state =
+ Eigen::Matrix<double, 6, 1>::Zero();
+ // Set y value to -1 m (approx distance from imu to board/world
+ nominal_initial_state(1, 0) = -1.0;
CalibrationParameters calibration_parameters;
calibration_parameters.initial_orientation = nominal_initial_orientation;
calibration_parameters.pivot_to_camera = nominal_pivot_to_camera;
+ calibration_parameters.pivot_to_imu = nominal_pivot_to_imu;
calibration_parameters.board_to_world = nominal_board_to_world;
+ calibration_parameters.initial_state = nominal_initial_state;
+ if (data.turret_samples_size() > 0) {
+ LOG(INFO) << "Have turret, so using pivot setup";
+ calibration_parameters.has_pivot = true;
+ }
Solve(data, &calibration_parameters);
LOG(INFO) << "Nominal initial_orientation "
<< nominal_initial_orientation.coeffs().transpose();
LOG(INFO) << "Nominal pivot_to_camera "
<< nominal_pivot_to_camera.coeffs().transpose();
-
- LOG(INFO) << "pivot_to_camera delta "
+ LOG(INFO) << "Nominal pivot_to_camera (rot-xyz) "
+ << frc971::controls::ToRotationVectorFromQuaternion(
+ nominal_pivot_to_camera)
+ .transpose();
+ LOG(INFO) << "pivot_to_camera change "
<< frc971::controls::ToRotationVectorFromQuaternion(
calibration_parameters.pivot_to_camera *
nominal_pivot_to_camera.inverse())
.transpose();
+ LOG(INFO) << "Nominal pivot_to_imu "
+ << nominal_pivot_to_imu.coeffs().transpose();
LOG(INFO) << "board_to_world delta "
<< frc971::controls::ToRotationVectorFromQuaternion(
calibration_parameters.board_to_world *