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: sergey.vfx@gmail.com (Sergey Sharybin) 301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// mierle@gmail.com (Keir Mierle) 311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// sameeragarwal@google.com (Sameer Agarwal) 321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#ifndef CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_ 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#define CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_ 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/autodiff.h" 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/scoped_ptr.h" 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/local_parameterization.h" 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace ceres { 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Create local parameterization with Jacobians computed via automatic 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// differentiation. For more information on local parameterizations, 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// see include/ceres/local_parameterization.h 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// To get an auto differentiated local parameterization, you must define 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// a class with a templated operator() (a functor) that computes 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// x_plus_delta = Plus(x, delta); 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// the template parameter T. The autodiff framework substitutes appropriate 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// "Jet" objects for T in order to compute the derivative when necessary, but 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this is hidden, and you should write the function as if T were a scalar type 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// (e.g. a double-precision floating point number). 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// The function must write the computed value in the last argument (the only 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// non-const one) and return true to indicate success. 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// For example, Quaternions have a three dimensional local 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// parameterization. It's plus operation can be implemented as (taken 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// from internal/ceres/auto_diff_local_parameterization_test.cc) 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// struct QuaternionPlus { 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// template<typename T> 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// bool operator()(const T* x, const T* delta, T* x_plus_delta) const { 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const T squared_norm_delta = 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// T q_delta[4]; 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// if (squared_norm_delta > T(0.0)) { 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// T norm_delta = sqrt(squared_norm_delta); 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// const T sin_delta_by_delta = sin(norm_delta) / norm_delta; 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// q_delta[0] = cos(norm_delta); 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// q_delta[1] = sin_delta_by_delta * delta[0]; 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// q_delta[2] = sin_delta_by_delta * delta[1]; 761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// q_delta[3] = sin_delta_by_delta * delta[2]; 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } else { 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// // We do not just use q_delta = [1,0,0,0] here because that is a 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// // constant and when used for automatic differentiation will 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// // lead to a zero derivative. Instead we take a first order 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// // approximation and evaluate it at zero. 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// q_delta[0] = T(1.0); 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// q_delta[1] = delta[0]; 841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// q_delta[2] = delta[1]; 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// q_delta[3] = delta[2]; 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// QuaternionProduct(q_delta, x, x_plus_delta); 891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// return true; 901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// } 911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// }; 921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Then given this struct, the auto differentiated local 941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// parameterization can now be constructed as 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// LocalParameterization* local_parameterization = 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// new AutoDiffLocalParameterization<QuaternionPlus, 4, 3>; 981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// | | 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Global Size ---------------+ | 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Local Size -------------------+ 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// WARNING: Since the functor will get instantiated with different types for 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// T, you must to convert from other numeric types to T before mixing 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// computations with other variables of type T. In the example above, this is 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// seen where instead of using k_ directly, k_ is wrapped with T(k_). 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename Functor, int kGlobalSize, int kLocalSize> 1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingclass AutoDiffLocalParameterization : public LocalParameterization { 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling public: 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual ~AutoDiffLocalParameterization() {} 1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual bool Plus(const double* x, 1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* delta, 1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* x_plus_delta) const { 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return Functor()(x, delta, x_plus_delta); 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual bool ComputeJacobian(const double* x, double* jacobian) const { 1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double zero_delta[kLocalSize]; 1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < kLocalSize; ++i) { 1201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling zero_delta[i] = 0.0; 1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x_plus_delta[kGlobalSize]; 1241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < kGlobalSize; ++i) { 1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x_plus_delta[i] = 0.0; 1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double* parameter_ptrs[2] = {x, zero_delta}; 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* jacobian_ptrs[2] = { NULL, jacobian }; 1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return internal::AutoDiff<Functor, double, kGlobalSize, kLocalSize> 1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ::Differentiate(Functor(), 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling parameter_ptrs, 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling kGlobalSize, 1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x_plus_delta, 1351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling jacobian_ptrs); 1361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual int GlobalSize() const { return kGlobalSize; } 1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling virtual int LocalSize() const { return kLocalSize; } 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace ceres 1431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#endif // CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_ 145