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 jacobian matrix computed using 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// automatic differentiation. 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/ceres.h" 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "glog/logging.h" 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingusing ceres::AutoDiffCostFunction; 401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingusing ceres::CostFunction; 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingusing ceres::Problem; 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingusing ceres::Solver; 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingusing ceres::Solve; 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// A templated cost functor that implements the residual r = 10 - 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// x. The method operator() is templated so that we can then use an 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// automatic differentiation wrapper around it to generate its 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// derivatives. 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct CostFunctor { 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling template <typename T> bool operator()(const T* const x, T* residual) const { 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling residual[0] = T(10.0) - x[0]; 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingint main(int argc, char** argv) { 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling google::InitGoogleLogging(argv[0]); 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The variable to solve for with its initial value. It will be 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // mutated in place by the solver. 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x = 0.5; 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double initial_x = x; 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Build the problem. 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Problem problem; 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Set up the only cost function (also known as residual). This uses 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // auto-differentiation to obtain the derivative (jacobian). 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CostFunction* cost_function = 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor); 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling problem.AddResidualBlock(cost_function, NULL, &x); 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Run the solver! 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Solver::Options options; 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.minimizer_progress_to_stdout = true; 761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Solver::Summary summary; 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Solve(options, &problem, &summary); 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling std::cout << summary.BriefReport() << "\n"; 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling std::cout << "x : " << initial_x 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << " -> " << x << "\n"; 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return 0; 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 84