11d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Ceres Solver - A fast non-linear least squares minimizer 21d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Copyright 2013 Google Inc. All rights reserved. 31d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// http://code.google.com/p/ceres-solver/ 41d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 51d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Redistribution and use in source and binary forms, with or without 61d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// modification, are permitted provided that the following conditions are met: 71d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 81d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions of source code must retain the above copyright notice, 91d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer. 101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions in binary form must reproduce the above copyright notice, 111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer in the documentation 121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// and/or other materials provided with the distribution. 131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Neither the name of Google Inc. nor the names of its contributors may be 141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// used to endorse or promote products derived from this software without 151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// specific prior written permission. 161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// POSSIBILITY OF SUCH DAMAGE. 281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Author: sameeragarwal@google.com (Sameer Agarwal) 301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/cost_function_to_functor.h" 321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/autodiff_cost_function.h" 331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "gtest/gtest.h" 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace ceres { 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace internal { 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingconst double kTolerance = 1e-18; 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid ExpectCostFunctionsAreEqual(const CostFunction& cost_function, 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const CostFunction& actual_cost_function) { 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(cost_function.num_residuals(), 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling actual_cost_function.num_residuals()); 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int num_residuals = cost_function.num_residuals(); 4579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const vector<int32>& parameter_block_sizes = 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cost_function.parameter_block_sizes(); 4779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const vector<int32>& actual_parameter_block_sizes = 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling actual_cost_function.parameter_block_sizes(); 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(parameter_block_sizes.size(), 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling actual_parameter_block_sizes.size()); 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int num_parameters = 0; 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < parameter_block_sizes.size(); ++i) { 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_EQ(parameter_block_sizes[i], actual_parameter_block_sizes[i]); 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling num_parameters += parameter_block_sizes[i]; 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_array<double> parameters(new double[num_parameters]); 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < num_parameters; ++i) { 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameters[i] = static_cast<double>(i) + 1.0; 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_array<double> residuals(new double[num_residuals]); 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_array<double> jacobians(new double[num_parameters * num_residuals]); 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_array<double> actual_residuals(new double[num_residuals]); 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_array<double> actual_jacobians 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (new double[num_parameters * num_residuals]); 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_array<double*> parameter_blocks( 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new double*[parameter_block_sizes.size()]); 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_array<double*> jacobian_blocks( 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new double*[parameter_block_sizes.size()]); 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_array<double*> actual_jacobian_blocks( 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new double*[parameter_block_sizes.size()]); 761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling num_parameters = 0; 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < parameter_block_sizes.size(); ++i) { 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_blocks[i] = parameters.get() + num_parameters; 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_blocks[i] = jacobians.get() + num_parameters * num_residuals; 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling actual_jacobian_blocks[i] = 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling actual_jacobians.get() + num_parameters * num_residuals; 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling num_parameters += parameter_block_sizes[i]; 841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.get(), 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals.get(), NULL)); 881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(actual_cost_function.Evaluate(parameter_blocks.get(), 891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling actual_residuals.get(), NULL)); 901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < num_residuals; ++i) { 911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NEAR(residuals[i], actual_residuals[i], kTolerance) 921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "residual id: " << i; 931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.get(), 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals.get(), 981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_blocks.get())); 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_TRUE(actual_cost_function.Evaluate(parameter_blocks.get(), 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling actual_residuals.get(), 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling actual_jacobian_blocks.get())); 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < num_residuals; ++i) { 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NEAR(residuals[i], actual_residuals[i], kTolerance) 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "residual : " << i; 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < num_residuals * num_parameters; ++i) { 1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EXPECT_NEAR(jacobians[i], actual_jacobians[i], kTolerance) 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "jacobian : " << i << " " 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << jacobians[i] << " " << actual_jacobians[i]; 1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct OneParameterBlockFunctor { 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(const T* x1, T* residuals) const { 1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[0] = x1[0] * x1[0]; 1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[1] = x1[1] * x1[1]; 1201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct TwoParameterBlockFunctor { 1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(const T* x1, const T* x2, T* residuals) const { 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[0] = x1[0] * x1[0] + x2[0] * x2[0]; 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[1] = x1[1] * x1[1] + x2[1] * x2[1]; 1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct ThreeParameterBlockFunctor { 1351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 1361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 1371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(const T* x1, const T* x2, const T* x3, T* residuals) const { 1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0]; 1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1]; 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct FourParameterBlockFunctor { 1451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 1461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 1471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(const T* x1, const T* x2, const T* x3, const T* x4, 1481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T* residuals) const { 1491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0] 1501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x4[0] * x4[0]; 1511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1] 1521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x4[1] * x4[1]; 1531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 1541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct FiveParameterBlockFunctor { 1581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 1591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 1601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(const T* x1, const T* x2, const T* x3, const T* x4, 1611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* x5, T* residuals) const { 1621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0] 1631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x4[0] * x4[0] + x5[0] * x5[0]; 1641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1] 1651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x4[1] * x4[1] + x5[1] * x5[1]; 1661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 1671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct SixParameterBlockFunctor { 1711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 1721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 1731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(const T* x1, const T* x2, const T* x3, const T* x4, 1741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* x5, const T* x6, T* residuals) const { 1751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0] 1761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x4[0] * x4[0] + x5[0] * x5[0] + x6[0] * x6[0]; 1771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1] 1781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x4[1] * x4[1] + x5[1] * x5[1] + x6[1] * x6[1]; 1791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 1801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct SevenParameterBlockFunctor { 1841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 1851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 1861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(const T* x1, const T* x2, const T* x3, const T* x4, 1871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* x5, const T* x6, const T* x7, T* residuals) const { 1881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0] 1891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x4[0] * x4[0] + x5[0] * x5[0] + x6[0] * x6[0] + x7[0] * x7[0]; 1901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1] 1911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x4[1] * x4[1] + x5[1] * x5[1] + x6[1] * x6[1] + x7[1] * x7[1]; 1921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 1931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct EightParameterBlockFunctor { 1971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 1981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 1991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(const T* x1, const T* x2, const T* x3, const T* x4, 2001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* x5, const T* x6, const T* x7, const T* x8, 2011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T* residuals) const { 2021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0] 2031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x4[0] * x4[0] + x5[0] * x5[0] + x6[0] * x6[0] + x7[0] * x7[0] 2041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x8[0] * x8[0]; 2051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1] 2061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x4[1] * x4[1] + x5[1] * x5[1] + x6[1] * x6[1] + x7[1] * x7[1] 2071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x8[1] * x8[1]; 2081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 2091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 2111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct NineParameterBlockFunctor { 2131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 2141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 2151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(const T* x1, const T* x2, const T* x3, const T* x4, 2161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* x5, const T* x6, const T* x7, const T* x8, 2171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* x9, T* residuals) const { 2181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0] 2191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x4[0] * x4[0] + x5[0] * x5[0] + x6[0] * x6[0] + x7[0] * x7[0] 2201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x8[0] * x8[0] + x9[0] * x9[0]; 2211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1] 2221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x4[1] * x4[1] + x5[1] * x5[1] + x6[1] * x6[1] + x7[1] * x7[1] 2231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x8[1] * x8[1] + x9[1] * x9[1]; 2241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 2251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 2271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct TenParameterBlockFunctor { 2291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 2301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> 2311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool operator()(const T* x1, const T* x2, const T* x3, const T* x4, 2321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* x5, const T* x6, const T* x7, const T* x8, 2331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* x9, const T* x10, T* residuals) const { 2341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[0] = x1[0] * x1[0] + x2[0] * x2[0] + x3[0] * x3[0] 2351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x4[0] * x4[0] + x5[0] * x5[0] + x6[0] * x6[0] + x7[0] * x7[0] 2361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x8[0] * x8[0] + x9[0] * x9[0] + x10[0] * x10[0]; 2371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[1] = x1[1] * x1[1] + x2[1] * x2[1] + x3[1] * x3[1] 2381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x4[1] * x4[1] + x5[1] * x5[1] + x6[1] * x6[1] + x7[1] * x7[1] 2391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + x8[1] * x8[1] + x9[1] * x9[1] + x10[1] * x10[1]; 2401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 2411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 2431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define TEST_BODY(NAME) \ 2451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling TEST(CostFunctionToFunctor, NAME) { \ 2461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_ptr<CostFunction> cost_function( \ 2471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new AutoDiffCostFunction< \ 2481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunctionToFunctor<2, PARAMETER_BLOCK_SIZES >, \ 2491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2, PARAMETER_BLOCK_SIZES>(new CostFunctionToFunctor< \ 2501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2, PARAMETER_BLOCK_SIZES >( \ 2511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new AutoDiffCostFunction< \ 2521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling NAME##Functor, 2, PARAMETER_BLOCK_SIZES >( \ 2531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new NAME##Functor)))); \ 2541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling \ 2551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingscoped_ptr<CostFunction> actual_cost_function( \ 2561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new AutoDiffCostFunction<NAME##Functor, 2, PARAMETER_BLOCK_SIZES >( \ 2571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new NAME##Functor)); \ 2581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingExpectCostFunctionsAreEqual(*cost_function, *actual_cost_function); \ 2591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define PARAMETER_BLOCK_SIZES 2 2621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_BODY(OneParameterBlock) 2631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#undef PARAMETER_BLOCK_SIZES 2641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define PARAMETER_BLOCK_SIZES 2,2 2661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_BODY(TwoParameterBlock) 2671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#undef PARAMETER_BLOCK_SIZES 2681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define PARAMETER_BLOCK_SIZES 2,2,2 2701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_BODY(ThreeParameterBlock) 2711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#undef PARAMETER_BLOCK_SIZES 2721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define PARAMETER_BLOCK_SIZES 2,2,2,2 2741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_BODY(FourParameterBlock) 2751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#undef PARAMETER_BLOCK_SIZES 2761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define PARAMETER_BLOCK_SIZES 2,2,2,2,2 2781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_BODY(FiveParameterBlock) 2791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#undef PARAMETER_BLOCK_SIZES 2801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define PARAMETER_BLOCK_SIZES 2,2,2,2,2,2 2821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_BODY(SixParameterBlock) 2831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#undef PARAMETER_BLOCK_SIZES 2841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define PARAMETER_BLOCK_SIZES 2,2,2,2,2,2,2 2861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_BODY(SevenParameterBlock) 2871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#undef PARAMETER_BLOCK_SIZES 2881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define PARAMETER_BLOCK_SIZES 2,2,2,2,2,2,2,2 2901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_BODY(EightParameterBlock) 2911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#undef PARAMETER_BLOCK_SIZES 2921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define PARAMETER_BLOCK_SIZES 2,2,2,2,2,2,2,2,2 2941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_BODY(NineParameterBlock) 2951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#undef PARAMETER_BLOCK_SIZES 2961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define PARAMETER_BLOCK_SIZES 2,2,2,2,2,2,2,2,2,2 2981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingTEST_BODY(TenParameterBlock) 2991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#undef PARAMETER_BLOCK_SIZES 3001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#undef TEST_BODY 3021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace internal 3051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace ceres 306