1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3 // http://code.google.com/p/ceres-solver/ 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are met: 7 // 8 // * Redistributions of source code must retain the above copyright notice, 9 // this list of conditions and the following disclaimer. 10 // * Redistributions in binary form must reproduce the above copyright notice, 11 // this list of conditions and the following disclaimer in the documentation 12 // and/or other materials provided with the distribution. 13 // * Neither the name of Google Inc. nor the names of its contributors may be 14 // used to endorse or promote products derived from this software without 15 // specific prior written permission. 16 // 17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 // POSSIBILITY OF SUCH DAMAGE. 28 // 29 // Author: keir (at) google.com (Keir Mierle) 30 // 31 // Tests the use of Cere's Jet type with the quaternions found in util/math/. In 32 // theory, the unittests for the quaternion class should be type parameterized 33 // to make for easier testing of instantiations of the quaternion class, but it 34 // is not so, and not obviously worth the work to make the switch at this time. 35 36 #include "base/stringprintf.h" 37 #include "gtest/gtest.h" 38 #include "util/math/mathlimits.h" 39 #include "util/math/matrix3x3.h" 40 #include "util/math/quaternion.h" 41 #include "util/math/vector3.h" 42 #include "ceres/test_util.h" 43 #include "ceres/jet.h" 44 #include "ceres/jet_traits.h" 45 46 namespace ceres { 47 namespace internal { 48 49 // Use a 4-element derivative to simulate the case where each of the 50 // quaternion elements are derivative parameters. 51 typedef Jet<double, 4> J; 52 53 struct JetTraitsTest : public ::testing::Test { 54 protected: 55 JetTraitsTest() 56 : a(J(1.1, 0), J(2.1, 1), J(3.1, 2), J(4.1, 3)), 57 b(J(0.1, 0), J(1.1, 1), J(2.1, 2), J(5.0, 3)), 58 double_a(a[0].a, a[1].a, a[2].a, a[3].a), 59 double_b(b[0].a, b[1].a, b[2].a, b[3].a) { 60 // The quaternions should be valid rotations, so normalize them. 61 a.Normalize(); 62 b.Normalize(); 63 double_a.Normalize(); 64 double_b.Normalize(); 65 } 66 67 virtual ~JetTraitsTest() {} 68 69 // A couple of arbitrary normalized quaternions. 70 Quaternion<J> a, b; 71 72 // The equivalent of a, b but in scalar form. 73 Quaternion<double> double_a, double_b; 74 }; 75 76 // Compare scalar multiplication to jet multiplication. Ignores derivatives. 77 TEST_F(JetTraitsTest, QuaternionScalarMultiplicationWorks) { 78 Quaternion<J> c = a * b; 79 Quaternion<double> double_c = double_a * double_b; 80 81 for (int i = 0; i < 4; ++i) { 82 EXPECT_EQ(double_c[i], c[i].a); 83 } 84 } 85 86 // Compare scalar slerp to jet slerp. Ignores derivatives. 87 TEST_F(JetTraitsTest, QuaternionScalarSlerpWorks) { 88 const J fraction(0.1); 89 Quaternion<J> c = Quaternion<J>::Slerp(a, b, fraction); 90 Quaternion<double> double_c = 91 Quaternion<double>::Slerp(double_a, double_b, fraction.a); 92 93 for (int i = 0; i < 4; ++i) { 94 EXPECT_EQ(double_c[i], c[i].a); 95 } 96 } 97 98 // On a 32-bit optimized build, the mismatch is about 1.4e-14. 99 double const kTolerance = 1e-13; 100 101 void ExpectJetsClose(const J &x, const J &y) { 102 ExpectClose(x.a, y.a, kTolerance); 103 ExpectClose(x.v[0], y.v[0], kTolerance); 104 ExpectClose(x.v[1], y.v[1], kTolerance); 105 ExpectClose(x.v[2], y.v[2], kTolerance); 106 ExpectClose(x.v[3], y.v[3], kTolerance); 107 } 108 109 void ExpectQuaternionsClose(const Quaternion<J>& x, const Quaternion<J>& y) { 110 for (int i = 0; i < 4; ++i) { 111 ExpectJetsClose(x[i], y[i]); 112 } 113 } 114 115 // Compare jet slurp to jet slerp using identies, checking derivatives. 116 TEST_F(JetTraitsTest, CheckSlerpIdentitiesWithNontrivialDerivatives) { 117 // Do a slerp to 0.75 directly. 118 Quaternion<J> direct = Quaternion<J>::Slerp(a, b, J(0.75)); 119 120 // Now go part way twice, in theory ending at the same place. 121 Quaternion<J> intermediate = Quaternion<J>::Slerp(a, b, J(0.5)); 122 Quaternion<J> indirect = Quaternion<J>::Slerp(intermediate, b, J(0.5)); 123 124 // Check that the destination is the same, including derivatives. 125 ExpectQuaternionsClose(direct, indirect); 126 } 127 128 TEST_F(JetTraitsTest, CheckAxisAngleIsInvertibleWithNontrivialDerivatives) { 129 Vector3<J> axis; 130 J angle; 131 a.GetAxisAngle(&axis, &angle); 132 b.SetFromAxisAngle(axis, angle); 133 134 ExpectQuaternionsClose(a, b); 135 } 136 137 TEST_F(JetTraitsTest, 138 CheckRotationMatrixIsInvertibleWithNontrivialDerivatives) { 139 Vector3<J> axis; 140 J angle; 141 Matrix3x3<J> R; 142 a.ToRotationMatrix(&R); 143 b.SetFromRotationMatrix(R); 144 145 ExpectQuaternionsClose(a, b); 146 } 147 148 // This doesn't check correctnenss, only that the instantiation compiles. 149 TEST_F(JetTraitsTest, CheckRotationBetweenIsCompilable) { 150 // Get two arbitrary vectors x and y. 151 Vector3<J> x, y; 152 J ignored_angle; 153 a.GetAxisAngle(&x, &ignored_angle); 154 b.GetAxisAngle(&y, &ignored_angle); 155 156 Quaternion<J> between_x_and_y = Quaternion<J>::RotationBetween(x, y); 157 158 // Prevent optimizing this away. 159 EXPECT_NE(between_x_and_y[0].a, 0.0); 160 } 161 162 TEST_F(JetTraitsTest, CheckRotatedWorksAsExpected) { 163 // Get two arbitrary vectors x and y. 164 Vector3<J> x; 165 J ignored_angle; 166 a.GetAxisAngle(&x, &ignored_angle); 167 168 // Rotate via a quaternion. 169 Vector3<J> y = b.Rotated(x); 170 171 // Rotate via a rotation matrix. 172 Matrix3x3<J> R; 173 b.ToRotationMatrix(&R); 174 Vector3<J> yp = R * x; 175 176 ExpectJetsClose(yp[0], y[0]); 177 ExpectJetsClose(yp[1], y[1]); 178 ExpectJetsClose(yp[2], y[2]); 179 } 180 181 TEST_F(JetTraitsTest, CheckRotatedWorksAsExpectedWithDoubles) { 182 // Get two arbitrary vectors x and y. 183 Vector3<double> x; 184 double ignored_angle; 185 double_a.GetAxisAngle(&x, &ignored_angle); 186 187 // Rotate via a quaternion. 188 Vector3<double> y = double_b.Rotated(x); 189 190 // Rotate via a rotation matrix. 191 Matrix3x3<double> R; 192 double_b.ToRotationMatrix(&R); 193 Vector3<double> yp = R * x; 194 195 ExpectClose(yp[0], y[0], kTolerance); 196 ExpectClose(yp[1], y[1], kTolerance); 197 ExpectClose(yp[2], y[2], kTolerance); 198 } 199 200 } // namespace internal 201 } // namespace ceres 202
