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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1974
.cproject
View File

File diff suppressed because it is too large Load Diff

2
gtsam/geometry/Cal3DS2.h
View File

@ -103,7 +103,7 @@ public:
boost::optional<Matrix&> Dcal = boost::none, boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ; 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; Point2 calibrate(const Point2& p, const double tol=1e-5) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates /// Derivative of uncalibrate wrpt intrinsic coordinates

5
gtsam/geometry/Cal3_S2.cpp
View File

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

7
gtsam/geometry/Cal3_S2.h
View File

@ -158,6 +158,13 @@ public:
*/ */
Point2 calibrate(const Point2& p) const; 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 /// @name Manifold
/// @{ /// @{

12
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -42,12 +42,18 @@ TEST( Cal3_S2, easy_constructor)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Cal3_S2, calibrate) TEST( Cal3_S2, calibrate)
{ {
Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2);
Point2 intrinsic(2,3); Point2 intrinsic(2,3);
Point2 expectedimage(1320.3, 1740); Point2 expectedimage(1320.3, 1740);
Point2 imagecoordinates = K1.uncalibrate(intrinsic); Point2 imagecoordinates = K.uncalibrate(intrinsic);
CHECK(assert_equal(expectedimage,imagecoordinates)); 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)));
} }
/* ************************************************************************* */ /* ************************************************************************* */