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

project_to_camera for Unit3 (points at infinity)

release/4.3a0
dellaert 2015-03-05 20:26:47 -08:00
parent 95e00d3052
commit f3f09b17a7
3 changed files with 40 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/CalibratedCamera.cpp
View File

@ -94,6 +94,19 @@ Point2 PinholeBase::project_to_camera(const Point3& pc,
return Point2(u, v); return Point2(u, v);
} }
/* ************************************************************************* */
Point2 PinholeBase::project_to_camera(const Unit3& pc,
OptionalJacobian<2, 2> Dpoint) {
if (Dpoint) {
Matrix32 Dpoint3_pc;
Matrix23 Duv_point3;
Point2 uv = project_to_camera(pc.point3(Dpoint3_pc), Duv_point3);
*Dpoint = Duv_point3 * Dpoint3_pc;
return uv;
} else
return project_to_camera(pc.point3());
}
/* ************************************************************************* */ /* ************************************************************************* */
pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const { pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const {
const Point3 pc = pose().transform_to(pw); const Point3 pc = pose().transform_to(pw);

8
gtsam/geometry/CalibratedCamera.h
View File

@ -167,6 +167,14 @@ public:
static Point2 project_to_camera(const Point3& pc, // static Point2 project_to_camera(const Point3& pc, //
OptionalJacobian<2, 3> Dpoint = boost::none); OptionalJacobian<2, 3> Dpoint = boost::none);
/**
* Project from 3D point at infinity in camera coordinates into image
* Does *not* throw a CheiralityException, even if pc behind image plane
* @param pc point in camera coordinates
*/
static Point2 project_to_camera(const Unit3& pc, //
OptionalJacobian<2, 2> Dpoint = boost::none);
/// Project a point into the image and check depth /// Project a point into the image and check depth
std::pair<Point2, bool> projectSafe(const Point3& pw) const; std::pair<Point2, bool> projectSafe(const Point3& pw) const;

22
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -88,12 +88,28 @@ TEST( CalibratedCamera, project)
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Point2 project_to_camera1(const Point3& point) {
return PinholeBase::project_to_camera(point);
}
TEST( CalibratedCamera, Dproject_to_camera1) { TEST( CalibratedCamera, Dproject_to_camera1) {
Point3 pp(155,233,131); Point3 pp(155, 233, 131);
Matrix test1; Matrix test1;
CalibratedCamera::project_to_camera(pp, test1); CalibratedCamera::project_to_camera(pp, test1);
Matrix test2 = numericalDerivative11<Point2,Point3>( Matrix test2 = numericalDerivative11<Point2, Point3>(project_to_camera1, pp);
boost::bind(CalibratedCamera::project_to_camera, _1, boost::none), pp); CHECK(assert_equal(test1, test2));
}
/* ************************************************************************* */
static Point2 project_to_camera2(const Unit3& point) {
return PinholeBase::project_to_camera(point);
}
TEST( CalibratedCamera, Dproject_to_cameraInfinity) {
Unit3 pp(0, 0, 1000);
Matrix test1;
CalibratedCamera::project_to_camera(pp, test1);
Matrix test2 = numericalDerivative11<Point2, Unit3>(project_to_camera2, pp);
CHECK(assert_equal(test1, test2)); CHECK(assert_equal(test1, test2));
} }