11d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Ceres Solver - A fast non-linear least squares minimizer 21d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) 301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// A simple example of using the Ceres minimizer. 321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Minimize 0.5 (10 - x)^2 using analytic jacobian matrix. 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <vector> 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/ceres.h" 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "glog/logging.h" 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingusing ceres::CostFunction; 401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingusing ceres::SizedCostFunction; 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingusing ceres::Problem; 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingusing ceres::Solver; 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingusing ceres::Solve; 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// A CostFunction implementing analytically derivatives for the 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// function f(x) = 10 - x. 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingclass QuadraticCostFunction 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling : public SizedCostFunction<1 /* number of residuals */, 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1 /* size of first parameter */> { 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual ~QuadraticCostFunction() {} 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual bool Evaluate(double const* const* parameters, 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* residuals, 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double** jacobians) const { 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x = parameters[0][0]; 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // f(x) = 10 - x. 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residuals[0] = 10 - x; 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // f'(x) = -1. Since there's only 1 parameter and that parameter 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // has 1 dimension, there is only 1 element to fill in the 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // jacobians. 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Since the Evaluate function can be called with the jacobians 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // pointer equal to NULL, the Evaluate function must check to see 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // if jacobians need to be computed. 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // For this simple problem it is overkill to check if jacobians[0] 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // is NULL, but in general when writing more complex 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // CostFunctions, it is possible that Ceres may only demand the 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // derivatives w.r.t. a subset of the parameter blocks. 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (jacobians != NULL && jacobians[0] != NULL) { 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobians[0][0] = -1; 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingint main(int argc, char** argv) { 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling google::InitGoogleLogging(argv[0]); 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The variable to solve for with its initial value. It will be 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // mutated in place by the solver. 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x = 0.5; 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double initial_x = x; 881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Build the problem. 901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Problem problem; 911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Set up the only cost function (also known as residual). 931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_function = new QuadraticCostFunction; 941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.AddResidualBlock(cost_function, NULL, &x); 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Run the solver! 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Solver::Options options; 981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.minimizer_progress_to_stdout = true; 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Solver::Summary summary; 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Solve(options, &problem, &summary); 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling std::cout << summary.BriefReport() << "\n"; 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling std::cout << "x : " << initial_x 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << " -> " << x << "\n"; 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return 0; 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 108