diff --git a/cpp/SimpleCamera.cpp b/cpp/SimpleCamera.cpp index 208967da3..a8243d5b3 100644 --- a/cpp/SimpleCamera.cpp +++ b/cpp/SimpleCamera.cpp @@ -30,7 +30,7 @@ namespace gtsam { return projection; } - SimpleCamera::SimpleCamera level(const Cal3_S2& K, const Pose2& pose2, double height) { + SimpleCamera SimpleCamera::level(const Cal3_S2& K, const Pose2& pose2, double height) { return SimpleCamera(K, CalibratedCamera::level(pose2, height)); } diff --git a/cpp/SimpleCamera.h b/cpp/SimpleCamera.h index 037c81c52..58099c853 100644 --- a/cpp/SimpleCamera.h +++ b/cpp/SimpleCamera.h @@ -42,7 +42,7 @@ namespace gtsam { /** * Create a level camera at the given 2D pose and height */ - static CalibratedCamera level(const Cal3_S2& K, const Pose2& pose2, double height); + static SimpleCamera level(const Cal3_S2& K, const Pose2& pose2, double height); // Friends friend Matrix Dproject_pose(const SimpleCamera& camera, const Point3& point); diff --git a/cpp/testSimpleCamera.cpp b/cpp/testSimpleCamera.cpp index 93824eef5..3886b049c 100644 --- a/cpp/testSimpleCamera.cpp +++ b/cpp/testSimpleCamera.cpp @@ -37,6 +37,20 @@ TEST( SimpleCamera, constructor) CHECK(assert_equal( camera.pose(), pose1)); } +/* ************************************************************************* */ +TEST( SimpleCamera, level2) +{ + // Create a level camera, looking in Y-direction + Pose2 pose2(400,300,M_PI_2); + SimpleCamera camera = SimpleCamera::level(K, pose2, 100); + + // expected + Point3 x(1,0,0),y(0,0,-1),z(0,1,0); + Rot3 wRc(x,y,z); + Pose3 expected(wRc,Point3(400,300,100)); + CHECK(assert_equal( camera.pose(), expected)); +} + /* ************************************************************************* */ TEST( SimpleCamera, project) {