Merge branch 'develop' into hybrid-timing

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); }
 

gtsam/base/FastList.h

@@ -62,6 +62,8 @@ public:
   /// Construct from c++11 initializer list:
   FastList(std::initializer_list<VALUE> l) : Base(l) {}
 
+  FastList& operator=(const FastList& other) = default;
+
 #ifdef GTSAM_ALLOCATOR_BOOSTPOOL
   /** Copy constructor from a standard STL container */
   FastList(const std::list<VALUE>& x) {

gtsam/base/FastMap.h

@@ -54,6 +54,8 @@ public:
   /** Copy constructor from another FastMap */
   FastMap(const FastMap<KEY,VALUE>& x) : Base(x) {}
 
+  FastMap& operator=(const FastMap<KEY,VALUE>& x) = default;
+
   /** Copy constructor from the base map class */
   FastMap(const Base& x) : Base(x) {}
 

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<VALUE>& x) {

gtsam/base/GenericValue.h

@@ -56,9 +56,10 @@ public:
   GenericValue(){}
 
   /// Construct from value
-  GenericValue(const T& value) :
+  GenericValue(const T& value) : Value(),
-      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<Value> clone() const override {
-		return std::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
+      return std::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
     }
 
     /// Generic Value interface version of retract

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;

gtsam/discrete/SignatureParser.cpp

@@ -38,7 +38,7 @@ std::optional<Row> static ParseConditional(const std::string& token) {
   } catch (...) {
     return std::nullopt;
   }
-  return std::move(row);
+  return row;
 }
 
 std::optional<Table> static ParseConditionalTable(
@@ -62,7 +62,7 @@ std::optional<Table> static ParseConditionalTable(
       }
     }
   }
-  return std::move(table);
+  return table;
 }
 
 std::vector<std::string> static Tokenize(const std::string& str) {

gtsam/discrete/TableFactor.cpp

@@ -87,7 +87,15 @@ static Eigen::SparseVector<double> ComputeSparseTable(
   });
   sparseTable.reserve(nrValues);
 
-  std::set<Key> allKeys(dt.keys().begin(), dt.keys().end());
+  KeySet allKeys(dt.keys().begin(), dt.keys().end());
+
+  // Compute denominators to be used in computing sparse table indices
+  std::map<Key, size_t> denominators;
+  double denom = sparseTable.size();
+  for (const DiscreteKey& dkey : dkeys) {
+    denom /= dkey.second;
+    denominators.insert(std::pair<Key, double>(dkey.first, denom));
+  }
 
   /**
    * @brief Functor which is called by the DecisionTree for each leaf.
@@ -102,13 +110,13 @@ static Eigen::SparseVector<double> ComputeSparseTable(
   auto op = [&](const Assignment<Key>& assignment, double p) {
     if (p > 0) {
       // Get all the keys involved in this assignment
-      std::set<Key> assignmentKeys;
+      KeySet assignmentKeys;
       for (auto&& [k, _] : assignment) {
         assignmentKeys.insert(k);
       }
 
       // Find the keys missing in the assignment
-      std::vector<Key> diff;
+      KeyVector diff;
       std::set_difference(allKeys.begin(), allKeys.end(),
                           assignmentKeys.begin(), assignmentKeys.end(),
                           std::back_inserter(diff));
@@ -127,12 +135,10 @@ static Eigen::SparseVector<double> 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;
+          idx += it->second * denominators.at(it->first);
-          previousCardinality *= dt.cardinality(it->first);
         }
         sparseTable.coeffRef(idx) = p;
       }
@@ -252,9 +258,19 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
 DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
   DiscreteKeys dkeys = discreteKeys();
 
-  std::vector<double> table;
+  // If no keys, then return empty DecisionTreeFactor
-  for (auto i = 0; i < sparse_table_.size(); i++) {
+  if (dkeys.size() == 0) {
-    table.push_back(sparse_table_.coeff(i));
+    AlgebraicDecisionTree<Key> tree;
+    // We can have an empty sparse_table_ or one with a single value.
+    if (sparse_table_.size() != 0) {
+      tree = AlgebraicDecisionTree<Key>(sparse_table_.coeff(0));
+    }
+    return DecisionTreeFactor(dkeys, tree);
+  }
+
+  std::vector<double> table(sparse_table_.size(), 0.0);
+  for (SparseIt it(sparse_table_); it; ++it) {
+    table[it.index()] = it.value();
   }
 
   AlgebraicDecisionTree<Key> tree(dkeys, table);

gtsam/discrete/tests/testTableFactor.cpp

@@ -173,6 +173,36 @@ 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<double>{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<AlgebraicDecisionTree<Key>>(dtf2,
+                                                  tf2.toDecisionTreeFactor()));
+}
+
+/* ************************************************************************* */
+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()));
 }
 
 /* ************************************************************************* */

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)

gtsam/geometry/Pose3.h

@@ -55,9 +55,10 @@ public:
   Pose3() : R_(traits<Rot3>::Identity()), t_(traits<Point3>::Identity()) {}
 
   /** Copy constructor */
-  Pose3(const Pose3& pose) :
+  Pose3(const Pose3& pose) = default;
-      R_(pose.R_), t_(pose.t_) {
+  //  :
-  }
+  //     R_(pose.R_), t_(pose.t_) {
+  // }
 
   /** Construct from R,t */
   Pose3(const Rot3& R, const Point3& t) :

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);

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; }

gtsam/linear/HessianFactor.cpp

@@ -379,7 +379,7 @@ GaussianFactor::shared_ptr HessianFactor::negate() const {
   shared_ptr result = std::make_shared<This>(*this);
   // Negate the information matrix of the result
   result->info_.negate();
-  return std::move(result);
+  return result;
 }
 
 /* ************************************************************************* */

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();
 

gtsam/linear/NoiseModel.cpp

@@ -64,7 +64,7 @@ std::optional<Vector> checkIfDiagonal(const Matrix& M) {
     Vector diagonal(n);
     for (j = 0; j < n; j++)
       diagonal(j) = M(j, j);
-    return std::move(diagonal);
+    return diagonal;
   }
 }
 

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);
 

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

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;
   }
 }
 

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);

gtsam_unstable/slam/Mechanization_bRn2.h

@@ -35,9 +35,7 @@ public:
   }
 
   /// Copy constructor
-  Mechanization_bRn2(const Mechanization_bRn2& other) :
+  Mechanization_bRn2(const Mechanization_bRn2& other) = default;
-      bRn_(other.bRn_), x_g_(other.x_g_), x_a_(other.x_a_) {
-  }
 
   /// gravity in the body frame
   Vector3 b_g(double g_e) const {