Merged in feature/extraSmartTests (pull request #109)

Extra test for smart factors

gtsam/slam/RegularHessianFactor.h

@@ -43,6 +43,15 @@ public:
       HessianFactor(js, Gs, gs, f) {
   }
 
+  /** Construct a binary factor.  Gxx are the upper-triangle blocks of the
+   * quadratic term (the Hessian matrix), gx the pieces of the linear vector
+   * term, and f the constant term.
+   */
+  RegularHessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12,
+      const Vector& g1, const Matrix& G22, const Vector& g2, double f) :
+      HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) {
+  }
+
   /** Constructor with an arbitrary number of keys and with the augmented information matrix
    *   specified as a block matrix. */
   template<typename KEYS>
@@ -172,6 +181,13 @@ public:
     }
   }
 
+  /* ************************************************************************* */
+
+}; // end class RegularHessianFactor
+
+// traits
+template<size_t D> struct traits<RegularHessianFactor<D> > : public Testable<
+    RegularHessianFactor<D> > {
 };
 
 }

gtsam/slam/RegularImplicitSchurFactor.h

@@ -89,18 +89,27 @@ public:
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     std::cout << " RegularImplicitSchurFactor " << std::endl;
     Factor::print(s);
-    std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl;
-    std::cout << " E_ \n" << E_ << std::endl;
-    std::cout << " b_ \n" << b_.transpose() << std::endl;
+    for (size_t pos = 0; pos < size(); ++pos) {
+      std::cout << "Fblock:\n" << Fblocks_[pos].second << std::endl;
+    }
+    std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl;
+    std::cout << "E:\n" << E_ << std::endl;
+    std::cout << "b:\n" << b_.transpose() << std::endl;
   }
 
   /// equals
   bool equals(const GaussianFactor& lf, double tol) const {
-    if (!dynamic_cast<const RegularImplicitSchurFactor*>(&lf))
-      return false;
-    else {
+    const This* f = dynamic_cast<const This*>(&lf);
+    if (!f)
       return false;
+    for (size_t pos = 0; pos < size(); ++pos) {
+      if (keys_[pos] != f->keys_[pos]) return false;
+      if (Fblocks_[pos].first != f->Fblocks_[pos].first) return false;
+      if (!equal_with_abs_tol(Fblocks_[pos].second,f->Fblocks_[pos].second,tol)) return false;
     }
+    return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
+        && equal_with_abs_tol(E_, f->E_, tol)
+        && equal_with_abs_tol(b_, f->b_, tol);
   }
 
   /// Degrees of freedom of camera
@@ -460,7 +469,12 @@ public:
 
 
 };
-// RegularImplicitSchurFactor
+// end class RegularImplicitSchurFactor
+
+// traits
+template<size_t D> struct traits<RegularImplicitSchurFactor<D> > : public Testable<
+    RegularImplicitSchurFactor<D> > {
+};
 
 }
 

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -380,6 +380,84 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
   EXPECT(assert_equal(bodyPose3,result.at<Pose3>(x3)));
 }
 
+/* *************************************************************************/
+TEST( SmartProjectionPoseFactor, Factors ){
+
+  // Default cameras for simple derivatives
+  Rot3 R;
+  static Cal3_S2::shared_ptr K(new Cal3_S2(100, 100, 0, 0, 0));
+  SimpleCamera cam1(Pose3(R,Point3(0,0,0)),*K), cam2(Pose3(R,Point3(1,0,0)),*K);
+
+  // one landmarks 1m in front of camera
+  Point3 landmark1(0,0,10);
+
+  vector<Point2> measurements_cam1;
+
+  // Project 2 landmarks into 2 cameras
+  cout << cam1.project(landmark1) << endl;
+  cout << cam2.project(landmark1) << endl;
+  measurements_cam1.push_back(cam1.project(landmark1));
+  measurements_cam1.push_back(cam2.project(landmark1));
+
+  // Create smart factors
+  std::vector<Key> views;
+  views.push_back(x1);
+  views.push_back(x2);
+
+  SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>();
+  smartFactor1->add(measurements_cam1, views, model, K);
+
+  SmartFactor::Cameras cameras;
+  cameras.push_back(cam1);
+  cameras.push_back(cam2);
+
+  // Make sure triangulation works
+  LONGS_EQUAL(2,smartFactor1->triangulateSafe(cameras));
+  CHECK(!smartFactor1->isDegenerate());
+  CHECK(!smartFactor1->isPointBehindCamera());
+  boost::optional<Point3> p = smartFactor1->point();
+  CHECK(p);
+  EXPECT(assert_equal(landmark1,*p));
+
+  {
+    // RegularHessianFactor<6>
+    Matrix66 G11; G11.setZero(); G11(0,0) = 100; G11(0,4) = -10; G11(4,0) = -10; G11(4,4) = 1;
+    Matrix66 G12; G12 = -G11; G12(0,2) = -10; G12(4,2) = 1;
+    Matrix66 G22; G22 = G11; G22(0,2) = 10; G22(2,0) = 10; G22(2,2) = 1; G22(2,4) = -1; G22(4,2) = -1;
+
+    Vector6 g1; g1.setZero();
+    Vector6 g2; g2.setZero();
+
+    double f = 0;
+
+    RegularHessianFactor<6> expected(x1, x2, 50 * G11, 50 * G12, g1, 50 * G22, g2, f);
+
+    boost::shared_ptr<RegularHessianFactor<6> > actual =
+          smartFactor1->createHessianFactor(cameras, 0.0);
+    CHECK(assert_equal(expected.information(),actual->information(),1e-8));
+    CHECK(assert_equal(expected,*actual,1e-8));
+  }
+
+  {
+    // RegularImplicitSchurFactor<6>
+    Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10;
+    Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10;
+    Matrix43 E; E.setZero(); E(0,0)=10; E(1,1)=10; E(2,0)=10; E(2,2)=1;E(3,1)=10;
+    const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
+        (make_pair(x1, F1))(make_pair(x2, F2));
+    Matrix3 P = (E.transpose() * E).inverse();
+    Vector4 b; b.setZero();
+
+    RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b);
+
+    boost::shared_ptr<RegularImplicitSchurFactor<6> > actual =
+          smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
+    CHECK(actual);
+    CHECK(assert_equal(expected,*actual));
+  }
+
+}
+
 
 /* *************************************************************************/
 TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){