From 846c29fa2edd91045c28c9f7384db08ce1c38f28 Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 29 Dec 2024 10:46:20 -0800 Subject: [PATCH 1/6] Fix deperecated copies and redundant moves --- gtsam/base/ConcurrentMap.h | 2 ++ gtsam/base/FastList.h | 2 ++ gtsam/base/FastMap.h | 2 ++ gtsam/base/FastSet.h | 2 ++ gtsam/base/GenericValue.h | 9 +++++---- gtsam/base/Value.h | 3 +++ gtsam/discrete/SignatureParser.cpp | 4 ++-- gtsam/geometry/Pose2.h | 5 ++++- gtsam/geometry/Pose3.h | 7 ++++--- gtsam/geometry/Rot2.h | 5 ++++- gtsam/linear/ConjugateGradientSolver.h | 2 ++ gtsam/linear/HessianFactor.cpp | 2 +- gtsam/linear/JacobianFactor.h | 2 ++ gtsam/linear/NoiseModel.cpp | 2 +- gtsam/linear/VectorValues.h | 2 ++ gtsam/nonlinear/ExpressionFactor.h | 2 +- gtsam/slam/FrobeniusFactor.cpp | 2 +- gtsam_unstable/geometry/Pose3Upright.h | 1 + gtsam_unstable/slam/Mechanization_bRn2.h | 4 +--- 19 files changed, 42 insertions(+), 18 deletions(-) diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h index 03687f562..8ce44dda3 100644 --- a/gtsam/base/ConcurrentMap.h +++ b/gtsam/base/ConcurrentMap.h @@ -85,6 +85,8 @@ public: /** Copy constructor from the base map class */ ConcurrentMap(const Base& x) : Base(x) {} + ConcurrentMap& operator=(const ConcurrentMap& other) = default; + /** Handy 'exists' function */ bool exists(const KEY& e) const { return this->count(e); } diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h index 1a3b9ed3f..821575ca9 100644 --- a/gtsam/base/FastList.h +++ b/gtsam/base/FastList.h @@ -62,6 +62,8 @@ public: /// Construct from c++11 initializer list: FastList(std::initializer_list l) : Base(l) {} + FastList& operator=(const FastList& other) = default; + #ifdef GTSAM_ALLOCATOR_BOOSTPOOL /** Copy constructor from a standard STL container */ FastList(const std::list& x) { diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h index 75998fd2f..30052908d 100644 --- a/gtsam/base/FastMap.h +++ b/gtsam/base/FastMap.h @@ -54,6 +54,8 @@ public: /** Copy constructor from another FastMap */ FastMap(const FastMap& x) : Base(x) {} + FastMap& operator=(const FastMap& x) = default; + /** Copy constructor from the base map class */ FastMap(const Base& x) : Base(x) {} diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 9a2f62497..1a2627e24 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -80,6 +80,8 @@ public: Base(x) { } + FastSet& operator=(const FastSet& other) = default; + #ifdef GTSAM_ALLOCATOR_BOOSTPOOL /** Copy constructor from a standard STL container */ FastSet(const std::set& x) { diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 62ea8ec39..396ca16ba 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -56,9 +56,10 @@ public: GenericValue(){} /// Construct from value - GenericValue(const T& value) : - value_(value) { - } + GenericValue(const T& value) : Value(), + value_(value) {} + + GenericValue(const GenericValue& other) = default; /// Return a constant value const T& value() const { @@ -112,7 +113,7 @@ public: * Clone this value (normal clone on the heap, delete with 'delete' operator) */ std::shared_ptr clone() const override { - return std::allocate_shared(Eigen::aligned_allocator(), *this); + return std::allocate_shared(Eigen::aligned_allocator(), *this); } /// Generic Value interface version of retract diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 09a3f4878..e80b9d4a4 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -38,6 +38,9 @@ namespace gtsam { */ class GTSAM_EXPORT Value { public: + // todo - not sure if valid + Value() = default; + Value(const Value& other) = default; /** Clone this value in a special memory pool, must be deleted with Value::deallocate_, *not* with the 'delete' operator. */ virtual Value* clone_() const = 0; diff --git a/gtsam/discrete/SignatureParser.cpp b/gtsam/discrete/SignatureParser.cpp index 31e425392..8e0221f01 100644 --- a/gtsam/discrete/SignatureParser.cpp +++ b/gtsam/discrete/SignatureParser.cpp @@ -38,7 +38,7 @@ std::optional static ParseConditional(const std::string& token) { } catch (...) { return std::nullopt; } - return std::move(row); + return row; } std::optional static ParseConditionalTable( @@ -62,7 +62,7 @@ std::optional
static ParseConditionalTable( } } } - return std::move(table); + return table; } std::vector static Tokenize(const std::string& str) { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 3f85a7f42..da7bc922e 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -60,7 +60,10 @@ public: } /** copy constructor */ - Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {} + Pose2(const Pose2& pose) = default; + // : r_(pose.r_), t_(pose.t_) {} + + Pose2& operator=(const Pose2& other) = default; /** * construct from (x,y,theta) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 5805ce41b..54bdd7f4c 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -55,9 +55,10 @@ public: Pose3() : R_(traits::Identity()), t_(traits::Identity()) {} /** Copy constructor */ - Pose3(const Pose3& pose) : - R_(pose.R_), t_(pose.t_) { - } + Pose3(const Pose3& pose) = default; + // : + // R_(pose.R_), t_(pose.t_) { + // } /** Construct from R,t */ Pose3(const Rot3& R, const Point3& t) : diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 07f328b96..7a6f25c4d 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -52,11 +52,14 @@ namespace gtsam { Rot2() : c_(1.0), s_(0.0) {} /** copy constructor */ - Rot2(const Rot2& r) : Rot2(r.c_, r.s_) {} + Rot2(const Rot2& r) = default; + // : Rot2(r.c_, r.s_) {} /// Constructor from angle in radians == exponential map at identity Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {} + // Rot2& operator=(const gtsam::Rot2& other) = default; + /// Named constructor from angle in radians static Rot2 fromAngle(double theta) { return Rot2(theta); diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 46d49ca4f..aaec039e9 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -69,6 +69,8 @@ struct GTSAM_EXPORT ConjugateGradientParameters epsilon_abs(p.epsilon_abs), blas_kernel(GTSAM) {} + ConjugateGradientParameters& operator=(const ConjugateGradientParameters& other) = default; + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 inline size_t getMinIterations() const { return minIterations; } inline size_t getMaxIterations() const { return maxIterations; } diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 4e8ef804c..1172dc281 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -379,7 +379,7 @@ GaussianFactor::shared_ptr HessianFactor::negate() const { shared_ptr result = std::make_shared(*this); // Negate the information matrix of the result result->info_.negate(); - return std::move(result); + return result; } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 4a5b6d5d1..a9933374f 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -117,6 +117,8 @@ namespace gtsam { /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ explicit JacobianFactor(const HessianFactor& hf); + JacobianFactor& operator=(const JacobianFactor& jf) = default; + /** default constructor for I/O */ JacobianFactor(); diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 39666b46b..2f693bbc4 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -64,7 +64,7 @@ std::optional checkIfDiagonal(const Matrix& M) { Vector diagonal(n); for (j = 0; j < n; j++) diagonal(j) = M(j, j); - return std::move(diagonal); + return diagonal; } } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index e60209a83..4728706c5 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -119,6 +119,8 @@ namespace gtsam { /// Constructor from Vector, with Scatter VectorValues(const Vector& c, const Scatter& scatter); + VectorValues& operator=(const VectorValues& other) = default; + /** Create a VectorValues with the same structure as \c other, but filled with zeros. */ static VectorValues Zero(const VectorValues& other); diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 4f6648849..df89f5b44 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -149,7 +149,7 @@ protected: noiseModel_->WhitenSystem(Ab.matrix(), b); } - return std::move(factor); + return factor; } /// @return a deep copy of this factor diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp index e70f64d96..982430d81 100644 --- a/gtsam/slam/FrobeniusFactor.cpp +++ b/gtsam/slam/FrobeniusFactor.cpp @@ -61,7 +61,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { return noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), isoModel); } else { - return std::move(isoModel); + return isoModel; } } diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index 20e0fde5d..464f46928 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -43,6 +43,7 @@ public: Pose3Upright(const Rot2& bearing, const Point3& t); Pose3Upright(double x, double y, double z, double theta); Pose3Upright(const Pose2& pose, double z); + Pose3Upright& operator=(const Pose3Upright& x) = default; /// Down-converts from a full Pose3 Pose3Upright(const Pose3& fullpose); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 4714f2b6f..3d91f6b0e 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -35,9 +35,7 @@ public: } /// Copy constructor - Mechanization_bRn2(const Mechanization_bRn2& other) : - bRn_(other.bRn_), x_g_(other.x_g_), x_a_(other.x_a_) { - } + Mechanization_bRn2(const Mechanization_bRn2& other) = default; /// gravity in the body frame Vector3 b_g(double g_e) const { From f42ed21137e78cec7af346dbde2d6027f8293871 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jan 2025 14:35:13 -0500 Subject: [PATCH 2/6] use faster key storage --- gtsam/discrete/TableFactor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index bf9662e34..459f36ccb 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -87,7 +87,7 @@ static Eigen::SparseVector ComputeSparseTable( }); sparseTable.reserve(nrValues); - std::set allKeys(dt.keys().begin(), dt.keys().end()); + KeySet allKeys(dt.keys().begin(), dt.keys().end()); /** * @brief Functor which is called by the DecisionTree for each leaf. @@ -102,13 +102,13 @@ static Eigen::SparseVector ComputeSparseTable( auto op = [&](const Assignment& assignment, double p) { if (p > 0) { // Get all the keys involved in this assignment - std::set assignmentKeys; + KeySet assignmentKeys; for (auto&& [k, _] : assignment) { assignmentKeys.insert(k); } // Find the keys missing in the assignment - std::vector diff; + KeyVector diff; std::set_difference(allKeys.begin(), allKeys.end(), assignmentKeys.begin(), assignmentKeys.end(), std::back_inserter(diff)); From 3fca55acc3f21c42a149a4ea3a29559098e0a266 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jan 2025 15:17:16 -0500 Subject: [PATCH 3/6] add test exposing issue with reverse ordered keys --- gtsam/discrete/tests/testTableFactor.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index a455faaaa..d920f978f 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -173,6 +173,27 @@ TEST(TableFactor, Conversion) { TableFactor tf(dtf.discreteKeys(), dtf); EXPECT(assert_equal(dtf, tf.toDecisionTreeFactor())); + + // Test for correct construction when keys are not in reverse order. + // This is possible in conditionals e.g. P(x1 | x0) + DiscreteKey X(1, 2), Y(0, 2); + DiscreteConditional dtf2( + X, {Y}, std::vector{0.33333333, 0.6, 0.66666667, 0.4}); + + TableFactor tf2(dtf2); + // GTSAM_PRINT(dtf2); + // GTSAM_PRINT(tf2); + // GTSAM_PRINT(tf2.toDecisionTreeFactor()); + + // Check for ADT equality since the order of keys is irrelevant + EXPECT(assert_equal>(dtf2, + tf2.toDecisionTreeFactor())); +} + +/* ************************************************************************* */ +TEST_DISABLED(TableFactor, Empty) { + // TableFactor empty({1, 2}, std::vector()); + // empty.print(); } /* ************************************************************************* */ From ff93c8be292941abd90e09450a9e2b3521ada9fa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jan 2025 15:18:22 -0500 Subject: [PATCH 4/6] use denominators to compute the correct index in ComputeSparseTable --- gtsam/discrete/TableFactor.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 459f36ccb..742538c87 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -89,6 +89,14 @@ static Eigen::SparseVector ComputeSparseTable( KeySet allKeys(dt.keys().begin(), dt.keys().end()); + // Compute denominators to be used in computing sparse table indices + std::map denominators; + double denom = sparseTable.size(); + for (const DiscreteKey& dkey : dkeys) { + denom /= dkey.second; + denominators.insert(std::pair(dkey.first, denom)); + } + /** * @brief Functor which is called by the DecisionTree for each leaf. * For each leaf value, we use the corresponding assignment to compute a @@ -127,12 +135,10 @@ static Eigen::SparseVector ComputeSparseTable( // Generate index and add to the sparse vector. Eigen::Index idx = 0; - size_t previousCardinality = 1; // We go in reverse since a DecisionTree has the highest label first for (auto&& it = updatedAssignment.rbegin(); it != updatedAssignment.rend(); it++) { - idx += previousCardinality * it->second; - previousCardinality *= dt.cardinality(it->first); + idx += it->second * denominators.at(it->first); } sparseTable.coeffRef(idx) = p; } @@ -252,9 +258,9 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const { DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { DiscreteKeys dkeys = discreteKeys(); - std::vector table; - for (auto i = 0; i < sparse_table_.size(); i++) { - table.push_back(sparse_table_.coeff(i)); + std::vector table(sparse_table_.size(), 0.0); + for (SparseIt it(sparse_table_); it; ++it) { + table[it.index()] = it.value(); } AlgebraicDecisionTree tree(dkeys, table); From bd32eb8203d2a1ff1bc1e8fb936e751f45072574 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jan 2025 15:32:11 -0500 Subject: [PATCH 5/6] unit test to expose another bug --- gtsam/discrete/tests/testTableFactor.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index d920f978f..e6c71e15c 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -191,9 +191,18 @@ TEST(TableFactor, Conversion) { } /* ************************************************************************* */ -TEST_DISABLED(TableFactor, Empty) { - // TableFactor empty({1, 2}, std::vector()); - // empty.print(); +TEST(TableFactor, Empty) { + DiscreteKey X(1, 2); + + TableFactor single = *TableFactor({X}, "1 1").sum(1); + // Should not throw a segfault + EXPECT(assert_equal(*DecisionTreeFactor(X, "1 1").sum(1), + single.toDecisionTreeFactor())); + + TableFactor empty = *TableFactor({X}, "0 0").sum(1); + // Should not throw a segfault + EXPECT(assert_equal(*DecisionTreeFactor(X, "0 0").sum(1), + empty.toDecisionTreeFactor())); } /* ************************************************************************* */ From f9a7eb0937e58b26ed4f96c46df9d4054c76c0c7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 2 Jan 2025 15:33:20 -0500 Subject: [PATCH 6/6] add fix --- gtsam/discrete/TableFactor.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 742538c87..a59095d40 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -258,6 +258,16 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const { DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { DiscreteKeys dkeys = discreteKeys(); + // If no keys, then return empty DecisionTreeFactor + if (dkeys.size() == 0) { + AlgebraicDecisionTree tree; + // We can have an empty sparse_table_ or one with a single value. + if (sparse_table_.size() != 0) { + tree = AlgebraicDecisionTree(sparse_table_.coeff(0)); + } + return DecisionTreeFactor(dkeys, tree); + } + std::vector table(sparse_table_.size(), 0.0); for (SparseIt it(sparse_table_); it; ++it) { table[it.index()] = it.value();