+4

src/external/CMakeLists.txt

··· 130 130 endif() 131 131 132 132 endif() 133 133 + 134 134 + # tinyceres 135 135 + add_library(xrt-external-tinyceres INTERFACE) 136 136 + target_include_directories(xrt-external-tinyceres SYSTEM INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/tinyceres/include)

+27

src/external/tinyceres/LICENSE

··· 1 1 + Ceres Solver - A fast non-linear least squares minimizer 2 2 + Copyright 2015 Google Inc. All rights reserved. 3 3 + http://ceres-solver.org/ 4 4 + 5 5 + Redistribution and use in source and binary forms, with or without 6 6 + modification, are permitted provided that the following conditions are met: 7 7 + 8 8 + * Redistributions of source code must retain the above copyright notice, 9 9 + this list of conditions and the following disclaimer. 10 10 + * Redistributions in binary form must reproduce the above copyright notice, 11 11 + this list of conditions and the following disclaimer in the documentation 12 12 + and/or other materials provided with the distribution. 13 13 + * Neither the name of Google Inc. nor the names of its contributors may be 14 14 + used to endorse or promote products derived from this software without 15 15 + specific prior written permission. 16 16 + 17 17 + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 18 + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 19 + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 20 + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 21 + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 22 + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 23 + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 24 + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 25 + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 26 + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 27 + POSSIBILITY OF SUCH DAMAGE.

+11

src/external/tinyceres/README.md

··· 1 1 + <!-- 2 2 + Copyright 2022, Collabora, Ltd. 3 3 + Authors: 4 4 + Moses Turner <moses@collabora.com> 5 5 + SPDX-License-Identifier: CC0-1.0 6 6 + --> 7 7 + 8 8 + tinyceres 9 9 + ============ 10 10 + 11 11 + 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

··· 1 1 + // SPDX-License-Identifier: BSD-3-Clause 2 2 + // Ceres Solver - A fast non-linear least squares minimizer 3 3 + // Copyright 2022 Google Inc. All rights reserved. 4 4 + // http://ceres-solver.org/ 5 5 + // 6 6 + // Redistribution and use in source and binary forms, with or without 7 7 + // modification, are permitted provided that the following conditions are met: 8 8 + // 9 9 + // * Redistributions of source code must retain the above copyright notice, 10 10 + // this list of conditions and the following disclaimer. 11 11 + // * Redistributions in binary form must reproduce the above copyright notice, 12 12 + // this list of conditions and the following disclaimer in the documentation 13 13 + // and/or other materials provided with the distribution. 14 14 + // * Neither the name of Google Inc. nor the names of its contributors may be 15 15 + // used to endorse or promote products derived from this software without 16 16 + // specific prior written permission. 17 17 + // 18 18 + // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 19 + // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 20 + // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 21 + // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 22 + // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 23 + // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 24 + // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 25 + // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 26 + // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 27 + // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 28 + // POSSIBILITY OF SUCH DAMAGE. 29 29 + // 30 30 + // Author: jodebo_beck@gmx.de (Johannes Beck) 31 31 + // sergiu.deitsch@gmail.com (Sergiu Deitsch) 32 32 + // 33 33 + // Algorithms to be used together with integer_sequence, like computing the sum 34 34 + // or the exclusive scan (sometimes called exclusive prefix sum) at compile 35 35 + // time. 36 36 + 37 37 + #ifndef CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_ 38 38 + #define CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_ 39 39 + 40 40 + #include <utility> 41 41 + 42 42 + #include "tinyceres/jet_fwd.hpp" 43 43 + 44 44 + namespace ceres::internal { 45 45 + 46 46 + // Implementation of calculating an exclusive scan (exclusive prefix sum) of an 47 47 + // integer sequence. Exclusive means that the i-th input element is not included 48 48 + // in the i-th sum. Calculating the exclusive scan for an input array I results 49 49 + // in the following output R: 50 50 + // 51 51 + // R[0] = 0 52 52 + // R[1] = I[0]; 53 53 + // R[2] = I[0] + I[1]; 54 54 + // R[3] = I[0] + I[1] + I[2]; 55 55 + // ... 56 56 + // 57 57 + // In C++17 std::exclusive_scan does the same operation at runtime (but 58 58 + // cannot be used to calculate the prefix sum at compile time). See 59 59 + // https://en.cppreference.com/w/cpp/algorithm/exclusive_scan for a more 60 60 + // detailed description. 61 61 + // 62 62 + // Example for integer_sequence<int, 1, 4, 3> (seq := integer_sequence): 63 63 + // T , Sum, Ns... , Rs... 64 64 + // ExclusiveScanImpl<int, 0, seq<int, 1, 4, 3>, seq<int >> 65 65 + // ExclusiveScanImpl<int, 1, seq<int, 4, 3>, seq<int, 0 >> 66 66 + // ExclusiveScanImpl<int, 5, seq<int, 3>, seq<int, 0, 1 >> 67 67 + // ExclusiveScanImpl<int, 8, seq<int >, seq<int, 0, 1, 5>> 68 68 + // ^^^^^^^^^^^^^^^^^ 69 69 + // resulting sequence 70 70 + template <typename T, T Sum, typename SeqIn, typename SeqOut> 71 71 + struct ExclusiveScanImpl; 72 72 + 73 73 + template <typename T, T Sum, T N, T... Ns, T... Rs> 74 74 + struct ExclusiveScanImpl<T, 75 75 + Sum, 76 76 + std::integer_sequence<T, N, Ns...>, 77 77 + std::integer_sequence<T, Rs...>> { 78 78 + using Type = 79 79 + typename ExclusiveScanImpl<T, 80 80 + Sum + N, 81 81 + std::integer_sequence<T, Ns...>, 82 82 + std::integer_sequence<T, Rs..., Sum>>::Type; 83 83 + }; 84 84 + 85 85 + // End of 'recursion'. The resulting type is SeqOut. 86 86 + template <typename T, T Sum, typename SeqOut> 87 87 + struct ExclusiveScanImpl<T, Sum, std::integer_sequence<T>, SeqOut> { 88 88 + using Type = SeqOut; 89 89 + }; 90 90 + 91 91 + // Calculates the exclusive scan of the specified integer sequence. The last 92 92 + // element (the total) is not included in the resulting sequence so they have 93 93 + // same length. This means the exclusive scan of integer_sequence<int, 1, 2, 3> 94 94 + // will be integer_sequence<int, 0, 1, 3>. 95 95 + template <typename Seq> 96 96 + class ExclusiveScanT { 97 97 + using T = typename Seq::value_type; 98 98 + 99 99 + public: 100 100 + using Type = 101 101 + typename ExclusiveScanImpl<T, T(0), Seq, std::integer_sequence<T>>::Type; 102 102 + }; 103 103 + 104 104 + // Helper to use exclusive scan without typename. 105 105 + template <typename Seq> 106 106 + using ExclusiveScan = typename ExclusiveScanT<Seq>::Type; 107 107 + 108 108 + // Removes all elements from a integer sequence corresponding to specified 109 109 + // ValueToRemove. 110 110 + // 111 111 + // This type should not be used directly but instead RemoveValue. 112 112 + template <typename T, T ValueToRemove, typename... Sequence> 113 113 + struct RemoveValueImpl; 114 114 + 115 115 + // Final filtered sequence 116 116 + template <typename T, T ValueToRemove, T... Values> 117 117 + struct RemoveValueImpl<T, 118 118 + ValueToRemove, 119 119 + std::integer_sequence<T, Values...>, 120 120 + std::integer_sequence<T>> { 121 121 + using type = std::integer_sequence<T, Values...>; 122 122 + }; 123 123 + 124 124 + // Found a matching value 125 125 + template <typename T, T ValueToRemove, T... Head, T... Tail> 126 126 + struct RemoveValueImpl<T, 127 127 + ValueToRemove, 128 128 + std::integer_sequence<T, Head...>, 129 129 + std::integer_sequence<T, ValueToRemove, Tail...>> 130 130 + : RemoveValueImpl<T, 131 131 + ValueToRemove, 132 132 + std::integer_sequence<T, Head...>, 133 133 + std::integer_sequence<T, Tail...>> {}; 134 134 + 135 135 + // Move one element from the tail to the head 136 136 + template <typename T, T ValueToRemove, T... Head, T MiddleValue, T... Tail> 137 137 + struct RemoveValueImpl<T, 138 138 + ValueToRemove, 139 139 + std::integer_sequence<T, Head...>, 140 140 + std::integer_sequence<T, MiddleValue, Tail...>> 141 141 + : RemoveValueImpl<T, 142 142 + ValueToRemove, 143 143 + std::integer_sequence<T, Head..., MiddleValue>, 144 144 + std::integer_sequence<T, Tail...>> {}; 145 145 + 146 146 + // Start recursion by splitting the integer sequence into two separate ones 147 147 + template <typename T, T ValueToRemove, T... Tail> 148 148 + struct RemoveValueImpl<T, ValueToRemove, std::integer_sequence<T, Tail...>> 149 149 + : RemoveValueImpl<T, 150 150 + ValueToRemove, 151 151 + std::integer_sequence<T>, 152 152 + std::integer_sequence<T, Tail...>> {}; 153 153 + 154 154 + // RemoveValue takes an integer Sequence of arbitrary type and removes all 155 155 + // elements matching ValueToRemove. 156 156 + // 157 157 + // In contrast to RemoveValueImpl, this implementation deduces the value type 158 158 + // eliminating the need to specify it explicitly. 159 159 + // 160 160 + // As an example, RemoveValue<std::integer_sequence<int, 1, 2, 3>, 4>::type will 161 161 + // not transform the type of the original sequence. However, 162 162 + // RemoveValue<std::integer_sequence<int, 0, 0, 2>, 2>::type will generate a new 163 163 + // sequence of type std::integer_sequence<int, 0, 0> by removing the value 2. 164 164 + template <typename Sequence, typename Sequence::value_type ValueToRemove> 165 165 + struct RemoveValue 166 166 + : RemoveValueImpl<typename Sequence::value_type, ValueToRemove, Sequence> { 167 167 + }; 168 168 + 169 169 + // Convenience template alias for RemoveValue. 170 170 + template <typename Sequence, typename Sequence::value_type ValueToRemove> 171 171 + using RemoveValue_t = typename RemoveValue<Sequence, ValueToRemove>::type; 172 172 + 173 173 + // Returns true if all elements of Values are equal to HeadValue. 174 174 + // 175 175 + // Returns true if Values is empty. 176 176 + template <typename T, T HeadValue, T... Values> 177 177 + inline constexpr bool AreAllEqual_v = ((HeadValue == Values) && ...); 178 178 + 179 179 + // Predicate determining whether an integer sequence is either empty or all 180 180 + // values are equal. 181 181 + template <typename Sequence> 182 182 + struct IsEmptyOrAreAllEqual; 183 183 + 184 184 + // Empty case. 185 185 + template <typename T> 186 186 + struct IsEmptyOrAreAllEqual<std::integer_sequence<T>> : std::true_type {}; 187 187 + 188 188 + // General case for sequences containing at least one value. 189 189 + template <typename T, T HeadValue, T... Values> 190 190 + struct IsEmptyOrAreAllEqual<std::integer_sequence<T, HeadValue, Values...>> 191 191 + : std::integral_constant<bool, AreAllEqual_v<T, HeadValue, Values...>> {}; 192 192 + 193 193 + // Convenience variable template for IsEmptyOrAreAllEqual. 194 194 + template <class Sequence> 195 195 + inline constexpr bool IsEmptyOrAreAllEqual_v = 196 196 + IsEmptyOrAreAllEqual<Sequence>::value; 197 197 + 198 198 + } // namespace ceres::internal 199 199 + 200 200 + #endif // CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_

+196

src/external/tinyceres/include/tinyceres/internal/jet_traits.hpp

··· 1 1 + // SPDX-License-Identifier: BSD-3-Clause 2 2 + // Ceres Solver - A fast non-linear least squares minimizer 3 3 + // Copyright 2022 Google Inc. All rights reserved. 4 4 + // http://ceres-solver.org/ 5 5 + // 6 6 + // Redistribution and use in source and binary forms, with or without 7 7 + // modification, are permitted provided that the following conditions are met: 8 8 + // 9 9 + // * Redistributions of source code must retain the above copyright notice, 10 10 + // this list of conditions and the following disclaimer. 11 11 + // * Redistributions in binary form must reproduce the above copyright notice, 12 12 + // this list of conditions and the following disclaimer in the documentation 13 13 + // and/or other materials provided with the distribution. 14 14 + // * Neither the name of Google Inc. nor the names of its contributors may be 15 15 + // used to endorse or promote products derived from this software without 16 16 + // specific prior written permission. 17 17 + // 18 18 + // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 19 + // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 20 + // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 21 + // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 22 + // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 23 + // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 24 + // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 25 + // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 26 + // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 27 + // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 28 + // POSSIBILITY OF SUCH DAMAGE. 29 29 + // 30 30 + // Author: sergiu.deitsch@gmail.com (Sergiu Deitsch) 31 31 + // 32 32 + 33 33 + #ifndef CERES_PUBLIC_INTERNAL_JET_TRAITS_H_ 34 34 + #define CERES_PUBLIC_INTERNAL_JET_TRAITS_H_ 35 35 + 36 36 + #include <tuple> 37 37 + #include <type_traits> 38 38 + #include <utility> 39 39 + 40 40 + #include "tinyceres/internal/integer_sequence_algorithm.hpp" 41 41 + #include "tinyceres/jet_fwd.hpp" 42 42 + 43 43 + namespace ceres { 44 44 + namespace internal { 45 45 + 46 46 + // Predicate that determines whether any of the Types is a Jet. 47 47 + template <typename... Types> 48 48 + struct AreAnyJet : std::false_type {}; 49 49 + 50 50 + template <typename T, typename... Types> 51 51 + struct AreAnyJet<T, Types...> : AreAnyJet<Types...> {}; 52 52 + 53 53 + template <typename T, int N, typename... Types> 54 54 + struct AreAnyJet<Jet<T, N>, Types...> : std::true_type {}; 55 55 + 56 56 + // Convenience variable template for AreAnyJet. 57 57 + template <typename... Types> 58 58 + inline constexpr bool AreAnyJet_v = AreAnyJet<Types...>::value; 59 59 + 60 60 + // Extracts the underlying floating-point from a type T. 61 61 + template <typename T, typename E = void> 62 62 + struct UnderlyingScalar { 63 63 + using type = T; 64 64 + }; 65 65 + 66 66 + template <typename T, int N> 67 67 + struct UnderlyingScalar<Jet<T, N>> : UnderlyingScalar<T> {}; 68 68 + 69 69 + // Convenience template alias for UnderlyingScalar type trait. 70 70 + template <typename T> 71 71 + using UnderlyingScalar_t = typename UnderlyingScalar<T>::type; 72 72 + 73 73 + // Predicate determining whether all Types in the pack are the same. 74 74 + // 75 75 + // Specifically, the predicate applies std::is_same recursively to pairs of 76 76 + // Types in the pack. 77 77 + template <typename T1, typename... Types> 78 78 + inline constexpr bool AreAllSame_v = (std::is_same<T1, Types>::value && ...); 79 79 + 80 80 + // Determines the rank of a type. This allows to ensure that types passed as 81 81 + // arguments are compatible to each other. The rank of Jet is determined by the 82 82 + // dimensions of the dual part. The rank of a scalar is always 0. 83 83 + // Non-specialized types default to a rank of -1. 84 84 + template <typename T, typename E = void> 85 85 + struct Rank : std::integral_constant<int, -1> {}; 86 86 + 87 87 + // The rank of a scalar is 0. 88 88 + template <typename T> 89 89 + struct Rank<T, std::enable_if_t<std::is_scalar<T>::value>> 90 90 + : std::integral_constant<int, 0> {}; 91 91 + 92 92 + // The rank of a Jet is given by its dimensionality. 93 93 + template <typename T, int N> 94 94 + struct Rank<Jet<T, N>> : std::integral_constant<int, N> {}; 95 95 + 96 96 + // Convenience variable template for Rank. 97 97 + template <typename T> 98 98 + inline constexpr int Rank_v = Rank<T>::value; 99 99 + 100 100 + // Constructs an integer sequence of ranks for each of the Types in the pack. 101 101 + template <typename... Types> 102 102 + using Ranks_t = std::integer_sequence<int, Rank_v<Types>...>; 103 103 + 104 104 + // Returns the scalar part of a type. This overload acts as an identity. 105 105 + template <typename T> 106 106 + constexpr decltype(auto) AsScalar(T&& value) noexcept { 107 107 + return std::forward<T>(value); 108 108 + } 109 109 + 110 110 + // Recursively unwraps the scalar part of a Jet until a non-Jet scalar type is 111 111 + // encountered. 112 112 + template <typename T, int N> 113 113 + constexpr decltype(auto) AsScalar(const Jet<T, N>& value) noexcept( 114 114 + noexcept(AsScalar(value.a))) { 115 115 + return AsScalar(value.a); 116 116 + } 117 117 + 118 118 + } // namespace internal 119 119 + 120 120 + // Type trait ensuring at least one of the types is a Jet, 121 121 + // the underlying scalar types are the same and Jet dimensions match. 122 122 + // 123 123 + // The type trait can be further specialized if necessary. 124 124 + // 125 125 + // This trait is a candidate for a concept definition once C++20 features can 126 126 + // be used. 127 127 + template <typename... Types> 128 128 + // clang-format off 129 129 + struct CompatibleJetOperands : std::integral_constant 130 130 + < 131 131 + bool, 132 132 + // At least one of the types is a Jet 133 133 + internal::AreAnyJet_v<Types...> && 134 134 + // The underlying floating-point types are exactly the same 135 135 + internal::AreAllSame_v<internal::UnderlyingScalar_t<Types>...> && 136 136 + // Non-zero ranks of types are equal 137 137 + internal::IsEmptyOrAreAllEqual_v<internal::RemoveValue_t<internal::Ranks_t<Types...>, 0>> 138 138 + > 139 139 + // clang-format on 140 140 + {}; 141 141 + 142 142 + // Single Jet operand is always compatible. 143 143 + template <typename T, int N> 144 144 + struct CompatibleJetOperands<Jet<T, N>> : std::true_type {}; 145 145 + 146 146 + // Single non-Jet operand is always incompatible. 147 147 + template <typename T> 148 148 + struct CompatibleJetOperands<T> : std::false_type {}; 149 149 + 150 150 + // Empty operands are always incompatible. 151 151 + template <> 152 152 + struct CompatibleJetOperands<> : std::false_type {}; 153 153 + 154 154 + // Convenience variable template ensuring at least one of the types is a Jet, 155 155 + // the underlying scalar types are the same and Jet dimensions match. 156 156 + // 157 157 + // This trait is a candidate for a concept definition once C++20 features can 158 158 + // be used. 159 159 + template <typename... Types> 160 160 + inline constexpr bool CompatibleJetOperands_v = 161 161 + CompatibleJetOperands<Types...>::value; 162 162 + 163 163 + // Type trait ensuring at least one of the types is a Jet, 164 164 + // the underlying scalar types are compatible among each other and Jet 165 165 + // dimensions match. 166 166 + // 167 167 + // The type trait can be further specialized if necessary. 168 168 + // 169 169 + // This trait is a candidate for a concept definition once C++20 features can 170 170 + // be used. 171 171 + template <typename... Types> 172 172 + // clang-format off 173 173 + struct PromotableJetOperands : std::integral_constant 174 174 + < 175 175 + bool, 176 176 + // Types can be compatible among each other 177 177 + internal::AreAnyJet_v<Types...> && 178 178 + // Non-zero ranks of types are equal 179 179 + internal::IsEmptyOrAreAllEqual_v<internal::RemoveValue_t<internal::Ranks_t<Types...>, 0>> 180 180 + > 181 181 + // clang-format on 182 182 + {}; 183 183 + 184 184 + // Convenience variable template ensuring at least one of the types is a Jet, 185 185 + // the underlying scalar types are compatible among each other and Jet 186 186 + // dimensions match. 187 187 + // 188 188 + // This trait is a candidate for a concept definition once C++20 features can 189 189 + // be used. 190 190 + template <typename... Types> 191 191 + inline constexpr bool PromotableJetOperands_v = 192 192 + PromotableJetOperands<Types...>::value; 193 193 + 194 194 + } // namespace ceres 195 195 + 196 196 + #endif // CERES_PUBLIC_INTERNAL_JET_TRAITS_H_

+1343

src/external/tinyceres/include/tinyceres/jet.hpp

··· 1 1 + // SPDX-License-Identifier: BSD-3-Clause 2 2 + // Ceres Solver - A fast non-linear least squares minimizer 3 3 + // Copyright 2022 Google Inc. All rights reserved. 4 4 + // http://ceres-solver.org/ 5 5 + // 6 6 + // Redistribution and use in source and binary forms, with or without 7 7 + // modification, are permitted provided that the following conditions are met: 8 8 + // 9 9 + // * Redistributions of source code must retain the above copyright notice, 10 10 + // this list of conditions and the following disclaimer. 11 11 + // * Redistributions in binary form must reproduce the above copyright notice, 12 12 + // this list of conditions and the following disclaimer in the documentation 13 13 + // and/or other materials provided with the distribution. 14 14 + // * Neither the name of Google Inc. nor the names of its contributors may be 15 15 + // used to endorse or promote products derived from this software without 16 16 + // specific prior written permission. 17 17 + // 18 18 + // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 19 + // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 20 + // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 21 + // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 22 + // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 23 + // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 24 + // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 25 + // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 26 + // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 27 + // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 28 + // POSSIBILITY OF SUCH DAMAGE. 29 29 + // 30 30 + // Author: keir@google.com (Keir Mierle) 31 31 + // 32 32 + // A simple implementation of N-dimensional dual numbers, for automatically 33 33 + // computing exact derivatives of functions. 34 34 + // 35 35 + // While a complete treatment of the mechanics of automatic differentiation is 36 36 + // beyond the scope of this header (see 37 37 + // http://en.wikipedia.org/wiki/Automatic_differentiation for details), the 38 38 + // basic idea is to extend normal arithmetic with an extra element, "e," often 39 39 + // denoted with the greek symbol epsilon, such that e != 0 but e^2 = 0. Dual 40 40 + // numbers are extensions of the real numbers analogous to complex numbers: 41 41 + // whereas complex numbers augment the reals by introducing an imaginary unit i 42 42 + // such that i^2 = -1, dual numbers introduce an "infinitesimal" unit e such 43 43 + // that e^2 = 0. Dual numbers have two components: the "real" component and the 44 44 + // "infinitesimal" component, generally written as x + y*e. Surprisingly, this 45 45 + // leads to a convenient method for computing exact derivatives without needing 46 46 + // to manipulate complicated symbolic expressions. 47 47 + // 48 48 + // For example, consider the function 49 49 + // 50 50 + // f(x) = x^2 , 51 51 + // 52 52 + // evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20. 53 53 + // Next, argument 10 with an infinitesimal to get: 54 54 + // 55 55 + // f(10 + e) = (10 + e)^2 56 56 + // = 100 + 2 * 10 * e + e^2 57 57 + // = 100 + 20 * e -+- 58 58 + // -- | 59 59 + // | +--- This is zero, since e^2 = 0 60 60 + // | 61 61 + // +----------------- This is df/dx! 62 62 + // 63 63 + // Note that the derivative of f with respect to x is simply the infinitesimal 64 64 + // component of the value of f(x + e). So, in order to take the derivative of 65 65 + // any function, it is only necessary to replace the numeric "object" used in 66 66 + // the function with one extended with infinitesimals. The class Jet, defined in 67 67 + // this header, is one such example of this, where substitution is done with 68 68 + // templates. 69 69 + // 70 70 + // To handle derivatives of functions taking multiple arguments, different 71 71 + // infinitesimals are used, one for each variable to take the derivative of. For 72 72 + // example, consider a scalar function of two scalar parameters x and y: 73 73 + // 74 74 + // f(x, y) = x^2 + x * y 75 75 + // 76 76 + // Following the technique above, to compute the derivatives df/dx and df/dy for 77 77 + // f(1, 3) involves doing two evaluations of f, the first time replacing x with 78 78 + // x + e, the second time replacing y with y + e. 79 79 + // 80 80 + // For df/dx: 81 81 + // 82 82 + // f(1 + e, y) = (1 + e)^2 + (1 + e) * 3 83 83 + // = 1 + 2 * e + 3 + 3 * e 84 84 + // = 4 + 5 * e 85 85 + // 86 86 + // --> df/dx = 5 87 87 + // 88 88 + // For df/dy: 89 89 + // 90 90 + // f(1, 3 + e) = 1^2 + 1 * (3 + e) 91 91 + // = 1 + 3 + e 92 92 + // = 4 + e 93 93 + // 94 94 + // --> df/dy = 1 95 95 + // 96 96 + // To take the gradient of f with the implementation of dual numbers ("jets") in 97 97 + // this file, it is necessary to create a single jet type which has components 98 98 + // for the derivative in x and y, and passing them to a templated version of f: 99 99 + // 100 100 + // template<typename T> 101 101 + // T f(const T &x, const T &y) { 102 102 + // return x * x + x * y; 103 103 + // } 104 104 + // 105 105 + // // The "2" means there should be 2 dual number components. 106 106 + // // It computes the partial derivative at x=10, y=20. 107 107 + // Jet<double, 2> x(10, 0); // Pick the 0th dual number for x. 108 108 + // Jet<double, 2> y(20, 1); // Pick the 1st dual number for y. 109 109 + // Jet<double, 2> z = f(x, y); 110 110 + // 111 111 + // LOG(INFO) << "df/dx = " << z.v[0] 112 112 + // << "df/dy = " << z.v[1]; 113 113 + // 114 114 + // Most users should not use Jet objects directly; a wrapper around Jet objects, 115 115 + // which makes computing the derivative, gradient, or jacobian of templated 116 116 + // functors simple, is in autodiff.h. Even autodiff.h should not be used 117 117 + // directly; instead autodiff_cost_function.h is typically the file of interest. 118 118 + // 119 119 + // For the more mathematically inclined, this file implements first-order 120 120 + // "jets". A 1st order jet is an element of the ring 121 121 + // 122 122 + // T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2 123 123 + // 124 124 + // which essentially means that each jet consists of a "scalar" value 'a' from T 125 125 + // and a 1st order perturbation vector 'v' of length N: 126 126 + // 127 127 + // x = a + \sum_i v[i] t_i 128 128 + // 129 129 + // A shorthand is to write an element as x = a + u, where u is the perturbation. 130 130 + // Then, the main point about the arithmetic of jets is that the product of 131 131 + // perturbations is zero: 132 132 + // 133 133 + // (a + u) * (b + v) = ab + av + bu + uv 134 134 + // = ab + (av + bu) + 0 135 135 + // 136 136 + // which is what operator* implements below. Addition is simpler: 137 137 + // 138 138 + // (a + u) + (b + v) = (a + b) + (u + v). 139 139 + // 140 140 + // The only remaining question is how to evaluate the function of a jet, for 141 141 + // which we use the chain rule: 142 142 + // 143 143 + // f(a + u) = f(a) + f'(a) u 144 144 + // 145 145 + // where f'(a) is the (scalar) derivative of f at a. 146 146 + // 147 147 + // By pushing these things through sufficiently and suitably templated 148 148 + // functions, we can do automatic differentiation. Just be sure to turn on 149 149 + // function inlining and common-subexpression elimination, or it will be very 150 150 + // slow! 151 151 + // 152 152 + // WARNING: Most Ceres users should not directly include this file or know the 153 153 + // details of how jets work. Instead the suggested method for automatic 154 154 + // derivatives is to use autodiff_cost_function.h, which is a wrapper around 155 155 + // both jets.h and autodiff.h to make taking derivatives of cost functions for 156 156 + // use in Ceres easier. 157 157 + 158 158 + #pragma once 159 159 + 160 160 + #include <cmath> 161 161 + #include <complex> 162 162 + #include <iosfwd> 163 163 + #include <iostream> // NOLINT 164 164 + #include <limits> 165 165 + #include <numeric> 166 166 + #include <string> 167 167 + #include <type_traits> 168 168 + 169 169 + #include "Eigen/Core" 170 170 + #include "tinyceres/internal/jet_traits.hpp" 171 171 + 172 172 + // Taken from port.h 173 173 + #define CERES_PREVENT_MACRO_SUBSTITUTION // Yes, it's empty 174 174 + 175 175 + #include "tinyceres/jet_fwd.hpp" 176 176 + 177 177 + // Here we provide partial specializations of std::common_type for the Jet class 178 178 + // to allow determining a Jet type with a common underlying arithmetic type. 179 179 + // Such an arithmetic type can be either a scalar or an another Jet. An example 180 180 + // for a common type, say, between a float and a Jet<double, N> is a Jet<double, 181 181 + // N> (i.e., std::common_type_t<float, ceres::Jet<double, N>> and 182 182 + // ceres::Jet<double, N> refer to the same type.) 183 183 + // 184 184 + // The partial specialization are also used for determining compatible types by 185 185 + // means of SFINAE and thus allow such types to be expressed as operands of 186 186 + // logical comparison operators. Missing (partial) specialization of 187 187 + // std::common_type for a particular (custom) type will therefore disable the 188 188 + // use of comparison operators defined by Ceres. 189 189 + // 190 190 + // Since these partial specializations are used as SFINAE constraints, they 191 191 + // enable standard promotion rules between various scalar types and consequently 192 192 + // their use in comparison against a Jet without providing implicit 193 193 + // conversions from a scalar, such as an int, to a Jet (see the implementation 194 194 + // of logical comparison operators below). 195 195 + 196 196 + template <typename T, int N, typename U> 197 197 + struct std::common_type<T, ceres::Jet<U, N>> { 198 198 + using type = ceres::Jet<common_type_t<T, U>, N>; 199 199 + }; 200 200 + 201 201 + template <typename T, int N, typename U> 202 202 + struct std::common_type<ceres::Jet<T, N>, U> { 203 203 + using type = ceres::Jet<common_type_t<T, U>, N>; 204 204 + }; 205 205 + 206 206 + template <typename T, int N, typename U> 207 207 + struct std::common_type<ceres::Jet<T, N>, ceres::Jet<U, N>> { 208 208 + using type = ceres::Jet<common_type_t<T, U>, N>; 209 209 + }; 210 210 + 211 211 + namespace ceres { 212 212 + 213 213 + template <typename T, int N> 214 214 + struct Jet { 215 215 + enum { DIMENSION = N }; 216 216 + using Scalar = T; 217 217 + 218 218 + // Default-construct "a" because otherwise this can lead to false errors about 219 219 + // uninitialized uses when other classes relying on default constructed T 220 220 + // (where T is a Jet<T, N>). This usually only happens in opt mode. Note that 221 221 + // the C++ standard mandates that e.g. default constructed doubles are 222 222 + // initialized to 0.0; see sections 8.5 of the C++03 standard. 223 223 + Jet() : a() { v.setConstant(Scalar()); } 224 224 + 225 225 + // Constructor from scalar: a + 0. 226 226 + explicit Jet(const T& value) { 227 227 + a = value; 228 228 + v.setConstant(Scalar()); 229 229 + } 230 230 + 231 231 + // Constructor from scalar plus variable: a + t_i. 232 232 + Jet(const T& value, int k) { 233 233 + a = value; 234 234 + v.setConstant(Scalar()); 235 235 + v[k] = T(1.0); 236 236 + } 237 237 + 238 238 + // Constructor from scalar and vector part 239 239 + // The use of Eigen::DenseBase allows Eigen expressions 240 240 + // to be passed in without being fully evaluated until 241 241 + // they are assigned to v 242 242 + template <typename Derived> 243 243 + EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase<Derived>& v) 244 244 + : a(a), v(v) {} 245 245 + 246 246 + // Compound operators 247 247 + Jet<T, N>& operator+=(const Jet<T, N>& y) { 248 248 + *this = *this + y; 249 249 + return *this; 250 250 + } 251 251 + 252 252 + Jet<T, N>& operator-=(const Jet<T, N>& y) { 253 253 + *this = *this - y; 254 254 + return *this; 255 255 + } 256 256 + 257 257 + Jet<T, N>& operator*=(const Jet<T, N>& y) { 258 258 + *this = *this * y; 259 259 + return *this; 260 260 + } 261 261 + 262 262 + Jet<T, N>& operator/=(const Jet<T, N>& y) { 263 263 + *this = *this / y; 264 264 + return *this; 265 265 + } 266 266 + 267 267 + // Compound with scalar operators. 268 268 + Jet<T, N>& operator+=(const T& s) { 269 269 + *this = *this + s; 270 270 + return *this; 271 271 + } 272 272 + 273 273 + Jet<T, N>& operator-=(const T& s) { 274 274 + *this = *this - s; 275 275 + return *this; 276 276 + } 277 277 + 278 278 + Jet<T, N>& operator*=(const T& s) { 279 279 + *this = *this * s; 280 280 + return *this; 281 281 + } 282 282 + 283 283 + Jet<T, N>& operator/=(const T& s) { 284 284 + *this = *this / s; 285 285 + return *this; 286 286 + } 287 287 + 288 288 + // The scalar part. 289 289 + T a; 290 290 + 291 291 + // The infinitesimal part. 292 292 + Eigen::Matrix<T, N, 1> v; 293 293 + 294 294 + // This struct needs to have an Eigen aligned operator new as it contains 295 295 + // fixed-size Eigen types. 296 296 + EIGEN_MAKE_ALIGNED_OPERATOR_NEW 297 297 + }; 298 298 + 299 299 + // Unary + 300 300 + template <typename T, int N> 301 301 + inline Jet<T, N> const& operator+(const Jet<T, N>& f) { 302 302 + return f; 303 303 + } 304 304 + 305 305 + // TODO(keir): Try adding __attribute__((always_inline)) to these functions to 306 306 + // see if it causes a performance increase. 307 307 + 308 308 + // Unary - 309 309 + template <typename T, int N> 310 310 + inline Jet<T, N> operator-(const Jet<T, N>& f) { 311 311 + return Jet<T, N>(-f.a, -f.v); 312 312 + } 313 313 + 314 314 + // Binary + 315 315 + template <typename T, int N> 316 316 + inline Jet<T, N> operator+(const Jet<T, N>& f, const Jet<T, N>& g) { 317 317 + return Jet<T, N>(f.a + g.a, f.v + g.v); 318 318 + } 319 319 + 320 320 + // Binary + with a scalar: x + s 321 321 + template <typename T, int N> 322 322 + inline Jet<T, N> operator+(const Jet<T, N>& f, T s) { 323 323 + return Jet<T, N>(f.a + s, f.v); 324 324 + } 325 325 + 326 326 + // Binary + with a scalar: s + x 327 327 + template <typename T, int N> 328 328 + inline Jet<T, N> operator+(T s, const Jet<T, N>& f) { 329 329 + return Jet<T, N>(f.a + s, f.v); 330 330 + } 331 331 + 332 332 + // Binary - 333 333 + template <typename T, int N> 334 334 + inline Jet<T, N> operator-(const Jet<T, N>& f, const Jet<T, N>& g) { 335 335 + return Jet<T, N>(f.a - g.a, f.v - g.v); 336 336 + } 337 337 + 338 338 + // Binary - with a scalar: x - s 339 339 + template <typename T, int N> 340 340 + inline Jet<T, N> operator-(const Jet<T, N>& f, T s) { 341 341 + return Jet<T, N>(f.a - s, f.v); 342 342 + } 343 343 + 344 344 + // Binary - with a scalar: s - x 345 345 + template <typename T, int N> 346 346 + inline Jet<T, N> operator-(T s, const Jet<T, N>& f) { 347 347 + return Jet<T, N>(s - f.a, -f.v); 348 348 + } 349 349 + 350 350 + // Binary * 351 351 + template <typename T, int N> 352 352 + inline Jet<T, N> operator*(const Jet<T, N>& f, const Jet<T, N>& g) { 353 353 + return Jet<T, N>(f.a * g.a, f.a * g.v + f.v * g.a); 354 354 + } 355 355 + 356 356 + // Binary * with a scalar: x * s 357 357 + template <typename T, int N> 358 358 + inline Jet<T, N> operator*(const Jet<T, N>& f, T s) { 359 359 + return Jet<T, N>(f.a * s, f.v * s); 360 360 + } 361 361 + 362 362 + // Binary * with a scalar: s * x 363 363 + template <typename T, int N> 364 364 + inline Jet<T, N> operator*(T s, const Jet<T, N>& f) { 365 365 + return Jet<T, N>(f.a * s, f.v * s); 366 366 + } 367 367 + 368 368 + // Binary / 369 369 + template <typename T, int N> 370 370 + inline Jet<T, N> operator/(const Jet<T, N>& f, const Jet<T, N>& g) { 371 371 + // This uses: 372 372 + // 373 373 + // a + u (a + u)(b - v) (a + u)(b - v) 374 374 + // ----- = -------------- = -------------- 375 375 + // b + v (b + v)(b - v) b^2 376 376 + // 377 377 + // which holds because v*v = 0. 378 378 + const T g_a_inverse = T(1.0) / g.a; 379 379 + const T f_a_by_g_a = f.a * g_a_inverse; 380 380 + return Jet<T, N>(f_a_by_g_a, (f.v - f_a_by_g_a * g.v) * g_a_inverse); 381 381 + } 382 382 + 383 383 + // Binary / with a scalar: s / x 384 384 + template <typename T, int N> 385 385 + inline Jet<T, N> operator/(T s, const Jet<T, N>& g) { 386 386 + const T minus_s_g_a_inverse2 = -s / (g.a * g.a); 387 387 + return Jet<T, N>(s / g.a, g.v * minus_s_g_a_inverse2); 388 388 + } 389 389 + 390 390 + // Binary / with a scalar: x / s 391 391 + template <typename T, int N> 392 392 + inline Jet<T, N> operator/(const Jet<T, N>& f, T s) { 393 393 + const T s_inverse = T(1.0) / s; 394 394 + return Jet<T, N>(f.a * s_inverse, f.v * s_inverse); 395 395 + } 396 396 + 397 397 + // Binary comparison operators for both scalars and jets. At least one of the 398 398 + // operands must be a Jet. Promotable scalars (e.g., int, float, double etc.) 399 399 + // can appear on either side of the operator. std::common_type_t is used as an 400 400 + // SFINAE constraint to selectively enable compatible operand types. This allows 401 401 + // comparison, for instance, against int literals without implicit conversion. 402 402 + // In case the Jet arithmetic type is a Jet itself, a recursive expansion of Jet 403 403 + // value is performed. 404 404 + #define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ 405 405 + template <typename Lhs, \ 406 406 + typename Rhs, \ 407 407 + std::enable_if_t<PromotableJetOperands_v<Lhs, Rhs>>* = nullptr> \ 408 408 + constexpr bool operator op(const Lhs& f, const Rhs& g) noexcept( \ 409 409 + noexcept(internal::AsScalar(f) op internal::AsScalar(g))) { \ 410 410 + using internal::AsScalar; \ 411 411 + return AsScalar(f) op AsScalar(g); \ 412 412 + } 413 413 + CERES_DEFINE_JET_COMPARISON_OPERATOR(<) // NOLINT 414 414 + CERES_DEFINE_JET_COMPARISON_OPERATOR(<=) // NOLINT 415 415 + CERES_DEFINE_JET_COMPARISON_OPERATOR(>) // NOLINT 416 416 + CERES_DEFINE_JET_COMPARISON_OPERATOR(>=) // NOLINT 417 417 + CERES_DEFINE_JET_COMPARISON_OPERATOR(==) // NOLINT 418 418 + CERES_DEFINE_JET_COMPARISON_OPERATOR(!=) // NOLINT 419 419 + #undef CERES_DEFINE_JET_COMPARISON_OPERATOR 420 420 + 421 421 + // Pull some functions from namespace std. 422 422 + // 423 423 + // This is necessary because we want to use the same name (e.g. 'sqrt') for 424 424 + // double-valued and Jet-valued functions, but we are not allowed to put 425 425 + // Jet-valued functions inside namespace std. 426 426 + using std::abs; 427 427 + using std::acos; 428 428 + using std::asin; 429 429 + using std::atan; 430 430 + using std::atan2; 431 431 + using std::cbrt; 432 432 + using std::ceil; 433 433 + using std::copysign; 434 434 + using std::cos; 435 435 + using std::cosh; 436 436 + using std::erf; 437 437 + using std::erfc; 438 438 + using std::exp; 439 439 + using std::exp2; 440 440 + using std::expm1; 441 441 + using std::fdim; 442 442 + using std::floor; 443 443 + using std::fma; 444 444 + using std::fmax; 445 445 + using std::fmin; 446 446 + using std::fpclassify; 447 447 + using std::hypot; 448 448 + using std::isfinite; 449 449 + using std::isinf; 450 450 + using std::isnan; 451 451 + using std::isnormal; 452 452 + using std::log; 453 453 + using std::log10; 454 454 + using std::log1p; 455 455 + using std::log2; 456 456 + using std::norm; 457 457 + using std::pow; 458 458 + using std::signbit; 459 459 + using std::sin; 460 460 + using std::sinh; 461 461 + using std::sqrt; 462 462 + using std::tan; 463 463 + using std::tanh; 464 464 + 465 465 + // MSVC (up to 1930) defines quiet comparison functions as template functions 466 466 + // which causes compilation errors due to ambiguity in the template parameter 467 467 + // type resolution for using declarations in the ceres namespace. Workaround the 468 468 + // issue by defining specific overload and bypass MSVC standard library 469 469 + // definitions. 470 470 + #if defined(_MSC_VER) 471 471 + inline bool isgreater(double lhs, 472 472 + double rhs) noexcept(noexcept(std::isgreater(lhs, rhs))) { 473 473 + return std::isgreater(lhs, rhs); 474 474 + } 475 475 + inline bool isless(double lhs, 476 476 + double rhs) noexcept(noexcept(std::isless(lhs, rhs))) { 477 477 + return std::isless(lhs, rhs); 478 478 + } 479 479 + inline bool islessequal(double lhs, 480 480 + double rhs) noexcept(noexcept(std::islessequal(lhs, 481 481 + rhs))) { 482 482 + return std::islessequal(lhs, rhs); 483 483 + } 484 484 + inline bool isgreaterequal(double lhs, double rhs) noexcept( 485 485 + noexcept(std::isgreaterequal(lhs, rhs))) { 486 486 + return std::isgreaterequal(lhs, rhs); 487 487 + } 488 488 + inline bool islessgreater(double lhs, double rhs) noexcept( 489 489 + noexcept(std::islessgreater(lhs, rhs))) { 490 490 + return std::islessgreater(lhs, rhs); 491 491 + } 492 492 + inline bool isunordered(double lhs, 493 493 + double rhs) noexcept(noexcept(std::isunordered(lhs, 494 494 + rhs))) { 495 495 + return std::isunordered(lhs, rhs); 496 496 + } 497 497 + #else 498 498 + using std::isgreater; 499 499 + using std::isgreaterequal; 500 500 + using std::isless; 501 501 + using std::islessequal; 502 502 + using std::islessgreater; 503 503 + using std::isunordered; 504 504 + #endif 505 505 + 506 506 + #ifdef CERES_HAS_CPP20 507 507 + using std::lerp; 508 508 + using std::midpoint; 509 509 + #endif // defined(CERES_HAS_CPP20) 510 510 + 511 511 + 512 512 + // In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule. 513 513 + 514 514 + // abs(x + h) ~= abs(x) + sgn(x)h 515 515 + template <typename T, int N> 516 516 + inline Jet<T, N> abs(const Jet<T, N>& f) { 517 517 + return Jet<T, N>(abs(f.a), copysign(T(1), f.a) * f.v); 518 518 + } 519 519 + 520 520 + // copysign(a, b) composes a float with the magnitude of a and the sign of b. 521 521 + // Therefore, the function can be formally defined as 522 522 + // 523 523 + // copysign(a, b) = sgn(b)|a| 524 524 + // 525 525 + // where 526 526 + // 527 527 + // d/dx |x| = sgn(x) 528 528 + // d/dx sgn(x) = 2δ(x) 529 529 + // 530 530 + // sgn(x) being the signum function. Differentiating copysign(a, b) with respect 531 531 + // to a and b gives: 532 532 + // 533 533 + // d/da sgn(b)|a| = sgn(a) sgn(b) 534 534 + // d/db sgn(b)|a| = 2|a|δ(b) 535 535 + // 536 536 + // with the dual representation given by 537 537 + // 538 538 + // copysign(a + da, b + db) ~= sgn(b)|a| + (sgn(a)sgn(b) da + 2|a|δ(b) db) 539 539 + // 540 540 + // where δ(b) is the Dirac delta function. 541 541 + template <typename T, int N> 542 542 + inline Jet<T, N> copysign(const Jet<T, N>& f, const Jet<T, N> g) { 543 543 + // The Dirac delta function δ(b) is undefined at b=0 (here it's 544 544 + // infinite) and 0 everywhere else. 545 545 + T d = fpclassify(g) == FP_ZERO ? std::numeric_limits<T>::infinity() : T(0); 546 546 + T sa = copysign(T(1), f.a); // sgn(a) 547 547 + T sb = copysign(T(1), g.a); // sgn(b) 548 548 + // The second part of the infinitesimal is 2|a|δ(b) which is either infinity 549 549 + // or 0 unless a or any of the values of the b infinitesimal are 0. In the 550 550 + // latter case, the corresponding values become NaNs (multiplying 0 by 551 551 + // infinity gives NaN). We drop the constant factor 2 since it does not change 552 552 + // the result (its values will still be either 0, infinity or NaN). 553 553 + return Jet<T, N>(copysign(f.a, g.a), sa * sb * f.v + abs(f.a) * d * g.v); 554 554 + } 555 555 + 556 556 + // log(a + h) ~= log(a) + h / a 557 557 + template <typename T, int N> 558 558 + inline Jet<T, N> log(const Jet<T, N>& f) { 559 559 + const T a_inverse = T(1.0) / f.a; 560 560 + return Jet<T, N>(log(f.a), f.v * a_inverse); 561 561 + } 562 562 + 563 563 + // log10(a + h) ~= log10(a) + h / (a log(10)) 564 564 + template <typename T, int N> 565 565 + inline Jet<T, N> log10(const Jet<T, N>& f) { 566 566 + // Most compilers will expand log(10) to a constant. 567 567 + const T a_inverse = T(1.0) / (f.a * log(T(10.0))); 568 568 + return Jet<T, N>(log10(f.a), f.v * a_inverse); 569 569 + } 570 570 + 571 571 + // log1p(a + h) ~= log1p(a) + h / (1 + a) 572 572 + template <typename T, int N> 573 573 + inline Jet<T, N> log1p(const Jet<T, N>& f) { 574 574 + const T a_inverse = T(1.0) / (T(1.0) + f.a); 575 575 + return Jet<T, N>(log1p(f.a), f.v * a_inverse); 576 576 + } 577 577 + 578 578 + // exp(a + h) ~= exp(a) + exp(a) h 579 579 + template <typename T, int N> 580 580 + inline Jet<T, N> exp(const Jet<T, N>& f) { 581 581 + const T tmp = exp(f.a); 582 582 + return Jet<T, N>(tmp, tmp * f.v); 583 583 + } 584 584 + 585 585 + // expm1(a + h) ~= expm1(a) + exp(a) h 586 586 + template <typename T, int N> 587 587 + inline Jet<T, N> expm1(const Jet<T, N>& f) { 588 588 + const T tmp = expm1(f.a); 589 589 + const T expa = tmp + T(1.0); // exp(a) = expm1(a) + 1 590 590 + return Jet<T, N>(tmp, expa * f.v); 591 591 + } 592 592 + 593 593 + // sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a)) 594 594 + template <typename T, int N> 595 595 + inline Jet<T, N> sqrt(const Jet<T, N>& f) { 596 596 + const T tmp = sqrt(f.a); 597 597 + const T two_a_inverse = T(1.0) / (T(2.0) * tmp); 598 598 + return Jet<T, N>(tmp, f.v * two_a_inverse); 599 599 + } 600 600 + 601 601 + // cos(a + h) ~= cos(a) - sin(a) h 602 602 + template <typename T, int N> 603 603 + inline Jet<T, N> cos(const Jet<T, N>& f) { 604 604 + return Jet<T, N>(cos(f.a), -sin(f.a) * f.v); 605 605 + } 606 606 + 607 607 + // acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h 608 608 + template <typename T, int N> 609 609 + inline Jet<T, N> acos(const Jet<T, N>& f) { 610 610 + const T tmp = -T(1.0) / sqrt(T(1.0) - f.a * f.a); 611 611 + return Jet<T, N>(acos(f.a), tmp * f.v); 612 612 + } 613 613 + 614 614 + // sin(a + h) ~= sin(a) + cos(a) h 615 615 + template <typename T, int N> 616 616 + inline Jet<T, N> sin(const Jet<T, N>& f) { 617 617 + return Jet<T, N>(sin(f.a), cos(f.a) * f.v); 618 618 + } 619 619 + 620 620 + // asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h 621 621 + template <typename T, int N> 622 622 + inline Jet<T, N> asin(const Jet<T, N>& f) { 623 623 + const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a); 624 624 + return Jet<T, N>(asin(f.a), tmp * f.v); 625 625 + } 626 626 + 627 627 + // tan(a + h) ~= tan(a) + (1 + tan(a)^2) h 628 628 + template <typename T, int N> 629 629 + inline Jet<T, N> tan(const Jet<T, N>& f) { 630 630 + const T tan_a = tan(f.a); 631 631 + const T tmp = T(1.0) + tan_a * tan_a; 632 632 + return Jet<T, N>(tan_a, tmp * f.v); 633 633 + } 634 634 + 635 635 + // atan(a + h) ~= atan(a) + 1 / (1 + a^2) h 636 636 + template <typename T, int N> 637 637 + inline Jet<T, N> atan(const Jet<T, N>& f) { 638 638 + const T tmp = T(1.0) / (T(1.0) + f.a * f.a); 639 639 + return Jet<T, N>(atan(f.a), tmp * f.v); 640 640 + } 641 641 + 642 642 + // sinh(a + h) ~= sinh(a) + cosh(a) h 643 643 + template <typename T, int N> 644 644 + inline Jet<T, N> sinh(const Jet<T, N>& f) { 645 645 + return Jet<T, N>(sinh(f.a), cosh(f.a) * f.v); 646 646 + } 647 647 + 648 648 + // cosh(a + h) ~= cosh(a) + sinh(a) h 649 649 + template <typename T, int N> 650 650 + inline Jet<T, N> cosh(const Jet<T, N>& f) { 651 651 + return Jet<T, N>(cosh(f.a), sinh(f.a) * f.v); 652 652 + } 653 653 + 654 654 + // tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h 655 655 + template <typename T, int N> 656 656 + inline Jet<T, N> tanh(const Jet<T, N>& f) { 657 657 + const T tanh_a = tanh(f.a); 658 658 + const T tmp = T(1.0) - tanh_a * tanh_a; 659 659 + return Jet<T, N>(tanh_a, tmp * f.v); 660 660 + } 661 661 + 662 662 + // The floor function should be used with extreme care as this operation will 663 663 + // result in a zero derivative which provides no information to the solver. 664 664 + // 665 665 + // floor(a + h) ~= floor(a) + 0 666 666 + template <typename T, int N> 667 667 + inline Jet<T, N> floor(const Jet<T, N>& f) { 668 668 + return Jet<T, N>(floor(f.a)); 669 669 + } 670 670 + 671 671 + // The ceil function should be used with extreme care as this operation will 672 672 + // result in a zero derivative which provides no information to the solver. 673 673 + // 674 674 + // ceil(a + h) ~= ceil(a) + 0 675 675 + template <typename T, int N> 676 676 + inline Jet<T, N> ceil(const Jet<T, N>& f) { 677 677 + return Jet<T, N>(ceil(f.a)); 678 678 + } 679 679 + 680 680 + // Some new additions to C++11: 681 681 + 682 682 + // cbrt(a + h) ~= cbrt(a) + h / (3 a ^ (2/3)) 683 683 + template <typename T, int N> 684 684 + inline Jet<T, N> cbrt(const Jet<T, N>& f) { 685 685 + const T derivative = T(1.0) / (T(3.0) * cbrt(f.a * f.a)); 686 686 + return Jet<T, N>(cbrt(f.a), f.v * derivative); 687 687 + } 688 688 + 689 689 + // exp2(x + h) = 2^(x+h) ~= 2^x + h*2^x*log(2) 690 690 + template <typename T, int N> 691 691 + inline Jet<T, N> exp2(const Jet<T, N>& f) { 692 692 + const T tmp = exp2(f.a); 693 693 + const T derivative = tmp * log(T(2)); 694 694 + return Jet<T, N>(tmp, f.v * derivative); 695 695 + } 696 696 + 697 697 + // log2(x + h) ~= log2(x) + h / (x * log(2)) 698 698 + template <typename T, int N> 699 699 + inline Jet<T, N> log2(const Jet<T, N>& f) { 700 700 + const T derivative = T(1.0) / (f.a * log(T(2))); 701 701 + return Jet<T, N>(log2(f.a), f.v * derivative); 702 702 + } 703 703 + 704 704 + // Like sqrt(x^2 + y^2), 705 705 + // but acts to prevent underflow/overflow for small/large x/y. 706 706 + // Note that the function is non-smooth at x=y=0, 707 707 + // so the derivative is undefined there. 708 708 + template <typename T, int N> 709 709 + inline Jet<T, N> hypot(const Jet<T, N>& x, const Jet<T, N>& y) { 710 710 + // d/da sqrt(a) = 0.5 / sqrt(a) 711 711 + // d/dx x^2 + y^2 = 2x 712 712 + // So by the chain rule: 713 713 + // d/dx sqrt(x^2 + y^2) = 0.5 / sqrt(x^2 + y^2) * 2x = x / sqrt(x^2 + y^2) 714 714 + // d/dy sqrt(x^2 + y^2) = y / sqrt(x^2 + y^2) 715 715 + const T tmp = hypot(x.a, y.a); 716 716 + return Jet<T, N>(tmp, x.a / tmp * x.v + y.a / tmp * y.v); 717 717 + } 718 718 + 719 719 + // Like sqrt(x^2 + y^2 + z^2), 720 720 + // but acts to prevent underflow/overflow for small/large x/y/z. 721 721 + // Note that the function is non-smooth at x=y=z=0, 722 722 + // so the derivative is undefined there. 723 723 + template <typename T, int N> 724 724 + inline Jet<T, N> hypot(const Jet<T, N>& x, 725 725 + const Jet<T, N>& y, 726 726 + const Jet<T, N>& z) { 727 727 + // d/da sqrt(a) = 0.5 / sqrt(a) 728 728 + // d/dx x^2 + y^2 + z^2 = 2x 729 729 + // So by the chain rule: 730 730 + // d/dx sqrt(x^2 + y^2 + z^2) 731 731 + // = 0.5 / sqrt(x^2 + y^2 + z^2) * 2x 732 732 + // = x / sqrt(x^2 + y^2 + z^2) 733 733 + // d/dy sqrt(x^2 + y^2 + z^2) = y / sqrt(x^2 + y^2 + z^2) 734 734 + // d/dz sqrt(x^2 + y^2 + z^2) = z / sqrt(x^2 + y^2 + z^2) 735 735 + const T tmp = hypot(x.a, y.a, z.a); 736 736 + return Jet<T, N>(tmp, x.a / tmp * x.v + y.a / tmp * y.v + z.a / tmp * z.v); 737 737 + } 738 738 + 739 739 + // Like x * y + z but rounded only once. 740 740 + template <typename T, int N> 741 741 + inline Jet<T, N> fma(const Jet<T, N>& x, 742 742 + const Jet<T, N>& y, 743 743 + const Jet<T, N>& z) { 744 744 + // d/dx fma(x, y, z) = y 745 745 + // d/dy fma(x, y, z) = x 746 746 + // d/dz fma(x, y, z) = 1 747 747 + return Jet<T, N>(fma(x.a, y.a, z.a), y.a * x.v + x.a * y.v + z.v); 748 748 + } 749 749 + 750 750 + // Returns the larger of the two arguments. NaNs are treated as missing data. 751 751 + // 752 752 + // NOTE: This function is NOT subject to any of the error conditions specified 753 753 + // in `math_errhandling`. 754 754 + template <typename Lhs, 755 755 + typename Rhs, 756 756 + std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr> 757 757 + inline decltype(auto) fmax(const Lhs& f, const Rhs& g) { 758 758 + using J = std::common_type_t<Lhs, Rhs>; 759 759 + return (isnan(g) || isgreater(f, g)) ? J{f} : J{g}; 760 760 + } 761 761 + 762 762 + // Returns the smaller of the two arguments. NaNs are treated as missing data. 763 763 + // 764 764 + // NOTE: This function is NOT subject to any of the error conditions specified 765 765 + // in `math_errhandling`. 766 766 + template <typename Lhs, 767 767 + typename Rhs, 768 768 + std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr> 769 769 + inline decltype(auto) fmin(const Lhs& f, const Rhs& g) { 770 770 + using J = std::common_type_t<Lhs, Rhs>; 771 771 + return (isnan(f) || isless(g, f)) ? J{g} : J{f}; 772 772 + } 773 773 + 774 774 + // Returns the positive difference (f - g) of two arguments and zero if f <= g. 775 775 + // If at least one argument is NaN, a NaN is return. 776 776 + // 777 777 + // NOTE At least one of the argument types must be a Jet, the other one can be a 778 778 + // scalar. In case both arguments are Jets, their dimensionality must match. 779 779 + template <typename Lhs, 780 780 + typename Rhs, 781 781 + std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr> 782 782 + inline decltype(auto) fdim(const Lhs& f, const Rhs& g) { 783 783 + using J = std::common_type_t<Lhs, Rhs>; 784 784 + if (isnan(f) || isnan(g)) { 785 785 + return std::numeric_limits<J>::quiet_NaN(); 786 786 + } 787 787 + return isgreater(f, g) ? J{f - g} : J{}; 788 788 + } 789 789 + 790 790 + // erf is defined as an integral that cannot be expressed analytically 791 791 + // however, the derivative is trivial to compute 792 792 + // erf(x + h) = erf(x) + h * 2*exp(-x^2)/sqrt(pi) 793 793 + template <typename T, int N> 794 794 + inline Jet<T, N> erf(const Jet<T, N>& x) { 795 795 + // We evaluate the constant as follows: 796 796 + // 2 / sqrt(pi) = 1 / sqrt(atan(1.)) 797 797 + // On POSIX systems it is defined as M_2_SQRTPI, but this is not 798 798 + // portable and the type may not be T. The above expression 799 799 + // evaluates to full precision with IEEE arithmetic and, since it's 800 800 + // constant, the compiler can generate exactly the same code. gcc 801 801 + // does so even at -O0. 802 802 + return Jet<T, N>(erf(x.a), x.v * exp(-x.a * x.a) * (T(1) / sqrt(atan(T(1))))); 803 803 + } 804 804 + 805 805 + // erfc(x) = 1-erf(x) 806 806 + // erfc(x + h) = erfc(x) + h * (-2*exp(-x^2)/sqrt(pi)) 807 807 + template <typename T, int N> 808 808 + inline Jet<T, N> erfc(const Jet<T, N>& x) { 809 809 + // See in erf() above for the evaluation of the constant in the derivative. 810 810 + return Jet<T, N>(erfc(x.a), 811 811 + -x.v * exp(-x.a * x.a) * (T(1) / sqrt(atan(T(1))))); 812 812 + } 813 813 + 814 814 + // Bessel functions of the first kind with integer order equal to 0, 1, n. 815 815 + // 816 816 + // Microsoft has deprecated the j[0,1,n]() POSIX Bessel functions in favour of 817 817 + // _j[0,1,n](). Where available on MSVC, use _j[0,1,n]() to avoid deprecated 818 818 + // function errors in client code (the specific warning is suppressed when 819 819 + // Ceres itself is built). 820 820 + inline double BesselJ0(double x) { 821 821 + #if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS) 822 822 + return _j0(x); 823 823 + #else 824 824 + return j0(x); 825 825 + #endif 826 826 + } 827 827 + inline double BesselJ1(double x) { 828 828 + #if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS) 829 829 + return _j1(x); 830 830 + #else 831 831 + return j1(x); 832 832 + #endif 833 833 + } 834 834 + inline double BesselJn(int n, double x) { 835 835 + #if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS) 836 836 + return _jn(n, x); 837 837 + #else 838 838 + return jn(n, x); 839 839 + #endif 840 840 + } 841 841 + 842 842 + // For the formulae of the derivatives of the Bessel functions see the book: 843 843 + // Olver, Lozier, Boisvert, Clark, NIST Handbook of Mathematical Functions, 844 844 + // Cambridge University Press 2010. 845 845 + // 846 846 + // Formulae are also available at http://dlmf.nist.gov 847 847 + 848 848 + // See formula http://dlmf.nist.gov/10.6#E3 849 849 + // j0(a + h) ~= j0(a) - j1(a) h 850 850 + template <typename T, int N> 851 851 + inline Jet<T, N> BesselJ0(const Jet<T, N>& f) { 852 852 + return Jet<T, N>(BesselJ0(f.a), -BesselJ1(f.a) * f.v); 853 853 + } 854 854 + 855 855 + // See formula http://dlmf.nist.gov/10.6#E1 856 856 + // j1(a + h) ~= j1(a) + 0.5 ( j0(a) - j2(a) ) h 857 857 + template <typename T, int N> 858 858 + inline Jet<T, N> BesselJ1(const Jet<T, N>& f) { 859 859 + return Jet<T, N>(BesselJ1(f.a), 860 860 + T(0.5) * (BesselJ0(f.a) - BesselJn(2, f.a)) * f.v); 861 861 + } 862 862 + 863 863 + // See formula http://dlmf.nist.gov/10.6#E1 864 864 + // j_n(a + h) ~= j_n(a) + 0.5 ( j_{n-1}(a) - j_{n+1}(a) ) h 865 865 + template <typename T, int N> 866 866 + inline Jet<T, N> BesselJn(int n, const Jet<T, N>& f) { 867 867 + return Jet<T, N>( 868 868 + BesselJn(n, f.a), 869 869 + T(0.5) * (BesselJn(n - 1, f.a) - BesselJn(n + 1, f.a)) * f.v); 870 870 + } 871 871 + 872 872 + // Classification and comparison functionality referencing only the scalar part 873 873 + // of a Jet. To classify the derivatives (e.g., for sanity checks), the dual 874 874 + // part should be referenced explicitly. For instance, to check whether the 875 875 + // derivatives of a Jet 'f' are reasonable, one can use 876 876 + // 877 877 + // isfinite(f.v.array()).all() 878 878 + // !isnan(f.v.array()).any() 879 879 + // 880 880 + // etc., depending on the desired semantics. 881 881 + // 882 882 + // NOTE: Floating-point classification and comparison functions and operators 883 883 + // should be used with care as no derivatives can be propagated by such 884 884 + // functions directly but only by expressions resulting from corresponding 885 885 + // conditional statements. At the same time, conditional statements can possibly 886 886 + // introduce a discontinuity in the cost function making it impossible to 887 887 + // evaluate its derivative and thus the optimization problem intractable. 888 888 + 889 889 + // Determines whether the scalar part of the Jet is finite. 890 890 + template <typename T, int N> 891 891 + inline bool isfinite(const Jet<T, N>& f) { 892 892 + return isfinite(f.a); 893 893 + } 894 894 + 895 895 + // Determines whether the scalar part of the Jet is infinite. 896 896 + template <typename T, int N> 897 897 + inline bool isinf(const Jet<T, N>& f) { 898 898 + return isinf(f.a); 899 899 + } 900 900 + 901 901 + // Determines whether the scalar part of the Jet is NaN. 902 902 + template <typename T, int N> 903 903 + inline bool isnan(const Jet<T, N>& f) { 904 904 + return isnan(f.a); 905 905 + } 906 906 + 907 907 + // Determines whether the scalar part of the Jet is neither zero, subnormal, 908 908 + // infinite, nor NaN. 909 909 + template <typename T, int N> 910 910 + inline bool isnormal(const Jet<T, N>& f) { 911 911 + return isnormal(f.a); 912 912 + } 913 913 + 914 914 + // Determines whether the scalar part of the Jet f is less than the scalar 915 915 + // part of g. 916 916 + // 917 917 + // NOTE: This function does NOT set any floating-point exceptions. 918 918 + template <typename Lhs, 919 919 + typename Rhs, 920 920 + std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr> 921 921 + inline bool isless(const Lhs& f, const Rhs& g) { 922 922 + using internal::AsScalar; 923 923 + return isless(AsScalar(f), AsScalar(g)); 924 924 + } 925 925 + 926 926 + // Determines whether the scalar part of the Jet f is greater than the scalar 927 927 + // part of g. 928 928 + // 929 929 + // NOTE: This function does NOT set any floating-point exceptions. 930 930 + template <typename Lhs, 931 931 + typename Rhs, 932 932 + std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr> 933 933 + inline bool isgreater(const Lhs& f, const Rhs& g) { 934 934 + using internal::AsScalar; 935 935 + return isgreater(AsScalar(f), AsScalar(g)); 936 936 + } 937 937 + 938 938 + // Determines whether the scalar part of the Jet f is less than or equal to the 939 939 + // scalar part of g. 940 940 + // 941 941 + // NOTE: This function does NOT set any floating-point exceptions. 942 942 + template <typename Lhs, 943 943 + typename Rhs, 944 944 + std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr> 945 945 + inline bool islessequal(const Lhs& f, const Rhs& g) { 946 946 + using internal::AsScalar; 947 947 + return islessequal(AsScalar(f), AsScalar(g)); 948 948 + } 949 949 + 950 950 + // Determines whether the scalar part of the Jet f is less than or greater than 951 951 + // (f < g || f > g) the scalar part of g. 952 952 + // 953 953 + // NOTE: This function does NOT set any floating-point exceptions. 954 954 + template <typename Lhs, 955 955 + typename Rhs, 956 956 + std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr> 957 957 + inline bool islessgreater(const Lhs& f, const Rhs& g) { 958 958 + using internal::AsScalar; 959 959 + return islessgreater(AsScalar(f), AsScalar(g)); 960 960 + } 961 961 + 962 962 + // Determines whether the scalar part of the Jet f is greater than or equal to 963 963 + // the scalar part of g. 964 964 + // 965 965 + // NOTE: This function does NOT set any floating-point exceptions. 966 966 + template <typename Lhs, 967 967 + typename Rhs, 968 968 + std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr> 969 969 + inline bool isgreaterequal(const Lhs& f, const Rhs& g) { 970 970 + using internal::AsScalar; 971 971 + return isgreaterequal(AsScalar(f), AsScalar(g)); 972 972 + } 973 973 + 974 974 + // Determines if either of the scalar parts of the arguments are NaN and 975 975 + // thus cannot be ordered with respect to each other. 976 976 + template <typename Lhs, 977 977 + typename Rhs, 978 978 + std::enable_if_t<CompatibleJetOperands_v<Lhs, Rhs>>* = nullptr> 979 979 + inline bool isunordered(const Lhs& f, const Rhs& g) { 980 980 + using internal::AsScalar; 981 981 + return isunordered(AsScalar(f), AsScalar(g)); 982 982 + } 983 983 + 984 984 + // Categorize scalar part as zero, subnormal, normal, infinite, NaN, or 985 985 + // implementation-defined. 986 986 + template <typename T, int N> 987 987 + inline int fpclassify(const Jet<T, N>& f) { 988 988 + return fpclassify(f.a); 989 989 + } 990 990 + 991 991 + // Determines whether the scalar part of the argument is negative. 992 992 + template <typename T, int N> 993 993 + inline bool signbit(const Jet<T, N>& f) { 994 994 + return signbit(f.a); 995 995 + } 996 996 + 997 997 + 998 998 + 999 999 + #ifdef CERES_HAS_CPP20 1000 1000 + // Computes the linear interpolation a + t(b - a) between a and b at the value 1001 1001 + // t. For arguments outside of the range 0 <= t <= 1, the values are 1002 1002 + // extrapolated. 1003 1003 + // 1004 1004 + // Differentiating lerp(a, b, t) with respect to a, b, and t gives: 1005 1005 + // 1006 1006 + // d/da lerp(a, b, t) = 1 - t 1007 1007 + // d/db lerp(a, b, t) = t 1008 1008 + // d/dt lerp(a, b, t) = b - a 1009 1009 + // 1010 1010 + // with the dual representation given by 1011 1011 + // 1012 1012 + // lerp(a + da, b + db, t + dt) 1013 1013 + // ~= lerp(a, b, t) + (1 - t) da + t db + (b - a) dt . 1014 1014 + template <typename T, int N> 1015 1015 + inline Jet<T, N> lerp(const Jet<T, N>& a, 1016 1016 + const Jet<T, N>& b, 1017 1017 + const Jet<T, N>& t) { 1018 1018 + return Jet<T, N>{lerp(a.a, b.a, t.a), 1019 1019 + (T(1) - t.a) * a.v + t.a * b.v + (b.a - a.a) * t.v}; 1020 1020 + } 1021 1021 + 1022 1022 + // Computes the midpoint a + (b - a) / 2. 1023 1023 + // 1024 1024 + // Differentiating midpoint(a, b) with respect to a and b gives: 1025 1025 + // 1026 1026 + // d/da midpoint(a, b) = 1/2 1027 1027 + // d/db midpoint(a, b) = 1/2 1028 1028 + // 1029 1029 + // with the dual representation given by 1030 1030 + // 1031 1031 + // midpoint(a + da, b + db) ~= midpoint(a, b) + (da + db) / 2 . 1032 1032 + template <typename T, int N> 1033 1033 + inline Jet<T, N> midpoint(const Jet<T, N>& a, const Jet<T, N>& b) { 1034 1034 + Jet<T, N> result{midpoint(a.a, b.a)}; 1035 1035 + // To avoid overflow in the differential, compute 1036 1036 + // (da + db) / 2 using midpoint. 1037 1037 + for (int i = 0; i < N; ++i) { 1038 1038 + result.v[i] = midpoint(a.v[i], b.v[i]); 1039 1039 + } 1040 1040 + return result; 1041 1041 + } 1042 1042 + #endif // defined(CERES_HAS_CPP20) 1043 1043 + 1044 1044 + // atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2) 1045 1045 + // 1046 1046 + // In words: the rate of change of theta is 1/r times the rate of 1047 1047 + // change of (x, y) in the positive angular direction. 1048 1048 + template <typename T, int N> 1049 1049 + inline Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) { 1050 1050 + // Note order of arguments: 1051 1051 + // 1052 1052 + // f = a + da 1053 1053 + // g = b + db 1054 1054 + 1055 1055 + T const tmp = T(1.0) / (f.a * f.a + g.a * g.a); 1056 1056 + return Jet<T, N>(atan2(g.a, f.a), tmp * (-g.a * f.v + f.a * g.v)); 1057 1057 + } 1058 1058 + 1059 1059 + // Computes the square x^2 of a real number x (not the Euclidean L^2 norm as 1060 1060 + // the name might suggest). 1061 1061 + // 1062 1062 + // NOTE: While std::norm is primarily intended for computing the squared 1063 1063 + // magnitude of a std::complex<> number, the current Jet implementation does not 1064 1064 + // support mixing a scalar T in its real part and std::complex<T> and in the 1065 1065 + // infinitesimal. Mixed Jet support is necessary for the type decay from 1066 1066 + // std::complex<T> to T (the squared magnitude of a complex number is always 1067 1067 + // real) performed by std::norm. 1068 1068 + // 1069 1069 + // norm(x + h) ~= norm(x) + 2x h 1070 1070 + template <typename T, int N> 1071 1071 + inline Jet<T, N> norm(const Jet<T, N>& f) { 1072 1072 + return Jet<T, N>(norm(f.a), T(2) * f.a * f.v); 1073 1073 + } 1074 1074 + 1075 1075 + // pow -- base is a differentiable function, exponent is a constant. 1076 1076 + // (a+da)^p ~= a^p + p*a^(p-1) da 1077 1077 + template <typename T, int N> 1078 1078 + inline Jet<T, N> pow(const Jet<T, N>& f, double g) { 1079 1079 + T const tmp = g * pow(f.a, g - T(1.0)); 1080 1080 + return Jet<T, N>(pow(f.a, g), tmp * f.v); 1081 1081 + } 1082 1082 + 1083 1083 + // pow -- base is a constant, exponent is a differentiable function. 1084 1084 + // We have various special cases, see the comment for pow(Jet, Jet) for 1085 1085 + // analysis: 1086 1086 + // 1087 1087 + // 1. For f > 0 we have: (f)^(g + dg) ~= f^g + f^g log(f) dg 1088 1088 + // 1089 1089 + // 2. For f == 0 and g > 0 we have: (f)^(g + dg) ~= f^g 1090 1090 + // 1091 1091 + // 3. For f < 0 and integer g we have: (f)^(g + dg) ~= f^g but if dg 1092 1092 + // != 0, the derivatives are not defined and we return NaN. 1093 1093 + 1094 1094 + template <typename T, int N> 1095 1095 + inline Jet<T, N> pow(T f, const Jet<T, N>& g) { 1096 1096 + Jet<T, N> result; 1097 1097 + 1098 1098 + if (fpclassify(f) == FP_ZERO && g > 0) { 1099 1099 + // Handle case 2. 1100 1100 + result = Jet<T, N>(T(0.0)); 1101 1101 + } else { 1102 1102 + if (f < 0 && g == floor(g.a)) { // Handle case 3. 1103 1103 + result = Jet<T, N>(pow(f, g.a)); 1104 1104 + for (int i = 0; i < N; i++) { 1105 1105 + if (fpclassify(g.v[i]) != FP_ZERO) { 1106 1106 + // Return a NaN when g.v != 0. 1107 1107 + result.v[i] = std::numeric_limits<T>::quiet_NaN(); 1108 1108 + } 1109 1109 + } 1110 1110 + } else { 1111 1111 + // Handle case 1. 1112 1112 + T const tmp = pow(f, g.a); 1113 1113 + result = Jet<T, N>(tmp, log(f) * tmp * g.v); 1114 1114 + } 1115 1115 + } 1116 1116 + 1117 1117 + return result; 1118 1118 + } 1119 1119 + 1120 1120 + // pow -- both base and exponent are differentiable functions. This has a 1121 1121 + // variety of special cases that require careful handling. 1122 1122 + // 1123 1123 + // 1. For f > 0: 1124 1124 + // (f + df)^(g + dg) ~= f^g + f^(g - 1) * (g * df + f * log(f) * dg) 1125 1125 + // The numerical evaluation of f * log(f) for f > 0 is well behaved, even for 1126 1126 + // extremely small values (e.g. 1e-99). 1127 1127 + // 1128 1128 + // 2. For f == 0 and g > 1: (f + df)^(g + dg) ~= 0 1129 1129 + // This cases is needed because log(0) can not be evaluated in the f > 0 1130 1130 + // expression. However the function f*log(f) is well behaved around f == 0 1131 1131 + // and its limit as f-->0 is zero. 1132 1132 + // 1133 1133 + // 3. For f == 0 and g == 1: (f + df)^(g + dg) ~= 0 + df 1134 1134 + // 1135 1135 + // 4. For f == 0 and 0 < g < 1: The value is finite but the derivatives are not. 1136 1136 + // 1137 1137 + // 5. For f == 0 and g < 0: The value and derivatives of f^g are not finite. 1138 1138 + // 1139 1139 + // 6. For f == 0 and g == 0: The C standard incorrectly defines 0^0 to be 1 1140 1140 + // "because there are applications that can exploit this definition". We 1141 1141 + // (arbitrarily) decree that derivatives here will be nonfinite, since that 1142 1142 + // is consistent with the behavior for f == 0, g < 0 and 0 < g < 1. 1143 1143 + // Practically any definition could have been justified because mathematical 1144 1144 + // consistency has been lost at this point. 1145 1145 + // 1146 1146 + // 7. For f < 0, g integer, dg == 0: (f + df)^(g + dg) ~= f^g + g * f^(g - 1) df 1147 1147 + // This is equivalent to the case where f is a differentiable function and g 1148 1148 + // is a constant (to first order). 1149 1149 + // 1150 1150 + // 8. For f < 0, g integer, dg != 0: The value is finite but the derivatives are 1151 1151 + // not, because any change in the value of g moves us away from the point 1152 1152 + // with a real-valued answer into the region with complex-valued answers. 1153 1153 + // 1154 1154 + // 9. For f < 0, g noninteger: The value and derivatives of f^g are not finite. 1155 1155 + 1156 1156 + template <typename T, int N> 1157 1157 + inline Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) { 1158 1158 + Jet<T, N> result; 1159 1159 + 1160 1160 + if (fpclassify(f) == FP_ZERO && g >= 1) { 1161 1161 + // Handle cases 2 and 3. 1162 1162 + if (g > 1) { 1163 1163 + result = Jet<T, N>(T(0.0)); 1164 1164 + } else { 1165 1165 + result = f; 1166 1166 + } 1167 1167 + 1168 1168 + } else { 1169 1169 + if (f < 0 && g == floor(g.a)) { 1170 1170 + // Handle cases 7 and 8. 1171 1171 + T const tmp = g.a * pow(f.a, g.a - T(1.0)); 1172 1172 + result = Jet<T, N>(pow(f.a, g.a), tmp * f.v); 1173 1173 + for (int i = 0; i < N; i++) { 1174 1174 + if (fpclassify(g.v[i]) != FP_ZERO) { 1175 1175 + // Return a NaN when g.v != 0. 1176 1176 + result.v[i] = T(std::numeric_limits<double>::quiet_NaN()); 1177 1177 + } 1178 1178 + } 1179 1179 + } else { 1180 1180 + // Handle the remaining cases. For cases 4,5,6,9 we allow the log() 1181 1181 + // function to generate -HUGE_VAL or NaN, since those cases result in a 1182 1182 + // nonfinite derivative. 1183 1183 + T const tmp1 = pow(f.a, g.a); 1184 1184 + T const tmp2 = g.a * pow(f.a, g.a - T(1.0)); 1185 1185 + T const tmp3 = tmp1 * log(f.a); 1186 1186 + result = Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v); 1187 1187 + } 1188 1188 + } 1189 1189 + 1190 1190 + return result; 1191 1191 + } 1192 1192 + 1193 1193 + // Note: This has to be in the ceres namespace for argument dependent lookup to 1194 1194 + // function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with 1195 1195 + // strange compile errors. 1196 1196 + template <typename T, int N> 1197 1197 + inline std::ostream& operator<<(std::ostream& s, const Jet<T, N>& z) { 1198 1198 + s << "[" << z.a << " ; "; 1199 1199 + for (int i = 0; i < N; ++i) { 1200 1200 + s << z.v[i]; 1201 1201 + if (i != N - 1) { 1202 1202 + s << ", "; 1203 1203 + } 1204 1204 + } 1205 1205 + s << "]"; 1206 1206 + return s; 1207 1207 + } 1208 1208 + } // namespace ceres 1209 1209 + 1210 1210 + namespace std { 1211 1211 + template <typename T, int N> 1212 1212 + struct numeric_limits<ceres::Jet<T, N>> { 1213 1213 + static constexpr bool is_specialized = true; 1214 1214 + static constexpr bool is_signed = std::numeric_limits<T>::is_signed; 1215 1215 + static constexpr bool is_integer = std::numeric_limits<T>::is_integer; 1216 1216 + static constexpr bool is_exact = std::numeric_limits<T>::is_exact; 1217 1217 + static constexpr bool has_infinity = std::numeric_limits<T>::has_infinity; 1218 1218 + static constexpr bool has_quiet_NaN = std::numeric_limits<T>::has_quiet_NaN; 1219 1219 + static constexpr bool has_signaling_NaN = 1220 1220 + std::numeric_limits<T>::has_signaling_NaN; 1221 1221 + static constexpr bool is_iec559 = std::numeric_limits<T>::is_iec559; 1222 1222 + static constexpr bool is_bounded = std::numeric_limits<T>::is_bounded; 1223 1223 + static constexpr bool is_modulo = std::numeric_limits<T>::is_modulo; 1224 1224 + 1225 1225 + static constexpr std::float_denorm_style has_denorm = 1226 1226 + std::numeric_limits<T>::has_denorm; 1227 1227 + static constexpr std::float_round_style round_style = 1228 1228 + std::numeric_limits<T>::round_style; 1229 1229 + 1230 1230 + static constexpr int digits = std::numeric_limits<T>::digits; 1231 1231 + static constexpr int digits10 = std::numeric_limits<T>::digits10; 1232 1232 + static constexpr int max_digits10 = std::numeric_limits<T>::max_digits10; 1233 1233 + static constexpr int radix = std::numeric_limits<T>::radix; 1234 1234 + static constexpr int min_exponent = std::numeric_limits<T>::min_exponent; 1235 1235 + static constexpr int min_exponent10 = std::numeric_limits<T>::max_exponent10; 1236 1236 + static constexpr int max_exponent = std::numeric_limits<T>::max_exponent; 1237 1237 + static constexpr int max_exponent10 = std::numeric_limits<T>::max_exponent10; 1238 1238 + static constexpr bool traps = std::numeric_limits<T>::traps; 1239 1239 + static constexpr bool tinyness_before = 1240 1240 + std::numeric_limits<T>::tinyness_before; 1241 1241 + 1242 1242 + static constexpr ceres::Jet<T, N> min 1243 1243 + CERES_PREVENT_MACRO_SUBSTITUTION() noexcept { 1244 1244 + return ceres::Jet<T, N>((std::numeric_limits<T>::min)()); 1245 1245 + } 1246 1246 + static constexpr ceres::Jet<T, N> lowest() noexcept { 1247 1247 + return ceres::Jet<T, N>(std::numeric_limits<T>::lowest()); 1248 1248 + } 1249 1249 + static constexpr ceres::Jet<T, N> epsilon() noexcept { 1250 1250 + return ceres::Jet<T, N>(std::numeric_limits<T>::epsilon()); 1251 1251 + } 1252 1252 + static constexpr ceres::Jet<T, N> round_error() noexcept { 1253 1253 + return ceres::Jet<T, N>(std::numeric_limits<T>::round_error()); 1254 1254 + } 1255 1255 + static constexpr ceres::Jet<T, N> infinity() noexcept { 1256 1256 + return ceres::Jet<T, N>(std::numeric_limits<T>::infinity()); 1257 1257 + } 1258 1258 + static constexpr ceres::Jet<T, N> quiet_NaN() noexcept { 1259 1259 + return ceres::Jet<T, N>(std::numeric_limits<T>::quiet_NaN()); 1260 1260 + } 1261 1261 + static constexpr ceres::Jet<T, N> signaling_NaN() noexcept { 1262 1262 + return ceres::Jet<T, N>(std::numeric_limits<T>::signaling_NaN()); 1263 1263 + } 1264 1264 + static constexpr ceres::Jet<T, N> denorm_min() noexcept { 1265 1265 + return ceres::Jet<T, N>(std::numeric_limits<T>::denorm_min()); 1266 1266 + } 1267 1267 + 1268 1268 + static constexpr ceres::Jet<T, N> max 1269 1269 + CERES_PREVENT_MACRO_SUBSTITUTION() noexcept { 1270 1270 + return ceres::Jet<T, N>((std::numeric_limits<T>::max)()); 1271 1271 + } 1272 1272 + }; 1273 1273 + 1274 1274 + } // namespace std 1275 1275 + 1276 1276 + namespace Eigen { 1277 1277 + 1278 1278 + // Creating a specialization of NumTraits enables placing Jet objects inside 1279 1279 + // Eigen arrays, getting all the goodness of Eigen combined with autodiff. 1280 1280 + template <typename T, int N> 1281 1281 + struct NumTraits<ceres::Jet<T, N>> { 1282 1282 + using Real = ceres::Jet<T, N>; 1283 1283 + using NonInteger = ceres::Jet<T, N>; 1284 1284 + using Nested = ceres::Jet<T, N>; 1285 1285 + using Literal = ceres::Jet<T, N>; 1286 1286 + 1287 1287 + static typename ceres::Jet<T, N> dummy_precision() { 1288 1288 + return ceres::Jet<T, N>(1e-12); 1289 1289 + } 1290 1290 + 1291 1291 + static inline Real epsilon() { 1292 1292 + return Real(std::numeric_limits<T>::epsilon()); 1293 1293 + } 1294 1294 + 1295 1295 + static inline int digits10() { return NumTraits<T>::digits10(); } 1296 1296 + 1297 1297 + enum { 1298 1298 + IsComplex = 0, 1299 1299 + IsInteger = 0, 1300 1300 + IsSigned, 1301 1301 + ReadCost = 1, 1302 1302 + AddCost = 1, 1303 1303 + // For Jet types, multiplication is more expensive than addition. 1304 1304 + MulCost = 3, 1305 1305 + HasFloatingPoint = 1, 1306 1306 + RequireInitialization = 1 1307 1307 + }; 1308 1308 + 1309 1309 + template <bool Vectorized> 1310 1310 + struct Div { 1311 1311 + enum { 1312 1312 + #if defined(EIGEN_VECTORIZE_AVX) 1313 1313 + AVX = true, 1314 1314 + #else 1315 1315 + AVX = false, 1316 1316 + #endif 1317 1317 + 1318 1318 + // Assuming that for Jets, division is as expensive as 1319 1319 + // multiplication. 1320 1320 + Cost = 3 1321 1321 + }; 1322 1322 + }; 1323 1323 + 1324 1324 + static inline Real highest() { return Real((std::numeric_limits<T>::max)()); } 1325 1325 + static inline Real lowest() { return Real(-(std::numeric_limits<T>::max)()); } 1326 1326 + }; 1327 1327 + 1328 1328 + // Specifying the return type of binary operations between Jets and scalar types 1329 1329 + // allows you to perform matrix/array operations with Eigen matrices and arrays 1330 1330 + // such as addition, subtraction, multiplication, and division where one Eigen 1331 1331 + // matrix/array is of type Jet and the other is a scalar type. This improves 1332 1332 + // performance by using the optimized scalar-to-Jet binary operations but 1333 1333 + // is only available on Eigen versions >= 3.3 1334 1334 + template <typename BinaryOp, typename T, int N> 1335 1335 + struct ScalarBinaryOpTraits<ceres::Jet<T, N>, T, BinaryOp> { 1336 1336 + using ReturnType = ceres::Jet<T, N>; 1337 1337 + }; 1338 1338 + template <typename BinaryOp, typename T, int N> 1339 1339 + struct ScalarBinaryOpTraits<T, ceres::Jet<T, N>, BinaryOp> { 1340 1340 + using ReturnType = ceres::Jet<T, N>; 1341 1341 + }; 1342 1342 + 1343 1343 + } // namespace Eigen

+42

src/external/tinyceres/include/tinyceres/jet_fwd.hpp

··· 1 1 + // SPDX-License-Identifier: BSD-3-Clause 2 2 + // Ceres Solver - A fast non-linear least squares minimizer 3 3 + // Copyright 2022 Google Inc. All rights reserved. 4 4 + // http://ceres-solver.org/ 5 5 + // 6 6 + // Redistribution and use in source and binary forms, with or without 7 7 + // modification, are permitted provided that the following conditions are met: 8 8 + // 9 9 + // * Redistributions of source code must retain the above copyright notice, 10 10 + // this list of conditions and the following disclaimer. 11 11 + // * Redistributions in binary form must reproduce the above copyright notice, 12 12 + // this list of conditions and the following disclaimer in the documentation 13 13 + // and/or other materials provided with the distribution. 14 14 + // * Neither the name of Google Inc. nor the names of its contributors may be 15 15 + // used to endorse or promote products derived from this software without 16 16 + // specific prior written permission. 17 17 + // 18 18 + // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 19 + // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 20 + // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 21 + // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 22 + // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 23 + // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 24 + // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 25 + // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 26 + // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 27 + // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 28 + // POSSIBILITY OF SUCH DAMAGE. 29 29 + // 30 30 + // Author: sergiu.deitsch@gmail.com (Sergiu Deitsch) 31 31 + // 32 32 + 33 33 + #pragma once 34 34 + 35 35 + namespace ceres { 36 36 + 37 37 + // Jet forward declaration necessary for the following partial specialization of 38 38 + // std::common_type and type traits. 39 39 + template <typename T, int N> 40 40 + struct Jet; 41 41 + 42 42 + } // namespace ceres

+401

src/external/tinyceres/include/tinyceres/tiny_solver.hpp

··· 1 1 + // SPDX-License-Identifier: BSD-3-Clause 2 2 + // Ceres Solver - A fast non-linear least squares minimizer 3 3 + // Copyright 2021 Google Inc. All rights reserved. 4 4 + // http://ceres-solver.org/ 5 5 + // 6 6 + // Redistribution and use in source and binary forms, with or without 7 7 + // modification, are permitted provided that the following conditions are met: 8 8 + // 9 9 + // * Redistributions of source code must retain the above copyright notice, 10 10 + // this list of conditions and the following disclaimer. 11 11 + // * Redistributions in binary form must reproduce the above copyright notice, 12 12 + // this list of conditions and the following disclaimer in the documentation 13 13 + // and/or other materials provided with the distribution. 14 14 + // * Neither the name of Google Inc. nor the names of its contributors may be 15 15 + // used to endorse or promote products derived from this software without 16 16 + // specific prior written permission. 17 17 + // 18 18 + // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 19 + // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 20 + // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 21 + // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 22 + // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 23 + // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 24 + // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 25 + // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 26 + // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 27 + // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 28 + // POSSIBILITY OF SUCH DAMAGE. 29 29 + // 30 30 + // Author: mierle@gmail.com (Keir Mierle) 31 31 + // 32 32 + // WARNING WARNING WARNING 33 33 + // WARNING WARNING WARNING Tiny solver is experimental and will change. 34 34 + // WARNING WARNING WARNING 35 35 + // 36 36 + // A tiny least squares solver using Levenberg-Marquardt, intended for solving 37 37 + // small dense problems with low latency and low overhead. The implementation 38 38 + // takes care to do all allocation up front, so that no memory is allocated 39 39 + // during solving. This is especially useful when solving many similar problems; 40 40 + // for example, inverse pixel distortion for every pixel on a grid. 41 41 + // 42 42 + // Note: This code has no dependencies beyond Eigen, including on other parts of 43 43 + // Ceres, so it is possible to take this file alone and put it in another 44 44 + // project without the rest of Ceres. 45 45 + // 46 46 + // Algorithm based off of: 47 47 + // 48 48 + // [1] K. Madsen, H. Nielsen, O. Tingleoff. 49 49 + // Methods for Non-linear Least Squares Problems. 50 50 + // http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf 51 51 + 52 52 + #ifndef CERES_PUBLIC_TINY_SOLVER_H_ 53 53 + #define CERES_PUBLIC_TINY_SOLVER_H_ 54 54 + 55 55 + #include <cassert> 56 56 + #include <cmath> 57 57 + 58 58 + #include "Eigen/Dense" 59 59 + 60 60 + namespace ceres { 61 61 + 62 62 + // To use tiny solver, create a class or struct that allows computing the cost 63 63 + // function (described below). This is similar to a ceres::CostFunction, but is 64 64 + // different to enable statically allocating all memory for the solver 65 65 + // (specifically, enum sizes). Key parts are the Scalar typedef, the enums to 66 66 + // describe problem sizes (needed to remove all heap allocations), and the 67 67 + // operator() overload to evaluate the cost and (optionally) jacobians. 68 68 + // 69 69 + // struct TinySolverCostFunctionTraits { 70 70 + // typedef double Scalar; 71 71 + // enum { 72 72 + // NUM_RESIDUALS = <int> OR Eigen::Dynamic, 73 73 + // NUM_PARAMETERS = <int> OR Eigen::Dynamic, 74 74 + // }; 75 75 + // bool operator()(const double* parameters, 76 76 + // double* residuals, 77 77 + // double* jacobian) const; 78 78 + // 79 79 + // int NumResiduals() const; -- Needed if NUM_RESIDUALS == Eigen::Dynamic. 80 80 + // int NumParameters() const; -- Needed if NUM_PARAMETERS == Eigen::Dynamic. 81 81 + // }; 82 82 + // 83 83 + // For operator(), the size of the objects is: 84 84 + // 85 85 + // double* parameters -- NUM_PARAMETERS or NumParameters() 86 86 + // double* residuals -- NUM_RESIDUALS or NumResiduals() 87 87 + // double* jacobian -- NUM_RESIDUALS * NUM_PARAMETERS in column-major format 88 88 + // (Eigen's default); or nullptr if no jacobian 89 89 + // requested. 90 90 + // 91 91 + // An example (fully statically sized): 92 92 + // 93 93 + // struct MyCostFunctionExample { 94 94 + // typedef double Scalar; 95 95 + // enum { 96 96 + // NUM_RESIDUALS = 2, 97 97 + // NUM_PARAMETERS = 3, 98 98 + // }; 99 99 + // bool operator()(const double* parameters, 100 100 + // double* residuals, 101 101 + // double* jacobian) const { 102 102 + // residuals[0] = x + 2*y + 4*z; 103 103 + // residuals[1] = y * z; 104 104 + // if (jacobian) { 105 105 + // jacobian[0 * 2 + 0] = 1; // First column (x). 106 106 + // jacobian[0 * 2 + 1] = 0; 107 107 + // 108 108 + // jacobian[1 * 2 + 0] = 2; // Second column (y). 109 109 + // jacobian[1 * 2 + 1] = z; 110 110 + // 111 111 + // jacobian[2 * 2 + 0] = 4; // Third column (z). 112 112 + // jacobian[2 * 2 + 1] = y; 113 113 + // } 114 114 + // return true; 115 115 + // } 116 116 + // }; 117 117 + // 118 118 + // The solver supports either statically or dynamically sized cost 119 119 + // functions. If the number of residuals is dynamic then the Function 120 120 + // must define: 121 121 + // 122 122 + // int NumResiduals() const; 123 123 + // 124 124 + // If the number of parameters is dynamic then the Function must 125 125 + // define: 126 126 + // 127 127 + // int NumParameters() const; 128 128 + // 129 129 + template <typename Function, 130 130 + typename LinearSolver = 131 131 + Eigen::LDLT<Eigen::Matrix<typename Function::Scalar, // 132 132 + Function::NUM_PARAMETERS, // 133 133 + Function::NUM_PARAMETERS>>> 134 134 + class TinySolver { 135 135 + public: 136 136 + // This class needs to have an Eigen aligned operator new as it contains 137 137 + // fixed-size Eigen types. 138 138 + EIGEN_MAKE_ALIGNED_OPERATOR_NEW 139 139 + 140 140 + enum { 141 141 + NUM_RESIDUALS = Function::NUM_RESIDUALS, 142 142 + NUM_PARAMETERS = Function::NUM_PARAMETERS 143 143 + }; 144 144 + using Scalar = typename Function::Scalar; 145 145 + using Parameters = typename Eigen::Matrix<Scalar, NUM_PARAMETERS, 1>; 146 146 + 147 147 + enum Status { 148 148 + // max_norm |J'(x) * f(x)| < gradient_tolerance 149 149 + GRADIENT_TOO_SMALL, 150 150 + // ||dx|| <= parameter_tolerance * (||x|| + parameter_tolerance) 151 151 + RELATIVE_STEP_SIZE_TOO_SMALL, 152 152 + // cost_threshold > ||f(x)||^2 / 2 153 153 + COST_TOO_SMALL, 154 154 + // num_iterations >= max_num_iterations 155 155 + HIT_MAX_ITERATIONS, 156 156 + // (new_cost - old_cost) < function_tolerance * old_cost 157 157 + COST_CHANGE_TOO_SMALL, 158 158 + 159 159 + // TODO(sameeragarwal): Deal with numerical failures. 160 160 + }; 161 161 + 162 162 + struct Options { 163 163 + int max_num_iterations = 50; 164 164 + 165 165 + // max_norm |J'(x) * f(x)| < gradient_tolerance 166 166 + Scalar gradient_tolerance = 1e-10; 167 167 + 168 168 + // ||dx|| <= parameter_tolerance * (||x|| + parameter_tolerance) 169 169 + Scalar parameter_tolerance = 1e-8; 170 170 + 171 171 + // (new_cost - old_cost) < function_tolerance * old_cost 172 172 + Scalar function_tolerance = 1e-6; 173 173 + 174 174 + // cost_threshold > ||f(x)||^2 / 2 175 175 + Scalar cost_threshold = std::numeric_limits<Scalar>::epsilon(); 176 176 + 177 177 + Scalar initial_trust_region_radius = 1e4; 178 178 + }; 179 179 + 180 180 + struct Summary { 181 181 + // 1/2 ||f(x_0)||^2 182 182 + Scalar initial_cost = -1; 183 183 + // 1/2 ||f(x)||^2 184 184 + Scalar final_cost = -1; 185 185 + // max_norm(J'f(x)) 186 186 + Scalar gradient_max_norm = -1; 187 187 + int iterations = -1; 188 188 + Status status = HIT_MAX_ITERATIONS; 189 189 + }; 190 190 + 191 191 + bool Update(const Function& function, const Parameters& x) { 192 192 + if (!function(x.data(), residuals_.data(), jacobian_.data())) { 193 193 + return false; 194 194 + } 195 195 + 196 196 + residuals_ = -residuals_; 197 197 + 198 198 + // On the first iteration, compute a diagonal (Jacobi) scaling 199 199 + // matrix, which we store as a vector. 200 200 + if (summary.iterations == 0) { 201 201 + // jacobi_scaling = 1 / (1 + diagonal(J'J)) 202 202 + // 203 203 + // 1 is added to the denominator to regularize small diagonal 204 204 + // entries. 205 205 + jacobi_scaling_ = 1.0 / (1.0 + jacobian_.colwise().norm().array()); 206 206 + } 207 207 + 208 208 + // This explicitly computes the normal equations, which is numerically 209 209 + // unstable. Nevertheless, it is often good enough and is fast. 210 210 + // 211 211 + // TODO(sameeragarwal): Refactor this to allow for DenseQR 212 212 + // factorization. 213 213 + jacobian_ = jacobian_ * jacobi_scaling_.asDiagonal(); 214 214 + jtj_ = jacobian_.transpose() * jacobian_; 215 215 + g_ = jacobian_.transpose() * residuals_; 216 216 + summary.gradient_max_norm = g_.array().abs().maxCoeff(); 217 217 + cost_ = residuals_.squaredNorm() / 2; 218 218 + return true; 219 219 + } 220 220 + 221 221 + const Summary& Solve(const Function& function, Parameters* x_and_min) { 222 222 + Initialize<NUM_RESIDUALS, NUM_PARAMETERS>(function); 223 223 + assert(x_and_min); 224 224 + Parameters& x = *x_and_min; 225 225 + summary = Summary(); 226 226 + summary.iterations = 0; 227 227 + 228 228 + // TODO(sameeragarwal): Deal with failure here. 229 229 + Update(function, x); 230 230 + summary.initial_cost = cost_; 231 231 + summary.final_cost = cost_; 232 232 + 233 233 + if (summary.gradient_max_norm < options.gradient_tolerance) { 234 234 + summary.status = GRADIENT_TOO_SMALL; 235 235 + return summary; 236 236 + } 237 237 + 238 238 + if (cost_ < options.cost_threshold) { 239 239 + summary.status = COST_TOO_SMALL; 240 240 + return summary; 241 241 + } 242 242 + 243 243 + Scalar u = 1.0 / options.initial_trust_region_radius; 244 244 + Scalar v = 2; 245 245 + 246 246 + for (summary.iterations = 1; 247 247 + summary.iterations < options.max_num_iterations; 248 248 + summary.iterations++) { 249 249 + jtj_regularized_ = jtj_; 250 250 + const Scalar min_diagonal = 1e-6; 251 251 + const Scalar max_diagonal = 1e32; 252 252 + for (int i = 0; i < lm_diagonal_.rows(); ++i) { 253 253 + lm_diagonal_[i] = std::sqrt( 254 254 + u * (std::min)((std::max)(jtj_(i, i), min_diagonal), max_diagonal)); 255 255 + jtj_regularized_(i, i) += lm_diagonal_[i] * lm_diagonal_[i]; 256 256 + } 257 257 + 258 258 + // TODO(sameeragarwal): Check for failure and deal with it. 259 259 + linear_solver_.compute(jtj_regularized_); 260 260 + lm_step_ = linear_solver_.solve(g_); 261 261 + dx_ = jacobi_scaling_.asDiagonal() * lm_step_; 262 262 + 263 263 + // Adding parameter_tolerance to x.norm() ensures that this 264 264 + // works if x is near zero. 265 265 + const Scalar parameter_tolerance = 266 266 + options.parameter_tolerance * 267 267 + (x.norm() + options.parameter_tolerance); 268 268 + if (dx_.norm() < parameter_tolerance) { 269 269 + summary.status = RELATIVE_STEP_SIZE_TOO_SMALL; 270 270 + break; 271 271 + } 272 272 + x_new_ = x + dx_; 273 273 + 274 274 + // TODO(keir): Add proper handling of errors from user eval of cost 275 275 + // functions. 276 276 + function(&x_new_[0], &f_x_new_[0], nullptr); 277 277 + 278 278 + const Scalar cost_change = (2 * cost_ - f_x_new_.squaredNorm()); 279 279 + // TODO(sameeragarwal): Better more numerically stable evaluation. 280 280 + const Scalar model_cost_change = lm_step_.dot(2 * g_ - jtj_ * lm_step_); 281 281 + 282 282 + // rho is the ratio of the actual reduction in error to the reduction 283 283 + // in error that would be obtained if the problem was linear. See [1] 284 284 + // for details. 285 285 + Scalar rho(cost_change / model_cost_change); 286 286 + if (rho > 0) { 287 287 + // Accept the Levenberg-Marquardt step because the linear 288 288 + // model fits well. 289 289 + x = x_new_; 290 290 + 291 291 + if (std::abs(cost_change) < options.function_tolerance) { 292 292 + cost_ = f_x_new_.squaredNorm() / 2; 293 293 + summary.status = COST_CHANGE_TOO_SMALL; 294 294 + break; 295 295 + } 296 296 + 297 297 + // TODO(sameeragarwal): Deal with failure. 298 298 + Update(function, x); 299 299 + if (summary.gradient_max_norm < options.gradient_tolerance) { 300 300 + summary.status = GRADIENT_TOO_SMALL; 301 301 + break; 302 302 + } 303 303 + 304 304 + if (cost_ < options.cost_threshold) { 305 305 + summary.status = COST_TOO_SMALL; 306 306 + break; 307 307 + } 308 308 + 309 309 + Scalar tmp = Scalar(2 * rho - 1); 310 310 + u = u * (std::max)(Scalar(1 / 3.), Scalar(1) - tmp * tmp * tmp); 311 311 + v = 2; 312 312 + 313 313 + } else { 314 314 + // Reject the update because either the normal equations failed to solve 315 315 + // or the local linear model was not good (rho < 0). 316 316 + 317 317 + // Additionally if the cost change is too small, then terminate. 318 318 + if (std::abs(cost_change) < options.function_tolerance) { 319 319 + // Terminate 320 320 + summary.status = COST_CHANGE_TOO_SMALL; 321 321 + break; 322 322 + } 323 323 + 324 324 + // Reduce the size of the trust region. 325 325 + u *= v; 326 326 + v *= 2; 327 327 + } 328 328 + } 329 329 + 330 330 + summary.final_cost = cost_; 331 331 + return summary; 332 332 + } 333 333 + 334 334 + Options options; 335 335 + Summary summary; 336 336 + 337 337 + private: 338 338 + // Preallocate everything, including temporary storage needed for solving the 339 339 + // linear system. This allows reusing the intermediate storage across solves. 340 340 + LinearSolver linear_solver_; 341 341 + Scalar cost_; 342 342 + Parameters dx_, x_new_, g_, jacobi_scaling_, lm_diagonal_, lm_step_; 343 343 + Eigen::Matrix<Scalar, NUM_RESIDUALS, 1> residuals_, f_x_new_; 344 344 + Eigen::Matrix<Scalar, NUM_RESIDUALS, NUM_PARAMETERS> jacobian_; 345 345 + Eigen::Matrix<Scalar, NUM_PARAMETERS, NUM_PARAMETERS> jtj_, jtj_regularized_; 346 346 + 347 347 + // The following definitions are needed for template metaprogramming. 348 348 + template <bool Condition, typename T> 349 349 + struct enable_if; 350 350 + 351 351 + template <typename T> 352 352 + struct enable_if<true, T> { 353 353 + using type = T; 354 354 + }; 355 355 + 356 356 + // The number of parameters and residuals are dynamically sized. 357 357 + template <int R, int P> 358 358 + typename enable_if<(R == Eigen::Dynamic && P == Eigen::Dynamic), void>::type 359 359 + Initialize(const Function& function) { 360 360 + Initialize(function.NumResiduals(), function.NumParameters()); 361 361 + } 362 362 + 363 363 + // The number of parameters is dynamically sized and the number of 364 364 + // residuals is statically sized. 365 365 + template <int R, int P> 366 366 + typename enable_if<(R == Eigen::Dynamic && P != Eigen::Dynamic), void>::type 367 367 + Initialize(const Function& function) { 368 368 + Initialize(function.NumResiduals(), P); 369 369 + } 370 370 + 371 371 + // The number of parameters is statically sized and the number of 372 372 + // residuals is dynamically sized. 373 373 + template <int R, int P> 374 374 + typename enable_if<(R != Eigen::Dynamic && P == Eigen::Dynamic), void>::type 375 375 + Initialize(const Function& function) { 376 376 + Initialize(R, function.NumParameters()); 377 377 + } 378 378 + 379 379 + // The number of parameters and residuals are statically sized. 380 380 + template <int R, int P> 381 381 + typename enable_if<(R != Eigen::Dynamic && P != Eigen::Dynamic), void>::type 382 382 + Initialize(const Function& /* function */) {} 383 383 + 384 384 + void Initialize(int num_residuals, int num_parameters) { 385 385 + dx_.resize(num_parameters); 386 386 + x_new_.resize(num_parameters); 387 387 + g_.resize(num_parameters); 388 388 + jacobi_scaling_.resize(num_parameters); 389 389 + lm_diagonal_.resize(num_parameters); 390 390 + lm_step_.resize(num_parameters); 391 391 + residuals_.resize(num_residuals); 392 392 + f_x_new_.resize(num_residuals); 393 393 + jacobian_.resize(num_residuals, num_parameters); 394 394 + jtj_.resize(num_parameters, num_parameters); 395 395 + jtj_regularized_.resize(num_parameters, num_parameters); 396 396 + } 397 397 + }; 398 398 + 399 399 + } // namespace ceres 400 400 + 401 401 + #endif // CERES_PUBLIC_TINY_SOLVER_H_

+209

src/external/tinyceres/include/tinyceres/tiny_solver_autodiff_function.hpp

··· 1 1 + // SPDX-License-Identifier: BSD-3-Clause 2 2 + // Ceres Solver - A fast non-linear least squares minimizer 3 3 + // Copyright 2019 Google Inc. All rights reserved. 4 4 + // http://ceres-solver.org/ 5 5 + // 6 6 + // Redistribution and use in source and binary forms, with or without 7 7 + // modification, are permitted provided that the following conditions are met: 8 8 + // 9 9 + // * Redistributions of source code must retain the above copyright notice, 10 10 + // this list of conditions and the following disclaimer. 11 11 + // * Redistributions in binary form must reproduce the above copyright notice, 12 12 + // this list of conditions and the following disclaimer in the documentation 13 13 + // and/or other materials provided with the distribution. 14 14 + // * Neither the name of Google Inc. nor the names of its contributors may be 15 15 + // used to endorse or promote products derived from this software without 16 16 + // specific prior written permission. 17 17 + // 18 18 + // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 19 + // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 20 + // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 21 + // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 22 + // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 23 + // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 24 + // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 25 + // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 26 + // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 27 + // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 28 + // POSSIBILITY OF SUCH DAMAGE. 29 29 + // 30 30 + // Author: mierle@gmail.com (Keir Mierle) 31 31 + // 32 32 + // WARNING WARNING WARNING 33 33 + // WARNING WARNING WARNING Tiny solver is experimental and will change. 34 34 + // WARNING WARNING WARNING 35 35 + 36 36 + #ifndef CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_ 37 37 + #define CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_ 38 38 + 39 39 + #include <memory> 40 40 + #include <type_traits> 41 41 + 42 42 + #include "Eigen/Core" 43 43 + #include "tinyceres/jet.hpp" 44 44 + 45 45 + //!@todo Really? 46 46 + const double kImpossibleValue = 1e302; 47 47 + 48 48 + namespace ceres { 49 49 + 50 50 + // An adapter around autodiff-style CostFunctors to enable easier use of 51 51 + // TinySolver. See the example below showing how to use it: 52 52 + // 53 53 + // // Example for cost functor with static residual size. 54 54 + // // Same as an autodiff cost functor, but taking only 1 parameter. 55 55 + // struct MyFunctor { 56 56 + // template<typename T> 57 57 + // bool operator()(const T* const parameters, T* residuals) const { 58 58 + // const T& x = parameters[0]; 59 59 + // const T& y = parameters[1]; 60 60 + // const T& z = parameters[2]; 61 61 + // residuals[0] = x + 2.*y + 4.*z; 62 62 + // residuals[1] = y * z; 63 63 + // return true; 64 64 + // } 65 65 + // }; 66 66 + // 67 67 + // typedef TinySolverAutoDiffFunction<MyFunctor, 2, 3> 68 68 + // AutoDiffFunction; 69 69 + // 70 70 + // MyFunctor my_functor; 71 71 + // AutoDiffFunction f(my_functor); 72 72 + // 73 73 + // Vec3 x = ...; 74 74 + // TinySolver<AutoDiffFunction> solver; 75 75 + // solver.Solve(f, &x); 76 76 + // 77 77 + // // Example for cost functor with dynamic residual size. 78 78 + // // NumResiduals() supplies dynamic size of residuals. 79 79 + // // Same functionality as in tiny_solver.h but with autodiff. 80 80 + // struct MyFunctorWithDynamicResiduals { 81 81 + // int NumResiduals() const { 82 82 + // return 2; 83 83 + // } 84 84 + // 85 85 + // template<typename T> 86 86 + // bool operator()(const T* const parameters, T* residuals) const { 87 87 + // const T& x = parameters[0]; 88 88 + // const T& y = parameters[1]; 89 89 + // const T& z = parameters[2]; 90 90 + // residuals[0] = x + static_cast<T>(2.)*y + static_cast<T>(4.)*z; 91 91 + // residuals[1] = y * z; 92 92 + // return true; 93 93 + // } 94 94 + // }; 95 95 + // 96 96 + // typedef TinySolverAutoDiffFunction<MyFunctorWithDynamicResiduals, 97 97 + // Eigen::Dynamic, 98 98 + // 3> 99 99 + // AutoDiffFunctionWithDynamicResiduals; 100 100 + // 101 101 + // MyFunctorWithDynamicResiduals my_functor_dyn; 102 102 + // AutoDiffFunctionWithDynamicResiduals f(my_functor_dyn); 103 103 + // 104 104 + // Vec3 x = ...; 105 105 + // TinySolver<AutoDiffFunctionWithDynamicResiduals> solver; 106 106 + // solver.Solve(f, &x); 107 107 + // 108 108 + // WARNING: The cost function adapter is not thread safe. 109 109 + template <typename CostFunctor, 110 110 + int kNumResiduals, 111 111 + int kNumParameters, 112 112 + typename T = double> 113 113 + class TinySolverAutoDiffFunction { 114 114 + public: 115 115 + // This class needs to have an Eigen aligned operator new as it contains 116 116 + // as a member a Jet type, which itself has a fixed-size Eigen type as member. 117 117 + EIGEN_MAKE_ALIGNED_OPERATOR_NEW 118 118 + 119 119 + explicit TinySolverAutoDiffFunction(const CostFunctor& cost_functor) 120 120 + : cost_functor_(cost_functor) { 121 121 + Initialize<kNumResiduals>(cost_functor); 122 122 + } 123 123 + 124 124 + using Scalar = T; 125 125 + enum { 126 126 + NUM_PARAMETERS = kNumParameters, 127 127 + NUM_RESIDUALS = kNumResiduals, 128 128 + }; 129 129 + 130 130 + // This is similar to AutoDifferentiate(), but since there is only one 131 131 + // parameter block it is easier to inline to avoid overhead. 132 132 + bool operator()(const T* parameters, T* residuals, T* jacobian) const { 133 133 + if (jacobian == nullptr) { 134 134 + // No jacobian requested, so just directly call the cost function with 135 135 + // doubles, skipping jets and derivatives. 136 136 + return cost_functor_(parameters, residuals); 137 137 + } 138 138 + // Initialize the input jets with passed parameters. 139 139 + for (int i = 0; i < kNumParameters; ++i) { 140 140 + jet_parameters_[i].a = parameters[i]; // Scalar part. 141 141 + jet_parameters_[i].v.setZero(); // Derivative part. 142 142 + jet_parameters_[i].v[i] = T(1.0); 143 143 + } 144 144 + 145 145 + // Initialize the output jets such that we can detect user errors. 146 146 + for (int i = 0; i < num_residuals_; ++i) { 147 147 + jet_residuals_[i].a = kImpossibleValue; 148 148 + jet_residuals_[i].v.setConstant(kImpossibleValue); 149 149 + } 150 150 + 151 151 + // Execute the cost function, but with jets to find the derivative. 152 152 + if (!cost_functor_(jet_parameters_, jet_residuals_.data())) { 153 153 + return false; 154 154 + } 155 155 + 156 156 + // Copy the jacobian out of the derivative part of the residual jets. 157 157 + Eigen::Map<Eigen::Matrix<T, kNumResiduals, kNumParameters>> jacobian_matrix( 158 158 + jacobian, num_residuals_, kNumParameters); 159 159 + for (int r = 0; r < num_residuals_; ++r) { 160 160 + residuals[r] = jet_residuals_[r].a; 161 161 + // Note that while this looks like a fast vectorized write, in practice it 162 162 + // unfortunately thrashes the cache since the writes to the column-major 163 163 + // jacobian are strided (e.g. rows are non-contiguous). 164 164 + jacobian_matrix.row(r) = jet_residuals_[r].v; 165 165 + } 166 166 + return true; 167 167 + } 168 168 + 169 169 + int NumResiduals() const { 170 170 + return num_residuals_; // Set by Initialize. 171 171 + } 172 172 + 173 173 + private: 174 174 + const CostFunctor& cost_functor_; 175 175 + 176 176 + // The number of residuals at runtime. 177 177 + // This will be overridden if NUM_RESIDUALS == Eigen::Dynamic. 178 178 + int num_residuals_ = kNumResiduals; 179 179 + 180 180 + // To evaluate the cost function with jets, temporary storage is needed. These 181 181 + // are the buffers that are used during evaluation; parameters for the input, 182 182 + // and jet_residuals_ are where the final cost and derivatives end up. 183 183 + // 184 184 + // Since this buffer is used for evaluation, the adapter is not thread safe. 185 185 + using JetType = Jet<T, kNumParameters>; 186 186 + mutable JetType jet_parameters_[kNumParameters]; 187 187 + // Eigen::Matrix serves as static or dynamic container. 188 188 + mutable Eigen::Matrix<JetType, kNumResiduals, 1> jet_residuals_; 189 189 + 190 190 + // The number of residuals is dynamically sized and the number of 191 191 + // parameters is statically sized. 192 192 + template <int R> 193 193 + typename std::enable_if<(R == Eigen::Dynamic), void>::type Initialize( 194 194 + const CostFunctor& function) { 195 195 + jet_residuals_.resize(function.NumResiduals()); 196 196 + num_residuals_ = function.NumResiduals(); 197 197 + } 198 198 + 199 199 + // The number of parameters and residuals are statically sized. 200 200 + template <int R> 201 201 + typename std::enable_if<(R != Eigen::Dynamic), void>::type Initialize( 202 202 + const CostFunctor& /* function */) { 203 203 + num_residuals_ = kNumResiduals; 204 204 + } 205 205 + }; 206 206 + 207 207 + } // namespace ceres 208 208 + 209 209 + #endif // CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_