diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp index 75cc171fb..42dfe7819 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.cpp @@ -106,8 +106,7 @@ size_t LinearContainerFactor::dim() const { } /* ************************************************************************* */ -boost::shared_ptr -LinearContainerFactor::linearize(const Values& c, const Ordering& ordering) const { +GaussianFactor::shared_ptr LinearContainerFactor::order(const Ordering& ordering) const { // clone factor boost::shared_ptr result = factor_->clone(); @@ -118,6 +117,12 @@ LinearContainerFactor::linearize(const Values& c, const Ordering& ordering) cons return result; } +/* ************************************************************************* */ +GaussianFactor::shared_ptr LinearContainerFactor::linearize( + const Values& c, const Ordering& ordering) const { + return order(ordering); +} + /* ************************************************************************* */ bool LinearContainerFactor::isJacobian() const { return boost::shared_dynamic_cast(factor_); @@ -147,6 +152,22 @@ NonlinearFactor::shared_ptr LinearContainerFactor::negate() const { return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor)); } +/* ************************************************************************* */ +NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( + const GaussianFactorGraph& linear_graph, const Ordering& ordering) { + return convertLinearGraph(linear_graph, ordering.invert()); +} + +/* ************************************************************************* */ +NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( + const GaussianFactorGraph& linear_graph, const InvertedOrdering& invOrdering) { + NonlinearFactorGraph result; + BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph) + if (f) + result.push_back(NonlinearFactorGraph::sharedFactor(new LinearContainerFactor(f, invOrdering))); + return result; +} + /* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam_unstable/nonlinear/LinearContainerFactor.h b/gtsam_unstable/nonlinear/LinearContainerFactor.h index 34da3a886..25a1ed98b 100644 --- a/gtsam_unstable/nonlinear/LinearContainerFactor.h +++ b/gtsam_unstable/nonlinear/LinearContainerFactor.h @@ -10,7 +10,7 @@ #pragma once #include -#include +#include namespace gtsam { @@ -70,9 +70,11 @@ public: /** get the dimension of the factor: rows of linear factor */ size_t dim() const; + /** Apply the ordering to a graph - same as linearize(), but without needing a linearization point */ + GaussianFactor::shared_ptr order(const Ordering& ordering) const; + /** linearize to a GaussianFactor: values has no effect, just clones/rekeys underlying factor */ - boost::shared_ptr - linearize(const Values& c, const Ordering& ordering) const; + GaussianFactor::shared_ptr linearize(const Values& c, const Ordering& ordering) const; /** * Creates an anti-factor directly and performs rekeying due to ordering @@ -85,8 +87,6 @@ public: */ NonlinearFactor::shared_ptr negate() const; - - /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow * for subclasses @@ -110,6 +110,18 @@ public: /** Casts to HessianFactor */ HessianFactor::shared_ptr toHessian() const; + /** + * Utility function for converting linear graphs to nonlinear graphs + * consisting of LinearContainerFactors. Two versions are available, using + * either the ordering the linear graph was linearized around, or the inverse ordering. + * If the inverse ordering is present, it will be faster. + */ + static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, + const Ordering& ordering); + + static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph, + const InvertedOrdering& invOrdering); + protected: void rekeyFactor(const Ordering& ordering); void rekeyFactor(const Ordering::InvertedMap& invOrdering);