1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2013 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/cost_function_to_functor.h" 32 #include "ceres/autodiff_cost_function.h" 33 #include "gtest/gtest.h" 34 35 namespace ceres { 36 namespace internal { 37 38 const double kTolerance = 1e-18; 39 40 void ExpectCostFunctionsAreEqual(const CostFunction& cost_function, 41 const CostFunction& actual_cost_function) { 42 EXPECT_EQ(cost_function.num_residuals(), 43 actual_cost_function.num_residuals()); 44 const int num_residuals = cost_function.num_residuals(); 45 const vector<int32>& parameter_block_sizes = 46 cost_function.parameter_block_sizes(); 47 const vector<int32>& actual_parameter_block_sizes = 48 actual_cost_function.parameter_block_sizes(); 49 EXPECT_EQ(parameter_block_sizes.size(), 50 actual_parameter_block_sizes.size()); 51 52 int num_parameters = 0; 53 for (int i = 0; i < parameter_block_sizes.size(); ++i) { 54 EXPECT_EQ(parameter_block_sizes[i], actual_parameter_block_sizes[i]); 55 num_parameters += parameter_block_sizes[i]; 56 } 57 58 scoped_array<double> parameters(new double[num_parameters]); 59 for (int i = 0; i < num_parameters; ++i) { 60 parameters[i] = static_cast<double>(i) + 1.0; 61 } 62 63 scoped_array<double> residuals(new double[num_residuals]); 64 scoped_array<double> jacobians(new double[num_parameters * num_residuals]); 65 66 scoped_array<double> actual_residuals(new double[num_residuals]); 67 scoped_array<double> actual_jacobians 68 (new double[num_parameters * num_residuals]); 69 70 scoped_array<double*> parameter_blocks( 71 new double*[parameter_block_sizes.size()]); 72 scoped_array<double*> jacobian_blocks( 73 new double*[parameter_block_sizes.size()]); 74 scoped_array<double*> actual_jacobian_blocks( 75 new double*[parameter_block_sizes.size()]); 76 77 num_parameters = 0; 78 for (int i = 0; i < parameter_block_sizes.size(); ++i) { 79 parameter_blocks[i] = parameters.get() + num_parameters; 80 jacobian_blocks[i] = jacobians.get() + num_parameters * num_residuals; 81 actual_jacobian_blocks[i] = 82 actual_jacobians.get() + num_parameters * num_residuals; 83 num_parameters += parameter_block_sizes[i]; 84 } 85 86 EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.get(), 87 residuals.get(), NULL)); 88 EXPECT_TRUE(actual_cost_function.Evaluate(parameter_blocks.get(), 89 actual_residuals.get(), NULL)); 90 for (int i = 0; i < num_residuals; ++i) { 91 EXPECT_NEAR(residuals[i], actual_residuals[i], kTolerance) 92 << "residual id: " << i; 93 } 94 95 96 EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.get(), 97 residuals.get(), 98 jacobian_blocks.get())); 99 EXPECT_TRUE(actual_cost_function.Evaluate(parameter_blocks.get(), 100 actual_residuals.get(), 101 actual_jacobian_blocks.get())); 102 for (int i = 0; i < num_residuals; ++i) { 103 EXPECT_NEAR(residuals[i], actual_residuals[i], kTolerance) 104 << "residual : " << i; 105 } 106 107 for (int i = 0; i < num_residuals * num_parameters; ++i) { 108 EXPECT_NEAR(jacobians[i], actual_jacobians[i], kTolerance) 109 << "jacobian : " << i << " " 110 << jacobians[i] << " " << actual_jacobians[i]; 111 } 112 }; 113 114 struct OneParameterBlockFunctor { 115 public: 116 template <typename T> 117 bool operator()(const T* x1, T* residuals) const { 118 residuals[0] = x1[0] * x1[0]; 119 residuals[1] = x1[1] * x1[1]; 120 return true; 121 } 122 }; 123 124 struct TwoParameterBlockFunctor { 125 public: 126 template <typename T> 127 bool operator()(const T* x1, const T* x2, T* residuals) const { 128 residuals[0] = x1[0] * x1[0] + x2[0] * x2[0]; 129 residuals[1] = x1[1] * x1[1] + x2[1] * x2[1]; 130 return true; 131 } 132 }; 133 134 struct ThreeParameterBlockFunctor { 135 public: 136 template <typename T> 137 bool operator()(const T* x1, const T* x2, const T* x3, T* residuals) const { 138 residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0]; 139 residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1]; 140 return true; 141 } 142 }; 143 144 struct FourParameterBlockFunctor { 145 public: 146 template <typename T> 147 bool operator()(const T* x1, const T* x2, const T* x3, const T* x4, 148 T* residuals) const { 149 residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0] 150 + x4[0] * x4[0]; 151 residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1] 152 + x4[1] * x4[1]; 153 return true; 154 } 155 }; 156 157 struct FiveParameterBlockFunctor { 158 public: 159 template <typename T> 160 bool operator()(const T* x1, const T* x2, const T* x3, const T* x4, 161 const T* x5, T* residuals) const { 162 residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0] 163 + x4[0] * x4[0] + x5[0] * x5[0]; 164 residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1] 165 + x4[1] * x4[1] + x5[1] * x5[1]; 166 return true; 167 } 168 }; 169 170 struct SixParameterBlockFunctor { 171 public: 172 template <typename T> 173 bool operator()(const T* x1, const T* x2, const T* x3, const T* x4, 174 const T* x5, const T* x6, T* residuals) const { 175 residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0] 176 + x4[0] * x4[0] + x5[0] * x5[0] + x6[0] * x6[0]; 177 residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1] 178 + x4[1] * x4[1] + x5[1] * x5[1] + x6[1] * x6[1]; 179 return true; 180 } 181 }; 182 183 struct SevenParameterBlockFunctor { 184 public: 185 template <typename T> 186 bool operator()(const T* x1, const T* x2, const T* x3, const T* x4, 187 const T* x5, const T* x6, const T* x7, T* residuals) const { 188 residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0] 189 + x4[0] * x4[0] + x5[0] * x5[0] + x6[0] * x6[0] + x7[0] * x7[0]; 190 residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1] 191 + x4[1] * x4[1] + x5[1] * x5[1] + x6[1] * x6[1] + x7[1] * x7[1]; 192 return true; 193 } 194 }; 195 196 struct EightParameterBlockFunctor { 197 public: 198 template <typename T> 199 bool operator()(const T* x1, const T* x2, const T* x3, const T* x4, 200 const T* x5, const T* x6, const T* x7, const T* x8, 201 T* residuals) const { 202 residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0] 203 + x4[0] * x4[0] + x5[0] * x5[0] + x6[0] * x6[0] + x7[0] * x7[0] 204 + x8[0] * x8[0]; 205 residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1] 206 + x4[1] * x4[1] + x5[1] * x5[1] + x6[1] * x6[1] + x7[1] * x7[1] 207 + x8[1] * x8[1]; 208 return true; 209 } 210 }; 211 212 struct NineParameterBlockFunctor { 213 public: 214 template <typename T> 215 bool operator()(const T* x1, const T* x2, const T* x3, const T* x4, 216 const T* x5, const T* x6, const T* x7, const T* x8, 217 const T* x9, T* residuals) const { 218 residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0] 219 + x4[0] * x4[0] + x5[0] * x5[0] + x6[0] * x6[0] + x7[0] * x7[0] 220 + x8[0] * x8[0] + x9[0] * x9[0]; 221 residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1] 222 + x4[1] * x4[1] + x5[1] * x5[1] + x6[1] * x6[1] + x7[1] * x7[1] 223 + x8[1] * x8[1] + x9[1] * x9[1]; 224 return true; 225 } 226 }; 227 228 struct TenParameterBlockFunctor { 229 public: 230 template <typename T> 231 bool operator()(const T* x1, const T* x2, const T* x3, const T* x4, 232 const T* x5, const T* x6, const T* x7, const T* x8, 233 const T* x9, const T* x10, T* residuals) const { 234 residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0] 235 + x4[0] * x4[0] + x5[0] * x5[0] + x6[0] * x6[0] + x7[0] * x7[0] 236 + x8[0] * x8[0] + x9[0] * x9[0] + x10[0] * x10[0]; 237 residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1] 238 + x4[1] * x4[1] + x5[1] * x5[1] + x6[1] * x6[1] + x7[1] * x7[1] 239 + x8[1] * x8[1] + x9[1] * x9[1] + x10[1] * x10[1]; 240 return true; 241 } 242 }; 243 244 #define TEST_BODY(NAME) \ 245 TEST(CostFunctionToFunctor, NAME) { \ 246 scoped_ptr<CostFunction> cost_function( \ 247 new AutoDiffCostFunction< \ 248 CostFunctionToFunctor<2, PARAMETER_BLOCK_SIZES >, \ 249 2, PARAMETER_BLOCK_SIZES>(new CostFunctionToFunctor< \ 250 2, PARAMETER_BLOCK_SIZES >( \ 251 new AutoDiffCostFunction< \ 252 NAME##Functor, 2, PARAMETER_BLOCK_SIZES >( \ 253 new NAME##Functor)))); \ 254 \ 255 scoped_ptr<CostFunction> actual_cost_function( \ 256 new AutoDiffCostFunction<NAME##Functor, 2, PARAMETER_BLOCK_SIZES >( \ 257 new NAME##Functor)); \ 258 ExpectCostFunctionsAreEqual(*cost_function, *actual_cost_function); \ 259 } 260 261 #define PARAMETER_BLOCK_SIZES 2 262 TEST_BODY(OneParameterBlock) 263 #undef PARAMETER_BLOCK_SIZES 264 265 #define PARAMETER_BLOCK_SIZES 2,2 266 TEST_BODY(TwoParameterBlock) 267 #undef PARAMETER_BLOCK_SIZES 268 269 #define PARAMETER_BLOCK_SIZES 2,2,2 270 TEST_BODY(ThreeParameterBlock) 271 #undef PARAMETER_BLOCK_SIZES 272 273 #define PARAMETER_BLOCK_SIZES 2,2,2,2 274 TEST_BODY(FourParameterBlock) 275 #undef PARAMETER_BLOCK_SIZES 276 277 #define PARAMETER_BLOCK_SIZES 2,2,2,2,2 278 TEST_BODY(FiveParameterBlock) 279 #undef PARAMETER_BLOCK_SIZES 280 281 #define PARAMETER_BLOCK_SIZES 2,2,2,2,2,2 282 TEST_BODY(SixParameterBlock) 283 #undef PARAMETER_BLOCK_SIZES 284 285 #define PARAMETER_BLOCK_SIZES 2,2,2,2,2,2,2 286 TEST_BODY(SevenParameterBlock) 287 #undef PARAMETER_BLOCK_SIZES 288 289 #define PARAMETER_BLOCK_SIZES 2,2,2,2,2,2,2,2 290 TEST_BODY(EightParameterBlock) 291 #undef PARAMETER_BLOCK_SIZES 292 293 #define PARAMETER_BLOCK_SIZES 2,2,2,2,2,2,2,2,2 294 TEST_BODY(NineParameterBlock) 295 #undef PARAMETER_BLOCK_SIZES 296 297 #define PARAMETER_BLOCK_SIZES 2,2,2,2,2,2,2,2,2,2 298 TEST_BODY(TenParameterBlock) 299 #undef PARAMETER_BLOCK_SIZES 300 301 #undef TEST_BODY 302 303 304 } // namespace internal 305 } // namespace ceres 306
