1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2013 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: sameeragarwal (at) google.com (Sameer Agarwal) 30 31 #include <cmath> 32 #include "ceres/autodiff_local_parameterization.h" 33 #include "ceres/fpclassify.h" 34 #include "ceres/local_parameterization.h" 35 #include "ceres/rotation.h" 36 #include "gtest/gtest.h" 37 38 namespace ceres { 39 namespace internal { 40 41 struct IdentityPlus { 42 template <typename T> 43 bool operator()(const T* x, const T* delta, T* x_plus_delta) const { 44 for (int i = 0; i < 3; ++i) { 45 x_plus_delta[i] = x[i] + delta[i]; 46 } 47 return true; 48 } 49 }; 50 51 TEST(AutoDiffLocalParameterizationTest, IdentityParameterization) { 52 AutoDiffLocalParameterization<IdentityPlus, 3, 3> 53 parameterization; 54 55 double x[3] = {1.0, 2.0, 3.0}; 56 double delta[3] = {0.0, 1.0, 2.0}; 57 double x_plus_delta[3] = {0.0, 0.0, 0.0}; 58 parameterization.Plus(x, delta, x_plus_delta); 59 60 EXPECT_EQ(x_plus_delta[0], 1.0); 61 EXPECT_EQ(x_plus_delta[1], 3.0); 62 EXPECT_EQ(x_plus_delta[2], 5.0); 63 64 double jacobian[9]; 65 parameterization.ComputeJacobian(x, jacobian); 66 int k = 0; 67 for (int i = 0; i < 3; ++i) { 68 for (int j = 0; j < 3; ++j, ++k) { 69 EXPECT_EQ(jacobian[k], (i == j) ? 1.0 : 0.0); 70 } 71 } 72 } 73 74 struct ScaledPlus { 75 ScaledPlus(const double &scale_factor) 76 : scale_factor_(scale_factor) 77 {} 78 79 template <typename T> 80 bool operator()(const T* x, const T* delta, T* x_plus_delta) const { 81 for (int i = 0; i < 3; ++i) { 82 x_plus_delta[i] = x[i] + T(scale_factor_) * delta[i]; 83 } 84 return true; 85 } 86 87 const double scale_factor_; 88 }; 89 90 TEST(AutoDiffLocalParameterizationTest, ScaledParameterization) { 91 const double kTolerance = 1e-14; 92 93 AutoDiffLocalParameterization<ScaledPlus, 3, 3> 94 parameterization(new ScaledPlus(1.2345)); 95 96 double x[3] = {1.0, 2.0, 3.0}; 97 double delta[3] = {0.0, 1.0, 2.0}; 98 double x_plus_delta[3] = {0.0, 0.0, 0.0}; 99 parameterization.Plus(x, delta, x_plus_delta); 100 101 EXPECT_NEAR(x_plus_delta[0], 1.0, kTolerance); 102 EXPECT_NEAR(x_plus_delta[1], 3.2345, kTolerance); 103 EXPECT_NEAR(x_plus_delta[2], 5.469, kTolerance); 104 105 double jacobian[9]; 106 parameterization.ComputeJacobian(x, jacobian); 107 int k = 0; 108 for (int i = 0; i < 3; ++i) { 109 for (int j = 0; j < 3; ++j, ++k) { 110 EXPECT_NEAR(jacobian[k], (i == j) ? 1.2345 : 0.0, kTolerance); 111 } 112 } 113 } 114 115 struct QuaternionPlus { 116 template<typename T> 117 bool operator()(const T* x, const T* delta, T* x_plus_delta) const { 118 const T squared_norm_delta = 119 delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; 120 121 T q_delta[4]; 122 if (squared_norm_delta > T(0.0)) { 123 T norm_delta = sqrt(squared_norm_delta); 124 const T sin_delta_by_delta = sin(norm_delta) / norm_delta; 125 q_delta[0] = cos(norm_delta); 126 q_delta[1] = sin_delta_by_delta * delta[0]; 127 q_delta[2] = sin_delta_by_delta * delta[1]; 128 q_delta[3] = sin_delta_by_delta * delta[2]; 129 } else { 130 // We do not just use q_delta = [1,0,0,0] here because that is a 131 // constant and when used for automatic differentiation will 132 // lead to a zero derivative. Instead we take a first order 133 // approximation and evaluate it at zero. 134 q_delta[0] = T(1.0); 135 q_delta[1] = delta[0]; 136 q_delta[2] = delta[1]; 137 q_delta[3] = delta[2]; 138 } 139 140 QuaternionProduct(q_delta, x, x_plus_delta); 141 return true; 142 } 143 }; 144 145 void QuaternionParameterizationTestHelper(const double* x, 146 const double* delta) { 147 const double kTolerance = 1e-14; 148 double x_plus_delta_ref[4] = {0.0, 0.0, 0.0, 0.0}; 149 double jacobian_ref[12]; 150 151 152 QuaternionParameterization ref_parameterization; 153 ref_parameterization.Plus(x, delta, x_plus_delta_ref); 154 ref_parameterization.ComputeJacobian(x, jacobian_ref); 155 156 double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0}; 157 double jacobian[12]; 158 AutoDiffLocalParameterization<QuaternionPlus, 4, 3> parameterization; 159 parameterization.Plus(x, delta, x_plus_delta); 160 parameterization.ComputeJacobian(x, jacobian); 161 162 for (int i = 0; i < 4; ++i) { 163 EXPECT_NEAR(x_plus_delta[i], x_plus_delta_ref[i], kTolerance); 164 } 165 166 const double x_plus_delta_norm = 167 sqrt(x_plus_delta[0] * x_plus_delta[0] + 168 x_plus_delta[1] * x_plus_delta[1] + 169 x_plus_delta[2] * x_plus_delta[2] + 170 x_plus_delta[3] * x_plus_delta[3]); 171 172 EXPECT_NEAR(x_plus_delta_norm, 1.0, kTolerance); 173 174 for (int i = 0; i < 12; ++i) { 175 EXPECT_TRUE(IsFinite(jacobian[i])); 176 EXPECT_NEAR(jacobian[i], jacobian_ref[i], kTolerance) 177 << "Jacobian mismatch: i = " << i 178 << "\n Expected \n" << ConstMatrixRef(jacobian_ref, 4, 3) 179 << "\n Actual \n" << ConstMatrixRef(jacobian, 4, 3); 180 } 181 } 182 183 TEST(AutoDiffLocalParameterization, QuaternionParameterizationZeroTest) { 184 double x[4] = {0.5, 0.5, 0.5, 0.5}; 185 double delta[3] = {0.0, 0.0, 0.0}; 186 QuaternionParameterizationTestHelper(x, delta); 187 } 188 189 190 TEST(AutoDiffLocalParameterization, QuaternionParameterizationNearZeroTest) { 191 double x[4] = {0.52, 0.25, 0.15, 0.45}; 192 double norm_x = sqrt(x[0] * x[0] + 193 x[1] * x[1] + 194 x[2] * x[2] + 195 x[3] * x[3]); 196 for (int i = 0; i < 4; ++i) { 197 x[i] = x[i] / norm_x; 198 } 199 200 double delta[3] = {0.24, 0.15, 0.10}; 201 for (int i = 0; i < 3; ++i) { 202 delta[i] = delta[i] * 1e-14; 203 } 204 205 QuaternionParameterizationTestHelper(x, delta); 206 } 207 208 TEST(AutoDiffLocalParameterization, QuaternionParameterizationNonZeroTest) { 209 double x[4] = {0.52, 0.25, 0.15, 0.45}; 210 double norm_x = sqrt(x[0] * x[0] + 211 x[1] * x[1] + 212 x[2] * x[2] + 213 x[3] * x[3]); 214 215 for (int i = 0; i < 4; ++i) { 216 x[i] = x[i] / norm_x; 217 } 218 219 double delta[3] = {0.24, 0.15, 0.10}; 220 QuaternionParameterizationTestHelper(x, delta); 221 } 222 223 } // namespace internal 224 } // namespace ceres 225
