From 2817a059119315243bca57fc938fcd7c02171ddc Mon Sep 17 00:00:00 2001
From: Natesh Srinivasan <natesh.rvce@gmail.com>
Date: Fri, 31 Jan 2014 18:11:50 -0500
Subject: [PATCH] Added camera pose and image co-ordinate measurements

---
 gtsam/slam/MultiDisparityFactor.cpp           | 12 +++++++++---
 gtsam/slam/MultiDisparityFactor.h             | 13 +++++++++----
 gtsam/slam/tests/testMultiDisparityFactor.cpp | 10 +++++++---
 3 files changed, 25 insertions(+), 10 deletions(-)

diff --git a/gtsam/slam/MultiDisparityFactor.cpp b/gtsam/slam/MultiDisparityFactor.cpp
index de10cfaed..1806ad932 100644
--- a/gtsam/slam/MultiDisparityFactor.cpp
+++ b/gtsam/slam/MultiDisparityFactor.cpp
@@ -18,8 +18,14 @@ 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: ");
+
+  for(int i = 0; i < disparities_.rows(); i++) {
+    cout << "Disparity @ (" << uv_(i,0) << ", " << uv_(i,1) << ") = " << disparities_(i) << "\n";
+  }
+
+  cameraPose_.print("Camera Pose ");
+  this->noiseModel_->print("  noise model: ");
+  cout << "\n";
 };
 
 //***************************************************************************
@@ -27,7 +33,7 @@ void MultiDisparityFactor::print(const string& s) const {
 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 5a9c77248..3d6d74d51 100644
--- a/gtsam/slam/MultiDisparityFactor.h
+++ b/gtsam/slam/MultiDisparityFactor.h
@@ -33,7 +33,10 @@ class MultiDisparityFactor: public NoiseModelFactor1<OrientedPlane3> {
   protected :
 
     Key landmarkKey_; // the key of the hidden plane in the world
-    Vector disparities_; // measured disparities in a set of Superpixels \mathcal{V}
+    Pose3 cameraPose_; // not a random variable , treated as a parameter to the factor
+    Vector disparities_; // measured disparity at a Pixel (u,v)
+    Eigen::Matrix<int,Eigen::Dynamic,2> uv_; // the 2D image coordinates. It is assumed here that the image co-ordinates are
+    // aligned with the disparity
 
     typedef NoiseModelFactor1<OrientedPlane3> Base;
 
@@ -44,11 +47,13 @@ class MultiDisparityFactor: public NoiseModelFactor1<OrientedPlane3> {
      {};
 
      /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol
-    MultiDisparityFactor (Key key, const Vector& disparities,
-        const SharedIsotropic& noiseModel)
+    MultiDisparityFactor (Key key, const Vector& disparities, const Eigen::Matrix<int,Eigen::Dynamic,2>& uv,
+        const Pose3& cameraPose, const SharedIsotropic& noiseModel)
       : Base (noiseModel, key),
         landmarkKey_ (key),
-        disparities_(disparities)
+        disparities_(disparities),
+        uv_(uv),
+        cameraPose_(cameraPose)
     {};
 
 
diff --git a/gtsam/slam/tests/testMultiDisparityFactor.cpp b/gtsam/slam/tests/testMultiDisparityFactor.cpp
index 5a932bbd1..a9a9efc7a 100644
--- a/gtsam/slam/tests/testMultiDisparityFactor.cpp
+++ b/gtsam/slam/tests/testMultiDisparityFactor.cpp
@@ -41,12 +41,16 @@ TEST(MutliDisparityFactor,error)
   Key key(1);
   Vector disparities = Vector_(2, 1.0, 1.0); // matlab generated values
 
+  Eigen::Matrix<int,Eigen::Dynamic,2> uv;
+  uv.resize(2,2);
+  uv.block<2,2>(0,0) << 20, 30, 40, 60;
   SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true);
 
-  cout << "Vector # main :" << disparities << endl;
+  Pose3 cameraPose;
+
+  MultiDisparityFactor factor(key, disparities, uv, cameraPose, model);
+  factor.print("Multi Disparity factor");
 
-  MultiDisparityFactor factor(key, disparities, model);
-  factor.print("Multi-disparity Factor");
 }
 
 /* ************************************************************************* */