Added utility functions for converting graphs with LinearContainerFactors, added direct order() to avoid the need to fake linearize.

gtsam_unstable/nonlinear/LinearContainerFactor.cpp

@@ -106,8 +106,7 @@ size_t LinearContainerFactor::dim() const {
 }
 
 /* ************************************************************************* */
-boost::shared_ptr<GaussianFactor>
+GaussianFactor::shared_ptr LinearContainerFactor::order(const Ordering& ordering) const {
-LinearContainerFactor::linearize(const Values& c, const Ordering& ordering) const {
 	// clone factor
 	boost::shared_ptr<GaussianFactor> 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<JacobianFactor>(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
 

gtsam_unstable/nonlinear/LinearContainerFactor.h

@@ -10,7 +10,7 @@
 #pragma once
 
 #include <gtsam/linear/HessianFactor.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
 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<GaussianFactor>
+  GaussianFactor::shared_ptr linearize(const Values& c, const Ordering& ordering) const;
-  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);