mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-01-30 18:38:32 +00:00
external: Add tinyceres
This commit is contained in:
parent
9651f66c4c
commit
db85ea2df5
src/external
4
src/external/CMakeLists.txt
vendored
4
src/external/CMakeLists.txt
vendored
|
@ -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
27
src/external/tinyceres/LICENSE
vendored
Normal 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
11
src/external/tinyceres/README.md
vendored
Normal 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)
|
200
src/external/tinyceres/include/tinyceres/internal/integer_sequence_algorithm.hpp
vendored
Normal file
200
src/external/tinyceres/include/tinyceres/internal/integer_sequence_algorithm.hpp
vendored
Normal 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_
|
196
src/external/tinyceres/include/tinyceres/internal/jet_traits.hpp
vendored
Normal file
196
src/external/tinyceres/include/tinyceres/internal/jet_traits.hpp
vendored
Normal 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_
|
1343
src/external/tinyceres/include/tinyceres/jet.hpp
vendored
Normal file
1343
src/external/tinyceres/include/tinyceres/jet.hpp
vendored
Normal file
File diff suppressed because it is too large
Load diff
42
src/external/tinyceres/include/tinyceres/jet_fwd.hpp
vendored
Normal file
42
src/external/tinyceres/include/tinyceres/jet_fwd.hpp
vendored
Normal 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
|
401
src/external/tinyceres/include/tinyceres/tiny_solver.hpp
vendored
Normal file
401
src/external/tinyceres/include/tinyceres/tiny_solver.hpp
vendored
Normal 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_
|
209
src/external/tinyceres/include/tinyceres/tiny_solver_autodiff_function.hpp
vendored
Normal file
209
src/external/tinyceres/include/tinyceres/tiny_solver_autodiff_function.hpp
vendored
Normal 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_
|
Loading…
Reference in a new issue