internal/ceres/residual_block_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: keir@google.com (Keir Mierle)
  30
  31 #include "ceres/residual_block.h"
  32
  33 #include "gtest/gtest.h"
  34 #include "ceres/parameter_block.h"
  35 #include "ceres/sized_cost_function.h"
  36 #include "ceres/internal/eigen.h"
  37 #include "ceres/local_parameterization.h"
  38
  39 namespace ceres {
  40 namespace internal {
  41
  42 using std::vector;
  43
  44 // Trivial cost function that accepts three arguments.
  45 class TernaryCostFunction: public CostFunction {
  46  public:
  47   TernaryCostFunction(int num_residuals,
  48                       int32 parameter_block1_size,
  49                       int32 parameter_block2_size,
  50                       int32 parameter_block3_size) {
  51     set_num_residuals(num_residuals);
  52     mutable_parameter_block_sizes()->push_back(parameter_block1_size);
  53     mutable_parameter_block_sizes()->push_back(parameter_block2_size);
  54     mutable_parameter_block_sizes()->push_back(parameter_block3_size);
  55   }
  56
  57   virtual bool Evaluate(double const* const* parameters,
  58                         double* residuals,
  59                         double** jacobians) const {
  60     for (int i = 0; i < num_residuals(); ++i) {
  61       residuals[i] = i;
  62     }
  63     if (jacobians) {
  64       for (int k = 0; k < 3; ++k) {
  65         if (jacobians[k] != NULL) {
  66           MatrixRef jacobian(jacobians[k],
  67                              num_residuals(),
  68                              parameter_block_sizes()[k]);
  69           jacobian.setConstant(k);
  70         }
  71       }
  72     }
  73     return true;
  74   }
  75 };
  76
  77 TEST(ResidualBlock, EvaluteWithNoLossFunctionOrLocalParameterizations) {
  78   double scratch[64];
  79
  80   // Prepare the parameter blocks.
  81   double values_x[2];
  82   ParameterBlock x(values_x, 2, -1);
  83
  84   double values_y[3];
  85   ParameterBlock y(values_y, 3, -1);
  86
  87   double values_z[4];
  88   ParameterBlock z(values_z, 4, -1);
  89
  90   vector<ParameterBlock*> parameters;
  91   parameters.push_back(&x);
  92   parameters.push_back(&y);
  93   parameters.push_back(&z);
  94
  95   TernaryCostFunction cost_function(3, 2, 3, 4);
  96
  97   // Create the object under tests.
  98   ResidualBlock residual_block(&cost_function, NULL, parameters, -1);
  99
 100   // Verify getters.
 101   EXPECT_EQ(&cost_function, residual_block.cost_function());
 102   EXPECT_EQ(NULL, residual_block.loss_function());
 103   EXPECT_EQ(parameters[0], residual_block.parameter_blocks()[0]);
 104   EXPECT_EQ(parameters[1], residual_block.parameter_blocks()[1]);
 105   EXPECT_EQ(parameters[2], residual_block.parameter_blocks()[2]);
 106   EXPECT_EQ(3, residual_block.NumScratchDoublesForEvaluate());
 107
 108   // Verify cost-only evaluation.
 109   double cost;
 110   residual_block.Evaluate(true, &cost, NULL, NULL, scratch);
 111   EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
 112
 113   // Verify cost and residual evaluation.
 114   double residuals[3];
 115   residual_block.Evaluate(true, &cost, residuals, NULL, scratch);
 116   EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
 117   EXPECT_EQ(0.0, residuals[0]);
 118   EXPECT_EQ(1.0, residuals[1]);
 119   EXPECT_EQ(2.0, residuals[2]);
 120
 121   // Verify cost, residual, and jacobian evaluation.
 122   cost = 0.0;
 123   VectorRef(residuals, 3).setConstant(0.0);
 124
 125   Matrix jacobian_rx(3, 2);
 126   Matrix jacobian_ry(3, 3);
 127   Matrix jacobian_rz(3, 4);
 128
 129   jacobian_rx.setConstant(-1.0);
 130   jacobian_ry.setConstant(-1.0);
 131   jacobian_rz.setConstant(-1.0);
 132
 133   double *jacobian_ptrs[3] = {
 134     jacobian_rx.data(),
 135     jacobian_ry.data(),
 136     jacobian_rz.data()
 137   };
 138
 139   residual_block.Evaluate(true, &cost, residuals, jacobian_ptrs, scratch);
 140   EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
 141   EXPECT_EQ(0.0, residuals[0]);
 142   EXPECT_EQ(1.0, residuals[1]);
 143   EXPECT_EQ(2.0, residuals[2]);
 144
 145   EXPECT_TRUE((jacobian_rx.array() == 0.0).all()) << "\n" << jacobian_rx;
 146   EXPECT_TRUE((jacobian_ry.array() == 1.0).all()) << "\n" << jacobian_ry;
 147   EXPECT_TRUE((jacobian_rz.array() == 2.0).all()) << "\n" << jacobian_rz;
 148
 149   // Verify cost, residual, and partial jacobian evaluation.
 150   cost = 0.0;
 151   VectorRef(residuals, 3).setConstant(0.0);
 152   jacobian_rx.setConstant(-1.0);
 153   jacobian_ry.setConstant(-1.0);
 154   jacobian_rz.setConstant(-1.0);
 155
 156   jacobian_ptrs[1] = NULL;  // Don't compute the jacobian for y.
 157
 158   residual_block.Evaluate(true, &cost, residuals, jacobian_ptrs, scratch);
 159   EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
 160   EXPECT_EQ(0.0, residuals[0]);
 161   EXPECT_EQ(1.0, residuals[1]);
 162   EXPECT_EQ(2.0, residuals[2]);
 163
 164   EXPECT_TRUE((jacobian_rx.array() ==  0.0).all()) << "\n" << jacobian_rx;
 165   EXPECT_TRUE((jacobian_ry.array() == -1.0).all()) << "\n" << jacobian_ry;
 166   EXPECT_TRUE((jacobian_rz.array() ==  2.0).all()) << "\n" << jacobian_rz;
 167 }
 168
 169 // Trivial cost function that accepts three arguments.
 170 class LocallyParameterizedCostFunction: public SizedCostFunction<3, 2, 3, 4> {
 171  public:
 172   virtual bool Evaluate(double const* const* parameters,
 173                         double* residuals,
 174                         double** jacobians) const {
 175     for (int i = 0; i < num_residuals(); ++i) {
 176       residuals[i] = i;
 177     }
 178     if (jacobians) {
 179       for (int k = 0; k < 3; ++k) {
 180         // The jacobians here are full sized, but they are transformed in the
 181         // evaluator into the "local" jacobian. In the tests, the "subset
 182         // constant" parameterization is used, which should pick out columns
 183         // from these jacobians. Put values in the jacobian that make this
 184         // obvious; in particular, make the jacobians like this:
 185         //
 186         //   0 1 2 3 4 ...
 187         //   0 1 2 3 4 ...
 188         //   0 1 2 3 4 ...
 189         //
 190         if (jacobians[k] != NULL) {
 191           MatrixRef jacobian(jacobians[k],
 192                              num_residuals(),
 193                              parameter_block_sizes()[k]);
 194           for (int j = 0; j < k + 2; ++j) {
 195             jacobian.col(j).setConstant(j);
 196           }
 197         }
 198       }
 199     }
 200     return true;
 201   }
 202 };
 203
 204 TEST(ResidualBlock, EvaluteWithLocalParameterizations) {
 205   double scratch[64];
 206
 207   // Prepare the parameter blocks.
 208   double values_x[2];
 209   ParameterBlock x(values_x, 2, -1);
 210
 211   double values_y[3];
 212   ParameterBlock y(values_y, 3, -1);
 213
 214   double values_z[4];
 215   ParameterBlock z(values_z, 4, -1);
 216
 217   vector<ParameterBlock*> parameters;
 218   parameters.push_back(&x);
 219   parameters.push_back(&y);
 220   parameters.push_back(&z);
 221
 222   // Make x have the first component fixed.
 223   vector<int> x_fixed;
 224   x_fixed.push_back(0);
 225   SubsetParameterization x_parameterization(2, x_fixed);
 226   x.SetParameterization(&x_parameterization);
 227
 228   // Make z have the last and last component fixed.
 229   vector<int> z_fixed;
 230   z_fixed.push_back(2);
 231   SubsetParameterization z_parameterization(4, z_fixed);
 232   z.SetParameterization(&z_parameterization);
 233
 234   LocallyParameterizedCostFunction cost_function;
 235
 236   // Create the object under tests.
 237   ResidualBlock residual_block(&cost_function, NULL, parameters, -1);
 238
 239   // Verify getters.
 240   EXPECT_EQ(&cost_function, residual_block.cost_function());
 241   EXPECT_EQ(NULL, residual_block.loss_function());
 242   EXPECT_EQ(parameters[0], residual_block.parameter_blocks()[0]);
 243   EXPECT_EQ(parameters[1], residual_block.parameter_blocks()[1]);
 244   EXPECT_EQ(parameters[2], residual_block.parameter_blocks()[2]);
 245   EXPECT_EQ(3*(2 + 4) + 3, residual_block.NumScratchDoublesForEvaluate());
 246
 247   // Verify cost-only evaluation.
 248   double cost;
 249   residual_block.Evaluate(true, &cost, NULL, NULL, scratch);
 250   EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
 251
 252   // Verify cost and residual evaluation.
 253   double residuals[3];
 254   residual_block.Evaluate(true, &cost, residuals, NULL, scratch);
 255   EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
 256   EXPECT_EQ(0.0, residuals[0]);
 257   EXPECT_EQ(1.0, residuals[1]);
 258   EXPECT_EQ(2.0, residuals[2]);
 259
 260   // Verify cost, residual, and jacobian evaluation.
 261   cost = 0.0;
 262   VectorRef(residuals, 3).setConstant(0.0);
 263
 264   Matrix jacobian_rx(3, 1);  // Since the first element is fixed.
 265   Matrix jacobian_ry(3, 3);
 266   Matrix jacobian_rz(3, 3);  // Since the third element is fixed.
 267
 268   jacobian_rx.setConstant(-1.0);
 269   jacobian_ry.setConstant(-1.0);
 270   jacobian_rz.setConstant(-1.0);
 271
 272   double *jacobian_ptrs[3] = {
 273     jacobian_rx.data(),
 274     jacobian_ry.data(),
 275     jacobian_rz.data()
 276   };
 277
 278   residual_block.Evaluate(true, &cost, residuals, jacobian_ptrs, scratch);
 279   EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
 280   EXPECT_EQ(0.0, residuals[0]);
 281   EXPECT_EQ(1.0, residuals[1]);
 282   EXPECT_EQ(2.0, residuals[2]);
 283
 284   Matrix expected_jacobian_rx(3, 1);
 285   expected_jacobian_rx << 1.0, 1.0, 1.0;
 286
 287   Matrix expected_jacobian_ry(3, 3);
 288   expected_jacobian_ry << 0.0, 1.0, 2.0,
 289                           0.0, 1.0, 2.0,
 290                           0.0, 1.0, 2.0;
 291
 292   Matrix expected_jacobian_rz(3, 3);
 293   expected_jacobian_rz << 0.0, 1.0, /* 2.0, */ 3.0,  // 3rd parameter constant.
 294                           0.0, 1.0, /* 2.0, */ 3.0,
 295                           0.0, 1.0, /* 2.0, */ 3.0;
 296
 297   EXPECT_EQ(expected_jacobian_rx, jacobian_rx)
 298       << "\nExpected:\n" << expected_jacobian_rx
 299       << "\nActual:\n"   << jacobian_rx;
 300   EXPECT_EQ(expected_jacobian_ry, jacobian_ry)
 301       << "\nExpected:\n" << expected_jacobian_ry
 302       << "\nActual:\n"   << jacobian_ry;
 303   EXPECT_EQ(expected_jacobian_rz, jacobian_rz)
 304       << "\nExpected:\n " << expected_jacobian_rz
 305       << "\nActual:\n"   << jacobian_rz;
 306
 307   // Verify cost, residual, and partial jacobian evaluation.
 308   cost = 0.0;
 309   VectorRef(residuals, 3).setConstant(0.0);
 310   jacobian_rx.setConstant(-1.0);
 311   jacobian_ry.setConstant(-1.0);
 312   jacobian_rz.setConstant(-1.0);
 313
 314   jacobian_ptrs[1] = NULL;  // Don't compute the jacobian for y.
 315
 316   residual_block.Evaluate(true, &cost, residuals, jacobian_ptrs, scratch);
 317   EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
 318   EXPECT_EQ(0.0, residuals[0]);
 319   EXPECT_EQ(1.0, residuals[1]);
 320   EXPECT_EQ(2.0, residuals[2]);
 321
 322   EXPECT_EQ(expected_jacobian_rx, jacobian_rx);
 323   EXPECT_TRUE((jacobian_ry.array() == -1.0).all()) << "\n" << jacobian_ry;
 324   EXPECT_EQ(expected_jacobian_rz, jacobian_rz);
 325 }
 326
 327 }  // namespace internal
 328 }  // namespace ceres