From 179b5c09ae11400037e3b7e1dd3191bbf5cdf273 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 29 Aug 2009 06:54:10 +0000 Subject: [PATCH] static members for creating "level" cameras as we find on idealized robots --- cpp/CalibratedCamera.cpp | 14 ++++++++++++++ cpp/CalibratedCamera.h | 6 ++++++ cpp/Pose2.h | 5 +++++ cpp/SimpleCamera.cpp | 12 ++++++++++++ cpp/SimpleCamera.h | 6 ++++++ cpp/testCalibratedCamera.cpp | 28 ++++++++++++++++++++++++++++ cpp/testSimpleCamera.cpp | 1 + 7 files changed, 72 insertions(+) diff --git a/cpp/CalibratedCamera.cpp b/cpp/CalibratedCamera.cpp index ed9317307..f714c499e 100644 --- a/cpp/CalibratedCamera.cpp +++ b/cpp/CalibratedCamera.cpp @@ -22,6 +22,8 @@ namespace gtsam { return Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); } + /* ************************************************************************* */ + // Methods /* ************************************************************************* */ CalibratedCamera::CalibratedCamera(const Pose3& pose) : @@ -31,6 +33,15 @@ namespace gtsam { CalibratedCamera::~CalibratedCamera() { } + CalibratedCamera CalibratedCamera::level(const Pose2& pose2, double height) { + double st = sin(pose2.theta()), ct = cos(pose2.theta()); + Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); + Rot3 wRc(x, y, z); + Point3 t(pose2.x(), pose2.y(), height); + Pose3 pose3(wRc, t); + return CalibratedCamera(pose3); + } + Point2 CalibratedCamera::project(const Point3 & P) const { Point3 cameraPoint = transform_to(pose_, P); Point2 intrinsic = project_to_camera(cameraPoint); @@ -38,6 +49,9 @@ namespace gtsam { } /* ************************************************************************* */ + // measurement functions and derivatives + /* ************************************************************************* */ + Point2 project(const CalibratedCamera& camera, const Point3& point) { return camera.project(point); } diff --git a/cpp/CalibratedCamera.h b/cpp/CalibratedCamera.h index cd48e7acf..36f17ba8a 100644 --- a/cpp/CalibratedCamera.h +++ b/cpp/CalibratedCamera.h @@ -9,6 +9,7 @@ #define CalibratedCAMERA_H_ #include "Point2.h" +#include "Pose2.h" #include "Pose3.h" namespace gtsam { @@ -41,6 +42,11 @@ namespace gtsam { return pose_; } + /** + * Create a level camera at the given 2D pose and height + */ + static CalibratedCamera level(const Pose2& pose2, double height); + Point2 project(const Point3& P) const; }; diff --git a/cpp/Pose2.h b/cpp/Pose2.h index 3eec8d79a..366b30e5c 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -42,6 +42,11 @@ namespace gtsam { x_(t.x()), y_(t.y()), theta_(theta) { } + /** get functions for x, y, theta */ + double x() const { return x_;} + double y() const { return y_;} + double theta() const { return theta_;} + /** print with optional string */ void print(const std::string& s = "") const; diff --git a/cpp/SimpleCamera.cpp b/cpp/SimpleCamera.cpp index c207291a7..208967da3 100644 --- a/cpp/SimpleCamera.cpp +++ b/cpp/SimpleCamera.cpp @@ -12,6 +12,11 @@ namespace gtsam { /* ************************************************************************* */ + SimpleCamera::SimpleCamera(const Cal3_S2& K, + const CalibratedCamera& calibrated) : + calibrated_(calibrated), K_(K) { + } + SimpleCamera::SimpleCamera(const Cal3_S2& K, const Pose3& pose) : calibrated_(pose), K_(K) { } @@ -25,7 +30,14 @@ namespace gtsam { return projection; } + SimpleCamera::SimpleCamera level(const Cal3_S2& K, const Pose2& pose2, double height) { + return SimpleCamera(K, CalibratedCamera::level(pose2, height)); + } + /* ************************************************************************* */ + // measurement functions and derivatives + /* ************************************************************************* */ + Point2 project(const SimpleCamera& camera, const Point3& point) { return camera.project(point); } diff --git a/cpp/SimpleCamera.h b/cpp/SimpleCamera.h index 40f0f3b0f..037c81c52 100644 --- a/cpp/SimpleCamera.h +++ b/cpp/SimpleCamera.h @@ -25,6 +25,7 @@ namespace gtsam { Cal3_S2 K_; // Calibration public: + SimpleCamera(const Cal3_S2& K, const CalibratedCamera& calibrated); SimpleCamera(const Cal3_S2& K, const Pose3& pose); virtual ~SimpleCamera(); @@ -38,6 +39,11 @@ namespace gtsam { Point2 project(const Point3& P) const; + /** + * Create a level camera at the given 2D pose and height + */ + static CalibratedCamera level(const Cal3_S2& K, const Pose2& pose2, double height); + // Friends friend Matrix Dproject_pose(const SimpleCamera& camera, const Point3& point); friend Matrix Dproject_point(const SimpleCamera& camera, const Point3& point); diff --git a/cpp/testCalibratedCamera.cpp b/cpp/testCalibratedCamera.cpp index 55b3f252c..fb7e6bc20 100644 --- a/cpp/testCalibratedCamera.cpp +++ b/cpp/testCalibratedCamera.cpp @@ -33,6 +33,34 @@ TEST( CalibratedCamera, constructor) CHECK(assert_equal( camera.pose(), pose1)); } +/* ************************************************************************* */ +TEST( CalibratedCamera, level1) +{ + // Create a level camera, looking in X-direction + Pose2 pose2(100,200,0); + CalibratedCamera camera = CalibratedCamera::level(pose2, 300); + + // expected + Point3 x(0,-1,0),y(0,0,-1),z(1,0,0); + Rot3 wRc(x,y,z); + Pose3 expected(wRc,Point3(100,200,300)); + CHECK(assert_equal( camera.pose(), expected)); +} + +/* ************************************************************************* */ +TEST( CalibratedCamera, level2) +{ + // Create a level camera, looking in Y-direction + Pose2 pose2(400,300,M_PI_2); + CalibratedCamera camera = CalibratedCamera::level(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( CalibratedCamera, project) { diff --git a/cpp/testSimpleCamera.cpp b/cpp/testSimpleCamera.cpp index 816e83734..93824eef5 100644 --- a/cpp/testSimpleCamera.cpp +++ b/cpp/testSimpleCamera.cpp @@ -4,6 +4,7 @@ * based on testVSLAMFactor.cpp */ +#include #include #include