From 0fdd49ca4e2bacf1196e49aabc6b3eca1c39e7f0 Mon Sep 17 00:00:00 2001
From: krunalchande <krunalchande@gmail.com>
Date: Tue, 23 Dec 2014 14:45:23 -0500
Subject: [PATCH] Removed LinearEqualityManifoldFactorGraph.

---
 .../LinearEqualityManifoldFactorGraph.h       | 62 -------------------
 .../nonlinear/NonlinearEqualityFactorGraph.h  | 43 +++++++++++--
 2 files changed, 39 insertions(+), 66 deletions(-)
 delete mode 100644 gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h

diff --git a/gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h b/gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h
deleted file mode 100644
index a20f2e5a3..000000000
--- a/gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-/**
- * @file	LinearEqualityManifoldFactorGraph.h
- * @author  Duy-Nguyen Ta
- * @author 	Krunal Chande
- * @author  Luca Carlone
- * @date    Dec 15, 2014
- */
-
-#pragma once
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam_unstable/linear/LinearEqualityFactorGraph.h>
-#include <gtsam_unstable/nonlinear/NonlinearConstraint.h>
-
-
-namespace gtsam {
-
-class LinearEqualityManifoldFactorGraph: public FactorGraph<NonlinearFactor> {
-public:
-  /// default constructor
-  LinearEqualityManifoldFactorGraph() {
-  }
-
-  /// linearize to a LinearEqualityFactorGraph
-  LinearEqualityFactorGraph::shared_ptr linearize(
-      const Values& linearizationPoint) const {
-    LinearEqualityFactorGraph::shared_ptr linearGraph(
-        new LinearEqualityFactorGraph());
-    BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
-      JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast<JacobianFactor>(
-          factor->linearize(linearizationPoint));
-      NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
-      linearGraph->add(LinearEquality(*jacobian, constraint->dualKey()));
-    }
-    return linearGraph;
-  }
-
-  /**
-   * Return true if the max absolute error all factors is less than a tolerance
-   */
-  bool checkFeasibility(const Values& values, double tol) const {
-    BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
-      NoiseModelFactor::shared_ptr noiseModelFactor = boost::dynamic_pointer_cast<NoiseModelFactor>(
-          factor);
-      Vector error = noiseModelFactor->unwhitenedError(values);
-      if (error.lpNorm<Eigen::Infinity>() > tol) {
-        return false;
-      }
-    }
-    return true;
-  }
-};
-}
diff --git a/gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h b/gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h
index 1d82c6fbf..fbc52f376 100644
--- a/gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h
+++ b/gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h
@@ -18,16 +18,49 @@
  */
 
 #pragma once
-#include <gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h>
-
+#include <gtsam_unstable/linear/LinearEqualityFactorGraph.h>
+#include <gtsam_unstable/nonlinear/NonlinearConstraint.h>
 
 namespace gtsam {
-class NonlinearEqualityFactorGraph: public LinearEqualityManifoldFactorGraph {
+
+class NonlinearEqualityFactorGraph: public FactorGraph<NonlinearFactor> {
 public:
-  /// default constructor
+  /// Default constructor
   NonlinearEqualityFactorGraph() {
   }
 
+  /// Linearize to a LinearEqualityFactorGraph
+  LinearEqualityFactorGraph::shared_ptr linearize(
+      const Values& linearizationPoint) const {
+    LinearEqualityFactorGraph::shared_ptr linearGraph(
+        new LinearEqualityFactorGraph());
+    BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
+      JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast<JacobianFactor>(
+          factor->linearize(linearizationPoint));
+      NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
+      linearGraph->add(LinearEquality(*jacobian, constraint->dualKey()));
+    }
+    return linearGraph;
+  }
+
+  /**
+   * Return true if the max absolute error all factors is less than a tolerance
+   */
+  bool checkFeasibility(const Values& values, double tol) const {
+    BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
+      NoiseModelFactor::shared_ptr noiseModelFactor = boost::dynamic_pointer_cast<NoiseModelFactor>(
+          factor);
+      Vector error = noiseModelFactor->unwhitenedError(values);
+      if (error.lpNorm<Eigen::Infinity>() > tol) {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  /**
+   * Additional cost for -lambda*ConstraintHessian for SQP
+   */
   GaussianFactorGraph::shared_ptr multipliedHessians(const Values& values, const VectorValues& duals) const {
     GaussianFactorGraph::shared_ptr constrainedHessians(new GaussianFactorGraph());
     BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) {
@@ -36,5 +69,7 @@ public:
     }
     return constrainedHessians;
   }
+
 };
+
 }