reprojectionErrors

gtsam/geometry/CameraSet.h

@@ -43,6 +43,24 @@ protected:
   static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
   static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
 
+  /// Make a vector of re-projection errors
+  static Vector ErrorVector(const std::vector<Z>& predicted,
+      const std::vector<Z>& measured) {
+
+    // Check size
+    size_t m = predicted.size();
+    if (measured.size() != m)
+      throw std::runtime_error("CameraSet::errors: size mismatch");
+
+    // Project and fill derivatives
+    Vector b(ZDim * m);
+    for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
+      Z e = predicted[i] - measured[i];
+      b.segment<ZDim>(row) = e.vector();
+    }
+    return b;
+  }
+
 public:
 
   /// Definitions for blocks of F
@@ -77,28 +95,71 @@ public:
    * Project a point, with derivatives in this, point, and calibration
    * throws CheiralityException
    */
-  std::vector<Z> project(const Point3& point, boost::optional<Matrix&> F =
+  std::vector<Z> project(const Point3& point, //
-      boost::none, boost::optional<Matrix&> E = boost::none,
+      boost::optional<Matrix&> F = boost::none, //
+      boost::optional<Matrix&> E = boost::none, //
       boost::optional<Matrix&> H = boost::none) const {
 
-    size_t nrCameras = this->size();
+    // Allocate result
-    if (F) F->resize(ZDim * nrCameras, 6);
+    size_t m = this->size();
-    if (E) E->resize(ZDim * nrCameras, 3);
+    std::vector<Z> z(m);
-    if (H && Dim > 6) H->resize(ZDim * nrCameras, Dim - 6);
-    std::vector<Z> z(nrCameras);
 
-    for (size_t i = 0; i < nrCameras; i++) {
+    // Allocate derivatives
-      Eigen::Matrix<double, ZDim, 6> Fi;
+    if (F)
-      Eigen::Matrix<double, ZDim, 3> Ei;
+      F->resize(ZDim * m, 6);
-      Eigen::Matrix<double, ZDim, Dim - 6> Hi;
+    if (E)
+      E->resize(ZDim * m, 3);
+    if (H && Dim > 6)
+      H->resize(ZDim * m, Dim - 6);
+
+    Eigen::Matrix<double, ZDim, 6> Fi;
+    Eigen::Matrix<double, ZDim, 3> Ei;
+    Eigen::Matrix<double, ZDim, Dim - 6> Hi;
+
+    // Project and fill derivatives
+    for (size_t i = 0; i < m; i++) {
       z[i] = this->at(i).project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0);
-      if (F) F->block<ZDim, 6>(ZDim * i, 0) = Fi;
+      if (F)
-      if (E) E->block<ZDim, 3>(ZDim * i, 0) = Ei;
+        F->block<ZDim, 6>(ZDim * i, 0) = Fi;
-      if (H) H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi;
+      if (E)
+        E->block<ZDim, 3>(ZDim * i, 0) = Ei;
+      if (H)
+        H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi;
     }
+
     return z;
   }
 
+  /**
+   * Project a point, with derivatives in this, point, and calibration
+   * throws CheiralityException
+   */
+  std::vector<Z> projectAtInfinity(const Point3& point) const {
+
+    // Allocate result
+    size_t m = this->size();
+    std::vector<Z> z(m);
+
+    // Project and fill derivatives
+    for (size_t i = 0; i < m; i++)
+      z[i] = this->at(i).projectPointAtInfinity(point);
+
+    return z;
+  }
+
+  /// Calculate vector of re-projection errors
+  Vector reprojectionErrors(const Point3& point,
+      const std::vector<Z>& measured) const {
+    return ErrorVector(project(point), measured);
+  }
+
+  /// Calculate vector of re-projection errors, from point at infinity
+  // TODO: take Unit3 instead
+  Vector reprojectionErrorsAtInfinity(const Point3& point,
+      const std::vector<Z>& measured) const {
+    return ErrorVector(projectAtInfinity(point), measured);
+  }
+
 private:
 
   /// Serialization function

gtsam/geometry/tests/testCameraSet.cpp

@@ -37,16 +37,16 @@ TEST(CameraSet, Pinhole) {
   set.push_back(camera);
   set.push_back(camera);
   Point3 p(0, 0, 1);
-  CHECK(assert_equal(set, set));
+  EXPECT(assert_equal(set, set));
   CameraSet<Camera> set2 = set;
   set2.push_back(camera);
-  CHECK(!set.equals(set2));
+  EXPECT(!set.equals(set2));
 
   // Check measurements
   Point2 expected;
   ZZ z = set.project(p);
-  CHECK(assert_equal(expected, z[0]));
+  EXPECT(assert_equal(expected, z[0]));
-  CHECK(assert_equal(expected, z[1]));
+  EXPECT(assert_equal(expected, z[1]));
 
   // Calculate expected derivatives using Pinhole
   Matrix46 actualF;
@@ -65,9 +65,27 @@ TEST(CameraSet, Pinhole) {
   // Check computed derivatives
   Matrix F, E, H;
   set.project(p, F, E, H);
-  CHECK(assert_equal(actualF, F));
+  EXPECT(assert_equal(actualF, F));
-  CHECK(assert_equal(actualE, E));
+  EXPECT(assert_equal(actualE, E));
-  CHECK(assert_equal(actualH, H));
+  EXPECT(assert_equal(actualH, H));
+
+  // Check errors
+  ZZ measured;
+  measured.push_back(Point2(1, 2));
+  measured.push_back(Point2(3, 4));
+  Vector4 expectedV;
+
+  // reprojectionErrors
+  expectedV << -1, -2, -3, -4;
+  Vector actualV = set.reprojectionErrors(p, measured);
+  EXPECT(assert_equal(expectedV, actualV));
+
+  // reprojectionErrorsAtInfinity
+  EXPECT(
+      assert_equal(Point3(0, 0, 1),
+          camera.backprojectPointAtInfinity(Point2())));
+  actualV = set.reprojectionErrorsAtInfinity(p, measured);
+  EXPECT(assert_equal(expectedV, actualV));
 }
 
 /* ************************************************************************* */
@@ -84,8 +102,8 @@ TEST(CameraSet, Stereo) {
   // Check measurements
   StereoPoint2 expected(0, -1, 0);
   ZZ z = set.project(p);
-  CHECK(assert_equal(expected, z[0]));
+  EXPECT(assert_equal(expected, z[0]));
-  CHECK(assert_equal(expected, z[1]));
+  EXPECT(assert_equal(expected, z[1]));
 
   // Calculate expected derivatives using Pinhole
   Matrix66 actualF;
@@ -101,8 +119,8 @@ TEST(CameraSet, Stereo) {
   // Check computed derivatives
   Matrix F, E;
   set.project(p, F, E);
-  CHECK(assert_equal(actualF, F));
+  EXPECT(assert_equal(actualF, F));
-  CHECK(assert_equal(actualE, E));
+  EXPECT(assert_equal(actualE, E));
 }
 
 /* ************************************************************************* */