From 298d0064a17f689d997c091e11f5b2b637388f2b Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 27 Nov 2014 17:55:26 +0100 Subject: [PATCH] With Natesh, trying out things --- gtsam/geometry/Cal3_S2.cpp | 6 -- gtsam/geometry/Cal3_S2.h | 11 +-- gtsam/geometry/PinholeCamera.h | 12 ++- gtsam/geometry/tests/testCal3DS2.cpp | 117 +++++++++++++++++++++++++++ 4 files changed, 129 insertions(+), 17 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index ff5a42b10..eb5d53eb8 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -80,12 +80,6 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); } -/* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p) const { - const double x = p.x(), y = p.y(); - return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); -} - /* ************************************************************************* */ Point2 Cal3_S2::calibrate(const Point2& p) const { const double u = p.x(), v = p.y(); diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 67f0d7e41..999a22853 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -141,13 +141,6 @@ public: 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0).finished(); } - /** - * convert intrinsic coordinates xy to image coordinates uv - * @param p point in intrinsic coordinates - * @return point in image coordinates - */ - Point2 uncalibrate(const Point2& p) const; - /** * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves * @param p point in intrinsic coordinates @@ -155,8 +148,8 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const; + Point2 uncalibrate(const Point2& p, boost::optional Dcal = boost::none, + boost::optional Dp = boost::none) const; /** * convert image coordinates uv to intrinsic coordinates xy diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 7e61c2b53..3ef6b8c11 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -42,7 +42,9 @@ private: Pose3 pose_; Calibration K_; + // Get dimension of calibration type at compile time static const int DimK = traits::dimension::value; + typedef Eigen::Matrix JacobianK; public: @@ -353,7 +355,7 @@ public: const Point3& pw, // boost::optional Dpose, boost::optional Dpoint, - boost::optional Dcal) const { + boost::optional Dcal) const { // Transform to camera coordinates and check cheirality const Point3 pc = pose_.transform_to(pw); @@ -379,7 +381,13 @@ public: } return pi; } else - return K_.uncalibrate(pn, Dcal, boost::none); + if (Dcal) { + JacobianK fixedDcal; + const Point2 pi = K_.uncalibrate(pn, fixedDcal); + *Dcal = fixedDcal; + return pi; + } else + return K_.uncalibrate(pn); } /** project a point at infinity from world coordinate to the image diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index c5a6be2d6..281b93c97 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -20,6 +20,7 @@ #include #include +using namespace std; using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Cal3DS2) @@ -92,6 +93,122 @@ TEST( Cal3DS2, retract) CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); } +/* ************************************************************************* */ +// Test Eigen::Ref +//TEST( Cal3DS2, Ref) { +// // In this case we don't want a copy +// Matrix25 fixedDcal; +// Eigen::Ref test1(fixedDcal); +// cout << test1 << endl; +// test1.resize(2, 5); +// test1 = Matrix25::Zero(); +// cout << test1 << endl; +// +// // In this case we want dynaDcal to be correctly allocate and filled +// Matrix dynaDcal(2,5); +// Eigen::Ref test2(dynaDcal); +// cout << test2.rows() << "," << test2.cols() << endl; +// test2.resize(2, 5); +// test2 = Matrix25::Zero(); +// cout << test2 << endl; +//} + +void test(Eigen::Ref H) { + cout << "test" << endl; + cout << H.size() << endl; + cout << H.rows() << "," << H.cols() << endl; + if (H.size()) { + cout << "H before:\n" << H << endl; + H.resize(2, 5); + H = Matrix25::Zero(); + cout << "H after:\n" << H << endl; + } +} + +TEST( Cal3DS2, Ref) { + + // In this case we don't want anything to happen + cout << "\nempty" << endl; + Matrix empty; + test(empty); + cout << "after" << empty << endl; + + // In this case we don't want a copy, TODO: does it copy??? + cout << "\nfixed" << endl; + Matrix25 fixedDcal; + fixedDcal.setOnes(); + + cout << fixedDcal << endl; + test(fixedDcal); + cout << "after" << fixedDcal << endl; + + // In this case we want dynaDcal to be correctly allocate and filled + cout << "\ndynamic wrong size" << endl; + Matrix dynaDcal(8,5); + dynaDcal.setOnes(); + + cout << dynaDcal << endl; + test(dynaDcal); + cout << "after" << dynaDcal << endl; + + // In this case we want dynaDcal to be correctly allocate and filled + cout << "\ndynamic right size" << endl; + Matrix dynamic2(2,5); + dynamic2.setOnes(); + + cout << dynamic2 << endl; + test(dynamic2); + cout << "after" << dynamic2 << endl; +} + +void test2(Eigen::Ref H) { + cout << "test2" << endl; + cout << H.size() << endl; + cout << H.rows() << "," << H.cols() << endl; + if (H.size()) { + cout << "H before:\n" << H << endl; + H.resize(2, 5); + H = Matrix25::Zero(); + cout << "H after:\n" << H << endl; + } +} + +TEST( Cal3DS2, Ref2) { + + // In this case we don't want anything to happen + cout << "\nempty" << endl; + Matrix empty; + test2(empty); + cout << "after" << empty << endl; + + // In this case we don't want a copy, TODO: does it copy??? + cout << "\nfixed" << endl; + Matrix25 fixedDcal; + fixedDcal.setOnes(); + + cout << fixedDcal << endl; + test2(fixedDcal); + cout << "after" << fixedDcal << endl; + + // In this case we want dynaDcal to be correctly allocate and filled + cout << "\ndynamic wrong size" << endl; + Matrix dynaDcal(8,5); + dynaDcal.setOnes(); + + cout << dynaDcal << endl; + test2(dynaDcal); + cout << "after" << dynaDcal << endl; + + // In this case we want dynaDcal to be correctly allocate and filled + cout << "\ndynamic right size" << endl; + Matrix dynamic2(2,5); + dynamic2.setOnes(); + + cout << dynamic2 << endl; + test2(dynamic2); + cout << "after" << dynamic2 << endl; +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */