From f7bf418c45f7bc7bb17207b798e4fae087164454 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 23 Jun 2015 01:46:50 -0400 Subject: [PATCH 1/8] feature: add jacobian to Cal3_D2 calibrate() --- gtsam/geometry/Cal3_S2.cpp | 18 ++++++++++++++++++ gtsam/geometry/Cal3_S2.h | 10 ++++++++++ 2 files changed, 28 insertions(+) diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 3ec29bbd2..81123d9a9 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -89,6 +89,24 @@ Point2 Cal3_S2::calibrate(const Point2& p) const { (1 / fy_) * (v - v0_)); } +/* ************************************************************************* */ +Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const { + const double u = p.x(), v = p.y(); + double delta_u = u - u0_, delta_v = v - v0_; + double inv_fx = 1/ fx_, inv_fy = 1/fy_; + if(Dcal) + *Dcal << - inv_fx * inv_fx * (delta_u - s_ * inv_fy * delta_v) + << inv_fx * (s_ * inv_fy * inv_fy) * delta_v << -inv_fx * inv_fy * delta_v + << -inv_fx << inv_fx * s_ * inv_fy + << 0 << -inv_fy * inv_fy * delta_v << 0 << 0 << -inv_fy; + if(Dp) + *Dp << inv_fx << -inv_fx * s_ * inv_fy << 0 << inv_fy; + return Point2(inv_fx * (delta_u - s_ * inv_fy * delta_v), + inv_fy * delta_v); +} + + /* ************************************************************************* */ Vector3 Cal3_S2::calibrate(const Vector3& p) const { return matrix_inverse() * p; diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 3d5342c92..c448cb0b1 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -160,6 +160,16 @@ public: */ Point2 calibrate(const Point2& p) const; + /** + * 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, OptionalJacobian<2,5> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const; + /** * convert homogeneous image coordinates to intrinsic coordinates * @param p point in image coordinates From d8b9cae25dcc054042b09846567c1f283f811ecc Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 23 Jun 2015 01:59:32 -0400 Subject: [PATCH 2/8] feature: add test to Cal3_S2 calibrate jacobian --- gtsam/geometry/tests/testCal3_S2.cpp | 29 ++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index aa06a2e29..668b69a55 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -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,33 @@ 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); + CHECK(assert_equal(numerical, computed, 1e-8)); + CHECK(assert_equal(expected, p_xy, 1e-8)); +} + +/* ************************************************************************* */ +TEST(Cal3_S2, Dcalibrate1) +{ + Matrix computed; + Point2 expected = K.calibrate(p_uv, boost::none, computed); + Matrix numerical = numericalDerivative22(calibrate_, K, p); + CHECK(assert_equal(numerical, computed, 1e-8)); + CHECK(assert_equal(expected, p_xy, 1e-8)); +} + +/* ************************************************************************* */ +TEST(Cal3_S2, Dcalibrate2) +{ + +} + /* ************************************************************************* */ TEST( Cal3_S2, assert_equal) { From 1c3ab0ce30fbb1eeaad251f00c0ed6807d353409 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 23 Jun 2015 02:03:44 -0400 Subject: [PATCH 3/8] fix: compile issue in previous jacobians... --- gtsam/geometry/Cal3_S2.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 81123d9a9..fb4fb191e 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -90,23 +90,22 @@ Point2 Cal3_S2::calibrate(const Point2& p) const { } /* ************************************************************************* */ -Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, - OptionalJacobian<2,2> Dp = boost::none) 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(); double delta_u = u - u0_, delta_v = v - v0_; double inv_fx = 1/ fx_, inv_fy = 1/fy_; if(Dcal) - *Dcal << - inv_fx * inv_fx * (delta_u - s_ * inv_fy * delta_v) - << inv_fx * (s_ * inv_fy * inv_fy) * delta_v << -inv_fx * inv_fy * delta_v - << -inv_fx << inv_fx * s_ * inv_fy - << 0 << -inv_fy * inv_fy * delta_v << 0 << 0 << -inv_fy; + *Dcal << - inv_fx * inv_fx * (delta_u - s_ * inv_fy * delta_v) , + inv_fx * (s_ * inv_fy * inv_fy) * delta_v, -inv_fx * inv_fy * delta_v, + -inv_fx, inv_fx * s_ * inv_fy, + 0, -inv_fy * inv_fy * delta_v, 0, 0, -inv_fy; if(Dp) - *Dp << inv_fx << -inv_fx * s_ * inv_fy << 0 << inv_fy; + *Dp << inv_fx, -inv_fx * s_ * inv_fy, 0, inv_fy; return Point2(inv_fx * (delta_u - s_ * inv_fy * delta_v), inv_fy * delta_v); } - /* ************************************************************************* */ Vector3 Cal3_S2::calibrate(const Vector3& p) const { return matrix_inverse() * p; From a37f81fed71cc03bcc3aef150ae322e520ca3881 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 23 Jun 2015 02:23:36 -0400 Subject: [PATCH 4/8] fix: some name conflicts. Change calibrate with optional jacobian to calibrate2 --- gtsam/geometry/Cal3_S2.cpp | 8 ++++---- gtsam/geometry/Cal3_S2.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index fb4fb191e..8765fee76 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -90,14 +90,14 @@ Point2 Cal3_S2::calibrate(const Point2& p) const { } /* ************************************************************************* */ -Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal, - OptionalJacobian<2,2> Dp) const { +Point2 Cal3_S2::calibrate2(const Point2& p, OptionalJacobian<2,5> Dcal, + OptionalJacobian<2,2> Dp) const { const double u = p.x(), v = p.y(); double delta_u = u - u0_, delta_v = v - v0_; double inv_fx = 1/ fx_, inv_fy = 1/fy_; if(Dcal) - *Dcal << - inv_fx * inv_fx * (delta_u - s_ * inv_fy * delta_v) , - inv_fx * (s_ * inv_fy * inv_fy) * delta_v, -inv_fx * inv_fy * delta_v, + *Dcal << - inv_fx * inv_fx * (delta_u - s_ * inv_fy * delta_v) , + inv_fx * (s_ * inv_fy * inv_fy) * delta_v, -inv_fx * inv_fy * delta_v, -inv_fx, inv_fx * s_ * inv_fy, 0, -inv_fy * inv_fy * delta_v, 0, 0, -inv_fy; if(Dp) diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index c448cb0b1..9347c563c 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -167,7 +167,7 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, + Point2 calibrate2(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, OptionalJacobian<2,2> Dp = boost::none) const; /** From 89dc6399e26bda00be7ac0b2570c79483890f665 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 23 Jun 2015 03:16:01 -0400 Subject: [PATCH 5/8] fix: correct a stupid test typo error. Jacobian is always fine, no issue --- gtsam/geometry/tests/testCal3_S2.cpp | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 668b69a55..8272760ef 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -80,26 +80,20 @@ 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); - CHECK(assert_equal(numerical, computed, 1e-8)); + Point2 expected = K.calibrate2(p_uv, computed, boost::none); + Matrix numerical = numericalDerivative21(calibrate_, K, p_uv); CHECK(assert_equal(expected, p_xy, 1e-8)); -} - -/* ************************************************************************* */ -TEST(Cal3_S2, Dcalibrate1) -{ - Matrix computed; - Point2 expected = K.calibrate(p_uv, boost::none, computed); - Matrix numerical = numericalDerivative22(calibrate_, K, p); CHECK(assert_equal(numerical, computed, 1e-8)); - CHECK(assert_equal(expected, p_xy, 1e-8)); } /* ************************************************************************* */ TEST(Cal3_S2, Dcalibrate2) { - + Matrix computed; + Point2 expected = K.calibrate2(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)); } /* ************************************************************************* */ From b758eafad2dc00420ef5c710a8fe7f36c2ae4061 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 23 Jun 2015 03:16:32 -0400 Subject: [PATCH 6/8] change: simplifies the equation --- gtsam/geometry/Cal3_S2.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 8765fee76..c2c0b7d79 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -95,15 +95,15 @@ Point2 Cal3_S2::calibrate2(const Point2& p, OptionalJacobian<2,5> Dcal, const double u = p.x(), v = p.y(); double delta_u = u - u0_, delta_v = v - v0_; double inv_fx = 1/ fx_, inv_fy = 1/fy_; + Point2 point(inv_fx * (delta_u - s_ * inv_fy * delta_v), + inv_fy * delta_v); if(Dcal) - *Dcal << - inv_fx * inv_fx * (delta_u - s_ * inv_fy * delta_v) , - inv_fx * (s_ * inv_fy * inv_fy) * delta_v, -inv_fx * inv_fy * delta_v, + *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 * inv_fy * delta_v, 0, 0, -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 Point2(inv_fx * (delta_u - s_ * inv_fy * delta_v), - inv_fy * delta_v); + return point; } /* ************************************************************************* */ From 93e1311e0de4408da845f09ff349d2daba5154ec Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 23 Jun 2015 10:01:02 -0400 Subject: [PATCH 7/8] change: reduce some temporary computation --- gtsam/geometry/Cal3_S2.cpp | 20 +++++++------------- gtsam/geometry/Cal3_S2.h | 9 +-------- 2 files changed, 8 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index c2c0b7d79..c131d46f7 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -83,26 +83,20 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal, } /* ************************************************************************* */ -Point2 Cal3_S2::calibrate(const Point2& p) const { - const double u = p.x(), v = p.y(); - return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)), - (1 / fy_) * (v - v0_)); -} - -/* ************************************************************************* */ -Point2 Cal3_S2::calibrate2(const Point2& p, OptionalJacobian<2,5> Dcal, +Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal, OptionalJacobian<2,2> Dp) const { const double u = p.x(), v = p.y(); double delta_u = u - u0_, delta_v = v - v0_; double inv_fx = 1/ fx_, inv_fy = 1/fy_; - Point2 point(inv_fx * (delta_u - s_ * inv_fy * delta_v), - inv_fy * delta_v); + 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, + *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; + *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy; return point; } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 9347c563c..4e62ca7e9 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -153,13 +153,6 @@ public: Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, OptionalJacobian<2,2> Dp = boost::none) const; - /** - * convert image coordinates uv to intrinsic coordinates xy - * @param p point in image coordinates - * @return point in intrinsic coordinates - */ - Point2 calibrate(const Point2& p) const; - /** * convert image coordinates uv to intrinsic coordinates xy * @param p point in image coordinates @@ -167,7 +160,7 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates */ - Point2 calibrate2(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, + Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, OptionalJacobian<2,2> Dp = boost::none) const; /** From bb58550ae40e1d63ff8c27122f518672987a7802 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Tue, 23 Jun 2015 10:14:19 -0400 Subject: [PATCH 8/8] change:oops, forget the change in the test --- gtsam/geometry/tests/testCal3_S2.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 8272760ef..3e93dedc1 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -80,7 +80,7 @@ Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {return k.calibrate(pt); TEST(Cal3_S2, Dcalibrate1) { Matrix computed; - Point2 expected = K.calibrate2(p_uv, computed, boost::none); + 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)); @@ -90,7 +90,7 @@ TEST(Cal3_S2, Dcalibrate1) TEST(Cal3_S2, Dcalibrate2) { Matrix computed; - Point2 expected = K.calibrate2(p_uv, boost::none, 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));