From c90e6336406b09ea18b45bf540f59a30e4d2686a Mon Sep 17 00:00:00 2001 From: Leo Koppel Date: Sat, 20 Oct 2018 02:11:53 -0400 Subject: [PATCH] Add dynamic expressions (squashed commit) Add dynamic expressions. - Add Proxy, Dynamic, RefProxy, and related classes in `dynamic` module. - Add tests and benchmaks - Add page to docs - Add dynamic forward and reverse Jacobian evaluators - Add MatrixMap container for returning Jacobians Change storage. Expressions hold references to other expressions if their StoreByRef trait is true (leaves are no longer special), unless they were received as rvalues. - simplify arg_t and wave_ref_sel_t: store lightweight expressions by ref, unless T&& - Consistently use traits::RhsDerived instead of T::RhsDerived - Make traits::RhsDerived include the && - Rework PrepareExpr (no recursion if returning a reference) - PreparedType trait is now T& for "no change / PrepareExpr::run returns reference" and T&& for "it is a new temporary expression" (e.g. for Convert) Minor fixes. Update some benchmarks and CMake. --- CHANGELOG.md | 5 +- benchmarks/CMakeLists.txt | 1 - benchmarks/bechmark_helpers.hpp | 5 +- benchmarks/rotate_chain/CMakeLists.txt | 20 +- .../rotate_chain/rotate_chain_gtsam_bench.cpp | 291 ++++-------------- .../rotate_chain/rotate_chain_hand_bench.cpp | 2 +- .../rotate_chain/rotate_chain_wave_bench.cpp | 2 +- .../rotate_chain_wave_dynamic_bench.cpp | 68 ++++ .../rotate_chain_wave_reverse_bench.cpp | 2 +- .../rotate_chain_wave_untyped_bench.cpp | 2 +- benchmarks/util_cross_matrix_bench.cpp | 2 +- docs/dynamic_expressions.md | 73 +++++ docs/index.rst | 1 + docs/install.md | 2 +- include/wave/geometry/core.hpp | 18 +- include/wave/geometry/debug.hpp | 1 - include/wave/geometry/dynamic.hpp | 31 ++ .../src/core/forward_declarations.hpp | 7 +- .../src/core/functions/AddConversions.hpp | 8 +- .../functions/DynamicJacobianEvaluator.hpp | 239 ++++++++++++++ .../DynamicReverseJacobianEvaluator.hpp | 263 ++++++++++++++++ .../geometry/src/core/functions/Evaluator.hpp | 26 +- .../geometry/src/core/functions/IsSame.hpp | 12 +- .../src/core/functions/IsSameType.hpp | 6 +- .../src/core/functions/JacobianEvaluator.hpp | 73 +++-- .../functions}/NumericalJacobian.hpp | 60 ++-- .../src/core/functions/PrepareExpr.hpp | 30 +- .../src/core/functions/PrepareOutput.hpp | 37 ++- .../functions/ReverseJacobianEvaluator.hpp | 10 +- .../core/functions/TypedJacobianEvaluator.hpp | 84 ++--- .../src/core/storage/BinaryExpression.hpp | 12 +- .../geometry/src/core/storage/RefSelector.hpp | 87 ++++-- .../src/core/storage/UnaryExpression.hpp | 3 +- .../geometry/src/core/traits/traits_bases.hpp | 19 +- .../geometry/src/core/traits/type_traits.hpp | 35 ++- include/wave/geometry/src/dynamic/Dynamic.hpp | 176 +++++++++++ .../wave/geometry/src/dynamic/DynamicBase.hpp | 122 ++++++++ include/wave/geometry/src/dynamic/Proxy.hpp | 256 +++++++++++++++ .../wave/geometry/src/dynamic/RefProxy.hpp | 107 +++++++ .../src/geometry/base/RigidTransformBase.hpp | 2 +- .../src/geometry/base/RotationBase.hpp | 2 +- .../geometry/src/geometry/base/ScalarBase.hpp | 2 +- .../geometry/src/geometry/base/VectorBase.hpp | 8 +- .../geometry/src/geometry/frames/Framed.hpp | 2 +- .../src/geometry/frames/frame_cast.hpp | 5 +- .../src/geometry/frames/frame_traits.hpp | 2 +- .../geometry/src/geometry/leaf/Identity.hpp | 2 +- .../geometry/src/geometry/leaf/Scalar.hpp | 2 +- .../geometry/src/geometry/leaf/ScalarRef.hpp | 2 +- .../wave/geometry/src/geometry/leaf/Zero.hpp | 2 +- .../geometry/src/geometry/op/BoxMinus.hpp | 14 +- .../wave/geometry/src/geometry/op/BoxPlus.hpp | 6 +- .../geometry/src/util/math/CrossMatrix.hpp | 8 +- .../geometry/src/util/math/IdentityMatrix.hpp | 8 +- .../wave/geometry/src/util/math/MatrixMap.hpp | 107 +++++++ test/CMakeLists.txt | 3 + test/dynamic_expression_test.cpp | 206 +++++++++++++ test/rotation_test.cpp | 5 +- test/rvalue_expression_test.cpp | 18 +- test/test.hpp | 74 ++++- test/util/identity_matrix_test.cpp | 3 +- 61 files changed, 2164 insertions(+), 517 deletions(-) create mode 100644 benchmarks/rotate_chain/rotate_chain_wave_dynamic_bench.cpp create mode 100644 docs/dynamic_expressions.md create mode 100644 include/wave/geometry/dynamic.hpp create mode 100644 include/wave/geometry/src/core/functions/DynamicJacobianEvaluator.hpp create mode 100644 include/wave/geometry/src/core/functions/DynamicReverseJacobianEvaluator.hpp rename include/wave/geometry/src/{debug => core/functions}/NumericalJacobian.hpp (82%) create mode 100644 include/wave/geometry/src/dynamic/Dynamic.hpp create mode 100644 include/wave/geometry/src/dynamic/DynamicBase.hpp create mode 100644 include/wave/geometry/src/dynamic/Proxy.hpp create mode 100644 include/wave/geometry/src/dynamic/RefProxy.hpp create mode 100644 include/wave/geometry/src/util/math/MatrixMap.hpp create mode 100644 test/dynamic_expression_test.cpp 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