Changes to camera extrinsic calibration to improve accuracy
Adds Outlier Rejection
Do proper rotation (Quaternion) averaging
When camera sees two targets at once, store that, and store
a single observation to match with other cameras. This leads
to a lot more good connections
Removed dead code, and refactored a couple pieces, e.g., writing extrinsic file
Also refactored some of the utilities to use quaternion averaging from utils,
and move other utility functions to vision_utils_lib
Change-Id: I918ce9c937d80717daa6659abbb139006506d4cc
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/frc971/control_loops/quaternion_utils.h b/frc971/control_loops/quaternion_utils.h
index 3e40e3f4..c2c854c 100644
--- a/frc971/control_loops/quaternion_utils.h
+++ b/frc971/control_loops/quaternion_utils.h
@@ -8,19 +8,17 @@
namespace frc971::controls {
-// Function to compute the quaternion average of a bunch of quaternions. Each
-// column in the input matrix is a quaternion (optionally scaled by it's
-// weight).
-template <int SM>
-inline Eigen::Matrix<double, 4, 1> QuaternionMean(
- Eigen::Matrix<double, 4, SM> input) {
+// Helper function to extract mean quaternion from A*A^T of quaternion list
+// This allows us to support multiple formats of the input quaternion list
+inline Eigen::Matrix<double, 4, 1> ExtractQuaternionMean(
+ Eigen::Matrix<double, 4, 4> input) {
// Algorithm to compute the average of a bunch of quaternions:
- // http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf
-
- Eigen::Matrix<double, 4, 4> m = input * input.transpose();
+ // http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf
+ // See also:
+ // https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20070017872.pdf
Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>> solver;
- solver.compute(m);
+ solver.compute(input);
Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvectorsType
eigenvectors = solver.eigenvectors();
@@ -47,6 +45,37 @@
return eigenvectors.col(max_index).real().normalized();
}
+// Function to compute the quaternion average of a bunch of quaternions. Each
+// column in the 4xN input Matrix is a quaternion (optionally scaled by it's
+// weight).
+template <int N>
+inline Eigen::Matrix<double, 4, 1> QuaternionMean(
+ Eigen::Matrix<double, 4, N> quaternions) {
+ Eigen::Matrix<double, 4, 4> m = quaternions * quaternions.transpose();
+
+ return ExtractQuaternionMean(m);
+}
+
+// Function to compute the quaternion average of a bunch of quaternions.
+// This allows for passing in a variable size list (vector) of quaternions
+
+// For reference (since I've been bitten by it):
+// Eigen::Quaternion stores and prints coefficients as [x, y, z, w]
+// and initializes using a Vector4d in this order, BUT
+// initializes with scalars as Eigen::Quaternion{w, x, y, z}
+inline Eigen::Vector4d QuaternionMean(
+ std::vector<Eigen::Vector4d> quaternion_list) {
+ CHECK(quaternion_list.size() != 0)
+ << "Must have at least one quaternion to compute an average!";
+
+ Eigen::Matrix<double, 4, 4> m = Eigen::Matrix4d::Zero();
+ for (Eigen::Vector4d quaternion : quaternion_list) {
+ m += quaternion * quaternion.transpose();
+ }
+
+ return Eigen::Vector4d(ExtractQuaternionMean(m));
+}
+
// Converts from a quaternion to a rotation vector, where the rotation vector's
// direction represents the axis to rotate around and its magnitude represents
// the number of radians to rotate.
@@ -74,7 +103,7 @@
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.
+ // another resource on quaternion integration and derivatives.
Eigen::Quaternion<double> q(q_matrix);
Eigen::Quaternion<double> omega_q;
diff --git a/frc971/control_loops/quaternion_utils_test.cc b/frc971/control_loops/quaternion_utils_test.cc
index 2eaeaa0..63f4507 100644
--- a/frc971/control_loops/quaternion_utils_test.cc
+++ b/frc971/control_loops/quaternion_utils_test.cc
@@ -17,6 +17,7 @@
// return the original quaternion.
TEST(DownEstimatorTest, QuaternionMean) {
Eigen::Matrix<double, 4, 7> vectors;
+ std::vector<Eigen::Vector4d> quaternion_list;
vectors.col(0) << 0, 0, 0, 1;
for (int i = 0; i < 3; ++i) {
Eigen::Matrix<double, 4, 1> perturbation;
@@ -29,6 +30,7 @@
for (int i = 0; i < 7; ++i) {
vectors.col(i).normalize();
+ quaternion_list.push_back(Eigen::Vector4d(vectors.col(i)));
}
Eigen::Matrix<double, 4, 1> mean = QuaternionMean(vectors);
@@ -36,6 +38,15 @@
for (int i = 0; i < 4; ++i) {
EXPECT_NEAR(mean(i, 0), vectors(i, 0), 0.001) << ": Failed on index " << i;
}
+
+ // Test version of code that passes in a vector of quaternions
+ mean = QuaternionMean(quaternion_list);
+
+ for (int i = 0; i < 4; ++i) {
+ EXPECT_NEAR(mean(i, 0), quaternion_list[0](i, 0), 0.001)
+ << ": Failed on index " << i << " with mean = " << mean
+ << " and quat_list = " << quaternion_list[0];
+ }
}
// Tests that ToRotationVectorFromQuaternion works for a 0 rotation.