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/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
    134