diff --git a/gtsam/inference/FactorGraphUnordered.h b/gtsam/inference/FactorGraphUnordered.h index b2467e82d..1d919145e 100644 --- a/gtsam/inference/FactorGraphUnordered.h +++ b/gtsam/inference/FactorGraphUnordered.h @@ -130,6 +130,10 @@ namespace gtsam { template void push_back_bayesTree(const BayesTreeUnordered& bayesTree); + template + void push_back(const DERIVEDFACTOR& factor) { + add(factor); } + public: /** += syntax for push_back, e.g. graph += f1, f2, f3 */ template diff --git a/gtsam/linear/GaussianConditionalUnordered-inl.h b/gtsam/linear/GaussianConditionalUnordered-inl.h index 10e651e96..25ade2327 100644 --- a/gtsam/linear/GaussianConditionalUnordered-inl.h +++ b/gtsam/linear/GaussianConditionalUnordered-inl.h @@ -28,7 +28,7 @@ namespace gtsam { template GaussianConditionalUnordered::GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R, const PARENTS& parents, const SharedDiagonal& sigmas) : - BaseFactor(boost::join(boost::assign::cref_list_of<1>(std::make_pair(key, R)), parents), d, sigmas), + BaseFactor(boost::join(boost::assign::cref_list_of<1,typename PARENTS::value_type>(std::make_pair(key, R)), parents), d, sigmas), BaseConditional(1) {} /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianConditionalUnordered.h b/gtsam/linear/GaussianConditionalUnordered.h index b71f1acb9..c19fcc2cd 100644 --- a/gtsam/linear/GaussianConditionalUnordered.h +++ b/gtsam/linear/GaussianConditionalUnordered.h @@ -46,26 +46,22 @@ namespace gtsam { /** default constructor needed for serialization */ GaussianConditionalUnordered() {} - /** constructor with no parents - * |Rx-d| - */ + /** constructor with no parents |Rx-d| */ GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas = SharedDiagonal()); - /** constructor with only one parent - * |Rx+Sy-d| */ + /** constructor with only one parent |Rx+Sy-d| */ GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R, Key name1, const Matrix& S, const SharedDiagonal& sigmas = SharedDiagonal()); - /** constructor with two parents - * |Rx+Sy+Tz-d| */ + /** constructor with two parents |Rx+Sy+Tz-d| */ GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R, Key name1, const Matrix& S, Key name2, const Matrix& T, const SharedDiagonal& sigmas = SharedDiagonal()); /** Constructor with number of arbitrary parents. \f$ |Rx+sum(Ai*xi)-d| \f$ - * @tparam PARENTS A container whose value type is std::pair, specifying the - * collection of parent keys and matrices. */ + * @tparam PARENTS A container whose value type is std::pair, specifying the + * collection of parent keys and matrices. */ template GaussianConditionalUnordered(Key key, const Vector& d, const Matrix& R, const PARENTS& parents, @@ -111,6 +107,12 @@ namespace gtsam { /** Get a view of the parent blocks. */ constABlock get_S() const { return Ab_.range(nrFrontals(), size()); } + /** Get a view of the S matrix for the variable pointed to by the given key iterator */ + constABlock get_S(const_iterator variable) const { return BaseFactor::getA(variable); } + + /** Get a view of the r.h.s. vector d */ + const constBVector get_d() const { return BaseFactor::getb(); } + /** * Solves a conditional Gaussian and writes the solution into the entries of * \c x for each frontal variable of the conditional. The parents are diff --git a/gtsam/linear/JacobianFactorUnordered.h b/gtsam/linear/JacobianFactorUnordered.h index 01245e52b..d72dd568c 100644 --- a/gtsam/linear/JacobianFactorUnordered.h +++ b/gtsam/linear/JacobianFactorUnordered.h @@ -213,11 +213,6 @@ namespace gtsam { */ size_t rows() const { return Ab_.rows(); } - /** - * return the number of rows in the corresponding linear system - */ - size_t numberOfRows() const { return rows(); } - /** * return the number of columns in the corresponding linear system */ diff --git a/gtsam/linear/VectorValuesUnordered.h b/gtsam/linear/VectorValuesUnordered.h index 60ca7bf5f..2c7162521 100644 --- a/gtsam/linear/VectorValuesUnordered.h +++ b/gtsam/linear/VectorValuesUnordered.h @@ -110,6 +110,17 @@ namespace gtsam { /** Merge two VectorValues into one, this is more efficient than inserting elements one by one. */ VectorValuesUnordered(const VectorValuesUnordered& first, const VectorValuesUnordered& second); + /** Create from another container holding pair. */ + template + explicit VectorValuesUnordered(const CONTAINER& c) : values_(c.begin(), c.end()) {} + + /** Implicit copy constructor to specialize the explicit constructor from any container. */ + VectorValuesUnordered(const VectorValuesUnordered& c) : values_(c.values_) {} + + /** Create from a pair of iterators over pair. */ + template + VectorValuesUnordered(ITERATOR first, ITERATOR last) : values_(first, last) {} + /** Create a VectorValues with the same structure as \c other, but filled with zeros. */ static VectorValuesUnordered Zero(const VectorValuesUnordered& other); @@ -157,9 +168,20 @@ namespace gtsam { * @param j The index with which the value will be associated. */ void insert(Key j, const Vector& value) { - if(!values_.insert(std::make_pair(j, value)).second) + insert(std::pair(j, value)); // Note only passing a reference to the Vector + } + + /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c j is already used. + * @param value The vector to be inserted. + * @param j The index with which the value will be associated. + */ + void insert(std::pair key_value) { + // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as + // it is inserted into the values_ map. + if(!values_.insert(key_value).second) throw std::invalid_argument( - "Requested to insert variable '" + DefaultKeyFormatter(j) + "' already in this VectorValues."); + "Requested to insert variable '" + DefaultKeyFormatter(key_value.first) + + "' already in this VectorValues."); } /** Insert all values from \c values. Throws an invalid_argument exception if any keys to be