Home | History | Annotate | Download | only in ceres
      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 "ceres/corrector.h"
     32 
     33 #include <algorithm>
     34 #include <cmath>
     35 #include <cstring>
     36 #include <cstdlib>
     37 #include "gtest/gtest.h"
     38 #include "ceres/random.h"
     39 #include "ceres/internal/eigen.h"
     40 
     41 namespace ceres {
     42 namespace internal {
     43 
     44 // If rho[1] is zero, the Corrector constructor should crash.
     45 TEST(Corrector, ZeroGradientDeathTest) {
     46   const double kRho[] = {0.0, 0.0, 0.0};
     47   EXPECT_DEATH_IF_SUPPORTED({Corrector c(1.0, kRho);},
     48                ".*");
     49 }
     50 
     51 // If rho[1] is negative, the Corrector constructor should crash.
     52 TEST(Corrector, NegativeGradientDeathTest) {
     53   const double kRho[] = {0.0, -0.1, 0.0};
     54   EXPECT_DEATH_IF_SUPPORTED({Corrector c(1.0, kRho);},
     55                ".*");
     56 }
     57 
     58 TEST(Corrector, ScalarCorrection) {
     59   double residuals = sqrt(3.0);
     60   double jacobian = 10.0;
     61   double sq_norm = residuals * residuals;
     62 
     63   const double kRho[] = {sq_norm, 0.1, -0.01};
     64 
     65   // In light of the rho'' < 0 clamping now implemented in
     66   // corrector.cc, alpha = 0 whenever rho'' < 0.
     67   const double kAlpha = 0.0;
     68 
     69   // Thus the expected value of the residual is
     70   // residual[i] * sqrt(kRho[1]) / (1.0 - kAlpha).
     71   const double kExpectedResidual =
     72       residuals * sqrt(kRho[1]) / (1 - kAlpha);
     73 
     74   // The jacobian in this case will be
     75   // sqrt(kRho[1]) * (1 - kAlpha) * jacobian.
     76   const double kExpectedJacobian = sqrt(kRho[1]) * (1 - kAlpha) * jacobian;
     77 
     78   Corrector c(sq_norm, kRho);
     79   c.CorrectJacobian(1.0, 1.0, &residuals, &jacobian);
     80   c.CorrectResiduals(1.0, &residuals);
     81 
     82   ASSERT_NEAR(residuals, kExpectedResidual, 1e-6);
     83   ASSERT_NEAR(kExpectedJacobian, jacobian, 1e-6);
     84 }
     85 
     86 TEST(Corrector, ScalarCorrectionZeroResidual) {
     87   double residuals = 0.0;
     88   double jacobian = 10.0;
     89   double sq_norm = residuals * residuals;
     90 
     91   const double kRho[] = {0.0, 0.1, -0.01};
     92   Corrector c(sq_norm, kRho);
     93 
     94   // The alpha equation is
     95   // 1/2 alpha^2 - alpha + 0.0 = 0.
     96   // i.e. alpha = 1.0 - sqrt(1.0).
     97   //      alpha = 0.0.
     98   // Thus the expected value of the residual is
     99   // residual[i] * sqrt(kRho[1])
    100   const double kExpectedResidual = residuals * sqrt(kRho[1]);
    101 
    102   // The jacobian in this case will be
    103   // sqrt(kRho[1]) * jacobian.
    104   const double kExpectedJacobian = sqrt(kRho[1]) * jacobian;
    105 
    106   c.CorrectJacobian(1, 1, &residuals, &jacobian);
    107   c.CorrectResiduals(1, &residuals);
    108 
    109   ASSERT_NEAR(residuals, kExpectedResidual, 1e-6);
    110   ASSERT_NEAR(kExpectedJacobian, jacobian, 1e-6);
    111 }
    112 
    113 // Scaling behaviour for one dimensional functions.
    114 TEST(Corrector, ScalarCorrectionAlphaClamped) {
    115   double residuals = sqrt(3.0);
    116   double jacobian = 10.0;
    117   double sq_norm = residuals * residuals;
    118 
    119   const double kRho[] = {3, 0.1, -0.1};
    120 
    121   // rho[2] < 0 -> alpha = 0.0
    122   const double kAlpha = 0.0;
    123 
    124   // Thus the expected value of the residual is
    125   // residual[i] * sqrt(kRho[1]) / (1.0 - kAlpha).
    126   const double kExpectedResidual =
    127       residuals * sqrt(kRho[1]) / (1.0 - kAlpha);
    128 
    129   // The jacobian in this case will be scaled by
    130   // sqrt(rho[1]) * (1 - alpha) * J.
    131   const double kExpectedJacobian = sqrt(kRho[1]) *
    132       (1.0 - kAlpha) * jacobian;
    133 
    134   Corrector c(sq_norm, kRho);
    135   c.CorrectJacobian(1, 1, &residuals, &jacobian);
    136   c.CorrectResiduals(1, &residuals);
    137 
    138   ASSERT_NEAR(residuals, kExpectedResidual, 1e-6);
    139   ASSERT_NEAR(kExpectedJacobian, jacobian, 1e-6);
    140 }
    141 
    142 // Test that the corrected multidimensional residual and jacobians
    143 // match the expected values and the resulting modified normal
    144 // equations match the robustified gauss newton approximation.
    145 TEST(Corrector, MultidimensionalGaussNewtonApproximation) {
    146   double residuals[3];
    147   double jacobian[2 * 3];
    148   double rho[3];
    149 
    150   // Eigen matrix references for linear algebra.
    151   MatrixRef jac(jacobian, 3, 2);
    152   VectorRef res(residuals, 3);
    153 
    154   // Ground truth values of the modified jacobian and residuals.
    155   Matrix g_jac(3, 2);
    156   Vector g_res(3);
    157 
    158   // Ground truth values of the robustified Gauss-Newton
    159   // approximation.
    160   Matrix g_hess(2, 2);
    161   Vector g_grad(2);
    162 
    163   // Corrected hessian and gradient implied by the modified jacobian
    164   // and hessians.
    165   Matrix c_hess(2, 2);
    166   Vector c_grad(2);
    167 
    168   srand(5);
    169   for (int iter = 0; iter < 10000; ++iter) {
    170     // Initialize the jacobian and residual.
    171     for (int i = 0; i < 2 * 3; ++i)
    172       jacobian[i] = RandDouble();
    173     for (int i = 0; i < 3; ++i)
    174       residuals[i] = RandDouble();
    175 
    176     const double sq_norm = res.dot(res);
    177 
    178     rho[0] = sq_norm;
    179     rho[1] = RandDouble();
    180     rho[2] = 2.0 * RandDouble() - 1.0;
    181 
    182     // If rho[2] > 0, then the curvature correction to the correction
    183     // and the gauss newton approximation will match. Otherwise, we
    184     // will clamp alpha to 0.
    185 
    186     const double kD = 1 + 2 * rho[2] / rho[1] * sq_norm;
    187     const double kAlpha = (rho[2] > 0.0) ? 1 - sqrt(kD) : 0.0;
    188 
    189     // Ground truth values.
    190     g_res = sqrt(rho[1]) / (1.0 - kAlpha) * res;
    191     g_jac = sqrt(rho[1]) * (jac - kAlpha / sq_norm *
    192                             res * res.transpose() * jac);
    193 
    194     g_grad = rho[1] * jac.transpose() * res;
    195     g_hess = rho[1] * jac.transpose() * jac +
    196         2.0 * rho[2] * jac.transpose() * res * res.transpose() * jac;
    197 
    198     Corrector c(sq_norm, rho);
    199     c.CorrectJacobian(3, 2, residuals, jacobian);
    200     c.CorrectResiduals(3, residuals);
    201 
    202     // Corrected gradient and hessian.
    203     c_grad  = jac.transpose() * res;
    204     c_hess = jac.transpose() * jac;
    205 
    206     ASSERT_NEAR((g_res - res).norm(), 0.0, 1e-10);
    207     ASSERT_NEAR((g_jac - jac).norm(), 0.0, 1e-10);
    208 
    209     ASSERT_NEAR((g_grad - c_grad).norm(), 0.0, 1e-10);
    210   }
    211 }
    212 
    213 TEST(Corrector, MultidimensionalGaussNewtonApproximationZeroResidual) {
    214   double residuals[3];
    215   double jacobian[2 * 3];
    216   double rho[3];
    217 
    218   // Eigen matrix references for linear algebra.
    219   MatrixRef jac(jacobian, 3, 2);
    220   VectorRef res(residuals, 3);
    221 
    222   // Ground truth values of the modified jacobian and residuals.
    223   Matrix g_jac(3, 2);
    224   Vector g_res(3);
    225 
    226   // Ground truth values of the robustified Gauss-Newton
    227   // approximation.
    228   Matrix g_hess(2, 2);
    229   Vector g_grad(2);
    230 
    231   // Corrected hessian and gradient implied by the modified jacobian
    232   // and hessians.
    233   Matrix c_hess(2, 2);
    234   Vector c_grad(2);
    235 
    236   srand(5);
    237   for (int iter = 0; iter < 10000; ++iter) {
    238     // Initialize the jacobian.
    239     for (int i = 0; i < 2 * 3; ++i)
    240       jacobian[i] = RandDouble();
    241 
    242     // Zero residuals
    243     res.setZero();
    244 
    245     const double sq_norm = res.dot(res);
    246 
    247     rho[0] = sq_norm;
    248     rho[1] = RandDouble();
    249     rho[2] = 2 * RandDouble() - 1.0;
    250 
    251     // Ground truth values.
    252     g_res = sqrt(rho[1]) * res;
    253     g_jac = sqrt(rho[1]) * jac;
    254 
    255     g_grad = rho[1] * jac.transpose() * res;
    256     g_hess = rho[1] * jac.transpose() * jac +
    257         2.0 * rho[2] * jac.transpose() * res * res.transpose() * jac;
    258 
    259     Corrector c(sq_norm, rho);
    260     c.CorrectJacobian(3, 2, residuals, jacobian);
    261     c.CorrectResiduals(3, residuals);
    262 
    263     // Corrected gradient and hessian.
    264     c_grad = jac.transpose() * res;
    265     c_hess = jac.transpose() * jac;
    266 
    267     ASSERT_NEAR((g_res - res).norm(), 0.0, 1e-10);
    268     ASSERT_NEAR((g_jac - jac).norm(), 0.0, 1e-10);
    269 
    270     ASSERT_NEAR((g_grad - c_grad).norm(), 0.0, 1e-10);
    271     ASSERT_NEAR((g_hess - c_hess).norm(), 0.0, 1e-10);
    272   }
    273 }
    274 
    275 }  // namespace internal
    276 }  // namespace ceres
    277