commit e53bf55e51ff14a64b18fa709f13a1bc963ea78c
author milind-u <milind.upadhyay@gmail.com> Sat Dec 11 14:42:00 2021 -0800
committer Austin Schuh <austin.linux@gmail.com> Sun Dec 26 14:14:18 2021 -0800
tree 7439a110476418b62c4c0ab28869031a03c27fc5
parent 921b9a85a8dc093800cb3992fe311c2ddf5d06ae

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>

frc971/control_loops/quaternion_utils_test.cc

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