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

add backproject

release/4.3a0
Kai Ni 2010-03-19 08:32:55 +00:00
parent b895760404
commit 62d24a8d48
5 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cpp/CalibratedCamera.cpp
View File

@ -23,6 +23,10 @@ namespace gtsam {
return Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); return Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
} }
Point3 backproject_from_camera(const Point2& p, const double scale) {
return Point3(p.x() * scale, p.y() * scale, scale);
}
/* ************************************************************************* */ /* ************************************************************************* */
// Methods // Methods
/* ************************************************************************* */ /* ************************************************************************* */

5
cpp/CalibratedCamera.h
View File

@ -26,6 +26,11 @@ namespace gtsam {
*/ */
Matrix Dproject_to_camera1(const Point3& cameraPoint); /*2by3 <--*/ Matrix Dproject_to_camera1(const Point3& cameraPoint); /*2by3 <--*/
/**
* backproject a 2-dimensional point to a 3-dimension point
*/
Point3 backproject_from_camera(const Point2& p, const double scale);
/** /**
* A Calibrated camera class [R|-R't], calibration K=I. * A Calibrated camera class [R|-R't], calibration K=I.
* If calibration is known, it is more computationally efficient * If calibration is known, it is more computationally efficient

6
cpp/SimpleCamera.cpp
View File

@ -38,6 +38,12 @@ namespace gtsam {
return projected.first; return projected.first;
} }
Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const {
Point2 intrinsic = K_.calibrate(projection);
Point3 cameraPoint = backproject_from_camera(intrinsic, scale);
return transform_from(calibrated_.pose(), cameraPoint);
}
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)); return SimpleCamera(K, CalibratedCamera::level(pose2, height));
} }

5
cpp/SimpleCamera.h
View File

@ -48,6 +48,11 @@ namespace gtsam {
*/ */
Point2 project(const Point3& P) const; Point2 project(const Point3& P) const;
/**
* backproject a 2d point from the camera up to a given scale
*/
Point3 backproject(const Point2& projection, const double scale) const;
/** /**
* Create a level camera at the given 2D pose and height * Create a level camera at the given 2D pose and height
* @param pose2 specifies the location and viewing direction * @param pose2 specifies the location and viewing direction

9
cpp/testSimpleCamera.cpp
View File

@ -60,6 +60,15 @@ TEST( SimpleCamera, project)
CHECK(assert_equal( camera.project(point4), Point2( 100, 100) )); CHECK(assert_equal( camera.project(point4), Point2( 100, 100) ));
} }
/* ************************************************************************* */
TEST( SimpleCamera, backproject)
{
CHECK(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1));
CHECK(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2));
CHECK(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3));
CHECK(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4));
}
/* ************************************************************************* */ /* ************************************************************************* */
Point2 project2(const Pose3& pose, const Point3& point) { Point2 project2(const Pose3& pose, const Point3& point) {
return project(SimpleCamera(K,pose), point); return project(SimpleCamera(K,pose), point);