From c9936763e29530fce51487656815d9a06c87778e Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Thu, 27 Nov 2014 10:34:44 -0500 Subject: [PATCH 01/76] testCal3S2 --- gtsam/geometry/Cal3_S2.cpp | 15 --------------- gtsam/geometry/Cal3_S2.h | 10 ---------- gtsam/geometry/PinholeCamera.h | 8 ++++---- gtsam/geometry/tests/testCal3_S2.cpp | 2 +- gtsam/slam/SmartFactorBase.h | 2 +- 5 files changed, 6 insertions(+), 31 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index aece1ded1..ff5a42b10 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -71,21 +71,6 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { return true; } -/* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const { - const double x = p.x(), y = p.y(); - if (Dcal) { - Dcal->resize(2,5); - *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; - } - if (Dp) { - Dp->resize(2,2); - *Dp << fx_, s_, 0.0, fy_; - } - return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); -} - /* ************************************************************************* */ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, boost::optional Dp) const { diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 6317d251d..67f0d7e41 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -158,16 +158,6 @@ public: Point2 uncalibrate(const Point2& p, boost::optional Dcal, boost::optional Dp) const; - /** - * convert intrinsic coordinates xy to image coordinates uv, dynamic derivaitves - * @param p point in intrinsic coordinates - * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters - * @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; - /** * convert image coordinates uv to intrinsic coordinates xy * @param p point in image coordinates diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index c5174ae65..7e61c2b53 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -353,7 +353,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); @@ -365,7 +365,7 @@ public: const double z = pc.z(), d = 1.0 / z; // uncalibration - Matrix Dpi_pn(2, 2); + Matrix2 Dpi_pn; const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); // chain the Jacobian matrices @@ -392,7 +392,7 @@ public: const Point3& pw, // boost::optional Dpose = boost::none, boost::optional Dpoint = boost::none, - boost::optional Dcal = boost::none) const { + boost::optional Dcal = boost::none) const { if (!Dpose && !Dpoint && !Dcal) { const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter) @@ -412,7 +412,7 @@ public: const Point2 pn = project_to_camera(pc, Dpn_pc); // uncalibration - Matrix Dpi_pn; // 2*2 + Matrix2 Dpi_pn; // 2*2 const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); // chain the Jacobian matrices diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 86c0b7e33..aa06a2e29 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -60,7 +60,7 @@ TEST( Cal3_S2, calibrate_homogeneous) { Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); } TEST( Cal3_S2, Duncalibrate1) { - Matrix computed; K.uncalibrate(p, computed, boost::none); + Matrix25 computed; K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p); CHECK(assert_equal(numerical,computed,1e-8)); } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index d9e7b9819..5d57d2862 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -280,7 +280,7 @@ public: Vector bi; try { - bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); + bi = -(cameras[i].project(point, Fi, Ei, static_cast(Hcali)) - this->measured_.at(i)).vector(); if(body_P_sensor_){ Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); Matrix J(6, 6); From 298d0064a17f689d997c091e11f5b2b637388f2b Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 27 Nov 2014 17:55:26 +0100 Subject: [PATCH 02/76] 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); } /* ************************************************************************* */ From d90b256d66ae8498d8e8ef5944ae7b2625962ca8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 27 Nov 2014 18:51:01 +0100 Subject: [PATCH 03/76] Promising prototype --- gtsam/geometry/tests/testCal3DS2.cpp | 180 ++++++++++++++++++--------- 1 file changed, 122 insertions(+), 58 deletions(-) diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index 281b93c97..9127b6ef8 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -113,63 +113,127 @@ TEST( Cal3DS2, retract) // 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; +//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; +//} + +/* ************************************************************************* */ + +template +struct OptionalJacobian { + bool empty_; + Matrix dynamic; + Eigen::Matrix fixed_; + OptionalJacobian():empty_(true) {} + OptionalJacobian(Eigen::Matrix& fixed):empty_(false),fixed_(fixed) {} + OptionalJacobian(Matrix& dynamic):empty_(true) {} + bool empty() const {return empty_;} // TODO cast to bool !empty + void operator=(const Eigen::Matrix& fixed) { + fixed_ = fixed; } -} + void print(const string& s) { + if (!empty_) cout << s << fixed_ << 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); +void test3(OptionalJacobian<2,5> H) { + cout << "test3" << endl; + if (!H.empty()) { + H.print("H before:\n"); H = Matrix25::Zero(); - cout << "H after:\n" << H << endl; + H.print("H after:\n"); } } @@ -178,7 +242,7 @@ TEST( Cal3DS2, Ref2) { // In this case we don't want anything to happen cout << "\nempty" << endl; Matrix empty; - test2(empty); + test3(empty); cout << "after" << empty << endl; // In this case we don't want a copy, TODO: does it copy??? @@ -187,7 +251,7 @@ TEST( Cal3DS2, Ref2) { fixedDcal.setOnes(); cout << fixedDcal << endl; - test2(fixedDcal); + test3(fixedDcal); cout << "after" << fixedDcal << endl; // In this case we want dynaDcal to be correctly allocate and filled @@ -196,7 +260,7 @@ TEST( Cal3DS2, Ref2) { dynaDcal.setOnes(); cout << dynaDcal << endl; - test2(dynaDcal); + test3(dynaDcal); cout << "after" << dynaDcal << endl; // In this case we want dynaDcal to be correctly allocate and filled @@ -205,7 +269,7 @@ TEST( Cal3DS2, Ref2) { dynamic2.setOnes(); cout << dynamic2 << endl; - test2(dynamic2); + test3(dynamic2); cout << "after" << dynamic2 << endl; } From 949c793e2365888530f0faddce105a36860a0ee5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 27 Nov 2014 22:04:27 +0100 Subject: [PATCH 04/76] Working prototype --- gtsam/geometry/tests/testCal3DS2.cpp | 137 +++++++++++++++------------ 1 file changed, 74 insertions(+), 63 deletions(-) diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index 9127b6ef8..0c7bff7a1 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -104,9 +104,9 @@ TEST( Cal3DS2, retract) // 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); +// // In this case we want dynamic1 to be correctly allocate and filled +// Matrix dynamic1(2,5); +// Eigen::Ref test2(dynamic1); // cout << test2.rows() << "," << test2.cols() << endl; // test2.resize(2, 5); // test2 = Matrix25::Zero(); @@ -129,9 +129,9 @@ TEST( Cal3DS2, retract) // // // In this case we don't want anything to happen // cout << "\nempty" << endl; -// Matrix empty; -// test(empty); -// cout << "after" << empty << endl; +// Matrix dynamic0; +// test(dynamic0); +// cout << "after" << dynamic0 << endl; // // // In this case we don't want a copy, TODO: does it copy??? // cout << "\nfixed" << endl; @@ -142,16 +142,16 @@ TEST( Cal3DS2, retract) // test(fixedDcal); // cout << "after" << fixedDcal << endl; // -// // In this case we want dynaDcal to be correctly allocate and filled +// // In this case we want dynamic1 to be correctly allocate and filled // cout << "\ndynamic wrong size" << endl; -// Matrix dynaDcal(8,5); -// dynaDcal.setOnes(); +// Matrix dynamic1(8,5); +// dynamic1.setOnes(); // -// cout << dynaDcal << endl; -// test(dynaDcal); -// cout << "after" << dynaDcal << endl; +// cout << dynamic1 << endl; +// test(dynamic1); +// cout << "after" << dynamic1 << endl; // -// // In this case we want dynaDcal to be correctly allocate and filled +// // In this case we want dynamic1 to be correctly allocate and filled // cout << "\ndynamic right size" << endl; // Matrix dynamic2(2,5); // dynamic2.setOnes(); @@ -177,9 +177,9 @@ TEST( Cal3DS2, retract) // // // In this case we don't want anything to happen // cout << "\nempty" << endl; -// Matrix empty; -// test2(empty); -// cout << "after" << empty << endl; +// Matrix dynamic0; +// test2(dynamic0); +// cout << "after" << dynamic0 << endl; // // // In this case we don't want a copy, TODO: does it copy??? // cout << "\nfixed" << endl; @@ -190,16 +190,16 @@ TEST( Cal3DS2, retract) // test2(fixedDcal); // cout << "after" << fixedDcal << endl; // -// // In this case we want dynaDcal to be correctly allocate and filled +// // In this case we want dynamic1 to be correctly allocate and filled // cout << "\ndynamic wrong size" << endl; -// Matrix dynaDcal(8,5); -// dynaDcal.setOnes(); +// Matrix dynamic1(8,5); +// dynamic1.setOnes(); // -// cout << dynaDcal << endl; -// test2(dynaDcal); -// cout << "after" << dynaDcal << endl; +// cout << dynamic1 << endl; +// test2(dynamic1); +// cout << "after" << dynamic1 << endl; // -// // In this case we want dynaDcal to be correctly allocate and filled +// // In this case we want dynamic1 to be correctly allocate and filled // cout << "\ndynamic right size" << endl; // Matrix dynamic2(2,5); // dynamic2.setOnes(); @@ -211,66 +211,77 @@ TEST( Cal3DS2, retract) /* ************************************************************************* */ -template +template struct OptionalJacobian { bool empty_; - Matrix dynamic; - Eigen::Matrix fixed_; - OptionalJacobian():empty_(true) {} - OptionalJacobian(Eigen::Matrix& fixed):empty_(false),fixed_(fixed) {} - OptionalJacobian(Matrix& dynamic):empty_(true) {} - bool empty() const {return empty_;} // TODO cast to bool !empty - void operator=(const Eigen::Matrix& fixed) { - fixed_ = fixed; + typedef Eigen::Matrix Fixed; + Eigen::Map map_; + OptionalJacobian() : + empty_(true), map_(NULL) { } + OptionalJacobian(Fixed& fixed) : + empty_(false), map_(NULL) { + // Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html + // to make map_ usurp the memory of the fixed size matrix + new (&map_) Eigen::Map(fixed.data()); + } + OptionalJacobian(Matrix& dynamic) : + empty_(false), map_(NULL) { + dynamic.resize(Rows, Cols); + // Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html + // to make map_ usurp the memory of the fixed size matrix + new (&map_) Eigen::Map(dynamic.data()); + } + /// Return true is allocated, false if default constructor was used + operator bool() const { + return !empty_; + } + /// Assign a fixed matrix to this + void operator=(const Fixed& fixed) { + map_ << fixed; + } + /// Print void print(const string& s) { - if (!empty_) cout << s << fixed_ << endl; + if (!empty_) + cout << s << map_ << endl; } }; -void test3(OptionalJacobian<2,5> H) { - cout << "test3" << endl; - if (!H.empty()) { - H.print("H before:\n"); - H = Matrix25::Zero(); - H.print("H after:\n"); - } +void test3(OptionalJacobian<2,3> H = OptionalJacobian<2,3>()) { + if (H) + H = Matrix23::Zero(); } TEST( Cal3DS2, Ref2) { - // In this case we don't want anything to happen - cout << "\nempty" << endl; - Matrix empty; - test3(empty); - cout << "after" << empty << endl; + Matrix expected; + expected = Matrix23::Zero(); - // In this case we don't want a copy, TODO: does it copy??? - cout << "\nfixed" << endl; - Matrix25 fixedDcal; + // Default argument does nothing + test3(); + + // Fixed size, no copy + Matrix23 fixedDcal; fixedDcal.setOnes(); - - cout << fixedDcal << endl; test3(fixedDcal); - cout << "after" << fixedDcal << endl; + EXPECT(assert_equal(expected,fixedDcal)); - // In this case we want dynaDcal to be correctly allocate and filled - cout << "\ndynamic wrong size" << endl; - Matrix dynaDcal(8,5); - dynaDcal.setOnes(); + // Empty is no longer a sign we don't want a matrix, we want it resized + Matrix dynamic0; + test3(dynamic0); + EXPECT(assert_equal(expected,dynamic0)); - cout << dynaDcal << endl; - test3(dynaDcal); - cout << "after" << dynaDcal << endl; + // Dynamic wrong size + Matrix dynamic1(3,5); + dynamic1.setOnes(); + test3(dynamic1); + EXPECT(assert_equal(expected,dynamic0)); - // In this case we want dynaDcal to be correctly allocate and filled - cout << "\ndynamic right size" << endl; + // Dynamic right size Matrix dynamic2(2,5); dynamic2.setOnes(); - - cout << dynamic2 << endl; test3(dynamic2); - cout << "after" << dynamic2 << endl; + EXPECT(assert_equal(dynamic2,dynamic0)); } /* ************************************************************************* */ From d1c94f095b9545bbf64b4fd9281980fd798f63fd Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 27 Nov 2014 22:06:02 +0100 Subject: [PATCH 05/76] Rename, removed old code --- gtsam/geometry/tests/testCal3DS2.cpp | 127 ++------------------------- 1 file changed, 5 insertions(+), 122 deletions(-) diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index 0c7bff7a1..f22e7d065 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -94,138 +94,21 @@ TEST( Cal3DS2, retract) } /* ************************************************************************* */ -// 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 dynamic1 to be correctly allocate and filled -// Matrix dynamic1(2,5); -// Eigen::Ref test2(dynamic1); -// 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 dynamic0; -// test(dynamic0); -// cout << "after" << dynamic0 << 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 dynamic1 to be correctly allocate and filled -// cout << "\ndynamic wrong size" << endl; -// Matrix dynamic1(8,5); -// dynamic1.setOnes(); -// -// cout << dynamic1 << endl; -// test(dynamic1); -// cout << "after" << dynamic1 << endl; -// -// // In this case we want dynamic1 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 dynamic0; -// test2(dynamic0); -// cout << "after" << dynamic0 << 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 dynamic1 to be correctly allocate and filled -// cout << "\ndynamic wrong size" << endl; -// Matrix dynamic1(8,5); -// dynamic1.setOnes(); -// -// cout << dynamic1 << endl; -// test2(dynamic1); -// cout << "after" << dynamic1 << endl; -// -// // In this case we want dynamic1 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; -//} - -/* ************************************************************************* */ - template -struct OptionalJacobian { +struct FixedRef { bool empty_; typedef Eigen::Matrix Fixed; Eigen::Map map_; - OptionalJacobian() : + FixedRef() : empty_(true), map_(NULL) { } - OptionalJacobian(Fixed& fixed) : + FixedRef(Fixed& fixed) : empty_(false), map_(NULL) { // Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html // to make map_ usurp the memory of the fixed size matrix new (&map_) Eigen::Map(fixed.data()); } - OptionalJacobian(Matrix& dynamic) : + FixedRef(Matrix& dynamic) : empty_(false), map_(NULL) { dynamic.resize(Rows, Cols); // Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html @@ -247,7 +130,7 @@ struct OptionalJacobian { } }; -void test3(OptionalJacobian<2,3> H = OptionalJacobian<2,3>()) { +void test3(FixedRef<2,3> H = FixedRef<2,3>()) { if (H) H = Matrix23::Zero(); } From 1137cb39d859c3b9b6a44b8444b55de7896bde19 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 27 Nov 2014 22:33:13 +0100 Subject: [PATCH 06/76] A number of targets --- .cproject | 1246 +++++++++++++++++++++-------------------------------- 1 file changed, 483 insertions(+), 763 deletions(-) diff --git a/.cproject b/.cproject index 9c03c5b7d..80e2b4a8b 100644 --- a/.cproject +++ b/.cproject @@ -510,22 +510,6 @@ true true - - make - -j5 - SFMExampleExpressions.run - true - true - true - - - make - -j5 - Pose2SLAMExampleExpressions.run - true - true - true - make -j5 @@ -680,18 +664,66 @@ false true - + make - -j2 - tests/testPose2.run + -j5 + testPoseRTV.run true true true - + + make + -j5 + testIMUSystem.run + true + true + true + + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + make -j2 - tests/testPose3.run + tests/testPose2.run true true true @@ -720,135 +752,94 @@ true true - + make -j5 - testAHRS.run + testValues.run true true true - + make -j5 - testInvDepthFactor3.run + testOrdering.run true true true - + make -j5 - testMultiProjectionFactor.run + testKey.run true true true - + make -j5 - testPoseRotationPrior.run + testLinearContainerFactor.run true true true - + make - -j5 - testPoseTranslationPrior.run + -j6 -j8 + testWhiteNoiseFactor.run true true true - + make -j5 - testReferenceFrameFactor.run + schedulingExample.run true true true - + make -j5 - testSmartProjectionFactor.run + testCSP.run true true true - + make -j5 - testTSAMFactors.run + testScheduler.run true true true - + make -j5 - testInertialNavFactor_GlobalVelocity.run + schedulingQuals12.run true true true - + make -j5 - testInvDepthFactorVariant3.run + testSudoku.run true true true - + make - -j5 - testInvDepthFactorVariant1.run + testErrors.run true true true - - make - -j5 - testEquivInertialNavFactor_GlobalVel.run - true - true - true - - - make - -j5 - testInvDepthFactorVariant2.run - true - true - true - - - make - -j5 - testRelativeElevationFactor.run - true - true - true - - - make - -j5 - testPoseBetweenFactor.run - true - true - true - - - make - -j5 - testGaussMarkov1stOrderFactor.run - true - true - true - - + make -j5 testGaussianFactorGraphUnordered.run @@ -864,154 +855,66 @@ true true - + make -j5 - testGaussianConditional.run + testInvDepthFactor3.run true true true - + make -j5 - testGaussianDensity.run + testPoseTranslationPrior.run true true true - + make -j5 - testGaussianJunctionTree.run + testPoseRotationPrior.run true true true - + make -j5 - testHessianFactor.run + testReferenceFrameFactor.run true true true - + make -j5 - testJacobianFactor.run + testAHRS.run true true true - + make - -j5 - testKalmanFilter.run - true - true - true - - - make - -j5 - testNoiseModel.run - true - true - true - - - make - -j5 - testSampler.run - true - true - true - - - make - -j5 - testSerializationLinear.run - true - true - true - - - make - -j5 - testVectorValues.run - true - true - true - - - make - -j5 - testGaussianBayesTree.run - true - true - true - - - make - -j5 - timeCameraExpression.run - true - true - true - - - make - -j5 - timeOneCameraExpression.run - true - true - true - - - make - -j5 - timeSFMExpressions.run - true - true - true - - - make - -j5 - timeAdaptAutoDiff.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - - - make - -j5 + -j8 testImuFactor.run true true true - + make -j5 - testAHRSFactor.run + testMultiProjectionFactor.run true true true - + make - -j8 - testAttitudeFactor.run + -j5 + testSmartProjectionFactor.run true true true @@ -1201,10 +1104,10 @@ make - -j2 - testDSFVector.run + VERBOSE=1 + wrap_gtsam true - true + false true @@ -1247,6 +1150,14 @@ true true + + make + -j5 + testParticleFactor.run + true + true + true + make -j2 @@ -1351,10 +1262,66 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make - -j2 - all + -j5 + testGaussianISAM2.run + true + true + true + + + make + + testSimulated2DOriented.run + true + false + true + + + make + -j5 + timing.tests true true true @@ -1431,22 +1398,6 @@ true true - - make - - testSimulated2DOriented.run - true - false - true - - - make - -j2 - testVSLAMConfig.run - true - true - true - make -j2 @@ -1495,46 +1446,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1575,10 +1486,98 @@ false true + + make + -j5 + testVectorValues.run + true + true + true + + + make + -j5 + testNoiseModel.run + true + true + true + + + make + -j5 + testHessianFactor.run + true + true + true + + + make + -j5 + testGaussianConditional.run + true + true + true + + + make + -j5 + testGaussianFactorGraphUnordered.run + true + true + true + + + make + -j5 + testGaussianJunctionTree.run + true + true + true + + + make + -j5 + testKalmanFilter.run + true + true + true + + + make + -j5 + testGaussianDensity.run + true + true + true + + + make + -j5 + testSerializationLinear.run + true + true + true + + + make + -j5 + testJacobianFactor.run + true + true + true + + + make + -j5 + testSampler.run + true + true + true + make - -j2 - check + -j5 + testGaussianFactorGraphB.run true true true @@ -1591,6 +1590,34 @@ true true + + cpack + -G DEB + true + false + true + + + cpack + -G RPM + true + false + true + + + cpack + -G TGZ + true + false + true + + + cpack + --config CPackSourceConfig.cmake + true + true + true + make -j3 @@ -1694,14 +1721,6 @@ true true - - make - -j2 VERBOSE=1 - check.geometry - true - false - true - make -j5 @@ -1790,34 +1809,6 @@ false true - - cpack - -G DEB - true - false - true - - - cpack - -G RPM - true - false - true - - - cpack - -G TGZ - true - false - true - - - cpack - --config CPackSourceConfig.cmake - true - false - true - make -j5 @@ -1971,25 +1962,9 @@ true - make - -j6 -j8 - check.nonlinear_unstable - true - true - true - - make -j5 - check.tests - true - true - true - - - make - -j2 - check + tests/testGaussianISAM2 true true true @@ -2005,7 +1980,7 @@ make -j2 - install + testPoint2.run true true true @@ -2021,10 +1996,58 @@ cmake .. + wrap true false true + + make + -j5 + testQPSolver.run + true + true + true + + + make + -j5 + testRot2.run + true + true + true + + + cmake-gui + .. + true + false + true + + + make + -j5 + testClp.run + true + true + true + + + make + -j5 + testlpsolve.run + true + true + true + + + make + -j5 + testLPSolver.run + true + true + true + make -j2 @@ -2035,7 +2058,7 @@ make - -j5 + -j4 testCal3Bundler.run true true @@ -2043,7 +2066,7 @@ make - -j5 + -j4 testCal3DS2.run true true @@ -2051,7 +2074,7 @@ make - -j5 + -j4 testCalibratedCamera.run true true @@ -2059,7 +2082,7 @@ make - -j5 + -j4 testEssentialMatrix.run true true @@ -2075,23 +2098,15 @@ make - -j5 + -j4 testPinholeCamera.run true true true - - make - -j5 - testPoint2.run - true - true - true - make - -j5 + -j4 testPoint3.run true true @@ -2099,7 +2114,7 @@ make - -j5 + -j4 testPose2.run true true @@ -2107,7 +2122,7 @@ make - -j5 + -j4 testPose3.run true true @@ -2115,7 +2130,7 @@ make - -j5 + -j4 testRot3M.run true true @@ -2123,7 +2138,7 @@ make - -j5 + -j4 testSphere2.run true true @@ -2131,40 +2146,40 @@ make - -j5 + -j4 testStereoCamera.run true true true - + make - -j5 - testCal3Unified.run + -j4 + timeCalibratedCamera.run true true true - + make - -j5 - testRot2.run + -j4 + timePinholeCamera.run true true true - + make - -j5 - testRot3Q.run + -j4 + timeStereoCamera.run true true true - + make - -j5 - testRot3.run + -j4 + testCal3_S2.run true true true @@ -2209,14 +2224,6 @@ false true - - make - -j2 - all - true - true - true - make -j2 @@ -2249,74 +2256,66 @@ true true - + make -j5 - testIMUSystem.run + testGeneralSFMFactor.run true true true - + make -j5 - testPoseRTV.run + testProjectionFactor.run true true true - + make -j5 - testVelocityConstraint.run + testGeneralSFMFactor_Cal3Bundler.run true true true - + make - -j5 - testVelocityConstraint3.run + -j6 -j8 + testAntiFactor.run true true true - + make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteConditional.run + -j6 -j8 + testBetweenFactor.run true true true - + make -j5 - testDiscreteFactor.run + testDataset.run true true true - + make -j5 - testDiscreteFactorGraph.run + testEssentialMatrixFactor.run true true true - + make -j5 - testDiscreteMarginals.run + testRotateFactor.run true true true @@ -2353,90 +2352,42 @@ true true - + make -j5 - testWrap.run + testDiscreteFactor.run true true true - + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + make -j5 - testSpirit.run + testDiscreteFactorGraph.run true true true - + make -j5 - check.wrap + testDiscreteConditional.run true true true - + make -j5 - testMethod.run - true - true - true - - - make - -j5 - testClass.run - true - true - true - - - make - -j5 - schedulingExample.run - true - true - true - - - make - -j5 - schedulingQuals12.run - true - true - true - - - make - -j5 - schedulingQuals13.run - true - true - true - - - make - -j5 - testCSP.run - true - true - true - - - make - -j5 - testScheduler.run - true - true - true - - - make - -j5 - testSudoku.run + testDiscreteMarginals.run true true true @@ -2449,6 +2400,38 @@ true true + + make + -j5 + testInference.run + true + true + true + + + make + -j1 + testSymbolicSequentialSolver.run + true + false + true + + + make + -j5 + testEliminationTree.run + true + true + true + + + make + -j1 + testSymbolicBayesTree.run + true + false + true + make -j2 @@ -2457,34 +2440,18 @@ true true - + make -j5 - testMatrix.run + testInvDepthCamera3.run true true true - + make -j5 - testVector.run - true - true - true - - - make - -j5 - testNumericalDerivative.run - true - true - true - - - make - -j5 - testVerticalBlockMatrix.run + testTriangulation.run true true true @@ -2513,14 +2480,6 @@ true true - - make - -j5 - testGaussianISAM2.run - true - true - true - make -j5 @@ -2593,14 +2552,6 @@ true true - - make - -j5 - timing.tests - true - true - true - make -j5 @@ -2627,14 +2578,16 @@ make - testGraph.run + + testSimulated2D.run true false true make - testJunctionTree.run + + testSimulated3D.run true false true @@ -2646,54 +2599,6 @@ false true - - make - -j5 - testGaussianISAM.run - true - true - true - - - make - -j5 - testDoglegOptimizer.run - true - true - true - - - make - -j5 - testNonlinearFactorGraph.run - true - true - true - - - make - -j5 - testIterative.run - true - true - true - - - make - -j5 - testSubgraphSolver.run - true - true - true - - - make - -j5 - testGaussianFactorGraphB.run - true - true - true - make -j5 @@ -2702,66 +2607,10 @@ true true - + make -j5 - testManifold.run - true - true - true - - - make - -j5 - testParticleFactor.run - true - true - true - - - make - -j5 - testExpressionFactor.run - true - true - true - - - make - -j5 - testExpressionMeta.run - true - true - true - - - make - -j5 - testAdaptAutoDiff.run - true - true - true - - - make - -j5 - testCallRecord.run - true - true - true - - - make - -j4 - testBasisDecompositions.run - true - true - true - - - make - -j4 - testCustomChartExpression.run + timeIncremental.run true true true @@ -2822,74 +2671,18 @@ true true - + make -j5 - testAntiFactor.run + testGPSFactor.run true true true - + make -j5 - testPriorFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - - - make - -j5 - testPoseRotationPrior.run + testGaussMarkov1stOrderFactor.run true true true @@ -2902,6 +2695,30 @@ true true + + make + -j5 + testQPSolver.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + make -j2 @@ -2998,18 +2815,9 @@ true true - - make - -j5 - SelfCalibrationExample.run - true - true - true - make - -j5 - SFMExample.run + testSymbolicBayesNetB.run true true true @@ -3054,78 +2862,6 @@ true true - - make - -j5 - Pose2SLAMExample_lago.run - true - true - true - - - make - -j5 - Pose2SLAMExample_g2o.run - true - true - true - - - make - -j5 - SFMExample_SmartFactor.run - true - true - true - - - make - -j5 - testLago.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testWhiteNoiseFactor.run - true - true - true - - - make - -j4 - testExpression.run - true - true - true - make -j4 @@ -3134,46 +2870,6 @@ true true - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j5 - timeLago.run - true - true - true - - - make - -j5 - timePose3.run - true - true - true - make -j2 @@ -3286,6 +2982,30 @@ true true + + make + -j5 + install + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + make -j5 From ec93e0a88545e0a02f2e8c11be19bfe4d73bf635 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 27 Nov 2014 22:33:54 +0100 Subject: [PATCH 07/76] Moved and refined prototype, trouble with boost::none as argument --- gtsam/geometry/Cal3_S2.cpp | 10 ++-- gtsam/geometry/Cal3_S2.h | 66 ++++++++++++++++++++++++- gtsam/geometry/tests/testCal3DS2.cpp | 74 ---------------------------- gtsam/geometry/tests/testCal3_S2.cpp | 38 ++++++++++++++ 4 files changed, 108 insertions(+), 80 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index eb5d53eb8..dbb6c09eb 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -72,11 +72,13 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { } /* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const { +Point2 Cal3_S2::uncalibrate(const Point2& p, FixedRef<2, 5> Dcal, + FixedRef<2, 2> Dp) const { const double x = p.x(), y = p.y(); - if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; - if (Dp) *Dp << fx_, s_, 0.0, fy_; + if (Dcal) + *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; + if (Dp) + *Dp << fx_, s_, 0.0, fy_; return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 999a22853..693e1dca8 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -26,6 +26,68 @@ namespace gtsam { +/** + * Eigen::Ref like class that cane take either a fixed size or dynamic + * Eigen matrix. In the latter case, the dynamic matrix will be resized. + * Finally, the default constructor acts like boost::none. + */ +template +class FixedRef { + +public: + + /// Fixed size type + typedef Eigen::Matrix Fixed; + +private: + + bool empty_; + Eigen::Map map_; + + // Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html + // to make map_ usurp the memory of the fixed size matrix + void usurp(double* data) { + new (&map_) Eigen::Map(data); + } + +public: + + /// Defdault constructo acts like boost::none + FixedRef() : + empty_(true), map_(NULL) { + } + + /// Constructor that will usurp data of a fixed size matrix + FixedRef(Fixed& fixed) : + empty_(false), map_(NULL) { + usurp(fixed.data()); + } + + /// Constructor that will resize a dynamic matrix + FixedRef(Matrix& dynamic) : + empty_(false), map_(NULL) { + } + + /// Constructor compatible with old-style + FixedRef(const boost::optional optional) : + empty_(!optional), map_(NULL) { + if (optional) { + optional->resize(Rows, Cols); + usurp(optional->data()); + } + } + + /// Return true is allocated, false if default constructor was used + operator bool() const { + return !empty_; + } + + /// De-reference, like boost optional + Eigen::Map& operator* () { + return map_; + } +}; + /** * @brief The most common 5DOF 3D->2D calibration * @addtogroup geometry @@ -148,8 +210,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::none, - boost::optional Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, FixedRef<2,5> Dcal = FixedRef<2,5>(), + FixedRef<2,2> Dp = FixedRef<2,2>()) const; /** * convert image coordinates uv to intrinsic coordinates xy diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index f22e7d065..d726f14f4 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -93,80 +93,6 @@ TEST( Cal3DS2, retract) CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); } -/* ************************************************************************* */ -template -struct FixedRef { - bool empty_; - typedef Eigen::Matrix Fixed; - Eigen::Map map_; - FixedRef() : - empty_(true), map_(NULL) { - } - FixedRef(Fixed& fixed) : - empty_(false), map_(NULL) { - // Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html - // to make map_ usurp the memory of the fixed size matrix - new (&map_) Eigen::Map(fixed.data()); - } - FixedRef(Matrix& dynamic) : - empty_(false), map_(NULL) { - dynamic.resize(Rows, Cols); - // Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html - // to make map_ usurp the memory of the fixed size matrix - new (&map_) Eigen::Map(dynamic.data()); - } - /// Return true is allocated, false if default constructor was used - operator bool() const { - return !empty_; - } - /// Assign a fixed matrix to this - void operator=(const Fixed& fixed) { - map_ << fixed; - } - /// Print - void print(const string& s) { - if (!empty_) - cout << s << map_ << endl; - } -}; - -void test3(FixedRef<2,3> H = FixedRef<2,3>()) { - if (H) - H = Matrix23::Zero(); -} - -TEST( Cal3DS2, Ref2) { - - Matrix expected; - expected = Matrix23::Zero(); - - // Default argument does nothing - test3(); - - // Fixed size, no copy - Matrix23 fixedDcal; - fixedDcal.setOnes(); - test3(fixedDcal); - EXPECT(assert_equal(expected,fixedDcal)); - - // Empty is no longer a sign we don't want a matrix, we want it resized - Matrix dynamic0; - test3(dynamic0); - EXPECT(assert_equal(expected,dynamic0)); - - // Dynamic wrong size - Matrix dynamic1(3,5); - dynamic1.setOnes(); - test3(dynamic1); - EXPECT(assert_equal(expected,dynamic0)); - - // Dynamic right size - Matrix dynamic2(2,5); - dynamic2.setOnes(); - test3(dynamic2); - EXPECT(assert_equal(dynamic2,dynamic0)); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index aa06a2e29..4214d7850 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -104,6 +104,44 @@ TEST(Cal3_S2, between) { } +/* ************************************************************************* */ +void test3(FixedRef<2,3> H = FixedRef<2,3>()) { + if (H) + *H = Matrix23::Zero(); +} + +TEST( Cal3DS2, Ref2) { + + Matrix expected; + expected = Matrix23::Zero(); + + // Default argument does nothing + test3(); + + // Fixed size, no copy + Matrix23 fixedDcal; + fixedDcal.setOnes(); + test3(fixedDcal); + EXPECT(assert_equal(expected,fixedDcal)); + + // Empty is no longer a sign we don't want a matrix, we want it resized + Matrix dynamic0; + test3(dynamic0); + EXPECT(assert_equal(expected,dynamic0)); + + // Dynamic wrong size + Matrix dynamic1(3,5); + dynamic1.setOnes(); + test3(dynamic1); + EXPECT(assert_equal(expected,dynamic0)); + + // Dynamic right size + Matrix dynamic2(2,5); + dynamic2.setOnes(); + test3(dynamic2); + EXPECT(assert_equal(dynamic2,dynamic0)); +} + /* ************************************************************************* */ int main() { TestResult tr; From a6b4823203c8dffd30aaf7163a5ea923ba811bfe Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 27 Nov 2014 22:50:37 +0100 Subject: [PATCH 08/76] This seems to work perfectly (compiles, at least) --- gtsam/geometry/Cal3_S2.h | 6 ++++++ gtsam/geometry/PinholeCamera.h | 10 ++-------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 693e1dca8..b8f10fccb 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -23,6 +23,7 @@ #include #include +#include namespace gtsam { @@ -57,6 +58,11 @@ public: empty_(true), map_(NULL) { } + /// Defdault constructo acts like boost::none + FixedRef(boost::none_t none) : + empty_(true), map_(NULL) { + } + /// Constructor that will usurp data of a fixed size matrix FixedRef(Fixed& fixed) : empty_(false), map_(NULL) { diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 3ef6b8c11..ad0c8d659 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -381,13 +381,7 @@ public: } return pi; } else - if (Dcal) { - JacobianK fixedDcal; - const Point2 pi = K_.uncalibrate(pn, fixedDcal); - *Dcal = fixedDcal; - return pi; - } else - return K_.uncalibrate(pn); + return K_.uncalibrate(pn, Dcal); } /** project a point at infinity from world coordinate to the image @@ -400,7 +394,7 @@ public: const Point3& pw, // boost::optional Dpose = boost::none, boost::optional Dpoint = boost::none, - boost::optional Dcal = boost::none) const { + boost::optional Dcal = boost::none) const { if (!Dpose && !Dpoint && !Dcal) { const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter) From 850af694bea34b3b51758ea6196d39ad57051c71 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 01:55:18 +0100 Subject: [PATCH 09/76] Fixed some targets --- .cproject | 272 ++++++++++++++++++++++++++++++++---------------------- 1 file changed, 160 insertions(+), 112 deletions(-) diff --git a/.cproject b/.cproject index 80e2b4a8b..5472443bf 100644 --- a/.cproject +++ b/.cproject @@ -576,6 +576,7 @@ make + tests/testBayesTree.run true false @@ -583,6 +584,7 @@ make + testBinaryBayesNet.run true false @@ -630,6 +632,7 @@ make + testSymbolicBayesNet.run true false @@ -637,6 +640,7 @@ make + tests/testSymbolicFactor.run true false @@ -644,6 +648,7 @@ make + testSymbolicFactorGraph.run true false @@ -659,6 +664,7 @@ make + tests/testBayesTree true false @@ -752,46 +758,6 @@ true true - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j6 -j8 - testWhiteNoiseFactor.run - true - true - true - make -j5 @@ -834,6 +800,7 @@ make + -j4 testErrors.run true true @@ -1025,6 +992,7 @@ make + testErrors.run true false @@ -1150,14 +1118,6 @@ true true - - make - -j5 - testParticleFactor.run - true - true - true - make -j2 @@ -1262,46 +1222,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1312,7 +1232,6 @@ make - testSimulated2DOriented.run true false @@ -1424,7 +1343,6 @@ make - testSimulated2D.run true false @@ -1432,7 +1350,6 @@ make - testSimulated3D.run true false @@ -1446,44 +1363,44 @@ true true - + make -j5 - testEliminationTree.run + testBTree.run true true true - + make -j5 - testInference.run + testDSF.run true true true - + make -j5 - testKey.run + testDSFMap.run true true true - + make - -j1 - testSymbolicBayesTree.run + -j5 + testDSFVector.run true - false + true true - + make - -j1 - testSymbolicSequentialSolver.run + -j5 + testFixedVector.run true - false + true true @@ -1574,6 +1491,46 @@ true true + + make + -j5 + testEliminationTree.run + true + true + true + + + make + -j5 + testInference.run + true + true + true + + + make + -j5 + testKey.run + true + true + true + + + make + -j1 + testSymbolicBayesTree.run + true + false + true + + + make + -j1 + testSymbolicSequentialSolver.run + true + false + true + make -j5 @@ -1592,6 +1549,7 @@ cpack + -G DEB true false @@ -1599,6 +1557,7 @@ cpack + -G RPM true false @@ -1606,6 +1565,7 @@ cpack + -G TGZ true false @@ -1613,6 +1573,7 @@ cpack + -j4 --config CPackSourceConfig.cmake true true @@ -1963,8 +1924,16 @@ make - -j5 - tests/testGaussianISAM2 + -j4 + check.nonlinear_unstable + true + true + true + + + make + -j4 + check.geometry true true true @@ -1979,8 +1948,8 @@ make - -j2 - testPoint2.run + -j4 + install true true true @@ -2578,7 +2547,6 @@ make - testSimulated2D.run true false @@ -2586,7 +2554,6 @@ make - testSimulated3D.run true false @@ -2594,6 +2561,7 @@ make + testSymbolicBayesNetB.run true false @@ -2615,6 +2583,38 @@ true true + + make + -j4 + testParticleFactor.run + true + true + true + + + make + -j4 + testBasisCompositions.run + true + true + true + + + make + -j4 + testExpressionFactor.run + true + true + true + + + make + -j4 + testExpressionMeta.run + true + true + true + make -j2 @@ -2817,6 +2817,7 @@ make + -j4 testSymbolicBayesNetB.run true true @@ -2862,6 +2863,54 @@ true true + + make + -j4 + testKey.run + true + true + true + + + make + -j4 + testLinearContainerFactor.run + true + true + true + + + make + -j4 + testOrdering.run + true + true + true + + + make + -j4 + testValues.run + true + true + true + + + make + -j4 + testWhiteNoiseFactor.run + true + true + true + + + make + -j4 + testExpression.run + true + true + true + make -j4 @@ -2880,7 +2929,6 @@ make - tests/testGaussianISAM2 true false From 49932cf0f34cf4e211f74215746e9ea28f4f6ed3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 01:55:31 +0100 Subject: [PATCH 10/76] New FixedRef type --- gtsam/base/Matrix.h | 69 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 69 insertions(+) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 132bf79ad..418fec1e5 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -534,6 +534,75 @@ Eigen::Matrix CayleyFixed(const Eigen::Matrix& A) { std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false); +/** + * Eigen::Ref like class that cane take either a fixed size or dynamic + * Eigen matrix. In the latter case, the dynamic matrix will be resized. + * Finally, the default constructor acts like boost::none. + */ +template +class FixedRef { + +public: + + /// Fixed size type + typedef Eigen::Matrix Fixed; + +private: + + bool empty_; + Eigen::Map map_; + + // Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html + // to make map_ usurp the memory of the fixed size matrix + void usurp(double* data) { + new (&map_) Eigen::Map(data); + } + +public: + + /// Defdault constructo acts like boost::none + FixedRef() : + empty_(true), map_(NULL) { + } + + /// Defdault constructo acts like boost::none + FixedRef(boost::none_t none) : + empty_(true), map_(NULL) { + } + + /// Constructor that will usurp data of a fixed size matrix + FixedRef(Fixed& fixed) : + empty_(false), map_(NULL) { + usurp(fixed.data()); + } + + /// Constructor that will resize a dynamic matrix + FixedRef(Matrix& dynamic) : + empty_(false), map_(NULL) { + dynamic.resize(Rows, Cols); + usurp(dynamic.data()); + } + + /// Constructor compatible with old-style + FixedRef(const boost::optional optional) : + empty_(!optional), map_(NULL) { + if (optional) { + optional->resize(Rows, Cols); + usurp(optional->data()); + } + } + + /// Return true is allocated, false if default constructor was used + operator bool() const { + return !empty_; + } + + /// De-reference, like boost optional + Eigen::Map& operator* () { + return map_; + } +}; + } // namespace gtsam #include From 1e4905ef04613afb911cbfd1ff479fba780f741d Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 01:56:28 +0100 Subject: [PATCH 11/76] Now use new FixedRef type in all methods that I used fixed-sized matrices in to develop Expressions. All copy/paste bloat is now gone, and boost::none arguments are back. --- gtsam/geometry/Cal3Bundler.cpp | 43 +--------- gtsam/geometry/Cal3Bundler.h | 23 +----- gtsam/geometry/Cal3DS2_Base.cpp | 23 +----- gtsam/geometry/Cal3DS2_Base.h | 9 +-- gtsam/geometry/Cal3_S2.h | 72 +---------------- gtsam/geometry/PinholeCamera.h | 139 +++++--------------------------- gtsam/geometry/Point3.cpp | 22 +---- gtsam/geometry/Point3.h | 8 +- gtsam/geometry/Pose2.cpp | 25 +----- gtsam/geometry/Pose2.h | 16 +--- gtsam/geometry/Pose3.cpp | 27 +------ gtsam/geometry/Pose3.h | 8 +- gtsam/geometry/Rot3.h | 11 +-- gtsam/geometry/Rot3M.cpp | 22 ++--- 14 files changed, 49 insertions(+), 399 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 8ff728d26..df4f385a8 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -64,21 +64,9 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { return true; } -/* ************************************************************************* */ -Point2 Cal3Bundler::uncalibrate(const Point2& p) const { - // r = x^2 + y^2; - // g = (1 + k(1)*r + k(2)*r^2); - // pi(:,i) = g * pn(:,i) - const double x = p.x(), y = p.y(); - const double r = x * x + y * y; - const double g = 1. + (k1_ + k2_ * r) * r; - const double u = g * x, v = g * y; - return Point2(u0_ + f_ * u, v0_ + f_ * v); -} - /* ************************************************************************* */ Point2 Cal3Bundler::uncalibrate(const Point2& p, // - boost::optional Dcal, boost::optional Dp) const { + FixedRef<2, 3> Dcal, FixedRef<2, 2> Dp) const { // r = x^2 + y^2; // g = (1 + k(1)*r + k(2)*r^2); // pi(:,i) = g * pn(:,i) @@ -103,35 +91,6 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, // return Point2(u0_ + f_ * u, v0_ + f_ * v); } -/* ************************************************************************* */ -Point2 Cal3Bundler::uncalibrate(const Point2& p, // - boost::optional Dcal, boost::optional Dp) const { - // r = x^2 + y^2; - // g = (1 + k(1)*r + k(2)*r^2); - // pi(:,i) = g * pn(:,i) - const double x = p.x(), y = p.y(); - const double r = x * x + y * y; - const double g = 1. + (k1_ + k2_ * r) * r; - const double u = g * x, v = g * y; - - // Derivatives make use of intermediate variables above - if (Dcal) { - double rx = r * x, ry = r * y; - Dcal->resize(2, 3); - *Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry; - } - - if (Dp) { - const double a = 2. * (k1_ + 2. * k2_ * r); - const double axx = a * x * x, axy = a * x * y, ayy = a * y * y; - Dp->resize(2,2); - *Dp << g + axx, axy, axy, g + ayy; - *Dp *= f_; - } - - return Point2(u0_ + f_ * u, v0_ + f_ * v); -} - /* ************************************************************************* */ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { // Copied from Cal3DS2 :-( diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 4c77fdf23..886a8fb52 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -106,31 +106,14 @@ public: /** - * 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; - - /** - * Version of uncalibrate with fixed derivatives + * Version of uncalibrate with derivatives * @param p point in intrinsic coordinates * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters * @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; - - /** - * Version of uncalibrate with dynamic derivatives - * @param p point in intrinsic coordinates - * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters - * @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, FixedRef<2, 3> Dcal = boost::none, + FixedRef<2, 2> Dp = boost::none) const; /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index 1105fecfb..3846178fa 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -91,8 +91,7 @@ static Matrix2 D2dintrinsic(double x, double y, double rr, /* ************************************************************************* */ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { + FixedRef<2,9> H1, FixedRef<2,2> H2) const { // rr = x^2 + y^2; // g = (1 + k(1)*rr + k(2)*rr^2); @@ -126,26 +125,6 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); } -/* ************************************************************************* */ -Point2 Cal3DS2_Base::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { - - if (H1 || H2) { - Matrix29 D1; - Matrix2 D2; - Point2 pu = uncalibrate(p, D1, D2); - if (H1) - *H1 = D1; - if (H2) - *H2 = D2; - return pu; - - } else { - return uncalibrate(p); - } -} - /* ************************************************************************* */ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { // Use the following fixed point iteration to invert the radial distortion. diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 61c01e349..cf80a7741 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -113,14 +113,9 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in (distorted) image coordinates */ - Point2 uncalibrate(const Point2& p, - boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const ; - - Point2 uncalibrate(const Point2& p, - boost::optional Dcal, - boost::optional Dp) const ; + FixedRef<2,9> Dcal = boost::none, + FixedRef<2,2> Dp = boost::none) const ; /// Convert (distorted) image coordinates uv to intrinsic coordinates xy Point2 calibrate(const Point2& p, const double tol=1e-5) const; diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index b8f10fccb..a380ff4db 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -23,77 +23,9 @@ #include #include -#include namespace gtsam { -/** - * Eigen::Ref like class that cane take either a fixed size or dynamic - * Eigen matrix. In the latter case, the dynamic matrix will be resized. - * Finally, the default constructor acts like boost::none. - */ -template -class FixedRef { - -public: - - /// Fixed size type - typedef Eigen::Matrix Fixed; - -private: - - bool empty_; - Eigen::Map map_; - - // Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html - // to make map_ usurp the memory of the fixed size matrix - void usurp(double* data) { - new (&map_) Eigen::Map(data); - } - -public: - - /// Defdault constructo acts like boost::none - FixedRef() : - empty_(true), map_(NULL) { - } - - /// Defdault constructo acts like boost::none - FixedRef(boost::none_t none) : - empty_(true), map_(NULL) { - } - - /// Constructor that will usurp data of a fixed size matrix - FixedRef(Fixed& fixed) : - empty_(false), map_(NULL) { - usurp(fixed.data()); - } - - /// Constructor that will resize a dynamic matrix - FixedRef(Matrix& dynamic) : - empty_(false), map_(NULL) { - } - - /// Constructor compatible with old-style - FixedRef(const boost::optional optional) : - empty_(!optional), map_(NULL) { - if (optional) { - optional->resize(Rows, Cols); - usurp(optional->data()); - } - } - - /// Return true is allocated, false if default constructor was used - operator bool() const { - return !empty_; - } - - /// De-reference, like boost optional - Eigen::Map& operator* () { - return map_; - } -}; - /** * @brief The most common 5DOF 3D->2D calibration * @addtogroup geometry @@ -216,8 +148,8 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, FixedRef<2,5> Dcal = FixedRef<2,5>(), - FixedRef<2,2> Dp = FixedRef<2,2>()) const; + Point2 uncalibrate(const Point2& p, FixedRef<2,5> Dcal = boost::none, + FixedRef<2,2> 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 ad0c8d659..134b21383 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -274,8 +274,8 @@ public: * @param P A point in camera coordinates * @param Dpoint is the 2*3 Jacobian w.r.t. P */ - inline static Point2 project_to_camera(const Point3& P, - boost::optional Dpoint = boost::none) { + static Point2 project_to_camera(const Point3& P, // + FixedRef<2, 3> Dpoint = boost::none) { #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (P.z() <= 0) throw CheiralityException(); @@ -294,20 +294,6 @@ public: return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); } - /** project a point from world coordinate to the image - * @param pw is a point in world coordinates - */ - inline Point2 project(const Point3& pw) const { - - // Transform to camera coordinates and check cheirality - const Point3 pc = pose_.transform_to(pw); - - // Project to normalized image coordinates - const Point2 pn = project_to_camera(pc); - - return K_.uncalibrate(pn); - } - typedef Eigen::Matrix Matrix2K; /** project a point from world coordinate to the image @@ -317,10 +303,10 @@ public: * @param Dcal is the Jacobian w.r.t. calibration */ inline Point2 project( - const Point3& pw, // - boost::optional Dpose, - boost::optional Dpoint, - boost::optional Dcal) const { + const Point3& pw, + FixedRef<2,6> Dpose = boost::none, + FixedRef<2,3> Dpoint = boost::none, + FixedRef<2,DimK> Dcal = boost::none) const { // Transform to camera coordinates and check cheirality const Point3 pc = pose_.transform_to(pw); @@ -341,45 +327,6 @@ public: if (Dpoint) calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); return pi; - } else - return K_.uncalibrate(pn, Dcal, boost::none); - } - - /** project a point from world coordinate to the image - * @param pw is a point in world coordinates - * @param Dpose is the Jacobian w.r.t. pose3 - * @param Dpoint is the Jacobian w.r.t. point3 - * @param Dcal is the Jacobian w.r.t. calibration - */ - inline Point2 project( - const Point3& pw, // - boost::optional Dpose, - boost::optional Dpoint, - boost::optional Dcal) const { - - // Transform to camera coordinates and check cheirality - const Point3 pc = pose_.transform_to(pw); - - // Project to normalized image coordinates - const Point2 pn = project_to_camera(pc); - - if (Dpose || Dpoint) { - const double z = pc.z(), d = 1.0 / z; - - // uncalibration - Matrix2 Dpi_pn; - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - // chain the Jacobian matrices - if (Dpose) { - Dpose->resize(2, 6); - calculateDpose(pn, d, Dpi_pn, *Dpose); - } - if (Dpoint) { - Dpoint->resize(2, 3); - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - } - return pi; } else return K_.uncalibrate(pn, Dcal); } @@ -391,10 +338,10 @@ public: * @param Dcal is the Jacobian w.r.t. calibration */ inline Point2 projectPointAtInfinity( - const Point3& pw, // - boost::optional Dpose = boost::none, - boost::optional Dpoint = boost::none, - boost::optional Dcal = boost::none) const { + const Point3& pw, + FixedRef<2,6> Dpose = boost::none, + FixedRef<2,2> Dpoint = boost::none, + FixedRef<2,DimK> Dcal = boost::none) const { if (!Dpose && !Dpoint && !Dcal) { const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter) @@ -403,11 +350,11 @@ public: } // world to camera coordinate - Matrix Dpc_rot /* 3*3 */, Dpc_point /* 3*3 */; + Matrix3 Dpc_rot, Dpc_point; const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point); - Matrix Dpc_pose = Matrix::Zero(3, 6); - Dpc_pose.block(0, 0, 3, 3) = Dpc_rot; + Matrix36 Dpc_pose; Dpc_pose.setZero(); + Dpc_pose.leftCols<3>() = Dpc_rot; // camera to normalized image coordinate Matrix23 Dpn_pc; // 2*3 @@ -418,34 +365,22 @@ public: const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); // chain the Jacobian matrices - const Matrix Dpi_pc = Dpi_pn * Dpn_pc; + const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc; if (Dpose) *Dpose = Dpi_pc * Dpc_pose; if (Dpoint) - *Dpoint = (Dpi_pc * Dpc_point).block(0, 0, 2, 2); // only 2dof are important for the point (direction-only) + *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only) return pi; } - typedef Eigen::Matrix Matrix2K6; - - /** project a point from world coordinate to the image - * @param pw is a point in the world coordinate - */ - Point2 project2(const Point3& pw) const { - const Point3 pc = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); - return K_.uncalibrate(pn); - } - /** project a point from world coordinate to the image, fixed Jacobians * @param pw is a point in the world coordinate * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] * @param Dpoint is the Jacobian w.r.t. point3 */ - Point2 project2( - const Point3& pw, // - boost::optional Dcamera, - boost::optional Dpoint) const { + Point2 project2(const Point3& pw, // + FixedRef<2, 6 + DimK> Dcamera = boost::none, + FixedRef<2, 3> Dpoint = boost::none) const { const Point3 pc = pose_.transform_to(pw); const Point2 pn = project_to_camera(pc); @@ -461,8 +396,8 @@ public: const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); if (Dcamera) { // TODO why does leftCols<6>() not compile ?? - calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols(6)); - Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib + calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6)); + (*Dcamera).rightCols(K_.dim()) = Dcal; // Jacobian wrt calib } if (Dpoint) { calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); @@ -471,40 +406,6 @@ public: } } - /** project a point from world coordinate to the image - * @param pw is a point in the world coordinate - * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] - * @param Dpoint is the Jacobian w.r.t. point3 - */ - Point2 project2(const Point3& pw, // - boost::optional Dcamera, boost::optional Dpoint) const { - - const Point3 pc = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); - - if (!Dcamera && !Dpoint) { - return K_.uncalibrate(pn); - } else { - const double z = pc.z(), d = 1.0 / z; - - // uncalibration - Matrix2K Dcal; - Matrix2 Dpi_pn; - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - if (Dcamera) { - Dcamera->resize(2, this->dim()); - calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>()); - Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib - } - if (Dpoint) { - Dpoint->resize(2, 3); - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - } - return pi; - } - } - /// backproject a 2-dimensional point to a 3-dimensional point at given depth inline Point3 backproject(const Point2& p, double depth) const { const Point2 pn = K_.calibrate(p); diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index ce56c78c1..43f2239e2 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -94,26 +94,8 @@ double Point3::dot(const Point3 &q) const { } /* ************************************************************************* */ -double Point3::norm() const { - return sqrt(x_ * x_ + y_ * y_ + z_ * z_); -} - -/* ************************************************************************* */ -double Point3::norm(boost::optional H) const { - double r = norm(); - if (H) { - H->resize(1,3); - if (fabs(r) > 1e-10) - *H << x_ / r, y_ / r, z_ / r; - else - *H << 1, 1, 1; // really infinity, why 1 ? - } - return r; -} - -/* ************************************************************************* */ -double Point3::norm(boost::optional&> H) const { - double r = norm(); +double Point3::norm(FixedRef<1,3> H) const { + double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_); if (H) { if (fabs(r) > 1e-10) *H << x_ / r, y_ / r, z_ / r; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index b7f7f8ffa..9ef6696b7 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -180,14 +180,8 @@ namespace gtsam { return (p2 - *this).norm(); } - /** Distance of the point from the origin */ - double norm() const; - /** Distance of the point from the origin, with Jacobian */ - double norm(boost::optional H) const; - - /** Distance of the point from the origin, with Jacobian */ - double norm(boost::optional&> H) const; + double norm(FixedRef<1,3> H = boost::none) const; /** normalize, with optional Jacobian */ Point3 normalize(boost::optional H = boost::none) const; diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 7a693ed3d..41929c242 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -123,17 +123,10 @@ Pose2 Pose2::inverse(boost::optional H1) const { return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } -/* ************************************************************************* */ -// see doc/math.lyx, SE(2) section -Point2 Pose2::transform_to(const Point2& point) const { - Point2 d = point - t_; - return r_.unrotate(d); -} - /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_to(const Point2& point, - boost::optional H1, boost::optional H2) const { + FixedRef<2, 3> H1, FixedRef<2, 2> H2) const { Point2 d = point - t_; Point2 q = r_.unrotate(d); if (!H1 && !H2) return q; @@ -144,20 +137,6 @@ Point2 Pose2::transform_to(const Point2& point, return q; } -/* ************************************************************************* */ -// see doc/math.lyx, SE(2) section -Point2 Pose2::transform_to(const Point2& point, - boost::optional H1, boost::optional H2) const { - Point2 d = point - t_; - Point2 q = r_.unrotate(d); - if (!H1 && !H2) return q; - if (H1) *H1 = (Matrix(2, 3) << - -1.0, 0.0, q.y(), - 0.0, -1.0, -q.x()).finished(); - if (H2) *H2 = r_.transpose(); - return q; -} - /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Pose2 Pose2::compose(const Pose2& p2, boost::optional H1, @@ -171,7 +150,7 @@ Pose2 Pose2::compose(const Pose2& p2, boost::optional H1, /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_from(const Point2& p, - boost::optional H1, boost::optional H2) const { + FixedRef<2, 3> H1, FixedRef<2, 2> H2) const { const Point2 q = r_ * p; if (H1 || H2) { const Matrix R = r_.matrix(); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index d43be6afb..cac79e6fb 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -190,23 +190,15 @@ public: /// @name Group Action on Point2 /// @{ - /** Return point coordinates in pose coordinate frame */ - Point2 transform_to(const Point2& point) const; - /** Return point coordinates in pose coordinate frame */ Point2 transform_to(const Point2& point, - boost::optional H1, - boost::optional H2) const; - - /** Return point coordinates in pose coordinate frame */ - Point2 transform_to(const Point2& point, - boost::optional H1, - boost::optional H2) const; + FixedRef<2, 3> H1 = boost::none, + FixedRef<2, 2> H2 = boost::none) const; /** Return point coordinates in global frame */ Point2 transform_from(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + FixedRef<2, 3> H1 = boost::none, + FixedRef<2, 2> H2 = boost::none) const; /** syntactic sugar for transform_from */ inline Point2 operator*(const Point2& point) const { return transform_from(point);} diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index b134553d9..42d207bb1 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -254,13 +254,8 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, } /* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p) const { - return R_.unrotate(p - t_); -} - -/* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, - boost::optional Dpoint) const { +Point3 Pose3::transform_to(const Point3& p, FixedRef<3,6> Dpose, + FixedRef<3,3> Dpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); @@ -277,24 +272,6 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, return q; } -/* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, - boost::optional Dpoint) const { - const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt*(p - t_).vector()); - if (Dpose) { - const double wx = q.x(), wy = q.y(), wz = q.z(); - Dpose->resize(3, 6); - (*Dpose) << - 0.0, -wz, +wy,-1.0, 0.0, 0.0, - +wz, 0.0, -wx, 0.0,-1.0, 0.0, - -wy, +wx, 0.0, 0.0, 0.0,-1.0; - } - if (Dpoint) - *Dpoint = Rt; - return q; -} - /* ************************************************************************* */ Pose3 Pose3::compose(const Pose3& p2, boost::optional H1, boost::optional H2) const { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 2090558a6..e5460fd82 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -249,13 +249,9 @@ public: * @param Dpoint optional 3*3 Jacobian wrpt point * @return point in Pose coordinates */ - Point3 transform_to(const Point3& p) const; - Point3 transform_to(const Point3& p, - boost::optional Dpose, boost::optional Dpoint) const; - - Point3 transform_to(const Point3& p, - boost::optional Dpose, boost::optional Dpoint) const; + FixedRef<3,6> Dpose = boost::none, + FixedRef<3,3> Dpoint = boost::none) const; /// @} /// @name Standard Interface diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index ef2d19750..ffa2f81c5 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -218,15 +218,8 @@ namespace gtsam { Rot3 inverse(boost::optional H1=boost::none) const; /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2) const; - - /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2, boost::optional H1, - boost::optional H2) const; - - /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2, boost::optional H1, - boost::optional H2) const; + Rot3 compose(const Rot3& R2, FixedRef<3, 3> H1 = boost::none, + FixedRef<3, 3> H2 = boost::none) const; /** compose two rotations */ Rot3 operator*(const Rot3& R2) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index d0c7e95f3..623a13664 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -142,23 +142,11 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) { } /* ************************************************************************* */ -Rot3 Rot3::compose (const Rot3& R2) const { - return *this * R2; -} - -/* ************************************************************************* */ -Rot3 Rot3::compose (const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return *this * R2; -} - -/* ************************************************************************* */ -Rot3 Rot3::compose (const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; +Rot3 Rot3::compose(const Rot3& R2, FixedRef<3, 3> H1, FixedRef<3, 3> H2) const { + if (H1) + *H1 = R2.transpose(); + if (H2) + *H2 = I3; return *this * R2; } From 3d9d29d1c52779161d242e23c67f4ab3a1807ece Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 01:56:56 +0100 Subject: [PATCH 12/76] Now only accept functiosn that use new FixedRef type --- gtsam/nonlinear/Expression-inl.h | 4 +--- gtsam/nonlinear/Expression.h | 5 ++--- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 08dd18ee3..a87d9896e 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -455,9 +455,7 @@ struct Jacobian { /// meta-function to generate JacobianTA optional reference template struct OptionalJacobian { - typedef Eigen::Matrix::value, - traits::dimension::value> Jacobian; - typedef boost::optional type; + typedef FixedRef::value, traits::dimension::value> type; }; /** diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index d44d21cd7..940ad1778 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -224,9 +224,8 @@ template struct apply_compose { typedef T result_type; static const int Dim = traits::dimension::value; - typedef Eigen::Matrix Jacobian; - T operator()(const T& x, const T& y, boost::optional H1, - boost::optional H2) const { + T operator()(const T& x, const T& y, FixedRef H1 = boost::none, + FixedRef H2 = boost::none) const { return x.compose(y, H1, H2); } }; From ab864530bfa161a42454674646cee000bfd1237b Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 01:57:28 +0100 Subject: [PATCH 13/76] Fixed small cast issue --- gtsam/slam/SmartFactorBase.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 5d57d2862..d9e7b9819 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -280,7 +280,7 @@ public: Vector bi; try { - bi = -(cameras[i].project(point, Fi, Ei, static_cast(Hcali)) - this->measured_.at(i)).vector(); + bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); if(body_P_sensor_){ Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); Matrix J(6, 6); From ee790839c6e727bf18759ef381883f420dd17d2c Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 01:57:45 +0100 Subject: [PATCH 14/76] Now only accept new FixedRef type --- gtsam_unstable/nonlinear/AdaptAutoDiff.h | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/nonlinear/AdaptAutoDiff.h b/gtsam_unstable/nonlinear/AdaptAutoDiff.h index 96978d9cf..70ba285c1 100644 --- a/gtsam_unstable/nonlinear/AdaptAutoDiff.h +++ b/gtsam_unstable/nonlinear/AdaptAutoDiff.h @@ -50,11 +50,8 @@ class AdaptAutoDiff { public: - typedef Eigen::Matrix JacobianTA1; - typedef Eigen::Matrix JacobianTA2; - - T operator()(const A1& a1, const A2& a2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) { + T operator()(const A1& a1, const A2& a2, FixedRef H1 = boost::none, + FixedRef H2 = boost::none) { using ceres::internal::AutoDiff; From 0d41523725338847456d8e7b988d37d0ed9fd5cf Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 01:57:55 +0100 Subject: [PATCH 15/76] Use new FixedRef type --- gtsam_unstable/slam/expressions.h | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/slam/expressions.h b/gtsam_unstable/slam/expressions.h index 009be46a1..c72a9d3f7 100644 --- a/gtsam_unstable/slam/expressions.h +++ b/gtsam_unstable/slam/expressions.h @@ -20,9 +20,8 @@ typedef Expression Rot2_; typedef Expression Pose2_; Point2_ transform_to(const Pose2_& x, const Point2_& p) { - Point2(Pose2::*transform)(const Point2& p, - boost::optional H1, - boost::optional H2) const = &Pose2::transform_to; + Point2 (Pose2::*transform)(const Point2& p, FixedRef<2, 3> H1, + FixedRef<2, 2> H2) const = &Pose2::transform_to; return Point2_(x, transform, p); } @@ -35,9 +34,8 @@ typedef Expression Pose3_; Point3_ transform_to(const Pose3_& x, const Point3_& p) { - Point3(Pose3::*transform)(const Point3& p, - boost::optional Dpose, - boost::optional Dpoint) const = &Pose3::transform_to; + Point3 (Pose3::*transform)(const Point3& p, FixedRef<3, 6> Dpose, + FixedRef<3, 3> Dpoint) const = &Pose3::transform_to; return Point3_(x, transform, p); } @@ -51,8 +49,7 @@ Point2_ project(const Point3_& p_cam) { } Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, - boost::optional Dpose, boost::optional Dpoint, - boost::optional Dcal) { + FixedRef<2, 6> Dpose, FixedRef<2, 3> Dpoint, FixedRef<2, 5> Dcal) { return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); } From dc40864a8f77bb08a9a7f9e654afc9b4b05358a2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 01:58:11 +0100 Subject: [PATCH 16/76] Excluded Paul's test --- gtsam_unstable/nonlinear/tests/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/nonlinear/tests/CMakeLists.txt b/gtsam_unstable/nonlinear/tests/CMakeLists.txt index 5b1fd07d4..ad14fcd9b 100644 --- a/gtsam_unstable/nonlinear/tests/CMakeLists.txt +++ b/gtsam_unstable/nonlinear/tests/CMakeLists.txt @@ -1 +1 @@ -gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "" "gtsam_unstable") +gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "testCustomChartExpression.cpp" "gtsam_unstable") From 747071138e2b2ab484fc2e515d2dfccaf23d99d4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 01:58:24 +0100 Subject: [PATCH 17/76] Use new FixedRef type in tests --- gtsam/nonlinear/tests/testExpression.cpp | 20 +++++----- .../tests/testBasisDecompositions.cpp | 2 +- .../nonlinear/tests/testExpressionFactor.cpp | 5 +-- .../nonlinear/tests/testExpressionMeta.cpp | 40 +++++++++---------- 4 files changed, 32 insertions(+), 35 deletions(-) diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index dfa60e54e..b33d0e5cd 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -34,8 +34,8 @@ using namespace gtsam; /* ************************************************************************* */ template -Point2 uncalibrate(const CAL& K, const Point2& p, - boost::optional Dcal, boost::optional Dp) { +Point2 uncalibrate(const CAL& K, const Point2& p, FixedRef<2, 5> Dcal, + FixedRef<2, 2> Dp) { return K.uncalibrate(p, Dcal, Dp); } @@ -75,13 +75,13 @@ TEST(Expression, Leaves) { /* ************************************************************************* */ // Unary(Leaf) namespace unary { -Point2 f0(const Point3& p, boost::optional H) { +Point2 f0(const Point3& p, FixedRef<2,3> H) { return Point2(); } -LieScalar f1(const Point3& p, boost::optional&> H) { +LieScalar f1(const Point3& p, FixedRef<1, 3> H) { return LieScalar(0.0); } -double f2(const Point3& p, boost::optional&> H) { +double f2(const Point3& p, FixedRef<1, 3> H) { return 0.0; } Expression p(1); @@ -117,7 +117,7 @@ TEST(Expression, NullaryMethod) { // Check dims as map std::map map; norm.dims(map); - LONGS_EQUAL(1,map.size()); + LONGS_EQUAL(1, map.size()); // Get value and Jacobians std::vector H(1); @@ -133,9 +133,8 @@ TEST(Expression, NullaryMethod) { // Binary(Leaf,Leaf) namespace binary { // Create leaves -double doubleF(const Pose3& pose, const Point3& point, - boost::optional&> H1, - boost::optional&> H2) { +double doubleF(const Pose3& pose, // + const Point3& point, FixedRef<1, 6> H1, FixedRef<1, 3> H2) { return 0.0; } Expression x(1); @@ -244,8 +243,7 @@ TEST(Expression, compose3) { /* ************************************************************************* */ // Test with ternary function Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - boost::optional H1, boost::optional H2, - boost::optional H3) { + FixedRef<3, 3> H1, FixedRef<3, 3> H2, FixedRef<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); diff --git a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp index f113a4f64..6afeac15b 100644 --- a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp +++ b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp @@ -60,7 +60,7 @@ public: } /// Given coefficients c, predict value for x - double operator()(const Coefficients& c, boost::optional H) { + double operator()(const Coefficients& c, FixedRef<1, N> H) { if (H) (*H) = H_; return H_ * c; diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index d146ea945..d37f1f80a 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -126,7 +126,7 @@ TEST(ExpressionFactor, Unary) { /* ************************************************************************* */ static Point2 myUncal(const Cal3_S2& K, const Point2& p, - boost::optional Dcal, boost::optional Dp) { + FixedRef<2,5> Dcal, FixedRef<2,2> Dp) { return K.uncalibrate(p, Dcal, Dp); } @@ -392,8 +392,7 @@ TEST(ExpressionFactor, compose3) { /* ************************************************************************* */ // Test compose with three arguments Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - boost::optional H1, boost::optional H2, - boost::optional H3) { + FixedRef<3, 3> H1, FixedRef<3, 3> H2, FixedRef<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp index d10e31002..178b1803c 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp @@ -179,24 +179,24 @@ TEST(ExpressionFactor, InvokeDerivatives) { // Let's assign it it to a boost function object // cast is needed because Pose3::transform_to is overloaded - typedef boost::function F; - F f = static_cast(&Pose3::transform_to); - - // Create arguments -Pose3 pose; - Point3 point; - typedef boost::fusion::vector Arguments; - Arguments args = boost::fusion::make_vector(pose, point); - - // Create fused function (takes fusion vector) and call it - boost::fusion::fused g(f); - Point3 actual = g(args); - CHECK(assert_equal(point,actual)); - - // We can *immediately* do this using invoke - Point3 actual2 = boost::fusion::invoke(f, args); - CHECK(assert_equal(point,actual2)); +// typedef boost::function F; +// F f = static_cast(&Pose3::transform_to); +// +// // Create arguments +// Pose3 pose; +// Point3 point; +// typedef boost::fusion::vector Arguments; +// Arguments args = boost::fusion::make_vector(pose, point); +// +// // Create fused function (takes fusion vector) and call it +// boost::fusion::fused g(f); +// Point3 actual = g(args); +// CHECK(assert_equal(point,actual)); +// +// // We can *immediately* do this using invoke +// Point3 actual2 = boost::fusion::invoke(f, args); +// CHECK(assert_equal(point,actual2)); // Now, let's create the optional Jacobian arguments typedef Point3 T; @@ -215,8 +215,8 @@ struct proxy { return pose.transform_to(point); } Point3 operator()(const Pose3& pose, const Point3& point, - boost::optional Dpose, - boost::optional Dpoint) const { + FixedRef<3,6> Dpose, + FixedRef<3,3> Dpoint) const { return pose.transform_to(point, Dpose, Dpoint); } }; From bae51b3ceae80c0be51933efe738d7e1c3974b68 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 02:12:10 +0100 Subject: [PATCH 18/76] Restored develope .cproject --- .cproject | 1170 ++++++++++++++++++++++++++++++++--------------------- 1 file changed, 701 insertions(+), 469 deletions(-) diff --git a/.cproject b/.cproject index 5472443bf..9c03c5b7d 100644 --- a/.cproject +++ b/.cproject @@ -510,6 +510,22 @@ true true + + make + -j5 + SFMExampleExpressions.run + true + true + true + + + make + -j5 + Pose2SLAMExampleExpressions.run + true + true + true + make -j5 @@ -576,7 +592,6 @@ make - tests/testBayesTree.run true false @@ -584,7 +599,6 @@ make - testBinaryBayesNet.run true false @@ -632,7 +646,6 @@ make - testSymbolicBayesNet.run true false @@ -640,7 +653,6 @@ make - tests/testSymbolicFactor.run true false @@ -648,7 +660,6 @@ make - testSymbolicFactorGraph.run true false @@ -664,68 +675,11 @@ make - tests/testBayesTree true false true - - make - -j5 - testPoseRTV.run - true - true - true - - - make - -j5 - testIMUSystem.run - true - true - true - - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - make -j2 @@ -734,6 +688,14 @@ true true + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -758,55 +720,135 @@ true true - + make -j5 - schedulingExample.run + testAHRS.run true true true - + make -j5 - testCSP.run + testInvDepthFactor3.run true true true - + make -j5 - testScheduler.run + testMultiProjectionFactor.run true true true - + make -j5 - schedulingQuals12.run + testPoseRotationPrior.run true true true - + make -j5 - testSudoku.run + testPoseTranslationPrior.run true true true - + make - -j4 - testErrors.run + -j5 + testReferenceFrameFactor.run true true true - + + make + -j5 + testSmartProjectionFactor.run + true + true + true + + + make + -j5 + testTSAMFactors.run + true + true + true + + + make + -j5 + testInertialNavFactor_GlobalVelocity.run + true + true + true + + + make + -j5 + testInvDepthFactorVariant3.run + true + true + true + + + make + -j5 + testInvDepthFactorVariant1.run + true + true + true + + + make + -j5 + testEquivInertialNavFactor_GlobalVel.run + true + true + true + + + make + -j5 + testInvDepthFactorVariant2.run + true + true + true + + + make + -j5 + testRelativeElevationFactor.run + true + true + true + + + make + -j5 + testPoseBetweenFactor.run + true + true + true + + + make + -j5 + testGaussMarkov1stOrderFactor.run + true + true + true + + make -j5 testGaussianFactorGraphUnordered.run @@ -822,66 +864,154 @@ true true - + make -j5 - testInvDepthFactor3.run + testGaussianConditional.run true true true - + make -j5 - testPoseTranslationPrior.run + testGaussianDensity.run true true true - + make -j5 - testPoseRotationPrior.run + testGaussianJunctionTree.run true true true - + make -j5 - testReferenceFrameFactor.run + testHessianFactor.run true true true - + make -j5 - testAHRS.run + testJacobianFactor.run true true true - + make - -j8 + -j5 + testKalmanFilter.run + true + true + true + + + make + -j5 + testNoiseModel.run + true + true + true + + + make + -j5 + testSampler.run + true + true + true + + + make + -j5 + testSerializationLinear.run + true + true + true + + + make + -j5 + testVectorValues.run + true + true + true + + + make + -j5 + testGaussianBayesTree.run + true + true + true + + + make + -j5 + timeCameraExpression.run + true + true + true + + + make + -j5 + timeOneCameraExpression.run + true + true + true + + + make + -j5 + timeSFMExpressions.run + true + true + true + + + make + -j5 + timeAdaptAutoDiff.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + + + make + -j5 testImuFactor.run true true true - + make -j5 - testMultiProjectionFactor.run + testAHRSFactor.run true true true - + make - -j5 - testSmartProjectionFactor.run + -j8 + testAttitudeFactor.run true true true @@ -992,7 +1122,6 @@ make - testErrors.run true false @@ -1072,10 +1201,10 @@ make - VERBOSE=1 - wrap_gtsam + -j2 + testDSFVector.run true - false + true true @@ -1224,23 +1353,8 @@ make - -j5 - testGaussianISAM2.run - true - true - true - - - make - testSimulated2DOriented.run - true - false - true - - - make - -j5 - timing.tests + -j2 + all true true true @@ -1317,6 +1431,22 @@ true true + + make + + testSimulated2DOriented.run + true + false + true + + + make + -j2 + testVSLAMConfig.run + true + true + true + make -j2 @@ -1343,6 +1473,7 @@ make + testSimulated2D.run true false @@ -1350,6 +1481,7 @@ make + testSimulated3D.run true false @@ -1403,94 +1535,6 @@ true true - - make - -j5 - testVectorValues.run - true - true - true - - - make - -j5 - testNoiseModel.run - true - true - true - - - make - -j5 - testHessianFactor.run - true - true - true - - - make - -j5 - testGaussianConditional.run - true - true - true - - - make - -j5 - testGaussianFactorGraphUnordered.run - true - true - true - - - make - -j5 - testGaussianJunctionTree.run - true - true - true - - - make - -j5 - testKalmanFilter.run - true - true - true - - - make - -j5 - testGaussianDensity.run - true - true - true - - - make - -j5 - testSerializationLinear.run - true - true - true - - - make - -j5 - testJacobianFactor.run - true - true - true - - - make - -j5 - testSampler.run - true - true - true - make -j5 @@ -1533,8 +1577,8 @@ make - -j5 - testGaussianFactorGraphB.run + -j2 + check true true true @@ -1547,38 +1591,6 @@ true true - - cpack - - -G DEB - true - false - true - - - cpack - - -G RPM - true - false - true - - - cpack - - -G TGZ - true - false - true - - - cpack - -j4 - --config CPackSourceConfig.cmake - true - true - true - make -j3 @@ -1682,6 +1694,14 @@ true true + + make + -j2 VERBOSE=1 + check.geometry + true + false + true + make -j5 @@ -1770,6 +1790,34 @@ false true + + cpack + -G DEB + true + false + true + + + cpack + -G RPM + true + false + true + + + cpack + -G TGZ + true + false + true + + + cpack + --config CPackSourceConfig.cmake + true + false + true + make -j5 @@ -1924,16 +1972,24 @@ make - -j4 + -j6 -j8 check.nonlinear_unstable true true true - + make - -j4 - check.geometry + -j5 + check.tests + true + true + true + + + make + -j2 + check true true true @@ -1948,7 +2004,7 @@ make - -j4 + -j2 install true true @@ -1965,58 +2021,10 @@ cmake .. - wrap true false true - - make - -j5 - testQPSolver.run - true - true - true - - - make - -j5 - testRot2.run - true - true - true - - - cmake-gui - .. - true - false - true - - - make - -j5 - testClp.run - true - true - true - - - make - -j5 - testlpsolve.run - true - true - true - - - make - -j5 - testLPSolver.run - true - true - true - make -j2 @@ -2027,7 +2035,7 @@ make - -j4 + -j5 testCal3Bundler.run true true @@ -2035,7 +2043,7 @@ make - -j4 + -j5 testCal3DS2.run true true @@ -2043,7 +2051,7 @@ make - -j4 + -j5 testCalibratedCamera.run true true @@ -2051,7 +2059,7 @@ make - -j4 + -j5 testEssentialMatrix.run true true @@ -2067,15 +2075,23 @@ make - -j4 + -j5 testPinholeCamera.run true true true + + make + -j5 + testPoint2.run + true + true + true + make - -j4 + -j5 testPoint3.run true true @@ -2083,7 +2099,7 @@ make - -j4 + -j5 testPose2.run true true @@ -2091,7 +2107,7 @@ make - -j4 + -j5 testPose3.run true true @@ -2099,7 +2115,7 @@ make - -j4 + -j5 testRot3M.run true true @@ -2107,7 +2123,7 @@ make - -j4 + -j5 testSphere2.run true true @@ -2115,40 +2131,40 @@ make - -j4 + -j5 testStereoCamera.run true true true - + make - -j4 - timeCalibratedCamera.run + -j5 + testCal3Unified.run true true true - + make - -j4 - timePinholeCamera.run + -j5 + testRot2.run true true true - + make - -j4 - timeStereoCamera.run + -j5 + testRot3Q.run true true true - + make - -j4 - testCal3_S2.run + -j5 + testRot3.run true true true @@ -2193,6 +2209,14 @@ false true + + make + -j2 + all + true + true + true + make -j2 @@ -2225,66 +2249,74 @@ true true - + make -j5 - testGeneralSFMFactor.run + testIMUSystem.run true true true - + make -j5 - testProjectionFactor.run + testPoseRTV.run true true true - + make -j5 - testGeneralSFMFactor_Cal3Bundler.run + testVelocityConstraint.run true true true - - make - -j6 -j8 - testAntiFactor.run - true - true - true - - - make - -j6 -j8 - testBetweenFactor.run - true - true - true - - + make -j5 - testDataset.run + testVelocityConstraint3.run true true true - + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + make -j5 - testEssentialMatrixFactor.run + testDiscreteConditional.run true true true - + make -j5 - testRotateFactor.run + testDiscreteFactor.run + true + true + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run true true true @@ -2321,42 +2353,90 @@ true true - + make -j5 - testDiscreteFactor.run + testWrap.run true true true - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - + make -j5 - testDiscreteFactorGraph.run + testSpirit.run true true true - + make -j5 - testDiscreteConditional.run + check.wrap true true true - + make -j5 - testDiscreteMarginals.run + testMethod.run + true + true + true + + + make + -j5 + testClass.run + true + true + true + + + make + -j5 + schedulingExample.run + true + true + true + + + make + -j5 + schedulingQuals12.run + true + true + true + + + make + -j5 + schedulingQuals13.run + true + true + true + + + make + -j5 + testCSP.run + true + true + true + + + make + -j5 + testScheduler.run + true + true + true + + + make + -j5 + testSudoku.run true true true @@ -2369,38 +2449,6 @@ true true - - make - -j5 - testInference.run - true - true - true - - - make - -j1 - testSymbolicSequentialSolver.run - true - false - true - - - make - -j5 - testEliminationTree.run - true - true - true - - - make - -j1 - testSymbolicBayesTree.run - true - false - true - make -j2 @@ -2409,18 +2457,34 @@ true true - + make -j5 - testInvDepthCamera3.run + testMatrix.run true true true - + make -j5 - testTriangulation.run + testVector.run + true + true + true + + + make + -j5 + testNumericalDerivative.run + true + true + true + + + make + -j5 + testVerticalBlockMatrix.run true true true @@ -2449,6 +2513,14 @@ true true + + make + -j5 + testGaussianISAM2.run + true + true + true + make -j5 @@ -2521,6 +2593,14 @@ true true + + make + -j5 + timing.tests + true + true + true + make -j5 @@ -2547,26 +2627,73 @@ make - testSimulated2D.run + testGraph.run true false true make - testSimulated3D.run + testJunctionTree.run true false true make - testSymbolicBayesNetB.run true false true + + make + -j5 + testGaussianISAM.run + true + true + true + + + make + -j5 + testDoglegOptimizer.run + true + true + true + + + make + -j5 + testNonlinearFactorGraph.run + true + true + true + + + make + -j5 + testIterative.run + true + true + true + + + make + -j5 + testSubgraphSolver.run + true + true + true + + + make + -j5 + testGaussianFactorGraphB.run + true + true + true + make -j5 @@ -2575,33 +2702,25 @@ true true - + make -j5 - timeIncremental.run + testManifold.run true true true make - -j4 + -j5 testParticleFactor.run true true true - - make - -j4 - testBasisCompositions.run - true - true - true - make - -j4 + -j5 testExpressionFactor.run true true @@ -2609,12 +2728,44 @@ make - -j4 + -j5 testExpressionMeta.run true true true + + make + -j5 + testAdaptAutoDiff.run + true + true + true + + + make + -j5 + testCallRecord.run + true + true + true + + + make + -j4 + testBasisDecompositions.run + true + true + true + + + make + -j4 + testCustomChartExpression.run + true + true + true + make -j2 @@ -2671,18 +2822,74 @@ true true - + make -j5 - testGPSFactor.run + testAntiFactor.run true true true - + make -j5 - testGaussMarkov1stOrderFactor.run + testPriorFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + + + make + -j5 + testPoseRotationPrior.run true true true @@ -2695,30 +2902,6 @@ true true - - make - -j5 - testQPSolver.run - true - true - true - - - make - -j5 - testVector.run - true - true - true - - - make - -j5 - testMatrix.run - true - true - true - make -j2 @@ -2815,10 +2998,18 @@ true true + + make + -j5 + SelfCalibrationExample.run + true + true + true + make - -j4 - testSymbolicBayesNetB.run + -j5 + SFMExample.run true true true @@ -2863,17 +3054,41 @@ true true - + make - -j4 - testKey.run + -j5 + Pose2SLAMExample_lago.run + true + true + true + + + make + -j5 + Pose2SLAMExample_g2o.run + true + true + true + + + make + -j5 + SFMExample_SmartFactor.run + true + true + true + + + make + -j5 + testLago.run true true true make - -j4 + -j5 testLinearContainerFactor.run true true @@ -2881,7 +3096,7 @@ make - -j4 + -j5 testOrdering.run true true @@ -2889,7 +3104,7 @@ make - -j4 + -j5 testValues.run true true @@ -2897,7 +3112,7 @@ make - -j4 + -j5 testWhiteNoiseFactor.run true true @@ -2919,6 +3134,46 @@ true true + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j5 + timeLago.run + true + true + true + + + make + -j5 + timePose3.run + true + true + true + make -j2 @@ -2929,6 +3184,7 @@ make + tests/testGaussianISAM2 true false @@ -3030,30 +3286,6 @@ true true - - make - -j5 - install - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - make -j5 From 9acc602d16cc0e1b015779142f8321f0c0db34a2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 02:18:00 +0100 Subject: [PATCH 19/76] Fixed comments --- gtsam/base/Matrix.h | 30 +++++++++++++++++------------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 418fec1e5..516ecd7b2 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -535,9 +535,11 @@ Eigen::Matrix CayleyFixed(const Eigen::Matrix& A) { std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false); /** - * Eigen::Ref like class that cane take either a fixed size or dynamic - * Eigen matrix. In the latter case, the dynamic matrix will be resized. - * Finally, the default constructor acts like boost::none. + * FixedRef is an Eigen::Ref like class that can take be constructed using + * either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic + * matrix will be resized. Finally, there is a constructor that takes + * boost::none, the default constructor acts like boost::none, and + * boost::optional is also supported for backwards compatibility. */ template class FixedRef { @@ -549,41 +551,41 @@ public: private: - bool empty_; - Eigen::Map map_; + bool empty_; ///< flag whether initialized or not + Eigen::Map map_; /// View on constructor argument, if given - // Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html - // to make map_ usurp the memory of the fixed size matrix + // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html + // uses "placement new" to make map_ usurp the memory of the fixed size matrix void usurp(double* data) { new (&map_) Eigen::Map(data); } public: - /// Defdault constructo acts like boost::none + /// Default constructor acts like boost::none FixedRef() : empty_(true), map_(NULL) { } - /// Defdault constructo acts like boost::none + /// Default constructor acts like boost::none FixedRef(boost::none_t none) : empty_(true), map_(NULL) { } - /// Constructor that will usurp data of a fixed size matrix + /// Constructor that will usurp data of a fixed-size matrix FixedRef(Fixed& fixed) : empty_(false), map_(NULL) { usurp(fixed.data()); } - /// Constructor that will resize a dynamic matrix + /// Constructor that will resize a dynamic matrix (unless already correct) FixedRef(Matrix& dynamic) : empty_(false), map_(NULL) { - dynamic.resize(Rows, Cols); + dynamic.resize(Rows, Cols); // no malloc if correct size usurp(dynamic.data()); } - /// Constructor compatible with old-style + /// Constructor compatible with old-style derivatives FixedRef(const boost::optional optional) : empty_(!optional), map_(NULL) { if (optional) { @@ -601,6 +603,8 @@ public: Eigen::Map& operator* () { return map_; } + + /// TODO: operator->() }; } // namespace gtsam From 8bbcc2f3d1d226999d24e643bf685be11a03aec7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 02:26:14 +0100 Subject: [PATCH 20/76] Cleaned up some small issues from reviewing PR --- gtsam/geometry/Cal3Bundler.h | 1 + gtsam/geometry/PinholeCamera.h | 1 - gtsam/geometry/tests/testCal3DS2.cpp | 1 - 3 files changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 886a8fb52..837888855 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -106,6 +106,7 @@ public: /** + * @brief: convert intrinsic coordinates xy to image coordinates uv * Version of uncalibrate with derivatives * @param p point in intrinsic coordinates * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 134b21383..96d227912 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -44,7 +44,6 @@ private: // Get dimension of calibration type at compile time static const int DimK = traits::dimension::value; - typedef Eigen::Matrix JacobianK; public: diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index d726f14f4..c5a6be2d6 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -20,7 +20,6 @@ #include #include -using namespace std; using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Cal3DS2) From bd342261e462c709f385972b0569bf8103ced3d1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 14:57:05 +0100 Subject: [PATCH 21/76] New OptionalJacobian header/cpp, moved unit test to base --- gtsam/base/Matrix.h | 73 -------------- gtsam/base/OptionalJacobian.h | 115 ++++++++++++++++++++++ gtsam/base/tests/testOptionalJacobian.cpp | 89 +++++++++++++++++ gtsam/geometry/tests/testCal3_S2.cpp | 38 ------- 4 files changed, 204 insertions(+), 111 deletions(-) create mode 100644 gtsam/base/OptionalJacobian.h create mode 100644 gtsam/base/tests/testOptionalJacobian.cpp diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 516ecd7b2..132bf79ad 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -534,79 +534,6 @@ Eigen::Matrix CayleyFixed(const Eigen::Matrix& A) { std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false); -/** - * FixedRef is an Eigen::Ref like class that can take be constructed using - * either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic - * matrix will be resized. Finally, there is a constructor that takes - * boost::none, the default constructor acts like boost::none, and - * boost::optional is also supported for backwards compatibility. - */ -template -class FixedRef { - -public: - - /// Fixed size type - typedef Eigen::Matrix Fixed; - -private: - - bool empty_; ///< flag whether initialized or not - Eigen::Map map_; /// View on constructor argument, if given - - // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html - // uses "placement new" to make map_ usurp the memory of the fixed size matrix - void usurp(double* data) { - new (&map_) Eigen::Map(data); - } - -public: - - /// Default constructor acts like boost::none - FixedRef() : - empty_(true), map_(NULL) { - } - - /// Default constructor acts like boost::none - FixedRef(boost::none_t none) : - empty_(true), map_(NULL) { - } - - /// Constructor that will usurp data of a fixed-size matrix - FixedRef(Fixed& fixed) : - empty_(false), map_(NULL) { - usurp(fixed.data()); - } - - /// Constructor that will resize a dynamic matrix (unless already correct) - FixedRef(Matrix& dynamic) : - empty_(false), map_(NULL) { - dynamic.resize(Rows, Cols); // no malloc if correct size - usurp(dynamic.data()); - } - - /// Constructor compatible with old-style derivatives - FixedRef(const boost::optional optional) : - empty_(!optional), map_(NULL) { - if (optional) { - optional->resize(Rows, Cols); - usurp(optional->data()); - } - } - - /// Return true is allocated, false if default constructor was used - operator bool() const { - return !empty_; - } - - /// De-reference, like boost optional - Eigen::Map& operator* () { - return map_; - } - - /// TODO: operator->() -}; - } // namespace gtsam #include diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h new file mode 100644 index 000000000..9b5615423 --- /dev/null +++ b/gtsam/base/OptionalJacobian.h @@ -0,0 +1,115 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file OptionalJacobian.h + * @brief Special class for optional Matrix arguments + * @author Frank Dellaert + * @author Natesh Srinivasan + * @date Nov 28, 2014 + */ + +#pragma once + +#include + +#ifndef OPTIONALJACOBIAN_NOBOOST +#include +#endif + +namespace gtsam { + +/** + * OptionalJacobian is an Eigen::Ref like class that can take be constructed using + * either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic + * matrix will be resized. Finally, there is a constructor that takes + * boost::none, the default constructor acts like boost::none, and + * boost::optional is also supported for backwards compatibility. + */ +template +class OptionalJacobian { + +public: + + /// Fixed size type + typedef Eigen::Matrix Fixed; + +private: + + bool empty_; ///< flag whether initialized or not + Eigen::Map map_; /// View on constructor argument, if given + + // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html + // uses "placement new" to make map_ usurp the memory of the fixed size matrix + void usurp(double* data) { + new (&map_) Eigen::Map(data); + } + +public: + + /// Default constructor acts like boost::none + OptionalJacobian() : + empty_(true), map_(NULL) { + } + + /// Constructor that will usurp data of a fixed-size matrix + OptionalJacobian(Fixed& fixed) : + empty_(false), map_(NULL) { + usurp(fixed.data()); + } + + /// Constructor that will usurp data of a fixed-size matrix, pointer version + OptionalJacobian(Fixed* fixedPtr) : + empty_(fixedPtr==NULL), map_(NULL) { + if (fixedPtr) + usurp(fixedPtr->data()); + } + + /// Constructor that will resize a dynamic matrix (unless already correct) + OptionalJacobian(Eigen::MatrixXd& dynamic) : + empty_(false), map_(NULL) { + dynamic.resize(Rows, Cols); // no malloc if correct size + usurp(dynamic.data()); + } + +#ifndef OPTIONALJACOBIAN_NOBOOST + + /// Constructor with boost::none just makes empty + OptionalJacobian(boost::none_t none) : + empty_(true), map_(NULL) { + } + + /// Constructor compatible with old-style derivatives + OptionalJacobian(const boost::optional optional) : + empty_(!optional), map_(NULL) { + if (optional) { + optional->resize(Rows, Cols); + usurp(optional->data()); + } + } + +#endif + + /// Return true is allocated, false if default constructor was used + operator bool() const { + return !empty_; + } + + /// De-reference, like boost optional + Eigen::Map& operator*() { + return map_; + } + + /// TODO: operator->() +}; + +} // namespace gtsam + diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp new file mode 100644 index 000000000..a03757e60 --- /dev/null +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -0,0 +1,89 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testOptionalJacobian.cpp + * @brief Unit test for OptionalJacobian + * @author Frank Dellaert + * @date Nov 28, 2014 + **/ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +TEST( OptionalJacobian, Constructors ) +{ + Matrix23 fixed; + Matrix dynamic; + OptionalJacobian<2,3> H1; + OptionalJacobian<2,3> H2(fixed); + OptionalJacobian<2,3> H3(&fixed); + OptionalJacobian<2,3> H4(dynamic); + OptionalJacobian<2,3> H5(boost::none); + boost::optional optional(dynamic); + OptionalJacobian<2,3> H6(optional); +} + +//****************************************************************************** +void test3(OptionalJacobian<2,3> H = OptionalJacobian<2,3>()) { + if (H) + *H = Matrix23::Zero(); +} + +TEST( OptionalJacobian, Ref2) { + + Matrix expected; + expected = Matrix23::Zero(); + + // Default argument does nothing + test3(); + + // Fixed size, no copy + Matrix23 fixed1; + fixed1.setOnes(); + test3(fixed1); + EXPECT(assert_equal(expected,fixed1)); + + // Fixed size, no copy, pointer style + Matrix23 fixed2; + fixed2.setOnes(); + test3(&fixed2); + EXPECT(assert_equal(expected,fixed2)); + + // Empty is no longer a sign we don't want a matrix, we want it resized + Matrix dynamic0; + test3(dynamic0); + EXPECT(assert_equal(expected,dynamic0)); + + // Dynamic wrong size + Matrix dynamic1(3,5); + dynamic1.setOnes(); + test3(dynamic1); + EXPECT(assert_equal(expected,dynamic0)); + + // Dynamic right size + Matrix dynamic2(2,5); + dynamic2.setOnes(); + test3(dynamic2); + EXPECT(assert_equal(dynamic2,dynamic0)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 4214d7850..aa06a2e29 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -104,44 +104,6 @@ TEST(Cal3_S2, between) { } -/* ************************************************************************* */ -void test3(FixedRef<2,3> H = FixedRef<2,3>()) { - if (H) - *H = Matrix23::Zero(); -} - -TEST( Cal3DS2, Ref2) { - - Matrix expected; - expected = Matrix23::Zero(); - - // Default argument does nothing - test3(); - - // Fixed size, no copy - Matrix23 fixedDcal; - fixedDcal.setOnes(); - test3(fixedDcal); - EXPECT(assert_equal(expected,fixedDcal)); - - // Empty is no longer a sign we don't want a matrix, we want it resized - Matrix dynamic0; - test3(dynamic0); - EXPECT(assert_equal(expected,dynamic0)); - - // Dynamic wrong size - Matrix dynamic1(3,5); - dynamic1.setOnes(); - test3(dynamic1); - EXPECT(assert_equal(expected,dynamic0)); - - // Dynamic right size - Matrix dynamic2(2,5); - dynamic2.setOnes(); - test3(dynamic2); - EXPECT(assert_equal(dynamic2,dynamic0)); -} - /* ************************************************************************* */ int main() { TestResult tr; From a89b4d216841ba23337f649c5810ea621ad4f106 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 17:13:54 +0100 Subject: [PATCH 22/76] empty_ is gone --- gtsam/base/OptionalJacobian.h | 15 ++++--- gtsam/base/tests/testOptionalJacobian.cpp | 50 +++++++++++++++-------- 2 files changed, 40 insertions(+), 25 deletions(-) diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 9b5615423..91cd1089a 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -44,7 +44,6 @@ public: private: - bool empty_; ///< flag whether initialized or not Eigen::Map map_; /// View on constructor argument, if given // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html @@ -57,25 +56,25 @@ public: /// Default constructor acts like boost::none OptionalJacobian() : - empty_(true), map_(NULL) { + map_(NULL) { } /// Constructor that will usurp data of a fixed-size matrix OptionalJacobian(Fixed& fixed) : - empty_(false), map_(NULL) { + map_(NULL) { usurp(fixed.data()); } /// Constructor that will usurp data of a fixed-size matrix, pointer version OptionalJacobian(Fixed* fixedPtr) : - empty_(fixedPtr==NULL), map_(NULL) { + map_(NULL) { if (fixedPtr) usurp(fixedPtr->data()); } /// Constructor that will resize a dynamic matrix (unless already correct) OptionalJacobian(Eigen::MatrixXd& dynamic) : - empty_(false), map_(NULL) { + map_(NULL) { dynamic.resize(Rows, Cols); // no malloc if correct size usurp(dynamic.data()); } @@ -84,12 +83,12 @@ public: /// Constructor with boost::none just makes empty OptionalJacobian(boost::none_t none) : - empty_(true), map_(NULL) { + map_(NULL) { } /// Constructor compatible with old-style derivatives OptionalJacobian(const boost::optional optional) : - empty_(!optional), map_(NULL) { + map_(NULL) { if (optional) { optional->resize(Rows, Cols); usurp(optional->data()); @@ -100,7 +99,7 @@ public: /// Return true is allocated, false if default constructor was used operator bool() const { - return !empty_; + return map_.data(); } /// De-reference, like boost optional diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp index a03757e60..6584c82d1 100644 --- a/gtsam/base/tests/testOptionalJacobian.cpp +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -24,21 +24,37 @@ using namespace std; using namespace gtsam; //****************************************************************************** -TEST( OptionalJacobian, Constructors ) -{ +TEST( OptionalJacobian, Constructors ) { Matrix23 fixed; + + OptionalJacobian<2, 3> H1; + EXPECT(!H1); + + OptionalJacobian<2, 3> H2(fixed); + EXPECT(H2); + + OptionalJacobian<2, 3> H3(&fixed); + EXPECT(H3); + Matrix dynamic; - OptionalJacobian<2,3> H1; - OptionalJacobian<2,3> H2(fixed); - OptionalJacobian<2,3> H3(&fixed); - OptionalJacobian<2,3> H4(dynamic); - OptionalJacobian<2,3> H5(boost::none); + OptionalJacobian<2, 3> H4(dynamic); + EXPECT(H4); + + OptionalJacobian<2, 3> H5(boost::none); + EXPECT(!H5); + boost::optional optional(dynamic); - OptionalJacobian<2,3> H6(optional); + OptionalJacobian<2, 3> H6(optional); + EXPECT(H6); } //****************************************************************************** -void test3(OptionalJacobian<2,3> H = OptionalJacobian<2,3>()) { +void test(OptionalJacobian<2, 3> H = boost::none) { + if (H) + *H = Matrix23::Zero(); +} + +void testPtr(Matrix23* H = NULL) { if (H) *H = Matrix23::Zero(); } @@ -49,35 +65,35 @@ TEST( OptionalJacobian, Ref2) { expected = Matrix23::Zero(); // Default argument does nothing - test3(); + test(); // Fixed size, no copy Matrix23 fixed1; fixed1.setOnes(); - test3(fixed1); + test(fixed1); EXPECT(assert_equal(expected,fixed1)); // Fixed size, no copy, pointer style Matrix23 fixed2; fixed2.setOnes(); - test3(&fixed2); + test(&fixed2); EXPECT(assert_equal(expected,fixed2)); // Empty is no longer a sign we don't want a matrix, we want it resized Matrix dynamic0; - test3(dynamic0); + test(dynamic0); EXPECT(assert_equal(expected,dynamic0)); // Dynamic wrong size - Matrix dynamic1(3,5); + Matrix dynamic1(3, 5); dynamic1.setOnes(); - test3(dynamic1); + test(dynamic1); EXPECT(assert_equal(expected,dynamic0)); // Dynamic right size - Matrix dynamic2(2,5); + Matrix dynamic2(2, 5); dynamic2.setOnes(); - test3(dynamic2); + test(dynamic2); EXPECT(assert_equal(dynamic2,dynamic0)); } From 6505e602d87c65b495b54a1111297ea7fe10fb27 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 28 Nov 2014 17:14:26 +0100 Subject: [PATCH 23/76] FixedRef is now OptionalJacobian --- gtsam/geometry/Cal3Bundler.cpp | 2 +- gtsam/geometry/Cal3Bundler.h | 4 +- gtsam/geometry/Cal3DS2_Base.cpp | 2 +- gtsam/geometry/Cal3DS2_Base.h | 4 +- gtsam/geometry/Cal3_S2.cpp | 4 +- gtsam/geometry/Cal3_S2.h | 4 +- gtsam/geometry/PinholeCamera.h | 18 +++--- gtsam/geometry/Point2.cpp | 16 ++--- gtsam/geometry/Point2.h | 27 ++++---- gtsam/geometry/Point3.cpp | 2 +- gtsam/geometry/Point3.h | 4 +- gtsam/geometry/Pose2.cpp | 4 +- gtsam/geometry/Pose2.h | 8 +-- gtsam/geometry/Pose3.cpp | 4 +- gtsam/geometry/Pose3.h | 4 +- gtsam/geometry/Rot3.h | 4 +- gtsam/geometry/Rot3M.cpp | 2 +- gtsam/nonlinear/Expression-inl.h | 62 ++++++++++--------- gtsam/nonlinear/Expression.h | 12 ++-- gtsam/nonlinear/NonlinearFactor.h | 6 +- gtsam/nonlinear/tests/testExpression.cpp | 14 ++--- gtsam_unstable/nonlinear/AdaptAutoDiff.h | 5 +- .../tests/testBasisDecompositions.cpp | 2 +- .../nonlinear/tests/testExpressionFactor.cpp | 4 +- .../nonlinear/tests/testExpressionMeta.cpp | 6 +- gtsam_unstable/slam/expressions.h | 10 +-- 26 files changed, 116 insertions(+), 118 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index df4f385a8..fd3392b67 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -66,7 +66,7 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { /* ************************************************************************* */ Point2 Cal3Bundler::uncalibrate(const Point2& p, // - FixedRef<2, 3> Dcal, FixedRef<2, 2> Dp) const { + OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const { // r = x^2 + y^2; // g = (1 + k(1)*r + k(2)*r^2); // pi(:,i) = g * pn(:,i) diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 837888855..ed4f8b391 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -113,8 +113,8 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, FixedRef<2, 3> Dcal = boost::none, - FixedRef<2, 2> Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index 3846178fa..1830d56a1 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -91,7 +91,7 @@ static Matrix2 D2dintrinsic(double x, double y, double rr, /* ************************************************************************* */ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, - FixedRef<2,9> H1, FixedRef<2,2> H2) const { + OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const { // rr = x^2 + y^2; // g = (1 + k(1)*rr + k(2)*rr^2); diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index cf80a7741..1b2143278 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -114,8 +114,8 @@ public: * @return point in (distorted) image coordinates */ Point2 uncalibrate(const Point2& p, - FixedRef<2,9> Dcal = boost::none, - FixedRef<2,2> Dp = boost::none) const ; + OptionalJacobian<2,9> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const ; /// Convert (distorted) image coordinates uv to intrinsic coordinates xy Point2 calibrate(const Point2& p, const double tol=1e-5) const; diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index dbb6c09eb..d0589cc65 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -72,8 +72,8 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { } /* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p, FixedRef<2, 5> Dcal, - FixedRef<2, 2> Dp) const { +Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal, + OptionalJacobian<2, 2> Dp) const { const double x = p.x(), y = p.y(); if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index a380ff4db..e28b24eae 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -148,8 +148,8 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, FixedRef<2,5> Dcal = boost::none, - FixedRef<2,2> Dp = boost::none) const; + 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 diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 96d227912..a70bb2301 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -274,7 +274,7 @@ public: * @param Dpoint is the 2*3 Jacobian w.r.t. P */ static Point2 project_to_camera(const Point3& P, // - FixedRef<2, 3> Dpoint = boost::none) { + OptionalJacobian<2, 3> Dpoint = boost::none) { #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (P.z() <= 0) throw CheiralityException(); @@ -303,9 +303,9 @@ public: */ inline Point2 project( const Point3& pw, - FixedRef<2,6> Dpose = boost::none, - FixedRef<2,3> Dpoint = boost::none, - FixedRef<2,DimK> Dcal = boost::none) const { + OptionalJacobian<2,6> Dpose = boost::none, + OptionalJacobian<2,3> Dpoint = boost::none, + OptionalJacobian<2,DimK> Dcal = boost::none) const { // Transform to camera coordinates and check cheirality const Point3 pc = pose_.transform_to(pw); @@ -338,9 +338,9 @@ public: */ inline Point2 projectPointAtInfinity( const Point3& pw, - FixedRef<2,6> Dpose = boost::none, - FixedRef<2,2> Dpoint = boost::none, - FixedRef<2,DimK> Dcal = boost::none) const { + OptionalJacobian<2,6> Dpose = boost::none, + OptionalJacobian<2,2> Dpoint = boost::none, + OptionalJacobian<2,DimK> Dcal = boost::none) const { if (!Dpose && !Dpoint && !Dcal) { const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter) @@ -378,8 +378,8 @@ public: * @param Dpoint is the Jacobian w.r.t. point3 */ Point2 project2(const Point3& pw, // - FixedRef<2, 6 + DimK> Dcamera = boost::none, - FixedRef<2, 3> Dpoint = boost::none) const { + OptionalJacobian<2, 6 + DimK> Dcamera = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const { const Point3 pc = pose_.transform_to(pw); const Point2 pn = project_to_camera(pc); diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 8b90aed63..6588f051f 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -38,15 +38,9 @@ bool Point2::equals(const Point2& q, double tol) const { } /* ************************************************************************* */ -double Point2::norm() const { - return sqrt(x_ * x_ + y_ * y_); -} - -/* ************************************************************************* */ -double Point2::norm(boost::optional H) const { - double r = norm(); +double Point2::norm(OptionalJacobian<1,2> H) const { + double r = sqrt(x_ * x_ + y_ * y_); if (H) { - H->resize(1,2); if (fabs(r) > 1e-10) *H << x_ / r, y_ / r; else @@ -57,11 +51,11 @@ double Point2::norm(boost::optional H) const { /* ************************************************************************* */ static const Matrix I2 = eye(2); -double Point2::distance(const Point2& point, boost::optional H1, - boost::optional H2) const { +double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1, + OptionalJacobian<1,2> H2) const { Point2 d = point - *this; if (H1 || H2) { - Matrix H; + Eigen::Matrix H; double r = d.norm(H); if (H1) *H1 = -H; if (H2) *H2 = H; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index a4e0cc296..a0b185b63 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include namespace gtsam { @@ -125,10 +125,10 @@ public: /// "Compose", just adds the coordinates of two points. With optional derivatives inline Point2 compose(const Point2& q, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(2); - if(H2) *H2 = eye(2); + OptionalJacobian<2,2> H1=boost::none, + OptionalJacobian<2,2> H2=boost::none) const { + if(H1) *H1 = Eigen::Matrix2d::Identity(); + if(H2) *H2 = Eigen::Matrix2d::Identity(); return *this + q; } @@ -137,10 +137,10 @@ public: /// "Between", subtracts point coordinates. between(p,q) == compose(inverse(p),q) inline Point2 between(const Point2& q, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(2); - if(H2) *H2 = eye(2); + OptionalJacobian<2,2> H1=boost::none, + OptionalJacobian<2,2> H2=boost::none) const { + if(H1) *H1 = -Eigen::Matrix2d::Identity(); + if(H2) *H2 = Eigen::Matrix2d::Identity(); return q - (*this); } @@ -180,15 +180,12 @@ public: /** creates a unit vector */ Point2 unit() const { return *this/norm(); } - /** norm of point */ - double norm() const; - /** norm of point, with derivative */ - double norm(boost::optional H) const; + double norm(OptionalJacobian<1,2> H = boost::none) const; /** distance between two points */ - double distance(const Point2& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none, + OptionalJacobian<1,2> H2 = boost::none) const; /** @deprecated The following function has been deprecated, use distance above */ inline double dist(const Point2& p2) const { diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 43f2239e2..03ed31ab5 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -94,7 +94,7 @@ double Point3::dot(const Point3 &q) const { } /* ************************************************************************* */ -double Point3::norm(FixedRef<1,3> H) const { +double Point3::norm(OptionalJacobian<1,3> H) const { double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_); if (H) { if (fabs(r) > 1e-10) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 9ef6696b7..96cf4e0c8 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -21,9 +21,9 @@ #pragma once -#include #include #include +#include #include @@ -181,7 +181,7 @@ namespace gtsam { } /** Distance of the point from the origin, with Jacobian */ - double norm(FixedRef<1,3> H = boost::none) const; + double norm(OptionalJacobian<1,3> H = boost::none) const; /** normalize, with optional Jacobian */ Point3 normalize(boost::optional H = boost::none) const; diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 41929c242..f0daa4054 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -126,7 +126,7 @@ Pose2 Pose2::inverse(boost::optional H1) const { /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_to(const Point2& point, - FixedRef<2, 3> H1, FixedRef<2, 2> H2) const { + OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { Point2 d = point - t_; Point2 q = r_.unrotate(d); if (!H1 && !H2) return q; @@ -150,7 +150,7 @@ Pose2 Pose2::compose(const Pose2& p2, boost::optional H1, /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_from(const Point2& p, - FixedRef<2, 3> H1, FixedRef<2, 2> H2) const { + OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { const Point2 q = r_ * p; if (H1 || H2) { const Matrix R = r_.matrix(); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index cac79e6fb..f39ca5a81 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -192,13 +192,13 @@ public: /** Return point coordinates in pose coordinate frame */ Point2 transform_to(const Point2& point, - FixedRef<2, 3> H1 = boost::none, - FixedRef<2, 2> H2 = boost::none) const; + OptionalJacobian<2, 3> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /** Return point coordinates in global frame */ Point2 transform_from(const Point2& point, - FixedRef<2, 3> H1 = boost::none, - FixedRef<2, 2> H2 = boost::none) const; + OptionalJacobian<2, 3> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /** syntactic sugar for transform_from */ inline Point2 operator*(const Point2& point) const { return transform_from(point);} diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 42d207bb1..93a27ed58 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -254,8 +254,8 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, } /* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p, FixedRef<3,6> Dpose, - FixedRef<3,3> Dpoint) const { +Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, + OptionalJacobian<3,3> Dpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index e5460fd82..c31cc48cc 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -250,8 +250,8 @@ public: * @return point in Pose coordinates */ Point3 transform_to(const Point3& p, - FixedRef<3,6> Dpose = boost::none, - FixedRef<3,3> Dpoint = boost::none) const; + OptionalJacobian<3,6> Dpose = boost::none, + OptionalJacobian<3,3> Dpoint = boost::none) const; /// @} /// @name Standard Interface diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index ffa2f81c5..4364678a5 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -218,8 +218,8 @@ namespace gtsam { Rot3 inverse(boost::optional H1=boost::none) const; /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2, FixedRef<3, 3> H1 = boost::none, - FixedRef<3, 3> H2 = boost::none) const; + Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; /** compose two rotations */ Rot3 operator*(const Rot3& R2) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 623a13664..39648ca1e 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -142,7 +142,7 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) { } /* ************************************************************************* */ -Rot3 Rot3::compose(const Rot3& R2, FixedRef<3, 3> H1, FixedRef<3, 3> H2) const { +Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { if (H1) *H1 = R2.transpose(); if (H2) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index a87d9896e..ebc1d8f15 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -42,21 +43,21 @@ namespace gtsam { const unsigned TraceAlignment = 16; -template -T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment){ +template +T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) { // right now only word sized types are supported. // Easy to extend if needed, // by somehow inferring the unsigned integer of same size BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t)); size_t & uiValue = reinterpret_cast(value); size_t misAlignment = uiValue % requiredAlignment; - if(misAlignment) { + if (misAlignment) { uiValue += requiredAlignment - misAlignment; } return value; } -template -T upAligned(T value, unsigned requiredAlignment = TraceAlignment){ +template +T upAligned(T value, unsigned requiredAlignment = TraceAlignment) { return upAlign(value, requiredAlignment); } @@ -88,19 +89,21 @@ public: namespace internal { -template +template struct UseBlockIf { static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key){ + JacobianMap& jacobians, Key key) { // block makes HUGE difference - jacobians(key).block(0, 0) += dTdA; - }; + jacobians(key).block( + 0, 0) += dTdA; + } + ; }; /// Handle Leaf Case for Dynamic Matrix type (slower) -template +template struct UseBlockIf { static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { + JacobianMap& jacobians, Key key) { jacobians(key) += dTdA; } }; @@ -111,10 +114,9 @@ template void handleLeafCase(const Eigen::MatrixBase& dTdA, JacobianMap& jacobians, Key key) { internal::UseBlockIf< - Derived::RowsAtCompileTime != Eigen::Dynamic && - Derived::ColsAtCompileTime != Eigen::Dynamic, - Derived> - ::addToJacobian(dTdA, jacobians, key); + Derived::RowsAtCompileTime != Eigen::Dynamic + && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian( + dTdA, jacobians, key); } //----------------------------------------------------------------------------- @@ -265,7 +267,7 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const = 0; + ExecutionTraceStorage* traceStorage) const = 0; }; //----------------------------------------------------------------------------- @@ -336,7 +338,7 @@ public: /// Construct an execution trace for reverse AD virtual const value_type& traceExecution(const Values& values, - ExecutionTrace& trace, + ExecutionTrace& trace, ExecutionTraceStorage* traceStorage) const { trace.setLeaf(key_); return dynamic_cast(values.at(key_)); @@ -454,8 +456,9 @@ struct Jacobian { /// meta-function to generate JacobianTA optional reference template -struct OptionalJacobian { - typedef FixedRef::value, traits::dimension::value> type; +struct MakeOptionalJacobian { + typedef OptionalJacobian::value, + traits::dimension::value> type; }; /** @@ -556,7 +559,7 @@ struct GenerateFunctionalNode: Argument, Base { }; /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, + void trace(const Values& values, Record* record, ExecutionTraceStorage*& traceStorage) const { Base::trace(values, record, traceStorage); // recurse // Write an Expression execution trace in record->trace @@ -632,7 +635,7 @@ struct FunctionalNode { }; /// Construct an execution trace for reverse AD - Record* trace(const Values& values, + Record* trace(const Values& values, ExecutionTraceStorage* traceStorage) const { assert(reinterpret_cast(traceStorage) % TraceAlignment == 0); @@ -657,7 +660,8 @@ class UnaryExpression: public FunctionalNode >::type { public: - typedef boost::function::type)> Function; + typedef boost::function< + T(const A1&, typename MakeOptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -702,8 +706,8 @@ class BinaryExpression: public FunctionalNode >::t public: typedef boost::function< - T(const A1&, const A2&, typename OptionalJacobian::type, - typename OptionalJacobian::type)> Function; + T(const A1&, const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -756,9 +760,10 @@ class TernaryExpression: public FunctionalNode public: typedef boost::function< - T(const A1&, const A2&, const A3&, typename OptionalJacobian::type, - typename OptionalJacobian::type, - typename OptionalJacobian::type)> Function; + T(const A1&, const A2&, const A3&, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -774,7 +779,8 @@ private: this->template reset(e2.root()); this->template reset(e3.root()); ExpressionNode::traceSize_ = // - upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() + e3.traceSize(); + upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() + + e3.traceSize(); } friend class Expression ; diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 940ad1778..71bd16b1c 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -20,8 +20,8 @@ #pragma once #include -#include #include +#include #include #include @@ -75,7 +75,7 @@ public: /// Construct a nullary method expression template Expression(const Expression& expression, - T (A::*method)(typename OptionalJacobian::type) const) : + T (A::*method)(typename MakeOptionalJacobian::type) const) : root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { } @@ -89,8 +89,8 @@ public: /// Construct a unary method expression template Expression(const Expression& expression1, - T (A1::*method)(const A2&, typename OptionalJacobian::type, - typename OptionalJacobian::type) const, + T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, const Expression& expression2) : root_( new BinaryExpression(boost::bind(method, _1, _2, _3, _4), @@ -224,8 +224,8 @@ template struct apply_compose { typedef T result_type; static const int Dim = traits::dimension::value; - T operator()(const T& x, const T& y, FixedRef H1 = boost::none, - FixedRef H2 = boost::none) const { + T operator()(const T& x, const T& y, OptionalJacobian H1 = + boost::none, OptionalJacobian H2 = boost::none) const { return x.compose(y, H1, H2); } }; diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 9fd4f8383..d7713d0d5 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -20,14 +20,14 @@ #pragma once -#include -#include - #include #include #include #include +#include +#include +#include /** * Macro to add a standard clone function to a derived factor diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index b33d0e5cd..1ea6236e8 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -34,8 +34,8 @@ using namespace gtsam; /* ************************************************************************* */ template -Point2 uncalibrate(const CAL& K, const Point2& p, FixedRef<2, 5> Dcal, - FixedRef<2, 2> Dp) { +Point2 uncalibrate(const CAL& K, const Point2& p, OptionalJacobian<2, 5> Dcal, + OptionalJacobian<2, 2> Dp) { return K.uncalibrate(p, Dcal, Dp); } @@ -75,13 +75,13 @@ TEST(Expression, Leaves) { /* ************************************************************************* */ // Unary(Leaf) namespace unary { -Point2 f0(const Point3& p, FixedRef<2,3> H) { +Point2 f0(const Point3& p, OptionalJacobian<2,3> H) { return Point2(); } -LieScalar f1(const Point3& p, FixedRef<1, 3> H) { +LieScalar f1(const Point3& p, OptionalJacobian<1, 3> H) { return LieScalar(0.0); } -double f2(const Point3& p, FixedRef<1, 3> H) { +double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; } Expression p(1); @@ -134,7 +134,7 @@ TEST(Expression, NullaryMethod) { namespace binary { // Create leaves double doubleF(const Pose3& pose, // - const Point3& point, FixedRef<1, 6> H1, FixedRef<1, 3> H2) { + const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) { return 0.0; } Expression x(1); @@ -243,7 +243,7 @@ TEST(Expression, compose3) { /* ************************************************************************* */ // Test with ternary function Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - FixedRef<3, 3> H1, FixedRef<3, 3> H2, FixedRef<3, 3> H3) { + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); diff --git a/gtsam_unstable/nonlinear/AdaptAutoDiff.h b/gtsam_unstable/nonlinear/AdaptAutoDiff.h index 70ba285c1..5a4b5a09a 100644 --- a/gtsam_unstable/nonlinear/AdaptAutoDiff.h +++ b/gtsam_unstable/nonlinear/AdaptAutoDiff.h @@ -20,6 +20,7 @@ #include #include +#include namespace gtsam { @@ -50,8 +51,8 @@ class AdaptAutoDiff { public: - T operator()(const A1& a1, const A2& a2, FixedRef H1 = boost::none, - FixedRef H2 = boost::none) { + T operator()(const A1& a1, const A2& a2, OptionalJacobian H1 = boost::none, + OptionalJacobian H2 = boost::none) { using ceres::internal::AutoDiff; diff --git a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp index 6afeac15b..eda90e203 100644 --- a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp +++ b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp @@ -60,7 +60,7 @@ public: } /// Given coefficients c, predict value for x - double operator()(const Coefficients& c, FixedRef<1, N> H) { + double operator()(const Coefficients& c, OptionalJacobian<1, N> H) { if (H) (*H) = H_; return H_ * c; diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index d37f1f80a..a412b47eb 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -126,7 +126,7 @@ TEST(ExpressionFactor, Unary) { /* ************************************************************************* */ static Point2 myUncal(const Cal3_S2& K, const Point2& p, - FixedRef<2,5> Dcal, FixedRef<2,2> Dp) { + OptionalJacobian<2,5> Dcal, OptionalJacobian<2,2> Dp) { return K.uncalibrate(p, Dcal, Dp); } @@ -392,7 +392,7 @@ TEST(ExpressionFactor, compose3) { /* ************************************************************************* */ // Test compose with three arguments Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - FixedRef<3, 3> H1, FixedRef<3, 3> H2, FixedRef<3, 3> H3) { + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp index 178b1803c..45e294c3d 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp @@ -201,7 +201,7 @@ TEST(ExpressionFactor, InvokeDerivatives) { // Now, let's create the optional Jacobian arguments typedef Point3 T; typedef boost::mpl::vector TYPES; - typedef boost::mpl::transform >::type Optionals; + typedef boost::mpl::transform >::type Optionals; // Unfortunately this is moot: we need a pointer to a function with the // optional derivatives; I don't see a way of calling a function that we @@ -215,8 +215,8 @@ struct proxy { return pose.transform_to(point); } Point3 operator()(const Pose3& pose, const Point3& point, - FixedRef<3,6> Dpose, - FixedRef<3,3> Dpoint) const { + OptionalJacobian<3,6> Dpose, + OptionalJacobian<3,3> Dpoint) const { return pose.transform_to(point, Dpose, Dpoint); } }; diff --git a/gtsam_unstable/slam/expressions.h b/gtsam_unstable/slam/expressions.h index c72a9d3f7..031baea3d 100644 --- a/gtsam_unstable/slam/expressions.h +++ b/gtsam_unstable/slam/expressions.h @@ -20,8 +20,8 @@ typedef Expression Rot2_; typedef Expression Pose2_; Point2_ transform_to(const Pose2_& x, const Point2_& p) { - Point2 (Pose2::*transform)(const Point2& p, FixedRef<2, 3> H1, - FixedRef<2, 2> H2) const = &Pose2::transform_to; + Point2 (Pose2::*transform)(const Point2& p, OptionalJacobian<2, 3> H1, + OptionalJacobian<2, 2> H2) const = &Pose2::transform_to; return Point2_(x, transform, p); } @@ -34,8 +34,8 @@ typedef Expression Pose3_; Point3_ transform_to(const Pose3_& x, const Point3_& p) { - Point3 (Pose3::*transform)(const Point3& p, FixedRef<3, 6> Dpose, - FixedRef<3, 3> Dpoint) const = &Pose3::transform_to; + Point3 (Pose3::*transform)(const Point3& p, OptionalJacobian<3, 6> Dpose, + OptionalJacobian<3, 3> Dpoint) const = &Pose3::transform_to; return Point3_(x, transform, p); } @@ -49,7 +49,7 @@ Point2_ project(const Point3_& p_cam) { } Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, - FixedRef<2, 6> Dpose, FixedRef<2, 3> Dpoint, FixedRef<2, 5> Dcal) { + OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) { return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); } From 4d495a0267d217e1e6b66279526b06c66c3f73ba Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sat, 29 Nov 2014 15:39:43 -0500 Subject: [PATCH 24/76] changed the between factor in Pose2 using Optional Jacobian. This fixes build errors in unstable also. --- gtsam/geometry/Pose2.cpp | 60 ++-------------------------------------- gtsam/geometry/Pose2.h | 15 ++-------- 2 files changed, 4 insertions(+), 71 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index f0daa4054..c63eb844c 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -162,30 +162,8 @@ Point2 Pose2::transform_from(const Point2& p, } /* ************************************************************************* */ -Pose2 Pose2::between(const Pose2& p2) const { - // get cosines and sines from rotation matrices - const Rot2& R1 = r_, R2 = p2.r(); - double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); - - // Assert that R1 and R2 are normalized - assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); - - // Calculate delta rotation = between(R1,R2) - double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; - Rot2 R(Rot2::atan2(s,c)); // normalizes - - // Calculate delta translation = unrotate(R1, dt); - Point2 dt = p2.t() - t_; - double x = dt.x(), y = dt.y(); - // t = R1' * (t2-t1) - Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); - - return Pose2(R,t); -} - -/* ************************************************************************* */ -Pose2 Pose2::between(const Pose2& p2, boost::optional H1, - boost::optional H2) const { +Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { // get cosines and sines from rotation matrices const Rot2& R1 = r_, R2 = p2.r(); double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); @@ -217,40 +195,6 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional H1, return Pose2(R,t); } -/* ************************************************************************* */ -Pose2 Pose2::between(const Pose2& p2, boost::optional H1, - boost::optional H2) const { - // get cosines and sines from rotation matrices - const Rot2& R1 = r_, R2 = p2.r(); - double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); - - // Assert that R1 and R2 are normalized - assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); - - // Calculate delta rotation = between(R1,R2) - double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; - Rot2 R(Rot2::atan2(s,c)); // normalizes - - // Calculate delta translation = unrotate(R1, dt); - Point2 dt = p2.t() - t_; - double x = dt.x(), y = dt.y(); - // t = R1' * (t2-t1) - Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); - - // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above - if (H1) { - double dt1 = -s2 * x + c2 * y; - double dt2 = -c2 * x - s2 * y; - *H1 = (Matrix(3, 3) << - -c, -s, dt1, - s, -c, dt2, - 0.0, 0.0,-1.0).finished(); - } - if (H2) *H2 = I3; - - return Pose2(R,t); -} - /* ************************************************************************* */ Rot2 Pose2::bearing(const Point2& point, boost::optional H1, boost::optional H2) const { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index f39ca5a81..6caeb196a 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -122,19 +122,8 @@ public: /** * Return relative pose between p1 and p2, in p1 coordinate frame */ - Pose2 between(const Pose2& p2) const; - - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p2, boost::optional H1, - boost::optional H2) const; - - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p2, boost::optional H1, - boost::optional H2) const; + Pose2 between(const Pose2& p2, OptionalJacobian<3,3> H1 = boost::none, + OptionalJacobian<3,3> H = boost::none) const; /// @} /// @name Manifold From 708a3d69e8553feb19397f7c2bcc92a0905a938a Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sat, 29 Nov 2014 15:41:19 -0500 Subject: [PATCH 25/76] ignore file for QtCreator IDE --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 60633adaf..9276d2f30 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ /examples/Data/dubrovnik-3-7-pre-rewritten.txt /examples/Data/pose2example-rewritten.txt /examples/Data/pose3example-rewritten.txt +*.txt.user From b4ee5e11052e2cb8358e4e9196796230dc333871 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sat, 29 Nov 2014 15:42:21 -0500 Subject: [PATCH 26/76] between factor in Cal3_S2 --- gtsam/geometry/Cal3_S2.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index e28b24eae..67395764d 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -167,8 +167,8 @@ public: /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q) inline Cal3_S2 between(const Cal3_S2& q, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { + OptionalJacobian<5,5> H1=boost::none, + OptionalJacobian<5,5> H2=boost::none) const { if(H1) *H1 = -eye(5); if(H2) *H2 = eye(5); return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_); From 1ccb395a6c9301f8eeff929b54e5fb380774f411 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sat, 29 Nov 2014 16:27:49 -0500 Subject: [PATCH 27/76] changed return value of adjoint map to Matrix3 and also updated the inverse factor os Pose2 --- gtsam/geometry/Pose2.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 6caeb196a..922757fe8 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -107,7 +107,7 @@ public: inline static Pose2 identity() { return Pose2(); } /// inverse transformation with derivatives - Pose2 inverse(boost::optional H1=boost::none) const; + Pose2 inverse(OptionalJacobian<3, 3> H1=boost::none) const; /// compose this transformation onto another (first *this and then p2) Pose2 compose(const Pose2& p2, @@ -155,7 +155,7 @@ public: * Calculate Adjoint map * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ - Matrix AdjointMap() const; + Matrix3 AdjointMap() const; inline Vector Adjoint(const Vector& xi) const { assert(xi.size() == 3); return AdjointMap()*xi; From 296de694119588d4b3a031f25cf53df730d3751c Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sat, 29 Nov 2014 16:35:27 -0500 Subject: [PATCH 28/76] Not sure if you really need this becasue bearing and transform_to have already got OptionalJacobian. But I guess it won't hurt. --- gtsam/geometry/Pose2.cpp | 10 +++++----- gtsam/geometry/Pose2.h | 8 ++++---- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index c63eb844c..fc09ed0cf 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -108,9 +108,9 @@ Vector Pose2::localCoordinates(const Pose2& p2) const { /* ************************************************************************* */ // Calculate Adjoint map // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) -Matrix Pose2::AdjointMap() const { +Matrix3 Pose2::AdjointMap() const { double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y(); - return (Matrix(3, 3) << + return (Matrix(3,3) << c, -s, y, s, c, -x, 0.0, 0.0, 1.0 @@ -118,7 +118,7 @@ Matrix Pose2::AdjointMap() const { } /* ************************************************************************* */ -Pose2 Pose2::inverse(boost::optional H1) const { +Pose2 Pose2::inverse(OptionalJacobian<3,3> H1) const { if (H1) *H1 = -AdjointMap(); return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } @@ -197,7 +197,7 @@ Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1, /* ************************************************************************* */ Rot2 Pose2::bearing(const Point2& point, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { Point2 d = transform_to(point, H1, H2); if (!H1 && !H2) return Rot2::relativeBearing(d); Matrix D_result_d; @@ -209,7 +209,7 @@ Rot2 Pose2::bearing(const Point2& point, /* ************************************************************************* */ Rot2 Pose2::bearing(const Pose2& point, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { Rot2 result = bearing(point.t(), H1, H2); if (H2) { Matrix H2_ = *H2 * point.r().matrix(); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 922757fe8..43ee4de97 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -226,8 +226,8 @@ public: * @return 2D rotation \f$ \in SO(2) \f$ */ Rot2 bearing(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<2, 3> H1=boost::none, + OptionalJacobian<2, 2> H2=boost::none) const; /** * Calculate bearing to another pose @@ -235,8 +235,8 @@ public: * @return 2D rotation \f$ \in SO(2) \f$ */ Rot2 bearing(const Pose2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<2, 3> H1=boost::none, + OptionalJacobian<2, 2> H2=boost::none) const; /** * Calculate range to a landmark From 72164170179d3d8fd11d45b193b76a93bd87d74e Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sat, 29 Nov 2014 16:54:21 -0500 Subject: [PATCH 29/76] changed eye() to Identity() --- gtsam/geometry/Cal3_S2.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 67395764d..261383757 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -169,8 +169,8 @@ public: inline Cal3_S2 between(const Cal3_S2& q, OptionalJacobian<5,5> H1=boost::none, OptionalJacobian<5,5> H2=boost::none) const { - if(H1) *H1 = -eye(5); - if(H2) *H2 = eye(5); + if(H1) *H1 = -Matrix5::Identity(); + if(H2) *H2 = Matrix5::Identity(); return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_); } From 85032364f124f8b3aa3817b724a86c9d8b08e176 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sat, 29 Nov 2014 19:36:43 -0500 Subject: [PATCH 30/76] as @cbeall3 pointed out, Matrix(3,3) is still a dynamic Matrix. so changed this to Matrix3 --- gtsam/geometry/Pose2.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index fc09ed0cf..7f8265938 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -110,11 +110,12 @@ Vector Pose2::localCoordinates(const Pose2& p2) const { // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) Matrix3 Pose2::AdjointMap() const { double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y(); - return (Matrix(3,3) << + Matrix3 rvalue; + rvalue << c, -s, y, s, c, -x, - 0.0, 0.0, 1.0 - ).finished(); + 0.0, 0.0, 1.0; + return rvalue; } /* ************************************************************************* */ From 0ac6c8b80bdc539857a286a9952617b1ce0bd24a Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sun, 30 Nov 2014 08:32:35 -0500 Subject: [PATCH 31/76] Fixed the return value of Rot3 --- gtsam/geometry/Rot2.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 0133c9440..6ae559a62 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -64,8 +64,10 @@ Rot2& Rot2::normalize() { } /* ************************************************************************* */ -Matrix Rot2::matrix() const { - return (Matrix(2, 2) << c_, -s_, s_, c_).finished(); +Matrix2 Rot2::matrix() const { + Matrix2 rvalue; + rvalue << c_, -s_, s_, c_; + return rvalue; } /* ************************************************************************* */ From bbe657d19d1f9eef561f1174bad90051cf060c78 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sun, 30 Nov 2014 08:32:52 -0500 Subject: [PATCH 32/76] Fixed REturn Value of Matrix() in Rot3 --- gtsam/geometry/Rot2.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index af9148fd3..91172a901 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -225,7 +225,7 @@ namespace gtsam { } /** return 2*2 rotation matrix */ - Matrix matrix() const; + Matrix2 matrix() const; /** return 2*2 transpose (inverse) rotation matrix */ Matrix transpose() const; From 2c4adcc6af83b8827273404e51c49b7a63d976bf Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sun, 30 Nov 2014 08:33:35 -0500 Subject: [PATCH 33/76] replaces insertsub in Pose2. make check works. Are all functions in Matrix.h required ? --- gtsam/geometry/Pose2.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 7f8265938..b95a81af0 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -213,9 +213,9 @@ Rot2 Pose2::bearing(const Pose2& point, OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { Rot2 result = bearing(point.t(), H1, H2); if (H2) { - Matrix H2_ = *H2 * point.r().matrix(); + Matrix2 H2_ = *H2 * point.r().matrix(); *H2 = zeros(1, 3); - insertSub(*H2, H2_, 0, 0); + (*H2).block(0, 0, H2_.rows(), H2_.cols()) = H2_; } return result; } From 61d9948c3d0edc34e626be29d126fa81917640d6 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sun, 30 Nov 2014 11:54:27 -0500 Subject: [PATCH 34/76] added more typedefs for matrices --- gtsam/base/Matrix.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 132bf79ad..fcdfbb757 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -37,6 +37,12 @@ namespace gtsam { typedef Eigen::MatrixXd Matrix; typedef Eigen::Matrix MatrixRowMajor; +typedef Eigen::Matrix Matrix12; +typedef Eigen::Matrix Matrix13; +typedef Eigen::Matrix Matrix14; +typedef Eigen::Matrix Matrix15; +typedef Eigen::Matrix Matrix16; + typedef Eigen::Matrix2d Matrix2; typedef Eigen::Matrix3d Matrix3; typedef Eigen::Matrix4d Matrix4; From 9137123f5f005db0674d4a11888331258bd726c9 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sun, 30 Nov 2014 11:59:32 -0500 Subject: [PATCH 35/76] added OptionalJacobian to relativeBearing --- gtsam/geometry/Rot2.cpp | 8 +++++--- gtsam/geometry/Rot2.h | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 6ae559a62..d3c6bf5f1 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -96,13 +96,15 @@ Point2 Rot2::unrotate(const Point2& p, } /* ************************************************************************* */ -Rot2 Rot2::relativeBearing(const Point2& d, boost::optional H) { +Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) { double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); if(fabs(n) > 1e-5) { - if (H) *H = (Matrix(1, 2) << -y / d2, x / d2).finished(); + if (H) + *H << -y / d2, x / d2; return Rot2::fromCosSin(x / n, y / n); } else { - if (H) *H = (Matrix(1, 2) << 0.0, 0.0).finished(); + if (H) + (*H) << 0.0, 0.0; return Rot2(); } } diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 91172a901..2052bb603 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -87,7 +87,7 @@ namespace gtsam { * @param H optional reference for Jacobian * @return 2D rotation \f$ \in SO(2) \f$ */ - static Rot2 relativeBearing(const Point2& d, boost::optional H = + static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H = boost::none); /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */ From 8457ef4182595f10a6839b975687b4ed5215c67a Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sun, 30 Nov 2014 15:42:50 -0500 Subject: [PATCH 36/76] make check works in this commit --- gtsam/geometry/Pose2.cpp | 26 ++++++++++++++------------ gtsam/geometry/Pose2.h | 10 ++++------ gtsam/geometry/Rot2.cpp | 10 ++++++---- gtsam/geometry/tests/testPose2.cpp | 4 ++-- 4 files changed, 26 insertions(+), 24 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index b95a81af0..dfda7b3b1 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -198,7 +198,7 @@ Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1, /* ************************************************************************* */ Rot2 Pose2::bearing(const Point2& point, - OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { + boost::optional H1, boost::optional H2) const { Point2 d = transform_to(point, H1, H2); if (!H1 && !H2) return Rot2::relativeBearing(d); Matrix D_result_d; @@ -209,27 +209,29 @@ Rot2 Pose2::bearing(const Point2& point, } /* ************************************************************************* */ -Rot2 Pose2::bearing(const Pose2& point, - OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { - Rot2 result = bearing(point.t(), H1, H2); +Rot2 Pose2::bearing(const Pose2& pose, + boost::optional H1, boost::optional H2) const { + Rot2 result = bearing(pose.t(), H1, H2); if (H2) { - Matrix2 H2_ = *H2 * point.r().matrix(); + Matrix H2_ = *H2 * pose.r().matrix(); *H2 = zeros(1, 3); - (*H2).block(0, 0, H2_.rows(), H2_.cols()) = H2_; + insertSub(*H2, H2_, 0, 0); } return result; } - /* ************************************************************************* */ double Pose2::range(const Point2& point, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<1,3> H1, OptionalJacobian<1,2> H2) const { Point2 d = point - t_; if (!H1 && !H2) return d.norm(); - Matrix H; + Matrix12 H; double r = d.norm(H); - if (H1) *H1 = H * (Matrix(2, 3) << - -r_.c(), r_.s(), 0.0, - -r_.s(), -r_.c(), 0.0).finished(); + if (H1) { + Matrix23 mvalue; // is this the correct name ? + mvalue << -r_.c(), r_.s(), 0.0, + -r_.s(), -r_.c(), 0.0; + *H1 = H * mvalue; + } if (H2) *H2 = H; return r; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 43ee4de97..2a9f91508 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -226,8 +226,7 @@ public: * @return 2D rotation \f$ \in SO(2) \f$ */ Rot2 bearing(const Point2& point, - OptionalJacobian<2, 3> H1=boost::none, - OptionalJacobian<2, 2> H2=boost::none) const; + boost::optional H1=boost::none, boost::optional H2=boost::none) const; /** * Calculate bearing to another pose @@ -235,8 +234,7 @@ public: * @return 2D rotation \f$ \in SO(2) \f$ */ Rot2 bearing(const Pose2& point, - OptionalJacobian<2, 3> H1=boost::none, - OptionalJacobian<2, 2> H2=boost::none) const; + boost::optional H1=boost::none, boost::optional H2=boost::none) const; /** * Calculate range to a landmark @@ -244,8 +242,8 @@ public: * @return range (double) */ double range(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1, 3> H1=boost::none, + OptionalJacobian<1, 2> H2=boost::none) const; /** * Calculate range to another pose diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index d3c6bf5f1..6a90dfb3d 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -99,12 +99,14 @@ Point2 Rot2::unrotate(const Point2& p, Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) { double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); if(fabs(n) > 1e-5) { - if (H) - *H << -y / d2, x / d2; + if (H) { + (*H).block(0,0,1,2) << -y / d2, x / d2; + } return Rot2::fromCosSin(x / n, y / n); } else { - if (H) - (*H) << 0.0, 0.0; + if (H) { + (*H).block(0,0,1,2) << 0.0, 0.0; + } return Rot2(); } } diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index b4de6eb7c..01111aafe 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -519,7 +519,7 @@ TEST( Pose2, bearing ) expectedH1 = numericalDerivative21(bearing_proxy, x2, l3); EXPECT(assert_equal(expectedH1,actualH1)); expectedH2 = numericalDerivative22(bearing_proxy, x2, l3); - EXPECT(assert_equal(expectedH1,actualH1)); + EXPECT(assert_equal(expectedH2,actualH2)); // establish bearing is indeed 45 degrees even if rotated Rot2 actual34 = x3.bearing(l4, actualH1, actualH2); @@ -529,7 +529,7 @@ TEST( Pose2, bearing ) expectedH1 = numericalDerivative21(bearing_proxy, x3, l4); expectedH2 = numericalDerivative22(bearing_proxy, x3, l4); EXPECT(assert_equal(expectedH1,actualH1)); - EXPECT(assert_equal(expectedH1,actualH1)); + EXPECT(assert_equal(expectedH2,actualH2)); } /* ************************************************************************* */ From 010631a2eb373f474a2bcc1dc4d8d528d1be76dc Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sun, 30 Nov 2014 15:45:37 -0500 Subject: [PATCH 37/76] removed unecessary block oprations --- gtsam/geometry/Rot2.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 6a90dfb3d..41cf2dd05 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -100,12 +100,12 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) { double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); if(fabs(n) > 1e-5) { if (H) { - (*H).block(0,0,1,2) << -y / d2, x / d2; + (*H) << -y / d2, x / d2; } return Rot2::fromCosSin(x / n, y / n); } else { if (H) { - (*H).block(0,0,1,2) << 0.0, 0.0; + (*H) << 0.0, 0.0; } return Rot2(); } From ca9c66073feb8352c5448db9941949bf0e0b5cdd Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sun, 30 Nov 2014 16:09:46 -0500 Subject: [PATCH 38/76] Rot2 rotate() and unrotate() changes to OptionalJacobians --- gtsam/base/Matrix.h | 7 +++++++ gtsam/geometry/Rot2.cpp | 24 +++++++++++++++++------- gtsam/geometry/Rot2.h | 10 +++++----- gtsam/geometry/tests/testRot2.cpp | 4 +++- 4 files changed, 32 insertions(+), 13 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index fcdfbb757..47203f9b5 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -43,6 +43,13 @@ typedef Eigen::Matrix Matrix14; typedef Eigen::Matrix Matrix15; typedef Eigen::Matrix Matrix16; +typedef Eigen::Matrix Matrix21; +typedef Eigen::Matrix Matrix31; +typedef Eigen::Matrix Matrix41; +typedef Eigen::Matrix Matrix51; +typedef Eigen::Matrix Matrix61; + + typedef Eigen::Matrix2d Matrix2; typedef Eigen::Matrix3d Matrix3; typedef Eigen::Matrix4d Matrix4; diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 41cf2dd05..08279450e 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -71,16 +71,22 @@ Matrix2 Rot2::matrix() const { } /* ************************************************************************* */ -Matrix Rot2::transpose() const { - return (Matrix(2, 2) << c_, s_, -s_, c_).finished(); +Matrix2 Rot2::transpose() const { + Matrix2 rvalue_; + rvalue_ << c_, s_, -s_, c_; + return rvalue_; } /* ************************************************************************* */ // see doc/math.lyx, SO(2) section -Point2 Rot2::rotate(const Point2& p, boost::optional H1, - boost::optional H2) const { +Point2 Rot2::rotate(const Point2& p, OptionalJacobian<2, 1> H1, + OptionalJacobian<2, 2> H2) const { const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y()); - if (H1) *H1 = (Matrix(2, 1) << -q.y(), q.x()).finished(); + if (H1) { + Matrix21 H1_; + H1_ << -q.y(), q.x(); + *H1 = H1_; + } if (H2) *H2 = matrix(); return q; } @@ -88,9 +94,13 @@ Point2 Rot2::rotate(const Point2& p, boost::optional H1, /* ************************************************************************* */ // see doc/math.lyx, SO(2) section Point2 Rot2::unrotate(const Point2& p, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<2, 1> H1, OptionalJacobian<2, 2> H2) const { const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y()); - if (H1) *H1 = (Matrix(2, 1) << q.y(), -q.x()).finished(); // R_{pi/2}q + if (H1) { + Matrix21 H1_; + H1_ << q.y(), -q.x(); + *H1 = H1_; // R_{pi/2}q + } if (H2) *H2 = transpose(); return q; } diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 2052bb603..2f9d1a398 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -180,8 +180,8 @@ namespace gtsam { /** * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ */ - Point2 rotate(const Point2& p, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /** syntactic sugar for rotate */ inline Point2 operator*(const Point2& p) const { @@ -191,8 +191,8 @@ namespace gtsam { /** * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ */ - Point2 unrotate(const Point2& p, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /// @} /// @name Standard Interface @@ -228,7 +228,7 @@ namespace gtsam { Matrix2 matrix() const; /** return 2*2 transpose (inverse) rotation matrix */ - Matrix transpose() const; + Matrix2 transpose() const; private: /** Serialization function */ diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 8a1f942d2..cfb103c5d 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -49,7 +49,9 @@ TEST( Rot2, unit) /* ************************************************************************* */ TEST( Rot2, transpose) { - CHECK(assert_equal(R.inverse().matrix(),R.transpose())); + Matrix expected = R.inverse().matrix(); + Matrix actual = R.transpose(); + CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ From c250f1d732f2d234ceb24c8a644a46134fcf8363 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sun, 30 Nov 2014 17:06:03 -0500 Subject: [PATCH 39/76] removed unecessary code --- gtsam/geometry/Rot2.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 08279450e..8be5e9d93 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -82,11 +82,7 @@ Matrix2 Rot2::transpose() const { Point2 Rot2::rotate(const Point2& p, OptionalJacobian<2, 1> H1, OptionalJacobian<2, 2> H2) const { const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y()); - if (H1) { - Matrix21 H1_; - H1_ << -q.y(), q.x(); - *H1 = H1_; - } + if (H1) *H1 << -q.y(), q.x(); if (H2) *H2 = matrix(); return q; } From 73564f1170d4f2e9fc1fbfc20fe431972de1b4ea Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sun, 30 Nov 2014 17:07:26 -0500 Subject: [PATCH 40/76] removed unecessary code --- gtsam/geometry/Rot2.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 8be5e9d93..5d9308b13 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -92,11 +92,7 @@ Point2 Rot2::rotate(const Point2& p, OptionalJacobian<2, 1> H1, Point2 Rot2::unrotate(const Point2& p, OptionalJacobian<2, 1> H1, OptionalJacobian<2, 2> H2) const { const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y()); - if (H1) { - Matrix21 H1_; - H1_ << q.y(), -q.x(); - *H1 = H1_; // R_{pi/2}q - } + if (H1) *H1 << q.y(), -q.x(); if (H2) *H2 = transpose(); return q; } From d9b6aed23cf22d450f288e42fc4151e8e4bb8242 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sun, 30 Nov 2014 17:24:02 -0500 Subject: [PATCH 41/76] Point2 changed to fixed size matrices. Make check works --- gtsam/geometry/Point2.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 6588f051f..68022e201 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -50,12 +50,12 @@ double Point2::norm(OptionalJacobian<1,2> H) const { } /* ************************************************************************* */ -static const Matrix I2 = eye(2); +static const Matrix2 I2 = Eigen::Matrix2d::Identity(); double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1, OptionalJacobian<1,2> H2) const { Point2 d = point - *this; if (H1 || H2) { - Eigen::Matrix H; + Matrix12 H; double r = d.norm(H); if (H1) *H1 = -H; if (H2) *H2 = H; From 186b01fd71f7dc61384973d7a65cbfd0a36e6212 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sun, 30 Nov 2014 17:49:01 -0500 Subject: [PATCH 42/76] Done with Point3 and fixed size matrices. make check works --- gtsam/geometry/Point3.cpp | 23 +++++++++++------------ gtsam/geometry/Point3.h | 38 ++++++++++++++++++++------------------ 2 files changed, 31 insertions(+), 30 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 03ed31ab5..34f939635 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -63,22 +63,22 @@ Point3 Point3::operator/(double s) const { } /* ************************************************************************* */ -Point3 Point3::add(const Point3 &q, boost::optional H1, - boost::optional H2) const { +Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { if (H1) - *H1 = eye(3, 3); + (*H1).setIdentity(); if (H2) - *H2 = eye(3, 3); + (*H2).setIdentity(); return *this + q; } /* ************************************************************************* */ -Point3 Point3::sub(const Point3 &q, boost::optional H1, - boost::optional H2) const { +Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { if (H1) - *H1 = eye(3, 3); + (*H1).setIdentity(); if (H2) - *H2 = -eye(3, 3); + (*H2).setIdentity(); return *this - q; } @@ -106,15 +106,14 @@ double Point3::norm(OptionalJacobian<1,3> H) const { } /* ************************************************************************* */ -Point3 Point3::normalize(boost::optional H) const { +Point3 Point3::normalize(OptionalJacobian<3,3> H) const { Point3 normalized = *this / norm(); if (H) { // 3*3 Derivative double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_; double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_; - H->resize(3, 3); - *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; - *H /= pow(x2 + y2 + z2, 1.5); + (*H) << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; + (*H) /= pow(x2 + y2 + z2, 1.5); } return normalized; } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 96cf4e0c8..410cd0e12 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -93,10 +93,10 @@ namespace gtsam { /// "Compose" - just adds coordinates of two points inline Point3 compose(const Point3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if (H1) *H1 = eye(3); - if (H2) *H2 = eye(3); + OptionalJacobian<3,3> H1=boost::none, + OptionalJacobian<3,3> H2=boost::none) const { + if (H1) (*H1).setIdentity(); + if (H2) (*H2).setIdentity(); return *this + p2; } @@ -105,10 +105,10 @@ namespace gtsam { /** Between using the default implementation */ inline Point3 between(const Point3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(3); - if(H2) *H2 = eye(3); + OptionalJacobian<3,3> H1=boost::none, + OptionalJacobian<3,3> H2=boost::none) const { + if(H1) (*H1) = (-Eigen::Matrix3d::Identity()); + if(H2) (*H2).setIdentity(); return p2 - *this; } @@ -142,13 +142,13 @@ namespace gtsam { static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); } /// Left-trivialized derivative of the exponential map - static Matrix dexpL(const Vector& v) { - return eye(3); + static Matrix3 dexpL(const Vector& v) { + return Eigen::Matrix3d::Identity(); } /// Left-trivialized derivative inverse of the exponential map - static Matrix dexpInvL(const Vector& v) { - return eye(3); + static Matrix3 dexpInvL(const Vector& v) { + return Eigen::Matrix3d::Identity(); } /// @} @@ -163,14 +163,16 @@ namespace gtsam { /** distance between two points */ inline double distance(const Point3& p2, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { + OptionalJacobian<1,3> H1 = boost::none, OptionalJacobian<1,3> H2 = boost::none) const { double d = (p2 - *this).norm(); if (H1) { - *H1 = (Matrix(1, 3) << x_-p2.x(), y_-p2.y(), z_-p2.z()).finished()*(1./d); + *H1 << x_-p2.x(), y_-p2.y(), z_-p2.z(); + (*H1) = (*H1) *(1./d); } if (H2) { - *H2 = (Matrix(1, 3) << -x_+p2.x(), -y_+p2.y(), -z_+p2.z()).finished()*(1./d); + *H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z(); + (*H2) = (*H2) *(1./d); } return d; } @@ -184,7 +186,7 @@ namespace gtsam { double norm(OptionalJacobian<1,3> H = boost::none) const; /** normalize, with optional Jacobian */ - Point3 normalize(boost::optional H = boost::none) const; + Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const; /** cross product @return this x q */ Point3 cross(const Point3 &q) const; @@ -213,11 +215,11 @@ namespace gtsam { /** add two points, add(this,q) is same as this + q */ Point3 add (const Point3 &q, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + OptionalJacobian<3, 3> H1=boost::none, OptionalJacobian<3, 3> H2=boost::none) const; /** subtract two points, sub(this,q) is same as this - q */ Point3 sub (const Point3 &q, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const; /// @} From bee32cc4722b9972d4bb200cdfcac23f9518c9de Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Sun, 30 Nov 2014 18:01:15 -0500 Subject: [PATCH 43/76] Rot2 done, make check works --- gtsam/geometry/Rot2.cpp | 6 +++--- gtsam/geometry/Rot2.h | 16 ++++++++-------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 5d9308b13..af8d701ec 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -65,9 +65,9 @@ Rot2& Rot2::normalize() { /* ************************************************************************* */ Matrix2 Rot2::matrix() const { - Matrix2 rvalue; - rvalue << c_, -s_, s_, c_; - return rvalue; + Matrix2 rvalue_; + rvalue_ << c_, -s_, s_, c_; + return rvalue_; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 2f9d1a398..558dd06f6 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -116,10 +116,10 @@ namespace gtsam { } /** Compose - make a new rotation by adding angles */ - inline Rot2 compose(const Rot2& R, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { - if (H1) *H1 = eye(1); - if (H2) *H2 = eye(1); + inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 = + boost::none, OptionalJacobian<1,1> H2 = boost::none) const { + if (H1) (*H1)<< 1; // set to 1x1 identity matrix + if (H2) (*H2)<< 1; // set to 1x1 identity matrix return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); } @@ -129,10 +129,10 @@ namespace gtsam { } /** Between using the default implementation */ - inline Rot2 between(const Rot2& R, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { - if (H1) *H1 = -eye(1); - if (H2) *H2 = eye(1); + inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 = + boost::none, OptionalJacobian<1,1> H2 = boost::none) const { + if (H1) *H1 << -1; // set to 1x1 identity matrix + if (H2) *H2 << 1; // set to 1x1 identity matrix return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_); } From 952f4d7810238bf008b2043307e9321441e8c68b Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Mon, 1 Dec 2014 15:16:55 +0100 Subject: [PATCH 44/76] operator -> --- gtsam/base/OptionalJacobian.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 91cd1089a..5651816ba 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -108,6 +108,7 @@ public: } /// TODO: operator->() + Eigen::Map* operator->(){ return &map_; } }; } // namespace gtsam From 490e75b103046421c523e721323008f0f27a2165 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Mon, 1 Dec 2014 10:51:01 -0500 Subject: [PATCH 45/76] finished Pose2.cpp using @dellaert 's temporary matrices idea. Still have a couple of functions that are not fixed for instance wedge, a template specialization from Lie.h. --- gtsam/geometry/Pose2.cpp | 91 ++++++++++++++++++------------ gtsam/geometry/Pose2.h | 28 ++++----- gtsam/geometry/tests/testPose2.cpp | 2 +- 3 files changed, 72 insertions(+), 49 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index dfda7b3b1..91e915bd0 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -33,15 +33,21 @@ INSTANTIATE_LIE(Pose2); /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose2); -static const Matrix I3 = eye(3), Z12 = zeros(1,2); +static const Matrix3 I3 = Eigen::Matrix3d::Identity(), Z12 = Eigen::Matrix3d::Zero(); static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); /* ************************************************************************* */ -Matrix Pose2::matrix() const { - Matrix R = r_.matrix(); - R = stack(2, &R, &Z12); - Matrix T = (Matrix(3, 1) << t_.x(), t_.y(), 1.0).finished(); - return collect(2, &R, &T); +Matrix3 Pose2::matrix() const { + Matrix2 R = r_.matrix(); + Matrix32 R0; + R0.block(0,0,2,2) = R; + R0.block(2,0,1,2).setZero(); + Matrix31 T; + T << t_.x(), t_.y(), 1.0; + Matrix3 RT_; + RT_.block(0,0,3,2) = R0; + RT_.block(0,2,3,1) = T; + return RT_; } /* ************************************************************************* */ @@ -140,8 +146,8 @@ Point2 Pose2::transform_to(const Point2& point, /* ************************************************************************* */ // see doc/math.lyx, SE(2) section -Pose2 Pose2::compose(const Pose2& p2, boost::optional H1, - boost::optional H2) const { +Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { // TODO: inline and reuse? if(H1) *H1 = p2.inverse().AdjointMap(); if(H2) *H2 = I3; @@ -154,10 +160,14 @@ Point2 Pose2::transform_from(const Point2& p, OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { const Point2 q = r_ * p; if (H1 || H2) { - const Matrix R = r_.matrix(); - const Matrix Drotate1 = (Matrix(2, 1) << -q.y(), q.x()).finished(); - if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q] - if (H2) *H2 = R; // R + const Matrix2 R = r_.matrix(); + Matrix21 Drotate1; + Drotate1 << -q.y(), q.x(); + if (H1) { + (*H1).block(0,0,2,2) = R; // [R R_{pi/2}q] + (*H1).block(0,2,2,1) = Drotate1; + } + if (H2) *H2 = R; // R } return q + t_; } @@ -198,24 +208,27 @@ Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1, /* ************************************************************************* */ Rot2 Pose2::bearing(const Point2& point, - boost::optional H1, boost::optional H2) const { - Point2 d = transform_to(point, H1, H2); + OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const { + // make temporary matrices + Matrix23 D1; Matrix2 D2; + Point2 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); // uses pointer version if (!H1 && !H2) return Rot2::relativeBearing(d); - Matrix D_result_d; + Matrix12 D_result_d; Rot2 result = Rot2::relativeBearing(d, D_result_d); - if (H1) *H1 = D_result_d * (*H1); - if (H2) *H2 = D_result_d * (*H2); + if (H1) *H1 = D_result_d * (D1); + if (H2) *H2 = D_result_d * (D2); return result; } /* ************************************************************************* */ Rot2 Pose2::bearing(const Pose2& pose, - boost::optional H1, boost::optional H2) const { - Rot2 result = bearing(pose.t(), H1, H2); + OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { + Matrix12 D2; + Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0); if (H2) { - Matrix H2_ = *H2 * pose.r().matrix(); - *H2 = zeros(1, 3); - insertSub(*H2, H2_, 0, 0); + Matrix12 H2_ = D2 * pose.r().matrix(); + (*H2).setZero(); + (*H2).block(0,0,1,2) = H2_; } return result; } @@ -227,29 +240,37 @@ double Pose2::range(const Point2& point, Matrix12 H; double r = d.norm(H); if (H1) { - Matrix23 mvalue; // is this the correct name ? - mvalue << -r_.c(), r_.s(), 0.0, + Matrix23 H1_; + H1_ << -r_.c(), r_.s(), 0.0, -r_.s(), -r_.c(), 0.0; - *H1 = H * mvalue; + *H1 = H * H1_; } if (H2) *H2 = H; return r; } /* ************************************************************************* */ -double Pose2::range(const Pose2& pose2, - boost::optional H1, - boost::optional H2) const { - Point2 d = pose2.t() - t_; +double Pose2::range(const Pose2& pose, + OptionalJacobian<1,3> H1, + OptionalJacobian<1,3> H2) const { + Point2 d = pose.t() - t_; if (!H1 && !H2) return d.norm(); - Matrix H; + Matrix12 H; double r = d.norm(H); - if (H1) *H1 = H * (Matrix(2, 3) << + if (H1) { + Matrix23 H1_; + H1_ << -r_.c(), r_.s(), 0.0, - -r_.s(), -r_.c(), 0.0).finished(); - if (H2) *H2 = H * (Matrix(2, 3) << - pose2.r_.c(), -pose2.r_.s(), 0.0, - pose2.r_.s(), pose2.r_.c(), 0.0).finished(); + -r_.s(), -r_.c(), 0.0; + *H1 = H * H1_; + } + if (H2) { + Matrix23 H2_; + H2_ << + pose.r_.c(), -pose.r_.s(), 0.0, + pose.r_.s(), pose.r_.c(), 0.0; + *H2 = H * H2_; + } return r; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 2a9f91508..7e3c29826 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -75,7 +75,7 @@ public: Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} /** Constructor from 3*3 matrix */ - Pose2(const Matrix &T) : + Pose2(const Matrix &T) : // TODO : Change this to Optional Jacobian ?? r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { assert(T.rows() == 3 && T.cols() == 3); } @@ -111,8 +111,8 @@ public: /// compose this transformation onto another (first *this and then p2) Pose2 compose(const Pose2& p2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; /// compose syntactic sugar inline Pose2 operator*(const Pose2& p2) const { @@ -168,11 +168,13 @@ public: * v (vx,vy) = 2D velocity * @return xihat, 3*3 element of Lie algebra that can be exponentiated */ - static inline Matrix wedge(double vx, double vy, double w) { - return (Matrix(3,3) << + static inline Matrix3 wedge(double vx, double vy, double w) { + Matrix3 wedge_; + wedge_ << 0.,-w, vx, w, 0., vy, - 0., 0., 0.).finished(); + 0., 0., 0.; + return wedge_; } /// @} @@ -218,7 +220,7 @@ public: inline const Rot2& rotation() const { return r_; } //// return transformation matrix - Matrix matrix() const; + Matrix3 matrix() const; /** * Calculate bearing to a landmark @@ -226,15 +228,15 @@ public: * @return 2D rotation \f$ \in SO(2) \f$ */ Rot2 bearing(const Point2& point, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const; /** * Calculate bearing to another pose * @param point SO(2) location of other pose * @return 2D rotation \f$ \in SO(2) \f$ */ - Rot2 bearing(const Pose2& point, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + Rot2 bearing(const Pose2& pose, + OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const; /** * Calculate range to a landmark @@ -251,8 +253,8 @@ public: * @return range (double) */ double range(const Pose2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1, 3> H1=boost::none, + OptionalJacobian<1, 3> H2=boost::none) const; /// @} /// @name Advanced Interface @@ -287,7 +289,7 @@ private: /** specialization for pose2 wedge function (generic template in Lie.h) */ template <> -inline Matrix wedge(const Vector& xi) { +inline Matrix wedge(const Vector& xi) { // TODO : Convert to Optional Jacobian ? return Pose2::wedge(xi(0),xi(1),xi(2)); } diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 01111aafe..f12d96899 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -44,7 +44,7 @@ TEST(Pose2, constructors) { Pose2 origin; assert_equal(pose,origin); Pose2 t(M_PI/2.0+0.018, Point2(1.015, 2.01)); - EXPECT(assert_equal(t,Pose2(t.matrix()))); + EXPECT(assert_equal(t,Pose2((Matrix)t.matrix()))); } /* ************************************************************************* */ From 96f2c8eebeb68221057545580d63ac451fe2d17a Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Mon, 1 Dec 2014 10:57:36 -0500 Subject: [PATCH 46/76] removed unecessary Z12 --- gtsam/geometry/Pose2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 91e915bd0..7ef7a4514 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -33,7 +33,7 @@ INSTANTIATE_LIE(Pose2); /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose2); -static const Matrix3 I3 = Eigen::Matrix3d::Identity(), Z12 = Eigen::Matrix3d::Zero(); +static const Matrix3 I3 = Eigen::Matrix3d::Identity(); static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); /* ************************************************************************* */ From ccda2e1b3b75df4e3048c6946107a9e9971f67ad Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Mon, 1 Dec 2014 14:29:32 -0500 Subject: [PATCH 47/76] replaced block with <> to identify sizes at compile time. --- gtsam/geometry/Pose2.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 7ef7a4514..cb77bfc7d 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -40,13 +40,13 @@ static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); Matrix3 Pose2::matrix() const { Matrix2 R = r_.matrix(); Matrix32 R0; - R0.block(0,0,2,2) = R; - R0.block(2,0,1,2).setZero(); + R0.block<2,2>(0,0) = R; + R0.block<1,2>(2,0).setZero(); Matrix31 T; T << t_.x(), t_.y(), 1.0; Matrix3 RT_; - RT_.block(0,0,3,2) = R0; - RT_.block(0,2,3,1) = T; + RT_.block<3,2<(0,0) = R0; + RT_.block<3,1>(0,2) = T; return RT_; } @@ -164,8 +164,8 @@ Point2 Pose2::transform_from(const Point2& p, Matrix21 Drotate1; Drotate1 << -q.y(), q.x(); if (H1) { - (*H1).block(0,0,2,2) = R; // [R R_{pi/2}q] - (*H1).block(0,2,2,1) = Drotate1; + (*H1).block<2,2>(0,0) = R; // [R R_{pi/2}q] + (*H1).block<2,1>(0,2) = Drotate1; } if (H2) *H2 = R; // R } @@ -228,7 +228,7 @@ Rot2 Pose2::bearing(const Pose2& pose, if (H2) { Matrix12 H2_ = D2 * pose.r().matrix(); (*H2).setZero(); - (*H2).block(0,0,1,2) = H2_; + (*H2).block<1,2>(0,0) = H2_; } return result; } From 1b2d86929a37bb141b238a20ae497b766a687477 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Mon, 1 Dec 2014 15:52:30 -0500 Subject: [PATCH 48/76] partially fixed Pose3(). Also adressed some of frank's comments from previous commits --- gtsam/geometry/Pose2.cpp | 2 +- gtsam/geometry/Pose2.h | 10 ++--- gtsam/geometry/Pose3.cpp | 85 +++++++++++++++++++++------------------- gtsam/geometry/Pose3.h | 28 ++++++------- 4 files changed, 64 insertions(+), 61 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index cb77bfc7d..af0d330b8 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -45,7 +45,7 @@ Matrix3 Pose2::matrix() const { Matrix31 T; T << t_.x(), t_.y(), 1.0; Matrix3 RT_; - RT_.block<3,2<(0,0) = R0; + RT_.block<3,2>(0,0) = R0; RT_.block<3,1>(0,2) = T; return RT_; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 7e3c29826..49eb444b2 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -75,7 +75,7 @@ public: Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} /** Constructor from 3*3 matrix */ - Pose2(const Matrix &T) : // TODO : Change this to Optional Jacobian ?? + Pose2(const Matrix &T) : r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { assert(T.rows() == 3 && T.cols() == 3); } @@ -169,12 +169,10 @@ public: * @return xihat, 3*3 element of Lie algebra that can be exponentiated */ static inline Matrix3 wedge(double vx, double vy, double w) { - Matrix3 wedge_; - wedge_ << + return (Matrix(3,3) << 0.,-w, vx, w, 0., vy, - 0., 0., 0.; - return wedge_; + 0., 0., 0.).finished(); } /// @} @@ -289,7 +287,7 @@ private: /** specialization for pose2 wedge function (generic template in Lie.h) */ template <> -inline Matrix wedge(const Vector& xi) { // TODO : Convert to Optional Jacobian ? +inline Matrix wedge(const Vector& xi) { return Pose2::wedge(xi(0),xi(1),xi(2)); } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 93a27ed58..2e3437573 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -32,8 +32,7 @@ INSTANTIATE_LIE(Pose3); /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3); -static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3; -static const Matrix6 I6 = eye(6); +static const Matrix3 I3 = Eigen::Matrix3d::Identity(), Z3 = Eigen::Matrix3d::Zero(), _I3 = -I3; /* ************************************************************************* */ Pose3::Pose3(const Pose2& pose2) : @@ -55,7 +54,7 @@ Matrix6 Pose3::AdjointMap() const { } /* ************************************************************************* */ -Matrix6 Pose3::adjointMap(const Vector& xi) { +Matrix6 Pose3::adjointMap(const Vector6& xi) { Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); Matrix6 adj; @@ -65,14 +64,15 @@ Matrix6 Pose3::adjointMap(const Vector& xi) { } /* ************************************************************************* */ -Vector Pose3::adjoint(const Vector& xi, const Vector& y, - boost::optional H) { +Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, + OptionalJacobian<6,6> H) { if (H) { - *H = zeros(6, 6); + (*H).setZero(); for (int i = 0; i < 6; ++i) { - Vector dxi = zero(6); + Vector6 dxi; + dxi.setZero(); dxi(i) = 1.0; - Matrix Gi = adjointMap(dxi); + Matrix6 Gi = adjointMap(dxi); (*H).col(i) = Gi * y; } } @@ -80,29 +80,31 @@ Vector Pose3::adjoint(const Vector& xi, const Vector& y, } /* ************************************************************************* */ -Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, - boost::optional H) { +Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, + OptionalJacobian<6,6> H) { if (H) { - *H = zeros(6, 6); + (*H).setZero(); for (int i = 0; i < 6; ++i) { - Vector dxi = zero(6); + Vector6 dxi; + dxi.setZero(); dxi(i) = 1.0; Matrix GTi = adjointMap(dxi).transpose(); (*H).col(i) = GTi * y; } } - Matrix adjT = adjointMap(xi).transpose(); return adjointMap(xi).transpose() * y; } /* ************************************************************************* */ -Matrix6 Pose3::dExpInv_exp(const Vector& xi) { +Matrix6 Pose3::dExpInv_exp(const Vector6& xi) { // Bernoulli numbers, from Wikipedia static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, 0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished(); static const int N = 5; // order of approximation - Matrix res = I6; - Matrix6 ad_i = I6; + Matrix6 res; + res.setIdentity(); + Matrix6 ad_i; + ad_i.setIdentity(); Matrix6 ad_xi = adjointMap(xi); double fac = 1.0; for (int i = 1; i < N; ++i) { @@ -225,7 +227,7 @@ Vector6 Pose3::localCoordinates(const Pose3& T, Matrix4 Pose3::matrix() const { const Matrix3 R = R_.matrix(); const Vector3 T = t_.vector(); - Eigen::Matrix A14; + Matrix14 A14; A14 << 0.0, 0.0, 0.0, 1.0; Matrix4 mat; mat << R, T, A14; @@ -240,12 +242,11 @@ Pose3 Pose3::transform_to(const Pose3& pose) const { } /* ************************************************************************* */ -Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, - boost::optional Dpoint) const { +Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, + OptionalJacobian<3,3> Dpoint) const { if (Dpose) { const Matrix3 R = R_.matrix(); Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); - Dpose->resize(3, 6); (*Dpose) << DR, R; } if (Dpoint) @@ -273,17 +274,17 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, } /* ************************************************************************* */ -Pose3 Pose3::compose(const Pose3& p2, boost::optional H1, - boost::optional H2) const { +Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1, + OptionalJacobian<6,6> H2) const { if (H1) *H1 = p2.inverse().AdjointMap(); if (H2) - *H2 = I6; + (*H2).setIdentity(); return (*this) * p2; } /* ************************************************************************* */ -Pose3 Pose3::inverse(boost::optional H1) const { +Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const { if (H1) *H1 = -AdjointMap(); Rot3 Rt = R_.inverse(); @@ -292,40 +293,44 @@ Pose3 Pose3::inverse(boost::optional H1) const { /* ************************************************************************* */ // between = compose(p2,inverse(p1)); -Pose3 Pose3::between(const Pose3& p2, boost::optional H1, - boost::optional H2) const { +Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1, + OptionalJacobian<6,6> H2) const { Pose3 result = inverse() * p2; if (H1) *H1 = -result.inverse().AdjointMap(); if (H2) - *H2 = I6; + (*H2).setIdentity(); return result; } /* ************************************************************************* */ -double Pose3::range(const Point3& point, boost::optional H1, - boost::optional H2) const { +double Pose3::range(const Point3& point, OptionalJacobian<1,6> H1, + OptionalJacobian<1,3> H2) const { if (!H1 && !H2) return transform_to(point).norm(); - Point3 d = transform_to(point, H1, H2); + Matrix36 D1; + Matrix3 D2; + Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt( d2); - Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n).finished(); + Matrix13 D_result_d ; + D_result_d << x / n, y / n, z / n; if (H1) - *H1 = D_result_d * (*H1); + *H1 = D_result_d * (D1); if (H2) - *H2 = D_result_d * (*H2); + *H2 = D_result_d * (D2); return n; } /* ************************************************************************* */ -double Pose3::range(const Pose3& point, boost::optional H1, - boost::optional H2) const { - double r = range(point.translation(), H1, H2); +double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1, + OptionalJacobian<1,6> H2) const { + Matrix13 D2; + double r = range(pose.translation(), H1, H2? &D2 : 0); if (H2) { - Matrix H2_ = *H2 * point.rotation().matrix(); - *H2 = zeros(1, 6); - insertSub(*H2, H2_, 0, 3); + Matrix13 H2_ = D2 * pose.rotation().matrix(); + (*H2).setZero(); + (*H2).block<1,3>(0,3) = H2_; } return r; } @@ -347,7 +352,7 @@ boost::optional align(const vector& pairs) { cq *= f; // Add to form H matrix - Matrix H = zeros(3, 3); + Matrix3 H = Eigen::Matrix3d::Zero(); BOOST_FOREACH(const Point3Pair& pair, pairs){ Vector dp = pair.first.vector() - cp; Vector dq = pair.second.vector() - cq; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index c31cc48cc..d396fe61f 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -99,11 +99,11 @@ public: } /// inverse transformation with derivatives - Pose3 inverse(boost::optional H1 = boost::none) const; + Pose3 inverse(OptionalJacobian<6, 6> H1 = boost::none) const; ///compose this transformation onto another (first *this and then p2) - Pose3 compose(const Pose3& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Pose3 compose(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none, + OptionalJacobian<6, 6> H2 = boost::none) const; /// compose syntactic sugar Pose3 operator*(const Pose3& T) const { @@ -114,8 +114,8 @@ public: * Return relative pose between p1 and p2, in p1 coordinate frame * as well as optionally the derivatives */ - Pose3 between(const Pose3& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Pose3 between(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none, + OptionalJacobian<6, 6> H2 = boost::none) const; /// @} /// @name Manifold @@ -186,17 +186,17 @@ public: * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. * */ - static Matrix6 adjointMap(const Vector& xi); + static Matrix6 adjointMap(const Vector6 &xi); /** * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives */ - static Vector adjoint(const Vector& xi, const Vector& y, boost::optional H = boost::none); + static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, OptionalJacobian<6,6> = boost::none); /** * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ - static Vector adjointTranspose(const Vector& xi, const Vector& y, boost::optional H = boost::none); + static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none); /** * Compute the inverse right-trivialized tangent (derivative) map of the exponential map, @@ -208,7 +208,7 @@ public: * Ernst Hairer, et al., Geometric Numerical Integration, * Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006. */ - static Matrix6 dExpInv_exp(const Vector& xi); + static Matrix6 dExpInv_exp(const Vector6 &xi); /** * wedge for Pose3: @@ -237,7 +237,7 @@ public: * @return point in world coordinates */ Point3 transform_from(const Point3& p, - boost::optional Dpose=boost::none, boost::optional Dpoint=boost::none) const; + OptionalJacobian<3,6> Dpose=boost::none, OptionalJacobian<3,3> Dpoint=boost::none) const; /** syntactic sugar for transform_from */ inline Point3 operator*(const Point3& p) const { return transform_from(p); } @@ -284,8 +284,8 @@ public: * @return range (double) */ double range(const Point3& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1,6> H1=boost::none, + OptionalJacobian<1,3> H2=boost::none) const; /** * Calculate range to another pose @@ -293,8 +293,8 @@ public: * @return range (double) */ double range(const Pose3& pose, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1,6> H1=boost::none, + OptionalJacobian<1,6> H2=boost::none) const; /// @} /// @name Advanced Interface From 595afb51fe65b06f00f57b7715843f40bc64cb57 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Mon, 1 Dec 2014 18:20:03 -0500 Subject: [PATCH 49/76] fixed Rot3(). @dellaert, I will do the '->', Identity(), setZero() etc . once I am fully done with geometry. --- gtsam/geometry/Pose3.cpp | 10 +++--- gtsam/geometry/Rot3.cpp | 54 ++++++++++--------------------- gtsam/geometry/Rot3.h | 29 +++++++---------- gtsam/geometry/Rot3M.cpp | 8 ++--- gtsam/geometry/Unit3.cpp | 6 ++-- gtsam/geometry/Unit3.h | 6 ++-- gtsam/geometry/tests/testRot3.cpp | 2 +- 7 files changed, 43 insertions(+), 72 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2e3437573..1eb4c8db5 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -83,12 +83,12 @@ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6,6> H) { if (H) { - (*H).setZero(); + H->setZero(); for (int i = 0; i < 6; ++i) { Vector6 dxi; dxi.setZero(); dxi(i) = 1.0; - Matrix GTi = adjointMap(dxi).transpose(); + Matrix6 GTi = adjointMap(dxi).transpose(); (*H).col(i) = GTi * y; } } @@ -360,13 +360,13 @@ boost::optional align(const vector& pairs) { } // Compute SVD - Matrix U, V; + Matrix U,V; Vector S; svd(H, U, S, V); // Recover transform with correction from Eggert97machinevisionandapplications - Matrix UVtranspose = U * V.transpose(); - Matrix detWeighting = eye(3, 3); + Matrix3 UVtranspose = U * V.transpose(); + Matrix3 detWeighting = Eigen::Matrix3d::Identity(); detWeighting(2, 2) = UVtranspose.determinant(); Rot3 R(Matrix(V * detWeighting * U.transpose())); Point3 t = Point3(cq) - R * Point3(cp); diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 846e0e070..db82c6f1e 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -72,10 +72,11 @@ Point3 Rot3::operator*(const Point3& p) const { /* ************************************************************************* */ Unit3 Rot3::rotate(const Unit3& p, - boost::optional HR, boost::optional Hp) const { - Unit3 q = Unit3(rotate(p.point3(Hp))); + OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { + Matrix32 Dp; + Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0))); if (Hp) - (*Hp) = q.basis().transpose() * matrix() * (*Hp); + (*Hp) = q.basis().transpose() * matrix() * Dp; if (HR) (*HR) = -q.basis().transpose() * matrix() * p.skew(); return q; @@ -83,10 +84,11 @@ Unit3 Rot3::rotate(const Unit3& p, /* ************************************************************************* */ Unit3 Rot3::unrotate(const Unit3& p, - boost::optional HR, boost::optional Hp) const { - Unit3 q = Unit3(unrotate(p.point3(Hp))); + OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { + Matrix32 Dp; + Unit3 q = Unit3(unrotate(p.point3(Dp))); if (Hp) - (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp); + (*Hp) = q.basis().transpose() * matrix().transpose () * Dp; if (HR) (*HR) = q.basis().transpose() * q.skew(); return q; @@ -99,8 +101,8 @@ Unit3 Rot3::operator*(const Unit3& p) const { /* ************************************************************************* */ // see doc/math.lyx, SO(3) section -Point3 Rot3::unrotate(const Point3& p, boost::optional H1, - boost::optional H2) const { +Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { const Matrix3& Rt = transpose(); Point3 q(Rt * p.vector()); // q = Rt*p const double wx = q.x(), wy = q.y(), wz = q.z(); @@ -111,32 +113,16 @@ Point3 Rot3::unrotate(const Point3& p, boost::optional H1, return q; } -/* ************************************************************************* */ -// see doc/math.lyx, SO(3) section -Point3 Rot3::unrotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - const Matrix3& Rt = transpose(); - Point3 q(Rt * p.vector()); // q = Rt*p - const double wx = q.x(), wy = q.y(), wz = q.z(); - if (H1) { - H1->resize(3,3); - *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; - } - if (H2) - *H2 = Rt; - return q; -} - /* ************************************************************************* */ /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) Matrix3 Rot3::dexpL(const Vector3& v) { if(zero(v)) return eye(3); - Matrix x = skewSymmetric(v); - Matrix x2 = x*x; + Matrix3 x = skewSymmetric(v); + Matrix3 x2 = x*x; double theta = v.norm(), vi = theta/2.0; double s1 = sin(vi)/vi; double s2 = (theta - sin(theta))/(theta*theta*theta); - Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2; + Matrix3 res = I3 - 0.5*s1*s1*x + s2*x2; return res; } @@ -144,11 +130,11 @@ Matrix3 Rot3::dexpL(const Vector3& v) { /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) Matrix3 Rot3::dexpInvL(const Vector3& v) { if(zero(v)) return eye(3); - Matrix x = skewSymmetric(v); - Matrix x2 = x*x; + Matrix3 x = skewSymmetric(v); + Matrix3 x2 = x*x; double theta = v.norm(), vi = theta/2.0; double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); - Matrix res = eye(3) + 0.5*x - s2*x2; + Matrix3 res = I3 + 0.5*x - s2*x2; return res; } @@ -167,7 +153,7 @@ Point3 Rot3::column(int index) const{ /* ************************************************************************* */ Vector3 Rot3::xyz() const { - Matrix I;Vector3 q; + Matrix3 I;Vector3 q; boost::tie(I,q)=RQ(matrix()); return q; } @@ -255,12 +241,6 @@ ostream &operator<<(ostream &os, const Rot3& R) { return os; } -/* ************************************************************************* */ -Point3 Rot3::unrotate(const Point3& p) const { - // Eigen expression - return Point3(transpose()*p.vector()); // q = Rt*p -} - /* ************************************************************************* */ Rot3 Rot3::slerp(double t, const Rot3& other) const { // amazingly simple in GTSAM :-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 4364678a5..edd39619f 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -215,7 +215,7 @@ namespace gtsam { } /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity - Rot3 inverse(boost::optional H1=boost::none) const; + Rot3 inverse(boost::optional H1=boost::none) const; /// Compose two rotations i.e., R= (*this) * R2 Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none, @@ -238,8 +238,8 @@ namespace gtsam { * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' */ Rot3 between(const Rot3& R2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<3,3> H1=boost::none, + OptionalJacobian<3,3> H2=boost::none) const; /// @} /// @name Manifold @@ -321,34 +321,27 @@ namespace gtsam { /** * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ */ - Point3 rotate(const Point3& p, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, + OptionalJacobian<3,3> H2 = boost::none) const; /// rotate point from rotated coordinate frame to world = R*p Point3 operator*(const Point3& p) const; /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p) const; - - /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p, boost::optional H1, - boost::optional H2) const; - - /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p, boost::optional H1, - boost::optional H2) const; + Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, + OptionalJacobian<3,3> H2=boost::none) const; /// @} /// @name Group Action on Unit3 /// @{ /// rotate 3D direction from rotated coordinate frame to world frame - Unit3 rotate(const Unit3& p, boost::optional HR = boost::none, - boost::optional Hp = boost::none) const; + Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none, + OptionalJacobian<2,2> Hp = boost::none) const; /// unrotate 3D direction from world frame to rotated coordinate frame - Unit3 unrotate(const Unit3& p, boost::optional HR = boost::none, - boost::optional Hp = boost::none) const; + Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none, + OptionalJacobian<2,2> Hp = boost::none) const; /// rotate 3D direction from rotated coordinate frame to world frame Unit3 operator*(const Unit3& p) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 39648ca1e..d657c7287 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -162,14 +162,14 @@ Matrix3 Rot3::transpose() const { } /* ************************************************************************* */ -Rot3 Rot3::inverse(boost::optional H1) const { +Rot3 Rot3::inverse(boost::optional H1) const { if (H1) *H1 = -rot_; return Rot3(Matrix3(transpose())); } /* ************************************************************************* */ Rot3 Rot3::between (const Rot3& R2, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = -(R2.transpose()*rot_); if (H2) *H2 = I3; Matrix3 R12 = transpose()*R2.rot_; @@ -178,7 +178,7 @@ Rot3 Rot3::between (const Rot3& R2, /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1 || H2) { if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = rot_; @@ -245,7 +245,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { } else if(mode == Rot3::CAYLEY) { return retractCayley(omega); } else if(mode == Rot3::SLOW_CAYLEY) { - Matrix Omega = skewSymmetric(omega); + Matrix3 Omega = skewSymmetric(omega); return (*this)*CayleyFixed<3>(-Omega/2); } else { assert(false); diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 5e13129cc..daff6b00b 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -67,7 +67,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { } /* ************************************************************************* */ -const Unit3::Matrix32& Unit3::basis() const { +const Matrix32& Unit3::basis() const { // Return cached version if exists if (B_) @@ -92,7 +92,7 @@ const Unit3::Matrix32& Unit3::basis() const { b2 = b2 / b2.norm(); // Create the basis matrix - B_.reset(Unit3::Matrix32()); + B_.reset(Matrix32()); (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); return *B_; } @@ -104,7 +104,7 @@ void Unit3::print(const std::string& s) const { } /* ************************************************************************* */ -Matrix Unit3::skew() const { +Matrix3 Unit3::skew() const { return skewSymmetric(p_.x(), p_.y(), p_.z()); } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index d04e57d4b..a906302af 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -32,8 +32,6 @@ class GTSAM_EXPORT Unit3{ private: - typedef Eigen::Matrix Matrix32; - Point3 p_; ///< The location of the point on the unit sphere mutable boost::optional B_; ///< Cached basis @@ -90,10 +88,10 @@ public: const Matrix32& basis() const; /// Return skew-symmetric associated with 3D point on unit sphere - Matrix skew() const; + Matrix3 skew() const; /// Return unit-norm Point3 - const Point3& point3(boost::optional H = boost::none) const { + const Point3& point3(OptionalJacobian<3,2> H = boost::none) const { if (H) *H = basis(); return p_; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 88accb90f..62edd7ea7 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -359,7 +359,7 @@ TEST( Rot3, inverse ) Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 I; - Matrix actualH; + Matrix3 actualH; Rot3 actual = R.inverse(actualH); CHECK(assert_equal(I,R*actual)); CHECK(assert_equal(I,actual*R)); From 22bbde6fe02b4a95af0ad676f2192c31fc2747db Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Tue, 2 Dec 2014 12:40:18 -0500 Subject: [PATCH 50/76] completed all calibration files --- gtsam/geometry/Cal3Bundler.cpp | 26 +++++++++++++++----------- gtsam/geometry/Cal3Bundler.h | 10 +++++----- gtsam/geometry/Cal3DS2_Base.cpp | 12 +++++++----- gtsam/geometry/Cal3DS2_Base.h | 6 +++--- gtsam/geometry/Cal3Unified.cpp | 5 ++--- gtsam/geometry/Cal3Unified.h | 4 ++-- gtsam/geometry/Cal3_S2.cpp | 2 +- gtsam/geometry/Cal3_S2.h | 16 ++++++++++------ gtsam/geometry/CalibratedCamera.cpp | 29 ++++++++++++++++------------- gtsam/geometry/CalibratedCamera.h | 23 +++++++++++------------ 10 files changed, 72 insertions(+), 61 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index fd3392b67..d6a20b3db 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -34,15 +34,17 @@ Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) : } /* ************************************************************************* */ -Matrix Cal3Bundler::K() const { +Matrix3 Cal3Bundler::K() const { Matrix3 K; K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1; return K; } /* ************************************************************************* */ -Vector Cal3Bundler::k() const { - return (Vector(4) << k1_, k2_, 0, 0).finished(); +Vector4 Cal3Bundler::k() const { + Vector4 rvalue_; + rvalue_ << k1_, k2_, 0, 0; + return rvalue_; } /* ************************************************************************* */ @@ -120,25 +122,27 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { } /* ************************************************************************* */ -Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const { - Matrix Dp; +Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const { + Matrix2 Dp; uncalibrate(p, boost::none, Dp); return Dp; } /* ************************************************************************* */ -Matrix Cal3Bundler::D2d_calibration(const Point2& p) const { - Matrix Dcal; +Matrix23 Cal3Bundler::D2d_calibration(const Point2& p) const { + Matrix23 Dcal; uncalibrate(p, Dcal, boost::none); return Dcal; } /* ************************************************************************* */ -Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { - Matrix Dcal, Dp; +Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { + Matrix23 Dcal; + Matrix2 Dp; uncalibrate(p, Dcal, Dp); - Matrix H(2, 5); - H << Dp, Dcal; + Matrix25 H; + H.block<2,2>(0,0) = Dp; + H.block<2,3>(0,2) = Dcal; return H; } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index ed4f8b391..fc1d63e10 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -69,8 +69,8 @@ public: /// @name Standard Interface /// @{ - Matrix K() const; ///< Standard 3*3 calibration matrix - Vector k() const; ///< Radial distortion parameters (4 of them, 2 0) + Matrix3 K() const; ///< Standard 3*3 calibration matrix + Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0) Vector3 vector() const; @@ -120,13 +120,13 @@ public: Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; /// @deprecated might be removed in next release, use uncalibrate - Matrix D2d_intrinsic(const Point2& p) const; + Matrix2 D2d_intrinsic(const Point2& p) const; /// @deprecated might be removed in next release, use uncalibrate - Matrix D2d_calibration(const Point2& p) const; + Matrix23 D2d_calibration(const Point2& p) const; /// @deprecated might be removed in next release, use uncalibrate - Matrix D2d_intrinsic_calibration(const Point2& p) const; + Matrix25 D2d_intrinsic_calibration(const Point2& p) const; /// @} /// @name Manifold diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index 1830d56a1..cfbce2b28 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -28,8 +28,10 @@ Cal3DS2_Base::Cal3DS2_Base(const Vector &v): fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){} /* ************************************************************************* */ -Matrix Cal3DS2_Base::K() const { - return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished(); +Matrix3 Cal3DS2_Base::K() const { + Matrix3 K; + K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; + return K; } /* ************************************************************************* */ @@ -39,7 +41,7 @@ Vector Cal3DS2_Base::vector() const { /* ************************************************************************* */ void Cal3DS2_Base::print(const std::string& s_) const { - gtsam::print(K(), s_ + ".K"); + gtsam::print((Matrix)K(), s_ + ".K"); gtsam::print(Vector(k()), s_ + ".k"); } @@ -156,7 +158,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { } /* ************************************************************************* */ -Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { +Matrix2 Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { const double x = p.x(), y = p.y(), xx = x * x, yy = y * y; const double rr = xx + yy; const double r4 = rr * rr; @@ -167,7 +169,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { } /* ************************************************************************* */ -Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const { +Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const { const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y; const double rr = xx + yy; const double r4 = rr * rr; diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 1b2143278..f9292d4f6 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -45,7 +45,7 @@ protected: double p1_, p2_ ; // tangential distortion public: - Matrix K() const ; + Matrix3 K() const ; Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); } Vector vector() const ; @@ -121,10 +121,10 @@ public: Point2 calibrate(const Point2& p, const double tol=1e-5) const; /// Derivative of uncalibrate wrpt intrinsic coordinates - Matrix D2d_intrinsic(const Point2& p) const ; + Matrix2 D2d_intrinsic(const Point2& p) const ; /// Derivative of uncalibrate wrpt the calibration parameters - Matrix D2d_calibration(const Point2& p) const ; + Matrix29 D2d_calibration(const Point2& p) const ; private: diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index 6bc4c4bb3..8e6c219bc 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -52,8 +52,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { /* ************************************************************************* */ // todo: make a fixed sized jacobian version of this Point2 Cal3Unified::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { + OptionalJacobian<2,10> H1, + OptionalJacobian<2,2> H2) const { // this part of code is modified from Cal3DS2, // since the second part of this model (after project to normalized plane) @@ -82,7 +82,6 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, DU << -xs * sqrt_nx * xi_sqrt_nx2, // -ys * sqrt_nx * xi_sqrt_nx2; - H1->resize(2,10); H1->block<2,9>(0,0) = H1base; H1->block<2,1>(0,9) = H2base * DU; } diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index f65b27780..133e330ba 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -96,8 +96,8 @@ public: * @return point in image coordinates */ Point2 uncalibrate(const Point2& p, - boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const ; + OptionalJacobian<2,10> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const ; /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& p, const double tol=1e-5) const; diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index d0589cc65..3ec29bbd2 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -53,7 +53,7 @@ Cal3_S2::Cal3_S2(const std::string &path) : /* ************************************************************************* */ void Cal3_S2::print(const std::string& s) const { - gtsam::print(matrix(), s); + gtsam::print((Matrix)matrix(), s); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 261383757..c15ce6b74 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -125,20 +125,24 @@ public: } /// return calibration matrix K - Matrix K() const { - return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished(); + Matrix3 K() const { + Matrix3 K; + K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; + return K; } /** @deprecated The following function has been deprecated, use K above */ - Matrix matrix() const { + Matrix3 matrix() const { return K(); } /// return inverted calibration matrix inv(K) - Matrix matrix_inverse() const { + Matrix3 matrix_inverse() const { const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_; - return (Matrix(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, - 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0).finished(); + Matrix3 K_inverse; + K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, + 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0; + return K_inverse; } /** diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 392a53858..88cbc4939 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -33,10 +33,10 @@ CalibratedCamera::CalibratedCamera(const Vector &v) : /* ************************************************************************* */ Point2 CalibratedCamera::project_to_camera(const Point3& P, - boost::optional H1) { + OptionalJacobian<2,3> H1) { if (H1) { double d = 1.0 / P.z(), d2 = d * d; - *H1 = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2).finished(); + (*H1) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2; } return Point2(P.x() / P.z(), P.y() / P.z()); } @@ -59,10 +59,12 @@ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { /* ************************************************************************* */ Point2 CalibratedCamera::project(const Point3& point, - boost::optional Dpose, boost::optional Dpoint) const { + OptionalJacobian<2,6> Dpose, OptionalJacobian<2,3> Dpoint) const { #ifdef CALIBRATEDCAMERA_CHAIN_RULE - Point3 q = pose_.transform_to(point, Dpose, Dpoint); + Matrix36 Dpose_; + Matrix3 Dpoint_; + Point3 q = pose_.transform_to(point, Dpose_, Dpoint_); #else Point3 q = pose_.transform_to(point); #endif @@ -75,23 +77,24 @@ Point2 CalibratedCamera::project(const Point3& point, if (Dpose || Dpoint) { #ifdef CALIBRATEDCAMERA_CHAIN_RULE // just implement chain rule - Matrix H; + Matrix23 H; project_to_camera(q,H); - if (Dpose) *Dpose = H * (*Dpose); - if (Dpoint) *Dpoint = H * (*Dpoint); + if (Dpose) *Dpose = H * (*Dpose_); + if (Dpoint) *Dpoint = H * (*Dpoint_); #else // optimized version, see CalibratedCamera.nb const double z = q.z(), d = 1.0 / z; const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v; if (Dpose) - *Dpose = (Matrix(2, 6) << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), - -uv, -u, 0., -d, d * v).finished(); + *Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), + -uv, -u, 0., -d, d * v; if (Dpoint) { - const Matrix R(pose_.rotation().matrix()); - *Dpoint = d - * (Matrix(2, 3) << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), + const Matrix3 R(pose_.rotation().matrix()); + Matrix23 Dpoint_; + Dpoint_ << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2), - R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2)).finished(); + R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); + *Dpoint = d * Dpoint_; } #endif } diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index c66941091..5741d71b3 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -90,20 +90,20 @@ public: /// compose the two camera poses: TODO Frank says this might not make sense inline const CalibratedCamera compose(const CalibratedCamera &c, - boost::optional H1 = boost::none, boost::optional H2 = + OptionalJacobian<6,6> H1=boost::none, OptionalJacobian<6,6> H2 = boost::none) const { return CalibratedCamera(pose_.compose(c.pose(), H1, H2)); } /// between the two camera poses: TODO Frank says this might not make sense inline const CalibratedCamera between(const CalibratedCamera& c, - boost::optional H1 = boost::none, boost::optional H2 = + OptionalJacobian<6,6> H1 = boost::none, OptionalJacobian<6,6> H2 = boost::none) const { return CalibratedCamera(pose_.between(c.pose(), H1, H2)); } /// invert the camera pose: TODO Frank says this might not make sense - inline const CalibratedCamera inverse(boost::optional H1 = + inline const CalibratedCamera inverse(OptionalJacobian<6,6> H1 = boost::none) const { return CalibratedCamera(pose_.inverse(H1)); } @@ -152,8 +152,7 @@ public: * @return the intrinsic coordinates of the projected point */ Point2 project(const Point3& point, - boost::optional Dpose = boost::none, - boost::optional Dpoint = boost::none) const; + OptionalJacobian<2,6> Dpose=boost::none, OptionalJacobian<2,3> Dpoint=boost::none) const; /** * projects a 3-dimensional point in camera coordinates into the @@ -161,7 +160,7 @@ public: * With optional 2by3 derivative */ static Point2 project_to_camera(const Point3& cameraPoint, - boost::optional H1 = boost::none); + OptionalJacobian<2, 3> H1 = boost::none); /** * backproject a 2-dimensional point to a 3-dimension point @@ -175,8 +174,8 @@ public: * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Point3& point, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + double range(const Point3& point, OptionalJacobian<1,6> H1 = boost::none, + OptionalJacobian<1,3> H2 = boost::none) const { return pose_.range(point, H1, H2); } @@ -187,8 +186,8 @@ public: * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Pose3& pose, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + double range(const Pose3& pose, OptionalJacobian<1,6> H1=boost::none, + OptionalJacobian<1,6> H2=boost::none) const { return pose_.range(pose, H1, H2); } @@ -199,8 +198,8 @@ public: * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const CalibratedCamera& camera, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { + double range(const CalibratedCamera& camera, OptionalJacobian<1,6> H1 = + boost::none, OptionalJacobian<1,6> H2 = boost::none) const { return pose_.range(camera.pose_, H1, H2); } From 7138263d85f3100bd2ed90461876d53bbf26a3ac Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Tue, 2 Dec 2014 13:49:03 -0500 Subject: [PATCH 51/76] completed essentialmatrix. --- gtsam/geometry/CalibratedCamera.cpp | 12 +++++--- gtsam/geometry/EssentialMatrix.cpp | 47 +++++++++++++++-------------- gtsam/geometry/EssentialMatrix.h | 14 ++++----- gtsam/geometry/Unit3.cpp | 3 +- gtsam/geometry/Unit3.h | 2 +- 5 files changed, 40 insertions(+), 38 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 88cbc4939..da246f23f 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -64,7 +64,7 @@ Point2 CalibratedCamera::project(const Point3& point, #ifdef CALIBRATEDCAMERA_CHAIN_RULE Matrix36 Dpose_; Matrix3 Dpoint_; - Point3 q = pose_.transform_to(point, Dpose_, Dpoint_); + Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0); #else Point3 q = pose_.transform_to(point); #endif @@ -77,10 +77,12 @@ Point2 CalibratedCamera::project(const Point3& point, if (Dpose || Dpoint) { #ifdef CALIBRATEDCAMERA_CHAIN_RULE // just implement chain rule - Matrix23 H; - project_to_camera(q,H); - if (Dpose) *Dpose = H * (*Dpose_); - if (Dpoint) *Dpoint = H * (*Dpoint_); + if(Dpose && Dpoint) { + Matrix23 H; + project_to_camera(q,H); + if (Dpose) *Dpose = H * (*Dpose_); + if (Dpoint) *Dpoint = H * (*Dpoint_); + } #else // optimized version, see CalibratedCamera.nb const double z = q.z(), d = 1.0 / z; diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index e65e5d097..aa1dee352 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -14,7 +14,7 @@ namespace gtsam { /* ************************************************************************* */ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, - boost::optional H) { + OptionalJacobian<5,6> H) { const Rot3& _1R2_ = _1P2_.rotation(); const Point3& _1T2_ = _1P2_.translation(); if (!H) { @@ -25,13 +25,12 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, // Calculate the 5*6 Jacobian H = D_E_1P2 // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation // First get 2*3 derivative from Unit3::FromPoint3 - Matrix D_direction_1T2; + Matrix23 D_direction_1T2; Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); - H->resize(5, 6); - H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left - H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left - H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right - H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right + H->block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); // upper left + H->block<2, 3>(3, 0).setZero(); // lower left + H->block<3, 3>(0, 3).setZero(); // upper right + H->block<2, 3>(3, 3) = D_direction_1T2 * _1R2_.matrix(); // lower right return EssentialMatrix(_1R2_, direction); } } @@ -61,16 +60,18 @@ Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { /* ************************************************************************* */ Point3 EssentialMatrix::transform_to(const Point3& p, - boost::optional DE, boost::optional Dpoint) const { + OptionalJacobian<3,5> DE, OptionalJacobian<3,3> Dpoint) const { Pose3 pose(aRb_, aTb_.point3()); - Point3 q = pose.transform_to(p, DE, Dpoint); + Matrix36 DE_; + Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint); if (DE) { // DE returned by pose.transform_to is 3*6, but we need it to be 3*5 // The last 3 columns are derivative with respect to change in translation // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() // Duy made an educated guess that this needs to be rotated to the local frame - Matrix H(3, 5); - H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis(); + Matrix35 H; + H.block<3,3>(0,0) = DE_.block<3, 3>(0, 0); + H.block<3,2>(0,3) = -aRb_.transpose() * aTb_.basis(); *DE = H; } return q; @@ -78,7 +79,7 @@ Point3 EssentialMatrix::transform_to(const Point3& p, /* ************************************************************************* */ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, - boost::optional HE, boost::optional HR) const { + OptionalJacobian<5,5> HE, OptionalJacobian<5,3> HR) const { // The rotation must be conjugated to act in the camera frame Rot3 c1Rc2 = aRb_.conjugate(cRb); @@ -89,18 +90,18 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, return EssentialMatrix(c1Rc2, c1Tc2); } else { // Calculate derivatives - Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2 + Matrix23 D_c1Tc2_cRb; // 2*3 + Matrix2 D_c1Tc2_aTb; // 2*2 Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); if (HE) { - *HE = zeros(5, 5); - HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2 - HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2) + HE->setZero(); + HE->block<3, 3>(0, 0) = cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2 + HE->block<2, 2>(3, 3) = D_c1Tc2_aTb; // (2*2) } if (HR) { throw runtime_error( "EssentialMatrix::rotate: derivative HR not implemented yet"); /* - HR->resize(5, 3); HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ? HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ? */ @@ -110,15 +111,15 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, } /* ************************************************************************* */ -double EssentialMatrix::error(const Vector& vA, const Vector& vB, // - boost::optional H) const { +double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // + OptionalJacobian<1,5> H) const { if (H) { - H->resize(1, 5); // See math.lyx - Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) + Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); + Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) * aTb_.basis(); - *H << HR, HD; + H->block<1,3>(0,0) = HR; + H->block<1,2>(0,3) = HD; } return dot(vA, E_ * vB); } diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index fd5f45160..b25286412 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -50,7 +50,7 @@ public: /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) static EssentialMatrix FromPose3(const Pose3& _1P2_, - boost::optional H = boost::none); + OptionalJacobian<5, 6> H = boost::none); /// Random, using Rot3::Random and Unit3::Random template @@ -132,16 +132,16 @@ public: * @return point in pose coordinates */ Point3 transform_to(const Point3& p, - boost::optional DE = boost::none, - boost::optional Dpoint = boost::none) const; + OptionalJacobian<3,5> DE = boost::none, + OptionalJacobian<3,3> Dpoint = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C * @param cRb rotation from body frame to camera frame * @param E essential matrix E in camera frame C */ - EssentialMatrix rotate(const Rot3& cRb, boost::optional HE = - boost::none, boost::optional HR = boost::none) const; + EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE = + boost::none, OptionalJacobian<5, 3> HR = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -153,8 +153,8 @@ public: } /// epipolar error, algebraic - double error(const Vector& vA, const Vector& vB, // - boost::optional H = boost::none) const; + double error(const Vector3& vA, const Vector3& vB, // + OptionalJacobian<1,5> H = boost::none) const; /// @} diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index daff6b00b..137d96a06 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -39,14 +39,13 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Unit3 Unit3::FromPoint3(const Point3& point, boost::optional H) { +Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { Unit3 direction(point); if (H) { // 3*3 Derivative of representation with respect to point is 3*3: Matrix D_p_point; point.normalize(D_p_point); // TODO, this calculates norm a second time :-( // Calculate the 2*3 Jacobian - H->resize(2, 3); *H << direction.basis().transpose() * D_p_point; } return direction; diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index a906302af..9b79dcd18 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -57,7 +57,7 @@ public: } /// Named constructor from Point3 with optional Jacobian - static Unit3 FromPoint3(const Point3& point, boost::optional H = + static Unit3 FromPoint3(const Point3& point, OptionalJacobian<2,3> H = boost::none); /// Random direction, using boost::uniform_on_sphere From a8c15103433f3475c0930103a813df2e7788045f Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Tue, 2 Dec 2014 14:20:23 -0500 Subject: [PATCH 52/76] fixed all. Cant deal with conservative resize becaue this is dependent on the template dimension. Maybe OptionalJacobian<3,dimK> or somehting like that ? --- gtsam/geometry/PinholeCamera.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index a70bb2301..ca6c64bb9 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -428,8 +428,8 @@ public: */ double range( const Point3& point, // - boost::optional Dpose = boost::none, - boost::optional Dpoint = boost::none) const { + OptionalJacobian<1,6> Dpose = boost::none, + OptionalJacobian<1,3> Dpoint = boost::none) const { double result = pose_.range(point, Dpose, Dpoint); if (Dpose) { // Add columns of zeros to Jacobian for calibration @@ -500,8 +500,8 @@ public: */ double range( const CalibratedCamera& camera, // - boost::optional Dpose = boost::none, - boost::optional Dother = boost::none) const { + OptionalJacobian<1,6> Dpose = boost::none, + OptionalJacobian<1,6> Dother = boost::none) const { return pose_.range(camera.pose_, Dpose, Dother); } From 4e557d38e68b44d7bdf967b6aee174e7c349146e Mon Sep 17 00:00:00 2001 From: Natesh Srinivasan Date: Wed, 3 Dec 2014 09:59:10 -0500 Subject: [PATCH 53/76] updated Matrix.h with commonly used matrices. --- .gitignore | 1 + gtsam/base/Matrix.h | 42 +++++++++++++++++++++++++++++ gtsam/geometry/Cal3_S2.h | 4 +-- gtsam/geometry/CalibratedCamera.cpp | 2 +- gtsam/geometry/EssentialMatrix.cpp | 4 +-- gtsam/geometry/Point2.cpp | 1 - gtsam/geometry/Point2.h | 8 +++--- gtsam/geometry/Point3.cpp | 8 +++--- gtsam/geometry/Point3.h | 4 +-- gtsam/geometry/Pose2.cpp | 1 - gtsam/geometry/Pose3.cpp | 12 ++++----- gtsam/geometry/Rot2.h | 8 +++--- gtsam/geometry/Rot3.cpp | 10 +++---- gtsam/geometry/Rot3M.cpp | 4 +-- gtsam/geometry/Unit3.cpp | 33 ++++++++++++----------- gtsam/geometry/Unit3.h | 15 +++++++---- gtsam/geometry/tests/testRot3.cpp | 3 +-- gtsam/geometry/tests/testRot3M.cpp | 1 - gtsam/geometry/tests/testUnit3.cpp | 6 ++--- gtsam_unstable/slam/AHRS.cpp | 3 --- 20 files changed, 103 insertions(+), 67 deletions(-) diff --git a/.gitignore b/.gitignore index 9276d2f30..f3f1efd5b 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,4 @@ /examples/Data/pose2example-rewritten.txt /examples/Data/pose3example-rewritten.txt *.txt.user +*.txt.user.6d59f0c diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 47203f9b5..37f446234 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -76,6 +76,48 @@ typedef Eigen::Matrix Matrix39; typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; + +// Identity Matrices +static const int I1 = 1; +static const Matrix2 I2 = Matrix2::Identity(); +static const Matrix3 I3 = Matrix3::Identity(); +static const Matrix4 I4 = Matrix4::Identity(); +static const Matrix5 I5 = Matrix5::Identity(); +static const Matrix6 I6 = Matrix6::Identity(); + +// Negative Identity +static const Matrix2 _I2 = -I2; +static const Matrix3 _I3 = -I3; +static const Matrix4 _I4 = -I4; +static const Matrix5 _I5 = -I5; +static const Matrix6 _I6 = -I6; + +// Zero matrices +// TODO : Make these for rectangular sized matrices as well. +static const int Z1 = 0; +static const Matrix2 Z2 = Matrix2::Zero(); +static const Matrix3 Z3 = Matrix3::Zero(); +static const Matrix4 Z4 = Matrix4::Zero(); +static const Matrix5 Z5 = Matrix5::Zero(); +static const Matrix6 Z6 = Matrix6::Zero(); + +// Ones matrices +// TODO : Make these for rectangular sized matrices as well. +static const int O1 = 1; +static const Matrix2 O2 = Matrix2::Ones(); +static const Matrix3 O3 = Matrix3::Ones(); +static const Matrix4 O4 = Matrix4::Ones(); +static const Matrix5 O5 = Matrix5::Ones(); +static const Matrix6 O6 = Matrix6::Ones(); + +// Negative Ones +static const Matrix2 _O2 = -O2; +static const Matrix3 _O3 = -O3; +static const Matrix4 _O4 = -O4; +static const Matrix5 _O5 = -O5; +static const Matrix6 _O6 = -O6; + + // Matlab-like syntax /** diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index c15ce6b74..aa4a2e2e8 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -173,8 +173,8 @@ public: inline Cal3_S2 between(const Cal3_S2& q, OptionalJacobian<5,5> H1=boost::none, OptionalJacobian<5,5> H2=boost::none) const { - if(H1) *H1 = -Matrix5::Identity(); - if(H2) *H2 = Matrix5::Identity(); + if(H1) *H1 = _I5; + if(H2) *H2 = I5; return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_); } diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index da246f23f..1f5f1f8a5 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -36,7 +36,7 @@ Point2 CalibratedCamera::project_to_camera(const Point3& P, OptionalJacobian<2,3> H1) { if (H1) { double d = 1.0 / P.z(), d2 = d * d; - (*H1) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2; + *H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2; } return Point2(P.x() / P.z(), P.y() / P.z()); } diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index aa1dee352..57367a494 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -27,9 +27,9 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, // First get 2*3 derivative from Unit3::FromPoint3 Matrix23 D_direction_1T2; Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); - H->block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); // upper left + H->block<3, 3>(0, 0) << I3; // upper left H->block<2, 3>(3, 0).setZero(); // lower left - H->block<3, 3>(0, 3).setZero(); // upper right + H->block<3, 3>(0, 3) << Z3; // upper right H->block<2, 3>(3, 3) = D_direction_1T2 * _1R2_.matrix(); // lower right return EssentialMatrix(_1R2_, direction); } diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 68022e201..17e3ef370 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -50,7 +50,6 @@ double Point2::norm(OptionalJacobian<1,2> H) const { } /* ************************************************************************* */ -static const Matrix2 I2 = Eigen::Matrix2d::Identity(); double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1, OptionalJacobian<1,2> H2) const { Point2 d = point - *this; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index a0b185b63..a98dd3e0e 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -127,8 +127,8 @@ public: inline Point2 compose(const Point2& q, OptionalJacobian<2,2> H1=boost::none, OptionalJacobian<2,2> H2=boost::none) const { - if(H1) *H1 = Eigen::Matrix2d::Identity(); - if(H2) *H2 = Eigen::Matrix2d::Identity(); + if(H1) *H1 = I2; + if(H2) *H2 = I2; return *this + q; } @@ -139,8 +139,8 @@ public: inline Point2 between(const Point2& q, OptionalJacobian<2,2> H1=boost::none, OptionalJacobian<2,2> H2=boost::none) const { - if(H1) *H1 = -Eigen::Matrix2d::Identity(); - if(H2) *H2 = Eigen::Matrix2d::Identity(); + if(H1) *H1 = _I2; + if(H2) *H2 = I2; return q - (*this); } diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 34f939635..bd686f142 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -66,9 +66,9 @@ Point3 Point3::operator/(double s) const { Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) - (*H1).setIdentity(); + *H1= I3; if (H2) - (*H2).setIdentity(); + *H2= I3; return *this + q; } @@ -76,9 +76,9 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) - (*H1).setIdentity(); + (*H1) = I3; if (H2) - (*H2).setIdentity(); + (*H2) = I3; return *this - q; } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 410cd0e12..c8e8eb5b3 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -143,12 +143,12 @@ namespace gtsam { /// Left-trivialized derivative of the exponential map static Matrix3 dexpL(const Vector& v) { - return Eigen::Matrix3d::Identity(); + return I3; } /// Left-trivialized derivative inverse of the exponential map static Matrix3 dexpInvL(const Vector& v) { - return Eigen::Matrix3d::Identity(); + return I3; } /// @} diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index af0d330b8..d8da104fa 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -33,7 +33,6 @@ INSTANTIATE_LIE(Pose2); /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose2); -static const Matrix3 I3 = Eigen::Matrix3d::Identity(); static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 1eb4c8db5..0b47df058 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -32,8 +32,6 @@ INSTANTIATE_LIE(Pose3); /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3); -static const Matrix3 I3 = Eigen::Matrix3d::Identity(), Z3 = Eigen::Matrix3d::Zero(), _I3 = -I3; - /* ************************************************************************* */ Pose3::Pose3(const Pose2& pose2) : R_(Rot3::rodriguez(0, 0, pose2.theta())), t_( @@ -102,9 +100,9 @@ Matrix6 Pose3::dExpInv_exp(const Vector6& xi) { 0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished(); static const int N = 5; // order of approximation Matrix6 res; - res.setIdentity(); + res = I6; Matrix6 ad_i; - ad_i.setIdentity(); + ad_i = I6; Matrix6 ad_xi = adjointMap(xi); double fac = 1.0; for (int i = 1; i < N; ++i) { @@ -279,7 +277,7 @@ Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1, if (H1) *H1 = p2.inverse().AdjointMap(); if (H2) - (*H2).setIdentity(); + *H2 = I6; return (*this) * p2; } @@ -299,7 +297,7 @@ Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1, if (H1) *H1 = -result.inverse().AdjointMap(); if (H2) - (*H2).setIdentity(); + (*H2) = I6; return result; } @@ -366,7 +364,7 @@ boost::optional align(const vector& pairs) { // Recover transform with correction from Eggert97machinevisionandapplications Matrix3 UVtranspose = U * V.transpose(); - Matrix3 detWeighting = Eigen::Matrix3d::Identity(); + Matrix3 detWeighting = I3; detWeighting(2, 2) = UVtranspose.determinant(); Rot3 R(Matrix(V * detWeighting * U.transpose())); Point3 t = Point3(cq) - R * Point3(cp); diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 558dd06f6..9da4724fb 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -118,8 +118,8 @@ namespace gtsam { /** Compose - make a new rotation by adding angles */ inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 = boost::none, OptionalJacobian<1,1> H2 = boost::none) const { - if (H1) (*H1)<< 1; // set to 1x1 identity matrix - if (H2) (*H2)<< 1; // set to 1x1 identity matrix + if (H1) *H1 << I1; // set to 1x1 identity matrix + if (H2) *H2 << I1; // set to 1x1 identity matrix return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); } @@ -131,8 +131,8 @@ namespace gtsam { /** Between using the default implementation */ inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 = boost::none, OptionalJacobian<1,1> H2 = boost::none) const { - if (H1) *H1 << -1; // set to 1x1 identity matrix - if (H2) *H2 << 1; // set to 1x1 identity matrix + if (H1) *H1 << -I1; // set to 1x1 identity matrix + if (H2) *H2 << I1; // set to 1x1 identity matrix return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_); } diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index db82c6f1e..52f2fdd7a 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -27,8 +27,6 @@ using namespace std; namespace gtsam { -static const Matrix3 I3 = Matrix3::Identity(); - /* ************************************************************************* */ void Rot3::print(const std::string& s) const { gtsam::print((Matrix)matrix(), s); @@ -186,11 +184,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) { double normx = norm_2(x); // rotation angle Matrix3 Jr; if (normx < 10e-8){ - Jr = Matrix3::Identity(); + Jr = I3; } else{ const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + + Jr = I3 - ((1-cos(normx))/(normx*normx)) * X + ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian } return Jr; @@ -203,11 +201,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) { Matrix3 Jrinv; if (normx < 10e-8){ - Jrinv = Matrix3::Identity(); + Jrinv = I3; } else{ const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jrinv = Matrix3::Identity() + + Jrinv = I3 + 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; } return Jrinv; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index d657c7287..a7512148d 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -30,10 +30,8 @@ using namespace std; namespace gtsam { -static const Matrix3 I3 = Matrix3::Identity(); - /* ************************************************************************* */ -Rot3::Rot3() : rot_(Matrix3::Identity()) {} +Rot3::Rot3() : rot_(I3) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 137d96a06..7471dca9d 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -43,7 +43,7 @@ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { Unit3 direction(point); if (H) { // 3*3 Derivative of representation with respect to point is 3*3: - Matrix D_p_point; + Matrix3 D_p_point; point.normalize(D_p_point); // TODO, this calculates norm a second time :-( // Calculate the 2*3 Jacobian *H << direction.basis().transpose() * D_p_point; @@ -108,33 +108,34 @@ Matrix3 Unit3::skew() const { } /* ************************************************************************* */ -Vector Unit3::error(const Unit3& q, boost::optional H) const { +Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 - Matrix Bt = basis().transpose(); - Vector xi = Bt * q.p_.vector(); + Matrix23 Bt = basis().transpose(); + Vector2 xi = Bt * q.p_.vector(); if (H) *H = Bt * q.basis(); return xi; } /* ************************************************************************* */ -double Unit3::distance(const Unit3& q, boost::optional H) const { - Vector xi = error(q, H); +double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { + Matrix2 H_; + Vector2 xi = error(q, H_); double theta = xi.norm(); if (H) - *H = (xi.transpose() / theta) * (*H); + *H = (xi.transpose() / theta) * H_; return theta; } /* ************************************************************************* */ -Unit3 Unit3::retract(const Vector& v) const { +Unit3 Unit3::retract(const Vector2& v) const { // Get the vector form of the point and the basis matrix - Vector p = Point3::Logmap(p_); - Matrix B = basis(); + Vector3 p = Point3::Logmap(p_); + Matrix32 B = basis(); // Compute the 3D xi_hat vector - Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1); + Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1); double xi_hat_norm = xi_hat.norm(); @@ -146,17 +147,17 @@ Unit3 Unit3::retract(const Vector& v) const { return Unit3(-point3()); } - Vector exp_p_xi_hat = cos(xi_hat_norm) * p + Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); return Unit3(exp_p_xi_hat); } /* ************************************************************************* */ -Vector Unit3::localCoordinates(const Unit3& y) const { +Vector2 Unit3::localCoordinates(const Unit3& y) const { - Vector p = Point3::Logmap(p_); - Vector q = Point3::Logmap(y.p_); + Vector3 p = Point3::Logmap(p_); + Vector3 q = Point3::Logmap(y.p_); double dot = p.dot(q); // Check for special cases @@ -167,7 +168,7 @@ Vector Unit3::localCoordinates(const Unit3& y) const { else { // no special case double theta = acos(dot); - Vector result_hat = (theta / sin(theta)) * (q - p * dot); + Vector3 result_hat = (theta / sin(theta)) * (q - p * dot); return basis().transpose() * result_hat; } } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 9b79dcd18..5dfa7375e 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -50,6 +50,11 @@ public: p_(p / p.norm()) { } + /// Construct from a vector3 + explicit Unit3(const Vector3& p) : + p_(p / p.norm()) { + } + /// Construct from x,y,z Unit3(double x, double y, double z) : p_(x, y, z) { @@ -103,12 +108,12 @@ public: } /// Signed, vector-valued error between two directions - Vector error(const Unit3& q, - boost::optional H = boost::none) const; + Vector2 error(const Unit3& q, + OptionalJacobian<2,2> H = boost::none) const; /// Distance between two directions double distance(const Unit3& q, - boost::optional H = boost::none) const; + OptionalJacobian<1,2> H = boost::none) const; /// @} @@ -131,10 +136,10 @@ public: }; /// The retract function - Unit3 retract(const Vector& v) const; + Unit3 retract(const Vector2& v) const; /// The local coordinates function - Vector localCoordinates(const Unit3& s) const; + Vector2 localCoordinates(const Unit3& s) const; /// @} diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 62edd7ea7..4e23ab744 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -37,7 +37,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3) static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); static Point3 P(0.2, 0.7, -2.0); static double error = 1e-9, epsilon = 0.001; -static const Matrix I3 = eye(3); /* ************************************************************************* */ TEST( Rot3, chart) @@ -578,7 +577,7 @@ TEST(Rot3, quaternion) { TEST( Rot3, Cayley ) { Matrix A = skewSymmetric(1,2,-3); Matrix Q = Cayley(A); - EXPECT(assert_equal(I3, trans(Q)*Q)); + EXPECT(assert_equal((Matrix)I3, trans(Q)*Q)); EXPECT(assert_equal(A, Cayley(Q))); } diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 1313a3be5..a25db07f6 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -37,7 +37,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3) static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); static Point3 P(0.2, 0.7, -2.0); -static const Matrix I3 = eye(3); /* ************************************************************************* */ TEST(Rot3, manifold_cayley) diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 0102477b3..072f3b7ad 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -108,9 +108,9 @@ TEST(Unit3, unrotate) { TEST(Unit3, error) { Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // r = p.retract(Vector2(0.8, 0)); - EXPECT(assert_equal(Vector2(0, 0), p.error(p), 1e-8)); - EXPECT(assert_equal(Vector2(0.479426, 0), p.error(q), 1e-5)); - EXPECT(assert_equal(Vector2(0.717356, 0), p.error(r), 1e-5)); + EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-8)); + EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5)); + EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5)); Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 2a756b5af..910c4f705 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -20,9 +20,6 @@ Matrix cov(const Matrix& m) { return DDt / (num_observations - 1); } -Matrix I3 = eye(3); -Matrix Z3 = zeros(3, 3); - /* ************************************************************************* */ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, bool flat) : From 7116661a2e12dab77557f43a1d62194a0c09d3e3 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Wed, 3 Dec 2014 10:57:42 -0500 Subject: [PATCH 54/76] changed naming convention of const matrices to _DxD. @dellaert --- gtsam/base/Matrix.h | 58 +++++++++++++++--------------- gtsam/geometry/Cal3_S2.h | 4 +-- gtsam/geometry/EssentialMatrix.cpp | 4 +-- gtsam/geometry/Point2.h | 8 ++--- gtsam/geometry/Point3.cpp | 8 ++--- gtsam/geometry/Point3.h | 4 +-- gtsam/geometry/Pose2.cpp | 4 +-- gtsam/geometry/Pose3.cpp | 14 ++++---- gtsam/geometry/Rot2.h | 8 ++--- gtsam/geometry/Rot3.cpp | 12 +++---- gtsam/geometry/Rot3M.cpp | 6 ++-- gtsam/geometry/tests/testRot3.cpp | 10 +++--- 12 files changed, 71 insertions(+), 69 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 37f446234..e48f641e3 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -78,44 +78,46 @@ typedef Eigen::Block ConstSubMatrix; // Identity Matrices -static const int I1 = 1; -static const Matrix2 I2 = Matrix2::Identity(); -static const Matrix3 I3 = Matrix3::Identity(); -static const Matrix4 I4 = Matrix4::Identity(); -static const Matrix5 I5 = Matrix5::Identity(); -static const Matrix6 I6 = Matrix6::Identity(); +static const int I_1x1 = 1; +static const Matrix2 I_2x2 = Matrix2::Identity(); +static const Matrix3 I_3x3 = Matrix3::Identity(); +static const Matrix4 I_4x4 = Matrix4::Identity(); +static const Matrix5 I_5x5 = Matrix5::Identity(); +static const Matrix6 I_6x6 = Matrix6::Identity(); // Negative Identity -static const Matrix2 _I2 = -I2; -static const Matrix3 _I3 = -I3; -static const Matrix4 _I4 = -I4; -static const Matrix5 _I5 = -I5; -static const Matrix6 _I6 = -I6; +static const int _I_1x1 = -1; +static const Matrix2 _I_2x2 = -I_2x2; +static const Matrix3 _I_3x3 = -I_3x3; +static const Matrix4 _I_4x4 = -I_4x4; +static const Matrix5 _I_5x5 = -I_5x5; +static const Matrix6 _I_6x6 = -I_6x6; // Zero matrices // TODO : Make these for rectangular sized matrices as well. -static const int Z1 = 0; -static const Matrix2 Z2 = Matrix2::Zero(); -static const Matrix3 Z3 = Matrix3::Zero(); -static const Matrix4 Z4 = Matrix4::Zero(); -static const Matrix5 Z5 = Matrix5::Zero(); -static const Matrix6 Z6 = Matrix6::Zero(); +static const int Z_1x1 = 0; +static const Matrix2 Z_2x2 = Matrix2::Zero(); +static const Matrix3 Z_3x3 = Matrix3::Zero(); +static const Matrix4 Z_4x4 = Matrix4::Zero(); +static const Matrix5 Z_5x5 = Matrix5::Zero(); +static const Matrix6 Z_6x6 = Matrix6::Zero(); // Ones matrices // TODO : Make these for rectangular sized matrices as well. -static const int O1 = 1; -static const Matrix2 O2 = Matrix2::Ones(); -static const Matrix3 O3 = Matrix3::Ones(); -static const Matrix4 O4 = Matrix4::Ones(); -static const Matrix5 O5 = Matrix5::Ones(); -static const Matrix6 O6 = Matrix6::Ones(); +static const int O_1x1 = 1; +static const Matrix2 O_2x2 = Matrix2::Ones(); +static const Matrix3 O_3x3 = Matrix3::Ones(); +static const Matrix4 O_4x4 = Matrix4::Ones(); +static const Matrix5 O_5x5 = Matrix5::Ones(); +static const Matrix6 O_6x6 = Matrix6::Ones(); // Negative Ones -static const Matrix2 _O2 = -O2; -static const Matrix3 _O3 = -O3; -static const Matrix4 _O4 = -O4; -static const Matrix5 _O5 = -O5; -static const Matrix6 _O6 = -O6; +static const int _O_1x1 = -1; +static const Matrix2 _O_2x2 = -O_2x2; +static const Matrix3 _O_3x3 = -O_3x3; +static const Matrix4 _O_4x4 = -O_4x4; +static const Matrix5 _O_5x5 = -O_5x5; +static const Matrix6 _O_6x6 = -O_6x6; // Matlab-like syntax diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index aa4a2e2e8..61eafa283 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -173,8 +173,8 @@ public: inline Cal3_S2 between(const Cal3_S2& q, OptionalJacobian<5,5> H1=boost::none, OptionalJacobian<5,5> H2=boost::none) const { - if(H1) *H1 = _I5; - if(H2) *H2 = I5; + if(H1) *H1 = _I_5x5; + if(H2) *H2 = I_5x5; return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_); } diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 57367a494..be4bba33f 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -27,9 +27,9 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, // First get 2*3 derivative from Unit3::FromPoint3 Matrix23 D_direction_1T2; Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); - H->block<3, 3>(0, 0) << I3; // upper left + H->block<3, 3>(0, 0) << I_3x3; // upper left H->block<2, 3>(3, 0).setZero(); // lower left - H->block<3, 3>(0, 3) << Z3; // upper right + H->block<3, 3>(0, 3) << Z_3x3; // upper right H->block<2, 3>(3, 3) = D_direction_1T2 * _1R2_.matrix(); // lower right return EssentialMatrix(_1R2_, direction); } diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index a98dd3e0e..2491d92d1 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -127,8 +127,8 @@ public: inline Point2 compose(const Point2& q, OptionalJacobian<2,2> H1=boost::none, OptionalJacobian<2,2> H2=boost::none) const { - if(H1) *H1 = I2; - if(H2) *H2 = I2; + if(H1) *H1 = I_2x2; + if(H2) *H2 = I_2x2; return *this + q; } @@ -139,8 +139,8 @@ public: inline Point2 between(const Point2& q, OptionalJacobian<2,2> H1=boost::none, OptionalJacobian<2,2> H2=boost::none) const { - if(H1) *H1 = _I2; - if(H2) *H2 = I2; + if(H1) *H1 = _I_2x2; + if(H2) *H2 = I_2x2; return q - (*this); } diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index bd686f142..d82faf971 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -66,9 +66,9 @@ Point3 Point3::operator/(double s) const { Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) - *H1= I3; + *H1= I_3x3; if (H2) - *H2= I3; + *H2= I_3x3; return *this + q; } @@ -76,9 +76,9 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) - (*H1) = I3; + (*H1) = I_3x3; if (H2) - (*H2) = I3; + (*H2) = I_3x3; return *this - q; } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index c8e8eb5b3..33d9e47a5 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -143,12 +143,12 @@ namespace gtsam { /// Left-trivialized derivative of the exponential map static Matrix3 dexpL(const Vector& v) { - return I3; + return I_3x3; } /// Left-trivialized derivative inverse of the exponential map static Matrix3 dexpInvL(const Vector& v) { - return I3; + return I_3x3; } /// @} diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index d8da104fa..83895ad7c 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -149,7 +149,7 @@ Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { // TODO: inline and reuse? if(H1) *H1 = p2.inverse().AdjointMap(); - if(H2) *H2 = I3; + if(H2) *H2 = I_3x3; return (*this)*p2; } @@ -200,7 +200,7 @@ Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1, s, -c, dt2, 0.0, 0.0,-1.0; } - if (H2) *H2 = I3; + if (H2) *H2 = I_3x3; return Pose2(R,t); } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0b47df058..2e241a1e1 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -47,7 +47,7 @@ Matrix6 Pose3::AdjointMap() const { const Vector3 t = t_.vector(); Matrix3 A = skewSymmetric(t) * R; Matrix6 adj; - adj << R, Z3, A, R; + adj << R, Z_3x3, A, R; return adj; } @@ -56,7 +56,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) { Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); Matrix6 adj; - adj << w_hat, Z3, v_hat, w_hat; + adj << w_hat, Z_3x3, v_hat, w_hat; return adj; } @@ -100,9 +100,9 @@ Matrix6 Pose3::dExpInv_exp(const Vector6& xi) { 0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished(); static const int N = 5; // order of approximation Matrix6 res; - res = I6; + res = I_6x6; Matrix6 ad_i; - ad_i = I6; + ad_i = I_6x6; Matrix6 ad_xi = adjointMap(xi); double fac = 1.0; for (int i = 1; i < N; ++i) { @@ -277,7 +277,7 @@ Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1, if (H1) *H1 = p2.inverse().AdjointMap(); if (H2) - *H2 = I6; + *H2 = I_6x6; return (*this) * p2; } @@ -297,7 +297,7 @@ Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1, if (H1) *H1 = -result.inverse().AdjointMap(); if (H2) - (*H2) = I6; + (*H2) = I_6x6; return result; } @@ -364,7 +364,7 @@ boost::optional align(const vector& pairs) { // Recover transform with correction from Eggert97machinevisionandapplications Matrix3 UVtranspose = U * V.transpose(); - Matrix3 detWeighting = I3; + Matrix3 detWeighting = I_3x3; detWeighting(2, 2) = UVtranspose.determinant(); Rot3 R(Matrix(V * detWeighting * U.transpose())); Point3 t = Point3(cq) - R * Point3(cp); diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 9da4724fb..324332c9e 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -118,8 +118,8 @@ namespace gtsam { /** Compose - make a new rotation by adding angles */ inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 = boost::none, OptionalJacobian<1,1> H2 = boost::none) const { - if (H1) *H1 << I1; // set to 1x1 identity matrix - if (H2) *H2 << I1; // set to 1x1 identity matrix + if (H1) *H1 << I_1x1; // set to 1x1 identity matrix + if (H2) *H2 << I_1x1; // set to 1x1 identity matrix return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); } @@ -131,8 +131,8 @@ namespace gtsam { /** Between using the default implementation */ inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 = boost::none, OptionalJacobian<1,1> H2 = boost::none) const { - if (H1) *H1 << -I1; // set to 1x1 identity matrix - if (H2) *H2 << I1; // set to 1x1 identity matrix + if (H1) *H1 << _I_1x1; // set to 1x1 identity matrix + if (H2) *H2 << I_1x1; // set to 1x1 identity matrix return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_); } diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 52f2fdd7a..95e7f85ce 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -120,7 +120,7 @@ Matrix3 Rot3::dexpL(const Vector3& v) { double theta = v.norm(), vi = theta/2.0; double s1 = sin(vi)/vi; double s2 = (theta - sin(theta))/(theta*theta*theta); - Matrix3 res = I3 - 0.5*s1*s1*x + s2*x2; + Matrix3 res = I_3x3 - 0.5*s1*s1*x + s2*x2; return res; } @@ -132,7 +132,7 @@ Matrix3 Rot3::dexpInvL(const Vector3& v) { Matrix3 x2 = x*x; double theta = v.norm(), vi = theta/2.0; double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); - Matrix3 res = I3 + 0.5*x - s2*x2; + Matrix3 res = I_3x3 + 0.5*x - s2*x2; return res; } @@ -184,11 +184,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) { double normx = norm_2(x); // rotation angle Matrix3 Jr; if (normx < 10e-8){ - Jr = I3; + Jr = I_3x3; } else{ const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jr = I3 - ((1-cos(normx))/(normx*normx)) * X + + Jr = I_3x3 - ((1-cos(normx))/(normx*normx)) * X + ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian } return Jr; @@ -201,11 +201,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) { Matrix3 Jrinv; if (normx < 10e-8){ - Jrinv = I3; + Jrinv = I_3x3; } else{ const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jrinv = I3 + + Jrinv = I_3x3 + 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; } return Jrinv; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index a7512148d..efc5d6f8b 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -31,7 +31,7 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Rot3::Rot3() : rot_(I3) {} +Rot3::Rot3() : rot_(I_3x3) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { @@ -144,7 +144,7 @@ Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3 if (H1) *H1 = R2.transpose(); if (H2) - *H2 = I3; + *H2 = I_3x3; return *this * R2; } @@ -169,7 +169,7 @@ Rot3 Rot3::inverse(boost::optional H1) const { Rot3 Rot3::between (const Rot3& R2, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = -(R2.transpose()*rot_); - if (H2) *H2 = I3; + if (H2) *H2 = I_3x3; Matrix3 R12 = transpose()*R2.rot_; return Rot3(R12); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 4e23ab744..6c3f1a7e7 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -49,7 +49,7 @@ TEST( Rot3, chart) /* ************************************************************************* */ TEST( Rot3, constructor) { - Rot3 expected(I3); + Rot3 expected(I_3x3); Point3 r1(1,0,0), r2(0,1,0), r3(0,0,1); Rot3 actual(r1, r2, r3); CHECK(assert_equal(actual,expected)); @@ -94,7 +94,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) { double t = norm_2(w); Matrix J = skewSymmetric(w / t); if (t < 1e-5) return Rot3(); - Matrix R = I3 + sin(t) * J + (1.0 - cos(t)) * (J * J); + Matrix R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J); return R; } @@ -481,7 +481,7 @@ TEST( Rot3, RQ) Vector actual; boost::tie(actualK, actual) = RQ(R.matrix()); Vector expected = Vector3(0.14715, 0.385821, 0.231671); - CHECK(assert_equal(I3,actualK)); + CHECK(assert_equal(I_3x3,actualK)); CHECK(assert_equal(expected,actual,1e-6)); // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] @@ -515,7 +515,7 @@ TEST( Rot3, expmapStability ) { w(2), 0.0, -w(0), -w(1), w(0), 0.0 ).finished(); Matrix W2 = W*W; - Matrix Rmat = I3 + (1.0-theta2/6.0 + theta2*theta2/120.0 + Matrix Rmat = I_3x3 + (1.0-theta2/6.0 + theta2*theta2/120.0 - theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ; Rot3 expectedR( Rmat ); CHECK(assert_equal(expectedR, actualR, 1e-10)); @@ -577,7 +577,7 @@ TEST(Rot3, quaternion) { TEST( Rot3, Cayley ) { Matrix A = skewSymmetric(1,2,-3); Matrix Q = Cayley(A); - EXPECT(assert_equal((Matrix)I3, trans(Q)*Q)); + EXPECT(assert_equal((Matrix)I_3x3, trans(Q)*Q)); EXPECT(assert_equal(A, Cayley(Q))); } From deff8b1e25109349345c69aca976a4c2e393bf7e Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Wed, 3 Dec 2014 11:27:18 -0500 Subject: [PATCH 55/76] fixed the (*) to -> and code some more code beautification. --- gtsam/geometry/Point3.cpp | 8 ++++---- gtsam/geometry/Point3.h | 12 ++++++------ gtsam/geometry/Pose2.cpp | 8 ++++---- gtsam/geometry/Pose3.cpp | 12 ++++++------ gtsam/geometry/Rot2.cpp | 6 ++---- gtsam/geometry/Rot3.cpp | 12 ++++-------- 6 files changed, 26 insertions(+), 32 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index d82faf971..152b5566b 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -76,9 +76,9 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) - (*H1) = I_3x3; + *H1 = I_3x3; if (H2) - (*H2) = I_3x3; + *H2 = I_3x3; return *this - q; } @@ -112,8 +112,8 @@ Point3 Point3::normalize(OptionalJacobian<3,3> H) const { // 3*3 Derivative double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_; double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_; - (*H) << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; - (*H) /= pow(x2 + y2 + z2, 1.5); + *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; + *H /= pow(x2 + y2 + z2, 1.5); } return normalized; } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 33d9e47a5..0cd038d90 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -95,8 +95,8 @@ namespace gtsam { inline Point3 compose(const Point3& p2, OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const { - if (H1) (*H1).setIdentity(); - if (H2) (*H2).setIdentity(); + if (H1) H1->setIdentity(); + if (H2) H2->setIdentity(); return *this + p2; } @@ -107,8 +107,8 @@ namespace gtsam { inline Point3 between(const Point3& p2, OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const { - if(H1) (*H1) = (-Eigen::Matrix3d::Identity()); - if(H2) (*H2).setIdentity(); + if(H1) *H1 = _I_3x3; + if(H2) H2->setIdentity(); return p2 - *this; } @@ -167,12 +167,12 @@ namespace gtsam { double d = (p2 - *this).norm(); if (H1) { *H1 << x_-p2.x(), y_-p2.y(), z_-p2.z(); - (*H1) = (*H1) *(1./d); + *H1 = *H1 *(1./d); } if (H2) { *H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z(); - (*H2) = (*H2) *(1./d); + *H2 = *H2 *(1./d); } return d; } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 83895ad7c..bf6f02e17 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -163,8 +163,8 @@ Point2 Pose2::transform_from(const Point2& p, Matrix21 Drotate1; Drotate1 << -q.y(), q.x(); if (H1) { - (*H1).block<2,2>(0,0) = R; // [R R_{pi/2}q] - (*H1).block<2,1>(0,2) = Drotate1; + H1->block<2,2>(0,0) = R; // [R R_{pi/2}q] + H1->block<2,1>(0,2) = Drotate1; } if (H2) *H2 = R; // R } @@ -226,8 +226,8 @@ Rot2 Pose2::bearing(const Pose2& pose, Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0); if (H2) { Matrix12 H2_ = D2 * pose.r().matrix(); - (*H2).setZero(); - (*H2).block<1,2>(0,0) = H2_; + H2->setZero(); + H2->block<1,2>(0,0) = H2_; } return result; } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2e241a1e1..35abf39a2 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -65,13 +65,13 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) { Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, OptionalJacobian<6,6> H) { if (H) { - (*H).setZero(); + H->setZero(); for (int i = 0; i < 6; ++i) { Vector6 dxi; dxi.setZero(); dxi(i) = 1.0; Matrix6 Gi = adjointMap(dxi); - (*H).col(i) = Gi * y; + H->col(i) = Gi * y; } } return adjointMap(xi) * y; @@ -87,7 +87,7 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, dxi.setZero(); dxi(i) = 1.0; Matrix6 GTi = adjointMap(dxi).transpose(); - (*H).col(i) = GTi * y; + H->col(i) = GTi * y; } } return adjointMap(xi).transpose() * y; @@ -297,7 +297,7 @@ Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1, if (H1) *H1 = -result.inverse().AdjointMap(); if (H2) - (*H2) = I_6x6; + *H2 = I_6x6; return result; } @@ -327,8 +327,8 @@ double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1, double r = range(pose.translation(), H1, H2? &D2 : 0); if (H2) { Matrix13 H2_ = D2 * pose.rotation().matrix(); - (*H2).setZero(); - (*H2).block<1,3>(0,3) = H2_; + H2->setZero(); + H2->block<1,3>(0,3) = H2_; } return r; } diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index af8d701ec..13c22ddc1 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -102,13 +102,11 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) { double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); if(fabs(n) > 1e-5) { if (H) { - (*H) << -y / d2, x / d2; + *H << -y / d2, x / d2; } return Rot2::fromCosSin(x / n, y / n); } else { - if (H) { - (*H) << 0.0, 0.0; - } + if (H) *H << 0.0, 0.0; return Rot2(); } } diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 95e7f85ce..5d7a2a473 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -73,10 +73,8 @@ Unit3 Rot3::rotate(const Unit3& p, OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { Matrix32 Dp; Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0))); - if (Hp) - (*Hp) = q.basis().transpose() * matrix() * Dp; - if (HR) - (*HR) = -q.basis().transpose() * matrix() * p.skew(); + if (Hp) *Hp << q.basis().transpose() * matrix() * Dp; + if (HR) *HR = -q.basis().transpose() * matrix() * p.skew(); return q; } @@ -85,10 +83,8 @@ Unit3 Rot3::unrotate(const Unit3& p, OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { Matrix32 Dp; Unit3 q = Unit3(unrotate(p.point3(Dp))); - if (Hp) - (*Hp) = q.basis().transpose() * matrix().transpose () * Dp; - if (HR) - (*HR) = q.basis().transpose() * q.skew(); + if (Hp) *Hp << q.basis().transpose() * matrix().transpose () * Dp; + if (HR) *HR = q.basis().transpose() * q.skew(); return q; } From aad0b2876b023d60d461a054205a59c234c42140 Mon Sep 17 00:00:00 2001 From: Natesh Srinivasan Date: Wed, 3 Dec 2014 15:16:55 -0500 Subject: [PATCH 56/76] Changed Matrix.h to correct return values amd impleemnted rectangular matrix types. Also changed block operations to << --- gtsam/base/Matrix.h | 151 ++++++++++++++++++++++------- gtsam/geometry/Cal3Bundler.cpp | 3 +- gtsam/geometry/Cal3Unified.cpp | 6 +- gtsam/geometry/Cal3_S2.h | 4 +- gtsam/geometry/EssentialMatrix.cpp | 21 ++-- gtsam/geometry/Point2.h | 2 +- gtsam/geometry/Point3.cpp | 8 +- gtsam/geometry/Point3.h | 10 +- gtsam/geometry/Pose2.cpp | 32 +++--- gtsam/geometry/Pose3.cpp | 24 ++--- gtsam/geometry/Pose3.h | 6 ++ gtsam/geometry/Rot2.h | 8 +- gtsam/geometry/Rot3M.cpp | 16 ++- gtsam/geometry/SimpleCamera.cpp | 14 +-- gtsam/geometry/SimpleCamera.h | 2 +- gtsam/geometry/StereoCamera.cpp | 3 +- gtsam/geometry/StereoCamera.h | 3 +- gtsam/geometry/tests/testRot3.cpp | 2 +- 18 files changed, 189 insertions(+), 126 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index e48f641e3..960373a62 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -49,7 +49,7 @@ typedef Eigen::Matrix Matrix41; typedef Eigen::Matrix Matrix51; typedef Eigen::Matrix Matrix61; - +typedef Eigen::Matrix Matrix1; typedef Eigen::Matrix2d Matrix2; typedef Eigen::Matrix3d Matrix3; typedef Eigen::Matrix4d Matrix4; @@ -78,46 +78,129 @@ typedef Eigen::Block ConstSubMatrix; // Identity Matrices -static const int I_1x1 = 1; -static const Matrix2 I_2x2 = Matrix2::Identity(); -static const Matrix3 I_3x3 = Matrix3::Identity(); -static const Matrix4 I_4x4 = Matrix4::Identity(); -static const Matrix5 I_5x5 = Matrix5::Identity(); -static const Matrix6 I_6x6 = Matrix6::Identity(); - -// Negative Identity -static const int _I_1x1 = -1; -static const Matrix2 _I_2x2 = -I_2x2; -static const Matrix3 _I_3x3 = -I_3x3; -static const Matrix4 _I_4x4 = -I_4x4; -static const Matrix5 _I_5x5 = -I_5x5; -static const Matrix6 _I_6x6 = -I_6x6; +static const Eigen::MatrixBase::IdentityReturnType I_1x1 = Matrix1::Identity(); +static const Eigen::MatrixBase::IdentityReturnType I_2x2 = Matrix2::Identity(); +static const Eigen::MatrixBase::IdentityReturnType I_3x3 = Matrix3::Identity(); +static const Eigen::MatrixBase::IdentityReturnType I_4x4 = Matrix4::Identity(); +static const Eigen::MatrixBase::IdentityReturnType I_5x5 = Matrix5::Identity(); +static const Eigen::MatrixBase::IdentityReturnType I_6x6 = Matrix6::Identity(); // Zero matrices -// TODO : Make these for rectangular sized matrices as well. -static const int Z_1x1 = 0; -static const Matrix2 Z_2x2 = Matrix2::Zero(); -static const Matrix3 Z_3x3 = Matrix3::Zero(); -static const Matrix4 Z_4x4 = Matrix4::Zero(); -static const Matrix5 Z_5x5 = Matrix5::Zero(); -static const Matrix6 Z_6x6 = Matrix6::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_1x1 = Matrix1::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_2x2 = Matrix2::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_3x3 = Matrix3::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_4x4 = Matrix4::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_5x5 = Matrix5::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_6x6 = Matrix6::Zero(); + +static const Eigen::DenseBase::ConstantReturnType Z_1x2 = Matrix12::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_1x3 = Matrix13::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_1x4 = Matrix14::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_1x5 = Matrix15::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_1x6 = Matrix16::Zero(); + + +static const Eigen::DenseBase::ConstantReturnType Z_2x3 = Matrix23::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_2x4 = Matrix24::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_2x5 = Matrix25::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_2x6 = Matrix26::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_2x7 = Matrix27::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_2x8 = Matrix28::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_2x9 = Matrix29::Zero(); + +static const Eigen::DenseBase::ConstantReturnType Z_3x2 = Matrix32::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_3x4 = Matrix34::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_3x5 = Matrix35::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_3x6 = Matrix36::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_3x7 = Matrix37::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_3x8 = Matrix38::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_3x9 = Matrix39::Zero(); + +static const Eigen::DenseBase::ConstantReturnType Z_2x1 = Matrix21::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_3x1 = Matrix31::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_4x1 = Matrix41::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_5x1 = Matrix51::Zero(); +static const Eigen::DenseBase::ConstantReturnType Z_6x1 = Matrix61::Zero(); + + + // Ones matrices // TODO : Make these for rectangular sized matrices as well. -static const int O_1x1 = 1; -static const Matrix2 O_2x2 = Matrix2::Ones(); -static const Matrix3 O_3x3 = Matrix3::Ones(); -static const Matrix4 O_4x4 = Matrix4::Ones(); -static const Matrix5 O_5x5 = Matrix5::Ones(); -static const Matrix6 O_6x6 = Matrix6::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_1x1 = Matrix1::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_2x2 = Matrix2::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_3x3 = Matrix3::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_4x4 = Matrix4::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_5x5 = Matrix5::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_6x6 = Matrix6::Ones(); + +static const Eigen::DenseBase::ConstantReturnType O_1x2 = Matrix12::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_1x3 = Matrix13::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_1x4 = Matrix14::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_1x5 = Matrix15::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_1x6 = Matrix16::Ones(); + + +static const Eigen::DenseBase::ConstantReturnType O_2x3 = Matrix23::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_2x4 = Matrix24::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_2x5 = Matrix25::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_2x6 = Matrix26::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_2x7 = Matrix27::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_2x8 = Matrix28::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_2x9 = Matrix29::Ones(); + +static const Eigen::DenseBase::ConstantReturnType O_3x2 = Matrix32::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_3x4 = Matrix34::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_3x5 = Matrix35::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_3x6 = Matrix36::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_3x7 = Matrix37::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_3x8 = Matrix38::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_3x9 = Matrix39::Ones(); + +static const Eigen::DenseBase::ConstantReturnType O_2x1 = Matrix21::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_3x1 = Matrix31::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_4x1 = Matrix41::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_5x1 = Matrix51::Ones(); +static const Eigen::DenseBase::ConstantReturnType O_6x1 = Matrix61::Ones(); + // Negative Ones -static const int _O_1x1 = -1; -static const Matrix2 _O_2x2 = -O_2x2; -static const Matrix3 _O_3x3 = -O_3x3; -static const Matrix4 _O_4x4 = -O_4x4; -static const Matrix5 _O_5x5 = -O_5x5; -static const Matrix6 _O_6x6 = -O_6x6; +static const Eigen::DenseBase::ConstantReturnType _O_1x1 = Matrix1::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_2x2 = Matrix2::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_3x3 = Matrix3::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_4x4 = Matrix4::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_5x5 = Matrix5::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_6x6 = Matrix6::Constant(-1); + +static const Eigen::DenseBase::ConstantReturnType _O_1x2 = Matrix12::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_1x3 = Matrix13::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_1x4 = Matrix14::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_1x5 = Matrix15::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_1x6 = Matrix16::Constant(-1); + + +static const Eigen::DenseBase::ConstantReturnType _O_2x3 = Matrix23::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_2x4 = Matrix24::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_2x5 = Matrix25::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_2x6 = Matrix26::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_2x7 = Matrix27::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_2x8 = Matrix28::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_2x9 = Matrix29::Constant(-1); + +static const Eigen::DenseBase::ConstantReturnType _O_3x2 = Matrix32::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_3x4 = Matrix34::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_3x5 = Matrix35::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_3x6 = Matrix36::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_3x7 = Matrix37::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_3x8 = Matrix38::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_3x9 = Matrix39::Constant(-1); + +static const Eigen::DenseBase::ConstantReturnType _O_2x1 = Matrix21::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_3x1 = Matrix31::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_4x1 = Matrix41::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_5x1 = Matrix51::Constant(-1); +static const Eigen::DenseBase::ConstantReturnType _O_6x1 = Matrix61::Constant(-1); + // Matlab-like syntax diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index d6a20b3db..ccd061d7c 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -141,8 +141,7 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { Matrix2 Dp; uncalibrate(p, Dcal, Dp); Matrix25 H; - H.block<2,2>(0,0) = Dp; - H.block<2,3>(0,2) = Dcal; + H << Dp, Dcal; return H; } diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index 8e6c219bc..5bdf89856 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -81,9 +81,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, Vector2 DU; DU << -xs * sqrt_nx * xi_sqrt_nx2, // -ys * sqrt_nx * xi_sqrt_nx2; - - H1->block<2,9>(0,0) = H1base; - H1->block<2,1>(0,9) = H2base * DU; + *H1 << H1base, H2base * DU; } // Inlined derivative for points @@ -95,7 +93,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, // mid, (sqrt_nx + xi*(xs*xs + 1)) * denom; - *H2 = H2base * DU; + *H2 << H2base * DU; } return puncalib; diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 61eafa283..3896ccb43 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -173,8 +173,8 @@ public: inline Cal3_S2 between(const Cal3_S2& q, OptionalJacobian<5,5> H1=boost::none, OptionalJacobian<5,5> H2=boost::none) const { - if(H1) *H1 = _I_5x5; - if(H2) *H2 = I_5x5; + if(H1) *H1 << -I_5x5; + if(H2) *H2 << I_5x5; return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_); } diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index be4bba33f..5ea05e10d 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -27,10 +27,8 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, // First get 2*3 derivative from Unit3::FromPoint3 Matrix23 D_direction_1T2; Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); - H->block<3, 3>(0, 0) << I_3x3; // upper left - H->block<2, 3>(3, 0).setZero(); // lower left - H->block<3, 3>(0, 3) << Z_3x3; // upper right - H->block<2, 3>(3, 3) = D_direction_1T2 * _1R2_.matrix(); // lower right + *H << I_3x3, Z_3x3, // + Z_2x3, D_direction_1T2 * _1R2_.matrix(); return EssentialMatrix(_1R2_, direction); } } @@ -69,9 +67,8 @@ Point3 EssentialMatrix::transform_to(const Point3& p, // The last 3 columns are derivative with respect to change in translation // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() // Duy made an educated guess that this needs to be rotated to the local frame - Matrix35 H; - H.block<3,3>(0,0) = DE_.block<3, 3>(0, 0); - H.block<3,2>(0,3) = -aRb_.transpose() * aTb_.basis(); + Matrix35 H; + H << DE_.block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis(); *DE = H; } return q; @@ -93,11 +90,8 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, Matrix23 D_c1Tc2_cRb; // 2*3 Matrix2 D_c1Tc2_aTb; // 2*2 Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); - if (HE) { - HE->setZero(); - HE->block<3, 3>(0, 0) = cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2 - HE->block<2, 2>(3, 3) = D_c1Tc2_aTb; // (2*2) - } + if (HE) *HE << cRb.matrix(), Z_3x2, // + Z_2x3, D_c1Tc2_aTb; if (HR) { throw runtime_error( "EssentialMatrix::rotate: derivative HR not implemented yet"); @@ -118,8 +112,7 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) * aTb_.basis(); - H->block<1,3>(0,0) = HR; - H->block<1,2>(0,3) = HD; + *H << HR, HD; } return dot(vA, E_ * vB); } diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 2491d92d1..8c8e03db4 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -139,7 +139,7 @@ public: inline Point2 between(const Point2& q, OptionalJacobian<2,2> H1=boost::none, OptionalJacobian<2,2> H2=boost::none) const { - if(H1) *H1 = _I_2x2; + if(H1) *H1 = -I_2x2; if(H2) *H2 = I_2x2; return q - (*this); } diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 152b5566b..0d84f32fe 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -66,9 +66,9 @@ Point3 Point3::operator/(double s) const { Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) - *H1= I_3x3; + *H1 << I_3x3; if (H2) - *H2= I_3x3; + *H2 << I_3x3; return *this + q; } @@ -76,9 +76,9 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) - *H1 = I_3x3; + *H1 << I_3x3; if (H2) - *H2 = I_3x3; + *H2 << I_3x3; return *this - q; } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 0cd038d90..24a3486ae 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -95,8 +95,8 @@ namespace gtsam { inline Point3 compose(const Point3& p2, OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const { - if (H1) H1->setIdentity(); - if (H2) H2->setIdentity(); + if (H1) *H1 << I_3x3; + if (H2) *H2 << I_3x3; return *this + p2; } @@ -107,8 +107,8 @@ namespace gtsam { inline Point3 between(const Point3& p2, OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const { - if(H1) *H1 = _I_3x3; - if(H2) H2->setIdentity(); + if(H1) *H1 << -I_3x3; + if(H2) *H2 << I_3x3; return p2 - *this; } @@ -172,7 +172,7 @@ namespace gtsam { if (H2) { *H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z(); - *H2 = *H2 *(1./d); + *H2 << *H2 *(1./d); } return d; } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index bf6f02e17..c882d7156 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -125,7 +125,7 @@ Matrix3 Pose2::AdjointMap() const { /* ************************************************************************* */ Pose2 Pose2::inverse(OptionalJacobian<3,3> H1) const { - if (H1) *H1 = -AdjointMap(); + if (H1) *H1 << -AdjointMap(); return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } @@ -139,7 +139,7 @@ Point2 Pose2::transform_to(const Point2& point, if (H1) *H1 << -1.0, 0.0, q.y(), 0.0, -1.0, -q.x(); - if (H2) *H2 = r_.transpose(); + if (H2) *H2 << r_.transpose(); return q; } @@ -148,8 +148,8 @@ Point2 Pose2::transform_to(const Point2& point, Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { // TODO: inline and reuse? - if(H1) *H1 = p2.inverse().AdjointMap(); - if(H2) *H2 = I_3x3; + if(H1) *H1 << p2.inverse().AdjointMap(); + if(H2) *H2 << I_3x3; return (*this)*p2; } @@ -162,11 +162,8 @@ Point2 Pose2::transform_from(const Point2& p, const Matrix2 R = r_.matrix(); Matrix21 Drotate1; Drotate1 << -q.y(), q.x(); - if (H1) { - H1->block<2,2>(0,0) = R; // [R R_{pi/2}q] - H1->block<2,1>(0,2) = Drotate1; - } - if (H2) *H2 = R; // R + if (H1) *H1 << R, Drotate1; + if (H2) *H2 << R; // R } return q + t_; } @@ -200,7 +197,7 @@ Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1, s, -c, dt2, 0.0, 0.0,-1.0; } - if (H2) *H2 = I_3x3; + if (H2) *H2 << I_3x3; return Pose2(R,t); } @@ -214,8 +211,8 @@ Rot2 Pose2::bearing(const Point2& point, if (!H1 && !H2) return Rot2::relativeBearing(d); Matrix12 D_result_d; Rot2 result = Rot2::relativeBearing(d, D_result_d); - if (H1) *H1 = D_result_d * (D1); - if (H2) *H2 = D_result_d * (D2); + if (H1) *H1 << D_result_d * (D1); + if (H2) *H2 << D_result_d * (D2); return result; } @@ -226,8 +223,7 @@ Rot2 Pose2::bearing(const Pose2& pose, Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0); if (H2) { Matrix12 H2_ = D2 * pose.r().matrix(); - H2->setZero(); - H2->block<1,2>(0,0) = H2_; + *H2 << H2_, Z_1x1; } return result; } @@ -242,9 +238,9 @@ double Pose2::range(const Point2& point, Matrix23 H1_; H1_ << -r_.c(), r_.s(), 0.0, -r_.s(), -r_.c(), 0.0; - *H1 = H * H1_; + *H1 << H * H1_; } - if (H2) *H2 = H; + if (H2) *H2 << H; return r; } @@ -261,14 +257,14 @@ double Pose2::range(const Pose2& pose, H1_ << -r_.c(), r_.s(), 0.0, -r_.s(), -r_.c(), 0.0; - *H1 = H * H1_; + *H1 << H * H1_; } if (H2) { Matrix23 H2_; H2_ << pose.r_.c(), -pose.r_.s(), 0.0, pose.r_.s(), pose.r_.c(), 0.0; - *H2 = H * H2_; + *H2 << H * H2_; } return r; } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 35abf39a2..09ff633d2 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -274,17 +274,14 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, /* ************************************************************************* */ Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1, OptionalJacobian<6,6> H2) const { - if (H1) - *H1 = p2.inverse().AdjointMap(); - if (H2) - *H2 = I_6x6; + if (H1) *H1 << p2.inverse().AdjointMap(); + if (H2) *H2 << I_6x6; return (*this) * p2; } /* ************************************************************************* */ Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const { - if (H1) - *H1 = -AdjointMap(); + if (H1) *H1 << -AdjointMap(); Rot3 Rt = R_.inverse(); return Pose3(Rt, Rt * (-t_)); } @@ -294,10 +291,8 @@ Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const { Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1, OptionalJacobian<6,6> H2) const { Pose3 result = inverse() * p2; - if (H1) - *H1 = -result.inverse().AdjointMap(); - if (H2) - *H2 = I_6x6; + if (H1) *H1 << -result.inverse().AdjointMap(); + if (H2) *H2 << I_6x6; return result; } @@ -313,10 +308,8 @@ double Pose3::range(const Point3& point, OptionalJacobian<1,6> H1, d2); Matrix13 D_result_d ; D_result_d << x / n, y / n, z / n; - if (H1) - *H1 = D_result_d * (D1); - if (H2) - *H2 = D_result_d * (D2); + if (H1) *H1 << D_result_d * D1; + if (H2) *H2 << D_result_d * D2; return n; } @@ -327,8 +320,7 @@ double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1, double r = range(pose.translation(), H1, H2? &D2 : 0); if (H2) { Matrix13 H2_ = D2 * pose.rotation().matrix(); - H2->setZero(); - H2->block<1,3>(0,3) = H2_; + *H2 << Z_1x3, H2_; } return r; } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d396fe61f..6a7ed8456 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -70,6 +70,12 @@ public: R_(R), t_(t) { } + /** Construct from R,t, where t \in vector3 */ + Pose3(const Rot3& R, const Vector3& t) : + R_(R), t_(t) { + } + + /** Construct from Pose2 */ explicit Pose3(const Pose2& pose2); diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 324332c9e..33685bc7c 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -118,8 +118,8 @@ namespace gtsam { /** Compose - make a new rotation by adding angles */ inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 = boost::none, OptionalJacobian<1,1> H2 = boost::none) const { - if (H1) *H1 << I_1x1; // set to 1x1 identity matrix - if (H2) *H2 << I_1x1; // set to 1x1 identity matrix + if (H1) *H1 << 1; // set to 1x1 identity matrix + if (H2) *H2 << 1; // set to 1x1 identity matrix return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); } @@ -131,8 +131,8 @@ namespace gtsam { /** Between using the default implementation */ inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 = boost::none, OptionalJacobian<1,1> H2 = boost::none) const { - if (H1) *H1 << _I_1x1; // set to 1x1 identity matrix - if (H2) *H2 << I_1x1; // set to 1x1 identity matrix + if (H1) *H1 << -1; // set to 1x1 identity matrix + if (H2) *H2 << 1; // set to 1x1 identity matrix return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_); } diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index efc5d6f8b..6171d3347 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -141,10 +141,8 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) { /* ************************************************************************* */ Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - if (H1) - *H1 = R2.transpose(); - if (H2) - *H2 = I_3x3; + if (H1) *H1 << R2.transpose(); + if (H2) *H2 << I_3x3; return *this * R2; } @@ -161,15 +159,15 @@ Matrix3 Rot3::transpose() const { /* ************************************************************************* */ Rot3 Rot3::inverse(boost::optional H1) const { - if (H1) *H1 = -rot_; + if (H1) *H1 << -rot_; return Rot3(Matrix3(transpose())); } /* ************************************************************************* */ Rot3 Rot3::between (const Rot3& R2, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { - if (H1) *H1 = -(R2.transpose()*rot_); - if (H2) *H2 = I_3x3; + if (H1) *H1 << -(R2.transpose()*rot_); + if (H2) *H2 << I_3x3; Matrix3 R12 = transpose()*R2.rot_; return Rot3(R12); } @@ -178,8 +176,8 @@ Rot3 Rot3::between (const Rot3& R2, Point3 Rot3::rotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1 || H2) { - if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 = rot_; + if (H1) *H1 << rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 << rot_; } return Point3(rot_ * p.vector()); } diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp index cab871874..d92c5bdd7 100644 --- a/gtsam/geometry/SimpleCamera.cpp +++ b/gtsam/geometry/SimpleCamera.cpp @@ -21,27 +21,27 @@ namespace gtsam { - SimpleCamera simpleCamera(const Matrix& P) { + SimpleCamera simpleCamera(const Matrix34& P) { // P = [A|a] = s K cRw [I|-T], with s the unknown scale - Matrix A = P.topLeftCorner(3, 3); - Vector a = P.col(3); + Matrix3 A = P.topLeftCorner(3, 3); + Vector3 a = P.col(3); // do RQ decomposition to get s*K and cRw angles - Matrix sK; - Vector xyz; + Matrix3 sK; + Vector3 xyz; boost::tie(sK, xyz) = RQ(A); // Recover scale factor s and K double s = sK(2, 2); - Matrix K = sK / s; + Matrix3 K = sK / s; // Recover cRw itself, and its inverse Rot3 cRw = Rot3::RzRyRx(xyz); Rot3 wRc = cRw.inverse(); // Now, recover T from a = - s K cRw T = - A T - Vector T = -(A.inverse() * a); + Vector3 T = -(A.inverse() * a); return SimpleCamera(Pose3(wRc, T), Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2))); } diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 557654d2d..c6d33bcb3 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -27,5 +27,5 @@ namespace gtsam { typedef PinholeCamera SimpleCamera; /// Recover camera from 3*4 camera matrix - GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix& P); + GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P); } diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index b9e03c01d..f48c188aa 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -30,8 +30,7 @@ namespace gtsam { /* ************************************************************************* */ StereoPoint2 StereoCamera::project(const Point3& point, - boost::optional H1, boost::optional H2, - boost::optional H3) const { + boost::optional H1, boost::optional H2) const { #ifdef STEREOCAMERA_CHAIN_RULE const Point3 q = leftCamPose_.transform_to(point, H1, H2); diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 60ea7693d..dfefc4bec 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -120,8 +120,7 @@ public: */ StereoPoint2 project(const Point3& point, boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const; + boost::optional H2 = boost::none) const; /** * diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 6c3f1a7e7..c89d2dacb 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -49,7 +49,7 @@ TEST( Rot3, chart) /* ************************************************************************* */ TEST( Rot3, constructor) { - Rot3 expected(I_3x3); + Rot3 expected((Matrix)I_3x3); Point3 r1(1,0,0), r2(0,1,0), r3(0,0,1); Rot3 actual(r1, r2, r3); CHECK(assert_equal(actual,expected)); From 0f95890215b3979e0d6b926104c40a5c8a9728e7 Mon Sep 17 00:00:00 2001 From: Natesh Srinivasan Date: Wed, 3 Dec 2014 16:34:58 -0500 Subject: [PATCH 57/76] done with geometry --- gtsam/base/Matrix.h | 40 ---------------------------- gtsam/geometry/StereoCamera.cpp | 23 +++++++--------- gtsam/geometry/StereoCamera.h | 10 +++---- gtsam/geometry/TriangulationFactor.h | 4 +-- gtsam/geometry/Unit3.h | 1 - gtsam/geometry/triangulation.h | 2 +- 6 files changed, 18 insertions(+), 62 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 960373a62..d1c343fcd 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -163,46 +163,6 @@ static const Eigen::DenseBase::ConstantReturnType O_4x1 = Matrix41::On static const Eigen::DenseBase::ConstantReturnType O_5x1 = Matrix51::Ones(); static const Eigen::DenseBase::ConstantReturnType O_6x1 = Matrix61::Ones(); - -// Negative Ones -static const Eigen::DenseBase::ConstantReturnType _O_1x1 = Matrix1::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_2x2 = Matrix2::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x3 = Matrix3::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_4x4 = Matrix4::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_5x5 = Matrix5::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_6x6 = Matrix6::Constant(-1); - -static const Eigen::DenseBase::ConstantReturnType _O_1x2 = Matrix12::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_1x3 = Matrix13::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_1x4 = Matrix14::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_1x5 = Matrix15::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_1x6 = Matrix16::Constant(-1); - - -static const Eigen::DenseBase::ConstantReturnType _O_2x3 = Matrix23::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_2x4 = Matrix24::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_2x5 = Matrix25::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_2x6 = Matrix26::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_2x7 = Matrix27::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_2x8 = Matrix28::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_2x9 = Matrix29::Constant(-1); - -static const Eigen::DenseBase::ConstantReturnType _O_3x2 = Matrix32::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x4 = Matrix34::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x5 = Matrix35::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x6 = Matrix36::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x7 = Matrix37::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x8 = Matrix38::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x9 = Matrix39::Constant(-1); - -static const Eigen::DenseBase::ConstantReturnType _O_2x1 = Matrix21::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x1 = Matrix31::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_4x1 = Matrix41::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_5x1 = Matrix51::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_6x1 = Matrix61::Constant(-1); - - - // Matlab-like syntax /** diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index f48c188aa..0e5197289 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -30,7 +30,7 @@ namespace gtsam { /* ************************************************************************* */ StereoPoint2 StereoCamera::project(const Point3& point, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const { #ifdef STEREOCAMERA_CHAIN_RULE const Point3 q = leftCamPose_.transform_to(point, H1, H2); @@ -57,26 +57,23 @@ namespace gtsam { if (H1 || H2) { #ifdef STEREOCAMERA_CHAIN_RULE // just implement chain rule - Matrix D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian + Matrix3 D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian if (H1) *H1 = D_project_point*(*H1); if (H2) *H2 = D_project_point*(*H2); #else // optimized version, see StereoCamera.nb if (H1) { const double v1 = v/fy, v2 = fx*v1, dx=d*x; - *H1 = (Matrix(3, 6) << - uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL, + *H1 << uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL, uR*v1, -fx-dx*uR, v2, -dfx, 0.0, d*uR, - fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v - ).finished(); + fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v; } if (H2) { - const Matrix R(leftCamPose_.rotation().matrix()); - *H2 = d * (Matrix(3, 3) << - fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL, - fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR, - fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v - ).finished(); + const Matrix3 R(leftCamPose_.rotation().matrix()); + *H2 << fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL, + fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR, + fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v; + *H2 << d * (*H2); } #endif } @@ -86,7 +83,7 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { + Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { double d = 1.0 / P.z(), d2 = d*d; const Cal3_S2Stereo& K = *K_; double f_x = K.fx(), f_y = K.fy(), b = K.baseline(); diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index dfefc4bec..4c19126e6 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -95,8 +95,8 @@ public: } Pose3 between(const StereoCamera &camera, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { + OptionalJacobian<6,6> H1=boost::none, + OptionalJacobian<6,6> H2=boost::none) const { return leftCamPose_.between(camera.pose(), H1, H2); } @@ -119,8 +119,8 @@ public: * @param H3 IGNORED (for calibration) */ StereoPoint2 project(const Point3& point, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; /** * @@ -138,7 +138,7 @@ public: private: /** utility functions */ - Matrix Dproject_to_stereo_camera1(const Point3& P) const; + Matrix3 Dproject_to_stereo_camera1(const Point3& P) const; friend class boost::serialization::access; template diff --git a/gtsam/geometry/TriangulationFactor.h b/gtsam/geometry/TriangulationFactor.h index fc8d546d3..9dcd5a5c3 100644 --- a/gtsam/geometry/TriangulationFactor.h +++ b/gtsam/geometry/TriangulationFactor.h @@ -112,14 +112,14 @@ public: } /// Evaluate error h(x)-z and optionally derivatives - Vector evaluateError(const Point3& point, boost::optional H2 = + Vector evaluateError(const Point3& point, OptionalJacobian<2,3> H2 = boost::none) const { try { Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_); return error.vector(); } catch (CheiralityException& e) { if (H2) - *H2 = zeros(2, 3); + *H2 << Z_2x3; if (verboseCheirality_) std::cout << e.what() << ": Landmark " << DefaultKeyFormatter(this->key()) << " moved behind camera" diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 5dfa7375e..f83404e74 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -147,7 +147,6 @@ private: /// @name Advanced Interface /// @{ - /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index fabcc4c02..864a60664 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -53,7 +53,7 @@ public: * @return Triangulated Point3 */ GTSAM_EXPORT Point3 triangulateDLT( - const std::vector& projection_matrices, + const std::vector& projection_matrices, // TODO: Use the fact that projection matrices sizes are known at compile time const std::vector& measurements, double rank_tol); /// From 30b77d73e73968655021ba4776e903c82ac879b2 Mon Sep 17 00:00:00 2001 From: Natesh Srinivasan Date: Wed, 3 Dec 2014 16:39:15 -0500 Subject: [PATCH 58/76] commited the wrong file. This one compiles --- gtsam/geometry/TriangulationFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/TriangulationFactor.h b/gtsam/geometry/TriangulationFactor.h index 9dcd5a5c3..fc8d546d3 100644 --- a/gtsam/geometry/TriangulationFactor.h +++ b/gtsam/geometry/TriangulationFactor.h @@ -112,14 +112,14 @@ public: } /// Evaluate error h(x)-z and optionally derivatives - Vector evaluateError(const Point3& point, OptionalJacobian<2,3> H2 = + Vector evaluateError(const Point3& point, boost::optional H2 = boost::none) const { try { Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_); return error.vector(); } catch (CheiralityException& e) { if (H2) - *H2 << Z_2x3; + *H2 = zeros(2, 3); if (verboseCheirality_) std::cout << e.what() << ": Landmark " << DefaultKeyFormatter(this->key()) << " moved behind camera" From cabf17f294739d870b4f5ab0d134c9f9a7418342 Mon Sep 17 00:00:00 2001 From: Natesh Srinivasan Date: Wed, 3 Dec 2014 18:23:08 -0500 Subject: [PATCH 59/76] fixed DLT to constant sized matrices --- gtsam/geometry/triangulation.cpp | 4 ++-- gtsam/geometry/triangulation.h | 18 ++++++++---------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 9e1575801..474689525 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -30,7 +30,7 @@ namespace gtsam { * @param rank_tol SVD rank tolerance * @return Triangulated Point3 */ -Point3 triangulateDLT(const std::vector& projection_matrices, +Point3 triangulateDLT(const std::vector& projection_matrices, const std::vector& measurements, double rank_tol) { // number of cameras @@ -41,7 +41,7 @@ Point3 triangulateDLT(const std::vector& projection_matrices, for (size_t i = 0; i < m; i++) { size_t row = i * 2; - const Matrix& projection = projection_matrices.at(i); + const Matrix34& projection = projection_matrices.at(i); const Point2& p = measurements.at(i); // build system of equations diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 864a60664..798a7e9dc 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -53,7 +53,7 @@ public: * @return Triangulated Point3 */ GTSAM_EXPORT Point3 triangulateDLT( - const std::vector& projection_matrices, // TODO: Use the fact that projection matrices sizes are known at compile time + const std::vector& projection_matrices, const std::vector& measurements, double rank_tol); /// @@ -189,12 +189,11 @@ Point3 triangulatePoint3(const std::vector& poses, throw(TriangulationUnderconstrainedException()); // construct projection matrices from poses & calibration - std::vector projection_matrices; + std::vector projection_matrices; BOOST_FOREACH(const Pose3& pose, poses) { projection_matrices.push_back( - sharedCal->K() * sub(pose.inverse().matrix(), 0, 3, 0, 4)); + sharedCal->K() * (pose.inverse().matrix()).block<3,4>(0,0)); } - // Triangulate linearly Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); @@ -240,12 +239,11 @@ Point3 triangulatePoint3( // construct projection matrices from poses & calibration typedef PinholeCamera Camera; - std::vector projection_matrices; - BOOST_FOREACH(const Camera& camera, cameras) - projection_matrices.push_back( - camera.calibration().K() - * sub(camera.pose().inverse().matrix(), 0, 3, 0, 4)); - + std::vector projection_matrices; + BOOST_FOREACH(const Camera& camera, cameras) { + Matrix P_ = (camera.pose().inverse().matrix()); + projection_matrices.push_back(camera.calibration().K()* (P_.block<3,4>(0,0)) ); + } Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); // The n refine using non-linear optimization From 52c4771bcb53fe75ab666500e2996992bd325936 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 4 Dec 2014 01:38:45 +0100 Subject: [PATCH 60/76] Sanitized dimensions. Does not compile because of range. --- gtsam/geometry/PinholeCamera.h | 135 +++++++++------------------------ 1 file changed, 35 insertions(+), 100 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index ca6c64bb9..4c2422824 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -115,9 +115,9 @@ public: /// @{ explicit PinholeCamera(const Vector &v) { - pose_ = Pose3::Expmap(v.head(Pose3::Dim())); - if (v.size() > Pose3::Dim()) { - K_ = Calibration(v.tail(Calibration::Dim())); + pose_ = Pose3::Expmap(v.head(6)); + if (v.size() > 6) { + K_ = Calibration(v.tail(DimK)); } } @@ -168,82 +168,20 @@ public: return K_; } - /// @} - /// @name Group ?? Frank says this might not make sense - /// @{ - - /// compose two cameras: TODO Frank says this might not make sense - inline const PinholeCamera compose(const PinholeCamera &c, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - PinholeCamera result(pose_.compose(c.pose(), H1, H2), K_); - if (H1) { - H1->conservativeResize(Dim(), Dim()); - H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - if (H2) { - H2->conservativeResize(Dim(), Dim()); - H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - return result; - } - - /// between two cameras: TODO Frank says this might not make sense - inline const PinholeCamera between(const PinholeCamera& c, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - PinholeCamera result(pose_.between(c.pose(), H1, H2), K_); - if (H1) { - H1->conservativeResize(Dim(), Dim()); - H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - if (H2) { - H2->conservativeResize(Dim(), Dim()); - H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - return result; - } - - /// inverse camera: TODO Frank says this might not make sense - inline const PinholeCamera inverse( - boost::optional H1 = boost::none) const { - PinholeCamera result(pose_.inverse(H1), K_); - if (H1) { - H1->conservativeResize(Dim(), Dim()); - H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - return result; - } - - /// compose two cameras: TODO Frank says this might not make sense - inline const PinholeCamera compose(const Pose3 &c) const { - return PinholeCamera(pose_.compose(c), K_); - } - /// @} /// @name Manifold /// @{ /// move a cameras according to d PinholeCamera retract(const Vector& d) const { - if ((size_t) d.size() == pose_.dim()) + if ((size_t) d.size() == 6) return PinholeCamera(pose().retract(d), calibration()); else - return PinholeCamera(pose().retract(d.head(pose().dim())), + return PinholeCamera(pose().retract(d.head(6)), calibration().retract(d.tail(calibration().dim()))); } - typedef Eigen::Matrix VectorK6; + typedef Eigen::Matrix VectorK6; /// return canonical coordinate VectorK6 localCoordinates(const PinholeCamera& T2) const { @@ -255,12 +193,12 @@ public: /// Manifold dimension inline size_t dim() const { - return pose_.dim() + K_.dim(); + return Dim(); } /// Manifold dimension inline static size_t Dim() { - return Pose3::Dim() + Calibration::Dim(); + return Dim(); } /// @} @@ -293,7 +231,7 @@ public: return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); } - typedef Eigen::Matrix Matrix2K; + typedef Eigen::Matrix Matrix2K; /** project a point from world coordinate to the image * @param pw is a point in world coordinates @@ -301,11 +239,9 @@ public: * @param Dpoint is the Jacobian w.r.t. point3 * @param Dcal is the Jacobian w.r.t. calibration */ - inline Point2 project( - const Point3& pw, - OptionalJacobian<2,6> Dpose = boost::none, - OptionalJacobian<2,3> Dpoint = boost::none, - OptionalJacobian<2,DimK> Dcal = boost::none) const { + inline Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { // Transform to camera coordinates and check cheirality const Point3 pc = pose_.transform_to(pw); @@ -336,11 +272,10 @@ public: * @param Dpoint is the Jacobian w.r.t. point3 * @param Dcal is the Jacobian w.r.t. calibration */ - inline Point2 projectPointAtInfinity( - const Point3& pw, - OptionalJacobian<2,6> Dpose = boost::none, - OptionalJacobian<2,2> Dpoint = boost::none, - OptionalJacobian<2,DimK> Dcal = boost::none) const { + inline Point2 projectPointAtInfinity(const Point3& pw, + OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { if (!Dpose && !Dpoint && !Dcal) { const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter) @@ -352,7 +287,8 @@ public: Matrix3 Dpc_rot, Dpc_point; const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point); - Matrix36 Dpc_pose; Dpc_pose.setZero(); + Matrix36 Dpc_pose; + Dpc_pose.setZero(); Dpc_pose.leftCols<3>() = Dpc_rot; // camera to normalized image coordinate @@ -377,8 +313,9 @@ public: * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] * @param Dpoint is the Jacobian w.r.t. point3 */ - Point2 project2(const Point3& pw, // - OptionalJacobian<2, 6 + DimK> Dcamera = boost::none, + Point2 project2( + const Point3& pw, // + OptionalJacobian<2, Dim()> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { const Point3 pc = pose_.transform_to(pw); @@ -396,7 +333,7 @@ public: if (Dcamera) { // TODO why does leftCols<6>() not compile ?? calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6)); - (*Dcamera).rightCols(K_.dim()) = Dcal; // Jacobian wrt calib + (*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib } if (Dpoint) { calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); @@ -428,14 +365,14 @@ public: */ double range( const Point3& point, // - OptionalJacobian<1,6> Dpose = boost::none, - OptionalJacobian<1,3> Dpoint = boost::none) const { + OptionalJacobian<1, 6> Dpose = boost::none, + OptionalJacobian<1, 3> Dpoint = boost::none) const { double result = pose_.range(point, Dpose, Dpoint); if (Dpose) { // Add columns of zeros to Jacobian for calibration Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); - H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); + H1r.conservativeResize(Eigen::NoChange, Dim()); + H1r.block<1, DimK>(0, 6).setZero(); } return result; } @@ -455,8 +392,8 @@ public: if (Dpose) { // Add columns of zeros to Jacobian for calibration Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); - H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); + H1r.conservativeResize(Eigen::NoChange, Dim()); + H1r.block(0, 6, 1, DimK) = Matrix::Zero(1, DimK); } return result; } @@ -477,16 +414,14 @@ public: if (Dpose) { // Add columns of zeros to Jacobian for calibration Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); - H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); + H1r.conservativeResize(Eigen::NoChange, Dim()); + H1r.block(0, 6, 1, DimK) = Matrix::Zero(1, DimK); } if (Dother) { // Add columns of zeros to Jacobian for calibration Matrix& H2r(*Dother); - H2r.conservativeResize(Eigen::NoChange, - camera.pose().dim() + camera.calibration().dim()); - H2r.block(0, camera.pose().dim(), 1, camera.calibration().dim()) = - Matrix::Zero(1, camera.calibration().dim()); + H2r.conservativeResize(Eigen::NoChange, Dim()); + H2r.block(0, 6, 1, DimK) = Matrix::Zero(1, DimK); } return result; } @@ -500,8 +435,8 @@ public: */ double range( const CalibratedCamera& camera, // - OptionalJacobian<1,6> Dpose = boost::none, - OptionalJacobian<1,6> Dother = boost::none) const { + OptionalJacobian<1, 6> Dpose = boost::none, + OptionalJacobian<1, 6> Dother = boost::none) const { return pose_.range(camera.pose_, Dpose, Dother); } @@ -565,7 +500,7 @@ private: namespace traits { template -struct GTSAM_EXPORT is_manifold > : public boost::true_type{ +struct GTSAM_EXPORT is_manifold > : public boost::true_type { }; template From 354de17fd78b06dff99e054b41966f99d1c2d4ed Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 4 Dec 2014 09:38:28 +0100 Subject: [PATCH 61/76] Fixed range --- gtsam/geometry/CalibratedCamera.h | 45 ++++--------- gtsam/geometry/PinholeCamera.h | 106 +++++++++++++----------------- 2 files changed, 57 insertions(+), 94 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 5741d71b3..f5a8b4469 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -18,9 +18,8 @@ #pragma once -#include -#include #include +#include namespace gtsam { @@ -88,26 +87,6 @@ public: return pose_; } - /// compose the two camera poses: TODO Frank says this might not make sense - inline const CalibratedCamera compose(const CalibratedCamera &c, - OptionalJacobian<6,6> H1=boost::none, OptionalJacobian<6,6> H2 = - boost::none) const { - return CalibratedCamera(pose_.compose(c.pose(), H1, H2)); - } - - /// between the two camera poses: TODO Frank says this might not make sense - inline const CalibratedCamera between(const CalibratedCamera& c, - OptionalJacobian<6,6> H1 = boost::none, OptionalJacobian<6,6> H2 = - boost::none) const { - return CalibratedCamera(pose_.between(c.pose(), H1, H2)); - } - - /// invert the camera pose: TODO Frank says this might not make sense - inline const CalibratedCamera inverse(OptionalJacobian<6,6> H1 = - boost::none) const { - return CalibratedCamera(pose_.inverse(H1)); - } - /** * Create a level camera at the given 2D pose and height * @param pose2 specifies the location and viewing direction @@ -152,7 +131,8 @@ public: * @return the intrinsic coordinates of the projected point */ Point2 project(const Point3& point, - OptionalJacobian<2,6> Dpose=boost::none, OptionalJacobian<2,3> Dpoint=boost::none) const; + OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const; /** * projects a 3-dimensional point in camera coordinates into the @@ -174,8 +154,8 @@ public: * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Point3& point, OptionalJacobian<1,6> H1 = boost::none, - OptionalJacobian<1,3> H2 = boost::none) const { + double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none) const { return pose_.range(point, H1, H2); } @@ -186,8 +166,8 @@ public: * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Pose3& pose, OptionalJacobian<1,6> H1=boost::none, - OptionalJacobian<1,6> H2=boost::none) const { + double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none, + OptionalJacobian<1, 6> H2 = boost::none) const { return pose_.range(pose, H1, H2); } @@ -198,8 +178,8 @@ public: * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const CalibratedCamera& camera, OptionalJacobian<1,6> H1 = - boost::none, OptionalJacobian<1,6> H2 = boost::none) const { + double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 = + boost::none, OptionalJacobian<1, 6> H2 = boost::none) const { return pose_.range(camera.pose_, H1, H2); } @@ -223,15 +203,16 @@ private: namespace traits { template<> -struct GTSAM_EXPORT is_group : public boost::true_type{ +struct GTSAM_EXPORT is_group : public boost::true_type { }; template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ +struct GTSAM_EXPORT is_manifold : public boost::true_type { }; template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ +struct GTSAM_EXPORT dimension : public boost::integral_constant< + int, 6> { }; } diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 4c2422824..41176d0d8 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -18,16 +18,9 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include #include +#include +#include namespace gtsam { @@ -42,8 +35,9 @@ private: Pose3 pose_; Calibration K_; - // Get dimension of calibration type at compile time - static const int DimK = traits::dimension::value; + // Get dimensions of calibration type and This at compile time + static const int DimK = traits::dimension::value, // + DimC = 6 + DimK; public: @@ -172,6 +166,18 @@ public: /// @name Manifold /// @{ + /// Manifold dimension + inline size_t dim() const { + return DimC; + } + + /// Manifold dimension + inline static size_t Dim() { + return DimC; + } + + typedef Eigen::Matrix VectorK6; + /// move a cameras according to d PinholeCamera retract(const Vector& d) const { if ((size_t) d.size() == 6) @@ -181,8 +187,6 @@ public: calibration().retract(d.tail(calibration().dim()))); } - typedef Eigen::Matrix VectorK6; - /// return canonical coordinate VectorK6 localCoordinates(const PinholeCamera& T2) const { VectorK6 d; // TODO: why does d.head<6>() not compile?? @@ -191,16 +195,6 @@ public: return d; } - /// Manifold dimension - inline size_t dim() const { - return Dim(); - } - - /// Manifold dimension - inline static size_t Dim() { - return Dim(); - } - /// @} /// @name Transformations and measurement functions /// @{ @@ -315,7 +309,7 @@ public: */ Point2 project2( const Point3& pw, // - OptionalJacobian<2, Dim()> Dcamera = boost::none, + OptionalJacobian<2, DimC> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { const Point3 pc = pose_.transform_to(pw); @@ -359,85 +353,73 @@ public: /** * Calculate range to a landmark * @param point 3D location of landmark - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dpoint the optionally computed Jacobian with respect to the landmark * @return range (double) */ double range( const Point3& point, // - OptionalJacobian<1, 6> Dpose = boost::none, + OptionalJacobian<1, DimC> Dcamera = boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { - double result = pose_.range(point, Dpose, Dpoint); - if (Dpose) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, Dim()); - H1r.block<1, DimK>(0, 6).setZero(); - } + Matrix16 Dpose; + double result = pose_.range(point, Dcamera ? &Dpose : 0, Dpoint); + if (Dcamera) + *Dcamera << Dpose, Eigen::Matrix::Zero(); return result; } /** * Calculate range to another pose * @param pose Other SO(3) pose - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dpose2 the optionally computed Jacobian with respect to the other pose * @return range (double) */ double range( const Pose3& pose, // - boost::optional Dpose = boost::none, - boost::optional Dpose2 = boost::none) const { - double result = pose_.range(pose, Dpose, Dpose2); - if (Dpose) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, Dim()); - H1r.block(0, 6, 1, DimK) = Matrix::Zero(1, DimK); - } + OptionalJacobian<1, DimC> Dcamera = boost::none, + OptionalJacobian<1, 6> Dpose2 = boost::none) const { + Matrix16 Dpose; + double result = pose_.range(pose, Dcamera ? &Dpose : 0, Dpose2); + if (Dcamera) + *Dcamera << Dpose, Eigen::Matrix::Zero(); return result; } /** * Calculate range to another camera * @param camera Other camera - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dother the optionally computed Jacobian with respect to the other camera * @return range (double) */ template double range( const PinholeCamera& camera, // - boost::optional Dpose = boost::none, - boost::optional Dother = boost::none) const { - double result = pose_.range(camera.pose_, Dpose, Dother); - if (Dpose) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, Dim()); - H1r.block(0, 6, 1, DimK) = Matrix::Zero(1, DimK); - } - if (Dother) { - // Add columns of zeros to Jacobian for calibration - Matrix& H2r(*Dother); - H2r.conservativeResize(Eigen::NoChange, Dim()); - H2r.block(0, 6, 1, DimK) = Matrix::Zero(1, DimK); - } + OptionalJacobian<1, DimC> Dcamera = boost::none, + OptionalJacobian<1, 6 + CalibrationB::Dim()> Dother = boost::none) const { + Matrix16 Dpose, Dpose2; + double result = pose_.range(camera.pose(), Dcamera ? &Dpose : 0, + Dother ? &Dpose2 : 0); + if (Dcamera) + *Dcamera << Dpose, Eigen::Matrix::Zero(); + if (Dother) + *Dother << Dpose2, Eigen::Matrix::Zero(); return result; } /** * Calculate range to another camera * @param camera Other camera - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dother the optionally computed Jacobian with respect to the other camera * @return range (double) */ double range( const CalibratedCamera& camera, // - OptionalJacobian<1, 6> Dpose = boost::none, + OptionalJacobian<1, DimC> Dcamera = boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { - return pose_.range(camera.pose_, Dpose, Dother); + return range(camera.pose_, Dcamera, Dother); } private: From cc96529eb6498371382534173d58366d7e7fb180 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 4 Dec 2014 10:41:09 +0100 Subject: [PATCH 62/76] Sanitized Matrix typedefs and constants a la Eigen --- gtsam/base/Matrix.h | 110 +++++------------------------ gtsam/geometry/EssentialMatrix.cpp | 29 ++++---- gtsam/geometry/Pose3.cpp | 2 +- 3 files changed, 32 insertions(+), 109 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index d1c343fcd..c6798859b 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -37,6 +37,22 @@ namespace gtsam { typedef Eigen::MatrixXd Matrix; typedef Eigen::Matrix MatrixRowMajor; +// Create handy typedefs and constants for square-size matrices +#define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX) \ +typedef Eigen::Matrix Matrix##SUFFIX; \ +static const Eigen::MatrixBase::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \ +static const Eigen::MatrixBase::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero(); + +GTSAM_MAKE_TYPEDEFS(1,1); +GTSAM_MAKE_TYPEDEFS(2,2); +GTSAM_MAKE_TYPEDEFS(3,3); +GTSAM_MAKE_TYPEDEFS(4,4); +GTSAM_MAKE_TYPEDEFS(5,5); +GTSAM_MAKE_TYPEDEFS(6,6); +GTSAM_MAKE_TYPEDEFS(7,7); +GTSAM_MAKE_TYPEDEFS(8,8); +GTSAM_MAKE_TYPEDEFS(9,9); + typedef Eigen::Matrix Matrix12; typedef Eigen::Matrix Matrix13; typedef Eigen::Matrix Matrix14; @@ -49,13 +65,6 @@ typedef Eigen::Matrix Matrix41; typedef Eigen::Matrix Matrix51; typedef Eigen::Matrix Matrix61; -typedef Eigen::Matrix Matrix1; -typedef Eigen::Matrix2d Matrix2; -typedef Eigen::Matrix3d Matrix3; -typedef Eigen::Matrix4d Matrix4; -typedef Eigen::Matrix Matrix5; -typedef Eigen::Matrix Matrix6; - typedef Eigen::Matrix Matrix23; typedef Eigen::Matrix Matrix24; typedef Eigen::Matrix Matrix25; @@ -76,93 +85,6 @@ typedef Eigen::Matrix Matrix39; typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; - -// Identity Matrices -static const Eigen::MatrixBase::IdentityReturnType I_1x1 = Matrix1::Identity(); -static const Eigen::MatrixBase::IdentityReturnType I_2x2 = Matrix2::Identity(); -static const Eigen::MatrixBase::IdentityReturnType I_3x3 = Matrix3::Identity(); -static const Eigen::MatrixBase::IdentityReturnType I_4x4 = Matrix4::Identity(); -static const Eigen::MatrixBase::IdentityReturnType I_5x5 = Matrix5::Identity(); -static const Eigen::MatrixBase::IdentityReturnType I_6x6 = Matrix6::Identity(); - -// Zero matrices -static const Eigen::DenseBase::ConstantReturnType Z_1x1 = Matrix1::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_2x2 = Matrix2::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x3 = Matrix3::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_4x4 = Matrix4::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_5x5 = Matrix5::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_6x6 = Matrix6::Zero(); - -static const Eigen::DenseBase::ConstantReturnType Z_1x2 = Matrix12::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_1x3 = Matrix13::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_1x4 = Matrix14::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_1x5 = Matrix15::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_1x6 = Matrix16::Zero(); - - -static const Eigen::DenseBase::ConstantReturnType Z_2x3 = Matrix23::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_2x4 = Matrix24::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_2x5 = Matrix25::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_2x6 = Matrix26::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_2x7 = Matrix27::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_2x8 = Matrix28::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_2x9 = Matrix29::Zero(); - -static const Eigen::DenseBase::ConstantReturnType Z_3x2 = Matrix32::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x4 = Matrix34::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x5 = Matrix35::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x6 = Matrix36::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x7 = Matrix37::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x8 = Matrix38::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x9 = Matrix39::Zero(); - -static const Eigen::DenseBase::ConstantReturnType Z_2x1 = Matrix21::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_3x1 = Matrix31::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_4x1 = Matrix41::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_5x1 = Matrix51::Zero(); -static const Eigen::DenseBase::ConstantReturnType Z_6x1 = Matrix61::Zero(); - - - - -// Ones matrices -// TODO : Make these for rectangular sized matrices as well. -static const Eigen::DenseBase::ConstantReturnType O_1x1 = Matrix1::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_2x2 = Matrix2::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x3 = Matrix3::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_4x4 = Matrix4::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_5x5 = Matrix5::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_6x6 = Matrix6::Ones(); - -static const Eigen::DenseBase::ConstantReturnType O_1x2 = Matrix12::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_1x3 = Matrix13::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_1x4 = Matrix14::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_1x5 = Matrix15::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_1x6 = Matrix16::Ones(); - - -static const Eigen::DenseBase::ConstantReturnType O_2x3 = Matrix23::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_2x4 = Matrix24::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_2x5 = Matrix25::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_2x6 = Matrix26::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_2x7 = Matrix27::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_2x8 = Matrix28::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_2x9 = Matrix29::Ones(); - -static const Eigen::DenseBase::ConstantReturnType O_3x2 = Matrix32::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x4 = Matrix34::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x5 = Matrix35::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x6 = Matrix36::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x7 = Matrix37::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x8 = Matrix38::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x9 = Matrix39::Ones(); - -static const Eigen::DenseBase::ConstantReturnType O_2x1 = Matrix21::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_3x1 = Matrix31::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_4x1 = Matrix41::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_5x1 = Matrix51::Ones(); -static const Eigen::DenseBase::ConstantReturnType O_6x1 = Matrix61::Ones(); - // Matlab-like syntax /** diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 5ea05e10d..c2b690496 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -14,7 +14,7 @@ namespace gtsam { /* ************************************************************************* */ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, - OptionalJacobian<5,6> H) { + OptionalJacobian<5, 6> H) { const Rot3& _1R2_ = _1P2_.rotation(); const Point3& _1T2_ = _1P2_.translation(); if (!H) { @@ -27,8 +27,8 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, // First get 2*3 derivative from Unit3::FromPoint3 Matrix23 D_direction_1T2; Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); - *H << I_3x3, Z_3x3, // - Z_2x3, D_direction_1T2 * _1R2_.matrix(); + *H << I_3x3, Z_3x3, // + Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix(); return EssentialMatrix(_1R2_, direction); } } @@ -52,13 +52,13 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const { /* ************************************************************************* */ Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { - return (Vector(5) << - aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_)).finished(); + return (Vector(5) << aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates( + other.aTb_)).finished(); } /* ************************************************************************* */ -Point3 EssentialMatrix::transform_to(const Point3& p, - OptionalJacobian<3,5> DE, OptionalJacobian<3,3> Dpoint) const { +Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE, + OptionalJacobian<3, 3> Dpoint) const { Pose3 pose(aRb_, aTb_.point3()); Matrix36 DE_; Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint); @@ -67,8 +67,8 @@ Point3 EssentialMatrix::transform_to(const Point3& p, // The last 3 columns are derivative with respect to change in translation // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() // Duy made an educated guess that this needs to be rotated to the local frame - Matrix35 H; - H << DE_.block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis(); + Matrix35 H; + H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis(); *DE = H; } return q; @@ -76,7 +76,7 @@ Point3 EssentialMatrix::transform_to(const Point3& p, /* ************************************************************************* */ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, - OptionalJacobian<5,5> HE, OptionalJacobian<5,3> HR) const { + OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const { // The rotation must be conjugated to act in the camera frame Rot3 c1Rc2 = aRb_.conjugate(cRb); @@ -88,10 +88,11 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, } else { // Calculate derivatives Matrix23 D_c1Tc2_cRb; // 2*3 - Matrix2 D_c1Tc2_aTb; // 2*2 + Matrix2 D_c1Tc2_aTb; // 2*2 Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); - if (HE) *HE << cRb.matrix(), Z_3x2, // - Z_2x3, D_c1Tc2_aTb; + if (HE) + *HE << cRb.matrix(), Matrix32::Zero(), // + Matrix23::Zero(), D_c1Tc2_aTb; if (HR) { throw runtime_error( "EssentialMatrix::rotate: derivative HR not implemented yet"); @@ -106,7 +107,7 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, /* ************************************************************************* */ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // - OptionalJacobian<1,5> H) const { + OptionalJacobian<1, 5> H) const { if (H) { // See math.lyx Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 09ff633d2..f85ad9641 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -320,7 +320,7 @@ double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1, double r = range(pose.translation(), H1, H2? &D2 : 0); if (H2) { Matrix13 H2_ = D2 * pose.rotation().matrix(); - *H2 << Z_1x3, H2_; + *H2 << Matrix13::Zero(), H2_; } return r; } From fcfd232639123638bb421118a776b08fdbb0c2e7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 4 Dec 2014 12:28:12 +0100 Subject: [PATCH 63/76] Added tests, everything checks out --- gtsam/geometry/PinholeCamera.h | 31 +-- gtsam/geometry/Pose3.cpp | 28 +-- gtsam/geometry/tests/testCalibratedCamera.cpp | 10 +- gtsam/geometry/tests/testPinholeCamera.cpp | 186 ++++++++++++------ 4 files changed, 167 insertions(+), 88 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 41176d0d8..1700b4960 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -361,10 +361,10 @@ public: const Point3& point, // OptionalJacobian<1, DimC> Dcamera = boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { - Matrix16 Dpose; - double result = pose_.range(point, Dcamera ? &Dpose : 0, Dpoint); + Matrix16 Dpose_; + double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint); if (Dcamera) - *Dcamera << Dpose, Eigen::Matrix::Zero(); + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } @@ -378,11 +378,11 @@ public: double range( const Pose3& pose, // OptionalJacobian<1, DimC> Dcamera = boost::none, - OptionalJacobian<1, 6> Dpose2 = boost::none) const { - Matrix16 Dpose; - double result = pose_.range(pose, Dcamera ? &Dpose : 0, Dpose2); + OptionalJacobian<1, 6> Dpose = boost::none) const { + Matrix16 Dpose_; + double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose); if (Dcamera) - *Dcamera << Dpose, Eigen::Matrix::Zero(); + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } @@ -397,14 +397,17 @@ public: double range( const PinholeCamera& camera, // OptionalJacobian<1, DimC> Dcamera = boost::none, - OptionalJacobian<1, 6 + CalibrationB::Dim()> Dother = boost::none) const { - Matrix16 Dpose, Dpose2; - double result = pose_.range(camera.pose(), Dcamera ? &Dpose : 0, + OptionalJacobian<1, 6 + traits::dimension::value> Dother = + boost::none) const { + Matrix16 Dpose_, Dpose2; + double result = pose_.range(camera.pose(), Dcamera ? &Dpose_ : 0, Dother ? &Dpose2 : 0); if (Dcamera) - *Dcamera << Dpose, Eigen::Matrix::Zero(); - if (Dother) - *Dother << Dpose2, Eigen::Matrix::Zero(); + *Dcamera << Dpose_, Eigen::Matrix::Zero(); + if (Dother) { + Dother->setZero(); + Dother->block(0, 0, 1, 6) = Dpose2; + } return result; } @@ -419,7 +422,7 @@ public: const CalibratedCamera& camera, // OptionalJacobian<1, DimC> Dcamera = boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { - return range(camera.pose_, Dcamera, Dother); + return range(camera.pose(), Dcamera, Dother); } private: diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f85ad9641..3158c95e8 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -297,20 +297,24 @@ Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1, } /* ************************************************************************* */ -double Pose3::range(const Point3& point, OptionalJacobian<1,6> H1, - OptionalJacobian<1,3> H2) const { +double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, + OptionalJacobian<1, 3> H2) const { if (!H1 && !H2) return transform_to(point).norm(); - Matrix36 D1; - Matrix3 D2; - Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); - double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt( - d2); - Matrix13 D_result_d ; - D_result_d << x / n, y / n, z / n; - if (H1) *H1 << D_result_d * D1; - if (H2) *H2 << D_result_d * D2; - return n; + else { + Matrix36 D1; + Matrix3 D2; + Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); + const double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, + n = sqrt(d2); + Matrix13 D_result_d; + D_result_d << x / n, y / n, z / n; + if (H1) + *H1 = D_result_d * D1; + if (H2) + *H2 = D_result_d * D2; + return n; + } } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 5aeee03d4..6a990e08e 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -15,12 +15,14 @@ * @brief test CalibratedCamera class */ -#include - -#include +#include +#include #include #include -#include + +#include + +#include using namespace std; using namespace gtsam; diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index ddeae2b7d..d11fbe3bf 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -15,29 +15,28 @@ * @brief test PinholeCamera class */ -#include -#include - #include #include #include #include #include +#include + +#include +#include using namespace std; using namespace gtsam; +typedef PinholeCamera Camera; + static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose1((Matrix)(Matrix(3,3) << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), - Point3(0,0,0.5)); +static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); +static const Camera camera(pose, K); -typedef PinholeCamera Camera; -static const Camera camera(pose1, K); +static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); +static const Camera camera1(pose1, K); static const Point3 point1(-0.08,-0.08, 0.0); static const Point3 point2(-0.08, 0.08, 0.0); @@ -52,8 +51,8 @@ static const Point3 point4_inf( 0.16,-0.16, -1.0); /* ************************************************************************* */ TEST( PinholeCamera, constructor) { - CHECK(assert_equal( camera.calibration(), K)); - CHECK(assert_equal( camera.pose(), pose1)); + EXPECT(assert_equal( camera.calibration(), K)); + EXPECT(assert_equal( camera.pose(), pose)); } /* ************************************************************************* */ @@ -67,7 +66,7 @@ TEST( PinholeCamera, level2) Point3 x(1,0,0),y(0,0,-1),z(0,1,0); Rot3 wRc(x,y,z); Pose3 expected(wRc,Point3(0.4,0.3,0.1)); - CHECK(assert_equal( camera.pose(), expected)); + EXPECT(assert_equal( camera.pose(), expected)); } /* ************************************************************************* */ @@ -80,72 +79,72 @@ TEST( PinholeCamera, lookat) // expected Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Pose3 expected(Rot3(xc,yc,zc),C); - CHECK(assert_equal( camera.pose(), expected)); + EXPECT(assert_equal( camera.pose(), expected)); Point3 C2(30.0,0.0,10.0); Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; - CHECK(assert_equal(I, eye(3))); + EXPECT(assert_equal(I, eye(3))); } /* ************************************************************************* */ TEST( PinholeCamera, project) { - CHECK(assert_equal( camera.project(point1), Point2(-100, 100) )); - CHECK(assert_equal( camera.project(point2), Point2(-100, -100) )); - CHECK(assert_equal( camera.project(point3), Point2( 100, -100) )); - CHECK(assert_equal( camera.project(point4), Point2( 100, 100) )); + EXPECT(assert_equal( camera.project(point1), Point2(-100, 100) )); + EXPECT(assert_equal( camera.project(point2), Point2(-100, -100) )); + EXPECT(assert_equal( camera.project(point3), Point2( 100, -100) )); + EXPECT(assert_equal( camera.project(point4), Point2( 100, 100) )); } /* ************************************************************************* */ TEST( PinholeCamera, backproject) { - CHECK(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); - CHECK(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); - CHECK(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); - CHECK(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); + EXPECT(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); + EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); + EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); + EXPECT(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); } /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity) { - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf)); - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf)); - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf)); - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf)); } /* ************************************************************************* */ TEST( PinholeCamera, backproject2) { Point3 origin; - Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); Point3 actual = camera.backproject(Point2(), 1.); Point3 expected(0., 1., 0.); pair x = camera.projectSafe(expected); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x.first)); - CHECK(x.second); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x.first)); + EXPECT(x.second); } /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity2) { Point3 origin; - Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); Point3 actual = camera.backprojectPointAtInfinity(Point2()); Point3 expected(0., 1., 0.); Point2 x = camera.projectPointAtInfinity(expected); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x)); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x)); } /* ************************************************************************* */ @@ -159,8 +158,8 @@ TEST( PinholeCamera, backprojectInfinity3) Point3 expected(0., 0., 1.); Point2 x = camera.projectPointAtInfinity(expected); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x)); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x)); } /* ************************************************************************* */ @@ -173,13 +172,13 @@ TEST( PinholeCamera, Dproject) { Matrix Dpose, Dpoint, Dcal; Point2 result = camera.project(point1, Dpose, Dpoint, Dcal); - Matrix numerical_pose = numericalDerivative31(project3, pose1, point1, K); - Matrix numerical_point = numericalDerivative32(project3, pose1, point1, K); - Matrix numerical_cal = numericalDerivative33(project3, pose1, point1, K); - CHECK(assert_equal(Point2(-100, 100), result)); - CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); - CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); - CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); + Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K); + Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K); + Matrix numerical_cal = numericalDerivative33(project3, pose, point1, K); + EXPECT(assert_equal(Point2(-100, 100), result)); + EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); + EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); + EXPECT(assert_equal(numerical_cal, Dcal, 1e-7)); } /* ************************************************************************* */ @@ -190,21 +189,21 @@ static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const C TEST( PinholeCamera, Dproject_Infinity) { Matrix Dpose, Dpoint, Dcal; - Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera + Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1 // test Projection Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); Point2 expected(-5.0, 5.0); - CHECK(assert_equal(actual, expected, 1e-7)); + EXPECT(assert_equal(actual, expected, 1e-7)); // test Jacobians - Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point3D, K); - Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point3D, K); - Matrix numerical_point2x2 = numerical_point.block(0,0,2,2); // only the direction to the point matters - Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point3D, K); - CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); - CHECK(assert_equal(numerical_point2x2, Dpoint, 1e-7)); - CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); + Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose, point3D, K); + Matrix Hexpected2 = numericalDerivative32(projectInfinity3, pose, point3D, K); + Matrix numerical_point2x2 = Hexpected2.block(0,0,2,2); // only the direction to the point matters + Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose, point3D, K); + EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); + EXPECT(assert_equal(numerical_point2x2, Dpoint, 1e-7)); + EXPECT(assert_equal(numerical_cal, Dcal, 1e-7)); } /* ************************************************************************* */ @@ -217,11 +216,82 @@ TEST( PinholeCamera, Dproject2) { Matrix Dcamera, Dpoint; Point2 result = camera.project2(point1, Dcamera, Dpoint); - Matrix numerical_camera = numericalDerivative21(project4, camera, point1); - Matrix numerical_point = numericalDerivative22(project4, camera, point1); - CHECK(assert_equal(result, Point2(-100, 100) )); - CHECK(assert_equal(numerical_camera, Dcamera, 1e-7)); - CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); + Matrix Hexpected1 = numericalDerivative21(project4, camera, point1); + Matrix Hexpected2 = numericalDerivative22(project4, camera, point1); + EXPECT(assert_equal(result, Point2(-100, 100) )); + EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7)); + EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +static double range0(const Camera& camera, const Point3& point) { + return camera.range(point); +} + +/* ************************************************************************* */ +typedef Eigen::Matrix Matrix1_11; +TEST( PinholeCamera, range0) { + Matrix1_11 D1; Matrix13 D2; + double result = camera.range(point1, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); + Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); + EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, + 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +static double range1(const Camera& camera, const Pose3& pose) { + return camera.range(pose); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range1) { + Matrix1_11 D1; Matrix16 D2; + double result = camera.range(pose1, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1); + Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +typedef PinholeCamera Camera2; +static const Cal3Bundler K2(625, 1e-3, 1e-3); +static const Camera2 camera2(pose1, K2); +static double range2(const Camera& camera, const Camera2& camera2) { + return camera.range(camera2); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range2) { + typedef Eigen::Matrix Matrix19; + Matrix1_11 D1; Matrix19 D2; + double result = camera.range(camera2, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2); + Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +static const CalibratedCamera camera3(pose1); +static double range3(const Camera& camera, const CalibratedCamera& camera3) { + return camera.range(camera3); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range3) { + Matrix1_11 D1; Matrix16 D2; + double result = camera.range(camera3, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3); + Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); } /* ************************************************************************* */ From 178e4fd61ceabc3eb1a412a554af130d09c2e353 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 4 Dec 2014 12:28:33 +0100 Subject: [PATCH 64/76] Cut out unsupported methods in GTSAM --- gtsam.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam.h b/gtsam.h index d63563028..f194995cc 100644 --- a/gtsam.h +++ b/gtsam.h @@ -758,10 +758,6 @@ class CalibratedCamera { gtsam::CalibratedCamera retract(const Vector& d) const; Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; - // Group - gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const; - gtsam::CalibratedCamera inverse() const; - // Action on Point3 gtsam::Point2 project(const gtsam::Point3& point) const; static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); @@ -2195,10 +2191,14 @@ typedef gtsam::RangeFactor RangeFactorPosePoint2; typedef gtsam::RangeFactor RangeFactorPosePoint3; typedef gtsam::RangeFactor RangeFactorPose2; typedef gtsam::RangeFactor RangeFactorPose3; -typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; -typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; -typedef gtsam::RangeFactor RangeFactorCalibratedCamera; -typedef gtsam::RangeFactor RangeFactorSimpleCamera; + +// Commented out by Frank Dec 2014: not poses! +// If needed, we need a RangeFactor that takes a camera, extracts the pose +// Should be easy with Expressions +//typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; +//typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; +//typedef gtsam::RangeFactor RangeFactorCalibratedCamera; +//typedef gtsam::RangeFactor RangeFactorSimpleCamera; #include From d2e53d4fe9d5073e0ad3bf12e319774adbbbce03 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 4 Dec 2014 12:30:41 +0100 Subject: [PATCH 65/76] "Fixed" AHRS --- gtsam_unstable/slam/AHRS.cpp | 76 +++++++++++++------------- gtsam_unstable/slam/AHRS.h | 22 +++++--- gtsam_unstable/slam/tests/testAHRS.cpp | 6 +- 3 files changed, 53 insertions(+), 51 deletions(-) diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 910c4f705..2a27730f4 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -12,12 +12,12 @@ using namespace std; namespace gtsam { -Matrix cov(const Matrix& m) { +/* ************************************************************************* */ +Matrix3 AHRS::Cov(const Vector3s& m) { const double num_observations = m.cols(); - const Vector mean = m.rowwise().sum() / num_observations; - Matrix D = m.colwise() - mean; - Matrix DDt = D * trans(D); - return DDt / (num_observations - 1); + const Vector3 mean = m.rowwise().sum() / num_observations; + Vector3s D = m.colwise() - mean; + return D * trans(D) / (num_observations - 1); } /* ************************************************************************* */ @@ -31,11 +31,11 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, size_t T = stationaryU.cols(); // estimate standard deviation on gyroscope readings - Pg_ = cov(stationaryU); - Vector sigmas_v_g = esqrt(Pg_.diagonal() * T); + Pg_ = Cov(stationaryU); + Vector3 sigmas_v_g = esqrt(Pg_.diagonal() * T); // estimate standard deviation on accelerometer readings - Pa_ = cov(stationaryF); + Pa_ = Cov(stationaryF); // Quantities needed for integration @@ -43,15 +43,13 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, double tau_g = 730; // correlation time for gyroscope double tau_a = 730; // correlation time for accelerometer - F_g_ = -I3 / tau_g; - F_a_ = -I3 / tau_a; + F_g_ = -I_3x3 / tau_g; + F_a_ = -I_3x3 / tau_a; Vector3 var_omega_w = 0 * ones(3); // TODO Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3); Vector3 var_omega_a = (0.034 * 0.034) * ones(3); - Vector3 sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g); - Vector vars(12); - vars << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a; - var_w_ = diag(vars); + Vector3 sigmas_v_g_sq = sigmas_v_g.cwiseProduct(sigmas_v_g); + var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a; // Quantities needed for aiding sigmas_v_a_ = esqrt(T * Pa_.diagonal()); @@ -92,15 +90,15 @@ std::pair AHRS::initialize(double g_e) Matrix P_plus_k2 = Matrix(9, 9); P_plus_k2.block<3,3>(0, 0) = P11; - P_plus_k2.block<3,3>(0, 3) = Z3; + P_plus_k2.block<3,3>(0, 3) = Z_3x3; P_plus_k2.block<3,3>(0, 6) = P12; - P_plus_k2.block<3,3>(3, 0) = Z3; + P_plus_k2.block<3,3>(3, 0) = Z_3x3; P_plus_k2.block<3,3>(3, 3) = Pg_; - P_plus_k2.block<3,3>(3, 6) = Z3; + P_plus_k2.block<3,3>(3, 6) = Z_3x3; P_plus_k2.block<3,3>(6, 0) = trans(P12); - P_plus_k2.block<3,3>(6, 3) = Z3; + P_plus_k2.block<3,3>(6, 3) = Z_3x3; P_plus_k2.block<3,3>(6, 6) = Pa; Vector dx = zero(9); @@ -120,26 +118,26 @@ std::pair AHRS::integrate( // FIXME: //if nargout>1 Matrix bRn = mech.bRn().matrix(); - Matrix I3 = eye(3); - Matrix Z3 = zeros(3, 3); - Matrix F_k = zeros(9, 9); + Matrix9 F_k; F_k.setZero(); F_k.block<3,3>(0, 3) = -bRn; F_k.block<3,3>(3, 3) = F_g_; F_k.block<3,3>(6, 6) = F_a_; - Matrix G_k = zeros(9, 12); + typedef Eigen::Matrix Matrix9_12; + Matrix9_12 G_k; G_k.setZero(); G_k.block<3,3>(0, 0) = -bRn; G_k.block<3,3>(0, 6) = -bRn; - G_k.block<3,3>(3, 3) = I3; - G_k.block<3,3>(6, 9) = I3; + G_k.block<3,3>(3, 3) = I_3x3; + G_k.block<3,3>(6, 9) = I_3x3; - Matrix Q_k = G_k * var_w_ * trans(G_k); - Matrix Psi_k = eye(9) + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix + Matrix9 Q_k = G_k * var_w_.asDiagonal() * G_k.transpose(); + Matrix9 Psi_k = I_9x9 + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix // This implements update in section 10.6 - Matrix B = zeros(9, 9); - Vector u2 = zero(9); + Matrix9 B; B.setZero(); + Vector9 u2; u2.setZero(); + // TODO predictQ should be templated to also take fixed size matrices. KalmanFilter::State newState = KF_.predictQ(state, Psi_k,B,u2,dt*Q_k); return make_pair(newMech, newState); } @@ -172,7 +170,7 @@ std::pair AHRS::aid( if (Farrell) { // calculate residual gravity measurement z = n_g_ - trans(bRn) * measured_b_g; - H = collect(3, &n_g_cross_, &Z3, &bRn); + H = collect(3, &n_g_cross_, &Z_3x3, &bRn); R = trans(bRn) * diag(emul(sigmas_v_a_, sigmas_v_a_)) * bRn; } else { // my measurement prediction (in body frame): @@ -186,7 +184,7 @@ std::pair AHRS::aid( z = bRn * n_g_ - measured_b_g; // Now the Jacobian H Matrix b_g = bRn * n_g_cross_; - H = collect(3, &b_g, &Z3, &I3); + H = collect(3, &b_g, &Z_3x3, &I_3x3); // And the measurement noise, TODO: should be created once where sigmas_v_a is given R = diag(emul(sigmas_v_a_, sigmas_v_a_)); } @@ -216,7 +214,7 @@ std::pair AHRS::aidGeneral( Vector z = f - increment * f_previous; //Vector z = increment * f_previous - f; Matrix b_g = skewSymmetric(increment* f_previous); - Matrix H = collect(3, &b_g, &I3, &Z3); + Matrix H = collect(3, &b_g, &I_3x3, &Z_3x3); // Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_)); // Matrix R = diag(Vector3(1.0, 0.2, 1.0)); // good for L_twice Matrix R = diag(Vector3(0.01, 0.0001, 0.01)); @@ -237,16 +235,16 @@ std::pair AHRS::aidGeneral( /* ************************************************************************* */ void AHRS::print(const std::string& s) const { mech0_.print(s + ".mech0_"); - gtsam::print(F_g_, s + ".F_g"); - gtsam::print(F_a_, s + ".F_a"); - gtsam::print(var_w_, s + ".var_w"); + gtsam::print((Matrix)F_g_, s + ".F_g"); + gtsam::print((Matrix)F_a_, s + ".F_a"); + gtsam::print((Vector)var_w_, s + ".var_w"); - gtsam::print(sigmas_v_a_, s + ".sigmas_v_a"); - gtsam::print(n_g_, s + ".n_g"); - gtsam::print(n_g_cross_, s + ".n_g_cross"); + gtsam::print((Vector)sigmas_v_a_, s + ".sigmas_v_a"); + gtsam::print((Vector)n_g_, s + ".n_g"); + gtsam::print((Matrix)n_g_cross_, s + ".n_g_cross"); - gtsam::print(Pg_, s + ".P_g"); - gtsam::print(Pa_, s + ".P_a"); + gtsam::print((Matrix)Pg_, s + ".P_g"); + gtsam::print((Matrix)Pa_, s + ".P_a"); } diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h index 81d74a9f5..e15e6e0f7 100644 --- a/gtsam_unstable/slam/AHRS.h +++ b/gtsam_unstable/slam/AHRS.h @@ -14,8 +14,6 @@ namespace gtsam { -GTSAM_UNSTABLE_EXPORT Matrix cov(const Matrix& m); - class GTSAM_UNSTABLE_EXPORT AHRS { private: @@ -25,18 +23,24 @@ private: KalmanFilter KF_; ///< initial Kalman filter // Quantities needed for integration - Matrix F_g_; ///< gyro bias dynamics - Matrix F_a_; ///< acc bias dynamics - Matrix var_w_; ///< dynamic noise variances + Matrix3 F_g_; ///< gyro bias dynamics + Matrix3 F_a_; ///< acc bias dynamics + + typedef Eigen::Matrix Variances; + Variances var_w_; ///< dynamic noise variances // Quantities needed for aiding - Vector sigmas_v_a_; ///< measurement sigma - Vector n_g_; ///< gravity in nav frame - Matrix n_g_cross_; ///< and its skew-symmetric matrix + Vector3 sigmas_v_a_; ///< measurement sigma + Vector3 n_g_; ///< gravity in nav frame + Matrix3 n_g_cross_; ///< and its skew-symmetric matrix - Matrix Pg_, Pa_; + Matrix3 Pg_, Pa_; public: + + typedef Eigen::Matrix Vector3s; + static Matrix3 Cov(const Vector3s& m); + /** * AHRS constructor * @param stationaryU initial interval of gyro measurements, 3xn matrix diff --git a/gtsam_unstable/slam/tests/testAHRS.cpp b/gtsam_unstable/slam/tests/testAHRS.cpp index 44f516ae9..d0980f452 100644 --- a/gtsam_unstable/slam/tests/testAHRS.cpp +++ b/gtsam_unstable/slam/tests/testAHRS.cpp @@ -30,7 +30,7 @@ TEST (AHRS, cov) { 9.0, 4.0, 7.0, 6.0, 3.0, 2.0).finished(); - Matrix actual = cov(trans(A)); + Matrix actual = AHRS::Cov(trans(A)); Matrix expected = (Matrix(3, 3) << 10.9167, 2.3333, 5.0000, 2.3333, 4.6667, -2.6667, @@ -42,7 +42,7 @@ TEST (AHRS, cov) { /* ************************************************************************* */ TEST (AHRS, covU) { - Matrix actual = cov(10000*stationaryU); + Matrix actual = AHRS::Cov(10000*stationaryU); Matrix expected = (Matrix(3, 3) << 33.3333333, -1.66666667, 53.3333333, -1.66666667, 0.333333333, -5.16666667, @@ -54,7 +54,7 @@ TEST (AHRS, covU) { /* ************************************************************************* */ TEST (AHRS, covF) { - Matrix actual = cov(100*stationaryF); + Matrix actual = AHRS::Cov(100*stationaryF); Matrix expected = (Matrix(3, 3) << 63.3808333, -0.432166667, -48.1706667, -0.432166667, 8.31053333, -16.6792667, From 4b2040fec61b664fe932c433a471f3db6e507858 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 4 Dec 2014 13:29:56 +0100 Subject: [PATCH 66/76] Fixed smart factors - but @lucacarlone should realize stereo-calibration cannot be optimized, so how come he is calling project with H3 ? --- gtsam/geometry/StereoCamera.cpp | 6 +++++- gtsam/geometry/StereoCamera.h | 3 ++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 0e5197289..cd0c8ee9a 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -30,7 +30,8 @@ namespace gtsam { /* ************************************************************************* */ StereoPoint2 StereoCamera::project(const Point3& point, - OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const { + OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2, + OptionalJacobian<3,6> H3) const { #ifdef STEREOCAMERA_CHAIN_RULE const Point3 q = leftCamPose_.transform_to(point, H1, H2); @@ -76,6 +77,9 @@ namespace gtsam { *H2 << d * (*H2); } #endif + if (H3) + // TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet + H3->setZero(); } // finally translate diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 4c19126e6..b69da71bd 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -120,7 +120,8 @@ public: */ StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; + OptionalJacobian<3, 3> H2 = boost::none, + OptionalJacobian<3, 6> H3 = boost::none) const; /** * From e6877dc27f9a030da6b1771f9421deefa642a07a Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 4 Dec 2014 13:30:41 +0100 Subject: [PATCH 67/76] All MatrixMN matrices now defined --- gtsam/base/Matrix.h | 38 ++++++++++---------------------------- 1 file changed, 10 insertions(+), 28 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index c6798859b..3ecfcf8fa 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -38,8 +38,18 @@ typedef Eigen::MatrixXd Matrix; typedef Eigen::Matrix MatrixRowMajor; // Create handy typedefs and constants for square-size matrices +// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9 #define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX) \ typedef Eigen::Matrix Matrix##SUFFIX; \ +typedef Eigen::Matrix Matrix1##SUFFIX; \ +typedef Eigen::Matrix Matrix2##SUFFIX; \ +typedef Eigen::Matrix Matrix3##SUFFIX; \ +typedef Eigen::Matrix Matrix4##SUFFIX; \ +typedef Eigen::Matrix Matrix5##SUFFIX; \ +typedef Eigen::Matrix Matrix6##SUFFIX; \ +typedef Eigen::Matrix Matrix7##SUFFIX; \ +typedef Eigen::Matrix Matrix8##SUFFIX; \ +typedef Eigen::Matrix Matrix9##SUFFIX; \ static const Eigen::MatrixBase::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \ static const Eigen::MatrixBase::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero(); @@ -53,34 +63,6 @@ GTSAM_MAKE_TYPEDEFS(7,7); GTSAM_MAKE_TYPEDEFS(8,8); GTSAM_MAKE_TYPEDEFS(9,9); -typedef Eigen::Matrix Matrix12; -typedef Eigen::Matrix Matrix13; -typedef Eigen::Matrix Matrix14; -typedef Eigen::Matrix Matrix15; -typedef Eigen::Matrix Matrix16; - -typedef Eigen::Matrix Matrix21; -typedef Eigen::Matrix Matrix31; -typedef Eigen::Matrix Matrix41; -typedef Eigen::Matrix Matrix51; -typedef Eigen::Matrix Matrix61; - -typedef Eigen::Matrix Matrix23; -typedef Eigen::Matrix Matrix24; -typedef Eigen::Matrix Matrix25; -typedef Eigen::Matrix Matrix26; -typedef Eigen::Matrix Matrix27; -typedef Eigen::Matrix Matrix28; -typedef Eigen::Matrix Matrix29; - -typedef Eigen::Matrix Matrix32; -typedef Eigen::Matrix Matrix34; -typedef Eigen::Matrix Matrix35; -typedef Eigen::Matrix Matrix36; -typedef Eigen::Matrix Matrix37; -typedef Eigen::Matrix Matrix38; -typedef Eigen::Matrix Matrix39; - // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; From fa66762394eb0027c99c42b19e94e19815cf4fe6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 4 Dec 2014 13:51:40 +0100 Subject: [PATCH 68/76] omitted extra typedef --- gtsam/geometry/tests/testPinholeCamera.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index d11fbe3bf..1cfb0ba25 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -267,7 +267,6 @@ static double range2(const Camera& camera, const Camera2& camera2) { /* ************************************************************************* */ TEST( PinholeCamera, range2) { - typedef Eigen::Matrix Matrix19; Matrix1_11 D1; Matrix19 D2; double result = camera.range(camera2, D1, D2); Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2); From add93f19a68856ee8193379c71d49830a752cc75 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 4 Dec 2014 13:51:51 +0100 Subject: [PATCH 69/76] Rename and reformat --- gtsam/slam/RangeFactor.h | 155 ++++++++++++++++++++------------------- 1 file changed, 81 insertions(+), 74 deletions(-) diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index af1c1a1bd..87bb77913 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -16,99 +16,106 @@ #pragma once -#include -#include #include +#include +#include namespace gtsam { - /** - * Binary factor for a range measurement - * @addtogroup SLAM - */ - template - class RangeFactor: public NoiseModelFactor2 { - private: +/** + * Binary factor for a range measurement + * @addtogroup SLAM + */ +template +class RangeFactor: public NoiseModelFactor2 { +private: - double measured_; /** measurement */ - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + double measured_; /** measurement */ + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - typedef RangeFactor This; - typedef NoiseModelFactor2 Base; + typedef RangeFactor This; + typedef NoiseModelFactor2 Base; - typedef POSE Pose; - typedef POINT Point; + // Concept requirements for this factor + GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(T1, T2) - // Concept requirements for this factor - GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, POINT) +public: - public: + RangeFactor() { + } /* Default constructor */ - RangeFactor() {} /* Default constructor */ + RangeFactor(Key key1, Key key2, double measured, + const SharedNoiseModel& model, boost::optional body_P_sensor = + boost::none) : + Base(model, key1, key2), measured_(measured), body_P_sensor_( + body_P_sensor) { + } - RangeFactor(Key poseKey, Key pointKey, double measured, - const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : - Base(model, poseKey, pointKey), measured_(measured), body_P_sensor_(body_P_sensor) { - } + virtual ~RangeFactor() { + } - virtual ~RangeFactor() {} + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** h(x)-z */ - Vector evaluateError(const POSE& pose, const POINT& point, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - double hx; - if(body_P_sensor_) { - if(H1) { - Matrix H0; - hx = pose.compose(*body_P_sensor_, H0).range(point, H1, H2); - *H1 = *H1 * H0; - } else { - hx = pose.compose(*body_P_sensor_).range(point, H1, H2); - } + /** h(x)-z */ + Vector evaluateError(const T1& t1, const T2& t2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const { + double hx; + if (body_P_sensor_) { + if (H1) { + Matrix H0; + hx = t1.compose(*body_P_sensor_, H0).range(t2, H1, H2); + *H1 = *H1 * H0; } else { - hx = pose.range(point, H1, H2); + hx = t1.compose(*body_P_sensor_).range(t2, H1, H2); } - return (Vector(1) << hx - measured_).finished(); + } else { + hx = t1.range(t2, H1, H2); } + return (Vector(1) << hx - measured_).finished(); + } - /** return the measured */ - double measured() const { - return measured_; - } + /** return the measured */ + double measured() const { + return measured_; + } - /** equals specialized to this factor */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL - && Base::equals(*e, tol) - && fabs(this->measured_ - e->measured_) < tol - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } + /** equals specialized to this factor */ + virtual bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) + && fabs(this->measured_ - e->measured_) < tol + && ((!body_P_sensor_ && !e->body_P_sensor_) + || (body_P_sensor_ && e->body_P_sensor_ + && body_P_sensor_->equals(*e->body_P_sensor_))); + } - /** print contents */ - void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "RangeFactor, range = " << measured_ << std::endl; - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - Base::print("", keyFormatter); - } + /** print contents */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "RangeFactor, range = " << measured_ << std::endl; + if (this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + Base::print("", keyFormatter); + } - private: +private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); - } - }; // RangeFactor + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } +}; +// RangeFactor -} // namespace gtsam +}// namespace gtsam From f7ebe4bfc46ec8d713e64c64dec7d803d85d7e7f Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 4 Dec 2014 14:08:46 +0100 Subject: [PATCH 70/76] Fixed RangeFactor --- .cproject | 120 +++++++++++++++------------ gtsam/slam/RangeFactor.h | 110 +++++++++++++++++++----- gtsam/slam/tests/testRangeFactor.cpp | 91 ++++++++++++++------ 3 files changed, 225 insertions(+), 96 deletions(-) diff --git a/.cproject b/.cproject index 9c03c5b7d..59ff12b72 100644 --- a/.cproject +++ b/.cproject @@ -592,6 +592,7 @@ make + tests/testBayesTree.run true false @@ -599,6 +600,7 @@ make + testBinaryBayesNet.run true false @@ -646,6 +648,7 @@ make + testSymbolicBayesNet.run true false @@ -653,6 +656,7 @@ make + tests/testSymbolicFactor.run true false @@ -660,6 +664,7 @@ make + testSymbolicFactorGraph.run true false @@ -675,6 +680,7 @@ make + tests/testBayesTree true false @@ -848,6 +854,14 @@ true true + + make + -j4 + testSmartStereoProjectionPoseFactor.run + true + true + true + make -j5 @@ -1122,6 +1136,7 @@ make + testErrors.run true false @@ -1351,6 +1366,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1433,7 +1488,6 @@ make - testSimulated2DOriented.run true false @@ -1473,7 +1527,6 @@ make - testSimulated2D.run true false @@ -1481,7 +1534,6 @@ make - testSimulated3D.run true false @@ -1495,46 +1547,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1792,6 +1804,7 @@ cpack + -G DEB true false @@ -1799,6 +1812,7 @@ cpack + -G RPM true false @@ -1806,6 +1820,7 @@ cpack + -G TGZ true false @@ -1813,6 +1828,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2627,6 +2643,7 @@ make + testGraph.run true false @@ -2634,18 +2651,12 @@ make + testJunctionTree.run true false true - - make - testSymbolicBayesNetB.run - true - false - true - make -j5 @@ -2902,6 +2913,14 @@ true true + + make + -j4 + testRangeFactor.run + true + true + true + make -j2 @@ -3184,7 +3203,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 87bb77913..88c122f6e 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -26,12 +26,11 @@ namespace gtsam { * Binary factor for a range measurement * @addtogroup SLAM */ -template +template class RangeFactor: public NoiseModelFactor2 { private: double measured_; /** measurement */ - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame typedef RangeFactor This; typedef NoiseModelFactor2 Base; @@ -45,10 +44,8 @@ public: } /* Default constructor */ RangeFactor(Key key1, Key key2, double measured, - const SharedNoiseModel& model, boost::optional body_P_sensor = - boost::none) : - Base(model, key1, key2), measured_(measured), body_P_sensor_( - body_P_sensor) { + const SharedNoiseModel& model) : + Base(model, key1, key2), measured_(measured) { } virtual ~RangeFactor() { @@ -64,16 +61,92 @@ public: Vector evaluateError(const T1& t1, const T2& t2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { double hx; - if (body_P_sensor_) { - if (H1) { - Matrix H0; - hx = t1.compose(*body_P_sensor_, H0).range(t2, H1, H2); - *H1 = *H1 * H0; - } else { - hx = t1.compose(*body_P_sensor_).range(t2, H1, H2); - } + hx = t1.range(t2, H1, H2); + return (Vector(1) << hx - measured_).finished(); + } + + /** return the measured */ + double measured() const { + return measured_; + } + + /** equals specialized to this factor */ + virtual bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) + && fabs(this->measured_ - e->measured_) < tol; + } + + /** print contents */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "RangeFactor, range = " << measured_ << std::endl; + Base::print("", keyFormatter); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } +}; +// RangeFactor + +/** + * Binary factor for a range measurement, with a transform applied + * @addtogroup SLAM + */ +template +class RangeFactorWithTransform: public NoiseModelFactor2 { +private: + + double measured_; /** measurement */ + POSE body_P_sensor_; ///< The pose of the sensor in the body frame + + typedef RangeFactorWithTransform This; + typedef NoiseModelFactor2 Base; + + // Concept requirements for this factor + GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, T2) + +public: + + RangeFactorWithTransform() { + } /* Default constructor */ + + RangeFactorWithTransform(Key key1, Key key2, double measured, + const SharedNoiseModel& model, const POSE& body_P_sensor) : + Base(model, key1, key2), measured_(measured), body_P_sensor_( + body_P_sensor) { + } + + virtual ~RangeFactorWithTransform() { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** h(x)-z */ + Vector evaluateError(const POSE& t1, const T2& t2, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + double hx; + if (H1) { + Matrix H0; + hx = t1.compose(body_P_sensor_, H0).range(t2, H1, H2); + *H1 = *H1 * H0; } else { - hx = t1.range(t2, H1, H2); + hx = t1.compose(body_P_sensor_).range(t2, H1, H2); } return (Vector(1) << hx - measured_).finished(); } @@ -89,17 +162,14 @@ public: const This *e = dynamic_cast(&expected); return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->measured_) < tol - && ((!body_P_sensor_ && !e->body_P_sensor_) - || (body_P_sensor_ && e->body_P_sensor_ - && body_P_sensor_->equals(*e->body_P_sensor_))); + && body_P_sensor_.equals(e->body_P_sensor_); } /** print contents */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "RangeFactor, range = " << measured_ << std::endl; - if (this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); + this->body_P_sensor_.print(" sensor pose in body frame: "); Base::print("", keyFormatter); } diff --git a/gtsam/slam/tests/testRangeFactor.cpp b/gtsam/slam/tests/testRangeFactor.cpp index 6bffa3ce9..a8455d685 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/slam/tests/testRangeFactor.cpp @@ -34,14 +34,30 @@ static SharedNoiseModel model(noiseModel::Unit::Create(1)); typedef RangeFactor RangeFactor2D; typedef RangeFactor RangeFactor3D; +typedef RangeFactorWithTransform RangeFactorWithTransform2D; +typedef RangeFactorWithTransform RangeFactorWithTransform3D; /* ************************************************************************* */ -Vector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) { +Vector factorError2D(const Pose2& pose, const Point2& point, + const RangeFactor2D& factor) { return factor.evaluateError(pose, point); } /* ************************************************************************* */ -Vector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) { +Vector factorError3D(const Pose3& pose, const Point3& point, + const RangeFactor3D& factor) { + return factor.evaluateError(pose, point); +} + +/* ************************************************************************* */ +Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point, + const RangeFactorWithTransform2D& factor) { + return factor.evaluateError(pose, point); +} + +/* ************************************************************************* */ +Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point, + const RangeFactorWithTransform3D& factor) { return factor.evaluateError(pose, point); } @@ -61,10 +77,13 @@ TEST( RangeFactor, ConstructorWithTransform) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2); - Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); - RangeFactor2D factor2D(poseKey, pointKey, measurement, model, body_P_sensor_2D); - RangeFactor3D factor3D(poseKey, pointKey, measurement, model, body_P_sensor_3D); + RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model, + body_P_sensor_2D); + RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model, + body_P_sensor_3D); } /* ************************************************************************* */ @@ -90,14 +109,19 @@ TEST( RangeFactor, EqualsWithTransform ) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2); - Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); - RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model, body_P_sensor_2D); - RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model, body_P_sensor_2D); + RangeFactorWithTransform2D factor2D_1(poseKey, pointKey, measurement, model, + body_P_sensor_2D); + RangeFactorWithTransform2D factor2D_2(poseKey, pointKey, measurement, model, + body_P_sensor_2D); CHECK(assert_equal(factor2D_1, factor2D_2)); - RangeFactor3D factor3D_1(poseKey, pointKey, measurement, model, body_P_sensor_3D); - RangeFactor3D factor3D_2(poseKey, pointKey, measurement, model, body_P_sensor_3D); + RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model, + body_P_sensor_3D); + RangeFactorWithTransform3D factor3D_2(poseKey, pointKey, measurement, model, + body_P_sensor_3D); CHECK(assert_equal(factor3D_1, factor3D_2)); } @@ -130,7 +154,8 @@ TEST( RangeFactor, Error2DWithTransform ) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor(0.25, -0.10, -M_PI_2); - RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor); + RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot2 R(0.57); @@ -176,8 +201,10 @@ TEST( RangeFactor, Error3DWithTransform ) { Key poseKey(1); Key pointKey(2); double measurement(10.0); - Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75); @@ -213,8 +240,10 @@ TEST( RangeFactor, Jacobian2D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorError2D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorError2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -228,7 +257,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor(0.25, -0.10, -M_PI_2); - RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor); + RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot2 R(0.57); @@ -242,8 +272,10 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -268,8 +300,10 @@ TEST( RangeFactor, Jacobian3D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorError3D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorError3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -282,8 +316,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { Key poseKey(1); Key pointKey(2); double measurement(10.0); - Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75); @@ -297,8 +333,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -306,6 +344,9 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ From bd6f210b877a42868ca3bab880e506a3954577d1 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Thu, 4 Dec 2014 09:36:00 -0500 Subject: [PATCH 71/76] changed << to =. gives error because range() was removed from PinholeCamera.h ? --- gtsam/geometry/Cal3_S2.h | 4 ++-- gtsam/geometry/Point3.cpp | 12 ++++-------- gtsam/geometry/Point3.h | 4 ++-- gtsam/geometry/Pose2.cpp | 22 +++++++++++----------- gtsam/geometry/Pose3.cpp | 16 +++++++--------- gtsam/geometry/Rot2.h | 8 ++++---- gtsam/geometry/Rot3.cpp | 4 ++-- gtsam/geometry/Rot3M.cpp | 14 +++++++------- 8 files changed, 39 insertions(+), 45 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 3896ccb43..cf358d0b2 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -173,8 +173,8 @@ public: inline Cal3_S2 between(const Cal3_S2& q, OptionalJacobian<5,5> H1=boost::none, OptionalJacobian<5,5> H2=boost::none) const { - if(H1) *H1 << -I_5x5; - if(H2) *H2 << I_5x5; + if(H1) *H1 = -I_5x5; + if(H2) *H2 = I_5x5; return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_); } diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 0d84f32fe..330fafb97 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -65,20 +65,16 @@ Point3 Point3::operator/(double s) const { /* ************************************************************************* */ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { - if (H1) - *H1 << I_3x3; - if (H2) - *H2 << I_3x3; + if (H1) *H1 = I_3x3; + if (H2) *H2 = I_3x3; return *this + q; } /* ************************************************************************* */ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { - if (H1) - *H1 << I_3x3; - if (H2) - *H2 << I_3x3; + if (H1) *H1 = I_3x3; + if (H2) *H2 = I_3x3; return *this - q; } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 24a3486ae..56d9b8bef 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -107,8 +107,8 @@ namespace gtsam { inline Point3 between(const Point3& p2, OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const { - if(H1) *H1 << -I_3x3; - if(H2) *H2 << I_3x3; + if(H1) *H1 = -I_3x3; + if(H2) *H2 = I_3x3; return p2 - *this; } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index c882d7156..c9aab4185 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -125,7 +125,7 @@ Matrix3 Pose2::AdjointMap() const { /* ************************************************************************* */ Pose2 Pose2::inverse(OptionalJacobian<3,3> H1) const { - if (H1) *H1 << -AdjointMap(); + if (H1) *H1 = -AdjointMap(); return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } @@ -148,8 +148,8 @@ Point2 Pose2::transform_to(const Point2& point, Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { // TODO: inline and reuse? - if(H1) *H1 << p2.inverse().AdjointMap(); - if(H2) *H2 << I_3x3; + if(H1) *H1 = p2.inverse().AdjointMap(); + if(H2) *H2 = I_3x3; return (*this)*p2; } @@ -163,7 +163,7 @@ Point2 Pose2::transform_from(const Point2& p, Matrix21 Drotate1; Drotate1 << -q.y(), q.x(); if (H1) *H1 << R, Drotate1; - if (H2) *H2 << R; // R + if (H2) *H2 = R; // R } return q + t_; } @@ -197,7 +197,7 @@ Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1, s, -c, dt2, 0.0, 0.0,-1.0; } - if (H2) *H2 << I_3x3; + if (H2) *H2 = I_3x3; return Pose2(R,t); } @@ -211,8 +211,8 @@ Rot2 Pose2::bearing(const Point2& point, if (!H1 && !H2) return Rot2::relativeBearing(d); Matrix12 D_result_d; Rot2 result = Rot2::relativeBearing(d, D_result_d); - if (H1) *H1 << D_result_d * (D1); - if (H2) *H2 << D_result_d * (D2); + if (H1) *H1 = D_result_d * (D1); + if (H2) *H2 = D_result_d * (D2); return result; } @@ -238,9 +238,9 @@ double Pose2::range(const Point2& point, Matrix23 H1_; H1_ << -r_.c(), r_.s(), 0.0, -r_.s(), -r_.c(), 0.0; - *H1 << H * H1_; + *H1 = H * H1_; } - if (H2) *H2 << H; + if (H2) *H2 = H; return r; } @@ -257,14 +257,14 @@ double Pose2::range(const Pose2& pose, H1_ << -r_.c(), r_.s(), 0.0, -r_.s(), -r_.c(), 0.0; - *H1 << H * H1_; + *H1 = H * H1_; } if (H2) { Matrix23 H2_; H2_ << pose.r_.c(), -pose.r_.s(), 0.0, pose.r_.s(), pose.r_.c(), 0.0; - *H2 << H * H2_; + *H2 = H * H2_; } return r; } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 3158c95e8..0bc07b753 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -274,14 +274,14 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, /* ************************************************************************* */ Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1, OptionalJacobian<6,6> H2) const { - if (H1) *H1 << p2.inverse().AdjointMap(); - if (H2) *H2 << I_6x6; + if (H1) *H1 = p2.inverse().AdjointMap(); + if (H2) *H2 = I_6x6; return (*this) * p2; } /* ************************************************************************* */ Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const { - if (H1) *H1 << -AdjointMap(); + if (H1) *H1 = -AdjointMap(); Rot3 Rt = R_.inverse(); return Pose3(Rt, Rt * (-t_)); } @@ -291,8 +291,8 @@ Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const { Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1, OptionalJacobian<6,6> H2) const { Pose3 result = inverse() * p2; - if (H1) *H1 << -result.inverse().AdjointMap(); - if (H2) *H2 << I_6x6; + if (H1) *H1 = -result.inverse().AdjointMap(); + if (H2) *H2 = I_6x6; return result; } @@ -309,10 +309,8 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, n = sqrt(d2); Matrix13 D_result_d; D_result_d << x / n, y / n, z / n; - if (H1) - *H1 = D_result_d * D1; - if (H2) - *H2 = D_result_d * D2; + if (H1) *H1 = D_result_d * D1; + if (H2) *H2 = D_result_d * D2; return n; } } diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 33685bc7c..744e246b3 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -118,8 +118,8 @@ namespace gtsam { /** Compose - make a new rotation by adding angles */ inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 = boost::none, OptionalJacobian<1,1> H2 = boost::none) const { - if (H1) *H1 << 1; // set to 1x1 identity matrix - if (H2) *H2 << 1; // set to 1x1 identity matrix + if (H1) *H1 = I_1x1; // set to 1x1 identity matrix + if (H2) *H2 = I_1x1; // set to 1x1 identity matrix return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); } @@ -131,8 +131,8 @@ namespace gtsam { /** Between using the default implementation */ inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 = boost::none, OptionalJacobian<1,1> H2 = boost::none) const { - if (H1) *H1 << -1; // set to 1x1 identity matrix - if (H2) *H2 << 1; // set to 1x1 identity matrix + if (H1) *H1 = -I_1x1; // set to 1x1 identity matrix + if (H2) *H2 = I_1x1; // set to 1x1 identity matrix return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_); } diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 5d7a2a473..232661360 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -73,7 +73,7 @@ Unit3 Rot3::rotate(const Unit3& p, OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { Matrix32 Dp; Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0))); - if (Hp) *Hp << q.basis().transpose() * matrix() * Dp; + if (Hp) *Hp = q.basis().transpose() * matrix() * Dp; if (HR) *HR = -q.basis().transpose() * matrix() * p.skew(); return q; } @@ -83,7 +83,7 @@ Unit3 Rot3::unrotate(const Unit3& p, OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { Matrix32 Dp; Unit3 q = Unit3(unrotate(p.point3(Dp))); - if (Hp) *Hp << q.basis().transpose() * matrix().transpose () * Dp; + if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp; if (HR) *HR = q.basis().transpose() * q.skew(); return q; } diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 6171d3347..fc3490fb5 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -141,8 +141,8 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) { /* ************************************************************************* */ Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - if (H1) *H1 << R2.transpose(); - if (H2) *H2 << I_3x3; + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I_3x3; return *this * R2; } @@ -159,15 +159,15 @@ Matrix3 Rot3::transpose() const { /* ************************************************************************* */ Rot3 Rot3::inverse(boost::optional H1) const { - if (H1) *H1 << -rot_; + if (H1) *H1 = -rot_; return Rot3(Matrix3(transpose())); } /* ************************************************************************* */ Rot3 Rot3::between (const Rot3& R2, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { - if (H1) *H1 << -(R2.transpose()*rot_); - if (H2) *H2 << I_3x3; + if (H1) *H1 = -(R2.transpose()*rot_); + if (H2) *H2 = I_3x3; Matrix3 R12 = transpose()*R2.rot_; return Rot3(R12); } @@ -176,8 +176,8 @@ Rot3 Rot3::between (const Rot3& R2, Point3 Rot3::rotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1 || H2) { - if (H1) *H1 << rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 << rot_; + if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = rot_; } return Point3(rot_ * p.vector()); } From 523ebb7b6fe993d61942398ed63303fce962d8fd Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 4 Dec 2014 11:18:20 -0500 Subject: [PATCH 72/76] moved TriangulationFactor to slam folder --- gtsam/geometry/tests/testTriangulation.cpp | 19 ----- gtsam/geometry/triangulation.h | 6 +- .../{geometry => slam}/TriangulationFactor.h | 3 +- gtsam/slam/tests/testTriangulationFactor.cpp | 70 +++++++++++++++++++ 4 files changed, 73 insertions(+), 25 deletions(-) rename gtsam/{geometry => slam}/TriangulationFactor.h (98%) create mode 100644 gtsam/slam/tests/testTriangulationFactor.cpp diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 0bd553a40..f986cca1d 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -262,25 +262,6 @@ TEST( triangulation, twoIdenticalPoses) { } */ -//****************************************************************************** -TEST( triangulation, TriangulationFactor ) { - // Create the factor with a measurement that is 3 pixels off in x - Key pointKey(1); - SharedNoiseModel model; - typedef TriangulationFactor<> Factor; - Factor factor(camera1, z1, model, pointKey); - - // Use the factor to calculate the Jacobians - Matrix HActual; - factor.evaluateError(landmark, HActual); - - Matrix HExpected = numericalDerivative11( - boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); - - // Verify the Jacobians are correct - CHECK(assert_equal(HExpected, HActual, 1e-3)); -} - //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 798a7e9dc..ce83f780b 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -19,12 +19,10 @@ #pragma once -#include +#include +#include #include #include -#include - -#include namespace gtsam { diff --git a/gtsam/geometry/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h similarity index 98% rename from gtsam/geometry/TriangulationFactor.h rename to gtsam/slam/TriangulationFactor.h index fc8d546d3..b7f6a20dc 100644 --- a/gtsam/geometry/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -10,14 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * testTriangulationFactor.h + * triangulationFactor.h * @date March 2, 2014 * @author Frank Dellaert */ #include #include -#include #include #include diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp new file mode 100644 index 000000000..6b79171df --- /dev/null +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -0,0 +1,70 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * testTriangulationFactor.cpp + * + * Created on: July 30th, 2013 + * Author: cbeall3 + */ + +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +// Some common constants +static const boost::shared_ptr sharedCal = // + boost::make_shared(1500, 1200, 0, 640, 480); + +// Looking along X-axis, 1 meter above ground plane (x-y) +static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2); +static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); +PinholeCamera camera1(pose1, *sharedCal); + +// landmark ~5 meters infront of camera +static const Point3 landmark(5, 0.5, 1.2); + +// 1. Project two landmarks into two cameras and triangulate +Point2 z1 = camera1.project(landmark); + +//****************************************************************************** +TEST( triangulation, TriangulationFactor ) { + // Create the factor with a measurement that is 3 pixels off in x + Key pointKey(1); + SharedNoiseModel model; + typedef TriangulationFactor<> Factor; + Factor factor(camera1, z1, model, pointKey); + + // Use the factor to calculate the Jacobians + Matrix HActual; + factor.evaluateError(landmark, HActual); + + Matrix HExpected = numericalDerivative11( + boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); + + // Verify the Jacobians are correct + CHECK(assert_equal(HExpected, HActual, 1e-3)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** From 4fd2a288a24323a6b6dc184299a76133cc952313 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 4 Dec 2014 21:08:15 +0100 Subject: [PATCH 73/76] Expressions now require OptionalJacobian --- gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 1107d1e30..5758509eb 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -131,14 +131,14 @@ TEST(ExpressionFactor, Unary) { // Unary(Leaf)) and Unary(Unary(Leaf))) // wide version (not handled in fixed-size pipeline) typedef Eigen::Matrix Matrix93; -Vector9 wide(const Point3& p, boost::optional H) { +Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) { Vector9 v; v << p.vector(), p.vector(), p.vector(); if (H) *H << eye(3), eye(3), eye(3); return v; } typedef Eigen::Matrix Matrix9; -Vector9 id9(const Vector9& v, boost::optional H) { +Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) { if (H) *H = Matrix9::Identity(); return v; } From 02075b75755400f0f597ba1ee2cc8d4e61e5de41 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 4 Dec 2014 21:08:31 +0100 Subject: [PATCH 74/76] Moved to project --- .../tests/testBasisDecompositions.cpp | 231 ------------------ 1 file changed, 231 deletions(-) delete mode 100644 gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp diff --git a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp deleted file mode 100644 index c3bf83f71..000000000 --- a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp +++ /dev/null @@ -1,231 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testBasisDecompositions.cpp - * @date November 23, 2014 - * @author Frank Dellaert - * @brief unit tests for Basis Decompositions w Expressions - */ - -#include - -namespace gtsam { - -/// Fourier -template -class Fourier { - -public: - - typedef Eigen::Matrix Coefficients; - typedef Eigen::Matrix Jacobian; - -private: - - double x_; - Jacobian H_; - -public: - - /// Constructor - Fourier(double x) : - x_(x) { - H_(0, 0) = 1; - for (size_t i = 1; i < N; i += 2) { - H_(0, i) = cos(i * x); - H_(0, i + 1) = sin(i * x); - } - } - - /// Given coefficients c, predict value for x - double operator()(const Coefficients& c, OptionalJacobian<1, N> H) { - if (H) - (*H) = H_; - return H_ * c; - } -}; - -} - -#include -#include -#include -#include - -namespace gtsam { - -/// For now, this is our sequence representation -typedef std::map Sequence; - -/** - * Class that does Fourier Decomposition via least squares - */ -class FourierDecomposition { -public: - - typedef Vector3 Coefficients; ///< Fourier coefficients - -private: - Coefficients c_; - -public: - - /// Create nonlinear FG from Sequence - static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence, - const SharedNoiseModel& model) { - NonlinearFactorGraph graph; - Expression c(0); - typedef std::pair Sample; - BOOST_FOREACH(Sample sample, sequence) { - Expression expression(Fourier<3>(sample.first), c); - ExpressionFactor factor(model, sample.second, expression); - graph.add(factor); - } - return graph; - } - - /// Create linear FG from Sequence - static GaussianFactorGraph::shared_ptr LinearGraph(const Sequence& sequence, - const SharedNoiseModel& model) { - NonlinearFactorGraph graph = NonlinearGraph(sequence, model); - Values values; - values.insert(0, Coefficients::Zero()); // does not matter - GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); - return gfg; - } - - /// Constructor - FourierDecomposition(const Sequence& sequence, - const SharedNoiseModel& model) { - GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model); - VectorValues solution = gfg->optimize(); - c_ = solution.at(0); - } - - /// Return Fourier coefficients - Coefficients coefficients() { - return c_; - } -}; - -} - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); - -//****************************************************************************** -TEST(BasisDecompositions, Fourier) { - Fourier<3> fx(0); - Eigen::Matrix expectedH, actualH; - Vector3 c(1.5661, 1.2717, 1.2717); - expectedH = numericalDerivative11( - boost::bind(&Fourier<3>::operator(), fx, _1, boost::none), c); - EXPECT_DOUBLES_EQUAL(c[0]+c[1], fx(c,actualH), 1e-9); - EXPECT(assert_equal((Matrix)expectedH, actualH)); -} - -//****************************************************************************** -TEST(BasisDecompositions, ManualFourier) { - - // Create linear factor graph - GaussianFactorGraph g; - Key key(1); - Expression c(key); - Values values; - values.insert(key, Vector3::Zero()); // does not matter - for (size_t i = 0; i < 16; i++) { - double x = i * M_PI / 8, y = exp(sin(x) + cos(x)); - - // Manual JacobianFactor - Matrix A(1, 3); - A << 1, cos(x), sin(x); - Vector b(1); - b << y; - JacobianFactor f1(key, A, b); - g.add(f1); - - // With ExpressionFactor - Expression expression(Fourier<3>(x), c); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, 1e-5, 1e-9); - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - } - - // Solve - VectorValues actual = g.optimize(); - - // Check - Vector3 expected(1.5661, 1.2717, 1.2717); - EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4)); -} - -//****************************************************************************** -TEST(BasisDecompositions, FourierDecomposition) { - - // Create example sequence - Sequence sequence; - for (size_t i = 0; i < 16; i++) { - double x = i * M_PI / 8, y = exp(sin(x) + cos(x)); - sequence[x] = y; - } - - // Do Fourier Decomposition - FourierDecomposition actual(sequence, model); - - // Check - Vector3 expected(1.5661, 1.2717, 1.2717); - EXPECT(assert_equal((Vector) expected, actual.coefficients(),1e-4)); -} - -//****************************************************************************** -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -//****************************************************************************** - From f5db91a56fefdcc49a91827d2cc26f5d061d8210 Mon Sep 17 00:00:00 2001 From: Natesh Srinivasan Date: Fri, 5 Dec 2014 11:08:13 -0500 Subject: [PATCH 75/76] works on gcc 4.8 --- gtsam/geometry/PinholeCamera.h | 18 +++++++++++------- gtsam/geometry/tests/testPinholeCamera.cpp | 9 ++++----- 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 1700b4960..9b881dc78 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -397,16 +397,20 @@ public: double range( const PinholeCamera& camera, // OptionalJacobian<1, DimC> Dcamera = boost::none, - OptionalJacobian<1, 6 + traits::dimension::value> Dother = +// OptionalJacobian<1, 6 + traits::dimension::value> Dother = + boost::optional Dother = boost::none) const { - Matrix16 Dpose_, Dpose2; - double result = pose_.range(camera.pose(), Dcamera ? &Dpose_ : 0, - Dother ? &Dpose2 : 0); - if (Dcamera) - *Dcamera << Dpose_, Eigen::Matrix::Zero(); + Matrix16 Dcamera_, Dother_; + double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0, + Dother ? &Dother_ : 0); + if (Dcamera) { + Dcamera->resize(1, 6 + DimK); + *Dcamera << Dcamera_, Eigen::Matrix::Zero(); + } if (Dother) { + Dother->resize(1, 6+traits::dimension::value); Dother->setZero(); - Dother->block(0, 0, 1, 6) = Dpose2; + Dother->block(0, 0, 1, 6) = Dother_; } return result; } diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 1cfb0ba25..6cb84ec85 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -229,9 +229,8 @@ static double range0(const Camera& camera, const Point3& point) { } /* ************************************************************************* */ -typedef Eigen::Matrix Matrix1_11; TEST( PinholeCamera, range0) { - Matrix1_11 D1; Matrix13 D2; + Matrix D1; Matrix D2; double result = camera.range(point1, D1, D2); Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); @@ -248,7 +247,7 @@ static double range1(const Camera& camera, const Pose3& pose) { /* ************************************************************************* */ TEST( PinholeCamera, range1) { - Matrix1_11 D1; Matrix16 D2; + Matrix D1; Matrix D2; double result = camera.range(pose1, D1, D2); Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1); Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1); @@ -267,7 +266,7 @@ static double range2(const Camera& camera, const Camera2& camera2) { /* ************************************************************************* */ TEST( PinholeCamera, range2) { - Matrix1_11 D1; Matrix19 D2; + Matrix D1; Matrix D2; double result = camera.range(camera2, D1, D2); Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2); Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2); @@ -284,7 +283,7 @@ static double range3(const Camera& camera, const CalibratedCamera& camera3) { /* ************************************************************************* */ TEST( PinholeCamera, range3) { - Matrix1_11 D1; Matrix16 D2; + Matrix D1; Matrix D2; double result = camera.range(camera3, D1, D2); Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3); Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3); From a5877a96b89e949615a15af4a0fc72d63189b9f9 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Fri, 5 Dec 2014 16:01:10 -0500 Subject: [PATCH 76/76] @dellaert. Works with quaternion switch on in gcc 4.7, 4.8 and MSVC 2012. Pls Merge if compiles on MAC --- gtsam/geometry/Rot3Q.cpp | 21 ++++----------------- 1 file changed, 4 insertions(+), 17 deletions(-) diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 19de92ca2..26ca25bf2 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -87,22 +87,9 @@ namespace gtsam { Rot3 Rot3::rodriguez(const Vector& w, double theta) { return Quaternion(Eigen::AngleAxisd(theta, w)); } - /* ************************************************************************* */ - Rot3 Rot3::compose(const Rot3& R2) const { - return Rot3(quaternion_ * R2.quaternion_); - } - /* ************************************************************************* */ Rot3 Rot3::compose(const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return Rot3(quaternion_ * R2.quaternion_); - } - - /* ************************************************************************* */ - Rot3 Rot3::compose(const Rot3& R2, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = R2.transpose(); if (H2) *H2 = I3; return Rot3(quaternion_ * R2.quaternion_); @@ -114,7 +101,7 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::inverse(boost::optional H1) const { + Rot3 Rot3::inverse(boost::optional H1) const { if (H1) *H1 = -matrix(); return Rot3(quaternion_.inverse()); } @@ -129,7 +116,7 @@ namespace gtsam { /* ************************************************************************* */ Rot3 Rot3::between(const Rot3& R2, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = -(R2.transpose()*matrix()); if (H2) *H2 = I3; return between_default(*this, R2); @@ -137,7 +124,7 @@ namespace gtsam { /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { Matrix R = matrix(); if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = R;