diff --git a/cmake/FindGooglePerfTools.cmake b/cmake/FindGooglePerfTools.cmake index 01243257b..f73f449ec 100644 --- a/cmake/FindGooglePerfTools.cmake +++ b/cmake/FindGooglePerfTools.cmake @@ -1,42 +1,38 @@ # -*- cmake -*- -# - Find Google perftools -# Find the Google perftools includes and libraries -# This module defines -# GOOGLE_PERFTOOLS_INCLUDE_DIR, where to find heap-profiler.h, etc. -# GOOGLE_PERFTOOLS_FOUND, If false, do not try to use Google perftools. -# also defined for general use are -# TCMALLOC_LIBRARY, where to find the tcmalloc library. - -FIND_PATH(GOOGLE_PERFTOOLS_INCLUDE_DIR google/heap-profiler.h -/usr/local/include -/usr/include -) +# - Find GPerfTools (formerly Google perftools) +# Find the GPerfTools libraries +# If false, do not try to use Google perftools. +# Also defined for general use are +# - GPERFTOOLS_TCMALLOC: where to find the tcmalloc library +# - GPERFTOOLS_PROFILER: where to find the profiler library SET(TCMALLOC_NAMES ${TCMALLOC_NAMES} tcmalloc) -FIND_LIBRARY(TCMALLOC_LIBRARY +find_library(GPERFTOOLS_TCMALLOC NAMES ${TCMALLOC_NAMES} PATHS /usr/lib /usr/local/lib - ) +) +find_library(GPERFTOOLS_PROFILER + NAMES profiler + PATHS /usr/lib /usr/local/lib +) -IF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) - SET(TCMALLOC_LIBRARIES ${TCMALLOC_LIBRARY}) - SET(GOOGLE_PERFTOOLS_FOUND "YES") -ELSE (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) - SET(GOOGLE_PERFTOOLS_FOUND "NO") -ENDIF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) +IF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) + SET(TCMALLOC_LIBRARIES ${GPERFTOOLS_TCMALLOC}) + SET(GPERFTOOLS_FOUND "YES") +ELSE (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) + SET(GPERFTOOLS_FOUND "NO") +ENDIF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) -IF (GOOGLE_PERFTOOLS_FOUND) - IF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY) - MESSAGE(STATUS "Found Google perftools: ${GOOGLE_PERFTOOLS_LIBRARIES}") - ENDIF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY) -ELSE (GOOGLE_PERFTOOLS_FOUND) +IF (GPERFTOOLS_FOUND) + MESSAGE(STATUS "Found Gperftools: ${GPERFTOOLS_PROFILER}") +ELSE (GPERFTOOLS_FOUND) IF (GOOGLE_PERFTOOLS_FIND_REQUIRED) MESSAGE(FATAL_ERROR "Could not find Google perftools library") ENDIF (GOOGLE_PERFTOOLS_FIND_REQUIRED) -ENDIF (GOOGLE_PERFTOOLS_FOUND) +ENDIF (GPERFTOOLS_FOUND) MARK_AS_ADVANCED( - TCMALLOC_LIBRARY - GOOGLE_PERFTOOLS_INCLUDE_DIR - ) + GPERFTOOLS_TCMALLOC + GPERFTOOLS_PROFILER +) diff --git a/cmake/HandleAllocators.cmake b/cmake/HandleAllocators.cmake index 63411b17b..38297bbba 100644 --- a/cmake/HandleAllocators.cmake +++ b/cmake/HandleAllocators.cmake @@ -7,7 +7,7 @@ else() list(APPEND possible_allocators BoostPool STL) set(preferred_allocator STL) endif() -if(GOOGLE_PERFTOOLS_FOUND) +if(GPERFTOOLS_FOUND) list(APPEND possible_allocators tcmalloc) endif() diff --git a/cmake/HandlePerfTools.cmake b/cmake/HandlePerfTools.cmake index 499caf80a..4b5e5c8b3 100644 --- a/cmake/HandlePerfTools.cmake +++ b/cmake/HandlePerfTools.cmake @@ -1,4 +1,4 @@ ############################################################################### # Find Google perftools -find_package(GooglePerfTools) +find_package(GooglePerfTools) \ No newline at end of file diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 1fc8e4570..9a4b6ac3a 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -147,6 +147,10 @@ if (GTSAM_USE_EIGEN_MKL) target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR}) endif() +if (GPERFTOOLS_FOUND) + target_link_libraries(gtsam PRIVATE ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER}) +endif() + # Add includes for source directories 'BEFORE' boost and any system include # paths so that the compiler uses GTSAM headers in our source directory instead # of any previously installed GTSAM headers. diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index caedab713..9ec3b0ac5 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -407,11 +407,9 @@ namespace gtsam { }; /* ************************************************************************ */ - DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const { - const size_t N = maxNrAssignments; - + double DecisionTreeFactor::computeThreshold(const size_t N) const { // Set of all keys - std::set allKeys(keys().begin(), keys().end()); + std::set allKeys = this->labels(); MinHeap min_heap; auto op = [&](const Assignment& a, double p) { @@ -433,18 +431,25 @@ namespace gtsam { nrAssignments *= cardinalities_.at(k); } + // If min-heap is empty, fill it initially. + // This is because there is nothing at the top. if (min_heap.empty()) { min_heap.push(p, std::min(nrAssignments, N)); } else { - // If p is larger than the smallest element, - // then we insert into the max heap. - if (p > min_heap.top()) { - for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { + for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { + // If p is larger than the smallest element, + // then we insert into the min heap. + // We check against the top each time because the + // heap maintains the smallest element at the top. + if (p > min_heap.top()) { if (min_heap.size() == N) { min_heap.pop(); } min_heap.push(p); + } else { + // p is <= min value so move to the next one + break; } } } @@ -452,7 +457,14 @@ namespace gtsam { }; this->visitWith(op); - double threshold = min_heap.top(); + return min_heap.top(); + } + + /* ************************************************************************ */ + DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const { + const size_t N = maxNrAssignments; + + double threshold = computeThreshold(N); // Now threshold the decision tree size_t total = 0; diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 7af729f3e..a8ab2644f 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -224,6 +224,17 @@ namespace gtsam { /// Get all the probabilities in order of assignment values std::vector probabilities() const; + /** + * @brief Compute the probability value which is the threshold above which + * only `N` leaves are present. + * + * This is used for pruning out the smaller probabilities. + * + * @param N The number of leaves to keep post pruning. + * @return double + */ + double computeThreshold(const size_t N) const; + /** * @brief Prune the decision tree of discrete variables. * diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp index a41d06c2b..756a0cebe 100644 --- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp +++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp @@ -140,11 +140,46 @@ TEST(DecisionTreeFactor, enumerate) { EXPECT(actual == expected); } +namespace pruning_fixture { + +DiscreteKey A(1, 2), B(2, 2), C(3, 2); +DecisionTreeFactor f(A& B& C, "1 5 3 7 2 6 4 8"); + +DiscreteKey D(4, 2); +DecisionTreeFactor factor( + D& C & B & A, + "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " + "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0"); + +} // namespace pruning_fixture + +/* ************************************************************************* */ +// Check if computing the correct threshold works. +TEST(DecisionTreeFactor, ComputeThreshold) { + using namespace pruning_fixture; + + // Only keep the leaves with the top 5 values. + double threshold = f.computeThreshold(5); + EXPECT_DOUBLES_EQUAL(4.0, threshold, 1e-9); + + // Check for more extreme pruning where we only keep the top 2 leaves + threshold = f.computeThreshold(2); + EXPECT_DOUBLES_EQUAL(7.0, threshold, 1e-9); + + threshold = factor.computeThreshold(5); + EXPECT_DOUBLES_EQUAL(0.99995287, threshold, 1e-9); + + threshold = factor.computeThreshold(3); + EXPECT_DOUBLES_EQUAL(1.0, threshold, 1e-9); + + threshold = factor.computeThreshold(6); + EXPECT_DOUBLES_EQUAL(0.61247742, threshold, 1e-9); +} + /* ************************************************************************* */ // Check pruning of the decision tree works as expected. TEST(DecisionTreeFactor, Prune) { - DiscreteKey A(1, 2), B(2, 2), C(3, 2); - DecisionTreeFactor f(A & B & C, "1 5 3 7 2 6 4 8"); + using namespace pruning_fixture; // Only keep the leaves with the top 5 values. size_t maxNrAssignments = 5; @@ -160,12 +195,6 @@ TEST(DecisionTreeFactor, Prune) { DecisionTreeFactor expected2(A & B & C, "0 0 0 7 0 0 0 8"); EXPECT(assert_equal(expected2, pruned2)); - DiscreteKey D(4, 2); - DecisionTreeFactor factor( - D & C & B & A, - "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " - "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0"); - DecisionTreeFactor expected3(D & C & B & A, "0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 " "0.999952870000 1.0 1.0 1.0 1.0"); diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 42d2fe550..a8af78f2f 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -360,6 +360,7 @@ class Rot3 { // Standard Interface static gtsam::Rot3 Expmap(gtsam::Vector v); static gtsam::Vector Logmap(const gtsam::Rot3& p); + gtsam::Rot3 expmap(const gtsam::Vector& v); gtsam::Vector logmap(const gtsam::Rot3& p); gtsam::Matrix matrix() const; gtsam::Matrix transpose() const; diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 193635a21..1b633e024 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -210,9 +210,11 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) { if (conditional->isHybrid()) { auto hybridGaussianCond = conditional->asHybrid(); - // Imperative - clique->conditional() = std::make_shared( - hybridGaussianCond->prune(parentData.prunedDiscreteProbs)); + if (!hybridGaussianCond->pruned()) { + // Imperative + clique->conditional() = std::make_shared( + hybridGaussianCond->prune(parentData.prunedDiscreteProbs)); + } } return parentData; } diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 1bec42810..54346679e 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -120,7 +120,7 @@ struct HybridGaussianConditional::Helper { /* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKeys &discreteParents, Helper &&helper) + const DiscreteKeys &discreteParents, Helper &&helper, bool pruned) : BaseFactor(discreteParents, FactorValuePairs( [&](const GaussianFactorValuePair @@ -130,7 +130,8 @@ HybridGaussianConditional::HybridGaussianConditional( }, std::move(helper.pairs))), BaseConditional(*helper.nrFrontals), - negLogConstant_(helper.minNegLogConstant) {} + negLogConstant_(helper.minNegLogConstant), + pruned_(pruned) {} HybridGaussianConditional::HybridGaussianConditional( const DiscreteKey &discreteParent, @@ -166,8 +167,9 @@ HybridGaussianConditional::HybridGaussianConditional( : HybridGaussianConditional(discreteParents, Helper(conditionals)) {} HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKeys &discreteParents, const FactorValuePairs &pairs) - : HybridGaussianConditional(discreteParents, Helper(pairs)) {} + const DiscreteKeys &discreteParents, const FactorValuePairs &pairs, + bool pruned) + : HybridGaussianConditional(discreteParents, Helper(pairs), pruned) {} /* *******************************************************************************/ const HybridGaussianConditional::Conditionals @@ -331,7 +333,7 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune( FactorValuePairs prunedConditionals = factors().apply(pruner); return std::make_shared(discreteKeys(), - prunedConditionals); + prunedConditionals, true); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index c485fafce..0d2b1323c 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -68,6 +68,9 @@ class GTSAM_EXPORT HybridGaussianConditional ///< Take advantage of the neg-log space so everything is a minimization double negLogConstant_; + /// Flag to indicate if the conditional has been pruned. + bool pruned_ = false; + public: /// @name Constructors /// @{ @@ -150,9 +153,10 @@ class GTSAM_EXPORT HybridGaussianConditional * * @param discreteParents the discrete parents. Will be placed last. * @param conditionalPairs Decision tree of GaussianFactor/scalar pairs. + * @param pruned Flag indicating if conditional has been pruned. */ HybridGaussianConditional(const DiscreteKeys &discreteParents, - const FactorValuePairs &pairs); + const FactorValuePairs &pairs, bool pruned = false); /// @} /// @name Testable @@ -233,6 +237,9 @@ class GTSAM_EXPORT HybridGaussianConditional HybridGaussianConditional::shared_ptr prune( const DecisionTreeFactor &discreteProbs) const; + /// Return true if the conditional has already been pruned. + bool pruned() const { return pruned_; } + /// @} private: @@ -241,7 +248,7 @@ class GTSAM_EXPORT HybridGaussianConditional /// Private constructor that uses helper struct above. HybridGaussianConditional(const DiscreteKeys &discreteParents, - Helper &&helper); + Helper &&helper, bool pruned = false); /// Check whether `given` has values for all frontal keys. bool allFrontalsGiven(const VectorValues &given) const;