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: sameeragarwal (at) google.com (Sameer Agarwal) 30 31 #include <cmath> 32 #include "ceres/fpclassify.h" 33 #include "ceres/internal/autodiff.h" 34 #include "ceres/internal/eigen.h" 35 #include "ceres/local_parameterization.h" 36 #include "ceres/rotation.h" 37 #include "gtest/gtest.h" 38 39 namespace ceres { 40 namespace internal { 41 42 TEST(IdentityParameterization, EverythingTest) { 43 IdentityParameterization parameterization(3); 44 EXPECT_EQ(parameterization.GlobalSize(), 3); 45 EXPECT_EQ(parameterization.LocalSize(), 3); 46 47 double x[3] = {1.0, 2.0, 3.0}; 48 double delta[3] = {0.0, 1.0, 2.0}; 49 double x_plus_delta[3] = {0.0, 0.0, 0.0}; 50 parameterization.Plus(x, delta, x_plus_delta); 51 EXPECT_EQ(x_plus_delta[0], 1.0); 52 EXPECT_EQ(x_plus_delta[1], 3.0); 53 EXPECT_EQ(x_plus_delta[2], 5.0); 54 55 double jacobian[9]; 56 parameterization.ComputeJacobian(x, jacobian); 57 int k = 0; 58 for (int i = 0; i < 3; ++i) { 59 for (int j = 0; j < 3; ++j, ++k) { 60 EXPECT_EQ(jacobian[k], (i == j) ? 1.0 : 0.0); 61 } 62 } 63 } 64 65 TEST(SubsetParameterization, DeathTests) { 66 vector<int> constant_parameters; 67 EXPECT_DEATH_IF_SUPPORTED( 68 SubsetParameterization parameterization(1, constant_parameters), 69 "at least"); 70 71 constant_parameters.push_back(0); 72 EXPECT_DEATH_IF_SUPPORTED( 73 SubsetParameterization parameterization(1, constant_parameters), 74 "Number of parameters"); 75 76 constant_parameters.push_back(1); 77 EXPECT_DEATH_IF_SUPPORTED( 78 SubsetParameterization parameterization(2, constant_parameters), 79 "Number of parameters"); 80 81 constant_parameters.push_back(1); 82 EXPECT_DEATH_IF_SUPPORTED( 83 SubsetParameterization parameterization(2, constant_parameters), 84 "duplicates"); 85 } 86 87 TEST(SubsetParameterization, NormalFunctionTest) { 88 double x[4] = {1.0, 2.0, 3.0, 4.0}; 89 for (int i = 0; i < 4; ++i) { 90 vector<int> constant_parameters; 91 constant_parameters.push_back(i); 92 SubsetParameterization parameterization(4, constant_parameters); 93 double delta[3] = {1.0, 2.0, 3.0}; 94 double x_plus_delta[4] = {0.0, 0.0, 0.0}; 95 96 parameterization.Plus(x, delta, x_plus_delta); 97 int k = 0; 98 for (int j = 0; j < 4; ++j) { 99 if (j == i) { 100 EXPECT_EQ(x_plus_delta[j], x[j]); 101 } else { 102 EXPECT_EQ(x_plus_delta[j], x[j] + delta[k++]); 103 } 104 } 105 106 double jacobian[4 * 3]; 107 parameterization.ComputeJacobian(x, jacobian); 108 int delta_cursor = 0; 109 int jacobian_cursor = 0; 110 for (int j = 0; j < 4; ++j) { 111 if (j != i) { 112 for (int k = 0; k < 3; ++k, jacobian_cursor++) { 113 EXPECT_EQ(jacobian[jacobian_cursor], delta_cursor == k ? 1.0 : 0.0); 114 } 115 ++delta_cursor; 116 } else { 117 for (int k = 0; k < 3; ++k, jacobian_cursor++) { 118 EXPECT_EQ(jacobian[jacobian_cursor], 0.0); 119 } 120 } 121 } 122 }; 123 } 124 125 // Functor needed to implement automatically differentiated Plus for 126 // quaternions. 127 struct QuaternionPlus { 128 template<typename T> 129 bool operator()(const T* x, const T* delta, T* x_plus_delta) const { 130 const T squared_norm_delta = 131 delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; 132 133 T q_delta[4]; 134 if (squared_norm_delta > T(0.0)) { 135 T norm_delta = sqrt(squared_norm_delta); 136 const T sin_delta_by_delta = sin(norm_delta) / norm_delta; 137 q_delta[0] = cos(norm_delta); 138 q_delta[1] = sin_delta_by_delta * delta[0]; 139 q_delta[2] = sin_delta_by_delta * delta[1]; 140 q_delta[3] = sin_delta_by_delta * delta[2]; 141 } else { 142 // We do not just use q_delta = [1,0,0,0] here because that is a 143 // constant and when used for automatic differentiation will 144 // lead to a zero derivative. Instead we take a first order 145 // approximation and evaluate it at zero. 146 q_delta[0] = T(1.0); 147 q_delta[1] = delta[0]; 148 q_delta[2] = delta[1]; 149 q_delta[3] = delta[2]; 150 } 151 152 QuaternionProduct(q_delta, x, x_plus_delta); 153 return true; 154 } 155 }; 156 157 void QuaternionParameterizationTestHelper(const double* x, 158 const double* delta, 159 const double* q_delta) { 160 const double kTolerance = 1e-14; 161 double x_plus_delta_ref[4] = {0.0, 0.0, 0.0, 0.0}; 162 QuaternionProduct(q_delta, x, x_plus_delta_ref); 163 164 double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0}; 165 QuaternionParameterization param; 166 param.Plus(x, delta, x_plus_delta); 167 for (int i = 0; i < 4; ++i) { 168 EXPECT_NEAR(x_plus_delta[i], x_plus_delta_ref[i], kTolerance); 169 } 170 171 const double x_plus_delta_norm = 172 sqrt(x_plus_delta[0] * x_plus_delta[0] + 173 x_plus_delta[1] * x_plus_delta[1] + 174 x_plus_delta[2] * x_plus_delta[2] + 175 x_plus_delta[3] * x_plus_delta[3]); 176 177 EXPECT_NEAR(x_plus_delta_norm, 1.0, kTolerance); 178 179 double jacobian_ref[12]; 180 double zero_delta[3] = {0.0, 0.0, 0.0}; 181 const double* parameters[2] = {x, zero_delta}; 182 double* jacobian_array[2] = { NULL, jacobian_ref }; 183 184 // Autodiff jacobian at delta_x = 0. 185 internal::AutoDiff<QuaternionPlus, double, 4, 3>::Differentiate( 186 QuaternionPlus(), parameters, 4, x_plus_delta, jacobian_array); 187 188 double jacobian[12]; 189 param.ComputeJacobian(x, jacobian); 190 for (int i = 0; i < 12; ++i) { 191 EXPECT_TRUE(IsFinite(jacobian[i])); 192 EXPECT_NEAR(jacobian[i], jacobian_ref[i], kTolerance) 193 << "Jacobian mismatch: i = " << i 194 << "\n Expected \n" << ConstMatrixRef(jacobian_ref, 4, 3) 195 << "\n Actual \n" << ConstMatrixRef(jacobian, 4, 3); 196 } 197 } 198 199 TEST(QuaternionParameterization, ZeroTest) { 200 double x[4] = {0.5, 0.5, 0.5, 0.5}; 201 double delta[3] = {0.0, 0.0, 0.0}; 202 double q_delta[4] = {1.0, 0.0, 0.0, 0.0}; 203 QuaternionParameterizationTestHelper(x, delta, q_delta); 204 } 205 206 207 TEST(QuaternionParameterization, NearZeroTest) { 208 double x[4] = {0.52, 0.25, 0.15, 0.45}; 209 double norm_x = sqrt(x[0] * x[0] + 210 x[1] * x[1] + 211 x[2] * x[2] + 212 x[3] * x[3]); 213 for (int i = 0; i < 4; ++i) { 214 x[i] = x[i] / norm_x; 215 } 216 217 double delta[3] = {0.24, 0.15, 0.10}; 218 for (int i = 0; i < 3; ++i) { 219 delta[i] = delta[i] * 1e-14; 220 } 221 222 double q_delta[4]; 223 q_delta[0] = 1.0; 224 q_delta[1] = delta[0]; 225 q_delta[2] = delta[1]; 226 q_delta[3] = delta[2]; 227 228 QuaternionParameterizationTestHelper(x, delta, q_delta); 229 } 230 231 TEST(QuaternionParameterization, AwayFromZeroTest) { 232 double x[4] = {0.52, 0.25, 0.15, 0.45}; 233 double norm_x = sqrt(x[0] * x[0] + 234 x[1] * x[1] + 235 x[2] * x[2] + 236 x[3] * x[3]); 237 238 for (int i = 0; i < 4; ++i) { 239 x[i] = x[i] / norm_x; 240 } 241 242 double delta[3] = {0.24, 0.15, 0.10}; 243 const double delta_norm = sqrt(delta[0] * delta[0] + 244 delta[1] * delta[1] + 245 delta[2] * delta[2]); 246 double q_delta[4]; 247 q_delta[0] = cos(delta_norm); 248 q_delta[1] = sin(delta_norm) / delta_norm * delta[0]; 249 q_delta[2] = sin(delta_norm) / delta_norm * delta[1]; 250 q_delta[3] = sin(delta_norm) / delta_norm * delta[2]; 251 252 QuaternionParameterizationTestHelper(x, delta, q_delta); 253 } 254 255 256 } // namespace internal 257 } // namespace ceres 258
