Build a dual graph to compute dual values for equality constrained factors

gtsam/linear/GaussianFactorGraph.cpp

@@ -391,6 +391,46 @@ namespace gtsam {
     return false;
   }
 
+  /* ************************************************************************* */
+  JacobianFactor::shared_ptr GaussianFactorGraph::createDualFactor(Key key,
+      const VariableIndex& variableIndex, const VectorValues& delta) const {
+    typedef GaussianFactor G;
+    typedef JacobianFactor J;
+    std::vector<std::pair<Key, Matrix> > Aterms;
+    Vector b = zero(delta.at(key).size());
+    BOOST_FOREACH(size_t factorIx, variableIndex[key]) {
+      // If it is a constraint, transpose the A matrix to have the jacobian of the dual key
+      J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(this->at(factorIx)));
+      if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) {
+        Matrix Ai = jacobian->getA(jacobian->find(key)).transpose();
+        boost::optional<Key> dualKey = jacobian->dualKey();
+        if (!dualKey) {
+          throw std::runtime_error("Fail to create dual factor! " \
+            "Constrained JacobianFactor doesn't have a dual key!");
+        }
+        Aterms.push_back(make_pair(*(dualKey), Ai));
+      }
+      else {
+        // If it is unconstrained, collect the gradient to the b vector
+        G::shared_ptr factor(boost::dynamic_pointer_cast<G>(this->at(factorIx)));
+        b += factor->gradient(key, delta);
+      }
+    }
+    return boost::make_shared<JacobianFactor>(Aterms, b);
+  }
+
+  /* ************************************************************************* */
+  GaussianFactorGraph::shared_ptr GaussianFactorGraph::buildDualGraph(
+      const KeySet& constrainedVariables,
+      const VariableIndex& variableIndex,
+      const VectorValues& delta) const {
+    GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
+    BOOST_FOREACH(const Key key, constrainedVariables) {
+      dualGraph->push_back(createDualFactor(key, variableIndex, delta));
+    }
+    return dualGraph;
+  }
+
   /* ************************************************************************* */
   boost::tuple<GaussianFactorGraph, GaussianFactorGraph, GaussianFactorGraph> GaussianFactorGraph::splitConstraints() const {
     typedef HessianFactor H;

gtsam/linear/GaussianFactorGraph.h

@@ -328,6 +328,18 @@ namespace gtsam {
      */
     boost::tuple<GaussianFactorGraph, GaussianFactorGraph, GaussianFactorGraph> splitConstraints() const;
 
+    /**
+     * Build a dual graph to estimate dual variables associated with constrained factors
+     */
+    GaussianFactorGraph::shared_ptr buildDualGraph(
+        const KeySet& constrainedVariables, const VariableIndex& variableIndex,
+        const VectorValues& delta) const;
+
+    /**
+     * Create a dual factor from a constrained key
+     */
+    JacobianFactor::shared_ptr createDualFactor(Key key,
+      const VariableIndex& variableIndex, const VectorValues& delta) const;
 
   private:
     /** Serialization function */

tests/smallExample.h

@@ -166,7 +166,7 @@ static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
 static SharedDiagonal constraintModel = noiseModel::Constrained::All(2);
 
 static const Key _l1_=0, _x1_=1, _x2_=2;
-static const Key _x_=0, _y_=1, _z_=2;
+static const Key _x_=0, _y_=1, _z_=2, _d_=3, _e_=4;
 } // \namespace impl
 
 
@@ -423,7 +423,7 @@ inline GaussianFactorGraph createSimpleConstraintGraph() {
   Matrix Ay1 = eye(2) * -1;
   Vector b2 = (Vector(2) << 0.0, 0.0);
   JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
-      constraintModel));
+      constraintModel, _d_));
 
   // construct the graph
   GaussianFactorGraph fg;
@@ -469,7 +469,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() {
   Matrix Ay1 = eye(2) * 10;
   Vector b2 = (Vector(2) << 1.0, 2.0);
   JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
-      constraintModel));
+      constraintModel, _d_));
 
   // construct the graph
   GaussianFactorGraph fg;
@@ -513,7 +513,7 @@ inline GaussianFactorGraph createMultiConstraintGraph() {
   b1(0) = 1.0;
   b1(1) = 2.0;
   JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1,
-      constraintModel));
+      constraintModel, _d_));
 
   // constraint 2
   Matrix A21(2, 2);
@@ -532,7 +532,7 @@ inline GaussianFactorGraph createMultiConstraintGraph() {
   b2(0) = 3.0;
   b2(1) = 4.0;
   JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2,
-      constraintModel));
+      constraintModel, _e_));
 
   // construct the graph
   GaussianFactorGraph fg;

tests/testGaussianFactorGraphB.cpp

@@ -593,6 +593,55 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) {
   }
 }
 
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, buildDualGraph1 )
+{
+  GaussianFactorGraph fgc1 = createSimpleConstraintGraph();
+  KeySet constrainedVariables1 = list_of(0)(1);
+  VariableIndex variableIndex1(fgc1);
+  VectorValues delta1 = createSimpleConstraintValues();
+  GaussianFactorGraph::shared_ptr dualGraph1 = fgc1.buildDualGraph(
+      constrainedVariables1, variableIndex1, delta1);
+  GaussianFactorGraph expectedDualGraph1;
+  expectedDualGraph1.push_back(JacobianFactor(3, eye(2), zero(2)));
+  expectedDualGraph1.push_back(JacobianFactor(3, -eye(2), zero(2)));
+  EXPECT(assert_equal(expectedDualGraph1, *dualGraph1));
+}
+
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, buildDualGraph2 )
+{
+  GaussianFactorGraph fgc2 = createSingleConstraintGraph();
+  KeySet constrainedVariables2 = list_of(0)(1);
+  VariableIndex variableIndex2(fgc2);
+  VectorValues delta2 = createSingleConstraintValues();
+  GaussianFactorGraph::shared_ptr dualGraph2 = fgc2.buildDualGraph(
+      constrainedVariables2, variableIndex2, delta2);
+  GaussianFactorGraph expectedDualGraph2;
+  expectedDualGraph2.push_back(JacobianFactor(3, (Matrix(2,2) << 1,2,2,1), zero(2)));
+  expectedDualGraph2.push_back(JacobianFactor(3, 10*eye(2), zero(2)));
+  EXPECT(assert_equal(expectedDualGraph2, *dualGraph2));
+}
+
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, buildDualGraph3 )
+{
+  GaussianFactorGraph fgc3 = createMultiConstraintGraph();
+  KeySet constrainedVariables3 = list_of(0)(1)(2);
+  VariableIndex variableIndex3(fgc3);
+  VectorValues delta3 = createMultiConstraintValues();
+  GaussianFactorGraph::shared_ptr dualGraph3 = fgc3.buildDualGraph(
+      constrainedVariables3, variableIndex3, delta3);
+  GaussianFactorGraph expectedDualGraph3;
+  expectedDualGraph3.push_back(
+      JacobianFactor(3, (Matrix(2, 2) << 1, 2, 2, 1), 4,
+          (Matrix(2, 2) << 3, -1, 4, -2), zero(2)));
+  expectedDualGraph3.push_back(JacobianFactor(3, 10*eye(2), zero(2)));
+  expectedDualGraph3.push_back(
+      JacobianFactor(4, (Matrix(2, 2) << 1, 1, 1, 2), zero(2)));
+  EXPECT(assert_equal(expectedDualGraph3, *dualGraph3));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */