Merge branch 'develop' into hybrid-timing
						commit
						49b74af075
					
				|  | @ -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); } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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) { | ||||
|  |  | |||
|  | @ -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) {} | ||||
| 
 | ||||
|  |  | |||
|  | @ -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) { | ||||
|  |  | |||
|  | @ -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<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
 | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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) { | ||||
|  |  | |||
|  | @ -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; | ||||
|           previousCardinality *= dt.cardinality(it->first); | ||||
|           idx += it->second * denominators.at(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; | ||||
|   for (auto i = 0; i < sparse_table_.size(); i++) { | ||||
|     table.push_back(sparse_table_.coeff(i)); | ||||
|   // If no keys, then return empty DecisionTreeFactor
 | ||||
|   if (dkeys.size() == 0) { | ||||
|     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); | ||||
|  |  | |||
|  | @ -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())); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -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) | ||||
|  |  | |||
|  | @ -55,9 +55,10 @@ public: | |||
|   Pose3() : R_(traits<Rot3>::Identity()), t_(traits<Point3>::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) : | ||||
|  |  | |||
|  | @ -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); | ||||
|  |  | |||
|  | @ -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; } | ||||
|  |  | |||
|  | @ -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; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -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(); | ||||
| 
 | ||||
|  |  | |||
|  | @ -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; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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); | ||||
| 
 | ||||
|  |  | |||
|  | @ -149,7 +149,7 @@ protected: | |||
|       noiseModel_->WhitenSystem(Ab.matrix(), b); | ||||
|     } | ||||
| 
 | ||||
|     return std::move(factor); | ||||
|     return factor; | ||||
|   } | ||||
| 
 | ||||
|   /// @return a deep copy of this factor
 | ||||
|  |  | |||
|  | @ -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; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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); | ||||
|  |  | |||
|  | @ -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 { | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue