Use ceres solver for extrinsics calibration
Using auto differentiation to solve for camera mount angle and imu bias.
Change-Id: I434f5bc7ac97acb5d18f09ec9174d79e6f5bbb06
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 644346a..c7a5dd7 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -99,7 +99,9 @@
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
+ ":jacobian",
":quaternion_utils",
+ ":runge_kutta",
"//aos/testing:googletest",
"//aos/testing:random_seed",
"@com_github_google_glog//:glog",
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.h b/frc971/control_loops/drivetrain/improved_down_estimator.h
index 548dba8..ddbf0bb 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.h
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.h
@@ -8,6 +8,7 @@
#include "aos/time/time.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/control_loops/runge_kutta.h"
#include "glog/logging.h"
@@ -216,22 +217,9 @@
// A good reference for angular velocity vectors with quaternions is at
// http://www.euclideanspace.com/physics/kinematics/angularvelocity/
- // Creates a rotational velocity vector to be integrated.
- //
- // omega is the rotational velocity vector in body coordinates.
- // q is a matrix with the compononents of the quaternion in it.
- //
- // Returns dq / dt
static Eigen::Vector4d QuaternionDerivative(Eigen::Vector3d omega,
const Eigen::Vector4d &q_matrix) {
- Eigen::Quaternion<double> q(q_matrix);
-
- Eigen::Quaternion<double> omega_q;
- omega_q.w() = 0.0;
- omega_q.vec() = 0.5 * (q * omega);
-
- Eigen::Quaternion<double> deriv = omega_q * q;
- return deriv.coeffs();
+ return frc971::controls::QuaternionDerivative(omega, q_matrix);
}
// Moves the robot by the provided rotation vector (U).
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index 4dcfe60..3312030 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -27,71 +27,6 @@
}
} // namespace
-// Do a known transformation to see if quaternion integration is working
-// correctly.
-TEST(DownEstimatorTest, QuaternionIntegral) {
- Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
- Eigen::Vector3d uy = Eigen::Vector3d::UnitY();
- Eigen::Vector3d uz = Eigen::Vector3d::UnitZ();
-
- Eigen::Quaternion<double> q(
- Eigen::AngleAxis<double>(0.5 * M_PI, Eigen::Vector3d::UnitY()));
-
- Eigen::Quaternion<double> q0(
- Eigen::AngleAxis<double>(0, Eigen::Vector3d::UnitY()));
-
- auto qux = q * ux;
-
- VLOG(1) << "Q is w: " << q.w() << " vec: " << q.vec();
- VLOG(1) << "ux is " << ux;
- VLOG(1) << "qux is " << qux;
-
- // Start by rotating around the X body vector for pi/2
- Eigen::Quaternion<double> integral1(
- RungeKutta(std::bind(&drivetrain::DrivetrainUkf::QuaternionDerivative, ux,
- std::placeholders::_1),
- q0.coeffs(), 0.5 * M_PI));
-
- VLOG(1) << "integral1 * uz => " << integral1 * uz;
-
- // Then rotate around the Y body vector for pi/2
- Eigen::Quaternion<double> integral2(
- RungeKutta(std::bind(&drivetrain::DrivetrainUkf::QuaternionDerivative, uy,
- std::placeholders::_1),
- integral1.normalized().coeffs(), 0.5 * M_PI));
-
- VLOG(1) << "integral2 * uz => " << integral2 * uz;
-
- // Then rotate around the X body vector for -pi/2
- Eigen::Quaternion<double> integral3(
- RungeKutta(std::bind(&drivetrain::DrivetrainUkf::QuaternionDerivative,
- -ux, std::placeholders::_1),
- integral2.normalized().coeffs(), 0.5 * M_PI));
-
- integral1.normalize();
- integral2.normalize();
- integral3.normalize();
-
- VLOG(1) << "Integral is w: " << integral1.w() << " vec: " << integral1.vec()
- << " norm " << integral1.norm();
-
- VLOG(1) << "Integral is w: " << integral3.w() << " vec: " << integral3.vec()
- << " norm " << integral3.norm();
-
- VLOG(1) << "ux => " << integral3 * ux;
- EXPECT_NEAR(0.0, (ux - integral1 * ux).norm(), 5e-2);
- EXPECT_NEAR(0.0, (uz - integral1 * uy).norm(), 5e-2);
- EXPECT_NEAR(0.0, (-uy - integral1 * uz).norm(), 5e-2);
-
- EXPECT_NEAR(0.0, (uy - integral2 * ux).norm(), 5e-2);
- EXPECT_NEAR(0.0, (uz - integral2 * uy).norm(), 5e-2);
- EXPECT_NEAR(0.0, (ux - integral2 * uz).norm(), 5e-2);
-
- EXPECT_NEAR(0.0, (uy - integral3 * ux).norm(), 5e-2);
- EXPECT_NEAR(0.0, (-ux - integral3 * uy).norm(), 5e-2);
- EXPECT_NEAR(0.0, (uz - integral3 * uz).norm(), 5e-2);
-}
-
TEST(DownEstimatorTest, UkfConstantRotation) {
drivetrain::DrivetrainUkf dtukf(
drivetrain::testing::GetTestDrivetrainConfig());
diff --git a/frc971/control_loops/quaternion_utils.cc b/frc971/control_loops/quaternion_utils.cc
index 0226e6d..bce78d0 100644
--- a/frc971/control_loops/quaternion_utils.cc
+++ b/frc971/control_loops/quaternion_utils.cc
@@ -5,52 +5,48 @@
namespace frc971 {
namespace controls {
+namespace {
-Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
- const Eigen::Matrix<double, 3, 1> &X, const double max_angle_cap) {
- const double unclipped_angle = X.norm();
- const double angle_scalar =
- (unclipped_angle > max_angle_cap) ? max_angle_cap / unclipped_angle : 1.0;
- const double angle = unclipped_angle * angle_scalar;
- const double half_angle = angle * 0.5;
-
- const double half_angle_squared = half_angle * half_angle;
+double SinXoverX(double x) {
+ const double xsquared = x * x;
// sin(x)/x = 1
double sinx_x = 1.0;
// - x^2/3!
- double value = half_angle_squared / 6.0;
+ double value = xsquared / 6.0;
sinx_x -= value;
// + x^4/5!
- value = value * half_angle_squared / 20.0;
+ value = value * xsquared / 20.0;
sinx_x += value;
// - x^6/7!
- value = value * half_angle_squared / (6.0 * 7.0);
+ value = value * xsquared / (6.0 * 7.0);
sinx_x -= value;
// + x^8/9!
- value = value * half_angle_squared / (8.0 * 9.0);
+ value = value * xsquared / (8.0 * 9.0);
sinx_x += value;
// - x^10/11!
- value = value * half_angle_squared / (10.0 * 11.0);
+ value = value * xsquared / (10.0 * 11.0);
sinx_x -= value;
// + x^12/13!
- value = value * half_angle_squared / (12.0 * 13.0);
+ value = value * xsquared / (12.0 * 13.0);
sinx_x += value;
// - x^14/15!
- value = value * half_angle_squared / (14.0 * 15.0);
+ value = value * xsquared / (14.0 * 15.0);
sinx_x -= value;
// + x^16/17!
- value = value * half_angle_squared / (16.0 * 17.0);
+ value = value * xsquared / (16.0 * 17.0);
sinx_x += value;
+ return sinx_x;
+
// To plot the residual in matplotlib, run:
// import numpy
// import scipy
@@ -65,15 +61,10 @@
// x ** 14 / scipy.misc.factorial(15) +
// x ** 16 / scipy.misc.factorial(17) -
// numpy.sin(x) / x)
-
- const double scalar = sinx_x * 0.5;
-
- Eigen::Matrix<double, 4, 1> result;
- result.block<3, 1>(0, 0) = X * scalar * angle_scalar;
- result(3, 0) = std::cos(half_angle);
- return result;
}
+} // namespace
+
inline Eigen::Matrix<double, 4, 1> MaybeFlipX(
const Eigen::Matrix<double, 4, 1> &X) {
if (X(3, 0) < 0.0) {
@@ -83,6 +74,24 @@
}
}
+Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
+ const Eigen::Matrix<double, 3, 1> &X, const double max_angle_cap) {
+ const double unclipped_angle = X.norm();
+ const double angle_scalar =
+ (unclipped_angle > max_angle_cap) ? max_angle_cap / unclipped_angle : 1.0;
+ const double angle = unclipped_angle * angle_scalar;
+ const double half_angle = angle * 0.5;
+
+ const double scalar = SinXoverX(half_angle) * 0.5;
+
+ Eigen::Matrix<double, 4, 1> result;
+ result.block<3, 1>(0, 0) = X * scalar * angle_scalar;
+ result(3, 0) = std::cos(half_angle);
+ return result;
+}
+
+// q = cos(a/2) + i ( x * sin(a/2)) + j (y * sin(a/2)) + k ( z * sin(a/2))
+
Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
const Eigen::Matrix<double, 4, 1> &X) {
// TODO(austin): Verify we still need it.
@@ -90,50 +99,38 @@
const double half_angle =
std::atan2(corrected_X.block<3, 1>(0, 0).norm(), corrected_X(3, 0));
- const double half_angle_squared = half_angle * half_angle;
-
- // TODO(austin): We are doing a division at the end of this. Do the taylor
- // series expansion of x/sin(x) instead to avoid this.
-
- // sin(x)/x = 1
- double sinx_x = 1.0;
-
- // - x^2/3!
- double value = half_angle_squared / 6.0;
- sinx_x -= value;
-
- // + x^4/5!
- value = value * half_angle_squared / 20.0;
- sinx_x += value;
-
- // - x^6/7!
- value = value * half_angle_squared / (6.0 * 7.0);
- sinx_x -= value;
-
- // + x^8/9!
- value = value * half_angle_squared / (8.0 * 9.0);
- sinx_x += value;
-
- // - x^10/11!
- value = value * half_angle_squared / (10.0 * 11.0);
- sinx_x -= value;
-
- // + x^12/13!
- value = value * half_angle_squared / (12.0 * 13.0);
- sinx_x += value;
-
- // - x^14/15!
- value = value * half_angle_squared / (14.0 * 15.0);
- sinx_x -= value;
-
- // + x^16/17!
- value = value * half_angle_squared / (16.0 * 17.0);
- sinx_x += value;
-
- const double scalar = 2.0 / sinx_x;
+ const double scalar = 2.0 / SinXoverX(half_angle);
return corrected_X.block<3, 1>(0, 0) * scalar;
}
+Eigen::Matrix<double, 4, 3> QuaternionDerivativeDerivitive(
+ const Eigen::Vector4d &q_matrix) {
+ // qa * qb = (a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
+ // a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
+ // a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x(),
+ // a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z())
+ //
+ // We want q * omega_q = result * omega.
+ Eigen::Matrix<double, 4, 3> result;
+ result(3, 0) = -q_matrix.x() * 0.5;
+ result(3, 1) = -q_matrix.y() * 0.5;
+ result(3, 2) = -q_matrix.z() * 0.5;
+
+ result(0, 0) = q_matrix.w() * 0.5;
+ result(0, 1) = -q_matrix.z() * 0.5;
+ result(0, 2) = q_matrix.y() * 0.5;
+
+ result(1, 0) = q_matrix.z() * 0.5;
+ result(1, 1) = q_matrix.w() * 0.5;
+ result(1, 2) = -q_matrix.x() * 0.5;
+
+ result(2, 0) = -q_matrix.y() * 0.5;
+ result(2, 1) = q_matrix.x() * 0.5;
+ result(2, 2) = q_matrix.w() * 0.5;
+
+ return result;
+}
+
} // namespace controls
} // namespace frc971
diff --git a/frc971/control_loops/quaternion_utils.h b/frc971/control_loops/quaternion_utils.h
index 25ffa91..e97935c 100644
--- a/frc971/control_loops/quaternion_utils.h
+++ b/frc971/control_loops/quaternion_utils.h
@@ -65,6 +65,34 @@
const Eigen::Matrix<double, 3, 1> &X,
const double max_angle_cap = std::numeric_limits<double>::infinity());
+// Creates a rotational velocity vector to be integrated.
+//
+// omega is the rotational velocity vector in body coordinates.
+// q is a matrix with the compononents of the quaternion in it.
+//
+// Returns dq / dt
+inline Eigen::Vector4d QuaternionDerivative(Eigen::Vector3d omega,
+ const Eigen::Vector4d &q_matrix) {
+ // See https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/ for
+ // another resource on quaternion integration and derivitives.
+ Eigen::Quaternion<double> q(q_matrix);
+
+ Eigen::Quaternion<double> omega_q;
+ omega_q.w() = 0.0;
+ omega_q.vec() = 0.5 * omega;
+
+ Eigen::Quaternion<double> deriv = q * omega_q;
+ return deriv.coeffs();
+}
+
+// d QuaternionDerivative / d omega
+Eigen::Matrix<double, 4, 3> QuaternionDerivativeDerivitive(
+ const Eigen::Vector4d &q_matrix);
+inline Eigen::Matrix<double, 4, 3> QuaternionDerivativeDerivitive(
+ const Eigen::Quaternion<double> &q) {
+ return QuaternionDerivativeDerivitive(q.coeffs());
+}
+
} // namespace controls
} // namespace frc971
diff --git a/frc971/control_loops/quaternion_utils_test.cc b/frc971/control_loops/quaternion_utils_test.cc
index f472a46..ada5e60 100644
--- a/frc971/control_loops/quaternion_utils_test.cc
+++ b/frc971/control_loops/quaternion_utils_test.cc
@@ -2,8 +2,10 @@
#include <random>
-#include "frc971/control_loops/quaternion_utils.h"
#include "aos/testing/random_seed.h"
+#include "frc971/control_loops/jacobian.h"
+#include "frc971/control_loops/quaternion_utils.h"
+#include "frc971/control_loops/runge_kutta.h"
#include "glog/logging.h"
#include "gtest/gtest.h"
@@ -147,6 +149,68 @@
}
}
+// Do a known transformation to see if quaternion integration is working
+// correctly.
+TEST(DownEstimatorTest, QuaternionIntegral) {
+ Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
+ Eigen::Vector3d uy = Eigen::Vector3d::UnitY();
+ Eigen::Vector3d uz = Eigen::Vector3d::UnitZ();
+
+ Eigen::Quaternion<double> q(
+ Eigen::AngleAxis<double>(0.5 * M_PI, Eigen::Vector3d::UnitY()));
+
+ Eigen::Quaternion<double> q0(
+ Eigen::AngleAxis<double>(0, Eigen::Vector3d::UnitY()));
+
+ auto qux = q * ux;
+
+ VLOG(1) << "Q is w: " << q.w() << " vec: " << q.vec();
+ VLOG(1) << "ux is " << ux;
+ VLOG(1) << "qux is " << qux;
+
+ // Start by rotating around the X body vector for pi/2
+ Eigen::Quaternion<double> integral1(control_loops::RungeKutta(
+ std::bind(&QuaternionDerivative, ux, std::placeholders::_1), q0.coeffs(),
+ 0.5 * M_PI));
+
+ VLOG(1) << "integral1 * uz => " << integral1 * uz;
+
+ // Then rotate around the Y body vector for pi/2
+ Eigen::Quaternion<double> integral2(control_loops::RungeKutta(
+ std::bind(&QuaternionDerivative, uy, std::placeholders::_1),
+ integral1.normalized().coeffs(), 0.5 * M_PI));
+
+ VLOG(1) << "integral2 * uz => " << integral2 * uz;
+
+ // Then rotate around the X body vector for -pi/2
+ Eigen::Quaternion<double> integral3(control_loops::RungeKutta(
+ std::bind(&QuaternionDerivative, -ux, std::placeholders::_1),
+ integral2.normalized().coeffs(), 0.5 * M_PI));
+
+ integral1.normalize();
+ integral2.normalize();
+ integral3.normalize();
+
+ VLOG(1) << "Integral is w: " << integral1.w() << " vec: " << integral1.vec()
+ << " norm " << integral1.norm();
+
+ VLOG(1) << "Integral is w: " << integral3.w() << " vec: " << integral3.vec()
+ << " norm " << integral3.norm();
+
+ VLOG(1) << "ux => " << integral3 * ux;
+ EXPECT_NEAR(0.0, (ux - integral1 * ux).norm(), 5e-2);
+ EXPECT_NEAR(0.0, (uz - integral1 * uy).norm(), 5e-2);
+ EXPECT_NEAR(0.0, (-uy - integral1 * uz).norm(), 5e-2);
+
+ EXPECT_NEAR(0.0, (uy - integral2 * ux).norm(), 5e-2);
+ EXPECT_NEAR(0.0, (uz - integral2 * uy).norm(), 5e-2);
+ EXPECT_NEAR(0.0, (ux - integral2 * uz).norm(), 5e-2);
+
+ EXPECT_NEAR(0.0, (uy - integral3 * ux).norm(), 5e-2);
+ EXPECT_NEAR(0.0, (-ux - integral3 * uy).norm(), 5e-2);
+ EXPECT_NEAR(0.0, (uz - integral3 * uz).norm(), 5e-2);
+}
+
} // namespace testing
} // namespace controls
} // namespace frc971
diff --git a/frc971/control_loops/runge_kutta.h b/frc971/control_loops/runge_kutta.h
index 61d674e..963f463 100644
--- a/frc971/control_loops/runge_kutta.h
+++ b/frc971/control_loops/runge_kutta.h
@@ -19,6 +19,18 @@
return X + dt / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
+// Implements Runge Kutta integration (4th order) split up into steps steps. fn
+// is the function to integrate. It must take 1 argument of type T. The
+// integration starts at an initial value X, and integrates for dt.
+template <typename F, typename T>
+T RungeKuttaSteps(const F &fn, T X, double dt, int steps) {
+ dt = dt / steps;
+ for (int i = 0; i < steps; ++i) {
+ X = RungeKutta(fn, X, dt);
+ }
+ return X;
+}
+
// Implements Runge Kutta integration (4th order). This integrates dy/dt = fn(t,
// y). It must have the call signature of fn(double t, T y). The
// integration starts at an initial value y, and integrates for dt.
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index 922e26f..bb85601 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -194,11 +194,13 @@
"//aos:init",
"//aos/events:shm_event_loop",
"//aos/events/logging:log_reader",
+ "//frc971/analysis:in_process_plotter",
"//frc971/control_loops/drivetrain:improved_down_estimator",
"//frc971/wpilib:imu_batch_fbs",
"//frc971/wpilib:imu_fbs",
"//third_party:opencv",
"@com_google_absl//absl/strings:str_format",
+ "@com_google_ceres_solver//:ceres",
"@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/y2020/vision/extrinsics_calibration.cc b/y2020/vision/extrinsics_calibration.cc
index be64efb..1d4de28 100644
--- a/y2020/vision/extrinsics_calibration.cc
+++ b/y2020/vision/extrinsics_calibration.cc
@@ -1,8 +1,3 @@
-#include <opencv2/aruco/charuco.hpp>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/features2d.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc.hpp>
#include "Eigen/Dense"
#include "Eigen/Geometry"
@@ -13,6 +8,8 @@
#include "aos/network/team_number.h"
#include "aos/time/time.h"
#include "aos/util/file.h"
+#include "ceres/ceres.h"
+#include "frc971/analysis/in_process_plotter.h"
#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/wpilib/imu_batch_generated.h"
@@ -32,45 +29,186 @@
using aos::distributed_clock;
using aos::monotonic_clock;
-class PoseFilter : public CalibrationDataObserver {
+// The basic ideas here are taken from Kalibr.
+// (https://github.com/ethz-asl/kalibr), but adapted to work with AOS, and to be
+// simpler.
+//
+// Camera readings and IMU readings come in at different times, on different
+// time scales. Our first problem is to align them in time so we can actually
+// compute an error. This is done in the calibration accumulator code. The
+// kalibr paper uses splines, while this uses kalman filters to solve the same
+// interpolation problem so we can get the expected vs actual pose at the time
+// each image arrives.
+//
+// The cost function is then fed the computed angular and positional error for
+// each camera sample before the kalman filter update. Intuitively, the smaller
+// the corrections to the kalman filter each step, the better the estimate
+// should be.
+//
+// We don't actually implement the angular kalman filter because the IMU is so
+// good. We give the solver an initial position and bias, and let it solve from
+// there. This lets us represent drift that is linear in time, which should be
+// good enough for ~1 minute calibration.
+//
+// TODO(austin): Kalman smoother ala
+// https://stanford.edu/~boyd/papers/pdf/auto_ks.pdf should allow for better
+// parallelism, and since we aren't causal, will take that into account a lot
+// better.
+
+// This class takes the initial parameters and biases, and computes the error
+// between the measured and expected camera readings. When optimized, this
+// gives us a cost function to minimize.
+template <typename Scalar>
+class CeresPoseFilter : public CalibrationDataObserver {
public:
- PoseFilter()
+ CeresPoseFilter(Eigen::Quaternion<Scalar> initial_orientation,
+ Eigen::Quaternion<Scalar> imu_to_camera,
+ Eigen::Matrix<Scalar, 3, 1> imu_bias)
: accel_(Eigen::Matrix<double, 3, 1>::Zero()),
omega_(Eigen::Matrix<double, 3, 1>::Zero()),
- x_hat_(Eigen::Matrix<double, 9, 1>::Zero()),
- q_(Eigen::Matrix<double, 9, 9>::Zero()) {}
+ imu_bias_(imu_bias),
+ orientation_(initial_orientation),
+ x_hat_(Eigen::Matrix<Scalar, 6, 1>::Zero()),
+ p_(Eigen::Matrix<Scalar, 6, 6>::Zero()),
+ imu_to_camera_(imu_to_camera) {}
+
+ virtual void ObserveCameraUpdate(distributed_clock::time_point /*t*/,
+ Eigen::Vector3d /*board_to_camera_rotation*/,
+ Eigen::Quaternion<Scalar> /*imu_to_world*/) {
+ }
void UpdateCamera(distributed_clock::time_point t,
std::pair<Eigen::Vector3d, Eigen::Vector3d> rt) override {
Integrate(t);
- // TODO(austin): take the sample.
- LOG(INFO) << t << " Camera " << rt.second.transpose();
+
+ Eigen::Quaternion<Scalar> board_to_camera(
+ frc971::controls::ToQuaternionFromRotationVector(rt.first)
+ .cast<Scalar>());
+
+ // This converts us from (facing the board),
+ // x right, y up, z towards us -> x right, y away, z up.
+ // Confirmed to be right.
+ Eigen::Quaternion<Scalar> board_to_world(
+ Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX()).cast<Scalar>());
+
+ // Want world -> imu rotation.
+ // world <- board <- camera <- imu.
+ const Eigen::Quaternion<Scalar> imu_to_world =
+ board_to_world * board_to_camera.inverse() * imu_to_camera_;
+
+ const Eigen::Quaternion<Scalar> error(imu_to_world.inverse() *
+ orientation());
+
+ errors_.emplace_back(
+ Eigen::Matrix<Scalar, 3, 1>(error.x(), error.y(), error.z()));
+
+ ObserveCameraUpdate(t, rt.first, imu_to_world);
}
+ virtual void ObserveIMUUpdate(
+ distributed_clock::time_point /*t*/,
+ std::pair<Eigen::Vector3d, Eigen::Vector3d> /*wa*/) {}
+
void UpdateIMU(distributed_clock::time_point t,
std::pair<Eigen::Vector3d, Eigen::Vector3d> wa) override {
Integrate(t);
omega_ = wa.first;
accel_ = wa.second;
- LOG(INFO) << t << " IMU " << wa.first.transpose();
+
+ ObserveIMUUpdate(t, wa);
}
+ const Eigen::Quaternion<Scalar> &orientation() const { return orientation_; }
+
+ std::vector<Eigen::Matrix<Scalar, 3, 1> > errors_;
+
+ // Returns the angular errors for each camera sample.
+ size_t num_errors() const { return errors_.size(); }
+ Scalar errorx(size_t i) const { return errors_[i].x(); }
+ Scalar errory(size_t i) const { return errors_[i].y(); }
+ Scalar errorz(size_t i) const { return errors_[i].z(); }
+
private:
- void Integrate(distributed_clock::time_point t) { LOG(INFO) << t; }
+ Eigen::Matrix<Scalar, 46, 1> Pack(Eigen::Quaternion<Scalar> q,
+ Eigen::Matrix<Scalar, 6, 1> x_hat,
+ Eigen::Matrix<Scalar, 6, 6> p) {
+ Eigen::Matrix<Scalar, 46, 1> result = Eigen::Matrix<Scalar, 46, 1>::Zero();
+ result.template block<4, 1>(0, 0) = q.coeffs();
+ result.template block<6, 1>(4, 0) = x_hat;
+ result.template block<36, 1>(10, 0) =
+ Eigen::Map<Eigen::Matrix<Scalar, 36, 1> >(p.data(), p.size());
+
+ return result;
+ }
+
+ std::tuple<Eigen::Quaternion<Scalar>, Eigen::Matrix<Scalar, 6, 1>,
+ Eigen::Matrix<Scalar, 6, 6> >
+ UnPack(Eigen::Matrix<Scalar, 46, 1> input) {
+ Eigen::Quaternion<Scalar> q(input.template block<4, 1>(0, 0));
+ Eigen::Matrix<Scalar, 6, 1> x_hat(input.template block<6, 1>(4, 0));
+ Eigen::Matrix<Scalar, 6, 6> p =
+ Eigen::Map<Eigen::Matrix<Scalar, 6, 6> >(input.data() + 10, 6, 6);
+ return std::make_tuple(q, x_hat, p);
+ }
+
+ Eigen::Matrix<Scalar, 46, 1> Derivitive(
+ const Eigen::Matrix<Scalar, 46, 1> &input) {
+ auto [q, x_hat, p] = UnPack(input);
+
+ Eigen::Quaternion<Scalar> omega_q;
+ omega_q.w() = Scalar(0.0);
+ omega_q.vec() = 0.5 * (omega_.cast<Scalar>() - imu_bias_);
+ Eigen::Matrix<Scalar, 4, 1> q_dot = (q * omega_q).coeffs();
+
+ Eigen::Matrix<Scalar, 6, 1> x_hat_dot = Eigen::Matrix<Scalar, 6, 1>::Zero();
+ x_hat_dot(0, 0) = x_hat(3, 0);
+ x_hat_dot(1, 0) = x_hat(4, 0);
+ x_hat_dot(2, 0) = x_hat(5, 0);
+ x_hat_dot.template block<3, 1>(3, 0) = accel_.cast<Scalar>();
+
+ Eigen::Matrix<Scalar, 6, 6> p_dot = Eigen::Matrix<Scalar, 6, 6>::Zero();
+
+ return Pack(Eigen::Quaternion<Scalar>(q_dot), x_hat_dot, p_dot);
+ }
+
+ virtual void ObserveIntegrated(distributed_clock::time_point /*t*/,
+ Eigen::Matrix<Scalar, 6, 1> /*x_hat*/,
+ Eigen::Quaternion<Scalar> /*orientation*/) {}
+
+ void Integrate(distributed_clock::time_point t) {
+ if (last_time_ != distributed_clock::min_time) {
+ Eigen::Matrix<Scalar, 46, 1> next = control_loops::RungeKutta(
+ [this](auto r) { return Derivitive(r); },
+ Pack(orientation_, x_hat_, p_),
+ aos::time::DurationInSeconds(t - last_time_));
+
+ std::tie(orientation_, x_hat_, p_) = UnPack(next);
+
+ // Normalize q so it doesn't drift.
+ orientation_.normalize();
+ }
+
+ last_time_ = t;
+ ObserveIntegrated(t, x_hat_, orientation_);
+ }
Eigen::Matrix<double, 3, 1> accel_;
Eigen::Matrix<double, 3, 1> omega_;
+ Eigen::Matrix<Scalar, 3, 1> imu_bias_;
- // TODO(austin): Actually use these. Or make a new "callback" object which
- // has these.
- Eigen::Matrix<double, 9, 1> x_hat_;
- Eigen::Matrix<double, 9, 9> q_;
+ Eigen::Quaternion<Scalar> orientation_;
+ Eigen::Matrix<Scalar, 6, 1> x_hat_;
+ Eigen::Matrix<Scalar, 6, 6> p_;
+ distributed_clock::time_point last_time_ = distributed_clock::min_time;
- // Proposed filter states:
+ Eigen::Quaternion<Scalar> imu_to_camera_;
+
+ // States outside the KF:
+ // orientation quaternion
+ //
// States:
// xyz position
// xyz velocity
- // orientation rotation vector
//
// Inputs
// xyz accel
@@ -81,6 +219,181 @@
// orientation rotation vector
};
+// Subclass of the filter above which has plotting. This keeps debug code and
+// actual code separate.
+class PoseFilter : public CeresPoseFilter<double> {
+ public:
+ PoseFilter(Eigen::Quaternion<double> initial_orientation,
+ Eigen::Quaternion<double> imu_to_camera,
+ Eigen::Matrix<double, 3, 1> imu_bias)
+ : CeresPoseFilter<double>(initial_orientation, imu_to_camera, imu_bias) {}
+
+ void Plot() {
+ std::vector<double> x;
+ std::vector<double> y;
+ std::vector<double> z;
+ for (const Eigen::Quaternion<double> &q : orientations_) {
+ Eigen::Matrix<double, 3, 1> rotation_vector =
+ frc971::controls::ToRotationVectorFromQuaternion(q);
+ x.emplace_back(rotation_vector(0, 0));
+ y.emplace_back(rotation_vector(1, 0));
+ z.emplace_back(rotation_vector(2, 0));
+ }
+ frc971::analysis::Plotter plotter;
+ plotter.AddFigure("position");
+ plotter.AddLine(times_, x, "x_hat(0)");
+ plotter.AddLine(times_, y, "x_hat(1)");
+ plotter.AddLine(times_, z, "x_hat(2)");
+ plotter.AddLine(ct, cx, "Camera x");
+ plotter.AddLine(ct, cy, "Camera y");
+ plotter.AddLine(ct, cz, "Camera z");
+ plotter.AddLine(ct, cerrx, "Camera error x");
+ plotter.AddLine(ct, cerry, "Camera error y");
+ plotter.AddLine(ct, cerrz, "Camera error z");
+ plotter.Publish();
+
+ plotter.AddFigure("error");
+ plotter.AddLine(times_, x, "x_hat(0)");
+ plotter.AddLine(times_, y, "x_hat(1)");
+ plotter.AddLine(times_, z, "x_hat(2)");
+ plotter.AddLine(ct, cerrx, "Camera error x");
+ plotter.AddLine(ct, cerry, "Camera error y");
+ plotter.AddLine(ct, cerrz, "Camera error z");
+ plotter.Publish();
+
+ plotter.AddFigure("imu");
+ plotter.AddLine(ct, world_gravity_x, "world_gravity(0)");
+ plotter.AddLine(ct, world_gravity_y, "world_gravity(1)");
+ plotter.AddLine(ct, world_gravity_z, "world_gravity(2)");
+ plotter.AddLine(imut, imu_x, "imu x");
+ plotter.AddLine(imut, imu_y, "imu y");
+ plotter.AddLine(imut, imu_z, "imu z");
+ plotter.Publish();
+
+ plotter.AddFigure("raw");
+ plotter.AddLine(imut, imu_x, "imu x");
+ plotter.AddLine(imut, imu_y, "imu y");
+ plotter.AddLine(imut, imu_z, "imu z");
+ plotter.AddLine(imut, imu_ratex, "omega x");
+ plotter.AddLine(imut, imu_ratey, "omega y");
+ plotter.AddLine(imut, imu_ratez, "omega z");
+ plotter.AddLine(ct, raw_cx, "Camera x");
+ plotter.AddLine(ct, raw_cy, "Camera y");
+ plotter.AddLine(ct, raw_cz, "Camera z");
+ plotter.Publish();
+
+ plotter.Spin();
+ }
+
+ void ObserveIntegrated(distributed_clock::time_point t,
+ Eigen::Matrix<double, 6, 1> x_hat,
+ Eigen::Quaternion<double> orientation) override {
+ times_.emplace_back(chrono::duration<double>(t.time_since_epoch()).count());
+ x_hats_.emplace_back(x_hat);
+ orientations_.emplace_back(orientation);
+ }
+
+ void ObserveIMUUpdate(
+ distributed_clock::time_point t,
+ std::pair<Eigen::Vector3d, Eigen::Vector3d> wa) override {
+ imut.emplace_back(chrono::duration<double>(t.time_since_epoch()).count());
+ imu_ratex.emplace_back(wa.first.x());
+ imu_ratey.emplace_back(wa.first.y());
+ imu_ratez.emplace_back(wa.first.z());
+ imu_x.emplace_back(wa.second.x());
+ imu_y.emplace_back(wa.second.y());
+ imu_z.emplace_back(wa.second.z());
+
+ last_accel_ = wa.second;
+ }
+
+ void ObserveCameraUpdate(distributed_clock::time_point t,
+ Eigen::Vector3d board_to_camera_rotation,
+ Eigen::Quaternion<double> imu_to_world) override {
+ raw_cx.emplace_back(board_to_camera_rotation(0, 0));
+ raw_cy.emplace_back(board_to_camera_rotation(1, 0));
+ raw_cz.emplace_back(board_to_camera_rotation(2, 0));
+
+ Eigen::Matrix<double, 3, 1> rotation_vector =
+ frc971::controls::ToRotationVectorFromQuaternion(imu_to_world);
+ ct.emplace_back(chrono::duration<double>(t.time_since_epoch()).count());
+
+ Eigen::Matrix<double, 3, 1> cerr =
+ frc971::controls::ToRotationVectorFromQuaternion(
+ imu_to_world.inverse() * orientation());
+
+ cx.emplace_back(rotation_vector(0, 0));
+ cy.emplace_back(rotation_vector(1, 0));
+ cz.emplace_back(rotation_vector(2, 0));
+
+ cerrx.emplace_back(cerr(0, 0));
+ cerry.emplace_back(cerr(1, 0));
+ cerrz.emplace_back(cerr(2, 0));
+
+ const Eigen::Vector3d world_gravity = imu_to_world * last_accel_;
+
+ world_gravity_x.emplace_back(world_gravity.x());
+ world_gravity_y.emplace_back(world_gravity.y());
+ world_gravity_z.emplace_back(world_gravity.z());
+ }
+
+ std::vector<double> ct;
+ std::vector<double> cx;
+ std::vector<double> cy;
+ std::vector<double> cz;
+ std::vector<double> raw_cx;
+ std::vector<double> raw_cy;
+ std::vector<double> raw_cz;
+ std::vector<double> cerrx;
+ std::vector<double> cerry;
+ std::vector<double> cerrz;
+
+ std::vector<double> world_gravity_x;
+ std::vector<double> world_gravity_y;
+ std::vector<double> world_gravity_z;
+ std::vector<double> imu_x;
+ std::vector<double> imu_y;
+ std::vector<double> imu_z;
+
+ std::vector<double> imut;
+ std::vector<double> imu_ratex;
+ std::vector<double> imu_ratey;
+ std::vector<double> imu_ratez;
+
+ std::vector<double> times_;
+ std::vector<Eigen::Matrix<double, 6, 1> > x_hats_;
+ std::vector<Eigen::Quaternion<double> > orientations_;
+
+ Eigen::Matrix<double, 3, 1> last_accel_ = Eigen::Matrix<double, 3, 1>::Zero();
+};
+
+// Adapter class from the KF above to a Ceres cost function.
+struct CostFunctor {
+ CostFunctor(CalibrationData *d) : data(d) {}
+
+ CalibrationData *data;
+
+ template <typename S>
+ bool operator()(const S *const q1, const S *const q2, const S *const v,
+ S *residual) const {
+ Eigen::Quaternion<S> initial_orientation(q1[3], q1[0], q1[1], q1[2]);
+ Eigen::Quaternion<S> mounting_orientation(q2[3], q2[0], q2[1], q2[2]);
+ Eigen::Matrix<S, 3, 1> imu_bias(v[0], v[1], v[2]);
+
+ CeresPoseFilter<S> filter(initial_orientation, mounting_orientation,
+ imu_bias);
+ data->ReviewData(&filter);
+
+ for (size_t i = 0; i < filter.num_errors(); ++i) {
+ residual[3 * i + 0] = filter.errorx(i);
+ residual[3 * i + 1] = filter.errory(i);
+ residual[3 * i + 2] = filter.errorz(i);
+ }
+
+ return true;
+ }
+};
+
void Main(int argc, char **argv) {
CalibrationData data;
@@ -124,8 +437,71 @@
LOG(INFO) << "Done with event_loop running";
// And now we have it, we can start processing it.
+ Eigen::Quaternion<double> nominal_initial_orientation(
+ frc971::controls::ToQuaternionFromRotationVector(
+ Eigen::Vector3d(0.0, 0.0, M_PI)));
+ Eigen::Quaternion<double> nominal_imu_to_camera(
+ Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitX()));
+
+ Eigen::Quaternion<double> initial_orientation =
+ Eigen::Quaternion<double>::Identity();
+ Eigen::Quaternion<double> imu_to_camera =
+ Eigen::Quaternion<double>::Identity();
+ Eigen::Vector3d imu_bias = Eigen::Vector3d::Zero();
+
{
- PoseFilter filter;
+ ceres::Problem problem;
+
+ ceres::EigenQuaternionParameterization *quaternion_local_parameterization =
+ new ceres::EigenQuaternionParameterization();
+ // Set up the only cost function (also known as residual). This uses
+ // auto-differentiation to obtain the derivative (jacobian).
+
+ ceres::CostFunction *cost_function =
+ new ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 4, 4, 3>(
+ new CostFunctor(&data), data.camera_samples_size() * 3);
+ problem.AddResidualBlock(cost_function, nullptr,
+ initial_orientation.coeffs().data(),
+ imu_to_camera.coeffs().data(), imu_bias.data());
+ problem.SetParameterization(initial_orientation.coeffs().data(),
+ quaternion_local_parameterization);
+ problem.SetParameterization(imu_to_camera.coeffs().data(),
+ quaternion_local_parameterization);
+ for (int i = 0; i < 3; ++i) {
+ problem.SetParameterLowerBound(imu_bias.data(), i, -0.05);
+ problem.SetParameterUpperBound(imu_bias.data(), i, 0.05);
+ }
+
+ // Run the solver!
+ ceres::Solver::Options options;
+ options.minimizer_progress_to_stdout = true;
+ options.gradient_tolerance = 1e-12;
+ options.function_tolerance = 1e-16;
+ options.parameter_tolerance = 1e-12;
+ ceres::Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ LOG(INFO) << summary.FullReport();
+
+ LOG(INFO) << "Nominal initial_orientation "
+ << nominal_initial_orientation.coeffs().transpose();
+ LOG(INFO) << "Nominal imu_to_camera "
+ << nominal_imu_to_camera.coeffs().transpose();
+
+ LOG(INFO) << "initial_orientation "
+ << initial_orientation.coeffs().transpose();
+ LOG(INFO) << "imu_to_camera " << imu_to_camera.coeffs().transpose();
+ LOG(INFO) << "imu_to_camera(rotation) "
+ << frc971::controls::ToRotationVectorFromQuaternion(imu_to_camera)
+ .transpose();
+ LOG(INFO) << "imu_to_camera delta "
+ << frc971::controls::ToRotationVectorFromQuaternion(
+ imu_to_camera * nominal_imu_to_camera.inverse())
+ .transpose();
+ LOG(INFO) << "imu_bias " << imu_bias.transpose();
+ }
+
+ {
+ PoseFilter filter(initial_orientation, imu_to_camera, imu_bias);
data.ReviewData(&filter);
}
}