Added a calibrate call for homogeneous coordinates

release/4.3a0
Frank Dellaert 2013-12-22 21:29:32 -05:00
parent 736e79169d
commit 5f6f4ac05a
5 changed files with 1019 additions and 981 deletions

1974
.cproject

File diff suppressed because it is too large Load Diff

View File

@ -103,7 +103,7 @@ public:
boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ;
/// Conver a pixel coordinate to ideal coordinate
/// Convert pixel coordinates to ideal coordinates
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates

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)));
}
/* ************************************************************************* */