external: Add tinyceres

This commit is contained in:
Moses Turner 2022-06-15 13:22:21 +01:00
parent 9651f66c4c
commit db85ea2df5
9 changed files with 2433 additions and 0 deletions

View file

@ -130,3 +130,7 @@ if(XRT_HAVE_OPENGL)
endif()
endif()
# tinyceres
add_library(xrt-external-tinyceres INTERFACE)
target_include_directories(xrt-external-tinyceres SYSTEM INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/tinyceres/include)

27
src/external/tinyceres/LICENSE vendored Normal file
View file

@ -0,0 +1,27 @@
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.

11
src/external/tinyceres/README.md vendored Normal file
View file

@ -0,0 +1,11 @@
<!--
Copyright 2022, Collabora, Ltd.
Authors:
Moses Turner <moses@collabora.com>
SPDX-License-Identifier: CC0-1.0
-->
tinyceres
============
tinyceres is a small template library for solving Nonlinear Least Squares problems, created from small subset of [ceres-solver](http://ceres-solver.org/) - mainly TinySolver and the files that TinySover includes. It was created for [Monado](https://monado.freedesktop.org/) for real-time optical hand tracking, and in order to avoid adding a submodule or another system dependency the code was simply copied into Monado's source tree. The source-controlled version can be found [here](https://gitlab.freedesktop.org/monado/utilities/hand-tracking-playground/tinyceres)

View file

@ -0,0 +1,200 @@
// SPDX-License-Identifier: BSD-3-Clause
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2022 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: jodebo_beck@gmx.de (Johannes Beck)
// sergiu.deitsch@gmail.com (Sergiu Deitsch)
//
// Algorithms to be used together with integer_sequence, like computing the sum
// or the exclusive scan (sometimes called exclusive prefix sum) at compile
// time.
#ifndef CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_
#define CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_
#include <utility>
#include "tinyceres/jet_fwd.hpp"
namespace ceres::internal {
// Implementation of calculating an exclusive scan (exclusive prefix sum) of an
// integer sequence. Exclusive means that the i-th input element is not included
// in the i-th sum. Calculating the exclusive scan for an input array I results
// in the following output R:
//
// R[0] = 0
// R[1] = I[0];
// R[2] = I[0] + I[1];
// R[3] = I[0] + I[1] + I[2];
// ...
//
// In C++17 std::exclusive_scan does the same operation at runtime (but
// cannot be used to calculate the prefix sum at compile time). See
// https://en.cppreference.com/w/cpp/algorithm/exclusive_scan for a more
// detailed description.
//
// Example for integer_sequence<int, 1, 4, 3> (seq := integer_sequence):
// T , Sum, Ns... , Rs...
// ExclusiveScanImpl<int, 0, seq<int, 1, 4, 3>, seq<int >>
// ExclusiveScanImpl<int, 1, seq<int, 4, 3>, seq<int, 0 >>
// ExclusiveScanImpl<int, 5, seq<int, 3>, seq<int, 0, 1 >>
// ExclusiveScanImpl<int, 8, seq<int >, seq<int, 0, 1, 5>>
// ^^^^^^^^^^^^^^^^^
// resulting sequence
template <typename T, T Sum, typename SeqIn, typename SeqOut>
struct ExclusiveScanImpl;
template <typename T, T Sum, T N, T... Ns, T... Rs>
struct ExclusiveScanImpl<T,
Sum,
std::integer_sequence<T, N, Ns...>,
std::integer_sequence<T, Rs...>> {
using Type =
typename ExclusiveScanImpl<T,
Sum + N,
std::integer_sequence<T, Ns...>,
std::integer_sequence<T, Rs..., Sum>>::Type;
};
// End of 'recursion'. The resulting type is SeqOut.
template <typename T, T Sum, typename SeqOut>
struct ExclusiveScanImpl<T, Sum, std::integer_sequence<T>, SeqOut> {
using Type = SeqOut;
};
// Calculates the exclusive scan of the specified integer sequence. The last
// element (the total) is not included in the resulting sequence so they have
// same length. This means the exclusive scan of integer_sequence<int, 1, 2, 3>
// will be integer_sequence<int, 0, 1, 3>.
template <typename Seq>
class ExclusiveScanT {
using T = typename Seq::value_type;
public:
using Type =
typename ExclusiveScanImpl<T, T(0), Seq, std::integer_sequence<T>>::Type;
};
// Helper to use exclusive scan without typename.
template <typename Seq>
using ExclusiveScan = typename ExclusiveScanT<Seq>::Type;
// Removes all elements from a integer sequence corresponding to specified
// ValueToRemove.
//
// This type should not be used directly but instead RemoveValue.
template <typename T, T ValueToRemove, typename... Sequence>
struct RemoveValueImpl;
// Final filtered sequence
template <typename T, T ValueToRemove, T... Values>
struct RemoveValueImpl<T,
ValueToRemove,
std::integer_sequence<T, Values...>,
std::integer_sequence<T>> {
using type = std::integer_sequence<T, Values...>;
};
// Found a matching value
template <typename T, T ValueToRemove, T... Head, T... Tail>
struct RemoveValueImpl<T,
ValueToRemove,
std::integer_sequence<T, Head...>,
std::integer_sequence<T, ValueToRemove, Tail...>>
: RemoveValueImpl<T,
ValueToRemove,
std::integer_sequence<T, Head...>,
std::integer_sequence<T, Tail...>> {};
// Move one element from the tail to the head
template <typename T, T ValueToRemove, T... Head, T MiddleValue, T... Tail>
struct RemoveValueImpl<T,
ValueToRemove,
std::integer_sequence<T, Head...>,
std::integer_sequence<T, MiddleValue, Tail...>>
: RemoveValueImpl<T,
ValueToRemove,
std::integer_sequence<T, Head..., MiddleValue>,
std::integer_sequence<T, Tail...>> {};
// Start recursion by splitting the integer sequence into two separate ones
template <typename T, T ValueToRemove, T... Tail>
struct RemoveValueImpl<T, ValueToRemove, std::integer_sequence<T, Tail...>>
: RemoveValueImpl<T,
ValueToRemove,
std::integer_sequence<T>,
std::integer_sequence<T, Tail...>> {};
// RemoveValue takes an integer Sequence of arbitrary type and removes all
// elements matching ValueToRemove.
//
// In contrast to RemoveValueImpl, this implementation deduces the value type
// eliminating the need to specify it explicitly.
//
// As an example, RemoveValue<std::integer_sequence<int, 1, 2, 3>, 4>::type will
// not transform the type of the original sequence. However,
// RemoveValue<std::integer_sequence<int, 0, 0, 2>, 2>::type will generate a new
// sequence of type std::integer_sequence<int, 0, 0> by removing the value 2.
template <typename Sequence, typename Sequence::value_type ValueToRemove>
struct RemoveValue
: RemoveValueImpl<typename Sequence::value_type, ValueToRemove, Sequence> {
};
// Convenience template alias for RemoveValue.
template <typename Sequence, typename Sequence::value_type ValueToRemove>
using RemoveValue_t = typename RemoveValue<Sequence, ValueToRemove>::type;
// Returns true if all elements of Values are equal to HeadValue.
//
// Returns true if Values is empty.
template <typename T, T HeadValue, T... Values>
inline constexpr bool AreAllEqual_v = ((HeadValue == Values) && ...);
// Predicate determining whether an integer sequence is either empty or all
// values are equal.
template <typename Sequence>
struct IsEmptyOrAreAllEqual;
// Empty case.
template <typename T>
struct IsEmptyOrAreAllEqual<std::integer_sequence<T>> : std::true_type {};
// General case for sequences containing at least one value.
template <typename T, T HeadValue, T... Values>
struct IsEmptyOrAreAllEqual<std::integer_sequence<T, HeadValue, Values...>>
: std::integral_constant<bool, AreAllEqual_v<T, HeadValue, Values...>> {};
// Convenience variable template for IsEmptyOrAreAllEqual.
template <class Sequence>
inline constexpr bool IsEmptyOrAreAllEqual_v =
IsEmptyOrAreAllEqual<Sequence>::value;
} // namespace ceres::internal
#endif // CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_

View file

@ -0,0 +1,196 @@
// SPDX-License-Identifier: BSD-3-Clause
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2022 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: sergiu.deitsch@gmail.com (Sergiu Deitsch)
//
#ifndef CERES_PUBLIC_INTERNAL_JET_TRAITS_H_
#define CERES_PUBLIC_INTERNAL_JET_TRAITS_H_
#include <tuple>
#include <type_traits>
#include <utility>
#include "tinyceres/internal/integer_sequence_algorithm.hpp"
#include "tinyceres/jet_fwd.hpp"
namespace ceres {
namespace internal {
// Predicate that determines whether any of the Types is a Jet.
template <typename... Types>
struct AreAnyJet : std::false_type {};
template <typename T, typename... Types>
struct AreAnyJet<T, Types...> : AreAnyJet<Types...> {};
template <typename T, int N, typename... Types>
struct AreAnyJet<Jet<T, N>, Types...> : std::true_type {};
// Convenience variable template for AreAnyJet.
template <typename... Types>
inline constexpr bool AreAnyJet_v = AreAnyJet<Types...>::value;
// Extracts the underlying floating-point from a type T.
template <typename T, typename E = void>
struct UnderlyingScalar {
using type = T;
};
template <typename T, int N>
struct UnderlyingScalar<Jet<T, N>> : UnderlyingScalar<T> {};
// Convenience template alias for UnderlyingScalar type trait.
template <typename T>
using UnderlyingScalar_t = typename UnderlyingScalar<T>::type;
// Predicate determining whether all Types in the pack are the same.
//
// Specifically, the predicate applies std::is_same recursively to pairs of
// Types in the pack.
template <typename T1, typename... Types>
inline constexpr bool AreAllSame_v = (std::is_same<T1, Types>::value && ...);
// Determines the rank of a type. This allows to ensure that types passed as
// arguments are compatible to each other. The rank of Jet is determined by the
// dimensions of the dual part. The rank of a scalar is always 0.
// Non-specialized types default to a rank of -1.
template <typename T, typename E = void>
struct Rank : std::integral_constant<int, -1> {};
// The rank of a scalar is 0.
template <typename T>
struct Rank<T, std::enable_if_t<std::is_scalar<T>::value>>
: std::integral_constant<int, 0> {};
// The rank of a Jet is given by its dimensionality.
template <typename T, int N>
struct Rank<Jet<T, N>> : std::integral_constant<int, N> {};
// Convenience variable template for Rank.
template <typename T>
inline constexpr int Rank_v = Rank<T>::value;
// Constructs an integer sequence of ranks for each of the Types in the pack.
template <typename... Types>
using Ranks_t = std::integer_sequence<int, Rank_v<Types>...>;
// Returns the scalar part of a type. This overload acts as an identity.
template <typename T>
constexpr decltype(auto) AsScalar(T&& value) noexcept {
return std::forward<T>(value);
}
// Recursively unwraps the scalar part of a Jet until a non-Jet scalar type is
// encountered.
template <typename T, int N>
constexpr decltype(auto) AsScalar(const Jet<T, N>& value) noexcept(
noexcept(AsScalar(value.a))) {
return AsScalar(value.a);
}
} // namespace internal
// Type trait ensuring at least one of the types is a Jet,
// the underlying scalar types are the same and Jet dimensions match.
//
// The type trait can be further specialized if necessary.
//
// This trait is a candidate for a concept definition once C++20 features can
// be used.
template <typename... Types>
// clang-format off
struct CompatibleJetOperands : std::integral_constant
<
bool,
// At least one of the types is a Jet
internal::AreAnyJet_v<Types...> &&
// The underlying floating-point types are exactly the same
internal::AreAllSame_v<internal::UnderlyingScalar_t<Types>...> &&
// Non-zero ranks of types are equal
internal::IsEmptyOrAreAllEqual_v<internal::RemoveValue_t<internal::Ranks_t<Types...>, 0>>
>
// clang-format on
{};
// Single Jet operand is always compatible.
template <typename T, int N>
struct CompatibleJetOperands<Jet<T, N>> : std::true_type {};
// Single non-Jet operand is always incompatible.
template <typename T>
struct CompatibleJetOperands<T> : std::false_type {};
// Empty operands are always incompatible.
template <>
struct CompatibleJetOperands<> : std::false_type {};
// Convenience variable template ensuring at least one of the types is a Jet,
// the underlying scalar types are the same and Jet dimensions match.
//
// This trait is a candidate for a concept definition once C++20 features can
// be used.
template <typename... Types>
inline constexpr bool CompatibleJetOperands_v =
CompatibleJetOperands<Types...>::value;
// Type trait ensuring at least one of the types is a Jet,
// the underlying scalar types are compatible among each other and Jet
// dimensions match.
//
// The type trait can be further specialized if necessary.
//
// This trait is a candidate for a concept definition once C++20 features can
// be used.
template <typename... Types>
// clang-format off
struct PromotableJetOperands : std::integral_constant
<
bool,
// Types can be compatible among each other
internal::AreAnyJet_v<Types...> &&
// Non-zero ranks of types are equal
internal::IsEmptyOrAreAllEqual_v<internal::RemoveValue_t<internal::Ranks_t<Types...>, 0>>
>
// clang-format on
{};
// Convenience variable template ensuring at least one of the types is a Jet,
// the underlying scalar types are compatible among each other and Jet
// dimensions match.
//
// This trait is a candidate for a concept definition once C++20 features can
// be used.
template <typename... Types>
inline constexpr bool PromotableJetOperands_v =
PromotableJetOperands<Types...>::value;
} // namespace ceres
#endif // CERES_PUBLIC_INTERNAL_JET_TRAITS_H_

File diff suppressed because it is too large Load diff

View file

@ -0,0 +1,42 @@
// SPDX-License-Identifier: BSD-3-Clause
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2022 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: sergiu.deitsch@gmail.com (Sergiu Deitsch)
//
#pragma once
namespace ceres {
// Jet forward declaration necessary for the following partial specialization of
// std::common_type and type traits.
template <typename T, int N>
struct Jet;
} // namespace ceres

View file

@ -0,0 +1,401 @@
// SPDX-License-Identifier: BSD-3-Clause
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2021 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: mierle@gmail.com (Keir Mierle)
//
// WARNING WARNING WARNING
// WARNING WARNING WARNING Tiny solver is experimental and will change.
// WARNING WARNING WARNING
//
// A tiny least squares solver using Levenberg-Marquardt, intended for solving
// small dense problems with low latency and low overhead. The implementation
// takes care to do all allocation up front, so that no memory is allocated
// during solving. This is especially useful when solving many similar problems;
// for example, inverse pixel distortion for every pixel on a grid.
//
// Note: This code has no dependencies beyond Eigen, including on other parts of
// Ceres, so it is possible to take this file alone and put it in another
// project without the rest of Ceres.
//
// Algorithm based off of:
//
// [1] K. Madsen, H. Nielsen, O. Tingleoff.
// Methods for Non-linear Least Squares Problems.
// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf
#ifndef CERES_PUBLIC_TINY_SOLVER_H_
#define CERES_PUBLIC_TINY_SOLVER_H_
#include <cassert>
#include <cmath>
#include "Eigen/Dense"
namespace ceres {
// To use tiny solver, create a class or struct that allows computing the cost
// function (described below). This is similar to a ceres::CostFunction, but is
// different to enable statically allocating all memory for the solver
// (specifically, enum sizes). Key parts are the Scalar typedef, the enums to
// describe problem sizes (needed to remove all heap allocations), and the
// operator() overload to evaluate the cost and (optionally) jacobians.
//
// struct TinySolverCostFunctionTraits {
// typedef double Scalar;
// enum {
// NUM_RESIDUALS = <int> OR Eigen::Dynamic,
// NUM_PARAMETERS = <int> OR Eigen::Dynamic,
// };
// bool operator()(const double* parameters,
// double* residuals,
// double* jacobian) const;
//
// int NumResiduals() const; -- Needed if NUM_RESIDUALS == Eigen::Dynamic.
// int NumParameters() const; -- Needed if NUM_PARAMETERS == Eigen::Dynamic.
// };
//
// For operator(), the size of the objects is:
//
// double* parameters -- NUM_PARAMETERS or NumParameters()
// double* residuals -- NUM_RESIDUALS or NumResiduals()
// double* jacobian -- NUM_RESIDUALS * NUM_PARAMETERS in column-major format
// (Eigen's default); or nullptr if no jacobian
// requested.
//
// An example (fully statically sized):
//
// struct MyCostFunctionExample {
// typedef double Scalar;
// enum {
// NUM_RESIDUALS = 2,
// NUM_PARAMETERS = 3,
// };
// bool operator()(const double* parameters,
// double* residuals,
// double* jacobian) const {
// residuals[0] = x + 2*y + 4*z;
// residuals[1] = y * z;
// if (jacobian) {
// jacobian[0 * 2 + 0] = 1; // First column (x).
// jacobian[0 * 2 + 1] = 0;
//
// jacobian[1 * 2 + 0] = 2; // Second column (y).
// jacobian[1 * 2 + 1] = z;
//
// jacobian[2 * 2 + 0] = 4; // Third column (z).
// jacobian[2 * 2 + 1] = y;
// }
// return true;
// }
// };
//
// The solver supports either statically or dynamically sized cost
// functions. If the number of residuals is dynamic then the Function
// must define:
//
// int NumResiduals() const;
//
// If the number of parameters is dynamic then the Function must
// define:
//
// int NumParameters() const;
//
template <typename Function,
typename LinearSolver =
Eigen::LDLT<Eigen::Matrix<typename Function::Scalar, //
Function::NUM_PARAMETERS, //
Function::NUM_PARAMETERS>>>
class TinySolver {
public:
// This class needs to have an Eigen aligned operator new as it contains
// fixed-size Eigen types.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
enum {
NUM_RESIDUALS = Function::NUM_RESIDUALS,
NUM_PARAMETERS = Function::NUM_PARAMETERS
};
using Scalar = typename Function::Scalar;
using Parameters = typename Eigen::Matrix<Scalar, NUM_PARAMETERS, 1>;
enum Status {
// max_norm |J'(x) * f(x)| < gradient_tolerance
GRADIENT_TOO_SMALL,
// ||dx|| <= parameter_tolerance * (||x|| + parameter_tolerance)
RELATIVE_STEP_SIZE_TOO_SMALL,
// cost_threshold > ||f(x)||^2 / 2
COST_TOO_SMALL,
// num_iterations >= max_num_iterations
HIT_MAX_ITERATIONS,
// (new_cost - old_cost) < function_tolerance * old_cost
COST_CHANGE_TOO_SMALL,
// TODO(sameeragarwal): Deal with numerical failures.
};
struct Options {
int max_num_iterations = 50;
// max_norm |J'(x) * f(x)| < gradient_tolerance
Scalar gradient_tolerance = 1e-10;
// ||dx|| <= parameter_tolerance * (||x|| + parameter_tolerance)
Scalar parameter_tolerance = 1e-8;
// (new_cost - old_cost) < function_tolerance * old_cost
Scalar function_tolerance = 1e-6;
// cost_threshold > ||f(x)||^2 / 2
Scalar cost_threshold = std::numeric_limits<Scalar>::epsilon();
Scalar initial_trust_region_radius = 1e4;
};
struct Summary {
// 1/2 ||f(x_0)||^2
Scalar initial_cost = -1;
// 1/2 ||f(x)||^2
Scalar final_cost = -1;
// max_norm(J'f(x))
Scalar gradient_max_norm = -1;
int iterations = -1;
Status status = HIT_MAX_ITERATIONS;
};
bool Update(const Function& function, const Parameters& x) {
if (!function(x.data(), residuals_.data(), jacobian_.data())) {
return false;
}
residuals_ = -residuals_;
// On the first iteration, compute a diagonal (Jacobi) scaling
// matrix, which we store as a vector.
if (summary.iterations == 0) {
// jacobi_scaling = 1 / (1 + diagonal(J'J))
//
// 1 is added to the denominator to regularize small diagonal
// entries.
jacobi_scaling_ = 1.0 / (1.0 + jacobian_.colwise().norm().array());
}
// This explicitly computes the normal equations, which is numerically
// unstable. Nevertheless, it is often good enough and is fast.
//
// TODO(sameeragarwal): Refactor this to allow for DenseQR
// factorization.
jacobian_ = jacobian_ * jacobi_scaling_.asDiagonal();
jtj_ = jacobian_.transpose() * jacobian_;
g_ = jacobian_.transpose() * residuals_;
summary.gradient_max_norm = g_.array().abs().maxCoeff();
cost_ = residuals_.squaredNorm() / 2;
return true;
}
const Summary& Solve(const Function& function, Parameters* x_and_min) {
Initialize<NUM_RESIDUALS, NUM_PARAMETERS>(function);
assert(x_and_min);
Parameters& x = *x_and_min;
summary = Summary();
summary.iterations = 0;
// TODO(sameeragarwal): Deal with failure here.
Update(function, x);
summary.initial_cost = cost_;
summary.final_cost = cost_;
if (summary.gradient_max_norm < options.gradient_tolerance) {
summary.status = GRADIENT_TOO_SMALL;
return summary;
}
if (cost_ < options.cost_threshold) {
summary.status = COST_TOO_SMALL;
return summary;
}
Scalar u = 1.0 / options.initial_trust_region_radius;
Scalar v = 2;
for (summary.iterations = 1;
summary.iterations < options.max_num_iterations;
summary.iterations++) {
jtj_regularized_ = jtj_;
const Scalar min_diagonal = 1e-6;
const Scalar max_diagonal = 1e32;
for (int i = 0; i < lm_diagonal_.rows(); ++i) {
lm_diagonal_[i] = std::sqrt(
u * (std::min)((std::max)(jtj_(i, i), min_diagonal), max_diagonal));
jtj_regularized_(i, i) += lm_diagonal_[i] * lm_diagonal_[i];
}
// TODO(sameeragarwal): Check for failure and deal with it.
linear_solver_.compute(jtj_regularized_);
lm_step_ = linear_solver_.solve(g_);
dx_ = jacobi_scaling_.asDiagonal() * lm_step_;
// Adding parameter_tolerance to x.norm() ensures that this
// works if x is near zero.
const Scalar parameter_tolerance =
options.parameter_tolerance *
(x.norm() + options.parameter_tolerance);
if (dx_.norm() < parameter_tolerance) {
summary.status = RELATIVE_STEP_SIZE_TOO_SMALL;
break;
}
x_new_ = x + dx_;
// TODO(keir): Add proper handling of errors from user eval of cost
// functions.
function(&x_new_[0], &f_x_new_[0], nullptr);
const Scalar cost_change = (2 * cost_ - f_x_new_.squaredNorm());
// TODO(sameeragarwal): Better more numerically stable evaluation.
const Scalar model_cost_change = lm_step_.dot(2 * g_ - jtj_ * lm_step_);
// rho is the ratio of the actual reduction in error to the reduction
// in error that would be obtained if the problem was linear. See [1]
// for details.
Scalar rho(cost_change / model_cost_change);
if (rho > 0) {
// Accept the Levenberg-Marquardt step because the linear
// model fits well.
x = x_new_;
if (std::abs(cost_change) < options.function_tolerance) {
cost_ = f_x_new_.squaredNorm() / 2;
summary.status = COST_CHANGE_TOO_SMALL;
break;
}
// TODO(sameeragarwal): Deal with failure.
Update(function, x);
if (summary.gradient_max_norm < options.gradient_tolerance) {
summary.status = GRADIENT_TOO_SMALL;
break;
}
if (cost_ < options.cost_threshold) {
summary.status = COST_TOO_SMALL;
break;
}
Scalar tmp = Scalar(2 * rho - 1);
u = u * (std::max)(Scalar(1 / 3.), Scalar(1) - tmp * tmp * tmp);
v = 2;
} else {
// Reject the update because either the normal equations failed to solve
// or the local linear model was not good (rho < 0).
// Additionally if the cost change is too small, then terminate.
if (std::abs(cost_change) < options.function_tolerance) {
// Terminate
summary.status = COST_CHANGE_TOO_SMALL;
break;
}
// Reduce the size of the trust region.
u *= v;
v *= 2;
}
}
summary.final_cost = cost_;
return summary;
}
Options options;
Summary summary;
private:
// Preallocate everything, including temporary storage needed for solving the
// linear system. This allows reusing the intermediate storage across solves.
LinearSolver linear_solver_;
Scalar cost_;
Parameters dx_, x_new_, g_, jacobi_scaling_, lm_diagonal_, lm_step_;
Eigen::Matrix<Scalar, NUM_RESIDUALS, 1> residuals_, f_x_new_;
Eigen::Matrix<Scalar, NUM_RESIDUALS, NUM_PARAMETERS> jacobian_;
Eigen::Matrix<Scalar, NUM_PARAMETERS, NUM_PARAMETERS> jtj_, jtj_regularized_;
// The following definitions are needed for template metaprogramming.
template <bool Condition, typename T>
struct enable_if;
template <typename T>
struct enable_if<true, T> {
using type = T;
};
// The number of parameters and residuals are dynamically sized.
template <int R, int P>
typename enable_if<(R == Eigen::Dynamic && P == Eigen::Dynamic), void>::type
Initialize(const Function& function) {
Initialize(function.NumResiduals(), function.NumParameters());
}
// The number of parameters is dynamically sized and the number of
// residuals is statically sized.
template <int R, int P>
typename enable_if<(R == Eigen::Dynamic && P != Eigen::Dynamic), void>::type
Initialize(const Function& function) {
Initialize(function.NumResiduals(), P);
}
// The number of parameters is statically sized and the number of
// residuals is dynamically sized.
template <int R, int P>
typename enable_if<(R != Eigen::Dynamic && P == Eigen::Dynamic), void>::type
Initialize(const Function& function) {
Initialize(R, function.NumParameters());
}
// The number of parameters and residuals are statically sized.
template <int R, int P>
typename enable_if<(R != Eigen::Dynamic && P != Eigen::Dynamic), void>::type
Initialize(const Function& /* function */) {}
void Initialize(int num_residuals, int num_parameters) {
dx_.resize(num_parameters);
x_new_.resize(num_parameters);
g_.resize(num_parameters);
jacobi_scaling_.resize(num_parameters);
lm_diagonal_.resize(num_parameters);
lm_step_.resize(num_parameters);
residuals_.resize(num_residuals);
f_x_new_.resize(num_residuals);
jacobian_.resize(num_residuals, num_parameters);
jtj_.resize(num_parameters, num_parameters);
jtj_regularized_.resize(num_parameters, num_parameters);
}
};
} // namespace ceres
#endif // CERES_PUBLIC_TINY_SOLVER_H_

View file

@ -0,0 +1,209 @@
// SPDX-License-Identifier: BSD-3-Clause
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2019 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: mierle@gmail.com (Keir Mierle)
//
// WARNING WARNING WARNING
// WARNING WARNING WARNING Tiny solver is experimental and will change.
// WARNING WARNING WARNING
#ifndef CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_
#define CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_
#include <memory>
#include <type_traits>
#include "Eigen/Core"
#include "tinyceres/jet.hpp"
//!@todo Really?
const double kImpossibleValue = 1e302;
namespace ceres {
// An adapter around autodiff-style CostFunctors to enable easier use of
// TinySolver. See the example below showing how to use it:
//
// // Example for cost functor with static residual size.
// // Same as an autodiff cost functor, but taking only 1 parameter.
// struct MyFunctor {
// template<typename T>
// bool operator()(const T* const parameters, T* residuals) const {
// const T& x = parameters[0];
// const T& y = parameters[1];
// const T& z = parameters[2];
// residuals[0] = x + 2.*y + 4.*z;
// residuals[1] = y * z;
// return true;
// }
// };
//
// typedef TinySolverAutoDiffFunction<MyFunctor, 2, 3>
// AutoDiffFunction;
//
// MyFunctor my_functor;
// AutoDiffFunction f(my_functor);
//
// Vec3 x = ...;
// TinySolver<AutoDiffFunction> solver;
// solver.Solve(f, &x);
//
// // Example for cost functor with dynamic residual size.
// // NumResiduals() supplies dynamic size of residuals.
// // Same functionality as in tiny_solver.h but with autodiff.
// struct MyFunctorWithDynamicResiduals {
// int NumResiduals() const {
// return 2;
// }
//
// template<typename T>
// bool operator()(const T* const parameters, T* residuals) const {
// const T& x = parameters[0];
// const T& y = parameters[1];
// const T& z = parameters[2];
// residuals[0] = x + static_cast<T>(2.)*y + static_cast<T>(4.)*z;
// residuals[1] = y * z;
// return true;
// }
// };
//
// typedef TinySolverAutoDiffFunction<MyFunctorWithDynamicResiduals,
// Eigen::Dynamic,
// 3>
// AutoDiffFunctionWithDynamicResiduals;
//
// MyFunctorWithDynamicResiduals my_functor_dyn;
// AutoDiffFunctionWithDynamicResiduals f(my_functor_dyn);
//
// Vec3 x = ...;
// TinySolver<AutoDiffFunctionWithDynamicResiduals> solver;
// solver.Solve(f, &x);
//
// WARNING: The cost function adapter is not thread safe.
template <typename CostFunctor,
int kNumResiduals,
int kNumParameters,
typename T = double>
class TinySolverAutoDiffFunction {
public:
// This class needs to have an Eigen aligned operator new as it contains
// as a member a Jet type, which itself has a fixed-size Eigen type as member.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
explicit TinySolverAutoDiffFunction(const CostFunctor& cost_functor)
: cost_functor_(cost_functor) {
Initialize<kNumResiduals>(cost_functor);
}
using Scalar = T;
enum {
NUM_PARAMETERS = kNumParameters,
NUM_RESIDUALS = kNumResiduals,
};
// This is similar to AutoDifferentiate(), but since there is only one
// parameter block it is easier to inline to avoid overhead.
bool operator()(const T* parameters, T* residuals, T* jacobian) const {
if (jacobian == nullptr) {
// No jacobian requested, so just directly call the cost function with
// doubles, skipping jets and derivatives.
return cost_functor_(parameters, residuals);
}
// Initialize the input jets with passed parameters.
for (int i = 0; i < kNumParameters; ++i) {
jet_parameters_[i].a = parameters[i]; // Scalar part.
jet_parameters_[i].v.setZero(); // Derivative part.
jet_parameters_[i].v[i] = T(1.0);
}
// Initialize the output jets such that we can detect user errors.
for (int i = 0; i < num_residuals_; ++i) {
jet_residuals_[i].a = kImpossibleValue;
jet_residuals_[i].v.setConstant(kImpossibleValue);
}
// Execute the cost function, but with jets to find the derivative.
if (!cost_functor_(jet_parameters_, jet_residuals_.data())) {
return false;
}
// Copy the jacobian out of the derivative part of the residual jets.
Eigen::Map<Eigen::Matrix<T, kNumResiduals, kNumParameters>> jacobian_matrix(
jacobian, num_residuals_, kNumParameters);
for (int r = 0; r < num_residuals_; ++r) {
residuals[r] = jet_residuals_[r].a;
// Note that while this looks like a fast vectorized write, in practice it
// unfortunately thrashes the cache since the writes to the column-major
// jacobian are strided (e.g. rows are non-contiguous).
jacobian_matrix.row(r) = jet_residuals_[r].v;
}
return true;
}
int NumResiduals() const {
return num_residuals_; // Set by Initialize.
}
private:
const CostFunctor& cost_functor_;
// The number of residuals at runtime.
// This will be overridden if NUM_RESIDUALS == Eigen::Dynamic.
int num_residuals_ = kNumResiduals;
// To evaluate the cost function with jets, temporary storage is needed. These
// are the buffers that are used during evaluation; parameters for the input,
// and jet_residuals_ are where the final cost and derivatives end up.
//
// Since this buffer is used for evaluation, the adapter is not thread safe.
using JetType = Jet<T, kNumParameters>;
mutable JetType jet_parameters_[kNumParameters];
// Eigen::Matrix serves as static or dynamic container.
mutable Eigen::Matrix<JetType, kNumResiduals, 1> jet_residuals_;
// The number of residuals is dynamically sized and the number of
// parameters is statically sized.
template <int R>
typename std::enable_if<(R == Eigen::Dynamic), void>::type Initialize(
const CostFunctor& function) {
jet_residuals_.resize(function.NumResiduals());
num_residuals_ = function.NumResiduals();
}
// The number of parameters and residuals are statically sized.
template <int R>
typename std::enable_if<(R != Eigen::Dynamic), void>::type Initialize(
const CostFunctor& /* function */) {
num_residuals_ = kNumResiduals;
}
};
} // namespace ceres
#endif // CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_