diff options
Diffstat (limited to 'internal/ceres/jet_quaternion_integration_test.cc')
-rw-r--r-- | internal/ceres/jet_quaternion_integration_test.cc | 201 |
1 files changed, 201 insertions, 0 deletions
diff --git a/internal/ceres/jet_quaternion_integration_test.cc b/internal/ceres/jet_quaternion_integration_test.cc new file mode 100644 index 0000000..63101fb --- /dev/null +++ b/internal/ceres/jet_quaternion_integration_test.cc @@ -0,0 +1,201 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Tests the use of Cere's Jet type with the quaternions found in util/math/. In +// theory, the unittests for the quaternion class should be type parameterized +// to make for easier testing of instantiations of the quaternion class, but it +// is not so, and not obviously worth the work to make the switch at this time. + +#include "base/stringprintf.h" +#include "gtest/gtest.h" +#include "util/math/mathlimits.h" +#include "util/math/matrix3x3.h" +#include "util/math/quaternion.h" +#include "util/math/vector3.h" +#include "ceres/test_util.h" +#include "ceres/jet.h" +#include "ceres/jet_traits.h" + +namespace ceres { +namespace internal { + +// Use a 4-element derivative to simulate the case where each of the +// quaternion elements are derivative parameters. +typedef Jet<double, 4> J; + +struct JetTraitsTest : public ::testing::Test { + protected: + JetTraitsTest() + : a(J(1.1, 0), J(2.1, 1), J(3.1, 2), J(4.1, 3)), + b(J(0.1, 0), J(1.1, 1), J(2.1, 2), J(5.0, 3)), + double_a(a[0].a, a[1].a, a[2].a, a[3].a), + double_b(b[0].a, b[1].a, b[2].a, b[3].a) { + // The quaternions should be valid rotations, so normalize them. + a.Normalize(); + b.Normalize(); + double_a.Normalize(); + double_b.Normalize(); + } + + virtual ~JetTraitsTest() {} + + // A couple of arbitrary normalized quaternions. + Quaternion<J> a, b; + + // The equivalent of a, b but in scalar form. + Quaternion<double> double_a, double_b; +}; + +// Compare scalar multiplication to jet multiplication. Ignores derivatives. +TEST_F(JetTraitsTest, QuaternionScalarMultiplicationWorks) { + Quaternion<J> c = a * b; + Quaternion<double> double_c = double_a * double_b; + + for (int i = 0; i < 4; ++i) { + EXPECT_EQ(double_c[i], c[i].a); + } +} + +// Compare scalar slerp to jet slerp. Ignores derivatives. +TEST_F(JetTraitsTest, QuaternionScalarSlerpWorks) { + const J fraction(0.1); + Quaternion<J> c = Quaternion<J>::Slerp(a, b, fraction); + Quaternion<double> double_c = + Quaternion<double>::Slerp(double_a, double_b, fraction.a); + + for (int i = 0; i < 4; ++i) { + EXPECT_EQ(double_c[i], c[i].a); + } +} + +// On a 32-bit optimized build, the mismatch is about 1.4e-14. +double const kTolerance = 1e-13; + +void ExpectJetsClose(const J &x, const J &y) { + ExpectClose(x.a, y.a, kTolerance); + ExpectClose(x.v[0], y.v[0], kTolerance); + ExpectClose(x.v[1], y.v[1], kTolerance); + ExpectClose(x.v[2], y.v[2], kTolerance); + ExpectClose(x.v[3], y.v[3], kTolerance); +} + +void ExpectQuaternionsClose(const Quaternion<J>& x, const Quaternion<J>& y) { + for (int i = 0; i < 4; ++i) { + ExpectJetsClose(x[i], y[i]); + } +} + +// Compare jet slurp to jet slerp using identies, checking derivatives. +TEST_F(JetTraitsTest, CheckSlerpIdentitiesWithNontrivialDerivatives) { + // Do a slerp to 0.75 directly. + Quaternion<J> direct = Quaternion<J>::Slerp(a, b, J(0.75)); + + // Now go part way twice, in theory ending at the same place. + Quaternion<J> intermediate = Quaternion<J>::Slerp(a, b, J(0.5)); + Quaternion<J> indirect = Quaternion<J>::Slerp(intermediate, b, J(0.5)); + + // Check that the destination is the same, including derivatives. + ExpectQuaternionsClose(direct, indirect); +} + +TEST_F(JetTraitsTest, CheckAxisAngleIsInvertibleWithNontrivialDerivatives) { + Vector3<J> axis; + J angle; + a.GetAxisAngle(&axis, &angle); + b.SetFromAxisAngle(axis, angle); + + ExpectQuaternionsClose(a, b); +} + +TEST_F(JetTraitsTest, + CheckRotationMatrixIsInvertibleWithNontrivialDerivatives) { + Vector3<J> axis; + J angle; + Matrix3x3<J> R; + a.ToRotationMatrix(&R); + b.SetFromRotationMatrix(R); + + ExpectQuaternionsClose(a, b); +} + +// This doesn't check correctnenss, only that the instantiation compiles. +TEST_F(JetTraitsTest, CheckRotationBetweenIsCompilable) { + // Get two arbitrary vectors x and y. + Vector3<J> x, y; + J ignored_angle; + a.GetAxisAngle(&x, &ignored_angle); + b.GetAxisAngle(&y, &ignored_angle); + + Quaternion<J> between_x_and_y = Quaternion<J>::RotationBetween(x, y); + + // Prevent optimizing this away. + EXPECT_NE(between_x_and_y[0].a, 0.0); +} + +TEST_F(JetTraitsTest, CheckRotatedWorksAsExpected) { + // Get two arbitrary vectors x and y. + Vector3<J> x; + J ignored_angle; + a.GetAxisAngle(&x, &ignored_angle); + + // Rotate via a quaternion. + Vector3<J> y = b.Rotated(x); + + // Rotate via a rotation matrix. + Matrix3x3<J> R; + b.ToRotationMatrix(&R); + Vector3<J> yp = R * x; + + ExpectJetsClose(yp[0], y[0]); + ExpectJetsClose(yp[1], y[1]); + ExpectJetsClose(yp[2], y[2]); +} + +TEST_F(JetTraitsTest, CheckRotatedWorksAsExpectedWithDoubles) { + // Get two arbitrary vectors x and y. + Vector3<double> x; + double ignored_angle; + double_a.GetAxisAngle(&x, &ignored_angle); + + // Rotate via a quaternion. + Vector3<double> y = double_b.Rotated(x); + + // Rotate via a rotation matrix. + Matrix3x3<double> R; + double_b.ToRotationMatrix(&R); + Vector3<double> yp = R * x; + + ExpectClose(yp[0], y[0], kTolerance); + ExpectClose(yp[1], y[1], kTolerance); + ExpectClose(yp[2], y[2], kTolerance); +} + +} // namespace internal +} // namespace ceres |