155 lines
6.2 KiB
C
155 lines
6.2 KiB
C
|
// Ceres Solver - A fast non-linear least squares minimizer
|
||
|
|
// Copyright 2015 Google Inc. All rights reserved.
|
||
|
|
// http://ceres-solver.org/
|
||
|
|
//
|
||
|
|
// Redistribution and use in source and binary forms, with or without
|
||
|
|
// modification, are permitted provided that the following conditions are met:
|
||
|
|
//
|
||
|
|
// * Redistributions of source code must retain the above copyright notice,
|
||
|
|
// this list of conditions and the following disclaimer.
|
||
|
|
// * Redistributions in binary form must reproduce the above copyright notice,
|
||
|
|
// this list of conditions and the following disclaimer in the documentation
|
||
|
|
// and/or other materials provided with the distribution.
|
||
|
|
// * Neither the name of Google Inc. nor the names of its contributors may be
|
||
|
|
// used to endorse or promote products derived from this software without
|
||
|
|
// specific prior written permission.
|
||
|
|
//
|
||
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||
|
|
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||
|
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||
|
|
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||
|
|
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||
|
|
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||
|
|
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||
|
|
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||
|
|
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||
|
|
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||
|
|
// POSSIBILITY OF SUCH DAMAGE.
|
||
|
|
//
|
||
|
|
// Author: sergey.vfx@gmail.com (Sergey Sharybin)
|
||
|
|
// mierle@gmail.com (Keir Mierle)
|
||
|
|
// sameeragarwal@google.com (Sameer Agarwal)
|
||
|
|
|
||
|
|
#ifndef CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_
|
||
|
|
#define CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_
|
||
|
|
|
||
|
|
#include "ceres/local_parameterization.h"
|
||
|
|
#include "ceres/internal/autodiff.h"
|
||
|
|
#include "ceres/internal/scoped_ptr.h"
|
||
|
|
|
||
|
|
namespace ceres {
|
||
|
|
|
||
|
|
// Create local parameterization with Jacobians computed via automatic
|
||
|
|
// differentiation. For more information on local parameterizations,
|
||
|
|
// see include/ceres/local_parameterization.h
|
||
|
|
//
|
||
|
|
// To get an auto differentiated local parameterization, you must define
|
||
|
|
// a class with a templated operator() (a functor) that computes
|
||
|
|
//
|
||
|
|
// x_plus_delta = Plus(x, delta);
|
||
|
|
//
|
||
|
|
// the template parameter T. The autodiff framework substitutes appropriate
|
||
|
|
// "Jet" objects for T in order to compute the derivative when necessary, but
|
||
|
|
// this is hidden, and you should write the function as if T were a scalar type
|
||
|
|
// (e.g. a double-precision floating point number).
|
||
|
|
//
|
||
|
|
// The function must write the computed value in the last argument (the only
|
||
|
|
// non-const one) and return true to indicate success.
|
||
|
|
//
|
||
|
|
// For example, Quaternions have a three dimensional local
|
||
|
|
// parameterization. It's plus operation can be implemented as (taken
|
||
|
|
// from internal/ceres/auto_diff_local_parameterization_test.cc)
|
||
|
|
//
|
||
|
|
// struct QuaternionPlus {
|
||
|
|
// template<typename T>
|
||
|
|
// bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
|
||
|
|
// const T squared_norm_delta =
|
||
|
|
// delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
|
||
|
|
//
|
||
|
|
// T q_delta[4];
|
||
|
|
// if (squared_norm_delta > T(0.0)) {
|
||
|
|
// T norm_delta = sqrt(squared_norm_delta);
|
||
|
|
// const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
|
||
|
|
// q_delta[0] = cos(norm_delta);
|
||
|
|
// q_delta[1] = sin_delta_by_delta * delta[0];
|
||
|
|
// q_delta[2] = sin_delta_by_delta * delta[1];
|
||
|
|
// q_delta[3] = sin_delta_by_delta * delta[2];
|
||
|
|
// } else {
|
||
|
|
// // We do not just use q_delta = [1,0,0,0] here because that is a
|
||
|
|
// // constant and when used for automatic differentiation will
|
||
|
|
// // lead to a zero derivative. Instead we take a first order
|
||
|
|
// // approximation and evaluate it at zero.
|
||
|
|
// q_delta[0] = T(1.0);
|
||
|
|
// q_delta[1] = delta[0];
|
||
|
|
// q_delta[2] = delta[1];
|
||
|
|
// q_delta[3] = delta[2];
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// QuaternionProduct(q_delta, x, x_plus_delta);
|
||
|
|
// return true;
|
||
|
|
// }
|
||
|
|
// };
|
||
|
|
//
|
||
|
|
// Then given this struct, the auto differentiated local
|
||
|
|
// parameterization can now be constructed as
|
||
|
|
//
|
||
|
|
// LocalParameterization* local_parameterization =
|
||
|
|
// new AutoDiffLocalParameterization<QuaternionPlus, 4, 3>;
|
||
|
|
// | |
|
||
|
|
// Global Size ---------------+ |
|
||
|
|
// Local Size -------------------+
|
||
|
|
//
|
||
|
|
// WARNING: Since the functor will get instantiated with different types for
|
||
|
|
// T, you must to convert from other numeric types to T before mixing
|
||
|
|
// computations with other variables of type T. In the example above, this is
|
||
|
|
// seen where instead of using k_ directly, k_ is wrapped with T(k_).
|
||
|
|
|
||
|
|
template <typename Functor, int kGlobalSize, int kLocalSize>
|
||
|
|
class AutoDiffLocalParameterization : public LocalParameterization {
|
||
|
|
public:
|
||
|
|
AutoDiffLocalParameterization() :
|
||
|
|
functor_(new Functor()) {}
|
||
|
|
|
||
|
|
// Takes ownership of functor.
|
||
|
|
explicit AutoDiffLocalParameterization(Functor* functor) :
|
||
|
|
functor_(functor) {}
|
||
|
|
|
||
|
|
virtual ~AutoDiffLocalParameterization() {}
|
||
|
|
virtual bool Plus(const double* x,
|
||
|
|
const double* delta,
|
||
|
|
double* x_plus_delta) const {
|
||
|
|
return (*functor_)(x, delta, x_plus_delta);
|
||
|
|
}
|
||
|
|
|
||
|
|
virtual bool ComputeJacobian(const double* x, double* jacobian) const {
|
||
|
|
double zero_delta[kLocalSize];
|
||
|
|
for (int i = 0; i < kLocalSize; ++i) {
|
||
|
|
zero_delta[i] = 0.0;
|
||
|
|
}
|
||
|
|
|
||
|
|
double x_plus_delta[kGlobalSize];
|
||
|
|
for (int i = 0; i < kGlobalSize; ++i) {
|
||
|
|
x_plus_delta[i] = 0.0;
|
||
|
|
}
|
||
|
|
|
||
|
|
const double* parameter_ptrs[2] = {x, zero_delta};
|
||
|
|
double* jacobian_ptrs[2] = { NULL, jacobian };
|
||
|
|
return internal::AutoDiff<Functor, double, kGlobalSize, kLocalSize>
|
||
|
|
::Differentiate(*functor_,
|
||
|
|
parameter_ptrs,
|
||
|
|
kGlobalSize,
|
||
|
|
x_plus_delta,
|
||
|
|
jacobian_ptrs);
|
||
|
|
}
|
||
|
|
|
||
|
|
virtual int GlobalSize() const { return kGlobalSize; }
|
||
|
|
virtual int LocalSize() const { return kLocalSize; }
|
||
|
|
|
||
|
|
private:
|
||
|
|
internal::scoped_ptr<Functor> functor_;
|
||
|
|
};
|
||
|
|
|
||
|
|
} // namespace ceres
|
||
|
|
|
||
|
|
#endif // CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_
|