From 57291e132faf3b395e9edbd04c4b3daaa249f8c5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 19 Jul 2023 13:09:13 +0200 Subject: [PATCH 01/26] added examples and a test on pMin --- gtsam/sfm/ShonanAveraging.cpp | 3 +++ gtsam/sfm/tests/testShonanAveraging.cpp | 14 ++++++++++++++ python/gtsam/tests/test_ShonanAveraging.py | 19 +++++++++++++++++-- 3 files changed, 34 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 8ace9d98c..7c8b07f37 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -894,6 +894,9 @@ template std::pair ShonanAveraging::run(const Values &initialEstimate, size_t pMin, size_t pMax) const { + if (pMin < d) { + throw std::runtime_error("pMin is smaller than the base dimension d"); + } Values Qstar; Values initialSOp = LiftTo(pMin, initialEstimate); // lift to pMin! for (size_t p = pMin; p <= pMax; p++) { diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index d398a2a87..dd4759daa 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -415,6 +415,20 @@ TEST(ShonanAveraging3, PriorWeights) { auto result = shonan.run(initial, 3, 3); EXPECT_DOUBLES_EQUAL(0.0015, shonan.cost(result.first), 1e-4); } + +/* ************************************************************************* */ +// Check a small graph created using binary measurements +TEST(ShonanAveraging3, BinaryMeasurements) { + std::vector> measurements; + auto unit3 = noiseModel::Unit::Create(3); + measurements.emplace_back(0, 1, Rot3::Yaw(M_PI_2), unit3); + measurements.emplace_back(1, 2, Rot3::Yaw(M_PI_2), unit3); + ShonanAveraging3 shonan(measurements); + Values initial = shonan.initializeRandomly(); + auto result = shonan.run(initial, 3, 5); + EXPECT_DOUBLES_EQUAL(0.0, shonan.cost(result.first), 1e-4); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 845f6cf1c..fc0943772 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -10,14 +10,16 @@ Author: Frank Dellaert """ # pylint: disable=invalid-name, no-name-in-module, no-member +import math import unittest import numpy as np from gtsam.utils.test_case import GtsamTestCase import gtsam -from gtsam import (BetweenFactorPose2, LevenbergMarquardtParams, Pose2, Rot2, - ShonanAveraging2, ShonanAveraging3, +from gtsam import (BetweenFactorPose2, BetweenFactorPose3, + BinaryMeasurementRot3, LevenbergMarquardtParams, Pose2, + Pose3, Rot2, Rot3, ShonanAveraging2, ShonanAveraging3, ShonanAveragingParameters2, ShonanAveragingParameters3) DEFAULT_PARAMS = ShonanAveragingParameters3( @@ -197,6 +199,19 @@ class TestShonanAveraging(GtsamTestCase): expected_thetas_deg = np.array([0.0, 90.0, 0.0]) np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1) + def test_measurements3(self): + """Create from Measurements.""" + measurements = [] + unit3 = gtsam.noiseModel.Unit.Create(3) + m01 = BinaryMeasurementRot3(0, 1, Rot3.Yaw(math.radians(90)), unit3) + m12 = BinaryMeasurementRot3(1, 2, Rot3.Yaw(math.radians(90)), unit3) + measurements.append(m01) + measurements.append(m12) + obj = ShonanAveraging3(measurements) + self.assertIsInstance(obj, ShonanAveraging3) + initial = obj.initializeRandomly() + _, cost = obj.run(initial, min_p=3, max_p=5) + self.assertAlmostEqual(cost, 0) if __name__ == "__main__": unittest.main() From 62d59c62d4497b76d07f736e900f897845e8d6b3 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Thu, 20 Jul 2023 08:03:42 +0300 Subject: [PATCH 02/26] Fix windows test --- .github/workflows/build-windows.yml | 78 ++++++++++++++----- CMakeLists.txt | 8 ++ gtsam/basis/Basis.h | 2 +- gtsam/discrete/SignatureParser.h | 4 +- gtsam/geometry/Line3.h | 2 +- gtsam/geometry/Pose2.h | 40 +++++----- gtsam/geometry/Pose3.h | 2 +- gtsam/hybrid/HybridFactorGraph.h | 2 +- gtsam/hybrid/HybridSmoother.h | 2 +- .../tests/testHybridGaussianFactorGraph.cpp | 1 + gtsam/inference/Ordering.h | 21 +++-- gtsam_unstable/linear/LPInitSolver.h | 3 +- gtsam_unstable/linear/QPSParser.h | 3 +- 13 files changed, 108 insertions(+), 60 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index cbe0c10f1..565a02756 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -99,26 +99,64 @@ jobs: cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" - name: Build + shell: cmd run: | - # Since Visual Studio is a multi-generator, we need to use --config - # https://stackoverflow.com/a/24470998/1236990 - cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam - cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam_unstable - cmake --build build -j 4 --config ${{ matrix.build_type }} --target wrap + :: Since Visual Studio is a multi-generator, we need to use --config + :: https://stackoverflow.com/a/24470998/1236990 + cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam + cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam_unstable - # Run GTSAM tests - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.basis - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.discrete - #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.geometry - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.inference - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.linear - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.navigation - #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.nonlinear - #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sam - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sfm - #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.slam - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.symbolic + :: Target doesn't exist + :: cmake --build build -j4 --config ${{ matrix.build_type }} --target wrap - # Run GTSAM_UNSTABLE tests - #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable + - name: Test + shell: cmd + run: | + :: Run GTSAM tests + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.basis + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.inference + + :: Compile. Fail with exception + :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear + + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.navigation + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sam + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic + + :: Compile. Fail with exception + :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid + + :: Compile. Fail with exception + :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear + + :: Compilation error + :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam + + + :: Run GTSAM_UNSTABLE tests + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable + + :: Compile. Fail with exception + :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable + + :: Compile. Fail with exception + :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable + + :: Compile. Fail with exception + :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable + + :: Compile. Fail with exception + :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable + + :: Compile. Fail with exception + :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable + + :: Compilation error + :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable + + :: Compilation error + :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition diff --git a/CMakeLists.txt b/CMakeLists.txt index ebe27443a..8b56ea9df 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,6 +32,14 @@ set (CMAKE_PROJECT_VERSION_PATCH ${GTSAM_VERSION_PATCH}) ############################################################################### # Gather information, perform checks, set defaults +if(MSVC) + set(MSVC_LINKER_FLAGS "/FORCE:MULTIPLE") + set(CMAKE_EXE_LINKER_FLAGS ${MSVC_LINKER_FLAGS}) + set(CMAKE_MODULE_LINKER_FLAGS ${MSVC_LINKER_FLAGS}) + set(CMAKE_SHARED_LINKER_FLAGS ${MSVC_LINKER_FLAGS}) + set(CMAKE_STATIC_LINKER_FLAGS ${MSVC_LINKER_FLAGS}) +endif() + set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake") include(GtsamMakeConfigFile) include(GNUInstallDirs) diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index 41cdeeaaa..0b2b3606b 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -80,7 +80,7 @@ using Weights = Eigen::Matrix; /* 1xN vector */ * * @ingroup basis */ -Matrix kroneckerProductIdentity(size_t M, const Weights& w); +Matrix GTSAM_EXPORT kroneckerProductIdentity(size_t M, const Weights& w); /** * CRTP Base class for function bases diff --git a/gtsam/discrete/SignatureParser.h b/gtsam/discrete/SignatureParser.h index e6b402e44..ddc548fc6 100644 --- a/gtsam/discrete/SignatureParser.h +++ b/gtsam/discrete/SignatureParser.h @@ -22,6 +22,8 @@ #include #include +#include + namespace gtsam { /** * @brief A simple parser that replaces the boost spirit parser. @@ -47,7 +49,7 @@ namespace gtsam { * * Also fails if the rows are not of the same size. */ -struct SignatureParser { +struct GTSAM_EXPORT SignatureParser { using Row = std::vector; using Table = std::vector; diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h index b1a1e4f6e..def49d268 100644 --- a/gtsam/geometry/Line3.h +++ b/gtsam/geometry/Line3.h @@ -146,7 +146,7 @@ class GTSAM_EXPORT Line3 { * @param Dline - OptionalJacobian of transformed line with respect to l * @return Transformed line in camera frame */ - friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL, + GTSAM_EXPORT friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian<4, 6> Dpose, OptionalJacobian<4, 4> Dline); }; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index f1b38c5a6..aad81493f 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -36,7 +36,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ -class Pose2: public LieGroup { +class GTSAM_EXPORT Pose2: public LieGroup { public: @@ -112,10 +112,10 @@ public: /// @{ /** print with optional string */ - GTSAM_EXPORT void print(const std::string& s = "") const; + void print(const std::string& s = "") const; /** assert equality up to a tolerance */ - GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const; + bool equals(const Pose2& pose, double tol = 1e-9) const; /// @} /// @name Group @@ -125,7 +125,7 @@ public: inline static Pose2 Identity() { return Pose2(); } /// inverse - GTSAM_EXPORT Pose2 inverse() const; + Pose2 inverse() const; /// compose syntactic sugar inline Pose2 operator*(const Pose2& p2) const { @@ -137,16 +137,16 @@ public: /// @{ ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ - GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {}); + static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {}); ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation - GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = {}); + static Vector3 Logmap(const Pose2& p, ChartJacobian H = {}); /** * Calculate Adjoint map * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ - GTSAM_EXPORT Matrix3 AdjointMap() const; + Matrix3 AdjointMap() const; /// Apply AdjointMap to twist xi inline Vector3 Adjoint(const Vector3& xi) const { @@ -156,7 +156,7 @@ public: /** * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19 */ - GTSAM_EXPORT static Matrix3 adjointMap(const Vector3& v); + static Matrix3 adjointMap(const Vector3& v); /** * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives @@ -192,15 +192,15 @@ public: } /// Derivative of Expmap - GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v); + static Matrix3 ExpmapDerivative(const Vector3& v); /// Derivative of Logmap - GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v); + static Matrix3 LogmapDerivative(const Pose2& v); // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP - struct ChartAtOrigin { - GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {}); - GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {}); + struct GTSAM_EXPORT ChartAtOrigin { + static Pose2 Retract(const Vector3& v, ChartJacobian H = {}); + static Vector3 Local(const Pose2& r, ChartJacobian H = {}); }; using LieGroup::inverse; // version with derivative @@ -210,7 +210,7 @@ public: /// @{ /** Return point coordinates in pose coordinate frame */ - GTSAM_EXPORT Point2 transformTo(const Point2& point, + Point2 transformTo(const Point2& point, OptionalJacobian<2, 3> Dpose = {}, OptionalJacobian<2, 2> Dpoint = {}) const; @@ -222,7 +222,7 @@ public: Matrix transformTo(const Matrix& points) const; /** Return point coordinates in global frame */ - GTSAM_EXPORT Point2 transformFrom(const Point2& point, + Point2 transformFrom(const Point2& point, OptionalJacobian<2, 3> Dpose = {}, OptionalJacobian<2, 2> Dpoint = {}) const; @@ -273,14 +273,14 @@ public: } //// return transformation matrix - GTSAM_EXPORT Matrix3 matrix() const; + Matrix3 matrix() const; /** * Calculate bearing to a landmark * @param point 2D location of landmark * @return 2D rotation \f$ \in SO(2) \f$ */ - GTSAM_EXPORT Rot2 bearing(const Point2& point, + Rot2 bearing(const Point2& point, OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const; /** @@ -288,7 +288,7 @@ public: * @param point SO(2) location of other pose * @return 2D rotation \f$ \in SO(2) \f$ */ - GTSAM_EXPORT Rot2 bearing(const Pose2& pose, + Rot2 bearing(const Pose2& pose, OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const; /** @@ -296,7 +296,7 @@ public: * @param point 2D location of landmark * @return range (double) */ - GTSAM_EXPORT double range(const Point2& point, + double range(const Point2& point, OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const; @@ -305,7 +305,7 @@ public: * @param point 2D location of other pose * @return range (double) */ - GTSAM_EXPORT double range(const Pose2& point, + double range(const Pose2& point, OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 6c91d7468..8a807cc23 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -204,7 +204,7 @@ public: static Matrix6 LogmapDerivative(const Pose3& xi); // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP - struct ChartAtOrigin { + struct GTSAM_EXPORT ChartAtOrigin { static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {}); static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {}); }; diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index b8e7ff588..8b59fd4f9 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -35,7 +35,7 @@ using SharedFactor = std::shared_ptr; * Hybrid Factor Graph * Factor graph with utilities for hybrid factors. */ -class HybridFactorGraph : public FactorGraph { +class GTSAM_EXPORT HybridFactorGraph : public FactorGraph { public: using Base = FactorGraph; using This = HybridFactorGraph; ///< this class diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index 0767da12f..5c2e4f546 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -24,7 +24,7 @@ namespace gtsam { -class HybridSmoother { +class GTSAM_EXPORT HybridSmoother { private: HybridBayesNet hybridBayesNet_; HybridGaussianFactorGraph remainingFactorGraph_; diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 1da897103..df38c171e 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include "Switching.h" #include "TinyHybridExample.h" diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 86d44e072..c3df1b8a0 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -30,7 +30,7 @@ namespace gtsam { -class Ordering: public KeyVector { +class GTSAM_EXPORT Ordering: public KeyVector { protected: typedef KeyVector Base; @@ -44,8 +44,7 @@ public: typedef Ordering This; ///< Typedef to this class typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class - /// Create an empty ordering - GTSAM_EXPORT + /// Create an empty ordering Ordering() { } @@ -99,7 +98,7 @@ public: } /// Compute a fill-reducing ordering using COLAMD from a VariableIndex. - static GTSAM_EXPORT Ordering Colamd(const VariableIndex& variableIndex); + static Ordering Colamd(const VariableIndex& variableIndex); /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details /// for note on performance). This internally builds a VariableIndex so if you already have a @@ -124,7 +123,7 @@ public: /// variables in \c constrainLast will be ordered in the same order specified in the KeyVector /// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. - static GTSAM_EXPORT Ordering ColamdConstrainedLast( + static Ordering ColamdConstrainedLast( const VariableIndex& variableIndex, const KeyVector& constrainLast, bool forceOrder = false); @@ -152,7 +151,7 @@ public: /// KeyVector \c constrainFirst. If \c forceOrder is false, the variables in \c /// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to /// reduce fill-in as well. - static GTSAM_EXPORT Ordering ColamdConstrainedFirst( + static Ordering ColamdConstrainedFirst( const VariableIndex& variableIndex, const KeyVector& constrainFirst, bool forceOrder = false); @@ -181,7 +180,7 @@ public: /// appear in \c groups in arbitrary order. Any variables not present in \c groups will be /// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the /// supplied indices, see the CCOLAMD documentation for more information. - static GTSAM_EXPORT Ordering ColamdConstrained( + static Ordering ColamdConstrained( const VariableIndex& variableIndex, const FastMap& groups); /// Return a natural Ordering. Typically used by iterative solvers @@ -195,11 +194,11 @@ public: /// METIS Formatting function template - static GTSAM_EXPORT void CSRFormat(std::vector& xadj, + static void CSRFormat(std::vector& xadj, std::vector& adj, const FACTOR_GRAPH& graph); /// Compute an ordering determined by METIS from a VariableIndex - static GTSAM_EXPORT Ordering Metis(const MetisIndex& met); + static Ordering Metis(const MetisIndex& met); template static Ordering Metis(const FACTOR_GRAPH& graph) { @@ -241,18 +240,16 @@ public: /// @name Testable /// @{ - GTSAM_EXPORT void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - GTSAM_EXPORT bool equals(const Ordering& other, double tol = 1e-9) const; /// @} private: /// Internal COLAMD function - static GTSAM_EXPORT Ordering ColamdConstrained( + static Ordering ColamdConstrained( const VariableIndex& variableIndex, std::vector& cmember); #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION diff --git a/gtsam_unstable/linear/LPInitSolver.h b/gtsam_unstable/linear/LPInitSolver.h index 7e326117b..9db2a34f0 100644 --- a/gtsam_unstable/linear/LPInitSolver.h +++ b/gtsam_unstable/linear/LPInitSolver.h @@ -19,6 +19,7 @@ #pragma once +#include #include #include @@ -49,7 +50,7 @@ namespace gtsam { * inequality constraint, we can't conclude that the problem is infeasible. * However, whether it is infeasible or unbounded, we don't have a unique solution anyway. */ -class LPInitSolver { +class GTSAM_UNSTABLE_EXPORT LPInitSolver { private: const LP& lp_; diff --git a/gtsam_unstable/linear/QPSParser.h b/gtsam_unstable/linear/QPSParser.h index bd4254d0f..e751f34d2 100644 --- a/gtsam_unstable/linear/QPSParser.h +++ b/gtsam_unstable/linear/QPSParser.h @@ -18,12 +18,13 @@ #pragma once +#include #include #include namespace gtsam { -class QPSParser { +class GTSAM_UNSTABLE_EXPORT QPSParser { private: std::string fileName_; From 0443bcd11e50e8a3b1e1dc33732a60565aebb6db Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Thu, 20 Jul 2023 15:58:31 +0000 Subject: [PATCH 03/26] fixes issues with TBB not being found when building downstream packages --- cmake/Config.cmake.in | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/cmake/Config.cmake.in b/cmake/Config.cmake.in index cc2a7df8f..36906d090 100644 --- a/cmake/Config.cmake.in +++ b/cmake/Config.cmake.in @@ -21,6 +21,10 @@ else() find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@) endif() +if(@GTSAM_USE_TBB@) + find_dependency(TBB 4.4 COMPONENTS tbb tbbmalloc) +endif() + if(@GTSAM_USE_SYSTEM_EIGEN@) find_dependency(Eigen3 REQUIRED) endif() From 0fc662399aa1c30b7ac9d2c8875791b65eeadc97 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Jul 2023 10:12:40 +0200 Subject: [PATCH 04/26] Add a test to debug issue #1588 --- gtsam/geometry/tests/testRot3.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 49b978567..1232348f0 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -597,6 +597,25 @@ TEST(Rot3, quaternion) { EXPECT(assert_equal(expected2, actual2)); } +/* ************************************************************************* */ +TEST(Rot3, ConvertQuaternion) { + Eigen::Quaterniond eigenQuaternion; + eigenQuaternion.w() = 1.0; + eigenQuaternion.x() = 2.0; + eigenQuaternion.y() = 3.0; + eigenQuaternion.z() = 4.0; + EXPECT_DOUBLES_EQUAL(1, eigenQuaternion.w(), 1e-9); + EXPECT_DOUBLES_EQUAL(2, eigenQuaternion.x(), 1e-9); + EXPECT_DOUBLES_EQUAL(3, eigenQuaternion.y(), 1e-9); + EXPECT_DOUBLES_EQUAL(4, eigenQuaternion.z(), 1e-9); + + Rot3 R(eigenQuaternion); + EXPECT_DOUBLES_EQUAL(1, R.toQuaternion().w(), 1e-9); + EXPECT_DOUBLES_EQUAL(2, R.toQuaternion().x(), 1e-9); + EXPECT_DOUBLES_EQUAL(3, R.toQuaternion().y(), 1e-9); + EXPECT_DOUBLES_EQUAL(4, R.toQuaternion().z(), 1e-9); +} + /* ************************************************************************* */ Matrix Cayley(const Matrix& A) { Matrix::Index n = A.cols(); From 47f39085bc12d556f547f564b591b33e5b5d4f3c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 3 Jul 2023 16:40:27 -0400 Subject: [PATCH 05/26] small improvements --- gtsam/discrete/DecisionTreeFactor.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 6cce6e5d4..e90a2f96f 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -147,6 +147,9 @@ namespace gtsam { /// @name Advanced Interface /// @{ + /// Inherit all the `apply` methods from AlgebraicDecisionTree + using ADT::apply; + /** * Apply binary operator (*this) "op" f * @param f the second argument for op From baf25de68446370575acbd3b3466d1f55fc40a9c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 8 Jul 2023 13:09:35 -0400 Subject: [PATCH 06/26] initial changes --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index e14023edd..10b8de797 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -274,7 +274,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, DecisionTree probabilities(eliminationResults, probability); return { std::make_shared(gaussianMixture), - std::make_shared(discreteSeparator, probabilities)}; + std::make_shared(discreteSeparator, dtf->probabilities())}; } else { // Otherwise, we create a resulting GaussianMixtureFactor on the separator, // taking care to correct for conditional constant. From 9c88e3ed96184866217b2ecc074604b9a328d3a8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 13 Jul 2023 16:39:55 -0400 Subject: [PATCH 07/26] Use TableFactor in hybrid elimination --- gtsam/hybrid/GaussianMixture.cpp | 6 ++---- gtsam/hybrid/GaussianMixture.h | 5 +++-- gtsam/hybrid/HybridBayesNet.cpp | 2 +- gtsam/hybrid/HybridBayesNet.h | 1 - gtsam/hybrid/HybridBayesTree.cpp | 28 +++++++++++++++------------- 5 files changed, 21 insertions(+), 21 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 659d44423..79c385262 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -234,7 +234,7 @@ std::set DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) { */ std::function &, const GaussianConditional::shared_ptr &)> -GaussianMixture::prunerFunc(const DecisionTreeFactor &discreteProbs) { +GaussianMixture::prunerFunc(const TableFactor &discreteProbs) { // Get the discrete keys as sets for the decision tree // and the gaussian mixture. auto discreteProbsKeySet = DiscreteKeysAsSet(discreteProbs.discreteKeys()); @@ -285,9 +285,7 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &discreteProbs) { } /* *******************************************************************************/ -void GaussianMixture::prune(const DecisionTreeFactor &discreteProbs) { - auto discreteProbsKeySet = DiscreteKeysAsSet(discreteProbs.discreteKeys()); - auto gmKeySet = DiscreteKeysAsSet(this->discreteKeys()); +void GaussianMixture::prune(const TableFactor &discreteProbs) { // Functional which loops over all assignments and create a set of // GaussianConditionals auto pruner = prunerFunc(discreteProbs); diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 0b68fcfd0..c193d065c 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -80,7 +81,7 @@ class GTSAM_EXPORT GaussianMixture */ std::function &, const GaussianConditional::shared_ptr &)> - prunerFunc(const DecisionTreeFactor &discreteProbs); + prunerFunc(const TableFactor &discreteProbs); public: /// @name Constructors @@ -238,7 +239,7 @@ class GTSAM_EXPORT GaussianMixture * * @param discreteProbs A pruned set of probabilities for the discrete keys. */ - void prune(const DecisionTreeFactor &discreteProbs); + void prune(const TableFactor &discreteProbs); /** * @brief Merge the Gaussian Factor Graphs in `this` and `sum` while diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 266e02b0d..302ccfd6c 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -64,7 +64,7 @@ DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const { * @return std::function &, double)> */ std::function &, double)> prunerFunc( - const DecisionTreeFactor &prunedDiscreteProbs, + const TableFactor &prunedDiscreteProbs, const HybridConditional &conditional) { // Get the discrete keys as sets for the decision tree // and the Gaussian mixture. diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 23fc4d5d3..a71768bac 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -17,7 +17,6 @@ #pragma once -#include #include #include #include diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index ae8fa0378..d71760b9a 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -175,14 +175,15 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { void HybridBayesTree::prune(const size_t maxNrLeaves) { auto discreteProbs = this->roots_.at(0)->conditional()->asDiscrete(); - DecisionTreeFactor prunedDiscreteProbs = discreteProbs->prune(maxNrLeaves); - discreteProbs->root_ = prunedDiscreteProbs.root_; + // TODO(Varun) + // TableFactor prunedDiscreteProbs = discreteProbs->prune(maxNrLeaves); + // discreteProbs->root_ = prunedDiscreteProbs.root_; /// Helper struct for pruning the hybrid bayes tree. struct HybridPrunerData { /// The discrete decision tree after pruning. - DecisionTreeFactor prunedDiscreteProbs; - HybridPrunerData(const DecisionTreeFactor& prunedDiscreteProbs, + TableFactor prunedDiscreteProbs; + HybridPrunerData(const TableFactor& prunedDiscreteProbs, const HybridBayesTree::sharedNode& parentClique) : prunedDiscreteProbs(prunedDiscreteProbs) {} @@ -210,15 +211,16 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) { } }; - HybridPrunerData rootData(prunedDiscreteProbs, 0); - { - treeTraversal::no_op visitorPost; - // Limits OpenMP threads since we're mixing TBB and OpenMP - TbbOpenMPMixedScope threadLimiter; - treeTraversal::DepthFirstForestParallel( - *this, rootData, HybridPrunerData::AssignmentPreOrderVisitor, - visitorPost); - } + // TODO(Varun) + // HybridPrunerData rootData(prunedDiscreteProbs, 0); + // { + // treeTraversal::no_op visitorPost; + // // Limits OpenMP threads since we're mixing TBB and OpenMP + // TbbOpenMPMixedScope threadLimiter; + // treeTraversal::DepthFirstForestParallel( + // *this, rootData, HybridPrunerData::AssignmentPreOrderVisitor, + // visitorPost); + // } } } // namespace gtsam From f238ba55d23d2c7ed80e9baa9ff180f53a070513 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 16 Jul 2023 22:04:09 -0400 Subject: [PATCH 08/26] TableFactor constructor from DecisionTreeFactor and AlgebraicDecisionTree --- gtsam/discrete/TableFactor.cpp | 10 ++++++++++ gtsam/discrete/TableFactor.h | 8 +++++++- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 74eb3ddb3..9a0520a34 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -56,6 +56,16 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys, sort(sorted_dkeys_.begin(), sorted_dkeys_.end()); } +/* ************************************************************************ */ +TableFactor::TableFactor(const DiscreteKeys& dkeys, + const DecisionTreeFactor& dtf) + : TableFactor(dkeys, dtf.probabilities()) {} + +/* ************************************************************************ */ +TableFactor::TableFactor(const DiscreteKeys& dkeys, + const DecisionTree& dtree) + : TableFactor(dkeys, DecisionTreeFactor(dkeys, dtree)) {} + /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteConditional& c) : TableFactor(c.discreteKeys(), c.probabilities()) {} diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index bd637bb7d..103e14bff 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -141,6 +141,12 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { TableFactor(const DiscreteKey& key, const std::vector& row) : TableFactor(DiscreteKeys{key}, row) {} + /// Constructor from DecisionTreeFactor + TableFactor(const DiscreteKeys& keys, const DecisionTreeFactor& dtf); + + /// Constructor from DecisionTree/AlgebraicDecisionTree + TableFactor(const DiscreteKeys& keys, const DecisionTree& dtree); + /** Construct from a DiscreteConditional type */ explicit TableFactor(const DiscreteConditional& c); @@ -177,7 +183,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { return apply(f, Ring::mul); }; - /// multiple with DecisionTreeFactor + /// multiply with DecisionTreeFactor DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; static double safe_div(const double& a, const double& b); From a581788bffa22cfbe307a0471aa9eb31308fc01b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 16 Jul 2023 22:04:48 -0400 Subject: [PATCH 09/26] simplify return --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 10b8de797..dc11cedb0 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -272,9 +272,9 @@ hybridElimination(const HybridGaussianFactorGraph &factors, }; DecisionTree probabilities(eliminationResults, probability); - return { - std::make_shared(gaussianMixture), - std::make_shared(discreteSeparator, dtf->probabilities())}; + + return {std::make_shared(gaussianMixture), + std::make_shared(discreteSeparator, probabilities)}; } else { // Otherwise, we create a resulting GaussianMixtureFactor on the separator, // taking care to correct for conditional constant. From 8462624c572d9d0fd8ac588d60a8a2fdc8a6f9e6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 3 Jul 2023 08:50:36 -0400 Subject: [PATCH 10/26] update HybridFactorGraph wrapper --- gtsam/hybrid/hybrid.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 7cc93d4b0..5e0e6dca0 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -179,6 +179,7 @@ class HybridGaussianFactorGraph { void push_back(const gtsam::HybridBayesTree& bayesTree); void push_back(const gtsam::GaussianMixtureFactor* gmm); void push_back(gtsam::DecisionTreeFactor* factor); + void push_back(gtsam::TableFactor* factor); void push_back(gtsam::JacobianFactor* factor); bool empty() const; From c8e9a57cacc8cd5465569f9848e4c1c762e785b6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 11 Jul 2023 21:12:05 -0400 Subject: [PATCH 11/26] unary apply methods for TableFactor --- gtsam/discrete/TableFactor.cpp | 41 +++++++++++++++++++++++- gtsam/discrete/TableFactor.h | 28 ++++++++++++++-- gtsam/discrete/tests/testTableFactor.cpp | 36 +++++++++++++++++++-- 3 files changed, 100 insertions(+), 5 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 9a0520a34..1b265d14f 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -74,7 +74,7 @@ TableFactor::TableFactor(const DiscreteConditional& c) Eigen::SparseVector TableFactor::Convert( const std::vector& table) { Eigen::SparseVector sparse_table(table.size()); - // Count number of nonzero elements in table and reserving the space. + // Count number of nonzero elements in table and reserve the space. const uint64_t nnz = std::count_if(table.begin(), table.end(), [](uint64_t i) { return i != 0; }); sparse_table.reserve(nnz); @@ -228,6 +228,45 @@ void TableFactor::print(const string& s, const KeyFormatter& formatter) const { cout << "number of nnzs: " << sparse_table_.nonZeros() << endl; } +/* ************************************************************************ */ +TableFactor TableFactor::apply(Unary op) const { + // Initialize new factor. + uint64_t cardi = 1; + for (auto [key, c] : cardinalities_) cardi *= c; + Eigen::SparseVector sparse_table(cardi); + sparse_table.reserve(sparse_table_.nonZeros()); + + // Populate + for (SparseIt it(sparse_table_); it; ++it) { + sparse_table.coeffRef(it.index()) = op(it.value()); + } + + // Free unused memory and return. + sparse_table.pruned(); + sparse_table.data().squeeze(); + return TableFactor(discreteKeys(), sparse_table); +} + +/* ************************************************************************ */ +TableFactor TableFactor::apply(UnaryAssignment op) const { + // Initialize new factor. + uint64_t cardi = 1; + for (auto [key, c] : cardinalities_) cardi *= c; + Eigen::SparseVector sparse_table(cardi); + sparse_table.reserve(sparse_table_.nonZeros()); + + // Populate + for (SparseIt it(sparse_table_); it; ++it) { + DiscreteValues assignment = findAssignments(it.index()); + sparse_table.coeffRef(it.index()) = op(assignment, it.value()); + } + + // Free unused memory and return. + sparse_table.pruned(); + sparse_table.data().squeeze(); + return TableFactor(discreteKeys(), sparse_table); +} + /* ************************************************************************ */ TableFactor TableFactor::apply(const TableFactor& f, Binary op) const { if (keys_.empty() && sparse_table_.nonZeros() == 0) diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 103e14bff..828e794e6 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -93,6 +93,9 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { typedef std::shared_ptr shared_ptr; typedef Eigen::SparseVector::InnerIterator SparseIt; typedef std::vector> AssignValList; + using Unary = std::function; + using UnaryAssignment = + std::function&, const double&)>; using Binary = std::function; public: @@ -224,6 +227,18 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { /// @name Advanced Interface /// @{ + /** + * Apply unary operator `op(*this)` where `op` accepts the discrete value. + * @param op a unary operator that operates on TableFactor + */ + TableFactor apply(Unary op) const; + /** + * Apply unary operator `op(*this)` where `op` accepts the discrete assignment + * and the value at that assignment. + * @param op a unary operator that operates on TableFactor + */ + TableFactor apply(UnaryAssignment op) const; + /** * Apply binary operator (*this) "op" f * @param f the second argument for op @@ -231,10 +246,19 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { */ TableFactor apply(const TableFactor& f, Binary op) const; - /// Return keys in contract mode. + /** + * Return keys in contract mode. + * + * Modes are each of the dimensions of a sparse tensor, + * and the contract modes represent which dimensions will + * be involved in contraction (aka tensor multiplication). + */ DiscreteKeys contractDkeys(const TableFactor& f) const; - /// Return keys in free mode. + /** + * @brief Return keys in free mode which are the dimensions + * not involved in the contraction operation. + */ DiscreteKeys freeDkeys(const TableFactor& f) const; /// Return union of DiscreteKeys in two factors. diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index b307d78f6..e85e4254c 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -93,8 +93,7 @@ void printTime(map> for (auto&& kv : measured_time) { cout << "dropout: " << kv.first << " | TableFactor time: " << kv.second.first.count() - << " | DecisionTreeFactor time: " << kv.second.second.count() << - endl; + << " | DecisionTreeFactor time: " << kv.second.second.count() << endl; } } @@ -361,6 +360,39 @@ TEST(TableFactor, htmlWithValueFormatter) { EXPECT(actual == expected); } +/* ************************************************************************* */ +TEST(TableFactor, Unary) { + // Declare a bunch of keys + DiscreteKey X(0, 2), Y(1, 3); + + // Create factors + TableFactor f(X & Y, "2 5 3 6 2 7"); + auto op = [](const double x) { return 2 * x; }; + auto g = f.apply(op); + + TableFactor expected(X & Y, "4 10 6 12 4 14"); + EXPECT(assert_equal(g, expected)); + + auto sq_op = [](const double x) { return x * x; }; + auto g_sq = f.apply(sq_op); + TableFactor expected_sq(X & Y, "4 25 9 36 4 49"); + EXPECT(assert_equal(g_sq, expected_sq)); +} + +/* ************************************************************************* */ +TEST(TableFactor, UnaryAssignment) { + // Declare a bunch of keys + DiscreteKey X(0, 2), Y(1, 3); + + // Create factors + TableFactor f(X & Y, "2 5 3 6 2 7"); + auto op = [](const Assignment& key, const double x) { return 2 * x; }; + auto g = f.apply(op); + + TableFactor expected(X & Y, "4 10 6 12 4 14"); + EXPECT(assert_equal(g, expected)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 2b85cfedd4845e3a4329b9cf6dc0a5acb84fc499 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 22 Jul 2023 00:50:33 -0400 Subject: [PATCH 12/26] DecisionTreeFactor apply methods --- gtsam/discrete/DecisionTreeFactor.cpp | 16 ++++++++++++++++ gtsam/discrete/DecisionTreeFactor.h | 14 ++++++++++++-- 2 files changed, 28 insertions(+), 2 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index ff18268b1..4aa5e5759 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -82,6 +82,22 @@ namespace gtsam { ADT::print("", formatter); } + /* ************************************************************************ */ + DecisionTreeFactor DecisionTreeFactor::apply(ADT::Unary op) const { + // apply operand + ADT result = ADT::apply(op); + // Make a new factor + return DecisionTreeFactor(discreteKeys(), result); + } + + /* ************************************************************************ */ + DecisionTreeFactor DecisionTreeFactor::apply(ADT::UnaryAssignment op) const { + // apply operand + ADT result = ADT::apply(op); + // Make a new factor + return DecisionTreeFactor(discreteKeys(), result); + } + /* ************************************************************************ */ DecisionTreeFactor DecisionTreeFactor::apply(const DecisionTreeFactor& f, ADT::Binary op) const { diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index e90a2f96f..77da93c5f 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -147,8 +147,18 @@ namespace gtsam { /// @name Advanced Interface /// @{ - /// Inherit all the `apply` methods from AlgebraicDecisionTree - using ADT::apply; + /** + * Apply unary operator (*this) "op" f + * @param op a unary operator that operates on AlgebraicDecisionTree + */ + DecisionTreeFactor apply(ADT::Unary op) const; + + /** + * Apply unary operator (*this) "op" f + * @param op a unary operator that operates on AlgebraicDecisionTree. Takes + * both the assignment and the value. + */ + DecisionTreeFactor apply(ADT::UnaryAssignment op) const; /** * Apply binary operator (*this) "op" f From 3d24d0128f71e51f889ff5b2790eb2bd79bf7a73 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 22 Jul 2023 00:51:34 -0400 Subject: [PATCH 13/26] efficient probabilities method --- gtsam/discrete/DecisionTreeFactor.cpp | 40 ++++++++++++++++++++++----- 1 file changed, 33 insertions(+), 7 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 4aa5e5759..19cc8e230 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -197,9 +197,39 @@ namespace gtsam { /* ************************************************************************ */ std::vector DecisionTreeFactor::probabilities() const { std::vector probs; - for (auto&& [key, value] : enumerate()) { - probs.push_back(value); + + // Get all the key cardinalities + std::map cardins; + for (auto [key, cardinality] : discreteKeys()) { + cardins[key] = cardinality; } + // Set of all keys + std::set allKeys(keys().begin(), keys().end()); + + // Go through the tree + std::vector ys; + this->apply([&](const Assignment a, double p) { + // Get all the keys in the current assignment + std::set assignment_keys; + for (auto&& [k, _] : a) { + assignment_keys.insert(k); + } + + // Find the keys missing in the assignment + std::vector diff; + std::set_difference(allKeys.begin(), allKeys.end(), + assignment_keys.begin(), assignment_keys.end(), + std::back_inserter(diff)); + + // Compute the total number of assignments in the (pruned) subtree + size_t nrAssignments = 1; + for (auto&& k : diff) { + nrAssignments *= cardins.at(k); + } + probs.insert(probs.end(), nrAssignments, p); + return p; + }); + return probs; } @@ -313,11 +343,7 @@ namespace gtsam { const size_t N = maxNrAssignments; // Get the probabilities in the decision tree so we can threshold. - std::vector probabilities; - // NOTE(Varun) this is potentially slow due to the cartesian product - for (auto&& [assignment, prob] : this->enumerate()) { - probabilities.push_back(prob); - } + std::vector probabilities = this->probabilities(); // The number of probabilities can be lower than max_leaves if (probabilities.size() <= N) { From 5f83464f0d4dafd0c1c780852ebfdd47a55e4322 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 22 Jul 2023 01:09:55 -0400 Subject: [PATCH 14/26] use existing cardinalities --- gtsam/discrete/DecisionTreeFactor.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 19cc8e230..7ba4c9011 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -196,18 +196,12 @@ namespace gtsam { /* ************************************************************************ */ std::vector DecisionTreeFactor::probabilities() const { - std::vector probs; - - // Get all the key cardinalities - std::map cardins; - for (auto [key, cardinality] : discreteKeys()) { - cardins[key] = cardinality; - } // Set of all keys std::set allKeys(keys().begin(), keys().end()); + std::vector probs; + // Go through the tree - std::vector ys; this->apply([&](const Assignment a, double p) { // Get all the keys in the current assignment std::set assignment_keys; @@ -224,7 +218,7 @@ namespace gtsam { // Compute the total number of assignments in the (pruned) subtree size_t nrAssignments = 1; for (auto&& k : diff) { - nrAssignments *= cardins.at(k); + nrAssignments *= cardinalities_.at(k); } probs.insert(probs.end(), nrAssignments, p); return p; From ad84163f663cad6081c399f2735ac36ab7ffce79 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 23 Jul 2023 12:13:07 -0400 Subject: [PATCH 15/26] use discrete base class in getting discrete factors --- gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index af3a23b94..13e7e2c8f 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -420,7 +420,7 @@ TEST(HybridFactorGraph, Full_Elimination) { DiscreteFactorGraph discrete_fg; // TODO(Varun) Make this a function of HybridGaussianFactorGraph? for (auto& factor : (*remainingFactorGraph_partial)) { - auto df = dynamic_pointer_cast(factor); + auto df = dynamic_pointer_cast(factor); assert(df); discrete_fg.push_back(df); } From 52f26e3e97befe2e784509eeec07453bd4fc8ff0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 23 Jul 2023 16:32:52 -0400 Subject: [PATCH 16/26] update TableFactor to use new version of DT probabilities --- gtsam/discrete/TableFactor.cpp | 38 ++++++++++++++++++++---- gtsam/discrete/tests/testTableFactor.cpp | 11 +++++++ 2 files changed, 43 insertions(+), 6 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 1b265d14f..f4e023a4d 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -56,19 +56,45 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys, sort(sorted_dkeys_.begin(), sorted_dkeys_.end()); } -/* ************************************************************************ */ -TableFactor::TableFactor(const DiscreteKeys& dkeys, - const DecisionTreeFactor& dtf) - : TableFactor(dkeys, dtf.probabilities()) {} - /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteKeys& dkeys, const DecisionTree& dtree) : TableFactor(dkeys, DecisionTreeFactor(dkeys, dtree)) {} +/** + * @brief Compute the correct ordering of the leaves in the decision tree. + * + * This is done by first taking all the values which have modulo 0 value with + * the cardinality of the innermost key `n`, and we go up to modulo n. + * + * @param dt The DecisionTree + * @return std::vector + */ +std::vector ComputeLeafOrdering(const DiscreteKeys& dkeys, + const DecisionTreeFactor& dt) { + std::vector probs = dt.probabilities(); + std::vector ordered; + + size_t n = dkeys[0].second; + + for (size_t k = 0; k < n; ++k) { + for (size_t idx = 0; idx < probs.size(); ++idx) { + if (idx % n == k) { + ordered.push_back(probs[idx]); + } + } + } + return ordered; +} + +/* ************************************************************************ */ +TableFactor::TableFactor(const DiscreteKeys& dkeys, + const DecisionTreeFactor& dtf) + : TableFactor(dkeys, ComputeLeafOrdering(dkeys, dtf)) {} + /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteConditional& c) - : TableFactor(c.discreteKeys(), c.probabilities()) {} + : TableFactor(c.discreteKeys(), c) {} /* ************************************************************************ */ Eigen::SparseVector TableFactor::Convert( diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index e85e4254c..0f7d7a615 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -131,6 +132,16 @@ TEST(TableFactor, constructors) { // Manually constructed via inspection and comparison to DecisionTreeFactor TableFactor expected(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8"); EXPECT(assert_equal(expected, f4)); + + // Test for 9=3x3 values. + DiscreteKey V(0, 3), W(1, 3); + DiscreteConditional conditional5(V | W = "1/2/3 5/6/7 9/10/11"); + TableFactor f5(conditional5); + // GTSAM_PRINT(f5); + TableFactor expected_f5( + X & Y, + "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667"); + EXPECT(assert_equal(expected_f5, f5, 1e-6)); } /* ************************************************************************* */ From 2df3cc80a90c860f911d213f6be939e0c97cc673 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 23 Jul 2023 17:09:51 -0400 Subject: [PATCH 17/26] undo previous changes --- gtsam/hybrid/HybridBayesTree.cpp | 28 ++++++++++------------ gtsam/hybrid/HybridGaussianFactorGraph.cpp | 5 ++-- 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index d71760b9a..ae8fa0378 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -175,15 +175,14 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { void HybridBayesTree::prune(const size_t maxNrLeaves) { auto discreteProbs = this->roots_.at(0)->conditional()->asDiscrete(); - // TODO(Varun) - // TableFactor prunedDiscreteProbs = discreteProbs->prune(maxNrLeaves); - // discreteProbs->root_ = prunedDiscreteProbs.root_; + DecisionTreeFactor prunedDiscreteProbs = discreteProbs->prune(maxNrLeaves); + discreteProbs->root_ = prunedDiscreteProbs.root_; /// Helper struct for pruning the hybrid bayes tree. struct HybridPrunerData { /// The discrete decision tree after pruning. - TableFactor prunedDiscreteProbs; - HybridPrunerData(const TableFactor& prunedDiscreteProbs, + DecisionTreeFactor prunedDiscreteProbs; + HybridPrunerData(const DecisionTreeFactor& prunedDiscreteProbs, const HybridBayesTree::sharedNode& parentClique) : prunedDiscreteProbs(prunedDiscreteProbs) {} @@ -211,16 +210,15 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) { } }; - // TODO(Varun) - // HybridPrunerData rootData(prunedDiscreteProbs, 0); - // { - // treeTraversal::no_op visitorPost; - // // Limits OpenMP threads since we're mixing TBB and OpenMP - // TbbOpenMPMixedScope threadLimiter; - // treeTraversal::DepthFirstForestParallel( - // *this, rootData, HybridPrunerData::AssignmentPreOrderVisitor, - // visitorPost); - // } + HybridPrunerData rootData(prunedDiscreteProbs, 0); + { + treeTraversal::no_op visitorPost; + // Limits OpenMP threads since we're mixing TBB and OpenMP + TbbOpenMPMixedScope threadLimiter; + treeTraversal::DepthFirstForestParallel( + *this, rootData, HybridPrunerData::AssignmentPreOrderVisitor, + visitorPost); + } } } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index dc11cedb0..7ff87c347 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -273,8 +273,9 @@ hybridElimination(const HybridGaussianFactorGraph &factors, DecisionTree probabilities(eliminationResults, probability); - return {std::make_shared(gaussianMixture), - std::make_shared(discreteSeparator, probabilities)}; + return { + std::make_shared(gaussianMixture), + std::make_shared(discreteSeparator, probabilities)}; } else { // Otherwise, we create a resulting GaussianMixtureFactor on the separator, // taking care to correct for conditional constant. From a4462a0a3eeb729502042fdcb61b5f4a89f3422a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 23 Jul 2023 17:12:27 -0400 Subject: [PATCH 18/26] undo some more --- gtsam/hybrid/GaussianMixture.cpp | 4 ++-- gtsam/hybrid/GaussianMixture.h | 5 ++--- gtsam/hybrid/HybridBayesNet.cpp | 2 +- gtsam/hybrid/HybridBayesNet.h | 4 +++- 4 files changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 79c385262..753e35bf0 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -234,7 +234,7 @@ std::set DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) { */ std::function &, const GaussianConditional::shared_ptr &)> -GaussianMixture::prunerFunc(const TableFactor &discreteProbs) { +GaussianMixture::prunerFunc(const DecisionTreeFactor &discreteProbs) { // Get the discrete keys as sets for the decision tree // and the gaussian mixture. auto discreteProbsKeySet = DiscreteKeysAsSet(discreteProbs.discreteKeys()); @@ -285,7 +285,7 @@ GaussianMixture::prunerFunc(const TableFactor &discreteProbs) { } /* *******************************************************************************/ -void GaussianMixture::prune(const TableFactor &discreteProbs) { +void GaussianMixture::prune(const DecisionTreeFactor &discreteProbs) { // Functional which loops over all assignments and create a set of // GaussianConditionals auto pruner = prunerFunc(discreteProbs); diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index c193d065c..0b68fcfd0 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -23,7 +23,6 @@ #include #include #include -#include #include #include #include @@ -81,7 +80,7 @@ class GTSAM_EXPORT GaussianMixture */ std::function &, const GaussianConditional::shared_ptr &)> - prunerFunc(const TableFactor &discreteProbs); + prunerFunc(const DecisionTreeFactor &discreteProbs); public: /// @name Constructors @@ -239,7 +238,7 @@ class GTSAM_EXPORT GaussianMixture * * @param discreteProbs A pruned set of probabilities for the discrete keys. */ - void prune(const TableFactor &discreteProbs); + void prune(const DecisionTreeFactor &discreteProbs); /** * @brief Merge the Gaussian Factor Graphs in `this` and `sum` while diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 302ccfd6c..266e02b0d 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -64,7 +64,7 @@ DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const { * @return std::function &, double)> */ std::function &, double)> prunerFunc( - const TableFactor &prunedDiscreteProbs, + const DecisionTreeFactor &prunedDiscreteProbs, const HybridConditional &conditional) { // Get the discrete keys as sets for the decision tree // and the Gaussian mixture. diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index a71768bac..825f3acfd 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -17,6 +17,7 @@ #pragma once +#include #include #include #include @@ -225,7 +226,8 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * * @param prunedDiscreteProbs */ - void updateDiscreteConditionals(const DecisionTreeFactor &prunedDiscreteProbs); + void updateDiscreteConditionals( + const DecisionTreeFactor &prunedDiscreteProbs); #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ From 62d020a53156948367dda1538473df7bfb2dff24 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 23 Jul 2023 17:36:36 -0400 Subject: [PATCH 19/26] remove duplicate definition --- gtsam/discrete/DecisionTreeFactor.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index f0472245b..7ba4c9011 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -117,14 +117,6 @@ namespace gtsam { return DecisionTreeFactor(keys, result); } - /* ************************************************************************ */ - DecisionTreeFactor DecisionTreeFactor::apply(ADT::UnaryAssignment op) const { - // apply operand - ADT result = ADT::apply(op); - // Make a new factor - return DecisionTreeFactor(discreteKeys(), result); - } - /* ************************************************************************ */ DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine( size_t nrFrontals, ADT::Binary op) const { From 00895911c0451347f004ba8a6175381c181364e9 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Mon, 24 Jul 2023 22:16:56 +0300 Subject: [PATCH 20/26] Tests are not failing without debug assertion --- .github/workflows/build-windows.yml | 63 +++++++++++++++-------------- 1 file changed, 32 insertions(+), 31 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 565a02756..98218a894 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -99,64 +99,65 @@ jobs: cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" - name: Build - shell: cmd run: | - :: Since Visual Studio is a multi-generator, we need to use --config - :: https://stackoverflow.com/a/24470998/1236990 + # Since Visual Studio is a multi-generator, we need to use --config + # https://stackoverflow.com/a/24470998/1236990 cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam_unstable - :: Target doesn't exist - :: cmake --build build -j4 --config ${{ matrix.build_type }} --target wrap + # Target doesn't exist + # cmake --build build -j4 --config ${{ matrix.build_type }} --target wrap - name: Test - shell: cmd run: | - :: Run GTSAM tests + # Run GTSAM tests cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base cmake --build build -j4 --config ${{ matrix.build_type }} --target check.basis cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete - cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry + + # Compilation error + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.inference - :: Compile. Fail with exception - :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear + # Compile. Fail with exception + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear cmake --build build -j4 --config ${{ matrix.build_type }} --target check.navigation cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sam cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic - :: Compile. Fail with exception - :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid + # Compile. Fail with exception + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid - :: Compile. Fail with exception - :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear + # Compile. Fail with exception + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear - :: Compilation error - :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam + # Compilation error + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam - :: Run GTSAM_UNSTABLE tests + # Run GTSAM_UNSTABLE tests cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable - :: Compile. Fail with exception - :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable + # Compile. Fail with exception + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable - :: Compile. Fail with exception - :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable + # Compile. Fail with exception + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable - :: Compile. Fail with exception - :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable + # Compile. Fail with exception + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable - :: Compile. Fail with exception - :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable + # Compile. Fail with exception + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable - :: Compile. Fail with exception - :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable + # Compile. Fail with exception + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable - :: Compilation error - :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable + # Compilation error + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable - :: Compilation error - :: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition + # Compilation error + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition From 8ac72e8e687f9962f541d2f29bd0018727556b3b Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Mon, 24 Jul 2023 23:53:55 +0300 Subject: [PATCH 21/26] Change to bash shell to fail on first error --- .github/workflows/build-windows.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 98218a894..f4442bdde 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -99,6 +99,7 @@ jobs: cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" - name: Build + shell: bash run: | # Since Visual Studio is a multi-generator, we need to use --config # https://stackoverflow.com/a/24470998/1236990 @@ -109,6 +110,7 @@ jobs: # cmake --build build -j4 --config ${{ matrix.build_type }} --target wrap - name: Test + shell: bash run: | # Run GTSAM tests cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base From df0c5d7ca0994260b01b851583141b4a53e13a74 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 24 Jul 2023 19:23:16 -0400 Subject: [PATCH 22/26] remove timers --- gtsam/hybrid/HybridBayesNet.cpp | 7 +------ gtsam/hybrid/HybridGaussianFactorGraph.cpp | 3 --- 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index b4bf61220..3bafe5a9c 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -129,7 +129,6 @@ std::function &, double)> prunerFunc( DecisionTreeFactor HybridBayesNet::pruneDiscreteConditionals( size_t maxNrLeaves) { // Get the joint distribution of only the discrete keys - gttic_(HybridBayesNet_PruneDiscreteConditionals); // The joint discrete probability. DiscreteConditional discreteProbs; @@ -147,12 +146,11 @@ DecisionTreeFactor HybridBayesNet::pruneDiscreteConditionals( discrete_factor_idxs.push_back(i); } } + const DecisionTreeFactor prunedDiscreteProbs = discreteProbs.prune(maxNrLeaves); - gttoc_(HybridBayesNet_PruneDiscreteConditionals); // Eliminate joint probability back into conditionals - gttic_(HybridBayesNet_UpdateDiscreteConditionals); DiscreteFactorGraph dfg{prunedDiscreteProbs}; DiscreteBayesNet::shared_ptr dbn = dfg.eliminateSequential(discrete_frontals); @@ -161,7 +159,6 @@ DecisionTreeFactor HybridBayesNet::pruneDiscreteConditionals( size_t idx = discrete_factor_idxs.at(i); this->at(idx) = std::make_shared(dbn->at(i)); } - gttoc_(HybridBayesNet_UpdateDiscreteConditionals); return prunedDiscreteProbs; } @@ -180,7 +177,6 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { HybridBayesNet prunedBayesNetFragment; - gttic_(HybridBayesNet_PruneMixtures); // Go through all the conditionals in the // Bayes Net and prune them as per prunedDiscreteProbs. for (auto &&conditional : *this) { @@ -197,7 +193,6 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { prunedBayesNetFragment.push_back(conditional); } } - gttoc_(HybridBayesNet_PruneMixtures); return prunedBayesNetFragment; } diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 2d4ac83f6..7a7ca0cbf 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -96,7 +96,6 @@ static GaussianFactorGraphTree addGaussian( // TODO(dellaert): it's probably more efficient to first collect the discrete // keys, and then loop over all assignments to populate a vector. GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { - gttic_(assembleGraphTree); GaussianFactorGraphTree result; @@ -129,8 +128,6 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { } } - gttoc_(assembleGraphTree); - return result; } From cb3c35b81a8b7684d5f1a07a05aba5f5f00608ac Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 25 Jul 2023 11:11:55 -0400 Subject: [PATCH 23/26] refactor and better document prune method --- gtsam/discrete/DecisionTreeFactor.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 7ba4c9011..17731453a 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -201,8 +201,14 @@ namespace gtsam { std::vector probs; - // Go through the tree - this->apply([&](const Assignment a, double p) { + /* An operation that takes each leaf probability, and computes the + * nrAssignments by checking the difference between the keys in the factor + * and the keys in the assignment. + * The nrAssignments is then used to append + * the correct number of leaf probability values to the `probs` vector + * defined above. + */ + auto op = [&](const Assignment& a, double p) { // Get all the keys in the current assignment std::set assignment_keys; for (auto&& [k, _] : a) { @@ -220,9 +226,14 @@ namespace gtsam { for (auto&& k : diff) { nrAssignments *= cardinalities_.at(k); } + // Add p `nrAssignments` times to the probs vector. probs.insert(probs.end(), nrAssignments, p); + return p; - }); + }; + + // Go through the tree + this->apply(op); return probs; } From 3a78499d36dc31a8512564ecf6f5e05f10beae58 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 25 Jul 2023 11:12:29 -0400 Subject: [PATCH 24/26] undo TableFactor changes --- gtsam/discrete/TableFactor.cpp | 38 +----------------------- gtsam/discrete/TableFactor.h | 8 +---- gtsam/discrete/tests/testTableFactor.cpp | 10 ------- 3 files changed, 2 insertions(+), 54 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index f4e023a4d..2be8e077d 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -56,45 +56,9 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys, sort(sorted_dkeys_.begin(), sorted_dkeys_.end()); } -/* ************************************************************************ */ -TableFactor::TableFactor(const DiscreteKeys& dkeys, - const DecisionTree& dtree) - : TableFactor(dkeys, DecisionTreeFactor(dkeys, dtree)) {} - -/** - * @brief Compute the correct ordering of the leaves in the decision tree. - * - * This is done by first taking all the values which have modulo 0 value with - * the cardinality of the innermost key `n`, and we go up to modulo n. - * - * @param dt The DecisionTree - * @return std::vector - */ -std::vector ComputeLeafOrdering(const DiscreteKeys& dkeys, - const DecisionTreeFactor& dt) { - std::vector probs = dt.probabilities(); - std::vector ordered; - - size_t n = dkeys[0].second; - - for (size_t k = 0; k < n; ++k) { - for (size_t idx = 0; idx < probs.size(); ++idx) { - if (idx % n == k) { - ordered.push_back(probs[idx]); - } - } - } - return ordered; -} - -/* ************************************************************************ */ -TableFactor::TableFactor(const DiscreteKeys& dkeys, - const DecisionTreeFactor& dtf) - : TableFactor(dkeys, ComputeLeafOrdering(dkeys, dtf)) {} - /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteConditional& c) - : TableFactor(c.discreteKeys(), c) {} + : TableFactor(c.discreteKeys(), c.probabilities()) {} /* ************************************************************************ */ Eigen::SparseVector TableFactor::Convert( diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 828e794e6..d5decd1a1 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -144,12 +144,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { TableFactor(const DiscreteKey& key, const std::vector& row) : TableFactor(DiscreteKeys{key}, row) {} - /// Constructor from DecisionTreeFactor - TableFactor(const DiscreteKeys& keys, const DecisionTreeFactor& dtf); - - /// Constructor from DecisionTree/AlgebraicDecisionTree - TableFactor(const DiscreteKeys& keys, const DecisionTree& dtree); - /** Construct from a DiscreteConditional type */ explicit TableFactor(const DiscreteConditional& c); @@ -181,7 +175,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { /// Calculate error for DiscreteValues `x`, is -log(probability). double error(const DiscreteValues& values) const; - /// multiply two TableFactors + /// multiple two TableFactors TableFactor operator*(const TableFactor& f) const { return apply(f, Ring::mul); }; diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index 0f7d7a615..7ad0ac1ff 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -132,16 +132,6 @@ TEST(TableFactor, constructors) { // Manually constructed via inspection and comparison to DecisionTreeFactor TableFactor expected(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8"); EXPECT(assert_equal(expected, f4)); - - // Test for 9=3x3 values. - DiscreteKey V(0, 3), W(1, 3); - DiscreteConditional conditional5(V | W = "1/2/3 5/6/7 9/10/11"); - TableFactor f5(conditional5); - // GTSAM_PRINT(f5); - TableFactor expected_f5( - X & Y, - "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667"); - EXPECT(assert_equal(expected_f5, f5, 1e-6)); } /* ************************************************************************* */ From 8c9fad8cb16db195114a064a1a0e1c0d30002a93 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 25 Jul 2023 11:13:25 -0400 Subject: [PATCH 25/26] undo more changes in TableFactor --- gtsam/discrete/TableFactor.h | 4 ++-- gtsam/discrete/tests/testTableFactor.cpp | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index d5decd1a1..981e1507b 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -175,12 +175,12 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { /// Calculate error for DiscreteValues `x`, is -log(probability). double error(const DiscreteValues& values) const; - /// multiple two TableFactors + /// multiply two TableFactors TableFactor operator*(const TableFactor& f) const { return apply(f, Ring::mul); }; - /// multiply with DecisionTreeFactor + /// multiple with DecisionTreeFactor DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; static double safe_div(const double& a, const double& b); diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index 7ad0ac1ff..e85e4254c 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include #include From ff3994647a9999b1b2b9e0360c207ea84971f0af Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 25 Jul 2023 11:20:38 -0400 Subject: [PATCH 26/26] add new TableFactor constructors --- gtsam/discrete/TableFactor.cpp | 38 +++++++++++++++++++++++- gtsam/discrete/TableFactor.h | 8 ++++- gtsam/discrete/tests/testTableFactor.cpp | 11 +++++++ 3 files changed, 55 insertions(+), 2 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 2be8e077d..f4e023a4d 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -56,9 +56,45 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys, sort(sorted_dkeys_.begin(), sorted_dkeys_.end()); } +/* ************************************************************************ */ +TableFactor::TableFactor(const DiscreteKeys& dkeys, + const DecisionTree& dtree) + : TableFactor(dkeys, DecisionTreeFactor(dkeys, dtree)) {} + +/** + * @brief Compute the correct ordering of the leaves in the decision tree. + * + * This is done by first taking all the values which have modulo 0 value with + * the cardinality of the innermost key `n`, and we go up to modulo n. + * + * @param dt The DecisionTree + * @return std::vector + */ +std::vector ComputeLeafOrdering(const DiscreteKeys& dkeys, + const DecisionTreeFactor& dt) { + std::vector probs = dt.probabilities(); + std::vector ordered; + + size_t n = dkeys[0].second; + + for (size_t k = 0; k < n; ++k) { + for (size_t idx = 0; idx < probs.size(); ++idx) { + if (idx % n == k) { + ordered.push_back(probs[idx]); + } + } + } + return ordered; +} + +/* ************************************************************************ */ +TableFactor::TableFactor(const DiscreteKeys& dkeys, + const DecisionTreeFactor& dtf) + : TableFactor(dkeys, ComputeLeafOrdering(dkeys, dtf)) {} + /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteConditional& c) - : TableFactor(c.discreteKeys(), c.probabilities()) {} + : TableFactor(c.discreteKeys(), c) {} /* ************************************************************************ */ Eigen::SparseVector TableFactor::Convert( diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 981e1507b..828e794e6 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -144,6 +144,12 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { TableFactor(const DiscreteKey& key, const std::vector& row) : TableFactor(DiscreteKeys{key}, row) {} + /// Constructor from DecisionTreeFactor + TableFactor(const DiscreteKeys& keys, const DecisionTreeFactor& dtf); + + /// Constructor from DecisionTree/AlgebraicDecisionTree + TableFactor(const DiscreteKeys& keys, const DecisionTree& dtree); + /** Construct from a DiscreteConditional type */ explicit TableFactor(const DiscreteConditional& c); @@ -180,7 +186,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { return apply(f, Ring::mul); }; - /// multiple with DecisionTreeFactor + /// multiply with DecisionTreeFactor DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; static double safe_div(const double& a, const double& b); diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index e85e4254c..0f7d7a615 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -131,6 +132,16 @@ TEST(TableFactor, constructors) { // Manually constructed via inspection and comparison to DecisionTreeFactor TableFactor expected(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8"); EXPECT(assert_equal(expected, f4)); + + // Test for 9=3x3 values. + DiscreteKey V(0, 3), W(1, 3); + DiscreteConditional conditional5(V | W = "1/2/3 5/6/7 9/10/11"); + TableFactor f5(conditional5); + // GTSAM_PRINT(f5); + TableFactor expected_f5( + X & Y, + "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667"); + EXPECT(assert_equal(expected_f5, f5, 1e-6)); } /* ************************************************************************* */