internal/ceres/normal_prior_test.cc

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   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 "ceres/normal_prior.h"
  32
  33 #include <cstddef>
  34
  35 #include "gtest/gtest.h"
  36 #include "ceres/internal/eigen.h"
  37 #include "ceres/random.h"
  38
  39 namespace ceres {
  40 namespace internal {
  41
  42 void RandomVector(Vector* v) {
  43   for (int r = 0; r < v->rows(); ++r)
  44     (*v)[r] = 2 * RandDouble() - 1;
  45 }
  46
  47 void RandomMatrix(Matrix* m) {
  48   for (int r = 0; r < m->rows(); ++r) {
  49     for (int c = 0; c < m->cols(); ++c) {
  50       (*m)(r, c) = 2 * RandDouble() - 1;
  51     }
  52   }
  53 }
  54
  55 TEST(NormalPriorTest, ResidualAtRandomPosition) {
  56   srand(5);
  57
  58   for (int num_rows = 1; num_rows < 5; ++num_rows) {
  59     for (int num_cols = 1; num_cols < 5; ++num_cols) {
  60       Vector b(num_cols);
  61       RandomVector(&b);
  62
  63       Matrix A(num_rows, num_cols);
  64       RandomMatrix(&A);
  65
  66       double * x = new double[num_cols];
  67       for (int i = 0; i < num_cols; ++i)
  68         x[i] = 2 * RandDouble() - 1;
  69
  70       double * jacobian = new double[num_rows * num_cols];
  71       Vector residuals(num_rows);
  72
  73       NormalPrior prior(A, b);
  74       prior.Evaluate(&x, residuals.data(), &jacobian);
  75
  76       // Compare the norm of the residual
  77       double residual_diff_norm =
  78           (residuals - A * (VectorRef(x, num_cols) - b)).squaredNorm();
  79       EXPECT_NEAR(residual_diff_norm, 0, 1e-10);
  80
  81       // Compare the jacobians
  82       MatrixRef J(jacobian, num_rows, num_cols);
  83       double jacobian_diff_norm = (J - A).norm();
  84       EXPECT_NEAR(jacobian_diff_norm, 0.0, 1e-10);
  85
  86       delete []x;
  87       delete []jacobian;
  88     }
  89   }
  90 }
  91
  92 TEST(NormalPriorTest, ResidualAtRandomPositionNullJacobians) {
  93   srand(5);
  94
  95   for (int num_rows = 1; num_rows < 5; ++num_rows) {
  96     for (int num_cols = 1; num_cols < 5; ++num_cols) {
  97       Vector b(num_cols);
  98       RandomVector(&b);
  99
 100       Matrix A(num_rows, num_cols);
 101       RandomMatrix(&A);
 102
 103       double * x = new double[num_cols];
 104       for (int i = 0; i < num_cols; ++i)
 105         x[i] = 2 * RandDouble() - 1;
 106
 107       double* jacobians[1];
 108       jacobians[0] = NULL;
 109
 110       Vector residuals(num_rows);
 111
 112       NormalPrior prior(A, b);
 113       prior.Evaluate(&x, residuals.data(), jacobians);
 114
 115       // Compare the norm of the residual
 116       double residual_diff_norm =
 117           (residuals - A * (VectorRef(x, num_cols) - b)).squaredNorm();
 118       EXPECT_NEAR(residual_diff_norm, 0, 1e-10);
 119
 120       prior.Evaluate(&x, residuals.data(), NULL);
 121       // Compare the norm of the residual
 122       residual_diff_norm =
 123           (residuals - A * (VectorRef(x, num_cols) - b)).squaredNorm();
 124       EXPECT_NEAR(residual_diff_norm, 0, 1e-10);
 125
 126
 127       delete []x;
 128     }
 129   }
 130 }
 131
 132 }  // namespace internal
 133 }  // namespace ceres