Home | History | Annotate | Download | only in ceres 
      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 
     52 TEST(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 
     75 struct 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 
    105 void 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 
    143 TEST(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 
    150 TEST(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 
    168 TEST(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