diff --git a/CHANGELOG.md b/CHANGELOG.md index ecb1f0a..464e8a0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,13 +2,16 @@ ## Unreleased ### New features +- New `dynamic` module for heap-allocated, dynamically-composable `Proxy` expressions - New documentation built with Sphinx ### Backward-incompatible API changes - C++14 is now required ### Fixes and minor changes -- Fixed finding googletest source package on Ubuntu bionic +- Fix finding googletest source package on Ubuntu bionic +- Fix (trivial) reverse-mode AD on a single leaf +- Move numerical Jacobian evaluator into `core` module ## [0.3.0](https://github.com/wavelab/wave_geometry/compare/0.2.0...0.3.0) (2018-08-19) ### New features diff --git a/benchmarks/CMakeLists.txt b/benchmarks/CMakeLists.txt index 7a11cd4..855d86a 100644 --- a/benchmarks/CMakeLists.txt +++ b/benchmarks/CMakeLists.txt @@ -1,7 +1,6 @@ find_package(benchmark REQUIRED) set(CMAKE_VERBOSE_MAKEFILE ON) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") wave_geometry_add_benchmark(util_cross_matrix_bench util_cross_matrix_bench.cpp) wave_geometry_add_benchmark(util_identity_bench util_identity_bench.cpp) diff --git a/benchmarks/bechmark_helpers.hpp b/benchmarks/bechmark_helpers.hpp index 5cc501d..ddd2337 100644 --- a/benchmarks/bechmark_helpers.hpp +++ b/benchmarks/bechmark_helpers.hpp @@ -26,9 +26,10 @@ /** Return a vector of random objects of type T, using setRandom() */ template std::vector> randomMatrices(int N) { - std::vector> v(N); + std::vector> v; + v.reserve(N); for (auto i = N; i--;) { - v[i] = T::Random(); + v.push_back(T::Random()); } return v; } diff --git a/benchmarks/rotate_chain/CMakeLists.txt b/benchmarks/rotate_chain/CMakeLists.txt index 245461b..7434d89 100644 --- a/benchmarks/rotate_chain/CMakeLists.txt +++ b/benchmarks/rotate_chain/CMakeLists.txt @@ -3,14 +3,30 @@ # # Each file tests a different implementation (ceres autodiff, hand, wave) for different N. +find_package(GTSAM QUIET) +if(GTSAM_FOUND) + add_executable(rotate_chain_gtsam_bench rotate_chain_gtsam_bench.cpp) + target_link_libraries(rotate_chain_gtsam_bench gtsam benchmark::benchmark GTest::GTest) + set_target_properties(rotate_chain_gtsam_bench PROPERTIES + RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/benchmarks) + target_include_directories(rotate_chain_gtsam_bench PRIVATE ${PROJECT_SOURCE_DIR}/include ${GTSAM_INCLUDE_DIR}) +endif() + # Use C++17 for structured bindings # Rotate_chain uses C++17 -list (FIND CMAKE_CXX_KNOWN_FEATURES "cxx_std_17" index) +list (FIND CMAKE_CXX_COMPILE_FEATURES "cxx_std_17" index) if (${index} GREATER -1) set(CMAKE_CXX_STANDARD 17) else() - message(STATUS "Not configuring rotate_chain benchmarks because C++17 is not known to this version of CMake") + message(STATUS "Not configuring rotate_chain benchmarks because (according to CMake) compiler does not support C++17") return() endif() +wave_geometry_add_benchmark(rotate_chain_hand_bench rotate_chain_hand_bench.cpp) +wave_geometry_add_benchmark(rotate_chain_wave_bench rotate_chain_wave_bench.cpp) +wave_geometry_add_benchmark(rotate_chain_wave_untyped_bench rotate_chain_wave_untyped_bench.cpp) +wave_geometry_add_benchmark(rotate_chain_wave_reverse_bench rotate_chain_wave_reverse_bench.cpp) +wave_geometry_add_benchmark(rotate_chain_wave_dynamic_bench rotate_chain_wave_dynamic_bench.cpp) + + wave_geometry_add_benchmark(imu_preint imu_preint.cpp) diff --git a/benchmarks/rotate_chain/rotate_chain_gtsam_bench.cpp b/benchmarks/rotate_chain/rotate_chain_gtsam_bench.cpp index 45affe3..c30f91b 100644 --- a/benchmarks/rotate_chain/rotate_chain_gtsam_bench.cpp +++ b/benchmarks/rotate_chain/rotate_chain_gtsam_bench.cpp @@ -1,10 +1,15 @@ -#include -#include "../bechmark_helpers.hpp" +#include +// Some adaptions for gtsam's bundled Eigen being old +#if !(EIGEN_VERSION_AT_LEAST(3, 3, 0)) +#define EIGEN_DEVICE_FUNC +namespace Eigen { +using Index = EIGEN_DEFAULT_DENSE_INDEX_TYPE; +} +#endif -#include +#include #include - - +#include #include "wave/geometry/src/util/math/math.hpp" @@ -12,242 +17,70 @@ template using EigenVector = std::vector>; -using gtsam::Rot3; -using gtsam::Point3; using gtsam::Expression; +using gtsam::Point3; +using gtsam::Rot3; -/** Return a vector of random gtsam rotations */ -EigenVector randomRot3s(int N) { - EigenVector v; - v.reserve(N); - for (auto i = N; i--;) { - const auto &q = wave::randomQuaternion(); - v.emplace_back(q); - } - return v; -} - -inline Eigen::Matrix3d crossMatrix(const Eigen::Vector3d &v) { - Eigen::Matrix3d m; - m << 0.0, -v.z(), v.y(), // - v.z(), 0.0, -v.x(), // - -v.y(), v.x(), 0.0; - return m; -}; - -class RotateChain : public benchmark::Fixture { - protected: - const int N = 1000; - std::vector vv; - const EigenVector R1 = randomRot3s(N); - const EigenVector R2 = randomRot3s(N); - const EigenVector R3 = randomRot3s(N); - const EigenVector R4 = randomRot3s(N); - const EigenVector R5 = randomRot3s(N); - const EigenVector R6 = randomRot3s(N); - const EigenVector R7 = randomRot3s(N); - const EigenVector R8 = randomRot3s(N); - const EigenVector R9 = randomRot3s(N); - const EigenVector R10 = randomRot3s(N); - const EigenVector v1 = randomMatrices(N); - - RotateChain() { - for (auto i = 0; i < N; ++i) { - gtsam::Values values{}; - values.insert(gtsam::Symbol{'R', 1}, Rot3{wave::randomQuaternion()}); - values.insert(gtsam::Symbol{'R', 2}, Rot3{wave::randomQuaternion()}); - values.insert(gtsam::Symbol{'R', 3}, Rot3{wave::randomQuaternion()}); - values.insert(gtsam::Symbol{'R', 4}, Rot3{wave::randomQuaternion()}); - values.insert(gtsam::Symbol{'R', 5}, Rot3{wave::randomQuaternion()}); - values.insert(gtsam::Symbol{'R', 6}, Rot3{wave::randomQuaternion()}); - values.insert(gtsam::Symbol{'R', 7}, Rot3{wave::randomQuaternion()}); - values.insert(gtsam::Symbol{'R', 8}, Rot3{wave::randomQuaternion()}); - values.insert(gtsam::Symbol{'R', 9}, Rot3{wave::randomQuaternion()}); - values.insert(gtsam::Symbol{'R', 10}, Rot3{wave::randomQuaternion()}); - values.insert(gtsam::Symbol{'p', 1}, Point3{Eigen::Vector3d::Random()}); - vv.push_back(values); - } - } -}; - -BENCHMARK_F(RotateChain, Gtsam1)(benchmark::State &state) { - for (auto _ : state) { - Expression R1_{'R', 1}; - Expression p1_{'p', 1}; - Expression p2_ = rotate(R1_, p1_); - std::vector jacs(2); - - for (auto i = N; i-- > 0;) { - Point3 p2 = p2_.value(vv[i], jacs); - - benchmark::DoNotOptimize(p2); - benchmark::DoNotOptimize(jacs.data()); - } - } -} - -BENCHMARK_F(RotateChain, Gtsam2)(benchmark::State &state) { - const Expression R1_{Rot3{}}, R2_{'R', 2}; - const Expression p1_{'p', 1}; - const Expression p2_ = rotate(R1_ * R2_, p1_); - - std::vector jacs(3); - - for (auto _ : state) { - for (auto i = N; i-- > 0;) { - Point3 p2 = p2_.value(vv[i], jacs); - - benchmark::DoNotOptimize(p2); - benchmark::DoNotOptimize(jacs); - } - } -} - -BENCHMARK_F(RotateChain, Gtsam3)(benchmark::State &state) { - const Expression R1_{'R', 1}, R2_{'R', 2}, R3_{'R', 3}; - const Expression p1_{'p', 1}; - const Expression p2_ = rotate(R1_ * R2_ * R3_, p1_); - - std::vector jacs(4); - - for (auto _ : state) { - for (auto i = N; i-- > 0;) { - Point3 p2 = p2_.value(vv[i], jacs); - - benchmark::DoNotOptimize(p2); - benchmark::DoNotOptimize(jacs); - } - } -} - -BENCHMARK_F(RotateChain, Gtsam4)(benchmark::State &state) { - const Expression R1_{'R', 1}, R2_{'R', 2}, R3_{'R', 3}, R4_{'R', 4}; - const Expression p1_{'p', 1}; - const Expression p2_ = rotate(R1_ * R2_ * R3_ * R4_, p1_); - - std::vector jacs(5); - - for (auto _ : state) { - for (auto i = N; i-- > 0;) { - Point3 p2 = p2_.value(vv[i], jacs); - - benchmark::DoNotOptimize(p2); - benchmark::DoNotOptimize(jacs); - } - } -} - -BENCHMARK_F(RotateChain, Gtsam5)(benchmark::State &state) { - const Expression R1_{'R', 1}, R2_{'R', 2}, R3_{'R', 3}, R4_{'R', 4}, - R5_{'R', 5}; - const Expression p1_{'p', 1}; - const Expression p2_ = rotate(R1_ * R2_ * R3_ * R4_ * R5_, p1_); - - std::vector jacs(6); - - for (auto _ : state) { - for (auto i = N; i-- > 0;) { - Point3 p2 = p2_.value(vv[i], jacs); - - benchmark::DoNotOptimize(p2); - benchmark::DoNotOptimize(jacs); - } - } -} - -BENCHMARK_F(RotateChain, Gtsam6)(benchmark::State &state) { - const Expression R1_{'R', 1}, R2_{'R', 2}, R3_{'R', 3}, R4_{'R', 4}, - R5_{'R', 5}, R6_{'R', 6}; - const Expression p1_{'p', 1}; - const Expression p2_ = rotate(R1_ * R2_ * R3_ * R4_ * R5_ * R6_, p1_); - - std::vector jacs(7); - - for (auto _ : state) { - for (auto i = N; i-- > 0;) { - Point3 p2 = p2_.value(vv[i], jacs); - - benchmark::DoNotOptimize(p2); - benchmark::DoNotOptimize(jacs); - } - } -} - -BENCHMARK_F(RotateChain, Gtsam7)(benchmark::State &state) { - const Expression R1_{'R', 1}, R2_{'R', 2}, R3_{'R', 3}, R4_{'R', 4}, - R5_{'R', 5}, R6_{'R', 6}, R7_{'R', 7}; - const Expression p1_{'p', 1}; - const Expression p2_ = rotate(R1_ * R2_ * R3_ * R4_ * R5_ * R6_ * R7_, p1_); - - std::vector jacs(8); - - for (auto _ : state) { - for (auto i = N; i-- > 0;) { - Point3 p2 = p2_.value(vv[i], jacs); - - benchmark::DoNotOptimize(p2); - benchmark::DoNotOptimize(jacs); - } +void BM_gtsamAll(benchmark::State &state) { + const auto N = state.range(0); + state.SetComplexityN(N); + // Produce the expression tree + gtsam::Values values; + auto p1 = Expression{'p', 1}; + auto expr = p1; + values.insert(gtsam::Symbol{'p', 1}, Point3{Eigen::Vector3d::Random()}); + + for (auto i = 0u; i < N; ++i) { + expr = Expression{rotate(Expression{'R', i}, expr)}; + values.insert(gtsam::Symbol{'R', i}, Rot3{wave::randomQuaternion()}); } -} - -BENCHMARK_F(RotateChain, Gtsam8)(benchmark::State &state) { - const Expression R1_{'R', 1}, R2_{'R', 2}, R3_{'R', 3}, R4_{'R', 4}, - R5_{'R', 5}, R6_{'R', 6}, R7_{'R', 7}, R8_{'R', 8}; - const Expression p1_{'p', 1}; - const Expression p2_ = - rotate(R1_ * R2_ * R3_ * R4_ * R5_ * R6_ * R7_ * R8_, p1_); - - std::vector jacs(9); + std::vector jacs(N + 1); for (auto _ : state) { - for (auto i = N; i-- > 0;) { - Point3 p2 = p2_.value(vv[i], jacs); + Point3 p2 = expr.value(values, jacs); - benchmark::DoNotOptimize(p2); - benchmark::DoNotOptimize(jacs); - } + benchmark::DoNotOptimize(p2); + benchmark::DoNotOptimize(jacs); } } - -BENCHMARK_F(RotateChain, Gtsam9)(benchmark::State &state) { - const Expression R1_{'R', 1}, R2_{'R', 2}, R3_{'R', 3}, R4_{'R', 4}, - R5_{'R', 5}, R6_{'R', 6}, R7_{'R', 7}, R8_{'R', 8}, R9_{'R', 9}; - const Expression p1_{'p', 1}; - const Expression p2_ = - rotate(R1_ * R2_ * R3_ * R4_ * R5_ * R6_ * R7_ * R8_ * R9_, p1_); - - std::vector jacs(10); - - for (auto _ : state) { - for (auto i = N; i-- > 0;) { - Point3 p2 = p2_.value(vv[i], jacs); - - benchmark::DoNotOptimize(p2); - benchmark::DoNotOptimize(jacs); - } +// This benchmark tests gtsam's valueAndJacobianMap() function, which is normally private +#ifdef LK_CUSTOM_GTSAM_PUBLIC +void BM_gtsamPrivate(benchmark::State &state) { + const auto N = state.range(0); + state.SetComplexityN(N); + // Produce the expression tree + gtsam::Values values; + auto p1 = Expression{'p', 1}; + auto expr = p1; + values.insert(gtsam::Symbol{'p', 1}, Point3{Eigen::Vector3d::Random()}); + + for (auto i = 0u; i < N; ++i) { + expr = Expression{rotate(Expression{'R', i}, expr)}; + values.insert(gtsam::Symbol{'R', i}, Rot3{wave::randomQuaternion()}); } -} - - -BENCHMARK_F(RotateChain, Gtsam10)(benchmark::State &state) { - const Expression R1_{'R', 1}, R2_{'R', 2}, R3_{'R', 3}, R4_{'R', 4}, - R5_{'R', 5}, R6_{'R', 6}, R7_{'R', 7}, R8_{'R', 8}, R9_{'R', 9}, R10_{'R', 10}; - const Expression p1_{'p', 1}; - const Expression p2_ = - rotate(R1_ * R2_ * R3_ * R4_ * R5_ * R6_ * R7_ * R8_ * R9_ * R10_, p1_); - - std::vector jacs(11); + std::vector jacs(N + 1); for (auto _ : state) { - for (auto i = N; i-- > 0;) { - Point3 p2 = p2_.value(vv[i], jacs); - - benchmark::DoNotOptimize(p2); - benchmark::DoNotOptimize(jacs); - } + // Pre-allocate and zero VerticalBlockMatrix + gtsam::KeyVector keys; + gtsam::FastVector dims; + boost::tie(keys, dims) = expr.keysAndDims(); + static const int Dim = gtsam::traits::dimension; + gtsam::VerticalBlockMatrix Ab(dims, Dim); + Ab.matrix().setZero(); + gtsam::internal::JacobianMap jacobianMap(keys, Ab); + + // Call unsafe version + Point3 p2 = expr.valueAndJacobianMap(values, jacobianMap); + + benchmark::DoNotOptimize(p2); + benchmark::DoNotOptimize(values); + benchmark::DoNotOptimize(jacobianMap); } } +BENCHMARK(BM_gtsamPrivate)->RangeMultiplier(2)->Range(1, 1 << 14)->Complexity(); +#endif -WAVE_BENCHMARK_MAIN() +BENCHMARK(BM_gtsamAll)->RangeMultiplier(2)->Range(1, 1 << 14)->Complexity(); +BENCHMARK_MAIN(); diff --git a/benchmarks/rotate_chain/rotate_chain_hand_bench.cpp b/benchmarks/rotate_chain/rotate_chain_hand_bench.cpp index 1f912d3..6dad808 100644 --- a/benchmarks/rotate_chain/rotate_chain_hand_bench.cpp +++ b/benchmarks/rotate_chain/rotate_chain_hand_bench.cpp @@ -23,7 +23,7 @@ using EigenVector = std::vector>; template class RotateChain : public benchmark::Fixture { protected: - const int N = 1000; + const int N = 1; // Use wave::RotationM just for the Random() method, which produces valid SO(3) using waveRot = wave::MatrixRotation; const EigenVector R1 = randomMatrices(N); diff --git a/benchmarks/rotate_chain/rotate_chain_wave_bench.cpp b/benchmarks/rotate_chain/rotate_chain_wave_bench.cpp index 5a7009d..c8df0d0 100644 --- a/benchmarks/rotate_chain/rotate_chain_wave_bench.cpp +++ b/benchmarks/rotate_chain/rotate_chain_wave_bench.cpp @@ -30,7 +30,7 @@ RMFd randomRMFd() { class RotateChain : public benchmark::Fixture { protected: - const int N = 1000; + const int N = 1; const RMFdVector<0, 1> R1 = randomMatrices>(N); const RMFdVector<1, 2> R2 = randomMatrices>(N); const RMFdVector<2, 3> R3 = randomMatrices>(N); diff --git a/benchmarks/rotate_chain/rotate_chain_wave_dynamic_bench.cpp b/benchmarks/rotate_chain/rotate_chain_wave_dynamic_bench.cpp new file mode 100644 index 0000000..29e4ec2 --- /dev/null +++ b/benchmarks/rotate_chain/rotate_chain_wave_dynamic_bench.cpp @@ -0,0 +1,68 @@ +#include + +#include "../bechmark_helpers.hpp" +#include "wave/geometry/dynamic.hpp" +#include "wave/geometry/geometry.hpp" + +template +using EigenVector = std::vector>; + +void BM_waveAll(benchmark::State &state) { + const auto N = state.range(0); + state.SetComplexityN(N); + // Produce the expression tree + // EigenVector> proxies; + auto expr = makeProxy(wave::Translationd::Random()); + for (auto i = N; i > 0; --i) { + expr = makeProxy(makeProxy(wave::RotationMd::Random()) * expr); + } + + for (auto _ : state) { + auto[res, jac_map] = wave::internal::evaluateWithDynamicReverseJacobians(expr); + + benchmark::DoNotOptimize(res); + benchmark::DoNotOptimize(jac_map); + } +} + +void BM_dynamicNoVirtual(benchmark::State &state) { + auto v = wave::Translationd::Random(); + std::array R{}; + for (int i = 0; i < 10; ++i) { + R[i] = wave::RotationMd::Random(); + } + + for (auto _ : state) { + auto expr = + R[0] * R[1] * R[2] * R[3] * R[4] * R[5] * R[6] * R[7] * R[8] * R[9] * v; + + auto[res, jac_map] = wave::internal::evaluateWithDynamicReverseJacobians(expr); + + benchmark::DoNotOptimize(res); + benchmark::DoNotOptimize(jac_map); + } +} + + +void BM_waveDynamicLeaves(benchmark::State &state) { + const auto N = state.range(0); + // Produce the expression tree + EigenVector> proxies; + auto expr = makeProxy(wave::Translationd::Random()); + for (auto i = N; i > 0; --i) { + proxies.emplace_back(wave::RotationMd::Random()); + expr = makeProxy(proxies.back() * expr); + } + + for (auto _ : state) { + auto map = wave::internal::getLeavesMap(expr); + benchmark::DoNotOptimize(map); + } +} + +// BENCHMARK(BM_waveDynamicLeaves)->Range(10, 200000)->Complexity(); +// BENCHMARK(BM_waveDynamic)->Arg(10); +BENCHMARK(BM_waveAll)->RangeMultiplier(2)->DenseRange(1, 1 << 14)->Complexity(); +// BENCHMARK(BM_dynamicNoVirtual); + +WAVE_BENCHMARK_MAIN() diff --git a/benchmarks/rotate_chain/rotate_chain_wave_reverse_bench.cpp b/benchmarks/rotate_chain/rotate_chain_wave_reverse_bench.cpp index f6bdad7..bf1ee68 100644 --- a/benchmarks/rotate_chain/rotate_chain_wave_reverse_bench.cpp +++ b/benchmarks/rotate_chain/rotate_chain_wave_reverse_bench.cpp @@ -30,7 +30,7 @@ RMFd randomRMFd() { class RotateChain : public benchmark::Fixture { protected: - const int N = 1000; + const int N = 1; const RMFdVector<0, 1> R1 = randomMatrices>(N); const RMFdVector<1, 2> R2 = randomMatrices>(N); const RMFdVector<2, 3> R3 = randomMatrices>(N); diff --git a/benchmarks/rotate_chain/rotate_chain_wave_untyped_bench.cpp b/benchmarks/rotate_chain/rotate_chain_wave_untyped_bench.cpp index ea8e448..b1d73f0 100644 --- a/benchmarks/rotate_chain/rotate_chain_wave_untyped_bench.cpp +++ b/benchmarks/rotate_chain/rotate_chain_wave_untyped_bench.cpp @@ -9,7 +9,7 @@ using EigenVector = std::vector>; class RotateChain : public benchmark::Fixture { protected: - const int N = 1000; + const int N = 1; const EigenVector R1 = randomMatrices(N); const EigenVector R2 = randomMatrices(N); const EigenVector R3 = randomMatrices(N); diff --git a/benchmarks/util_cross_matrix_bench.cpp b/benchmarks/util_cross_matrix_bench.cpp index e4995f0..2de2691 100644 --- a/benchmarks/util_cross_matrix_bench.cpp +++ b/benchmarks/util_cross_matrix_bench.cpp @@ -237,4 +237,4 @@ BENCHMARK_TEMPLATE(BM_CrossTimesCross, ManualCross)->Arg(reps); BENCHMARK_TEMPLATE(BM_CrossTimesCross, WaveCross)->Arg(reps); -BENCHMARK_MAIN() +BENCHMARK_MAIN(); diff --git a/docs/dynamic_expressions.md b/docs/dynamic_expressions.md new file mode 100644 index 0000000..d0034d1 --- /dev/null +++ b/docs/dynamic_expressions.md @@ -0,0 +1,73 @@ +# Dynamic expressions + +Sometimes, we want to do something not possible with pure expression templates: decide the +structure of an expression at runtime, make heterogenous vectors of expressions, or spread +compilation across translation units. Specifically, we want expressions with _dynamic_ +polymorphism. We may also want to store objects on the heap and share their ownership. + +These tasks are possible with `wave_geometry`'s `dynamic` module. +This module comes in a separate header: + +```cpp +#include +#include +``` + +## Proxy expressions + +For any leaf type `L`, the class `Proxy` wraps a heap-allocated expression that will +evaluate to `L`, but whose concrete type is determined at runtime. Proxy objects hold a +`shared_ptr` to the expression. Consider the example: + +```cpp +wave::Proxy t = wave::Translationd::Random(); +wave::Proxy R1 = wave::RotationMd::Random(); +wave::Proxy R2 = wave::RotationMd::Random(); + +wave::Proxy result = R1 * t; +if (R2_is_needed) { + result = R2 * result; +} +return result; +``` + +This example above can equivalently be written as: + +```cpp +auto t = makeProxy(wave::Translationd::Random()); +auto R1 = makeProxy(wave::RotationMd::Random()); +auto R2 = makeProxy(wave::RotationMd::Random()); + +auto result = makeProxy(R1 * t); +if (R2_is_needed) { + result = R2 * result; +} +return result; +``` + +The first three lines construct random-valued leaf objects on the heap and +give their ownership to `Proxy` objects. `result` is a `Proxy`, the same +type as `t`, but it points to a `Rotate` expression of other `Proxy` objects. We can +safely `return result` knowing the dynamic expression tree it points to on the heap will +remain, because its lifetime is controlled by `shared_ptr`s. We can assign to proxies with +shared pointer semantics. + +`Proxy` is analogous to GTSAM's `Expression`. + + +Proxies are fully compatible with "regular" expressions: they can be evaluated using +`eval()` or by assignment to a leaf, they can be used in static expressions, and they can +hold arbitrary expressions. Caveat: it is usually a bad idea put an expression that +contains references to stack objects in a `Proxy`. If those objects are destroyed before +the `Proxy`, it will be left with dangling references. + + +Under the hood, creating a `Proxy` constructs a `Dynamic` expression. For example, the line + +```cpp +auto result = makeProxy(R1 * t); +``` + +puts an object of type `Dynamic&&,Proxy&&>&&>` on +the heap. The resulting `Proxy` holds a shared pointer to that object +through its abstract base class, `DynamicBase`. diff --git a/docs/index.rst b/docs/index.rst index e8d53bd..dfd40ac 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -18,6 +18,7 @@ wave_geometry manifold_operations autodiff frame_semantics + dynamic_expressions changelog cite license diff --git a/docs/install.md b/docs/install.md index 034289c..af74520 100644 --- a/docs/install.md +++ b/docs/install.md @@ -13,7 +13,7 @@ git clone https://github.com/wavelab/wave_geometry wave_geometry requires: * an existing installation of [Eigen](http://eigen.tuxfamily.org) 3.3.2 or above * an existing installation of Boost (only the header-only Optional library is used) - * a modern C++11 compiler (tested on GCC 5.4, clang 4.0) + * a C++14 compiler (tested on GCC 5.4, clang 4.0) It has only been tested on Linux. diff --git a/include/wave/geometry/core.hpp b/include/wave/geometry/core.hpp index 26bfd22..23df2b4 100644 --- a/include/wave/geometry/core.hpp +++ b/include/wave/geometry/core.hpp @@ -5,6 +5,14 @@ #ifndef WAVE_GEOMETRY_CORE_HPP #define WAVE_GEOMETRY_CORE_HPP +// For shared_ptr, used by Proxy +#include +// For optional, used by JacobianEvaluator +#include +// Used by DynamicReverseJacobianEvaluator +#include +#include + // Tick library for traits checking #include #include @@ -16,22 +24,26 @@ #include "src/util/meta/type_list.hpp" #include "src/util/math/math.hpp" #include "src/util/math/IdentityMatrix.hpp" +#include "src/util/math/MatrixMap.hpp" // Forward declarations and standalone type traits #include "src/core/forward_declarations.hpp" #include "src/core/traits/type_traits.hpp" #include "src/core/storage/RefSelector.hpp" -// Evaluation helpers +// Evaluators #include "src/core/functions/IsSame.hpp" #include "src/core/functions/IsSameType.hpp" #include "src/core/functions/AddConversions.hpp" #include "src/core/functions/PrepareExpr.hpp" #include "src/core/functions/Evaluator.hpp" #include "src/core/functions/PrepareOutput.hpp" +#include "src/core/functions/DynamicJacobianEvaluator.hpp" #include "src/core/functions/JacobianEvaluator.hpp" #include "src/core/functions/TypedJacobianEvaluator.hpp" #include "src/core/functions/ReverseJacobianEvaluator.hpp" +#include "src/core/functions/DynamicReverseJacobianEvaluator.hpp" +#include "src/core/functions/NumericalJacobian.hpp" // Storage and traits bases #include "src/core/storage/UnaryExpression.hpp" @@ -40,9 +52,7 @@ #include "src/core/traits/traits_bases.hpp" // Expressions -#include "src/core/op/Convert.hpp" - -// Generic expressions #include "src/core/base/ExpressionBase.hpp" +#include "src/core/op/Convert.hpp" #endif // WAVE_GEOMETRY_CORE_HPP diff --git a/include/wave/geometry/debug.hpp b/include/wave/geometry/debug.hpp index 3a0fdcd..39ec10e 100644 --- a/include/wave/geometry/debug.hpp +++ b/include/wave/geometry/debug.hpp @@ -8,6 +8,5 @@ #include "geometry.hpp" #include "src/debug/PrintExpression.hpp" -#include "src/debug/NumericalJacobian.hpp" #endif // WAVE_GEOMETRY_DEBUG_HPP diff --git a/include/wave/geometry/dynamic.hpp b/include/wave/geometry/dynamic.hpp new file mode 100644 index 0000000..972fd91 --- /dev/null +++ b/include/wave/geometry/dynamic.hpp @@ -0,0 +1,31 @@ +/** + * @file Definitions of dynamic expressions + */ + +#ifndef WAVE_GEOMETRY_DYNAMIC_HPP +#define WAVE_GEOMETRY_DYNAMIC_HPP + +#include "core.hpp" + +namespace wave { + +template +class DynamicBase; + +template +struct Dynamic; + +template +class Proxy; + +template +class RefProxy; + +} // namespace wave + +#include "src/dynamic/DynamicBase.hpp" +#include "src/dynamic/Dynamic.hpp" +#include "src/dynamic/Proxy.hpp" +#include "src/dynamic/RefProxy.hpp" + +#endif // WAVE_GEOMETRY_DYNAMIC_HPP diff --git a/include/wave/geometry/src/core/forward_declarations.hpp b/include/wave/geometry/src/core/forward_declarations.hpp index ed47af9..1960e9b 100644 --- a/include/wave/geometry/src/core/forward_declarations.hpp +++ b/include/wave/geometry/src/core/forward_declarations.hpp @@ -30,10 +30,11 @@ struct NotImplemented; /** Marker of a disabled call (should be accompanied by static_assert) */ struct NotAllowed {}; -/** Every expression must have an ExpressionImpl with these members: - * @todo */ +/** Marker of ADL-enabled functions in internal namespace */ +struct adl {}; + template -struct ExpressionImpl; +struct PrepareExpr; } // namespace internal diff --git a/include/wave/geometry/src/core/functions/AddConversions.hpp b/include/wave/geometry/src/core/functions/AddConversions.hpp index 24077d5..fc910d1 100644 --- a/include/wave/geometry/src/core/functions/AddConversions.hpp +++ b/include/wave/geometry/src/core/functions/AddConversions.hpp @@ -51,7 +51,7 @@ struct first_directly_evaluable_conversion_unary, eval_t>, is_directly_evaluable_unary> { - using type = Rebind>; + using type = Rebind &&>; }; template @@ -126,14 +126,14 @@ struct first_directly_evaluable_conversion_binary, eval_t>, is_directly_evaluable_binary>> { - using type = Rebind, Rhs>; + using type = Rebind &&, Rhs>; }; template struct convert_right_test : tmp::conjunction, eval_t>, is_directly_evaluable_binary, ToRhs>> { - using type = Rebind>; + using type = Rebind &&>; }; template @@ -141,7 +141,7 @@ struct first_directly_evaluable_conversion_binary, eval_t>, is_directly_evaluable_unary, eval_t>, is_directly_evaluable_binary> { - using type = Rebind, Convert>; + using type = Rebind &&, Convert &&>; }; template diff --git a/include/wave/geometry/src/core/functions/DynamicJacobianEvaluator.hpp b/include/wave/geometry/src/core/functions/DynamicJacobianEvaluator.hpp new file mode 100644 index 0000000..cfaf497 --- /dev/null +++ b/include/wave/geometry/src/core/functions/DynamicJacobianEvaluator.hpp @@ -0,0 +1,239 @@ +/** + * @file + */ + +#ifndef WAVE_GEOMETRY_DYNAMICJACOBIANEVALUATOR_HPP +#define WAVE_GEOMETRY_DYNAMICJACOBIANEVALUATOR_HPP + +namespace wave { +namespace internal { + +/** Evaluates an expression's Jacobian with respect to one target in forward mode + * The target is known only by address. + */ +template +struct DynamicJacobianEvaluator; + +// Convenience typedef for dynamic-size matrix +template +using DynamicMatrix = Eigen::Matrix; + +/** Specialization for leaf expression */ +template +struct DynamicJacobianEvaluator> { + private: + static constexpr int JSize = traits::TangentSize; + + public: + WAVE_STRONG_INLINE DynamicJacobianEvaluator(const Evaluator &evaluator, + const void *target) + : evaluator{evaluator}, target{target} {} + + /** Finds Jacobian of the leaf expression + */ + WAVE_STRONG_INLINE boost::optional> jacobian() const { + if (isSame(evaluator.expr, target)) { + // Jacobian wrt self is always identity (dx/dx = 1) + return identity_t{}; + } else { + // Jacobian of a leaf expression wrt something else is always zero. + // Note we can't know if they're somehow related elsewhere; we only consider + // the expressions given, and assume leaf expressions are the end of the line. + return boost::none; + } + } + + private: + const Evaluator &evaluator; + const void *target; +}; + +/** Specialization for unary expression */ +template +struct DynamicJacobianEvaluator> { + using Jacobian = DynamicMatrix>; + enum : int { TangentSize = eval_traits::TangentSize }; + using RhsEval = DynamicJacobianEvaluator::RhsDerived>; + + private: + // Wrapped Evaluator and nested jacobian-evaluators + const Evaluator &evaluator; + // We leave rhs_eval empty (and stop the recursion) if we match the target + const boost::optional rhs_eval; + + boost::optional initializeRhsEval(const Evaluator &evaluator, + const void *target) const { + if (isSame(evaluator.expr, target)) { + return boost::none; + } + return RhsEval{evaluator.rhs_eval, target}; + } + + public: + WAVE_STRONG_INLINE DynamicJacobianEvaluator(const Evaluator &evaluator, + const void *target) + : evaluator{evaluator}, rhs_eval{initializeRhsEval(evaluator, target)} {} + + /** @returns jacobian matrix if expr contains target type, or zero matrix otherwise. + */ + WAVE_STRONG_INLINE boost::optional jacobian() const { + if (!this->rhs_eval) { + return Jacobian::Identity(TangentSize, TangentSize) + .eval(); // We match the target + } + + const auto &rhs_jac = this->rhs_eval->jacobian(); + if (rhs_jac) { + return Jacobian{jacobianImpl(get_expr_tag_t{}, + this->evaluator(), + this->evaluator.rhs_eval()) * + (*rhs_jac)}; + } + return boost::none; + } +}; + +/** Specialization for binary expression where both sides *might* contain target */ +template +struct DynamicJacobianEvaluator> { + using Jacobian = DynamicMatrix>; + enum : int { TangentSize = eval_traits::TangentSize }; + using LhsEval = DynamicJacobianEvaluator::LhsDerived>; + using RhsEval = DynamicJacobianEvaluator::RhsDerived>; + + private: + // Wrapped Evaluator and nested jacobian-evaluators + const Evaluator &evaluator; + // We leave lhs_eval and rhs_eval empty (and stop recursing) if we match the target + const boost::optional lhs_eval; + const boost::optional rhs_eval; + + boost::optional initializeLhsEval(const Evaluator &evaluator, + const void *target) const { + if (isSame(evaluator.expr, target)) { + return boost::none; + } + return LhsEval{evaluator.lhs_eval, target}; + } + + boost::optional initializeRhsEval(const Evaluator &evaluator, + const void *target) const { + if (isSame(evaluator.expr, target)) { + return boost::none; + } + return RhsEval{evaluator.rhs_eval, target}; + } + + + public: + WAVE_STRONG_INLINE DynamicJacobianEvaluator(const Evaluator &evaluator, + const void *target) + : evaluator{evaluator}, + lhs_eval{initializeLhsEval(evaluator, target)}, + rhs_eval{initializeRhsEval(evaluator, target)} {} + + /** @returns jacobian matrix if expr contains target type, or zero matrix otherwise. + */ + WAVE_STRONG_INLINE boost::optional jacobian() const { + if (!this->rhs_eval) { + // We match the target + return Jacobian::Identity(TangentSize, TangentSize).eval(); + } + const auto &lhs_jac = this->lhs_eval->jacobian(); + const auto &rhs_jac = this->rhs_eval->jacobian(); + if (lhs_jac && rhs_jac) { + return Jacobian{leftJacobianImpl(get_expr_tag_t{}, + this->evaluator(), + this->evaluator.lhs_eval(), + this->evaluator.rhs_eval()) * + (*lhs_jac) + + rightJacobianImpl(get_expr_tag_t{}, + this->evaluator(), + this->evaluator.lhs_eval(), + this->evaluator.rhs_eval()) * + (*rhs_jac)}; + } else if (lhs_jac) { + return Jacobian{leftJacobianImpl(get_expr_tag_t{}, + this->evaluator(), + this->evaluator.lhs_eval(), + this->evaluator.rhs_eval()) * + (*lhs_jac)}; + } else if (rhs_jac) { + return Jacobian{rightJacobianImpl(get_expr_tag_t{}, + this->evaluator(), + this->evaluator.lhs_eval(), + this->evaluator.rhs_eval()) * + (*rhs_jac)}; + } + return boost::none; + } +}; + +/** Evaluate a jacobian using an existing Evaluator tree. Use target pointer. + */ +template +inline auto evaluateOneDynamicJacobianRaw(const Evaluator &v_eval, + const void *target) + -> boost::optional>> { + internal::DynamicJacobianEvaluator j_eval{v_eval, target}; + const auto &result = j_eval.jacobian(); + + // Convert Jacobian type to dense where needed + if (result) { + return static_cast>>(*result); + } else { + return boost::none; + } +} + +/** Evaluate a jacobian using an existing Evaluator tree + */ +template +inline auto evaluateOneDynamicJacobian(const Evaluator &v_eval, + const Target &target) + -> jacobian_t { + const auto &actual_target = getWrtTarget(leaf{}, target); + internal::DynamicJacobianEvaluator j_eval{v_eval, &actual_target}; + const auto &result = j_eval.jacobian(); + if (result) { + return *result; + } else { + return jacobian_t::Zero(); + } +} + +/** Evaluate a jacobian of a expression tree by folding it with + * internal::DynamicJacobianEvaluator as the functor + * + * @note this also calculates the value and discards it + */ +template +auto evaluateDynamicJacobian(const ExpressionBase &expr, const Target &target) + -> jacobian_t { + // Note since we don't return the value, we don't need the user-facing OutputType + using OutType = eval_output_t; + const auto &v_eval = prepareEvaluatorTo(expr.derived()); + return evaluateOneDynamicJacobian(v_eval, target); +} + +/** Evaluate the result of an expression tree and any number of jacobians + * + * @return the value of the expression + * @param targets N dependent leaf expressions + * @param jacobians N matrices for the output + */ +template +auto evaluateWithDynamicJacobians(const ExpressionBase &expr, + const Targets &... targets) + -> std::tuple, jacobian_t...> { + // Get the value once + const auto &v_eval = prepareEvaluatorTo>(expr.derived()); + + return std::make_tuple(prepareOutput(v_eval), + evaluateOneDynamicJacobian(v_eval, targets)...); +} + +} // namespace internal +} // namespace wave + +#endif // WAVE_GEOMETRY_DYNAMICJACOBIANEVALUATOR_HPP diff --git a/include/wave/geometry/src/core/functions/DynamicReverseJacobianEvaluator.hpp b/include/wave/geometry/src/core/functions/DynamicReverseJacobianEvaluator.hpp new file mode 100644 index 0000000..006946a --- /dev/null +++ b/include/wave/geometry/src/core/functions/DynamicReverseJacobianEvaluator.hpp @@ -0,0 +1,263 @@ +/** + * @file + */ + +#ifndef WAVE_GEOMETRY_DYNAMICREVERSEJACOBIANEVALUATOR_HPP +#define WAVE_GEOMETRY_DYNAMICREVERSEJACOBIANEVALUATOR_HPP + +namespace wave { +namespace internal { + +/** Evaluates Jacobians of an expression graph in reverse mode. + * + * Unlike ReverseJacobianEvaluator, this evaluator does not require unique types or a true + * tree. + */ +template +struct DynamicReverseJacobianEvaluator; + +/** + * Each DynamicReverseJacobianEvaluator::jacobian() method returns a map of leaf addresses + * to Jacobian matrices + */ +template +using DynamicReverseResult = MatrixMap; + +/** getLeaves() functions build up a vector of addresses and tangent sizes. + * It will be sorted later. + */ +using DynamicLeavesVec = std::vector>; + +template = 0> +void getLeaves(adl, DynamicLeavesVec &vec, const ExpressionBase &expr) { + vec.emplace_back(&expr.derived(), traits::TangentSize); +} + +template = 0> +void getLeaves(adl, DynamicLeavesVec &vec, const ExpressionBase &expr) { + return getLeaves(adl{}, vec, expr.derived().rhs()); +} + +template = 0> +auto getLeaves(adl, DynamicLeavesVec &vec, const ExpressionBase &expr) -> void { + getLeaves(adl{}, vec, expr.derived().lhs()); + getLeaves(adl{}, vec, expr.derived().rhs()); +} + +/** Returns a map of address to leaf tangent size for the given expression */ +template +auto getLeavesMap(const Derived &expr, std::size_t expected_size = 0) + -> DynamicLeavesVec { + // Get a vector of leaf addresses, with duplicates + auto vec = DynamicLeavesVec{}; + vec.reserve(expected_size); + getLeaves(adl{}, vec, expr); + + // Sort the vector + std::sort(vec.begin(), vec.end(), [](auto &a, auto &b) { return a.first < b.first; }); + // Don't remove duplicates; the flat_map will do it on insert + + return vec; +} + +/** Either initialize a matrix in the map, or add to an existing matrix + * Insert a new element into the map if it's not there */ +template +void updateJacobianMap(DynamicReverseResult &jac_map, + const void *target, + const Adjoint &adjoint) { + jac_map[target] += adjoint; +} + +/** Specialization for leaf expression */ +template +struct DynamicReverseJacobianEvaluator> { + using CleanDerived = tmp::remove_cr_t; + + WAVE_STRONG_INLINE DynamicReverseJacobianEvaluator( + DynamicReverseResult> &jac_map, + const Evaluator &evaluator, + const Adjoint &adjoint) + : evaluator{evaluator} { + updateJacobianMap(jac_map, &evaluator.expr, adjoint); + } + + private: + const Evaluator &evaluator; +}; + +/** Specialization for unary expression */ +template +struct DynamicReverseJacobianEvaluator> { + private: + using SelfJacobian = decltype( + jacobianImpl(get_expr_tag_t{}, + std::declval>(), + std::declval::RhsDerived>>())); + using RhsAdjoint = decltype(std::declval() * std::declval()); + + + private: + // Wrapped Evaluator and + const Evaluator &evaluator; + + // Results cache + jac_ref_sel_t self_jac; + jac_ref_sel_t adjoint; + jac_ref_sel_t rhs_adjoint; + + // Nested jacobian-evaluators + const DynamicReverseJacobianEvaluator::RhsDerived, + RhsAdjoint> + rhs_eval; + + + public: + WAVE_STRONG_INLINE DynamicReverseJacobianEvaluator( + DynamicReverseResult> &jac_map, + const Evaluator &evaluator, + const Adjoint &adjoint_in) + : evaluator{evaluator}, + self_jac{jacobianImpl( + get_expr_tag_t{}, this->evaluator(), this->evaluator.rhs_eval())}, + adjoint{adjoint_in}, + rhs_adjoint{adjoint * self_jac}, + rhs_eval{jac_map, evaluator.rhs_eval, rhs_adjoint} {} +}; + +/** Specialization for a binary expression */ +template +struct DynamicReverseJacobianEvaluator> { + private: + using LhsSelfJacobian = decltype( + leftJacobianImpl(get_expr_tag_t{}, + std::declval>(), + std::declval::LhsDerived>>(), + std::declval::RhsDerived>>())); + using RhsSelfJacobian = decltype( + rightJacobianImpl(get_expr_tag_t{}, + std::declval>(), + std::declval::LhsDerived>>(), + std::declval::RhsDerived>>())); + using LhsAdjoint = + decltype(std::declval() * std::declval()); + using RhsAdjoint = + decltype(std::declval() * std::declval()); + + + private: + // Wrapped Evaluator + const Evaluator &evaluator; + + // Results cache + jac_ref_sel_t lhs_jac; + jac_ref_sel_t rhs_jac; + jac_ref_sel_t adjoint; + jac_ref_sel_t lhs_adjoint; + jac_ref_sel_t rhs_adjoint; + + // Nested jacobian-evaluators + const DynamicReverseJacobianEvaluator::LhsDerived, + LhsAdjoint> + lhs_eval; + const DynamicReverseJacobianEvaluator::RhsDerived, + RhsAdjoint> + rhs_eval; + + public: + WAVE_STRONG_INLINE DynamicReverseJacobianEvaluator( + DynamicReverseResult> &jac_map, + const Evaluator &evaluator, + const Adjoint &adjoint_in) + : evaluator{evaluator}, + lhs_jac{leftJacobianImpl(get_expr_tag_t{}, + this->evaluator(), + this->evaluator.lhs_eval(), + this->evaluator.rhs_eval())}, + rhs_jac{rightJacobianImpl(get_expr_tag_t{}, + this->evaluator(), + this->evaluator.lhs_eval(), + this->evaluator.rhs_eval())}, + adjoint{adjoint_in}, + lhs_adjoint{adjoint * lhs_jac}, + rhs_adjoint{adjoint * rhs_jac}, + lhs_eval{jac_map, evaluator.lhs_eval, lhs_adjoint}, + rhs_eval{jac_map, evaluator.rhs_eval, rhs_adjoint} {} +}; + +/** Helper to construct DynamicReverseJacobianEvaluator */ +template +WAVE_STRONG_INLINE void evaluateDynamicReverseJacobiansImpl( + DynamicReverseResult> &jac_map, + const Evaluator &v_eval, + const Adjoint &init_adjoint) { + // Make the DynamicReverseJacobianEvaluator tree and simultaneously fill the map + internal::DynamicReverseJacobianEvaluator{ + jac_map, v_eval, init_adjoint}; +} + +/** Get user-facing matrix for one target in DynamicReverseResult map + */ +template +auto evaluateDynamicReverseJacobiansHelper( + const DynamicReverseResult> &jac_map, const Target &target) + -> jacobian_t { + auto it = jac_map.find(&target); + if (it != jac_map.end() && it->second.size() != 0) { + // Convert dynamic matrix to user-facing fixed-size matrix + return it->second; + } else { + // An empty (size 0x0) dynamic matrix indicates "not found": Jacobian is zero + return jacobian_t::Zero(); + } +} + +/** Evaluate all leaf Jacobians in reverse mode using an existing Evaluator tree. + * + * @param expected_leaves number of leaves to preallocate (a few bytes) for + * @return result and map of leaf address to Jacobians as dynamic matrices + */ +template +WAVE_STRONG_INLINE auto evaluateDynamicReverseJacobians(const Evaluator &v_eval, + std::size_t expected_leaves = 256) + -> DynamicReverseResult> { + const auto leaves = getLeavesMap(v_eval.expr, expected_leaves); + auto jac_map = MatrixMap>{ + leaves.begin(), leaves.end(), eval_traits::TangentSize}; + jac_map.setZero(); + + evaluateDynamicReverseJacobiansImpl(jac_map, v_eval, identity_t{}); + return jac_map; +} + +/** Evaluate result and all Jacobians in reverse mode + * + * @return result and map of leaf address to Jacobians as dynamic matrices + */ +template +WAVE_STRONG_INLINE auto evaluateWithDynamicReverseJacobians( + const ExpressionBase &expr) + -> std::pair, DynamicReverseResult>> { + // @todo debug-mode checks + + // Get the correct evaluator + using OutputType = plain_output_t; + const auto &v_eval = prepareEvaluatorTo(expr.derived()); + + // Get result and map of leaf Jacobians + return {prepareOutput(v_eval), evaluateDynamicReverseJacobians(v_eval)}; +} + +} // namespace internal +} // namespace wave + +#endif // WAVE_GEOMETRY_DYNAMICREVERSEJACOBIANEVALUATOR_HPP diff --git a/include/wave/geometry/src/core/functions/Evaluator.hpp b/include/wave/geometry/src/core/functions/Evaluator.hpp index 1a289e8..880884f 100644 --- a/include/wave/geometry/src/core/functions/Evaluator.hpp +++ b/include/wave/geometry/src/core/functions/Evaluator.hpp @@ -18,9 +18,16 @@ namespace internal { template struct Evaluator; +template +struct Evaluator { + static_assert(tmp::alwaysFalse(), + "Internal error: Evaluator must be instantiated with clean type"); +}; + /** Specialization for leaf expression */ template struct Evaluator> { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW using EvalType = eval_t; WAVE_STRONG_INLINE explicit Evaluator(const Derived &expr) @@ -31,13 +38,14 @@ struct Evaluator> { } public: - const wave_ref_sel_t expr; + const eval_ref_sel_t expr; const EvalType result; }; /** Specialization for scalar type */ template -struct Evaluator{}>> { +struct Evaluator> { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW using EvalType = Derived; WAVE_STRONG_INLINE explicit Evaluator(const Derived &scalar) : expr{scalar} {} const EvalType &operator()() const { @@ -45,14 +53,15 @@ struct Evaluator{}>> { } public: - const wave_ref_sel_t expr; + const eval_ref_sel_t expr; }; /** Specialization for unary expression */ template struct Evaluator> { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW using EvalType = eval_t; - using RhsEval = Evaluator; + using RhsEval = Evaluator::RhsDerived>; WAVE_STRONG_INLINE explicit Evaluator(const Derived &expr) : expr{expr}, @@ -64,7 +73,7 @@ struct Evaluator> { } public: - const wave_ref_sel_t expr; + const eval_ref_sel_t expr; const RhsEval rhs_eval; const EvalType result; }; @@ -72,9 +81,10 @@ struct Evaluator> { /** Specialization for a binary expression */ template struct Evaluator> { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW using EvalType = eval_t; - using LhsEval = Evaluator; - using RhsEval = Evaluator; + using LhsEval = Evaluator::LhsDerived>; + using RhsEval = Evaluator::RhsDerived>; WAVE_STRONG_INLINE explicit Evaluator(const Derived &expr) : expr{expr}, @@ -88,7 +98,7 @@ struct Evaluator> { } public: - const wave_ref_sel_t expr; + const eval_ref_sel_t expr; const LhsEval lhs_eval; const RhsEval rhs_eval; const EvalType result; diff --git a/include/wave/geometry/src/core/functions/IsSame.hpp b/include/wave/geometry/src/core/functions/IsSame.hpp index 9293fd4..f107a63 100644 --- a/include/wave/geometry/src/core/functions/IsSame.hpp +++ b/include/wave/geometry/src/core/functions/IsSame.hpp @@ -7,17 +7,17 @@ namespace wave { -// Not in internal namespace due to some ADL(?) problem with recursive function templates +// Not in internal namespace due to ADL // @todo figure this out and move to internal namespace -/** Attempt to determine whether `a` and `b` are identical expressions. +/** Determines whether `a` and `b` are the same expression, for the purposes of autodiff. * * The criteria are * - `a` and `b` have the same derived type * - leaf expressions in `a` and `b` have the same storage address * * @warning This checks for identity in terms of C++ objects, not mathematical equality - * or equality of contents. Expressions which evaluate to the same result may not be + * or equality of contents. Expressions which evaluate to the same result might not be * considered identical by isSame. */ template @@ -63,6 +63,12 @@ inline constexpr bool isSame(const ExpressionBase &a, isSame(a.derived().rhs(), b.derived().rhs()); } +// Version for unknown target type (used by dynamic evaluators) +template +inline constexpr bool isSame(const ExpressionBase &a, const void *b) noexcept { + return static_cast(&getWrtTarget(internal::leaf{}, a.derived())) == b; +} + /** Attempt to determine whether `a` contains an expression identical to `b` * * @see isSame diff --git a/include/wave/geometry/src/core/functions/IsSameType.hpp b/include/wave/geometry/src/core/functions/IsSameType.hpp index 5dc7c40..d721b70 100644 --- a/include/wave/geometry/src/core/functions/IsSameType.hpp +++ b/include/wave/geometry/src/core/functions/IsSameType.hpp @@ -50,13 +50,13 @@ struct contains_same_type> : std::is_same struct contains_same_type> : tmp::bool_constant{} || - contains_same_type{}> {}; + contains_same_type::RhsDerived, B>{}> {}; template struct contains_same_type> : tmp::bool_constant{} || - contains_same_type{} || - contains_same_type{}> {}; + contains_same_type::LhsDerived, B>{} || + contains_same_type::RhsDerived, B>{}> {}; } // namespace internal diff --git a/include/wave/geometry/src/core/functions/JacobianEvaluator.hpp b/include/wave/geometry/src/core/functions/JacobianEvaluator.hpp index 9af36e1..2d9ba22 100644 --- a/include/wave/geometry/src/core/functions/JacobianEvaluator.hpp +++ b/include/wave/geometry/src/core/functions/JacobianEvaluator.hpp @@ -5,21 +5,30 @@ #ifndef WAVE_GEOMETRY_JACOBIANEVALUATOR_HPP #define WAVE_GEOMETRY_JACOBIANEVALUATOR_HPP -#include - namespace wave { namespace internal { -/** Functor to evaluate an expression tree with variables identified only by type - * - * @warning experimental +/** Evaluates an expression's Jacobian with respect to one target in forward mode */ template struct JacobianEvaluator; -/** Specialization for leaf expression of same type */ +/** Gets the target to differentiate w.r.t. for a given type. + * + * For most types, this is simply a passthrough. Allows special types like Proxy to + * differentiate w.r.t. another object. + * + * @note the unused leaf tag lets us put the function in the internal namespace and still + * have ADL + */ +template +decltype(auto) getWrtTarget(leaf, const ExpressionBase &target) { + return target.derived(); +} + +/** Specialization for expression of same type as the target */ template -struct JacobianEvaluator> { +struct JacobianEvaluator { using Scalar = scalar_t; WAVE_STRONG_INLINE JacobianEvaluator(const Evaluator &evaluator, @@ -51,7 +60,10 @@ struct JacobianEvaluator> /** Specialization for leaf expression of different type */ template -struct JacobianEvaluator> { +struct JacobianEvaluator< + Derived, + Target, + std::enable_if_t{} && !std::is_same{}>> { WAVE_STRONG_INLINE JacobianEvaluator(const Evaluator &, const Target &) {} /** Finds (trivial) jacobian of the leaf expression @@ -64,15 +76,18 @@ struct JacobianEvaluator> { }; -/** Specialization for unary expression */ +/** Specialization for unary expression of different type */ template -struct JacobianEvaluator> { +struct JacobianEvaluator< + Derived, + Target, + std::enable_if_t{} && !std::is_same{}>> { using Jacobian = jacobian_t; private: // Wrapped Evaluator and nested jacobian-evaluators const Evaluator &evaluator; - const JacobianEvaluator rhs_eval; + const JacobianEvaluator::RhsDerived, Target> rhs_eval; public: WAVE_STRONG_INLINE JacobianEvaluator(const Evaluator &evaluator, @@ -99,16 +114,18 @@ template struct JacobianEvaluator< Derived, Target, - std::enable_if_t{} && - contains_same_type::value && - contains_same_type::value>> { + std::enable_if_t< + is_binary_expression{} && + contains_same_type::LhsDerived, Target>::value && + contains_same_type::RhsDerived, Target>::value && + !std::is_same{}>> { using Jacobian = jacobian_t; private: // Wrapped Evaluator and nested jacobian-evaluators const Evaluator &evaluator; - const JacobianEvaluator lhs_eval; - const JacobianEvaluator rhs_eval; + const JacobianEvaluator::LhsDerived, Target> lhs_eval; + const JacobianEvaluator::RhsDerived, Target> rhs_eval; public: WAVE_STRONG_INLINE JacobianEvaluator(const Evaluator &evaluator, @@ -156,15 +173,16 @@ template struct JacobianEvaluator< Derived, Target, - std::enable_if_t::value && - contains_same_type::value && - !contains_same_type::value>> { + std::enable_if_t< + is_binary_expression::value && + contains_same_type::LhsDerived, Target>::value && + !contains_same_type::RhsDerived, Target>::value>> { using Jacobian = jacobian_t; private: // Wrapped Evaluator and nested jacobian-evaluators const Evaluator &evaluator; - const JacobianEvaluator lhs_eval; + const JacobianEvaluator::LhsDerived, Target> lhs_eval; public: WAVE_STRONG_INLINE JacobianEvaluator(const Evaluator &evaluator, @@ -192,15 +210,16 @@ template struct JacobianEvaluator< Derived, Target, - std::enable_if_t{} && - !contains_same_type::value && - contains_same_type::value>> { + std::enable_if_t< + is_binary_expression{} && + !contains_same_type::LhsDerived, Target>::value && + contains_same_type::RhsDerived, Target>::value>> { using Jacobian = jacobian_t; private: // Wrapped Evaluator and nested jacobian-evaluators const Evaluator &evaluator; - const JacobianEvaluator rhs_eval; + const JacobianEvaluator::RhsDerived, Target> rhs_eval; public: WAVE_STRONG_INLINE JacobianEvaluator(const Evaluator &evaluator, @@ -228,7 +247,7 @@ struct JacobianEvaluator< template inline auto evaluateOneJacobian(const Evaluator &v_eval, const Target &target) -> jacobian_t { - internal::JacobianEvaluator j_eval{v_eval, target}; + const auto j_eval = internal::JacobianEvaluator{v_eval, target}; const auto &result = j_eval.jacobian(); if (result) { return *result; @@ -248,7 +267,7 @@ auto evaluateJacobian(const ExpressionBase &expr, const Target &target) // Note since we don't return the value, we don't need the user-facing OutputType using OutType = eval_output_t; const auto &v_eval = prepareEvaluatorTo(expr.derived()); - return evaluateOneJacobian(v_eval, target); + return evaluateOneJacobian(v_eval, getWrtTarget(leaf{}, target)); } /** Evaluate the result of an expression tree and any number of jacobians @@ -265,7 +284,7 @@ auto evaluateWithJacobians(const ExpressionBase &expr, const auto &v_eval = prepareEvaluatorTo>(expr.derived()); return std::make_tuple(prepareOutput(v_eval), - evaluateOneJacobian(v_eval, targets)...); + evaluateOneJacobian(v_eval, getWrtTarget(leaf{}, targets))...); } } // namespace internal diff --git a/include/wave/geometry/src/debug/NumericalJacobian.hpp b/include/wave/geometry/src/core/functions/NumericalJacobian.hpp similarity index 82% rename from include/wave/geometry/src/debug/NumericalJacobian.hpp rename to include/wave/geometry/src/core/functions/NumericalJacobian.hpp index bbe6957..71ad06b 100644 --- a/include/wave/geometry/src/debug/NumericalJacobian.hpp +++ b/include/wave/geometry/src/core/functions/NumericalJacobian.hpp @@ -34,9 +34,9 @@ auto makeTangent(const Vector &v) { /** Evaluate an expression adding an offset along one dimension if expr === target * This helper requires the expression to already have been evaluated, yielding `value` */ -template +template inline auto evaluateWithDeltaImpl(const Derived &expr, - const TargetDerived &target, + const void *target, const plain_eval_t &value, int coeff, Scalar delta) -> plain_eval_t { @@ -55,56 +55,56 @@ inline auto evaluateWithDeltaImpl(const Derived &expr, } /** Evaluates an an expression tree, adding a small offset in one dimension to the target + * with the given address. * * Recurses down an existing Evaluator tree to save work. * * @note This implementation fully evaluates Eigen expressions at each step, in order for * the returned type to be the same whether or not expr === target. It is not as the * regular Evaluator, and is intended for testing only. - * - * Specialization for leaf expression */ -template +template struct EvaluatorWithDelta; -/** Specialization for leaf expression */ -template -struct EvaluatorWithDelta> { +/**Specialization for leaf expression +*/ +template +struct EvaluatorWithDelta> { using Scalar = scalar_t; using PlainType = plain_eval_t; PlainType operator()(const Derived &expr, - const TargetDerived &target, + const void *target, int coeff, Scalar delta) const { const auto &value = evalImpl(get_expr_tag_t{}, expr); return evaluateWithDeltaImpl(expr, target, PlainType{value}, coeff, delta); - }; + } }; /** Specialization for scalar type */ -template -struct EvaluatorWithDelta> { +template +struct EvaluatorWithDelta> { using PlainType = plain_eval_t; PlainType operator()(const Scalar &value, - const TargetDerived &target, + const void *target, int coeff, Scalar delta) const { return evaluateWithDeltaImpl(value, target, PlainType{value}, coeff, delta); - }; + } }; /** Specialization for unary expression */ -template -struct EvaluatorWithDelta> { +template +struct EvaluatorWithDelta> { using Scalar = scalar_t; - using RhsEval = EvaluatorWithDelta; + using RhsEval = EvaluatorWithDelta::RhsDerived>; using RhsValue = typename RhsEval::PlainType; using PlainExpr = typename traits::template rebind; using PlainType = plain_eval_t; PlainType operator()(const Derived &expr, - const TargetDerived &target, + const void *target, int coeff, Scalar delta) const { const auto &rhs_eval = RhsEval{}; @@ -116,17 +116,17 @@ struct EvaluatorWithDelta> { using ExprType = tmp::remove_cr_t; const auto value = PlainType{Evaluator{evaluable_expr}()}; - return evaluateWithDeltaImpl(evaluable_expr, target, value, coeff, delta); + return evaluateWithDeltaImpl(expr, target, value, coeff, delta); } }; /** Specialization for binary expression */ -template -struct EvaluatorWithDelta> { +template +struct EvaluatorWithDelta> { using Scalar = scalar_t; - using LhsEval = EvaluatorWithDelta; - using RhsEval = EvaluatorWithDelta; + using LhsEval = EvaluatorWithDelta::LhsDerived>; + using RhsEval = EvaluatorWithDelta::RhsDerived>; using LhsValue = typename LhsEval::PlainType; using RhsValue = typename RhsEval::PlainType; using PlainExpr = typename traits::template rebind; @@ -134,7 +134,7 @@ struct EvaluatorWithDelta> { PlainType operator()(const Derived &expr, - const TargetDerived &target, + const void *target, int coeff, Scalar delta) const { const auto &lhs_eval = LhsEval{}; @@ -152,7 +152,7 @@ struct EvaluatorWithDelta> { using ExprType = tmp::remove_cr_t; const auto value = PlainType{Evaluator{evaluable_expr}()}; - return evaluateWithDeltaImpl(evaluable_expr, target, value, coeff, delta); + return evaluateWithDeltaImpl(expr, target, value, coeff, delta); } }; @@ -167,6 +167,7 @@ auto evaluateNumericalJacobianImpl(const Evaluator &evaluator, auto jacobian = internal::jacobian_t{}; const auto &expr = evaluator.expr; + using ExprType = tmp::remove_cr_t; const auto &value = prepareOutput(evaluator); // @todo pick appropriate delta. Machine epsilon is not obviously appropriate since @@ -176,7 +177,7 @@ auto evaluateNumericalJacobianImpl(const Evaluator &evaluator, for (int i = 0; i < jacobian.cols(); ++i) { const auto forward_eval = - internal::EvaluatorWithDelta{}(expr, target, i, delta); + internal::EvaluatorWithDelta{}(expr, &target, i, delta); // Apply output functor (e.g. wrap in Framed) const auto forward_value = internal::prepareLeafForOutput(forward_eval); @@ -190,7 +191,6 @@ auto evaluateNumericalJacobianImpl(const Evaluator &evaluator, } // namespace internal - /** Numerically evaluate a jacobian of an expression tree. * * @warning This function is intended only for testing analytic Jacobians. It may be @@ -203,7 +203,8 @@ auto evaluateNumericalJacobian(const ExpressionBase &expr, using OutputType = internal::plain_output_t; const auto &v_eval = internal::prepareEvaluatorTo(expr.derived()); - return internal::evaluateNumericalJacobianImpl(v_eval, target); + return internal::evaluateNumericalJacobianImpl( + v_eval, getWrtTarget(internal::leaf{}, target)); } /** Numerically evaluate multiple Jacobians of am expression tree. @@ -222,7 +223,8 @@ auto evaluateNumericalJacobians(const ExpressionBase &expr, using OutputType = internal::plain_output_t; const auto &v_eval = internal::prepareEvaluatorTo(expr.derived()); - return std::make_tuple(internal::evaluateNumericalJacobianImpl(v_eval, targets)...); + return std::make_tuple(internal::evaluateNumericalJacobianImpl( + v_eval, getWrtTarget(internal::leaf{}, targets))...); } } // namespace wave diff --git a/include/wave/geometry/src/core/functions/PrepareExpr.hpp b/include/wave/geometry/src/core/functions/PrepareExpr.hpp index f3026b8..97bb82b 100644 --- a/include/wave/geometry/src/core/functions/PrepareExpr.hpp +++ b/include/wave/geometry/src/core/functions/PrepareExpr.hpp @@ -12,9 +12,6 @@ namespace internal { * Transforms the expression tree: keeps leaves intact, but converts unary and binary * expressions to their traits::PreparedType */ -template -struct PrepareExpr; - template struct PrepareExpr>> { using Leaf = tmp::remove_cr_t; @@ -25,36 +22,27 @@ struct PrepareExpr }; template -struct PrepareExpr> { - using PreparedType = typename traits::PreparedType; +struct PrepareExpr>> { + using OutType = tmp::remove_cr_t::PreparedType>; using Rhs = typename traits::RhsDerived; - template - static auto run(const T &unary) -> PreparedType { - return PreparedType{PrepareExpr::run(unary.derived().rhs())}; + static auto run(const Derived &unary) { + return OutType{PrepareExpr::run(unary.derived().rhs())}; } }; template -struct PrepareExpr> { - using PreparedType = typename traits::PreparedType; +struct PrepareExpr>> { + using OutType = tmp::remove_cr_t::PreparedType>; using Lhs = typename traits::LhsDerived; using Rhs = typename traits::RhsDerived; - template - static auto run(const T &binary) -> PreparedType { - return PreparedType{PrepareExpr::run(binary.derived().lhs()), - PrepareExpr::run(binary.derived().rhs())}; + static auto run(const Derived &binary) { + return OutType{PrepareExpr::run(binary.derived().lhs()), + PrepareExpr::run(binary.derived().rhs())}; } }; -// Unary and binary expressions: always stored by value, so dicsard && -template -struct PrepareExpr> : PrepareExpr {}; - -template -struct PrepareExpr> : PrepareExpr {}; - /** Functor which returns the given argument * To be used an OutputFunctor */ struct IdentityFunctor { diff --git a/include/wave/geometry/src/core/functions/PrepareOutput.hpp b/include/wave/geometry/src/core/functions/PrepareOutput.hpp index 1371ef7..e5818b9 100644 --- a/include/wave/geometry/src/core/functions/PrepareOutput.hpp +++ b/include/wave/geometry/src/core/functions/PrepareOutput.hpp @@ -8,6 +8,25 @@ namespace wave { namespace internal { + +/** Prepare an expression tree with the given Target, and initialize an Evaluator + * Internal implementation - does not check whether root needs conversion. + */ +template +WAVE_STRONG_INLINE auto prepareEvaluator(Derived &&expr) { + // First, transform the expression + const auto &evaluable_expr = + PrepareExpr>::run(std::forward(expr)); + using ExprType = tmp::remove_cr_t; + + // Construct Evaluator tree + return Evaluator{evaluable_expr}; + + static_assert( + std::is_same::PreparedType>>{}, + "Internal sanity check"); +} + /** Prepare an expression tree with the given Target, and initialize an Evaluator * * This function is enabled when the Destination type is already produced by the @@ -22,18 +41,8 @@ template < typename Destination, typename Derived, std::enable_if_t>>{}, int> = 0> -WAVE_STRONG_INLINE auto prepareEvaluatorTo(Derived &&expr) - -> Evaluator>::PreparedType> { - // First, transform the expression - const auto &evaluable_expr = - PrepareExpr>::run(std::forward(expr)); - using ExprType = tmp::remove_cr_t; - - static_assert(std::is_same>::PreparedType>{}, - "Internal sanity check"); - - // Construct Evaluator tree - return internal::Evaluator{evaluable_expr}; +WAVE_STRONG_INLINE auto prepareEvaluatorTo(Derived &&expr) { + return prepareEvaluator(std::forward(expr)); } /** Prepare an expression tree with the given Target, and initialize an Evaluator. @@ -49,8 +58,7 @@ template < typename Destination, typename Derived, std::enable_if_t>>{}, int> = 0> -WAVE_STRONG_INLINE auto prepareEvaluatorTo(Derived &&expr) -> Evaluator< - typename traits, arg_t>>::PreparedType> { +WAVE_STRONG_INLINE auto prepareEvaluatorTo(Derived &&expr) { // Add the needed conversion using ConvertedType = Convert, arg_t>; @@ -103,7 +111,6 @@ auto eval(const ExpressionBase &expr) -> internal::plain_output_t +template struct ReverseJacobianEvaluator; /** Helper template to get the evaluated type of an eigen matrix */ @@ -29,7 +30,8 @@ struct ReverseJacobianEvaluator std::tuple { + using JacobianTuple = std::tuple; + auto jacobian() const -> JacobianTuple { return std::forward_as_tuple(adjoint); } diff --git a/include/wave/geometry/src/core/functions/TypedJacobianEvaluator.hpp b/include/wave/geometry/src/core/functions/TypedJacobianEvaluator.hpp index 2308daf..9404c3c 100644 --- a/include/wave/geometry/src/core/functions/TypedJacobianEvaluator.hpp +++ b/include/wave/geometry/src/core/functions/TypedJacobianEvaluator.hpp @@ -49,12 +49,12 @@ struct TypedJacobianEvaluator< private: // Wrapped Evaluator and nested jacobian-evaluators const Evaluator &evaluator; - const TypedJacobianEvaluator rhs_eval; + const TypedJacobianEvaluator::RhsDerived, Target> rhs_eval; - using SelfJacobian = - decltype(jacobianImpl(get_expr_tag_t{}, - std::declval>(), - std::declval>())); + using SelfJacobian = decltype( + jacobianImpl(get_expr_tag_t{}, + std::declval>(), + std::declval::RhsDerived>>())); using RhsJacobian = decltype(rhs_eval.jacobian()); using Jacobian = decltype(std::declval() * std::declval()); @@ -87,26 +87,26 @@ template struct TypedJacobianEvaluator< Derived, Target, - std::enable_if_t::value && - !std::is_same{} && - contains_same_type::value && - contains_same_type::value>> { + std::enable_if_t< + is_binary_expression::value && !std::is_same{} && + contains_same_type::LhsDerived, Target>::value && + contains_same_type::RhsDerived, Target>::value>> { private: // Wrapped Evaluator and nested jacobian-evaluators const Evaluator &evaluator; - const TypedJacobianEvaluator lhs_eval; - const TypedJacobianEvaluator rhs_eval; - - using LhsSelfJacobian = - decltype(leftJacobianImpl(get_expr_tag_t{}, - std::declval>(), - std::declval>(), - std::declval>())); - using RhsSelfJacobian = - decltype(rightJacobianImpl(get_expr_tag_t{}, - std::declval>(), - std::declval>(), - std::declval>())); + const TypedJacobianEvaluator::LhsDerived, Target> lhs_eval; + const TypedJacobianEvaluator::RhsDerived, Target> rhs_eval; + + using LhsSelfJacobian = decltype( + leftJacobianImpl(get_expr_tag_t{}, + std::declval>(), + std::declval::LhsDerived>>(), + std::declval::RhsDerived>>())); + using RhsSelfJacobian = decltype( + rightJacobianImpl(get_expr_tag_t{}, + std::declval>(), + std::declval::LhsDerived>>(), + std::declval::RhsDerived>>())); using LhsJacobian = decltype(lhs_eval.jacobian()); using RhsJacobian = decltype(rhs_eval.jacobian()); using Jacobian = @@ -152,20 +152,20 @@ template struct TypedJacobianEvaluator< Derived, Target, - std::enable_if_t::value && - !std::is_same{} && - contains_same_type::value && - !contains_same_type::value>> { + std::enable_if_t< + is_binary_expression::value && !std::is_same{} && + contains_same_type::LhsDerived, Target>::value && + !contains_same_type::RhsDerived, Target>::value>> { private: // Wrapped Evaluator and nested jacobian-evaluators const Evaluator &evaluator; - const TypedJacobianEvaluator lhs_eval; + const TypedJacobianEvaluator::LhsDerived, Target> lhs_eval; - using LhsSelfJacobian = - decltype(leftJacobianImpl(get_expr_tag_t{}, - std::declval>(), - std::declval>(), - std::declval>())); + using LhsSelfJacobian = decltype( + leftJacobianImpl(get_expr_tag_t{}, + std::declval>(), + std::declval::LhsDerived>>(), + std::declval::RhsDerived>>())); using LhsJacobian = decltype(lhs_eval.jacobian()); using Jacobian = decltype(std::declval() * std::declval()); @@ -201,20 +201,20 @@ template struct TypedJacobianEvaluator< Derived, Target, - std::enable_if_t::value && - !std::is_same{} && - !contains_same_type::value && - contains_same_type::value>> { + std::enable_if_t< + is_binary_expression::value && !std::is_same{} && + !contains_same_type::LhsDerived, Target>::value && + contains_same_type::RhsDerived, Target>::value>> { private: // Wrapped Evaluator and nested jacobian-evaluators const Evaluator &evaluator; - const TypedJacobianEvaluator rhs_eval; + const TypedJacobianEvaluator::RhsDerived, Target> rhs_eval; - using RhsSelfJacobian = - decltype(rightJacobianImpl(get_expr_tag_t{}, - std::declval>(), - std::declval>(), - std::declval>())); + using RhsSelfJacobian = decltype( + rightJacobianImpl(get_expr_tag_t{}, + std::declval>(), + std::declval::LhsDerived>>(), + std::declval::RhsDerived>>())); using RhsJacobian = decltype(rhs_eval.jacobian()); using Jacobian = decltype(std::declval() * std::declval()); diff --git a/include/wave/geometry/src/core/storage/BinaryExpression.hpp b/include/wave/geometry/src/core/storage/BinaryExpression.hpp index c3afedf..b4fcdc6 100644 --- a/include/wave/geometry/src/core/storage/BinaryExpression.hpp +++ b/include/wave/geometry/src/core/storage/BinaryExpression.hpp @@ -16,12 +16,12 @@ struct BinaryExpression> { using LhsDerivedOrig = LhsDerived_; using RhsDerivedOrig = RhsDerived_; - // Hold a reference to leaf expressions to avoid copies, but a copy of other - // expressions to avoid references to temporaries. - using LhsStore = internal::wave_ref_sel_t; - using RhsStore = internal::wave_ref_sel_t; + // Hold a reference to each expression, unless the type is given as T&& -- then + // store it by value + using LhsStore = internal::ref_sel_t; + using RhsStore = internal::ref_sel_t; - using Derived = Tmpl_; + using Derived = Tmpl_; template BinaryExpression(LhsArg &&l, RhsArg &&r) @@ -57,7 +57,7 @@ struct BinaryExpression> { return std::move(rhs_); } - protected: + private: LhsStore lhs_; RhsStore rhs_; }; diff --git a/include/wave/geometry/src/core/storage/RefSelector.hpp b/include/wave/geometry/src/core/storage/RefSelector.hpp index 127d162..ea8b213 100644 --- a/include/wave/geometry/src/core/storage/RefSelector.hpp +++ b/include/wave/geometry/src/core/storage/RefSelector.hpp @@ -7,14 +7,41 @@ namespace wave { namespace internal { -/** Helper to choose storage type (reference or value) for expressions. +/** Chooses storage type (reference or value) for expressions. * - * T should be a wave_geometry expression or scalar type. + * The input T is expected to be the type parameter to a unary or binary expression + * template. It should be a wave_geometry expression or scalar type D with some ref + * qualifier. It is usually chosen by arg_t, below. * - * If the input T is an rvalue reference, we want to store it by value. If T is an lvalue - * reference, we store it by lvalue reference. Otherwise, we want to see whether it - * refers to a leaf or to a lightweight expression, and store it by value if the latter - * (as in Eigen). + * If T is an D&& (denoting an expression passed as a temporary), we want to + * store it by value. If T is D& we store it by lvalue reference. If T is clean (denoting + * an expression passed by lvalue reference), we store it according to its StoreByRef + * trait. + * + * Unlike Eigen, we store even lightweight expressions by reference (unless they set + * StoreByRef false (indicated by Prox below). + * + * | input | output | + * |---------|--------------| + * | Expr | const Expr& | + * | Expr& | const Expr& | + * | Expr&& | Expr | + * | Prox | const Prox& | + * | Prox& | Prox | + * | Prox&& | Prox | + */ +template +using ref_sel_t = + std::conditional_t{} || + !(std::is_lvalue_reference{} || traits::StoreByRef), + std::remove_reference_t, + const std::remove_reference_t &>; + +/** Chooses storage type (reference or value) for expressions in evaluators. + * + * This choice differs from ref_sel_t because expressions being evaluated may have been + * transformed (e.g., conversions added). Only expressions which are unchanged after + * transformation, including leaves, are stored by reference. * * | input | output | * |--------|-------------| @@ -22,18 +49,31 @@ namespace internal { * | Expr&& | Expr | * | Leaf | const Leaf& | * | Leaf&& | Leaf | + */ +template +using eval_ref_sel_t = + std::conditional_t::PreparedType>{}, + std::remove_reference_t, + const T &>; + +/** Chooses template argument for an expression template when an expression is + * passed into a function with a forwaring reference. + * + * For example, template auto inverse(T&& expr) -> Inverse> * + * | input | output | + * |--------|-------------| + * | Expr& | const Expr& | + * | Expr | Expr&& | + * + * It has the same goal as ref_sel_t, but is meant to be used in a user-facing function, + * e.g. operator+(lhs, rhs). It should either give an non-ref non-const type or an rvalue + * reference, to be used later by ref_sel_t. */ template -using wave_ref_sel_t = - std::conditional_t{}, - tmp::remove_cr_t, - std::conditional_t>::value || - is_scalar>::value, - const T &, - tmp::remove_cr_t>>; +using arg_t = std::conditional_t{}, tmp::remove_cr_t, T &&>; -/** Helper to choose cache storage type (reference or value) for Jacobian evaluators. +/** Chooses cache storage type (reference or value) for Jacobian evaluators. * * We pass the result of decltype() to this template. T should be an Eigen matrix * expression. @@ -49,25 +89,6 @@ using jac_ref_sel_t = typename Eigen::internal::ref_selector>::type, const tmp::remove_cr_t>; -/** Helper to choose template argument for an expression template when an expression is - * passed into a function with a forwaring reference. - * - * For example, template auto inverse(T&& expr) -> Inverse> - * - * If T is const Leaf &, we want Inverse - * If T is Leaf, we want Inverse - * If T is const Unary & or Unary, we want Inverse - * - * It has the same goal as wave_ref_sel_t, but is meant to be used in a user-facing - * function, e.g. operator+(lhs, rhs). It should either give an non-ref non-const type or - * an rvalue reference, to be used later by wave_ref_sel_t. - */ -template -using arg_t = std::conditional_t< - is_leaf_expression>{}, - std::conditional_t{}, tmp::remove_cr_t, T &&>, - tmp::remove_cr_t>; - } // namespace internal } // namespace wave diff --git a/include/wave/geometry/src/core/storage/UnaryExpression.hpp b/include/wave/geometry/src/core/storage/UnaryExpression.hpp index 334610a..606beb5 100644 --- a/include/wave/geometry/src/core/storage/UnaryExpression.hpp +++ b/include/wave/geometry/src/core/storage/UnaryExpression.hpp @@ -11,11 +11,10 @@ template struct UnaryExpressionBase { using Derived = Derived_; using RhsDerived = tmp::remove_cr_t; - using RhsDerivedOrig = RhsDerived_; // Hold a reference to leaf expressions to avoid copies, but a copy of other // expressions to avoid references to temporaries. - using RhsStore = internal::wave_ref_sel_t; + using RhsStore = internal::ref_sel_t; // Forward args to rhs (version for one argument) // Disable if copy ctor would apply - https://stackoverflow.com/a/39646176 diff --git a/include/wave/geometry/src/core/traits/traits_bases.hpp b/include/wave/geometry/src/core/traits/traits_bases.hpp index 2e27589..609a2b3 100644 --- a/include/wave/geometry/src/core/traits/traits_bases.hpp +++ b/include/wave/geometry/src/core/traits/traits_bases.hpp @@ -42,7 +42,7 @@ struct nullary_traits_base> : traits { public: // A tag for calling evalImpl() using Tag = internal::expr>; - using PreparedType = Derived; + using PreparedType = Derived &; using EvalType = eval_t_unary>; using UniqueLeaves = has_unique_leaves_leaf; }; @@ -50,12 +50,13 @@ struct nullary_traits_base> : traits { template struct leaf_traits_base { using Tag = leaf; - using PreparedType = T; + using PreparedType = T &; using EvalType = T; using OutputFunctor = IdentityFunctor; using PlainType = T; using UniqueLeaves = has_unique_leaves_leaf; using ConvertTo = tmp::type_list<>; + static constexpr bool StoreByRef = true; }; template