12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

lookat: a convenient function to set up camera pose

release/4.3a0
Duy-Nguyen Ta 2012-06-06 09:33:03 +00:00
parent 5256130afd
commit 092093444c
2 changed files with 38 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
gtsam/geometry/PinholeCamera.h
View File

@ -149,16 +149,34 @@ namespace gtsam {
* (theta 0 = looking in direction of positive X axis)
*/
static PinholeCamera level(const Pose2& pose2, double height) {
return PinholeCamera::level(Calibration(), pose2, height);
return PinholeCamera::level(Calibration(), pose2, height);
}
static PinholeCamera level(const Calibration &K, const Pose2& pose2, double height) {
const double st = sin(pose2.theta()), ct = cos(pose2.theta());
const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
const Rot3 wRc(x, y, z);
const Point3 t(pose2.x(), pose2.y(), height);
const Pose3 pose3(wRc, t);
return PinholeCamera(pose3, K);
const double st = sin(pose2.theta()), ct = cos(pose2.theta());
const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
const Rot3 wRc(x, y, z);
const Point3 t(pose2.x(), pose2.y(), height);
const Pose3 pose3(wRc, t);
return PinholeCamera(pose3, K);
}
/**
* Create a camera at the given eye position looking at a target point in the scene
* with the specified up direction vector.
* @param eye specifies the camera position
* @param target the point to look at
* @param upVector specifies the camera up direction vector,
* doesn't need to be on the image plane nor orthogonal to the viewing axis
* @param K optional calibration parameter
*/
static PinholeCamera lookat(const Point3& eye, const Point3& target, const Point3& upVector, const Calibration& K = Calibration()) {
Point3 zc = target-eye;
zc = zc/zc.norm();
Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
Point3 yc = zc.cross(xc);
Pose3 pose3(Rot3(xc,yc,zc), eye);
return PinholeCamera(pose3, K);
}
/// @}

13
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -63,6 +63,19 @@ TEST( SimpleCamera, level2)
CHECK(assert_equal( camera.pose(), expected));
}
/* ************************************************************************* */
TEST( SimpleCamera, lookat)
{
// Create a level camera, looking in Y-direction
Point3 C(10.0,0.0,0.0);
SimpleCamera camera = SimpleCamera::lookat(C, Point3(), Point3(0.0,0.0,1.0));
// expected
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
Pose3 expected(Rot3(xc,yc,zc),C);
CHECK(assert_equal( camera.pose(), expected));
}
/* ************************************************************************* */
TEST( SimpleCamera, project)
{