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