Added Multi Disparity Factor

gtsam/slam/MultiDisparityFactor.cpp

@@ -32,8 +32,63 @@ void MultiDisparityFactor::print(const string& s) const {
 
 Vector MultiDisparityFactor::evaluateError(const OrientedPlane3& plane,
     boost::optional<Matrix&> H) const {
+  Vector e;
+  vector p2 = plane.normal().basis().transpose()
+  e.resize(uv_.rows());
+  if(H) {
+    (*H).resize(uv_.rows(), 3);
 
+    R(plane);
+    for(int i = 0 ; i < uv_.rows() ; i++ ) {
+      Matrix d = Rd_ * plane.planeCoefficients();
+      (*H).row(i) = (plane.planeCoefficients().transpose() * R_.at(i) ) /( pow(d(0,0) ,2) );
+    }
+    e = diff(plane);
+    return e;
+  } else {
+    R(plane); // recompute the Rd_, R_, Rn_
+    e = diff(plane);
+    return e;
+  }
+}
 
-};
+void MultiDisparityFactor::Rn(const OrientedPlane3& p) const {
+
+  Rn_.resize(uv_.rows(),3);
+  Matrix wRc = cameraPose_.rotation().matrix();
+  Rn_.setZero();
+  Rn_ << uv_ *  wRc.transpose() * p.normal().basis();
+
+  return;
+}
+
+void MultiDisparityFactor::Rd(const OrientedPlane3& p) const {
+
+  Rd_.resize(1,3);
+  Vector wTc = cameraPose_.translation().vector();
+
+  Rd_.block<1,2>(0,0) << wTc.transpose() * p.normal().basis();
+  Rd_.block<1,1>(0,2) << 0.0;
+  return;
+
+}
+
+Vector MultiDisparityFactor::diff(const OrientedPlane3& theta) const {
+    Vector e;
+    e.resize(uv_.rows(),1);
+    Matrix wRc = cameraPose_.rotation().matrix();
+    Vector wTc = cameraPose_.translation().vector();
+    Vector planecoeffs = theta.planeCoefficients();
+    for(int i=0; i < uv_.rows(); i++) {
+      Matrix numerator = planecoeffs.block(0,0,3,1).transpose() * wRc * uv_.row(i).transpose();
+      Matrix denominator = planecoeffs.block(0,0,3,1).transpose() * wTc;
+      cout << "Numerator : \n" << numerator << "Denominator \n" << denominator <<"\n";
+      e(i,0) =  disparities_(i,0) - ( numerator(0,0) /( denominator(0,0) + planecoeffs(0,3) ) );
+      cout << e(i,0) << " = " << disparities_(i,0) << " - " << ( numerator(0,0) /( denominator(0,0) + planecoeffs(0,3) ) ) <<  "\n";
+    }
+    cout << "\n";
+    return e;
+
+}
 
 }

gtsam/slam/MultiDisparityFactor.h

@@ -33,11 +33,15 @@ class MultiDisparityFactor: public NoiseModelFactor1<OrientedPlane3> {
   protected :
 
     Key landmarkKey_; // the key of the hidden plane in the world
-    Pose3 cameraPose_; // not a random variable , treated as a parameter to the factor
+    gtsam::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
+    Eigen::Matrix<double,Eigen::Dynamic,3> uv_; // the 2D image coordinates. It is assumed here that the image co-ordinates are
     // aligned with the disparity
 
+    mutable Eigen::MatrixXd  Rd_; // the denominator matrix
+    mutable Eigen::Matrix<double, Eigen::Dynamic, 3> Rn_; // the numerator matrix
+    mutable std::vector<Eigen::Matrix<double,3,3> > R_;
+
     typedef NoiseModelFactor1<OrientedPlane3> Base;
 
   public:
@@ -47,8 +51,8 @@ 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 Eigen::Matrix<int,Eigen::Dynamic,2>& uv,
+    MultiDisparityFactor (Key key, const Vector& disparities, const Eigen::Matrix<double,Eigen::Dynamic,3>& uv,
-        const Pose3& cameraPose, const SharedIsotropic& noiseModel)
+        const gtsam::Pose3& cameraPose,const SharedIsotropic& noiseModel)
       : Base (noiseModel, key),
         landmarkKey_ (key),
         disparities_(disparities),
@@ -63,6 +67,37 @@ class MultiDisparityFactor: public NoiseModelFactor1<OrientedPlane3> {
      virtual Vector evaluateError(const OrientedPlane3& plane,
                                   boost::optional<Matrix&> H1 = boost::none) const;
 
+
+     void Rn(const OrientedPlane3& p) const;
+     inline const Eigen::MatrixXd Rn() {
+       return Rn_;
+     }
+
+     void Rd(const OrientedPlane3& p) const;
+     inline const Eigen::MatrixXd Rd() {
+       return Rd_;
+     }
+
+     void R(const OrientedPlane3& p) const {
+       Rd(p);
+       Rn(p);
+       for(int i =0; i < Rn_.rows(); i++) {
+         Matrix Rnr = Rn_.row(i);
+         R_.push_back( Rd_.transpose() * Rnr - Rnr.transpose() * Rd_ );
+       }
+     }
+
+
+     inline const Eigen::Matrix<double,3,3> getR(int i) {
+       return R_.at(i);
+     }
+
+     bool equals(const NonlinearFactor &p, double tol = 1e-9) const  {
+
+     }
+
+     // compute the differene between predivted and actual disparity
+     Vector diff(const OrientedPlane3& theta) const;
 };
 
 } // gtsam

gtsam/slam/tests/testMultiDisparityFactor.cpp

@@ -35,22 +35,125 @@ using namespace std;
 GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
 GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
 
-TEST(MutliDisparityFactor,error)
+TEST(MutliDisparityFactor,Rd)
 {
 
   Key key(1);
   Vector disparities = Vector_(2, 1.0, 1.0); // matlab generated values
 
-  Eigen::Matrix<int,Eigen::Dynamic,2> uv;
+  Eigen::Matrix<double,Eigen::Dynamic,3> uv;
-  uv.resize(2,2);
+  uv.resize(2,3);
-  uv.block<2,2>(0,0) << 20, 30, 40, 60;
+  uv.block<2,3>(0,0) << 20.0, 30.0, 70.0, 40.0, 60.0, 70.0;
   SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true);
 
-  Pose3 cameraPose;
+  gtsam::Pose3 cameraPose( gtsam::Rot3(), gtsam::Point3(1.0, 1.0, 1.0) );
 
   MultiDisparityFactor factor(key, disparities, uv, cameraPose, model);
-  factor.print("Multi Disparity factor");
 
+  // basis = [0 1 0; -1 0 0]
+  Vector theta = Vector_(4,0.0,0.0,1.0,20.0);
+  OrientedPlane3 p(theta);
+  factor.Rd(p);
+  Matrix actualRd = factor.Rd();
+  Matrix expectedRd = Matrix_(1,3,1.0,-1.0,0.0);
+  EXPECT(assert_equal( expectedRd,actualRd,1e-8) );
+
+}
+
+TEST(MutliDisparityFactor,Rn)
+{
+
+  Key key(1);
+  Vector disparities = Vector_(2, 1.0, 1.0); // matlab generated values
+
+  Eigen::Matrix<double,Eigen::Dynamic,3> uv;
+  uv.resize(2,3);
+  uv.block<2,3>(0,0) << 20.0, 30.0, 70.0, 40.0, 60.0, 70.0;
+  SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true);
+
+  gtsam::Pose3 cameraPose( gtsam::Rot3(), gtsam::Point3(1.0, 1.0, 1.0) );
+
+  MultiDisparityFactor factor(key, disparities, uv, cameraPose, model);
+
+  // basis = [0 1 0; -1 0 0]
+  Vector theta = Vector_(4,0.0,0.0,1.0,20.0);
+  OrientedPlane3 p(theta);
+  factor.Rn(p);
+  Matrix actualRn = factor.Rn();
+  Matrix expectedRn = Matrix_(2,3, 30.0, -20.0, 0.0, 60.0, -40.0, 0.0);
+
+  EXPECT(assert_equal( expectedRn,actualRn,1e-8) );
+}
+
+TEST(MutliDisparityFactor,R)
+{
+  Key key(1);
+  Vector disparities = Vector_(2, 1.0, 1.0); // matlab generated values
+
+  Eigen::Matrix<double,Eigen::Dynamic,3> uv;
+  uv.resize(2,3);
+  uv.block<2,3>(0,0) << 20.0, 30.0, 70.0, 40.0, 60.0, 70.0;
+  SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true);
+
+  gtsam::Pose3 cameraPose( gtsam::Rot3(), gtsam::Point3(1.0, 1.0, 1.0) );
+
+  MultiDisparityFactor factor(key, disparities, uv, cameraPose, model);
+
+  // basis = [0 1 0; -1 0 0]
+  Vector theta = Vector_(4,0.0,0.0,1.0,20.0);
+  OrientedPlane3 p(theta);
+  factor.Rn(p);
+  factor.Rd(p);
+  factor.R(p);
+
+  Matrix expectedR;
+  expectedR.resize(3,3);
+  expectedR <<  0,    10,     0,
+               -10,     0,     0,
+                 0,     0,     0;
+  Matrix actualR = factor.getR(0);
+
+  EXPECT(assert_equal( expectedR,actualR,1e-8) );
+  expectedR <<  0,    20,     0,
+                 -20,     0,     0,
+                   0,     0,     0;
+
+  actualR = factor.getR(1);
+  EXPECT(assert_equal( expectedR,actualR,1e-8) );
+}
+
+TEST(MutliDisparityFactor,H)
+{
+  Key key(1);
+  Vector disparities = Vector_(2, 20.0, 40.0); // matlab generated values
+
+  Eigen::Matrix<double,Eigen::Dynamic,3> uv;
+  uv.resize(2,3);
+  uv.block<2,3>(0,0) << 20.0, 30.0, 70.0, 40.0, 60.0, 70.0;
+  SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true);
+
+  gtsam::Pose3 cameraPose( gtsam::Rot3(), gtsam::Point3(1.0, 1.0, 1.0) );
+
+  MultiDisparityFactor factor(key, disparities, uv, cameraPose, model);
+
+  // basis = [0 1 0; -1 0 0]
+  Vector theta = Vector_(4,0.25,1.75,1.0,20.0);
+  OrientedPlane3 p(theta);
+
+  Matrix actualH;
+  factor.R(p);
+
+  Vector theta1 = Vector_(4,0.45,0.45,1.0,20.0);
+  OrientedPlane3 p1(theta1);
+
+  Vector err = factor.evaluateError(p1,actualH);
+
+  Matrix expectedH = numericalDerivative11<OrientedPlane3>(
+      boost::bind(&MultiDisparityFactor::evaluateError, &factor, _1, boost::none), p1);
+
+  cout << "expectedH :" << expectedH << "\n";
+  cout << "actualH :" << actualH << "\n";
+//  EXPECT(assert_equal( expectedH,actualH,1e-8) );
 }
 
 /* ************************************************************************* */