diff --git a/gtsam/inference/FactorUnordered.h b/gtsam/inference/FactorUnordered.h index 7a3e36628..8425c81fd 100644 --- a/gtsam/inference/FactorUnordered.h +++ b/gtsam/inference/FactorUnordered.h @@ -137,12 +137,13 @@ namespace gtsam { /// print only keys void printKeys(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; + protected: /// check equality bool equals(const This& other, double tol = 1e-9) const; /// @} - + public: /// @name Advanced Interface /// @{ diff --git a/gtsam/linear/GaussianConditionalUnordered.h b/gtsam/linear/GaussianConditionalUnordered.h index cebbcd52d..78240594b 100644 --- a/gtsam/linear/GaussianConditionalUnordered.h +++ b/gtsam/linear/GaussianConditionalUnordered.h @@ -128,7 +128,7 @@ namespace gtsam { /** Scale the values in \c gy according to the sigmas for the frontal variables in this * conditional. */ - void scaleFrontalsBySigma(VectorValuesUnordered& gy) const; + __declspec(deprecated) void scaleFrontalsBySigma(VectorValuesUnordered& gy) const; private: @@ -136,10 +136,8 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(IndexConditional); - ar & BOOST_SERIALIZATION_NVP(matrix_); - ar & BOOST_SERIALIZATION_NVP(rsd_); - ar & BOOST_SERIALIZATION_NVP(sigmas_); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional); } }; // GaussianConditionalUnordered diff --git a/gtsam/linear/GaussianFactorUnordered.h b/gtsam/linear/GaussianFactorUnordered.h index 59e3b2e9c..296375088 100644 --- a/gtsam/linear/GaussianFactorUnordered.h +++ b/gtsam/linear/GaussianFactorUnordered.h @@ -60,6 +60,24 @@ namespace gtsam { /** Return the dimension of the variable pointed to by the given key iterator */ virtual size_t getDim(const_iterator variable) const = 0; + /** + * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ + * Jacobian matrix, augmented with b with the noise models baked + * into A and b. The negative log-likelihood is + * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also + * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. + */ + virtual Matrix augmentedJacobian() const = 0; + + /** + * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$, + * with the noise models baked into A and b. The negative log-likelihood + * is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also + * GaussianFactorGraph::augmentedJacobian and + * GaussianFactorGraph::sparseJacobian. + */ + virtual std::pair jacobian() const = 0; + /** Return the augmented information matrix represented by this GaussianFactorUnordered. * The augmented information matrix contains the information matrix with an * additional column holding the information vector, and an additional row diff --git a/gtsam/symbolic/SymbolicFactorUnordered.cpp b/gtsam/symbolic/SymbolicFactorUnordered.cpp index 6b431438f..02476aed4 100644 --- a/gtsam/symbolic/SymbolicFactorUnordered.cpp +++ b/gtsam/symbolic/SymbolicFactorUnordered.cpp @@ -59,4 +59,11 @@ namespace gtsam { SymbolicFactorUnordered::FromIterator(orderedKeys.begin() + nFrontals, orderedKeys.end()))); } + /* ************************************************************************* */ + std::pair, boost::shared_ptr > + SymbolicFactorUnordered::eliminate(const OrderingUnordered& keys) const + { + + } + } // gtsam diff --git a/gtsam/symbolic/SymbolicFactorUnordered.h b/gtsam/symbolic/SymbolicFactorUnordered.h index 56775598c..9666e8e3e 100644 --- a/gtsam/symbolic/SymbolicFactorUnordered.h +++ b/gtsam/symbolic/SymbolicFactorUnordered.h @@ -29,14 +29,9 @@ namespace gtsam { // Forward declarations class SymbolicConditionalUnordered; - /** - * SymbolicFactorUnordered serves two purposes. It is the base class for all linear - * factors (GaussianFactor, JacobianFactor, HessianFactor), and also functions - * as a symbolic factor, used to do symbolic elimination by JunctionTree. - * - * It derives from Factor with a key type of Key, an unsigned integer. - * \nosubgrouping - */ + /** SymbolicFactorUnordered represents a symbolic factor that specifies graph topology but is not + * associated with any numerical function. + * \nosubgrouping */ class GTSAM_EXPORT SymbolicFactorUnordered: public FactorUnordered { public: @@ -92,7 +87,8 @@ namespace gtsam { static SymbolicFactorUnordered FromIterator(KEYITERATOR beginKey, KEYITERATOR endKey) { return SymbolicFactorUnordered(Base::FromIterators(beginKey, endKey)); } - /** Constructor from a collection of keys */ + /** Constructor from a collection of keys - compatible with boost::assign::list_of and + * boost::assign::cref_list_of */ template static SymbolicFactorUnordered FromKeys(const CONTAINER& keys) { return SymbolicFactorUnordered(Base::FromKeys(keys)); } @@ -105,6 +101,13 @@ namespace gtsam { /** Whether the factor is empty (involves zero variables). */ bool empty() const { return keys_.empty(); } + /** Eliminate the variables in \c keys, in the order specified in \c keys, returning a + * conditional and marginal. */ + std::pair, boost::shared_ptr > + eliminate(const OrderingUnordered& keys) const; + + /// @} + private: /** Serialization function */ friend class boost::serialization::access;