autodiff_local_parameterization_test.cc revision 1d2624a10e2c559f8ba9ef89eaa30832c0a83a96
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@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 38namespace ceres { 39namespace internal { 40 41struct 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 52TEST(AutoDiffLocalParameterizationTest, IdentityParameterization) { 53 AutoDiffLocalParameterization<IdentityPlus, 3, 3> 54 parameterization; 55 56 double x[3] = {1.0, 2.0, 3.0}; 57 double delta[3] = {0.0, 1.0, 2.0}; 58 double x_plus_delta[3] = {0.0, 0.0, 0.0}; 59 parameterization.Plus(x, delta, x_plus_delta); 60 61 EXPECT_EQ(x_plus_delta[0], 1.0); 62 EXPECT_EQ(x_plus_delta[1], 3.0); 63 EXPECT_EQ(x_plus_delta[2], 5.0); 64 65 double jacobian[9]; 66 parameterization.ComputeJacobian(x, jacobian); 67 int k = 0; 68 for (int i = 0; i < 3; ++i) { 69 for (int j = 0; j < 3; ++j, ++k) { 70 EXPECT_EQ(jacobian[k], (i == j) ? 1.0 : 0.0); 71 } 72 } 73} 74 75struct QuaternionPlus { 76 template<typename T> 77 bool operator()(const T* x, const T* delta, T* x_plus_delta) const { 78 const T squared_norm_delta = 79 delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; 80 81 T q_delta[4]; 82 if (squared_norm_delta > T(0.0)) { 83 T norm_delta = sqrt(squared_norm_delta); 84 const T sin_delta_by_delta = sin(norm_delta) / norm_delta; 85 q_delta[0] = cos(norm_delta); 86 q_delta[1] = sin_delta_by_delta * delta[0]; 87 q_delta[2] = sin_delta_by_delta * delta[1]; 88 q_delta[3] = sin_delta_by_delta * delta[2]; 89 } else { 90 // We do not just use q_delta = [1,0,0,0] here because that is a 91 // constant and when used for automatic differentiation will 92 // lead to a zero derivative. Instead we take a first order 93 // approximation and evaluate it at zero. 94 q_delta[0] = T(1.0); 95 q_delta[1] = delta[0]; 96 q_delta[2] = delta[1]; 97 q_delta[3] = delta[2]; 98 } 99 100 QuaternionProduct(q_delta, x, x_plus_delta); 101 return true; 102 } 103}; 104 105void QuaternionParameterizationTestHelper(const double* x, 106 const double* delta) { 107 const double kTolerance = 1e-14; 108 double x_plus_delta_ref[4] = {0.0, 0.0, 0.0, 0.0}; 109 double jacobian_ref[12]; 110 111 112 QuaternionParameterization ref_parameterization; 113 ref_parameterization.Plus(x, delta, x_plus_delta_ref); 114 ref_parameterization.ComputeJacobian(x, jacobian_ref); 115 116 double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0}; 117 double jacobian[12]; 118 AutoDiffLocalParameterization<QuaternionPlus, 4, 3> parameterization; 119 parameterization.Plus(x, delta, x_plus_delta); 120 parameterization.ComputeJacobian(x, jacobian); 121 122 for (int i = 0; i < 4; ++i) { 123 EXPECT_NEAR(x_plus_delta[i], x_plus_delta_ref[i], kTolerance); 124 } 125 126 const double x_plus_delta_norm = 127 sqrt(x_plus_delta[0] * x_plus_delta[0] + 128 x_plus_delta[1] * x_plus_delta[1] + 129 x_plus_delta[2] * x_plus_delta[2] + 130 x_plus_delta[3] * x_plus_delta[3]); 131 132 EXPECT_NEAR(x_plus_delta_norm, 1.0, kTolerance); 133 134 for (int i = 0; i < 12; ++i) { 135 EXPECT_TRUE(IsFinite(jacobian[i])); 136 EXPECT_NEAR(jacobian[i], jacobian_ref[i], kTolerance) 137 << "Jacobian mismatch: i = " << i 138 << "\n Expected \n" << ConstMatrixRef(jacobian_ref, 4, 3) 139 << "\n Actual \n" << ConstMatrixRef(jacobian, 4, 3); 140 } 141} 142 143TEST(AutoDiffLocalParameterization, QuaternionParameterizationZeroTest) { 144 double x[4] = {0.5, 0.5, 0.5, 0.5}; 145 double delta[3] = {0.0, 0.0, 0.0}; 146 QuaternionParameterizationTestHelper(x, delta); 147} 148 149 150TEST(AutoDiffLocalParameterization, QuaternionParameterizationNearZeroTest) { 151 double x[4] = {0.52, 0.25, 0.15, 0.45}; 152 double norm_x = sqrt(x[0] * x[0] + 153 x[1] * x[1] + 154 x[2] * x[2] + 155 x[3] * x[3]); 156 for (int i = 0; i < 4; ++i) { 157 x[i] = x[i] / norm_x; 158 } 159 160 double delta[3] = {0.24, 0.15, 0.10}; 161 for (int i = 0; i < 3; ++i) { 162 delta[i] = delta[i] * 1e-14; 163 } 164 165 QuaternionParameterizationTestHelper(x, delta); 166} 167 168TEST(AutoDiffLocalParameterization, QuaternionParameterizationNonZeroTest) { 169 double x[4] = {0.52, 0.25, 0.15, 0.45}; 170 double norm_x = sqrt(x[0] * x[0] + 171 x[1] * x[1] + 172 x[2] * x[2] + 173 x[3] * x[3]); 174 175 for (int i = 0; i < 4; ++i) { 176 x[i] = x[i] / norm_x; 177 } 178 179 double delta[3] = {0.24, 0.15, 0.10}; 180 QuaternionParameterizationTestHelper(x, delta); 181} 182 183} // namespace internal 184} // namespace ceres 185