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_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.