diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 9d678181b..43b806857 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -64,8 +64,8 @@ protected: public: /// Definitions for blocks of F - typedef Eigen::Matrix MatrixZD; // F - typedef std::pair FBlock; // Fblocks + typedef Eigen::Matrix MatrixZ6; // F + typedef std::vector FBlocks; /** * print @@ -93,10 +93,12 @@ public: /** * Project a point, with derivatives in this, point, and calibration + * Note that F is a sparse block-diagonal matrix, so instead of a large dense + * matrix this function returns the diagonal blocks. * throws CheiralityException */ std::vector project(const Point3& point, // - boost::optional F = boost::none, // + boost::optional F = boost::none, // boost::optional E = boost::none, // boost::optional H = boost::none) const { @@ -105,8 +107,6 @@ public: std::vector z(m); // Allocate derivatives - if (F) - F->resize(ZDim * m, 6); if (E) E->resize(ZDim * m, 3); if (H && Dim > 6) @@ -120,7 +120,7 @@ public: 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; + F->push_back(Fi); if (E) E->block(ZDim * i, 0) = Ei; if (H) diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 4ed9f2f07..b4f8a75ef 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -49,23 +49,24 @@ TEST(CameraSet, Pinhole) { EXPECT(assert_equal(expected, z[1])); // Calculate expected derivatives using Pinhole - Matrix46 actualF; Matrix43 actualE; Matrix43 actualH; + Matrix F1; { - Matrix26 F1; Matrix23 E1; Matrix23 H1; camera.project(p, F1, E1, H1); actualE << E1, E1; - actualF << F1, F1; actualH << H1, H1; } // Check computed derivatives - Matrix F, E, H; + CameraSet::FBlocks F; + Matrix E, H; set.project(p, F, E, H); - EXPECT(assert_equal(actualF, F)); + LONGS_EQUAL(2,F.size()); + EXPECT(assert_equal(F1, F[0])); + EXPECT(assert_equal(F1, F[1])); EXPECT(assert_equal(actualE, E)); EXPECT(assert_equal(actualH, H)); @@ -106,20 +107,21 @@ TEST(CameraSet, Stereo) { EXPECT(assert_equal(expected, z[1])); // Calculate expected derivatives using Pinhole - Matrix66 actualF; Matrix63 actualE; + Matrix F1; { - Matrix36 F1; Matrix33 E1; camera.project(p, F1, E1); actualE << E1, E1; - actualF << F1, F1; } // Check computed derivatives - Matrix F, E; + CameraSet::FBlocks F; + Matrix E; set.project(p, F, E); - EXPECT(assert_equal(actualF, F)); + LONGS_EQUAL(2,F.size()); + EXPECT(assert_equal(F1, F[0])); + EXPECT(assert_equal(F1, F[1])); EXPECT(assert_equal(actualE, E)); }