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