From 81538aac55ea9974dd1c0f42bd648dc23412cd41 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 21:48:21 +0100 Subject: [PATCH] reprojectionErrors --- gtsam/geometry/CameraSet.h | 89 ++++++++++++++++++++++---- gtsam/geometry/tests/testCameraSet.cpp | 40 ++++++++---- 2 files changed, 104 insertions(+), 25 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 3a34ca1fd..9d678181b 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -43,6 +43,24 @@ protected: static const int ZDim = traits::dimension; ///< Measurement dimension static const int Dim = traits::dimension; ///< Camera dimension + /// Make a vector of re-projection errors + static Vector ErrorVector(const std::vector& predicted, + const std::vector& 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(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 project(const Point3& point, boost::optional F = - boost::none, boost::optional E = boost::none, + std::vector project(const Point3& point, // + boost::optional F = boost::none, // + boost::optional E = boost::none, // boost::optional H = boost::none) const { - size_t nrCameras = this->size(); - if (F) F->resize(ZDim * nrCameras, 6); - if (E) E->resize(ZDim * nrCameras, 3); - if (H && Dim > 6) H->resize(ZDim * nrCameras, Dim - 6); - std::vector z(nrCameras); + // Allocate result + size_t m = this->size(); + std::vector z(m); - for (size_t i = 0; i < nrCameras; i++) { - Eigen::Matrix Fi; - Eigen::Matrix Ei; - Eigen::Matrix Hi; + // Allocate derivatives + if (F) + F->resize(ZDim * m, 6); + if (E) + E->resize(ZDim * m, 3); + if (H && Dim > 6) + H->resize(ZDim * m, Dim - 6); + + Eigen::Matrix Fi; + Eigen::Matrix Ei; + Eigen::Matrix 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 * i, 0) = Fi; - if (E) E->block(ZDim * i, 0) = Ei; - if (H) H->block(ZDim * i, 0) = Hi; + if (F) + F->block(ZDim * i, 0) = Fi; + if (E) + E->block(ZDim * i, 0) = Ei; + if (H) + H->block(ZDim * i, 0) = Hi; } + return z; } + /** + * Project a point, with derivatives in this, point, and calibration + * throws CheiralityException + */ + std::vector projectAtInfinity(const Point3& point) const { + + // Allocate result + size_t m = this->size(); + std::vector 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& 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& measured) const { + return ErrorVector(projectAtInfinity(point), measured); + } + private: /// Serialization function diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 42cf0f299..4ed9f2f07 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/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 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])); - CHECK(assert_equal(expected, z[1])); + EXPECT(assert_equal(expected, z[0])); + 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)); - CHECK(assert_equal(actualE, E)); - CHECK(assert_equal(actualH, H)); + EXPECT(assert_equal(actualF, F)); + EXPECT(assert_equal(actualE, E)); + 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])); - CHECK(assert_equal(expected, z[1])); + EXPECT(assert_equal(expected, z[0])); + 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)); - CHECK(assert_equal(actualE, E)); + EXPECT(assert_equal(actualF, F)); + EXPECT(assert_equal(actualE, E)); } /* ************************************************************************* */