project2 on Unit3, with derivatives

gtsam/geometry/CameraSet.h

@@ -109,6 +109,8 @@ public:
     // Allocate derivatives
     if (E)
       E->resize(ZDim * m, 3);
+    if (Fs)
+      Fs->resize(m);
 
     // Project and fill derivatives
     for (size_t i = 0; i < m; i++) {
@@ -116,7 +118,7 @@ public:
       Eigen::Matrix<double, ZDim, 3> Ei;
       z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0);
       if (Fs)
-        Fs->push_back(Fi);
+        (*Fs)[i] = Fi;
       if (E)
         E->block<ZDim, 3>(ZDim * i, 0) = Ei;
     }
@@ -125,18 +127,33 @@ public:
   }
 
   /**
-   * Project a point, with derivatives in this, point, and calibration
+   * Project a point at infinity, with derivatives in this, point, and calibration
    * throws CheiralityException
    */
-  std::vector<Z> projectAtInfinity(const Point3& point) const {
+  std::vector<Z> project2(const Unit3& point, //
+      boost::optional<FBlocks&> Fs = boost::none, //
+      boost::optional<Matrix&> E = boost::none) const {
 
     // Allocate result
     size_t m = this->size();
     std::vector<Z> z(m);
 
+    // Allocate derivatives
+    if (E)
+      E->resize(ZDim * m, 2);
+    if (Fs)
+      Fs->resize(m);
+
     // Project and fill derivatives
-    for (size_t i = 0; i < m; i++)
+    for (size_t i = 0; i < m; i++) {
-      z[i] = this->at(i).projectPointAtInfinity(point);
+      MatrixZD Fi;
+      Eigen::Matrix<double, ZDim, 2> Ei;
+      z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0);
+      if (Fs)
+        (*Fs)[i] = Fi;
+      if (E)
+        E->block<ZDim, 2>(ZDim * i, 0) = Ei;
+    }
 
     return z;
   }
@@ -149,10 +166,10 @@ public:
   }
 
   /// Calculate vector [project2(point)-z] of re-projection errors, from point at infinity
-  // TODO: take Unit3 instead
+  Vector reprojectionError(const Unit3& point, const std::vector<Z>& measured,
-  Vector reprojectionErrorAtInfinity(const Point3& point,
+      boost::optional<FBlocks&> Fs = boost::none, //
-      const std::vector<Z>& measured) const {
+      boost::optional<Matrix&> E = boost::none) const {
-    return ErrorVector(projectAtInfinity(point), measured);
+    return ErrorVector(project2(point,Fs,E), measured);
   }
 
   /**

gtsam/geometry/tests/testCameraSet.cpp

@@ -50,12 +50,12 @@ TEST(CameraSet, Pinhole) {
   EXPECT(assert_equal(expected, z[1]));
 
   // Calculate expected derivatives using Pinhole
-  Matrix43 actualE;
+  Matrix actualE;
   Matrix29 F1;
   {
     Matrix23 E1;
-    Matrix23 H1;
     camera.project2(p, F1, E1);
+    actualE.resize(4,3);
     actualE << E1, E1;
   }
 
@@ -114,11 +114,22 @@ TEST(CameraSet, Pinhole) {
   EXPECT(assert_equal((Matrix )(2.0 * schur + A), actualReduced.matrix()));
 
   // reprojectionErrorAtInfinity
+  Unit3 pointAtInfinity(0, 0, 1000);
   EXPECT(
-      assert_equal(Point3(0, 0, 1),
+      assert_equal(pointAtInfinity,
           camera.backprojectPointAtInfinity(Point2())));
-  actualV = set.reprojectionErrorAtInfinity(p, measured);
+  actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E);
   EXPECT(assert_equal(expectedV, actualV));
+  LONGS_EQUAL(2, Fs.size());
+  {
+    Matrix22 E1;
+    camera.project2(pointAtInfinity, F1, E1);
+    actualE.resize(4,2);
+    actualE << E1, E1;
+  }
+  EXPECT(assert_equal(F1, Fs[0]));
+  EXPECT(assert_equal(F1, Fs[1]));
+  EXPECT(assert_equal(actualE, E));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testPinholeSet.cpp

@@ -53,12 +53,12 @@ TEST(PinholeSet, Stereo) {
   }
 
   // Check computed derivatives
-  PinholeSet<CalibratedCamera>::FBlocks F;
+  PinholeSet<CalibratedCamera>::FBlocks Fs;
   Matrix E;
-  set.project2(p, F, E);
+  set.project2(p, Fs, E);
-  LONGS_EQUAL(2, F.size());
+  LONGS_EQUAL(2, Fs.size());
-  EXPECT(assert_equal(F1, F[0]));
+  EXPECT(assert_equal(F1, Fs[0]));
-  EXPECT(assert_equal(F1, F[1]));
+  EXPECT(assert_equal(F1, Fs[1]));
   EXPECT(assert_equal(actualE, E));
 
   // Instantiate triangulateSafe
@@ -90,23 +90,25 @@ TEST(PinholeSet, Pinhole) {
   EXPECT(assert_equal(expected, z[1]));
 
   // Calculate expected derivatives using Pinhole
-  Matrix43 actualE;
+  Matrix actualE;
   Matrix F1;
   {
     Matrix23 E1;
-    Matrix23 H1;
     camera.project2(p, F1, E1);
+    actualE.resize(4, 3);
     actualE << E1, E1;
   }
 
   // Check computed derivatives
-  PinholeSet<Camera>::FBlocks F;
+  {
-  Matrix E, H;
+    PinholeSet<Camera>::FBlocks Fs;
-  set.project2(p, F, E);
+    Matrix E;
-  LONGS_EQUAL(2, F.size());
+    set.project2(p, Fs, E);
-  EXPECT(assert_equal(F1, F[0]));
-  EXPECT(assert_equal(F1, F[1]));
     EXPECT(assert_equal(actualE, E));
+    LONGS_EQUAL(2, Fs.size());
+    EXPECT(assert_equal(F1, Fs[0]));
+    EXPECT(assert_equal(F1, Fs[1]));
+  }
 
   // Check errors
   ZZ measured;
@@ -120,10 +122,25 @@ TEST(PinholeSet, Pinhole) {
   EXPECT(assert_equal(expectedV, actualV));
 
   // reprojectionErrorAtInfinity
+  Unit3 pointAtInfinity(0, 0, 1000);
+  {
+    Matrix22 E1;
+    camera.project2(pointAtInfinity, F1, E1);
+    actualE.resize(4, 2);
+    actualE << E1, E1;
+  }
   EXPECT(
-      assert_equal(Point3(0, 0, 1),
+      assert_equal(pointAtInfinity,
           camera.backprojectPointAtInfinity(Point2())));
-  actualV = set.reprojectionErrorAtInfinity(p, measured);
+  {
+    PinholeSet<Camera>::FBlocks Fs;
+    Matrix E;
+    actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E);
+    EXPECT(assert_equal(actualE, E));
+    LONGS_EQUAL(2, Fs.size());
+    EXPECT(assert_equal(F1, Fs[0]));
+    EXPECT(assert_equal(F1, Fs[1]));
+  }
   EXPECT(assert_equal(expectedV, actualV));
 
   // Instantiate triangulateSafe