Merge Cal3_S2 changes from branch 'origin/release/2.4.0'

release/4.3a0
Frank Dellaert 2013-12-22 21:44:37 -05:00
commit 2d90fdc9e8
3 changed files with 21 additions and 3 deletions

View File

@ -89,6 +89,11 @@ Point2 Cal3_S2::calibrate(const Point2& p) const {
(1 / fy_) * (v - v0_));
}
/* ************************************************************************* */
Vector3 Cal3_S2::calibrate(const Vector3& p) const {
return matrix_inverse() * p;
}
/* ************************************************************************* */
} // namespace gtsam

View File

@ -158,6 +158,13 @@ public:
*/
Point2 calibrate(const Point2& p) const;
/**
* convert homogeneous image coordinates to intrinsic coordinates
* @param p point in image coordinates
* @return point in intrinsic coordinates
*/
Vector3 calibrate(const Vector3& p) const;
/// @}
/// @name Manifold
/// @{

View File

@ -42,12 +42,18 @@ TEST( Cal3_S2, easy_constructor)
/* ************************************************************************* */
TEST( Cal3_S2, calibrate)
{
Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2);
Point2 intrinsic(2,3);
Point2 expectedimage(1320.3, 1740);
Point2 imagecoordinates = K1.uncalibrate(intrinsic);
Point2 imagecoordinates = K.uncalibrate(intrinsic);
CHECK(assert_equal(expectedimage,imagecoordinates));
CHECK(assert_equal(intrinsic,K1.calibrate(imagecoordinates)));
CHECK(assert_equal(intrinsic,K.calibrate(imagecoordinates)));
}
/* ************************************************************************* */
TEST( Cal3_S2, calibrate_homogeneous) {
Vector3 intrinsic(2, 3, 1);
Vector3 image(1320.3, 1740, 1);
CHECK(assert_equal((Vector)intrinsic,(Vector)K.calibrate(image)));
}
/* ************************************************************************* */