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

Merged in feature/addJacobian_Cal3_S2 (pull request #162) 

Add optional Jacobian to calibrate for Cal3_D2 camera instrinsics
release/4.3a0
Frank Dellaert 2015-06-23 09:43:51 -07:00
parent b7b7182148 bb58550ae4
commit 348e2144ce
3 changed files with 42 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/geometry/Cal3_S2.cpp
View File

@ -83,10 +83,21 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
}
/* ************************************************************************* */
Point2 Cal3_S2::calibrate(const Point2& p) const {
Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal,
OptionalJacobian<2,2> Dp) const {
const double u = p.x(), v = p.y();
return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)),
(1 / fy_) * (v - v0_));
double delta_u = u - u0_, delta_v = v - v0_;
double inv_fx = 1/ fx_, inv_fy = 1/fy_;
double inv_fy_delta_v = inv_fy * delta_v, inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v),
inv_fy_delta_v);
if(Dcal)
*Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v, -inv_fx * point.y(),
-inv_fx, inv_fx_s_inv_fy,
0, -inv_fy * point.y(), 0, 0, -inv_fy;
if(Dp)
*Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
return point;
}
/* ************************************************************************* */

5
gtsam/geometry/Cal3_S2.h
View File

@ -156,9 +156,12 @@ public:
/**
* convert image coordinates uv to intrinsic coordinates xy
* @param p point in image coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& p) const;
Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const;
/**
* convert homogeneous image coordinates to intrinsic coordinates

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

@ -26,6 +26,8 @@ GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2)
static Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2);
static Point2 p(1, -2);
static Point2 p_uv(1320.3, 1740);
static Point2 p_xy(2, 3);
/* ************************************************************************* */
TEST( Cal3_S2, easy_constructor)
@ -73,6 +75,27 @@ TEST( Cal3_S2, Duncalibrate2)
CHECK(assert_equal(numerical,computed,1e-9));
}
Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {return k.calibrate(pt); }
/* ************************************************************************* */
TEST(Cal3_S2, Dcalibrate1)
{
Matrix computed;
Point2 expected = K.calibrate(p_uv, computed, boost::none);
Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
CHECK(assert_equal(expected, p_xy, 1e-8));
CHECK(assert_equal(numerical, computed, 1e-8));
}
/* ************************************************************************* */
TEST(Cal3_S2, Dcalibrate2)
{
Matrix computed;
Point2 expected = K.calibrate(p_uv, boost::none, computed);
Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
CHECK(assert_equal(expected, p_xy, 1e-8));
CHECK(assert_equal(numerical, computed, 1e-8));
}
/* ************************************************************************* */
TEST( Cal3_S2, assert_equal)
{