diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 5bf79e0b4..8c9d11532 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -20,6 +20,7 @@ #include #include #include +#include using namespace std; using namespace gtsam; @@ -29,15 +30,33 @@ using namespace gtsam; #include TEST(CameraSet, Pinhole) { typedef PinholeCamera Camera; + typedef vector ZZ; CameraSet set; - set.add(Camera()); - set.add(Camera()); - Point3 p(0,0,1); - Matrix F,E,H; - vector z = set.project(p,F,E,H); + Camera camera; + set.add(camera); + set.add(camera); + Point3 p(0, 0, 1); + // Calculate actual + Matrix46 actualF; + Matrix43 actualE; + Matrix43 actualH; + { + Matrix26 F1; + Matrix23 E1; + Matrix23 H1; + camera.project(p, F1, E1, H1); + actualE << E1, E1; + actualF << F1, F1; + actualH << H1, H1; + } Point2 expected; - CHECK(assert_equal(expected,z[0])); - CHECK(assert_equal(expected,z[1])); + Matrix F, E, H; + ZZ z = set.project(p, F, E, H); + CHECK(assert_equal(expected, z[0])); + CHECK(assert_equal(expected, z[1])); + CHECK(assert_equal(actualF, F)); + CHECK(assert_equal(actualE, E)); + CHECK(assert_equal(actualH, H)); } /* ************************************************************************* */