From a75df298d77f44a652c406fc9625bc16e7b9d417 Mon Sep 17 00:00:00 2001
From: Natesh Srinivasan <natesh.rvce@gmail.com>
Date: Fri, 31 Jan 2014 15:33:02 -0500
Subject: [PATCH] Fixed Link errors ,etc for Multi-Disparity Factor

---
 gtsam/slam/MultiDisparityFactor.cpp           | 23 +++++
 gtsam/slam/MultiDisparityFactor.h             | 91 ++++++-------------
 gtsam/slam/tests/testMultiDisparityFactor.cpp | 58 ++++++++++++
 3 files changed, 107 insertions(+), 65 deletions(-)
 create mode 100644 gtsam/slam/tests/testMultiDisparityFactor.cpp

diff --git a/gtsam/slam/MultiDisparityFactor.cpp b/gtsam/slam/MultiDisparityFactor.cpp
index 449a312e7..de10cfaed 100644
--- a/gtsam/slam/MultiDisparityFactor.cpp
+++ b/gtsam/slam/MultiDisparityFactor.cpp
@@ -6,5 +6,28 @@
  */
 
 
+#include "MultiDisparityFactor.h"
+#include <gtsam/nonlinear/NonlinearFactor.h>
 
 
+using namespace std;
+
+namespace gtsam {
+
+//***************************************************************************
+
+void MultiDisparityFactor::print(const string& s) const {
+  cout << "Prior Factor on " << landmarkKey_ << "\n";
+  cout << "Measured Disparities : \n " << disparities_ << "\n";
+  this->noiseModel_->print("  Noise model: ");
+};
+
+//***************************************************************************
+
+Vector MultiDisparityFactor::evaluateError(const OrientedPlane3& plane,
+    boost::optional<Matrix&> H) const {
+
+  return Vector_(3,1,1,1);
+};
+
+}
diff --git a/gtsam/slam/MultiDisparityFactor.h b/gtsam/slam/MultiDisparityFactor.h
index 813264893..5a9c77248 100644
--- a/gtsam/slam/MultiDisparityFactor.h
+++ b/gtsam/slam/MultiDisparityFactor.h
@@ -16,7 +16,11 @@
  * @brief A factor for modeling the disparity across multiple views
  */
 
+#pragma once
+
 #include <gtsam/geometry/OrientedPlane3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
 
 namespace gtsam {
 
@@ -24,79 +28,36 @@ namespace gtsam {
  * Unary factor on measured disparity from multiple views as deterministic function of camera pose
  */
 
-class MutiDisparityFactor : public NoiseModelFactor1<OrientedPlane3> {
+class MultiDisparityFactor: public NoiseModelFactor1<OrientedPlane3> {
 
-protected :
-  Symbol landmarkSymbol_;
-  OrientedPlane3 measured_p_;
+  protected :
 
-  typedef NoiseModelFactor1<OrientedPlane3 > Base;
+    Key landmarkKey_; // the key of the hidden plane in the world
+    Vector disparities_; // measured disparities in a set of Superpixels \mathcal{V}
 
-public:
+    typedef NoiseModelFactor1<OrientedPlane3> Base;
 
-  /// Constructor
-  MutiDisparityFactor ()
-   {}
+  public:
 
-   /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol
-  MutiDisparityFactor (const Vector&z, const SharedGaussian& noiseModel,
-                const Symbol& pose,
-                const Symbol& landmark)
-     : Base (noiseModel, landmark),
-       poseSymbol_ (pose),
-       landmarkSymbol_ (landmark),
-       measured_coeffs_ (z)
-   {
-     measured_p_ = OrientedPlane3 (Sphere2 (z (0), z (1), z (2)), z (3));
-   }
+  // Constructor
+    MultiDisparityFactor()
+     {};
 
-   /// print
-   void print(const std::string& s="PlaneFactor") const;
+     /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol
+    MultiDisparityFactor (Key key, const Vector& disparities,
+        const SharedIsotropic& noiseModel)
+      : Base (noiseModel, key),
+        landmarkKey_ (key),
+        disparities_(disparities)
+    {};
 
-   virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
-                                boost::optional<Matrix&> H1 = boost::none,
-                                boost::optional<Matrix&> H2 = boost::none) const
-   {
-     OrientedPlane3 predicted_plane = OrientedPlane3::Transform (plane, pose, H1, H2);
-     Vector error = predicted_plane.error (measured_p_);
-     return (error);
-   };
-};
 
-class OrientedPlane3DirectionPrior: public NoiseModelFactor1<OrientedPlane3> {
-
- protected:
-   OrientedPlane3 measured_p_; /// measured plane parameters
-   Symbol landmarkSymbol_;
-
-   typedef NoiseModelFactor1<OrientedPlane3 > Base;
-
- public:
-
-   typedef OrientedPlane3DirectionPrior This;
-   /// Constructor
-   OrientedPlane3DirectionPrior ()
-   {}
-
-   /// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol
-   OrientedPlane3DirectionPrior (const Symbol& landmark, const Vector&z,
-       const SharedGaussian& noiseModel)
-     : Base (noiseModel, landmark),
-       landmarkSymbol_ (landmark)
-   {
-     measured_p_ = OrientedPlane3 (Sphere2 (z (0), z (1), z (2)), z (3));
-   }
-
-   /// print
-   void print(const std::string& s) const;
-
-   /** equals */
-   virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
-
-   virtual Vector evaluateError(const OrientedPlane3& plane,
-                                boost::optional<Matrix&> H = boost::none) const;
-};
+     /// print
+     void print(const std::string& s="Multi-View DisaprityFactor") const;
 
+     virtual Vector evaluateError(const OrientedPlane3& plane,
+                                  boost::optional<Matrix&> H1 = boost::none) const;
 
 };
-}
+
+} // gtsam
diff --git a/gtsam/slam/tests/testMultiDisparityFactor.cpp b/gtsam/slam/tests/testMultiDisparityFactor.cpp
new file mode 100644
index 000000000..5a932bbd1
--- /dev/null
+++ b/gtsam/slam/tests/testMultiDisparityFactor.cpp
@@ -0,0 +1,58 @@
+/*
+ * testMultiDisparityFactor.cpp
+ *
+ *  Created on: Jan 31, 2014
+ *      Author: nsrinivasan7
+ *      @brief: Unittest for MultidisparityFactor
+ */
+
+
+#include <gtsam/geometry/Sphere2.h>
+#include <gtsam/geometry/OrientedPlane3.h>
+#include <gtsam/slam/OrientedPlane3Factor.h>
+#include <gtsam/slam/MultiDisparityFactor.h>
+#include <gtsam/nonlinear/Symbol.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/FactorGraph.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <CppUnitLite/TestHarness.h>
+#include <boost/bind.hpp>
+#include <boost/foreach.hpp>
+#include <boost/assign/std/vector.hpp>
+
+using namespace boost::assign;
+using namespace gtsam;
+using namespace std;
+
+GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
+GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
+
+TEST(MutliDisparityFactor,error)
+{
+
+  Key key(1);
+  Vector disparities = Vector_(2, 1.0, 1.0); // matlab generated values
+
+  SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true);
+
+  cout << "Vector # main :" << disparities << endl;
+
+  MultiDisparityFactor factor(key, disparities, model);
+  factor.print("Multi-disparity Factor");
+}
+
+/* ************************************************************************* */
+int main() {
+  srand(time(NULL));
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */