From 1e76082888d316503174e12738b76c47ceb11f3e Mon Sep 17 00:00:00 2001 From: jing Date: Sun, 9 Mar 2014 00:40:02 -0500 Subject: [PATCH 001/101] add unified projection model: a calibration class for omni-directional camera. Not test yet --- gtsam/geometry/Cal3Unify.cpp | 201 +++++++++++++++++++++++++++++++++++ gtsam/geometry/Cal3Unify.h | 168 +++++++++++++++++++++++++++++ 2 files changed, 369 insertions(+) create mode 100644 gtsam/geometry/Cal3Unify.cpp create mode 100644 gtsam/geometry/Cal3Unify.h diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp new file mode 100644 index 000000000..d49417492 --- /dev/null +++ b/gtsam/geometry/Cal3Unify.cpp @@ -0,0 +1,201 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3Unify.cpp + * @date Mar 8, 2014 + * @author Jing Dong + */ + +#include +#include +#include +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +Cal3Unify::Cal3Unify(const Vector &v): + xi_(v[0]), fx_(v[1]), fy_(v[2]), s_(v[3]), u0_(v[4]), v0_(v[5]), k1_(v[6]), k2_(v[7]), k3_(v[8]), k4_(v[9]){} + +/* ************************************************************************* */ +Matrix Cal3Unify::K() const { + return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); +} + +/* ************************************************************************* */ +Vector Cal3Unify::vector() const { + return (Vector(10) << xi_, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); +} + +/* ************************************************************************* */ +void Cal3Unify::print(const std::string& s) const { + gtsam::print(K(), s + ".K"); + gtsam::print(Vector(k()), s + ".k"); + gtsam::print(Vector(xi_), s + ".xi"); +} + +/* ************************************************************************* */ +bool Cal3Unify::equals(const Cal3Unify& K, double tol) const { + if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || + fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || + fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol || + fabs(xi_ - K.xi_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +Point2 Cal3Unify::uncalibrate(const Point2& p, + boost::optional H1, + boost::optional H2) const { + + // this part of code is modified from Cal3DS2, + // since the second part of this model (after project to normalized plane) + // is same as Cal3DS2 + + // parameters + const double xi = xi_, fx = fx_, fy = fy_, s = s_; + const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_; + + // Part1: project 3D space to NPlane + const double xs = p.x(), ys = p.y(); // normalized points in 3D space + const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0); + const double xi_sqrt_nx = 1 + xi * sqrt_nx; + const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx; + const double x = xs / xi_sqrt_nx, y = ys / xi_sqrt_nx; // points on NPlane + + // Part2: project NPlane point to pixel plane: same as Cal3DS2 + const double xy = x * y, xx = x * x, yy = y * y; + const double rr = xx + yy; + const double r4 = rr * rr; + const double g = 1. + k1 * rr + k2 * r4; + const double dx = 2. * k3 * xy + k4 * (rr + 2. * xx); + const double dy = 2. * k4 * xy + k3 * (rr + 2. * yy); + + const double pnx = g*x + dx; + const double pny = g*y + dy; + + // DDS2 will be used both in H1 and H2 + Matrix DDS2; + if (H1 || H2) { + // part2 + const double dr_dx = 2. * x; + const double dr_dy = 2. * y; + const double dg_dx = k1 * dr_dx + k2 * 2. * rr * dr_dx; + const double dg_dy = k1 * dr_dy + k2 * 2. * rr * dr_dy; + + const double dDx_dx = 2. * k3 * y + k4 * (dr_dx + 4. * x); + const double dDx_dy = 2. * k3 * x + k4 * dr_dy; + const double dDy_dx = 2. * k4 * y + k3 * dr_dx; + const double dDy_dy = 2. * k4 * x + k3 * (dr_dy + 4. * y); + + Matrix DK = (Matrix(2, 2) << fx, s_, 0.0, fy); + Matrix DR = (Matrix(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy, + y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy); + + DDS2 = DK * DR; + } + + // Inlined derivative for calibration + if (H1) { + // part1 + Matrix DU = (Matrix(2,1) << xs * sqrt_nx / xi_sqrt_nx2, + ys * sqrt_nx / xi_sqrt_nx2); + Matrix DDS2U = DDS2 * DU; + // part2 + Matrix DDS2V = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr, + fx * x * r4 + s * y * r4, fx * 2. * xy + s * (rr + 2. * yy), + fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0, + fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy)); + + *H1 = collect(2, &DDS2U, &DDS2V); + } + // Inlined derivative for points + if (H2) { + // part1 + Matrix DU = (Matrix(2, 2) << (xi_sqrt_nx - xs * xs / sqrt_nx) / xi_sqrt_nx2, + -(ys * ys / (sqrt_nx * xi_sqrt_nx2)), + -(xs * xs / (sqrt_nx * xi_sqrt_nx2)), + (xi_sqrt_nx - ys * ys / sqrt_nx) / xi_sqrt_nx2); + + *H2 = DDS2 * DU; + } + + + return Point2(fx * pnx + s * pny + u0_, fy * pny + v0_); +} + +/* ************************************************************************* */ +Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { + // Use the following fixed point iteration to invert the radial distortion. + // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t}) + + // point on the normalized plane, input for DS2 + Point2 pnpl = this->imageToNPlane(pi); + double px = pnpl.x(); + double py = pnpl.y(); + const Point2 invKPi ((1 / fx_) * (px - u0_ - (s_ / fy_) * (py - v0_)), + (1 / fy_) * (py - v0_)); + + // initialize by ignoring the distortion at all, might be problematic for pixels around boundary + Point2 pn = invKPi; + + // iterate until the uncalibrate is close to the actual pixel coordinate + const int maxIterations = 10; + int iteration; + for ( iteration = 0; iteration < maxIterations; ++iteration ) { + + if ( uncalibrate(pn).distance(pi) <= tol ) break; + + // part1: image -> normalized plane + pnpl = this->imageToNPlane(pn); + // part2: normalized plane -> 3D space + px = pnpl.x(), py = pnpl.y(); + const double xy = px*py, xx = px*px, yy = py*py; + const double rr = xx + yy; + const double g = (1+k1_*rr+k2_*rr*rr); + const double dx = 2*k3_*xy + k4_*(rr+2*xx); + const double dy = 2*k4_*xy + k3_*(rr+2*yy); + pn = (invKPi - Point2(dx,dy))/g; + } + + if ( iteration >= maxIterations ) + throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization"); + + return pn; +} +/* ************************************************************************* */ +Point2 Cal3Unify::imageToNPlane(const Point2& p) const { + + const double x = p.x(), y = p.y(); + const double xy2 = x * x + y * y; + const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1); + + return Point2((sq_xy * x / (sq_xy - xi_)), (sq_xy * y / (sq_xy - xi_))); +} + +/* ************************************************************************* */ +Cal3Unify Cal3Unify::retract(const Vector& d) const { + return Cal3Unify(vector() + d); +} + +/* ************************************************************************* */ +Vector Cal3Unify::localCoordinates(const Cal3Unify& T2) const { + return T2.vector() - vector(); +} + +} +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/Cal3Unify.h b/gtsam/geometry/Cal3Unify.h new file mode 100644 index 000000000..58f19ece5 --- /dev/null +++ b/gtsam/geometry/Cal3Unify.h @@ -0,0 +1,168 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3Unify.h + * @brief Unified Calibration Model, see Mei07icra for details + * @date Mar 8, 2014 + * @author Jing Dong + */ + +/** + * @addtogroup geometry + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * @brief Calibration of a omni-directional camera with mirror + lens radial distortion + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Cal3Unify : public DerivedValue { + +private: + + double xi_; // mirror parameter + double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point + double k1_, k2_ ; // radial 2nd-order and 4th-order + double k3_, k4_ ; // tangential distortion + + // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] + // Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})] + // rr = Pn.x^2 + Pn.y^2 + // \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; + // k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] + // pi = K*pn + +public: + Matrix K() const ; + Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, k3_, k4_); } + Vector vector() const ; + + /// @name Standard Constructors + /// @{ + + /// Default Constructor with only unit focal length + Cal3Unify() : xi_(0), fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {} + + Cal3Unify(double xi, double fx, double fy, double s, double u0, double v0, + double k1, double k2, double k3 = 0.0, double k4 = 0.0) : + xi_(xi), fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {} + + /// @} + /// @name Advanced Constructors + /// @{ + + Cal3Unify(const Vector &v) ; + + /// @} + /// @name Testable + /// @{ + + /// print with optional string + void print(const std::string& s = "") const ; + + /// assert equality up to a tolerance + bool equals(const Cal3Unify& K, double tol = 10e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// mirror parameter + inline double xi() const { return xi_;} + + /// focal length x + inline double fx() const { return fx_;} + + /// focal length y + inline double fy() const { return fy_;} + + /// skew + inline double skew() const { return s_;} + + /// image center in x + inline double px() const { return u0_;} + + /// image center in y + inline double py() const { return v0_;} + + /** + * convert intrinsic coordinates xy to image coordinates uv + * @param p point in intrinsic coordinates + * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters + * @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 ; + + /// Conver a pixel coordinate to ideal coordinate + Point2 calibrate(const Point2& p, const double tol=1e-5) const; + + /// Convert a image point to normalized unit plane + Point2 imageToNPlane(const Point2& p) const; + + /// @} + /// @name Manifold + /// @{ + + /// Given delta vector, update calibration + Cal3Unify retract(const Vector& d) const ; + + /// Given a different calibration, calculate update to obtain it + Vector localCoordinates(const Cal3Unify& T2) const ; + + /// Return dimensions of calibration manifold object + virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + + /// Return dimensions of calibration manifold object + static size_t Dim() { return 10; } //TODO: make a final dimension variable + +private: + + /// @} + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & boost::serialization::make_nvp("Cal3Unify", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(xi_); + ar & BOOST_SERIALIZATION_NVP(fx_); + ar & BOOST_SERIALIZATION_NVP(fy_); + ar & BOOST_SERIALIZATION_NVP(s_); + ar & BOOST_SERIALIZATION_NVP(u0_); + ar & BOOST_SERIALIZATION_NVP(v0_); + ar & BOOST_SERIALIZATION_NVP(k1_); + ar & BOOST_SERIALIZATION_NVP(k2_); + ar & BOOST_SERIALIZATION_NVP(k3_); + ar & BOOST_SERIALIZATION_NVP(k4_); + } + + + /// @} + +}; + +} + From e311e993ad8956ca64f3d48d1b3fd8f9745544e5 Mon Sep 17 00:00:00 2001 From: jing Date: Sun, 9 Mar 2014 01:18:06 -0500 Subject: [PATCH 002/101] add unit test --- gtsam/geometry/tests/testCal3Unify.cpp | 91 ++++++++++++++++++++++++++ 1 file changed, 91 insertions(+) create mode 100644 gtsam/geometry/tests/testCal3Unify.cpp diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp new file mode 100644 index 000000000..775f95159 --- /dev/null +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCal3Unify.cpp + * @brief Unit tests for transform derivatives + */ + +#include +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify) + +/* ground truth data will from matlab, code : +X = [2 3 1]'; +V = [0.01, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; +[P, J] = spaceToImgPlane(X, V); +matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */ + +static Cal3Unify K(0.01, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); +static Point2 p(2,3); + +/* ************************************************************************* */ +TEST( Cal3Unify, uncalibrate) +{ + Point2 p_i(582.5228344366194, 649.6685266099726) ; + Point2 q = K.uncalibrate(p); + CHECK(assert_equal(q,p_i)); +} + +TEST( Cal3Unify, calibrate ) +{ + Point2 pi = K.uncalibrate(p); + Point2 pn_hat = K.calibrate(pi); + CHECK( p.equals(pn_hat, 1e-5)); +} + +Point2 uncalibrate_(const Cal3Unify& k, const Point2& pt) { return k.uncalibrate(pt); } + +/* ************************************************************************* */ +TEST( Cal3Unify, Duncalibrate1) +{ + Matrix computed; + K.uncalibrate(p, computed, boost::none); + Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); + CHECK(assert_equal(numerical,computed,1e-5)); +} + +/* ************************************************************************* */ +TEST( Cal3Unify, Duncalibrate2) +{ + Matrix computed; + K.uncalibrate(p, boost::none, computed); + Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); + CHECK(assert_equal(numerical,computed,1e-5)); +} + +/* ************************************************************************* */ +TEST( Cal3Unify, assert_equal) +{ + CHECK(assert_equal(K,K,1e-5)); +} + +/* ************************************************************************* */ +TEST( Cal3Unify, retract) +{ + Cal3Unify expected(0.01 + 1, 100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10); + Vector d(10); + d << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10; + Cal3Unify actual = K.retract(d); + CHECK(assert_equal(expected,actual,1e-7)); + CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ From b43b84b59a20e2fc98907e613f1ca13c0ae56b3d Mon Sep 17 00:00:00 2001 From: jing Date: Sun, 9 Mar 2014 03:34:57 -0400 Subject: [PATCH 003/101] some bug fixed, still not pass unit test: calibrate() converge so slow, and Jacobians are buggy --- gtsam/geometry/Cal3Unify.cpp | 31 +++++++++++++++++--------- gtsam/geometry/Cal3Unify.h | 7 ++++-- gtsam/geometry/tests/testCal3Unify.cpp | 22 +++++++++++++----- 3 files changed, 42 insertions(+), 18 deletions(-) diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp index d49417492..1ddbcd2b2 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unify.cpp @@ -142,24 +142,24 @@ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t}) // point on the normalized plane, input for DS2 - Point2 pnpl = this->imageToNPlane(pi); - double px = pnpl.x(); - double py = pnpl.y(); + double px = pi.x(); + double py = pi.y(); const Point2 invKPi ((1 / fx_) * (px - u0_ - (s_ / fy_) * (py - v0_)), (1 / fy_) * (py - v0_)); // initialize by ignoring the distortion at all, might be problematic for pixels around boundary - Point2 pn = invKPi; + Point2 pn = nPlaneToSpace(invKPi); // iterate until the uncalibrate is close to the actual pixel coordinate - const int maxIterations = 10; + const int maxIterations = 100; int iteration; for ( iteration = 0; iteration < maxIterations; ++iteration ) { - if ( uncalibrate(pn).distance(pi) <= tol ) break; + pn.print(); + if ( pn.distance(pi) <= tol ) break; - // part1: image -> normalized plane - pnpl = this->imageToNPlane(pn); + // part1: 3D space -> normalized plane + Point2 pnpl = spaceToNPlane(pn); // part2: normalized plane -> 3D space px = pnpl.x(), py = pnpl.y(); const double xy = px*py, xx = px*px, yy = py*py; @@ -167,16 +167,16 @@ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { const double g = (1+k1_*rr+k2_*rr*rr); const double dx = 2*k3_*xy + k4_*(rr+2*xx); const double dy = 2*k4_*xy + k3_*(rr+2*yy); - pn = (invKPi - Point2(dx,dy))/g; + pn = nPlaneToSpace((invKPi - Point2(dx,dy))/g); } if ( iteration >= maxIterations ) - throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization"); + throw std::runtime_error("Cal3Unify::calibrate fails to converge. need a better initialization"); return pn; } /* ************************************************************************* */ -Point2 Cal3Unify::imageToNPlane(const Point2& p) const { +Point2 Cal3Unify::nPlaneToSpace(const Point2& p) const { const double x = p.x(), y = p.y(); const double xy2 = x * x + y * y; @@ -185,6 +185,15 @@ Point2 Cal3Unify::imageToNPlane(const Point2& p) const { return Point2((sq_xy * x / (sq_xy - xi_)), (sq_xy * y / (sq_xy - xi_))); } +/* ************************************************************************* */ +Point2 Cal3Unify::spaceToNPlane(const Point2& p) const { + + const double x = p.x(), y = p.y(); + const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1); + + return Point2((x / sq_xy), (y / sq_xy)); +} + /* ************************************************************************* */ Cal3Unify Cal3Unify::retract(const Vector& d) const { return Cal3Unify(vector() + d); diff --git a/gtsam/geometry/Cal3Unify.h b/gtsam/geometry/Cal3Unify.h index 58f19ece5..4d70db051 100644 --- a/gtsam/geometry/Cal3Unify.h +++ b/gtsam/geometry/Cal3Unify.h @@ -115,8 +115,11 @@ public: /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& p, const double tol=1e-5) const; - /// Convert a image point to normalized unit plane - Point2 imageToNPlane(const Point2& p) const; + /// Convert a 3D point to normalized unit plane + Point2 spaceToNPlane(const Point2& p) const; + + /// Convert a normalized unit plane point to 3D space + Point2 nPlaneToSpace(const Point2& p) const; /// @} /// @name Manifold diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp index 775f95159..9ff9d7646 100644 --- a/gtsam/geometry/tests/testCal3Unify.cpp +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -19,16 +19,20 @@ #include #include -using namespace gtsam; +#include +using namespace gtsam; +using namespace std; GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify) GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify) -/* ground truth data will from matlab, code : +/* +ground truth from matlab, code : X = [2 3 1]'; V = [0.01, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; [P, J] = spaceToImgPlane(X, V); -matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */ +matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox +*/ static Cal3Unify K(0.01, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); static Point2 p(2,3); @@ -41,10 +45,18 @@ TEST( Cal3Unify, uncalibrate) CHECK(assert_equal(q,p_i)); } -TEST( Cal3Unify, calibrate ) +/* ************************************************************************* */ +TEST( Cal3Unify, spaceNplane) +{ + Point2 q = K.spaceToNPlane(p); + CHECK(assert_equal(p, K.nPlaneToSpace(q))); +} + +/* ************************************************************************* */ +TEST( Cal3Unify, calibrate) { Point2 pi = K.uncalibrate(p); - Point2 pn_hat = K.calibrate(pi); + Point2 pn_hat = K.calibrate(pi, 1); CHECK( p.equals(pn_hat, 1e-5)); } From 4affb9a0d2d9a5ecc0141f6f0f4e7ce6d3cae4b4 Mon Sep 17 00:00:00 2001 From: jing Date: Mon, 10 Mar 2014 16:27:41 -0400 Subject: [PATCH 004/101] fix jacobians, calibration still buggy --- gtsam/geometry/Cal3Unify.cpp | 19 ++++++++++--------- gtsam/geometry/tests/testCal3Unify.cpp | 6 +++--- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp index 1ddbcd2b2..5f2e330bd 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unify.cpp @@ -42,7 +42,7 @@ Vector Cal3Unify::vector() const { void Cal3Unify::print(const std::string& s) const { gtsam::print(K(), s + ".K"); gtsam::print(Vector(k()), s + ".k"); - gtsam::print(Vector(xi_), s + ".xi"); + gtsam::print((Vector)(Vector(1) << xi_), s + ".xi"); } /* ************************************************************************* */ @@ -71,9 +71,9 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, // Part1: project 3D space to NPlane const double xs = p.x(), ys = p.y(); // normalized points in 3D space const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0); - const double xi_sqrt_nx = 1 + xi * sqrt_nx; + const double xi_sqrt_nx = 1.0 / (1 + xi * sqrt_nx); const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx; - const double x = xs / xi_sqrt_nx, y = ys / xi_sqrt_nx; // points on NPlane + const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane // Part2: project NPlane point to pixel plane: same as Cal3DS2 const double xy = x * y, xx = x * x, yy = y * y; @@ -110,8 +110,8 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, // Inlined derivative for calibration if (H1) { // part1 - Matrix DU = (Matrix(2,1) << xs * sqrt_nx / xi_sqrt_nx2, - ys * sqrt_nx / xi_sqrt_nx2); + Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2, + -ys * sqrt_nx * xi_sqrt_nx2); Matrix DDS2U = DDS2 * DU; // part2 Matrix DDS2V = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr, @@ -124,10 +124,11 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, // Inlined derivative for points if (H2) { // part1 - Matrix DU = (Matrix(2, 2) << (xi_sqrt_nx - xs * xs / sqrt_nx) / xi_sqrt_nx2, - -(ys * ys / (sqrt_nx * xi_sqrt_nx2)), - -(xs * xs / (sqrt_nx * xi_sqrt_nx2)), - (xi_sqrt_nx - ys * ys / sqrt_nx) / xi_sqrt_nx2); + const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx; + const double mid = -(xi * xs*ys) * denom; + Matrix DU = (Matrix(2, 2) << + (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, + mid, (sqrt_nx + xi*(xs*xs + 1)) * denom); *H2 = DDS2 * DU; } diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp index 9ff9d7646..3995e03b2 100644 --- a/gtsam/geometry/tests/testCal3Unify.cpp +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -35,7 +35,7 @@ matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */ static Cal3Unify K(0.01, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); -static Point2 p(2,3); +static Point2 p(2, 3); /* ************************************************************************* */ TEST( Cal3Unify, uncalibrate) @@ -68,7 +68,7 @@ TEST( Cal3Unify, Duncalibrate1) Matrix computed; K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-5)); + CHECK(assert_equal(numerical,computed,1e-6)); } /* ************************************************************************* */ @@ -77,7 +77,7 @@ TEST( Cal3Unify, Duncalibrate2) Matrix computed; K.uncalibrate(p, boost::none, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-5)); + CHECK(assert_equal(numerical,computed,1e-6)); } /* ************************************************************************* */ From 7ff7482792b9586682269a486a0612c380ffa86e Mon Sep 17 00:00:00 2001 From: jing Date: Mon, 10 Mar 2014 17:09:56 -0400 Subject: [PATCH 005/101] all unit test failure fixed --- gtsam/geometry/Cal3Unify.cpp | 5 ++--- gtsam/geometry/tests/testCal3Unify.cpp | 23 +++++++++++------------ 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp index 5f2e330bd..8e200ff2a 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unify.cpp @@ -152,12 +152,11 @@ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { Point2 pn = nPlaneToSpace(invKPi); // iterate until the uncalibrate is close to the actual pixel coordinate - const int maxIterations = 100; + const int maxIterations = 20; int iteration; for ( iteration = 0; iteration < maxIterations; ++iteration ) { - pn.print(); - if ( pn.distance(pi) <= tol ) break; + if ( uncalibrate(pn).distance(pi) <= tol ) break; // part1: 3D space -> normalized plane Point2 pnpl = spaceToNPlane(pn); diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp index 3995e03b2..370877ef3 100644 --- a/gtsam/geometry/tests/testCal3Unify.cpp +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -19,28 +19,26 @@ #include #include - -#include using namespace gtsam; -using namespace std; + GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify) GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify) /* ground truth from matlab, code : -X = [2 3 1]'; -V = [0.01, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; +X = [0.5 0.7 1]'; +V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; [P, J] = spaceToImgPlane(X, V); matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */ -static Cal3Unify K(0.01, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); -static Point2 p(2, 3); +static Cal3Unify K(0.1, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); +static Point2 p(0.5, 0.7); /* ************************************************************************* */ TEST( Cal3Unify, uncalibrate) { - Point2 p_i(582.5228344366194, 649.6685266099726) ; + Point2 p_i(364.7791831734982, 305.6677211952602) ; Point2 q = K.uncalibrate(p); CHECK(assert_equal(q,p_i)); } @@ -49,6 +47,7 @@ TEST( Cal3Unify, uncalibrate) TEST( Cal3Unify, spaceNplane) { Point2 q = K.spaceToNPlane(p); + CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q)); CHECK(assert_equal(p, K.nPlaneToSpace(q))); } @@ -56,7 +55,7 @@ TEST( Cal3Unify, spaceNplane) TEST( Cal3Unify, calibrate) { Point2 pi = K.uncalibrate(p); - Point2 pn_hat = K.calibrate(pi, 1); + Point2 pn_hat = K.calibrate(pi); CHECK( p.equals(pn_hat, 1e-5)); } @@ -68,7 +67,7 @@ TEST( Cal3Unify, Duncalibrate1) Matrix computed; K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-6)); + CHECK(assert_equal(numerical,computed,1e-5)); } /* ************************************************************************* */ @@ -77,7 +76,7 @@ TEST( Cal3Unify, Duncalibrate2) Matrix computed; K.uncalibrate(p, boost::none, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-6)); + CHECK(assert_equal(numerical,computed,1e-5)); } /* ************************************************************************* */ @@ -89,7 +88,7 @@ TEST( Cal3Unify, assert_equal) /* ************************************************************************* */ TEST( Cal3Unify, retract) { - Cal3Unify expected(0.01 + 1, 100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + Cal3Unify expected(0.1 + 1, 100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10); Vector d(10); d << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10; From 6d356fbf8e278513932ad71f4d66e20ed2bf7d2e Mon Sep 17 00:00:00 2001 From: jing Date: Thu, 27 Mar 2014 14:42:53 -0400 Subject: [PATCH 006/101] midterm commit, now Cal3Unify live on Cal3DS2, jacobian sequnce changes --- gtsam/geometry/Cal3DS2.h | 2 +- gtsam/geometry/Cal3Unify.cpp | 11 +++++------ gtsam/geometry/Cal3Unify.h | 22 +++++++++++----------- gtsam/geometry/tests/testCal3Unify.cpp | 12 ++++++------ 4 files changed, 23 insertions(+), 24 deletions(-) diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 5453e1481..ced05086b 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -30,7 +30,7 @@ namespace gtsam { */ class GTSAM_EXPORT Cal3DS2 : public DerivedValue { -private: +protected: double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point double k1_, k2_ ; // radial 2nd-order and 4th-order diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp index 8e200ff2a..86a82ff4c 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unify.cpp @@ -26,22 +26,21 @@ namespace gtsam { /* ************************************************************************* */ Cal3Unify::Cal3Unify(const Vector &v): - xi_(v[0]), fx_(v[1]), fy_(v[2]), s_(v[3]), u0_(v[4]), v0_(v[5]), k1_(v[6]), k2_(v[7]), k3_(v[8]), k4_(v[9]){} + Cal3DS2(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} /* ************************************************************************* */ Matrix Cal3Unify::K() const { - return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); + return Base::K(); } /* ************************************************************************* */ Vector Cal3Unify::vector() const { - return (Vector(10) << xi_, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); + return (Vector(10) << Base::vector(), xi_); } /* ************************************************************************* */ void Cal3Unify::print(const std::string& s) const { - gtsam::print(K(), s + ".K"); - gtsam::print(Vector(k()), s + ".k"); + Base::print(s); gtsam::print((Vector)(Vector(1) << xi_), s + ".xi"); } @@ -119,7 +118,7 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0, fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy)); - *H1 = collect(2, &DDS2U, &DDS2V); + *H1 = collect(2, &DDS2V, &DDS2U); } // Inlined derivative for points if (H2) { diff --git a/gtsam/geometry/Cal3Unify.h b/gtsam/geometry/Cal3Unify.h index 4d70db051..37b291994 100644 --- a/gtsam/geometry/Cal3Unify.h +++ b/gtsam/geometry/Cal3Unify.h @@ -22,7 +22,7 @@ #pragma once -#include +#include #include namespace gtsam { @@ -32,14 +32,14 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3Unify : public DerivedValue { +class GTSAM_EXPORT Cal3Unify : protected Cal3DS2 { + + typedef Cal3Unify This; + typedef Cal3DS2 Base; private: double xi_; // mirror parameter - double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point - double k1_, k2_ ; // radial 2nd-order and 4th-order - double k3_, k4_ ; // tangential distortion // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] // Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})] @@ -50,18 +50,18 @@ private: public: Matrix K() const ; - Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, k3_, k4_); } + Eigen::Vector4d k() const { return Base::k(); } Vector vector() const ; /// @name Standard Constructors /// @{ /// Default Constructor with only unit focal length - Cal3Unify() : xi_(0), fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {} + Cal3Unify() : Cal3DS2(), xi_(0) {} - Cal3Unify(double xi, double fx, double fy, double s, double u0, double v0, - double k1, double k2, double k3 = 0.0, double k4 = 0.0) : - xi_(xi), fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {} + Cal3Unify(double fx, double fy, double s, double u0, double v0, + double k1, double k2, double k3 = 0.0, double k4 = 0.0, double xi = 0.0) : + Cal3DS2(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {} /// @} /// @name Advanced Constructors @@ -150,7 +150,6 @@ private: { ar & boost::serialization::make_nvp("Cal3Unify", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(xi_); ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(s_); @@ -160,6 +159,7 @@ private: ar & BOOST_SERIALIZATION_NVP(k2_); ar & BOOST_SERIALIZATION_NVP(k3_); ar & BOOST_SERIALIZATION_NVP(k4_); + ar & BOOST_SERIALIZATION_NVP(xi_); } diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp index 370877ef3..015cff237 100644 --- a/gtsam/geometry/tests/testCal3Unify.cpp +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -32,7 +32,7 @@ V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */ -static Cal3Unify K(0.1, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); +static Cal3Unify K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1); static Point2 p(0.5, 0.7); /* ************************************************************************* */ @@ -67,7 +67,7 @@ TEST( Cal3Unify, Duncalibrate1) Matrix computed; K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-5)); + CHECK(assert_equal(numerical,computed,1e-6)); } /* ************************************************************************* */ @@ -76,7 +76,7 @@ TEST( Cal3Unify, Duncalibrate2) Matrix computed; K.uncalibrate(p, boost::none, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-5)); + CHECK(assert_equal(numerical,computed,1e-6)); } /* ************************************************************************* */ @@ -88,10 +88,10 @@ TEST( Cal3Unify, assert_equal) /* ************************************************************************* */ TEST( Cal3Unify, retract) { - Cal3Unify expected(0.1 + 1, 100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, - 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10); + Cal3Unify expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1); Vector d(10); - d << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10; + d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1; Cal3Unify actual = K.retract(d); CHECK(assert_equal(expected,actual,1e-7)); CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); From 3f5bbe3fd238394d87d2c951bb7a97db891dc912 Mon Sep 17 00:00:00 2001 From: jing Date: Thu, 27 Mar 2014 15:03:06 -0400 Subject: [PATCH 007/101] jacobian fixed, now use base class jacobians and chain rule --- gtsam/geometry/Cal3Unify.cpp | 52 +++++++----------------------------- 1 file changed, 9 insertions(+), 43 deletions(-) diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp index 86a82ff4c..eefd33801 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unify.cpp @@ -64,8 +64,7 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, // is same as Cal3DS2 // parameters - const double xi = xi_, fx = fx_, fy = fy_, s = s_; - const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_; + const double xi = xi_; // Part1: project 3D space to NPlane const double xs = p.x(), ys = p.y(); // normalized points in 3D space @@ -74,51 +73,19 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx; const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane - // Part2: project NPlane point to pixel plane: same as Cal3DS2 - const double xy = x * y, xx = x * x, yy = y * y; - const double rr = xx + yy; - const double r4 = rr * rr; - const double g = 1. + k1 * rr + k2 * r4; - const double dx = 2. * k3 * xy + k4 * (rr + 2. * xx); - const double dy = 2. * k4 * xy + k3 * (rr + 2. * yy); - - const double pnx = g*x + dx; - const double pny = g*y + dy; - - // DDS2 will be used both in H1 and H2 - Matrix DDS2; - if (H1 || H2) { - // part2 - const double dr_dx = 2. * x; - const double dr_dy = 2. * y; - const double dg_dx = k1 * dr_dx + k2 * 2. * rr * dr_dx; - const double dg_dy = k1 * dr_dy + k2 * 2. * rr * dr_dy; - - const double dDx_dx = 2. * k3 * y + k4 * (dr_dx + 4. * x); - const double dDx_dy = 2. * k3 * x + k4 * dr_dy; - const double dDy_dx = 2. * k4 * y + k3 * dr_dx; - const double dDy_dy = 2. * k4 * x + k3 * (dr_dy + 4. * y); - - Matrix DK = (Matrix(2, 2) << fx, s_, 0.0, fy); - Matrix DR = (Matrix(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy, - y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy); - - DDS2 = DK * DR; - } + // Part2: project NPlane point to pixel plane: use Cal3DS2 + Point2 m(x,y); + Matrix H1base, H2base; // jacobians from Base class + Point2 puncalib = Base::uncalibrate(m, H1base, H2base); // Inlined derivative for calibration if (H1) { // part1 Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2, -ys * sqrt_nx * xi_sqrt_nx2); - Matrix DDS2U = DDS2 * DU; - // part2 - Matrix DDS2V = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr, - fx * x * r4 + s * y * r4, fx * 2. * xy + s * (rr + 2. * yy), - fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0, - fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy)); + Matrix DDS2U = H2base * DU; - *H1 = collect(2, &DDS2V, &DDS2U); + *H1 = collect(2, &H1base, &DDS2U); } // Inlined derivative for points if (H2) { @@ -129,11 +96,10 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, mid, (sqrt_nx + xi*(xs*xs + 1)) * denom); - *H2 = DDS2 * DU; + *H2 = H2base * DU; } - - return Point2(fx * pnx + s * pny + u0_, fy * pny + v0_); + return puncalib; } /* ************************************************************************* */ From 77f494b341dadbf4b9cdcf54a7c9f55afccfe241 Mon Sep 17 00:00:00 2001 From: jing Date: Thu, 27 Mar 2014 15:09:26 -0400 Subject: [PATCH 008/101] calibrate() done by using base class --- gtsam/geometry/Cal3Unify.cpp | 37 +++----------------------- gtsam/geometry/tests/testCal3Unify.cpp | 8 +++--- 2 files changed, 8 insertions(+), 37 deletions(-) diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp index eefd33801..e837eff5d 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unify.cpp @@ -104,41 +104,12 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, /* ************************************************************************* */ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { - // Use the following fixed point iteration to invert the radial distortion. - // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t}) - // point on the normalized plane, input for DS2 - double px = pi.x(); - double py = pi.y(); - const Point2 invKPi ((1 / fx_) * (px - u0_ - (s_ / fy_) * (py - v0_)), - (1 / fy_) * (py - v0_)); + // calibrate point to Nplane use base class::calibrate() + Point2 pnplane = Base::calibrate(pi, tol); - // initialize by ignoring the distortion at all, might be problematic for pixels around boundary - Point2 pn = nPlaneToSpace(invKPi); - - // iterate until the uncalibrate is close to the actual pixel coordinate - const int maxIterations = 20; - int iteration; - for ( iteration = 0; iteration < maxIterations; ++iteration ) { - - if ( uncalibrate(pn).distance(pi) <= tol ) break; - - // part1: 3D space -> normalized plane - Point2 pnpl = spaceToNPlane(pn); - // part2: normalized plane -> 3D space - px = pnpl.x(), py = pnpl.y(); - const double xy = px*py, xx = px*px, yy = py*py; - const double rr = xx + yy; - const double g = (1+k1_*rr+k2_*rr*rr); - const double dx = 2*k3_*xy + k4_*(rr+2*xx); - const double dy = 2*k4_*xy + k3_*(rr+2*yy); - pn = nPlaneToSpace((invKPi - Point2(dx,dy))/g); - } - - if ( iteration >= maxIterations ) - throw std::runtime_error("Cal3Unify::calibrate fails to converge. need a better initialization"); - - return pn; + // call nplane to space + return this->nPlaneToSpace(pnplane); } /* ************************************************************************* */ Point2 Cal3Unify::nPlaneToSpace(const Point2& p) const { diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp index 015cff237..c36b756e3 100644 --- a/gtsam/geometry/tests/testCal3Unify.cpp +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -56,7 +56,7 @@ TEST( Cal3Unify, calibrate) { Point2 pi = K.uncalibrate(p); Point2 pn_hat = K.calibrate(pi); - CHECK( p.equals(pn_hat, 1e-5)); + CHECK( p.equals(pn_hat, 1e-8)); } Point2 uncalibrate_(const Cal3Unify& k, const Point2& pt) { return k.uncalibrate(pt); } @@ -82,7 +82,7 @@ TEST( Cal3Unify, Duncalibrate2) /* ************************************************************************* */ TEST( Cal3Unify, assert_equal) { - CHECK(assert_equal(K,K,1e-5)); + CHECK(assert_equal(K,K,1e-9)); } /* ************************************************************************* */ @@ -93,8 +93,8 @@ TEST( Cal3Unify, retract) Vector d(10); d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1; Cal3Unify actual = K.retract(d); - CHECK(assert_equal(expected,actual,1e-7)); - CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); + CHECK(assert_equal(expected,actual,1e-9)); + CHECK(assert_equal(d,K.localCoordinates(actual),1e-9)); } /* ************************************************************************* */ From 8eeb7e8cfb175699be8d7eb83031dae8432186b0 Mon Sep 17 00:00:00 2001 From: jing Date: Thu, 27 Mar 2014 15:15:30 -0400 Subject: [PATCH 009/101] change name from Cal3Unify to Cal3Unified --- .../{Cal3Unify.cpp => Cal3Unified.cpp} | 28 +++++++++---------- gtsam/geometry/{Cal3Unify.h => Cal3Unified.h} | 20 ++++++------- gtsam/geometry/tests/testCal3Unify.cpp | 28 +++++++++---------- 3 files changed, 38 insertions(+), 38 deletions(-) rename gtsam/geometry/{Cal3Unify.cpp => Cal3Unified.cpp} (84%) rename gtsam/geometry/{Cal3Unify.h => Cal3Unified.h} (89%) diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unified.cpp similarity index 84% rename from gtsam/geometry/Cal3Unify.cpp rename to gtsam/geometry/Cal3Unified.cpp index e837eff5d..d2a2be287 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -18,34 +18,34 @@ #include #include #include -#include +#include #include namespace gtsam { /* ************************************************************************* */ -Cal3Unify::Cal3Unify(const Vector &v): - Cal3DS2(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} +Cal3Unified::Cal3Unified(const Vector &v): + Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} /* ************************************************************************* */ -Matrix Cal3Unify::K() const { +Matrix Cal3Unified::K() const { return Base::K(); } /* ************************************************************************* */ -Vector Cal3Unify::vector() const { +Vector Cal3Unified::vector() const { return (Vector(10) << Base::vector(), xi_); } /* ************************************************************************* */ -void Cal3Unify::print(const std::string& s) const { +void Cal3Unified::print(const std::string& s) const { Base::print(s); gtsam::print((Vector)(Vector(1) << xi_), s + ".xi"); } /* ************************************************************************* */ -bool Cal3Unify::equals(const Cal3Unify& K, double tol) const { +bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol || @@ -55,7 +55,7 @@ bool Cal3Unify::equals(const Cal3Unify& K, double tol) const { } /* ************************************************************************* */ -Point2 Cal3Unify::uncalibrate(const Point2& p, +Point2 Cal3Unified::uncalibrate(const Point2& p, boost::optional H1, boost::optional H2) const { @@ -103,7 +103,7 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, } /* ************************************************************************* */ -Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { +Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const { // calibrate point to Nplane use base class::calibrate() Point2 pnplane = Base::calibrate(pi, tol); @@ -112,7 +112,7 @@ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { return this->nPlaneToSpace(pnplane); } /* ************************************************************************* */ -Point2 Cal3Unify::nPlaneToSpace(const Point2& p) const { +Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const { const double x = p.x(), y = p.y(); const double xy2 = x * x + y * y; @@ -122,7 +122,7 @@ Point2 Cal3Unify::nPlaneToSpace(const Point2& p) const { } /* ************************************************************************* */ -Point2 Cal3Unify::spaceToNPlane(const Point2& p) const { +Point2 Cal3Unified::spaceToNPlane(const Point2& p) const { const double x = p.x(), y = p.y(); const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1); @@ -131,12 +131,12 @@ Point2 Cal3Unify::spaceToNPlane(const Point2& p) const { } /* ************************************************************************* */ -Cal3Unify Cal3Unify::retract(const Vector& d) const { - return Cal3Unify(vector() + d); +Cal3Unified Cal3Unified::retract(const Vector& d) const { + return Cal3Unified(vector() + d); } /* ************************************************************************* */ -Vector Cal3Unify::localCoordinates(const Cal3Unify& T2) const { +Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const { return T2.vector() - vector(); } diff --git a/gtsam/geometry/Cal3Unify.h b/gtsam/geometry/Cal3Unified.h similarity index 89% rename from gtsam/geometry/Cal3Unify.h rename to gtsam/geometry/Cal3Unified.h index 37b291994..d8c0251f0 100644 --- a/gtsam/geometry/Cal3Unify.h +++ b/gtsam/geometry/Cal3Unified.h @@ -32,9 +32,9 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3Unify : protected Cal3DS2 { +class GTSAM_EXPORT Cal3Unified : protected Cal3DS2 { - typedef Cal3Unify This; + typedef Cal3Unified This; typedef Cal3DS2 Base; private: @@ -57,17 +57,17 @@ public: /// @{ /// Default Constructor with only unit focal length - Cal3Unify() : Cal3DS2(), xi_(0) {} + Cal3Unified() : Base(), xi_(0) {} - Cal3Unify(double fx, double fy, double s, double u0, double v0, + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3 = 0.0, double k4 = 0.0, double xi = 0.0) : - Cal3DS2(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {} + Base(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {} /// @} /// @name Advanced Constructors /// @{ - Cal3Unify(const Vector &v) ; + Cal3Unified(const Vector &v) ; /// @} /// @name Testable @@ -77,7 +77,7 @@ public: void print(const std::string& s = "") const ; /// assert equality up to a tolerance - bool equals(const Cal3Unify& K, double tol = 10e-9) const; + bool equals(const Cal3Unified& K, double tol = 10e-9) const; /// @} /// @name Standard Interface @@ -126,10 +126,10 @@ public: /// @{ /// Given delta vector, update calibration - Cal3Unify retract(const Vector& d) const ; + Cal3Unified retract(const Vector& d) const ; /// Given a different calibration, calculate update to obtain it - Vector localCoordinates(const Cal3Unify& T2) const ; + Vector localCoordinates(const Cal3Unified& T2) const ; /// Return dimensions of calibration manifold object virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) @@ -148,7 +148,7 @@ private: template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3Unify", + ar & boost::serialization::make_nvp("Cal3Unified", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp index c36b756e3..b260415f1 100644 --- a/gtsam/geometry/tests/testCal3Unify.cpp +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -17,12 +17,12 @@ #include #include #include -#include +#include using namespace gtsam; -GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify) -GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify) +GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unified) /* ground truth from matlab, code : @@ -32,11 +32,11 @@ V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */ -static Cal3Unify K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1); +static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1); static Point2 p(0.5, 0.7); /* ************************************************************************* */ -TEST( Cal3Unify, uncalibrate) +TEST( Cal3Unified, uncalibrate) { Point2 p_i(364.7791831734982, 305.6677211952602) ; Point2 q = K.uncalibrate(p); @@ -44,7 +44,7 @@ TEST( Cal3Unify, uncalibrate) } /* ************************************************************************* */ -TEST( Cal3Unify, spaceNplane) +TEST( Cal3Unified, spaceNplane) { Point2 q = K.spaceToNPlane(p); CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q)); @@ -52,17 +52,17 @@ TEST( Cal3Unify, spaceNplane) } /* ************************************************************************* */ -TEST( Cal3Unify, calibrate) +TEST( Cal3Unified, calibrate) { Point2 pi = K.uncalibrate(p); Point2 pn_hat = K.calibrate(pi); CHECK( p.equals(pn_hat, 1e-8)); } -Point2 uncalibrate_(const Cal3Unify& k, const Point2& pt) { return k.uncalibrate(pt); } +Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); } /* ************************************************************************* */ -TEST( Cal3Unify, Duncalibrate1) +TEST( Cal3Unified, Duncalibrate1) { Matrix computed; K.uncalibrate(p, computed, boost::none); @@ -71,7 +71,7 @@ TEST( Cal3Unify, Duncalibrate1) } /* ************************************************************************* */ -TEST( Cal3Unify, Duncalibrate2) +TEST( Cal3Unified, Duncalibrate2) { Matrix computed; K.uncalibrate(p, boost::none, computed); @@ -80,19 +80,19 @@ TEST( Cal3Unify, Duncalibrate2) } /* ************************************************************************* */ -TEST( Cal3Unify, assert_equal) +TEST( Cal3Unified, assert_equal) { CHECK(assert_equal(K,K,1e-9)); } /* ************************************************************************* */ -TEST( Cal3Unify, retract) +TEST( Cal3Unified, retract) { - Cal3Unify expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1); Vector d(10); d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1; - Cal3Unify actual = K.retract(d); + Cal3Unified actual = K.retract(d); CHECK(assert_equal(expected,actual,1e-9)); CHECK(assert_equal(d,K.localCoordinates(actual),1e-9)); } From fe81828a2cc0514492aa23868b547873a736743c Mon Sep 17 00:00:00 2001 From: jing Date: Fri, 11 Apr 2014 15:33:35 -0400 Subject: [PATCH 010/101] add Serialization function to Unit3 and EssentialMatrix classes --- gtsam/geometry/EssentialMatrix.h | 27 +++++++++++++++++++++++++++ gtsam/geometry/Unit3.h | 18 ++++++++++++++++++ 2 files changed, 45 insertions(+) diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 7de68c7a1..adde77511 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -157,6 +157,33 @@ public: /// @} +private: + + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("EssentialMatrix", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(aRb_); + ar & BOOST_SERIALIZATION_NVP(aTb_); + + ar & boost::serialization::make_nvp("E11", E_(0,0)); + ar & boost::serialization::make_nvp("E12", E_(0,1)); + ar & boost::serialization::make_nvp("E13", E_(0,2)); + ar & boost::serialization::make_nvp("E21", E_(1,0)); + ar & boost::serialization::make_nvp("E22", E_(1,1)); + ar & boost::serialization::make_nvp("E23", E_(1,2)); + ar & boost::serialization::make_nvp("E31", E_(2,0)); + ar & boost::serialization::make_nvp("E32", E_(2,1)); + ar & boost::serialization::make_nvp("E33", E_(2,2)); + } + + /// @} + }; } // gtsam diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 9e2ef0945..8eac7f1a8 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -136,6 +136,24 @@ public: Vector localCoordinates(const Unit3& s) const; /// @} + +private: + + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Unit3", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(B_); + } + + /// @} + }; } // namespace gtsam From d3333c1c853f12942851ba4b5d71a34655135d3c Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Thu, 17 Apr 2014 22:07:55 -0400 Subject: [PATCH 011/101] Fix errors on Windows with VS 2010 --- gtsam/geometry/EssentialMatrix.h | 2 +- gtsam/geometry/Unit3.h | 2 +- gtsam/geometry/tests/testPoint3.cpp | 2 +- gtsam/geometry/tests/testSphere2.cpp | 2 +- gtsam/navigation/tests/testGeographicLib.cpp | 1 + gtsam/slam/EssentialMatrixConstraint.h | 2 +- 6 files changed, 6 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index adde77511..74a1cd798 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -20,7 +20,7 @@ namespace gtsam { * but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * We can then non-linearly optimize immediately on this 5-dimensional manifold. */ -class EssentialMatrix: public DerivedValue { +class GTSAM_EXPORT EssentialMatrix: public DerivedValue { private: diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 8eac7f1a8..39e2c9799 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -27,7 +27,7 @@ namespace gtsam { /// Represents a 3D point on a unit sphere. -class Unit3: public DerivedValue { +class GTSAM_EXPORT Unit3: public DerivedValue { private: diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index ab0d12b9e..65c610c25 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -81,7 +81,7 @@ TEST( Point3, stream) { TEST (Point3, normalize) { Matrix actualH; Point3 point(1, -2, 3); // arbitrary point - Point3 expected(point / sqrt(14)); + Point3 expected(point / sqrt(14.0)); EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8)); Matrix expectedH = numericalDerivative11( boost::bind(&Point3::normalize, _1, boost::none), point); diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index 2725df061..3009d5d0d 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -43,7 +43,7 @@ Point3 point3_(const Unit3& p) { TEST(Unit3, point3) { vector ps; ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0) - / sqrt(2); + / sqrt(2.0); Matrix actualH, expectedH; BOOST_FOREACH(Point3 p,ps) { Unit3 s(p); diff --git a/gtsam/navigation/tests/testGeographicLib.cpp b/gtsam/navigation/tests/testGeographicLib.cpp index 44964c569..aaa01b54d 100644 --- a/gtsam/navigation/tests/testGeographicLib.cpp +++ b/gtsam/navigation/tests/testGeographicLib.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index f2eb76e2d..0e4fb48cf 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -27,7 +27,7 @@ namespace gtsam { * Binary factor between two Pose3 variables induced by an EssentialMatrix measurement * @addtogroup SLAM */ -class EssentialMatrixConstraint: public NoiseModelFactor2 { +class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactor2 { private: From 1ed27005156c8b086701beb32b1194436d3e591d Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Thu, 17 Apr 2014 22:49:04 -0400 Subject: [PATCH 012/101] Fix errors on Windows with VS 2010 --- gtsam/geometry/EssentialMatrix.h | 4 ++-- gtsam/geometry/tests/testSphere2.cpp | 5 +++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 74a1cd798..3c9568c3d 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -150,10 +150,10 @@ public: /// @{ /// stream to stream - friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E); + GTSAM_EXPORT friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E); /// stream from stream - friend std::istream& operator >>(std::istream& is, EssentialMatrix& E); + GTSAM_EXPORT friend std::istream& operator >>(std::istream& is, EssentialMatrix& E); /// @} diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index 3009d5d0d..eb45ea60f 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -233,7 +234,7 @@ TEST(Unit3, localCoordinates_retract) { for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). - sleep(0); + boost::this_thread::sleep(boost::posix_time::milliseconds(0)); // Create the two Unit3s. // NOTE: You can not create two totally random Unit3's because you cannot always compute @@ -263,7 +264,7 @@ TEST(Unit3, localCoordinates_retract_expmap) { for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). - sleep(0); + boost::this_thread::sleep(boost::posix_time::milliseconds(0)); // Create the two Unit3s. // Unlike the above case, we can use any two sphers. From 36f53f7226efc964966a0120c3fe9deda3e7d2bc Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Thu, 17 Apr 2014 23:53:41 -0400 Subject: [PATCH 013/101] Fix errors on Windows with VS 2010 --- gtsam/inference/VariableSlots.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index d393b61b1..9a16ca788 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -55,7 +55,7 @@ class VariableSlots : public FastMap > { public: typedef FastMap > Base; - static const size_t Empty; + GTSAM_EXPORT static const size_t Empty; /// @name Standard Constructors /// @{ From 9dbabbc847b51b52412e9704c2665946643a6a59 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Fri, 18 Apr 2014 10:36:33 -0400 Subject: [PATCH 014/101] Another Windows fix. Everything now builds, as long as METIS and GeographicLIB are off. --- gtsam/navigation/AttitudeFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 44dd4183a..64603bd99 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -61,7 +61,7 @@ public: * Version of AttitudeFactor for Rot3 * @addtogroup Navigation */ -class Rot3AttitudeFactor: public NoiseModelFactor1, public AttitudeFactor { +class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactor1, public AttitudeFactor { typedef NoiseModelFactor1 Base; @@ -129,7 +129,7 @@ private: * Version of AttitudeFactor for Pose3 * @addtogroup Navigation */ -class Pose3AttitudeFactor: public NoiseModelFactor1, +class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactor1, public AttitudeFactor { typedef NoiseModelFactor1 Base; From b82524882c58d44fcfdc285c5b01cf049b4cd569 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 30 Apr 2014 18:21:00 -0700 Subject: [PATCH 015/101] Fix typo in conditional in SymmetricBlockMatrix, fixes #75 --- gtsam/base/SymmetricBlockMatrix.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index d95fe84b7..546b6a7f1 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -53,7 +53,7 @@ SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf( VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial( DenseIndex nFrontals) { // Do dense elimination - if (!blockStart() == 0) + if (blockStart() != 0) throw std::invalid_argument( "Can only do Cholesky when the SymmetricBlockMatrix is not a restricted view, i.e. when blockStart == 0."); if (!gtsam::choleskyPartial(matrix_, offset(nFrontals))) From 2b20d612212cd54d29fbabf384c536fc14671b1a Mon Sep 17 00:00:00 2001 From: Vadim Indelman Date: Wed, 30 Apr 2014 22:32:59 -0400 Subject: [PATCH 016/101] Wrapped measured() for BearingRangeFactor. --- gtsam.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam.h b/gtsam.h index de4c1381d..56a1a601d 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2140,6 +2140,8 @@ template virtual class BearingRangeFactor : gtsam::NoiseModelFactor { BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); + pair measured() const; + // enabling serialization functionality void serialize() const; }; From d8121a25bb0689302810f18d34453f43d4590d4c Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 1 May 2014 12:53:05 -0400 Subject: [PATCH 017/101] Trivial change, but VS 2012 was choking on this in release mode. --- gtsam/linear/HessianFactor.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 18fa44e8b..e62af9a05 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -367,13 +367,12 @@ void HessianFactor::hessianDiagonal(double* d) const { typedef Eigen::Map DMap; // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { - Key j = keys_[pos]; - // Get the diagonal block, and insert its diagonal - Matrix B = info_(pos, pos).selfadjointView(); - DVector dj = B.diagonal(); - DMap(d + 9 * j) += dj; - } + for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + const Matrix& B = info_(pos, pos).selfadjointView(); + DMap(d + 9 * j) += B.diagonal(); + } } /* ************************************************************************* */ From 312d3fac1e28b40c41b3324f13a3e1d5af0fe3d9 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sat, 3 May 2014 11:53:10 -0400 Subject: [PATCH 018/101] ignore /doc* --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 030d51b09..6d274af09 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ /build* +/doc* *.pyc *.DS_Store \ No newline at end of file From 267b86ec1412a1b9cfffce639237ea26838b24fb Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sat, 3 May 2014 11:54:01 -0400 Subject: [PATCH 019/101] Remove svd inverse_square_root --- gtsam/base/Matrix.cpp | 22 ---------------------- gtsam/base/Matrix.h | 2 +- 2 files changed, 1 insertion(+), 23 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 3b16beb51..92c9a525e 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -642,29 +642,7 @@ Matrix cholesky_inverse(const Matrix &A) // return BNU::prod(trans(inv), inv); } -#if 0 /* ************************************************************************* */ -// TODO, would be faster with Cholesky -Matrix inverse_square_root(const Matrix& A) { - size_t m = A.cols(), n = A.rows(); - if (m!=n) - throw invalid_argument("inverse_square_root: A must be square"); - - // Perform SVD, TODO: symmetric SVD? - Matrix U,V; - Vector S; - svd(A,U,S,V); - - // invert and sqrt diagonal of S - // We also arbitrarily choose sign to make result have positive signs - for(size_t i = 0; i inline Matrix3 skewSymmetric(const Eigen::MatrixBase& w) { return skewSymmetric(w(0),w(1),w(2));} -/** Use SVD to calculate inverse square root of a matrix */ +/** Use Cholesky to calculate inverse square root of a matrix */ GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A); /** Calculate the LL^t decomposition of a S.P.D matrix */ From 1ed07ca4ed30311e4e97c8642c9b28cb999e7622 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 3 May 2014 12:23:20 -0400 Subject: [PATCH 020/101] Moved to unstable --- {tests => gtsam_unstable/slam/tests}/testOccupancyGrid.cpp | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename {tests => gtsam_unstable/slam/tests}/testOccupancyGrid.cpp (100%) diff --git a/tests/testOccupancyGrid.cpp b/gtsam_unstable/slam/tests/testOccupancyGrid.cpp similarity index 100% rename from tests/testOccupancyGrid.cpp rename to gtsam_unstable/slam/tests/testOccupancyGrid.cpp From 92c9e9c0aa41d210e60476241998d6af3445a46d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 3 May 2014 12:23:49 -0400 Subject: [PATCH 021/101] Fixed "unused" warnings --- gtsam/geometry/tests/testPinholeCamera.cpp | 13 ++++++------- .../linear/tests/testGaussianBayesNetUnordered.cpp | 2 +- gtsam/symbolic/tests/testSymbolicBayesNet.cpp | 1 - .../dynamics/tests/testSimpleHelicopter.cpp | 2 +- tests/testGaussianISAM2.cpp | 2 -- 5 files changed, 8 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index dcf3b2596..6ed49d0d9 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -183,12 +183,11 @@ TEST( PinholeCamera, Dproject) } /* ************************************************************************* */ -static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) { - Point3 point(point2D.x(), point2D.y(), 1.0); - return Camera(pose,cal).projectPointAtInfinity(point); -} - -/* ************************************************************************* */ +//static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) { +// Point3 point(point2D.x(), point2D.y(), 1.0); +// return Camera(pose,cal).projectPointAtInfinity(point); +//} +// //TEST( PinholeCamera, Dproject_Infinity) //{ // Matrix Dpose, Dpoint, Dcal; @@ -202,7 +201,7 @@ static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const C // CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); // CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); //} - +// /* ************************************************************************* */ static Point2 project4(const Camera& camera, const Point3& point) { return camera.project2(point); diff --git a/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp b/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp index d87007d0b..ca2b4c3d2 100644 --- a/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp +++ b/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp @@ -35,7 +35,7 @@ using namespace boost::assign; using namespace std; using namespace gtsam; -static const Key _x_=0, _y_=1, _z_=2; +static const Key _x_=0, _y_=1; static GaussianBayesNet smallBayesNet = list_of (GaussianConditional(_x_, (Vector(1) << 9.0), (Matrix(1, 1) << 1.0), _y_, (Matrix(1, 1) << 1.0))) diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp index 610e98c58..99a09adc9 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -31,7 +31,6 @@ static const Key _A_ = 1; static const Key _B_ = 2; static const Key _C_ = 3; static const Key _D_ = 4; -static const Key _E_ = 5; static SymbolicConditional::shared_ptr B(new SymbolicConditional(_B_)), diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 25d996772..82197302b 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -14,8 +14,8 @@ using namespace gtsam::symbol_shorthand; const double tol=1e-5; const double h = 0.01; -const double deg2rad = M_PI/180.0; +//const double deg2rad = M_PI/180.0; //Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0)); //LieVector v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 3bcb313b0..55329d8e9 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -35,8 +35,6 @@ using namespace std; using namespace gtsam; using boost::shared_ptr; -const double tol = 1e-4; - // SETDEBUG("ISAM2 update", true); // SETDEBUG("ISAM2 update verbose", true); // SETDEBUG("ISAM2 recalculate", true); From 46859474cb39bdcfcfae7f7d58ceb5b23f29ca79 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Sat, 3 May 2014 12:55:51 -0400 Subject: [PATCH 022/101] Fix warning --- gtsam/base/Matrix.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 92c9a525e..d9ab7d71c 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -695,7 +695,7 @@ Matrix expm(const Matrix& A, size_t K) { /* ************************************************************************* */ Matrix Cayley(const Matrix& A) { - int n = A.cols(); + size_t n = A.cols(); assert(A.rows() == n); // original From a6ae176a54d1bfe29c7b7f913dd6a605010935c7 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sat, 3 May 2014 14:44:39 -0400 Subject: [PATCH 023/101] Upgraded to GeographicLib 1.35 --- gtsam/3rdparty/GeographicLib/CMakeLists.txt | 51 ++++-- gtsam/3rdparty/GeographicLib/Makefile.am | 2 +- gtsam/3rdparty/GeographicLib/Makefile.in | 2 +- gtsam/3rdparty/GeographicLib/NEWS | 25 ++- .../GeographicLib/cmake/CMakeLists.txt | 15 +- .../cmake/project-config.cmake.in | 2 +- gtsam/3rdparty/GeographicLib/configure | 24 +-- gtsam/3rdparty/GeographicLib/configure.ac | 6 +- .../GeographicLib/doc/GeographicLib.dox | 151 +++++++++++------- .../GeographicLib/doc/NETGeographicLib.dox | 14 +- .../doc/scripts/GeographicLib/Geodesic.js | 2 +- .../doc/scripts/geod-google-instructions.html | 4 +- .../doc/scripts/geod-google.html | 15 +- .../include/GeographicLib/Config.h | 4 +- .../include/GeographicLib/Constants.hpp | 11 ++ .../include/GeographicLib/Geodesic.hpp | 4 +- .../include/GeographicLib/GeodesicExact.hpp | 10 +- .../include/GeographicLib/GeodesicLine.hpp | 4 +- .../GeographicLib/GeodesicLineExact.hpp | 6 +- .../include/GeographicLib/Math.hpp | 7 +- .../include/GeographicLib/UTMUPS.hpp | 2 +- .../GeographicLib/legacy/C/geodesic.c | 2 +- .../GeographicLib/legacy/Fortran/geodesic.for | 2 +- .../3rdparty/GeographicLib/man/CMakeLists.txt | 2 +- .../3rdparty/GeographicLib/man/CartConvert.1 | 2 +- .../GeographicLib/man/CartConvert.1.html | 2 +- .../GeographicLib/man/CartConvert.usage | 2 +- gtsam/3rdparty/GeographicLib/man/ConicProj.1 | 2 +- .../GeographicLib/man/ConicProj.1.html | 2 +- .../GeographicLib/man/ConicProj.usage | 2 +- gtsam/3rdparty/GeographicLib/man/GeoConvert.1 | 2 +- .../GeographicLib/man/GeoConvert.1.html | 2 +- .../GeographicLib/man/GeoConvert.usage | 2 +- gtsam/3rdparty/GeographicLib/man/GeodSolve.1 | 2 +- .../GeographicLib/man/GeodSolve.1.html | 2 +- .../GeographicLib/man/GeodSolve.usage | 2 +- .../3rdparty/GeographicLib/man/GeodesicProj.1 | 2 +- .../GeographicLib/man/GeodesicProj.1.html | 2 +- .../GeographicLib/man/GeodesicProj.usage | 2 +- gtsam/3rdparty/GeographicLib/man/GeoidEval.1 | 2 +- .../GeographicLib/man/GeoidEval.1.html | 2 +- .../GeographicLib/man/GeoidEval.usage | 2 +- gtsam/3rdparty/GeographicLib/man/Gravity.1 | 2 +- .../3rdparty/GeographicLib/man/Gravity.1.html | 2 +- .../3rdparty/GeographicLib/man/Gravity.usage | 2 +- .../GeographicLib/man/MagneticField.1 | 2 +- .../GeographicLib/man/MagneticField.1.html | 2 +- .../GeographicLib/man/MagneticField.usage | 2 +- gtsam/3rdparty/GeographicLib/man/Makefile.am | 2 +- gtsam/3rdparty/GeographicLib/man/Makefile.in | 2 +- gtsam/3rdparty/GeographicLib/man/Planimeter.1 | 4 +- .../GeographicLib/man/Planimeter.1.html | 4 +- .../3rdparty/GeographicLib/man/Planimeter.pod | 2 +- .../GeographicLib/man/Planimeter.usage | 4 +- .../man/TransverseMercatorProj.1 | 2 +- .../man/TransverseMercatorProj.1.html | 2 +- .../man/TransverseMercatorProj.usage | 2 +- .../GeographicLib/matlab/geodesicdirect.cpp | 92 ++++++----- .../GeographicLib/matlab/geodesicinverse.cpp | 90 ++++++----- .../GeographicLib/matlab/geodesicline.cpp | 68 ++++---- gtsam/3rdparty/GeographicLib/pom.xml | 82 ++++++++++ gtsam/3rdparty/GeographicLib/src/UTMUPS.cpp | 10 +- .../GeographicLib/tools/CMakeLists.txt | 139 +++++++++------- 63 files changed, 584 insertions(+), 336 deletions(-) create mode 100644 gtsam/3rdparty/GeographicLib/pom.xml diff --git a/gtsam/3rdparty/GeographicLib/CMakeLists.txt b/gtsam/3rdparty/GeographicLib/CMakeLists.txt index a72a06bf1..e698f622a 100644 --- a/gtsam/3rdparty/GeographicLib/CMakeLists.txt +++ b/gtsam/3rdparty/GeographicLib/CMakeLists.txt @@ -2,21 +2,48 @@ project (GeographicLib) # Version information set (PROJECT_VERSION_MAJOR 1) -set (PROJECT_VERSION_MINOR 34) +set (PROJECT_VERSION_MINOR 35) set (PROJECT_VERSION_PATCH 0) set (PROJECT_VERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}") if (PROJECT_VERSION_PATCH GREATER 0) set (PROJECT_VERSION "${PROJECT_VERSION}.${PROJECT_VERSION_PATCH}") endif () -set (CPACK_PACKAGE_VERSION_MAJOR ${PROJECT_VERSION_MAJOR}) -set (CPACK_PACKAGE_VERSION_MINOR ${PROJECT_VERSION_MINOR}) -set (CPACK_PACKAGE_VERSION_PATCH ${PROJECT_VERSION_PATCH}) +if (DEFINED CPACK_PACKAGE_VERSION_COUNT) + + # majic (version 0.1.9 and later) invokes cmake defining, e.g., + # -D CPACK_PACKAGE_VERSION=1.35-SNAPSHOT + # -D CPACK_PACKAGE_VERSION_COUNT=2 + # -D CPACK_PACKAGE_VERSION_MAJOR=1 + # -D CPACK_PACKAGE_VERSION_MINOR=35 + # -D CPACK_PACKAGE_VERSION_SUFFIX=-SNAPSHOT + # Check that the first two version numbers are consistent. + if (CPACK_PACKAGE_VERSION_COUNT EQUAL 2) + set (CPACK_PACKAGE_VERSION_PATCH ${PROJECT_VERSION_PATCH}) + elseif (CPACK_PACKAGE_VERSION_COUNT LESS 2) + message (FATAL_ERROR "CPACK_PACKAGE_VERSION_COUNT must be 2 or more") + endif () + if (NOT ( + CPACK_PACKAGE_VERSION_MAJOR EQUAL PROJECT_VERSION_MAJOR AND + CPACK_PACKAGE_VERSION_MINOR EQUAL PROJECT_VERSION_MINOR)) + message (FATAL_ERROR "Inconsistency in CPACK and PROJECT version numbers") + endif () + set (PROJECT_VERSION_PATCH ${CPACK_PACKAGE_VERSION_PATCH}) + set (PROJECT_VERSION ${CPACK_PACKAGE_VERSION}) + +else () + + set (CPACK_PACKAGE_VERSION_MAJOR ${PROJECT_VERSION_MAJOR}) + set (CPACK_PACKAGE_VERSION_MINOR ${PROJECT_VERSION_MINOR}) + set (CPACK_PACKAGE_VERSION_PATCH ${PROJECT_VERSION_PATCH}) + set (CPACK_PACKAGE_VERSION ${PROJECT_VERSION}) + +endif () # The library version tracks the numbering given by libtool in the # autoconf set up. set (LIBVERSION 10) -set (LIBVERSIONFULL 10.1.1) +set (LIBVERSIONFULL 10.1.2) string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER) @@ -201,8 +228,9 @@ if (NOT MSVC) endif () endif () -if (NOT CYGWIN) - # cygwin has a long double but the support for ::cbrtl etc is missing +if (NOT (CYGWIN OR ANDROID)) + # cygwin and android have a long double but the support for ::cbrtl, + # etc., is missing include (CheckTypeSize) check_type_size ("long double" LONG_DOUBLE BUILTIN_TYPES_ONLY) endif () @@ -296,7 +324,12 @@ if (MSVC) set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /W4") else () set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") - # check for C++11 support + # check for C++11 support. If available, the C++11 static_assert and + # various math functions (std::atanh, etc.) are used. This flag is + # *not* propagated to clients that use GeographicLib. However, this + # is of no consequence. When the client code is being compiled (and + # the GeographicLib headers being included), work-alike substitutions + # for static_assert and std::atanh are used. include (CheckCXXCompilerFlag) set (CXX11FLAG "-std=c++11") check_cxx_compiler_flag (${CXX11FLAG} CXX11TEST1) @@ -388,7 +421,7 @@ set (CPACK_SOURCE_IGNORE_FILES "${PROJECT_SOURCE_DIR}/BUILD" "${PROJECT_SOURCE_DIR}/(tests|testdata|cgi-bin|.*\\\\.cache)/" "${PROJECT_SOURCE_DIR}/(distrib|.*-distrib|.*-installer|geodesic-papers)/" - "${PROJECT_SOURCE_DIR}/[^/]*\\\\.(html|kmz|pdf|xml)\$" + "${PROJECT_SOURCE_DIR}/[^/]*\\\\.(html|kmz|pdf)\$" "${PROJECT_SOURCE_DIR}/(autogen|biblio|js-compress)\\\\.sh\$" "${PROJECT_SOURCE_DIR}/(geodesic-biblio.txt|makefile-admin|[^/]*\\\\.png)\$" "${PROJECT_SOURCE_DIR}/matlab/matlab-.*blurb.txt\$" ) diff --git a/gtsam/3rdparty/GeographicLib/Makefile.am b/gtsam/3rdparty/GeographicLib/Makefile.am index 4008b07b8..8accc4719 100644 --- a/gtsam/3rdparty/GeographicLib/Makefile.am +++ b/gtsam/3rdparty/GeographicLib/Makefile.am @@ -9,7 +9,7 @@ ACLOCAL_AMFLAGS = -I m4 SUBDIRS = src man tools doc include matlab python cmake examples -EXTRA_DIST = AUTHORS 00README.txt LICENSE.txt NEWS INSTALL \ +EXTRA_DIST = AUTHORS 00README.txt LICENSE.txt NEWS INSTALL pom.xml \ Makefile.mk CMakeLists.txt windows maxima doc legacy java dotnet dist-hook: diff --git a/gtsam/3rdparty/GeographicLib/Makefile.in b/gtsam/3rdparty/GeographicLib/Makefile.in index 44520c36c..a8345a2e2 100644 --- a/gtsam/3rdparty/GeographicLib/Makefile.in +++ b/gtsam/3rdparty/GeographicLib/Makefile.in @@ -276,7 +276,7 @@ top_srcdir = @top_srcdir@ AUTOMAKE_OPTIONS = foreign ACLOCAL_AMFLAGS = -I m4 SUBDIRS = src man tools doc include matlab python cmake examples -EXTRA_DIST = AUTHORS 00README.txt LICENSE.txt NEWS INSTALL \ +EXTRA_DIST = AUTHORS 00README.txt LICENSE.txt NEWS INSTALL pom.xml \ Makefile.mk CMakeLists.txt windows maxima doc legacy java dotnet all: all-recursive diff --git a/gtsam/3rdparty/GeographicLib/NEWS b/gtsam/3rdparty/GeographicLib/NEWS index 1d5614ce7..71b3d25d2 100644 --- a/gtsam/3rdparty/GeographicLib/NEWS +++ b/gtsam/3rdparty/GeographicLib/NEWS @@ -4,7 +4,30 @@ For more information, see http://geographiclib.sourceforge.net/ -The current version of the library is 1.34. +The current version of the library is 1.35. + +Changes between 1.35 (released 2014-03-13) and 1.34 versions: + + * Fix blunder in UTMUPS::EncodeEPSG (found by Ben Adler). + + * Matlab wrapper routines geodesic{direct,inverse,line} switch to + "exact" routes if |f| > 0.02. + + * GeodSolve.cgi allows ellipsoid to be set (and uses the -E option + for GeodSolve). + + * Set title in HTML versions of man pages for the utility programs. + + * Changes in cmake support: + + add _d to names of executables in debug mode of Visual Studio; + + add support for Android (cmake-only), thanks to Pullan Yu; + + check CPACK version numbers supplied on command line; + + configured version of project-config.cmake.in is + project-config.cmake (instead of geographiclib-config.cmake), to + prevent find_package incorrectly using this file; + + fix tests with multi-line output; + + this release includes a file, pom.xml, which is used by an + experimental build system (based on maven) at SRI. Changes between 1.34 (released 2013-12-11) and 1.33 versions: diff --git a/gtsam/3rdparty/GeographicLib/cmake/CMakeLists.txt b/gtsam/3rdparty/GeographicLib/cmake/CMakeLists.txt index 51e6c44b2..bbb5192cf 100644 --- a/gtsam/3rdparty/GeographicLib/cmake/CMakeLists.txt +++ b/gtsam/3rdparty/GeographicLib/cmake/CMakeLists.txt @@ -31,14 +31,17 @@ endif () # it to prevent the source and build paths appearing in the installed # config files set (PROJECT_INCLUDE_DIRS) -configure_file (project-config.cmake.in - ${PROJECT_NAME_LOWER}-config.cmake @ONLY) +configure_file (project-config.cmake.in project-config.cmake @ONLY) configure_file (project-config-version.cmake.in - ${PROJECT_NAME_LOWER}-config-version.cmake @ONLY) + project-config-version.cmake @ONLY) install (FILES - "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME_LOWER}-config.cmake" - "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME_LOWER}-config-version.cmake" - DESTINATION "${INSTALL_CMAKE_DIR}") + "${CMAKE_CURRENT_BINARY_DIR}/project-config.cmake" + DESTINATION "${INSTALL_CMAKE_DIR}" + RENAME "${PROJECT_NAME_LOWER}-config.cmake") +install (FILES + "${CMAKE_CURRENT_BINARY_DIR}/project-config-version.cmake" + DESTINATION "${INSTALL_CMAKE_DIR}" + RENAME "${PROJECT_NAME_LOWER}-config-version.cmake") # Make information about the cmake targets (the library and the tools) # available. install (EXPORT depends diff --git a/gtsam/3rdparty/GeographicLib/cmake/project-config.cmake.in b/gtsam/3rdparty/GeographicLib/cmake/project-config.cmake.in index a8b15a098..9bd50c24d 100644 --- a/gtsam/3rdparty/GeographicLib/cmake/project-config.cmake.in +++ b/gtsam/3rdparty/GeographicLib/cmake/project-config.cmake.in @@ -30,7 +30,7 @@ if (IS_ABSOLUTE "@PROJECT_ROOT_DIR@") set (_ROOT "@PROJECT_ROOT_DIR@") set (@PROJECT_NAME@_INCLUDE_DIRS "@PROJECT_INCLUDE_DIRS@") set (@PROJECT_NAME@_LIBRARY_DIRS "${_ROOT}/src") - set (@PROJECT_NAME@_BINARY_DIRS "${_ROOT}/src") + set (@PROJECT_NAME@_BINARY_DIRS "${_ROOT}/tools") else () # This is an installed package; figure out the paths relative to the # current directory. diff --git a/gtsam/3rdparty/GeographicLib/configure b/gtsam/3rdparty/GeographicLib/configure index cc81327f5..a9a70828a 100755 --- a/gtsam/3rdparty/GeographicLib/configure +++ b/gtsam/3rdparty/GeographicLib/configure @@ -1,6 +1,6 @@ #! /bin/sh # Guess values for system-dependent variables and create Makefiles. -# Generated by GNU Autoconf 2.69 for GeographicLib 1.34. +# Generated by GNU Autoconf 2.69 for GeographicLib 1.35. # # Report bugs to . # @@ -590,8 +590,8 @@ MAKEFLAGS= # Identity of this package. PACKAGE_NAME='GeographicLib' PACKAGE_TARNAME='geographiclib' -PACKAGE_VERSION='1.34' -PACKAGE_STRING='GeographicLib 1.34' +PACKAGE_VERSION='1.35' +PACKAGE_STRING='GeographicLib 1.35' PACKAGE_BUGREPORT='charles@karney.com' PACKAGE_URL='' @@ -1339,7 +1339,7 @@ if test "$ac_init_help" = "long"; then # Omit some internal or obsolete options to make the list less imposing. # This message is too long to be a string in the A/UX 3.1 sh. cat <<_ACEOF -\`configure' configures GeographicLib 1.34 to adapt to many kinds of systems. +\`configure' configures GeographicLib 1.35 to adapt to many kinds of systems. Usage: $0 [OPTION]... [VAR=VALUE]... @@ -1410,7 +1410,7 @@ fi if test -n "$ac_init_help"; then case $ac_init_help in - short | recursive ) echo "Configuration of GeographicLib 1.34:";; + short | recursive ) echo "Configuration of GeographicLib 1.35:";; esac cat <<\_ACEOF @@ -1519,7 +1519,7 @@ fi test -n "$ac_init_help" && exit $ac_status if $ac_init_version; then cat <<\_ACEOF -GeographicLib configure 1.34 +GeographicLib configure 1.35 generated by GNU Autoconf 2.69 Copyright (C) 2012 Free Software Foundation, Inc. @@ -2063,7 +2063,7 @@ cat >config.log <<_ACEOF This file contains any messages produced by compilers while running configure, to aid debugging if configure makes a mistake. -It was created by GeographicLib $as_me 1.34, which was +It was created by GeographicLib $as_me 1.35, which was generated by GNU Autoconf 2.69. Invocation command line was $ $0 $@ @@ -3001,7 +3001,7 @@ fi # Define the identity of the package. PACKAGE='geographiclib' - VERSION='1.34' + VERSION='1.35' cat >>confdefs.h <<_ACEOF @@ -3049,7 +3049,7 @@ am__tar='$${TAR-tar} chof - "$$tardir"' am__untar='$${TAR-tar} xf -' GEOGRAPHICLIB_VERSION_MAJOR=1 -GEOGRAPHICLIB_VERSION_MINOR=34 +GEOGRAPHICLIB_VERSION_MINOR=35 GEOGRAPHICLIB_VERSION_PATCH=0 cat >>confdefs.h <<_ACEOF @@ -3098,7 +3098,7 @@ ac_config_headers="$ac_config_headers include/GeographicLib/Config-ac.h" LT_CURRENT=11 -LT_REVISION=1 +LT_REVISION=2 LT_AGE=1 @@ -16672,7 +16672,7 @@ cat >>$CONFIG_STATUS <<\_ACEOF || ac_write_fail=1 # report actual input values of CONFIG_FILES etc. instead of their # values after options handling. ac_log=" -This file was extended by GeographicLib $as_me 1.34, which was +This file was extended by GeographicLib $as_me 1.35, which was generated by GNU Autoconf 2.69. Invocation command line was CONFIG_FILES = $CONFIG_FILES @@ -16738,7 +16738,7 @@ _ACEOF cat >>$CONFIG_STATUS <<_ACEOF || ac_write_fail=1 ac_cs_config="`$as_echo "$ac_configure_args" | sed 's/^ //; s/[\\""\`\$]/\\\\&/g'`" ac_cs_version="\\ -GeographicLib config.status 1.34 +GeographicLib config.status 1.35 configured by $0, generated by GNU Autoconf 2.69, with options \\"\$ac_cs_config\\" diff --git a/gtsam/3rdparty/GeographicLib/configure.ac b/gtsam/3rdparty/GeographicLib/configure.ac index e9fb7b6f2..c0b539c5c 100644 --- a/gtsam/3rdparty/GeographicLib/configure.ac +++ b/gtsam/3rdparty/GeographicLib/configure.ac @@ -1,7 +1,7 @@ dnl dnl Copyright (C) 2009, Francesco P. Lovergine -AC_INIT([GeographicLib],[1.34],[charles@karney.com]) +AC_INIT([GeographicLib],[1.35],[charles@karney.com]) AC_CANONICAL_SYSTEM AC_PREREQ(2.61) AC_CONFIG_SRCDIR(src/Geodesic.cpp) @@ -9,7 +9,7 @@ AC_CONFIG_MACRO_DIR(m4) AM_INIT_AUTOMAKE GEOGRAPHICLIB_VERSION_MAJOR=1 -GEOGRAPHICLIB_VERSION_MINOR=34 +GEOGRAPHICLIB_VERSION_MINOR=35 GEOGRAPHICLIB_VERSION_PATCH=0 AC_DEFINE_UNQUOTED([GEOGRAPHICLIB_VERSION_MAJOR], [$GEOGRAPHICLIB_VERSION_MAJOR],[major version number]) @@ -35,7 +35,7 @@ dnl Interfaces changed/added/removed: CURRENT++ REVISION=0 dnl Interfaces added: AGE++ dnl Interfaces removed: AGE=0 LT_CURRENT=11 -LT_REVISION=1 +LT_REVISION=2 LT_AGE=1 AC_SUBST(LT_CURRENT) AC_SUBST(LT_REVISION) diff --git a/gtsam/3rdparty/GeographicLib/doc/GeographicLib.dox b/gtsam/3rdparty/GeographicLib/doc/GeographicLib.dox index b0f980d19..7fd60e992 100644 --- a/gtsam/3rdparty/GeographicLib/doc/GeographicLib.dox +++ b/gtsam/3rdparty/GeographicLib/doc/GeographicLib.dox @@ -11,8 +11,8 @@ /** \mainpage GeographicLib library \author Charles F. F. Karney (charles@karney.com) -\version 1.34 -\date 2013-12-11 +\version 1.35 +\date 2014-03-13 \section abstract Abstract @@ -45,22 +45,22 @@ The main project page is at http://sourceforge.net/projects/geographiclib . The code is available for download at -- - GeographicLib-1.34.tar.gz -- - GeographicLib-1.34.zip +- + GeographicLib-1.35.tar.gz +- + GeographicLib-1.35.zip . as either a compressed tar file (tar.gz) or a zip file. (The two archives have identical contents, except that the zip file has DOS line endings.) Alternatively you can get the latest release using git \verbatim - git clone -b r1.34 git://git.code.sf.net/p/geographiclib/code geographiclib + git clone -b r1.35 git://git.code.sf.net/p/geographiclib/code geographiclib \endverbatim There are also binary installers for Windows available at -- - GeographicLib-1.34-win32.exe -- - GeographicLib-1.34-win64.exe +- + GeographicLib-1.35-win32.exe +- + GeographicLib-1.35-win64.exe . It is licensed under the MIT/X11 License; @@ -180,14 +180,14 @@ Back to \ref intro. Forward to \ref start. Up to \ref contents. (versions 4.0 and later) and under Windows with Visual Studio 2005, 2008, and 2010. Earlier versions were tested also under Darwin and Solaris. It should compile on a wide range of other systems. First download either - -GeographicLib-1.34.tar.gz or - -GeographicLib-1.34.zip (or - -GeographicLib-1.34-win32.exe or - -GeographicLib-1.34-win64.exe for binary installation under Windows). + +GeographicLib-1.35.tar.gz or + +GeographicLib-1.35.zip (or + +GeographicLib-1.35-win32.exe or + +GeographicLib-1.35-win64.exe for binary installation under Windows). Then pick one of the first five options below: - \ref cmake. This is the preferred installation method as it will work on the widest range of platforms. However it requires that you have @@ -262,10 +262,10 @@ g++ on Linux and with the Visual Studio IDE on Windows. Here are the steps to compile and install %GeographicLib: - Unpack the source, running one of \verbatim - tar xfpz GeographicLib-1.34.tar.gz - unzip -q GeographicLib-1.34.zip \endverbatim + tar xfpz GeographicLib-1.35.tar.gz + unzip -q GeographicLib-1.35.zip \endverbatim then enter the directory created with one of \verbatim - cd GeographicLib-1.34 \endverbatim + cd GeographicLib-1.35 \endverbatim - Create a separate build directory and enter it, for example, \verbatim mkdir BUILD cd BUILD \endverbatim @@ -273,8 +273,8 @@ Here are the steps to compile and install %GeographicLib: and MacOSX systems, the command is \verbatim cmake .. \endverbatim For Windows, the command is typically one of \verbatim - cmake -G "Visual Studio 10" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc10/GeographicLib-1.34 .. - cmake -G "Visual Studio 9 2008" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc9/GeographicLib-1.34 .. + cmake -G "Visual Studio 10" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc10/GeographicLib-1.35 .. + cmake -G "Visual Studio 9 2008" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc9/GeographicLib-1.35 .. \endverbatim The definitions of CMAKE_INSTALL_PREFIX are optional (see below). The settings given above are recommended to ensure that packages that use @@ -290,7 +290,7 @@ Here are the steps to compile and install %GeographicLib: convention. If it is on ON (the Linux default), the installation is to a common directory, e.g., /usr/local. If it is OFF (the Windows default), the installation directory contains the package - name, e.g., C:/pkg/GeographicLib-1.34. The installation + name, e.g., C:/pkg/GeographicLib-1.35. The installation directories for the documentation, cmake configuration, python and matlab interfaces all depend on the variable with deeper paths relative to CMAKE_INSTALL_PREFIX being used when it's ON: @@ -305,7 +305,7 @@ Here are the steps to compile and install %GeographicLib: For windows systems, it is recommended to use a prefix which includes the compiler version, as shown above (and also, possibly, whether this is a 64-bit build, e.g., cmake -G "Visual Studio - 10 Win64" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc10-x64/GeographicLib-1.34 + 10 Win64" -D CMAKE_INSTALL_PREFIX=C:/pkg-vc10-x64/GeographicLib-1.35 ..). If you just want to try the library to see if it suits your needs, pick, for example, CMAKE_INSTALL_PREFIX=/tmp/geographic. @@ -394,9 +394,9 @@ Here are the steps to compile and install %GeographicLib: The method works on most Unix-like systems including Linux and Mac OS X. Here are the steps to compile and install %GeographicLib: - Unpack the source, running \verbatim - tar xfpz GeographicLib-1.34.tar.gz \endverbatim + tar xfpz GeographicLib-1.35.tar.gz \endverbatim then enter the directory created \verbatim - cd GeographicLib-1.34 \endverbatim + cd GeographicLib-1.35 \endverbatim - Create a separate build directory and enter it, for example, \verbatim mkdir BUILD cd BUILD \endverbatim @@ -425,9 +425,9 @@ and g++. This builds a static library and the examples. Here are the steps to compile and install %GeographicLib: - Unpack the source, running \verbatim - tar xfpz GeographicLib-1.34.tar.gz \endverbatim + tar xfpz GeographicLib-1.35.tar.gz \endverbatim then enter the directory created \verbatim - cd GeographicLib-1.34 \endverbatim + cd GeographicLib-1.35 \endverbatim - Edit \verbatim include/GeographicLib/Config.h \endverbatim If your C++ compiler does not recognize the long double type @@ -458,8 +458,8 @@ static library and the utilities. If you only have Visual Studio 2003, use cmake to create the necessary solution file, see \ref cmake. (cmake is needed to build the Matlab interface and to run the tests.) - Unpack the source, running \verbatim - unzip -q GeographicLib-1.34.zip \endverbatim -- Open GeographicLib-1.34/windows/GeographicLib-vc10.sln in Visual Studio + unzip -q GeographicLib-1.35.zip \endverbatim +- Open GeographicLib-1.35/windows/GeographicLib-vc10.sln in Visual Studio 2010 (for Visual Studio 2005 and 2008, replace -vc10 by -vc8 or -vc9). - Pick the build type (e.g., Release), and select "Build Solution". - The library and the compiled examples are in the windows/Release. @@ -486,14 +486,14 @@ be advisable to build it with the compiler you are using for your own code using either \ref cmake or \ref windows. Download and run - -GeographicLib-1.34-win32.exe or - -GeographicLib-1.34-win64.exe: + +GeographicLib-1.35-win32.exe or + +GeographicLib-1.35-win64.exe: - read the MIT/X11 License agreement, - select whether you want your PATH modified, - select the installation folder, by default - C:\\pkg-vc10\\GeographicLib-1.34 or C:\\pkg-vc10-x64\\GeographicLib-1.34, + C:\\pkg-vc10\\GeographicLib-1.35 or C:\\pkg-vc10-x64\\GeographicLib-1.35, - select the start menu folder, - and install. . @@ -501,7 +501,7 @@ GeographicLib-1.34-win64.exe: given in \ref cmake.) The start menu will now include links to the documentation for the library and for the utilities (and a link for uninstalling the library). If you ask for your PATH to be modified, it -will include C:/pkg-vc10/GeographicLib-1.34/bin where the utilities are +will include C:/pkg-vc10/GeographicLib-1.35/bin where the utilities are installed. The headers and library are installed in the include/GeographicLib and lib folders. With the 64-bit installer, the Matlab interface is installed in the matlab folder. Add this to your @@ -552,7 +552,7 @@ Check the code out of git with \verbatim Here the "master" branch is checked out. There are three branches in the git repository: - master: the main branch for code maintainence. Releases are - tagged on this branch as, e.g., v1.34. + tagged on this branch as, e.g., v1.35. - devel: the development branch; changes made here are merged into master. - release: the release branch created by unpacking the source @@ -562,7 +562,7 @@ the git repository: specifying a branch). This differs from the master branch in that some administrative files are excluded while some intermediate files are included (in order to aid building on as many platforms as - possible). Releases are tagged on this branch as, e.g., r1.34. + possible). Releases are tagged on this branch as, e.g., r1.35. . The autoconf configuration script and the formatted man pages are not checked into master branch of the repository. In order to create the @@ -578,8 +578,8 @@ In the case of cmake, you then run \verbatim which will copy the man pages from the build directory back into the source tree and package the resulting source tree for distribution as \verbatim - GeographicLib-1.34.tar.gz - GeographicLib-1.34.zip \endverbatim + GeographicLib-1.35.tar.gz + GeographicLib-1.35.zip \endverbatim Finally, \verbatim make package \endverbatim or building PACKAGE in Visual Studio will create a binary installer for @@ -605,7 +605,7 @@ With configure, run \verbatim make dist-gzip \endverbatim which will create the additional files and packages the results ready for distribution as \verbatim - geographiclib-1.34.tar.gz \endverbatim + geographiclib-1.35.tar.gz \endverbatim
Back to \ref intro. Forward to \ref start. Up to \ref contents. @@ -695,7 +695,7 @@ In order to use %GeographicLib from C++ code, you will need to If %GeographicLib is found, then the following cmake variables are set: - GeographicLib_FOUND = 1 - - GeographicLib_VERSION = 1.34 + - GeographicLib_VERSION = 1.35 - GeographicLib_INCLUDE_DIRS - GeographicLib_LIBRARIES = one of the following two: - GeographicLib_SHARED_LIBRARIES = %GeographicLib @@ -1120,9 +1120,9 @@ feature of %GeographicLib, but want your code still to work with older versions. In that case, you can test the values of the macros GEOGRAPHICLIB_VERSION_MAJOR, GEOGRAPHICLIB_VERSION_MINOR, and GEOGRAPHICLIB_VERSION_PATCH; these expand to numbers (and the last one -is usually 0); these macros appeared starting in version 1.34. There's +is usually 0); these macros appeared starting in version 1.31. There's also a macro GEOGRAPHICLIB_VERSION_STRING which expands to, e.g., -"1.34"; this macro has been defined since version 1.9. +"1.35"; this macro has been defined since version 1.9.
Back to \ref utilities. Forward to \ref other. Up to \ref contents. @@ -1274,7 +1274,7 @@ The matlab directory contains - Native Matlab implementations of the geodesic routines. To use these, start Matlab or Octave and run one of (for example) \verbatim addpath /usr/local/libexec/GeographicLib/matlab - addpath 'C:/pkg-vc10-x64/GeographicLib-1.34/libexec/GeographicLib/matlab' + addpath 'C:/pkg-vc10-x64/GeographicLib-1.35/libexec/GeographicLib/matlab' \endverbatim The available functions are: - geoddoc: briefly descibe the routines @@ -1345,9 +1345,9 @@ There are two ways of compiling the interface code: (1) using cmake and - Invoking the compiler from Matlab or Octave: Start Matlab or Octave and run, e.g., \code mex -setup - cd 'C:/pkg-vc10-x64/GeographicLib-1.34/matlab' + cd 'C:/pkg-vc10-x64/GeographicLib-1.35/matlab' help geographiclibinterface - geographiclibinterface('C:/pkg-vc10/GeographicLib-1.34'); + geographiclibinterface('C:/pkg-vc10/GeographicLib-1.35'); addpath(pwd); \endcode The first command allows you to select the compiler to use (which @@ -1356,7 +1356,7 @@ There are two ways of compiling the interface code: (1) using cmake and To use the interface routines for %GeographicLib, run one of (for example) \verbatim addpath /usr/local/libexec/GeographicLib/matlab - addpath 'C:/pkg-vc10-x64/GeographicLib-1.34/libexec/GeographicLib/matlab' + addpath 'C:/pkg-vc10-x64/GeographicLib-1.35/libexec/GeographicLib/matlab' \endverbatim in Octave or Matlab. The available functions are: - geodesicdirect: solve direct geodesic problem @@ -4048,7 +4048,7 @@ starting point of this geodesic is \f$\beta_1 = 87.48^\circ\f$, \f$\omega_1 = 0^\circ\f$, and \f$\alpha_1 = 90^\circ\f$. If the starting point is \f$\beta_1 = 90^\circ\f$, \f$\omega_1 \in -(0^\circ, 180^\circ)\f$, and \f$\alpha_1 = 0^\circ\f$, then the geodesic +(0^\circ, 180^\circ)\f$, and \f$\alpha_1 = 180^\circ\f$, then the geodesic encircles the ellipsoid in a "transpolar" sense. The geodesic oscillates east and west of the ellipse \f$x = 0\f$; on each oscillation it completes slightly more that a full circuit around the ellipsoid @@ -4067,7 +4067,7 @@ Fig. 4 Fig. 4: Example of a transpolar geodesic on a triaxial ellipsoid. The starting point of this geodesic is \f$\beta_1 = 90^\circ\f$, \f$\omega_1 = -39.9^\circ\f$, and \f$\alpha_1 = 0^\circ\f$. +39.9^\circ\f$, and \f$\alpha_1 = 180^\circ\f$.
). - If only one point is an umbilicial point, the azimuth at the non-umbilical point is found using the generalization of Clairaut's equation (given above) with \f$\gamma = 0\f$. - - If both points lie on the equator \f$\beta = 0\f$, then determine the - reduced length \f$m_{12}\f$ for the geodesic which is the shorter - path along the ellipse \f$z = 0\f$. If \f$m_{12} \ge 0\f$, then this - is the shortest path on the ellipsoid; otherwise proceed to the - general case (next). + - Treat the cases where the geodesic might follow a line of constant + \f$\beta\f$. There are two such cases: (a) the points lie on the + ellipse \f$z = 0\f$ on a general ellipsoid and (b) the points lie on + an ellipse whose major axis is the \f$x\f$ axis on a prolate ellipsoid + (\f$a = b > c\f$). Determine the reduced length \f$m_{12}\f$ for the + geodesic which is the shorter path along the ellipse. If \f$m_{12} + \ge 0\f$, then this is the shortest path on the ellipsoid; otherwise + proceed to the general case (next). - Swap the points, if necessary, so that the first point is the one closest to a pole. Estimate \f$\alpha_1\f$ (by some means) and solve the \e hybrid problem, i.e., determine the longitude \f$\omega_2\f$ @@ -4238,6 +4241,12 @@ The shortest path found by this method is unique unless: - The points are opposite umbilical points. In this case, \f$\alpha_1\f$ can take on any value and \f$\alpha_2\f$ needs to be adjusted to maintain the value of \f$\tan\alpha_1 / \tan\alpha_2\f$. + Note that \f$\alpha\f$ increases by \f$\pm 90^\circ\f$ as the + geodesic passes through an umbilical point, depending on whether the + geodesic is considered as passing to the right or left of the point. + Here \f$\alpha_2\f$ is the \e forward azimuth at the second umbilical + point, i.e., its azimuth immediately \e after passage through the + umbilical point. - \f$\beta_1 + \beta_2 = 0\f$ and \f$\cos\alpha_1\f$ and \f$\cos\alpha_2\f$ have opposite signs. In this case, there another shortest geodesic with azimuths \f$\pi - \alpha_1\f$ and @@ -4757,6 +4766,26 @@ been migrated to the archive subdirectory). All the releases are available as tags “rm.nn” in the the "release" branch of the git repository for %GeographicLib. + - Version 1.35 + (released 2014-03-13) + - Fix blunder in GeographicLib::UTMUPS::EncodeEPSG (found by Ben + Adler). + - Matlab wrapper routines geodesic{direct,inverse,line} switch to + "exact" routes if |f| > 0.02. + - GeodSolve.cgi allows ellipsoid to be set (and uses the -E option + for GeodSolve). + - Set title in HTML versions of man pages for the \ref utilities. + - Changes in cmake support: + - add _d to names of executables in debug mode of Visual Studio; + - add support for Android (cmake-only), thanks to Pullan Yu; + - check CPACK version numbers supplied on command line; + - configured version of project-config.cmake.in is + project-config.cmake (instead of geographiclib-config.cmake), to + prevent find_package incorrectly using this file; + - fix tests with multi-line output; + - this release includes a file, pom.xml, which is used by an + experimental build system (based on maven) at SRI. + - Version 1.34 (released 2013-12-11) - Many changes in cmake support: diff --git a/gtsam/3rdparty/GeographicLib/doc/NETGeographicLib.dox b/gtsam/3rdparty/GeographicLib/doc/NETGeographicLib.dox index 89b934a7d..fd8f5e057 100644 --- a/gtsam/3rdparty/GeographicLib/doc/NETGeographicLib.dox +++ b/gtsam/3rdparty/GeographicLib/doc/NETGeographicLib.dox @@ -11,8 +11,8 @@ /** \mainpage NETGeographicLib library \author Scott Heiman (mrmtdew2@outlook.com) -\version 1.34 -\date 2013-12-11 +\version 1.35 +\date 2014-03-13 \section abstract Abstract @@ -26,14 +26,14 @@ to the GeographicLib classes. GeographicLib and NETGeographicLib is an integrated product. The NETGeographic project in the GeographicLib-vc10.sln file located in -\/GeographicLib-1.34/windows will create the NETGeographicLib +\/GeographicLib-1.35/windows will create the NETGeographicLib DLL. The source code for NETGeographicLib is located in -\/GeographicLib-1.34/dotnet/NETGeographicLib. NETGeographicLib +\/GeographicLib-1.35/dotnet/NETGeographicLib. NETGeographicLib is not available for older versions of Microsoft Visual Studio. NETGeographicLib has been tested with C#, Managed C++, and Visual Basic. Sample code snippets can be found in -\/GeographicLib-1.34/dotnet/examples. +\/GeographicLib-1.35/dotnet/examples. \section differences Differences between NETGeographicLib and GeographicLib @@ -135,7 +135,7 @@ to any Visual Basic source that uses NETGeographicLib classes. A C# sample application is provided that demonstrates NETGeographicLib classes. The source code for the sample application is located in -\/GeographicLib-1.34/dotnet/Projections. The sample +\/GeographicLib-1.35/dotnet/Projections. The sample application creates a tabbed dialog. Each tab provides data entry fields that allow the user to exercise one or more NETGeographicLib classes. @@ -200,7 +200,7 @@ code using the installed library: \verbatim project (geodesictest) cmake_minimum_required (VERSION 2.8.7) # required for VS_DOTNET_REFERENCES -find_package (GeographicLib 1.34 REQUIRED COMPONENTS NETGeographicLib) +find_package (GeographicLib 1.35 REQUIRED COMPONENTS NETGeographicLib) add_executable (${PROJECT_NAME} example-Geodesic-small.cpp) set_target_properties (${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "/clr") diff --git a/gtsam/3rdparty/GeographicLib/doc/scripts/GeographicLib/Geodesic.js b/gtsam/3rdparty/GeographicLib/doc/scripts/GeographicLib/Geodesic.js index 09d22593a..cf059126f 100644 --- a/gtsam/3rdparty/GeographicLib/doc/scripts/GeographicLib/Geodesic.js +++ b/gtsam/3rdparty/GeographicLib/doc/scripts/GeographicLib/Geodesic.js @@ -753,7 +753,7 @@ GeographicLib.GeodesicLine = {}; // Add the check for sig12 since zero length geodesics might yield // m12 < 0. Test case was // - // echo 20.001 0 20.001 0 | Geod -i + // echo 20.001 0 20.001 0 | GeodSolve -i // // In fact, we will have sig12 > pi/2 for meridional geodesic // which is not a shortest path. diff --git a/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google-instructions.html b/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google-instructions.html index 8d6f25c56..89c2e5605 100644 --- a/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google-instructions.html +++ b/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google-instructions.html @@ -101,8 +101,8 @@ In putting together this Google Maps demonstration, I started with the sample code - - geometry-headings.html. + + geometry-headings.

Charles Karney diff --git a/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google.html b/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google.html index 35576cf87..5a48d7e2e 100644 --- a/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google.html +++ b/gtsam/3rdparty/GeographicLib/doc/scripts/geod-google.html @@ -24,13 +24,18 @@ Google Maps, WGS84 ellipsoid, GeographicLib" /> - + -
diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Config.h b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Config.h index 1e5e30b9b..2249dd127 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Config.h +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Config.h @@ -1,8 +1,8 @@ // This will be overwritten by ./configure -#define GEOGRAPHICLIB_VERSION_STRING "1.34" +#define GEOGRAPHICLIB_VERSION_STRING "1.35" #define GEOGRAPHICLIB_VERSION_MAJOR 1 -#define GEOGRAPHICLIB_VERSION_MINOR 34 +#define GEOGRAPHICLIB_VERSION_MINOR 35 #define GEOGRAPHICLIB_VERSION_PATCH 0 // Undefine HAVE_LONG_DOUBLE if this type is unknown to the compiler diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Constants.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Constants.hpp index e37ec62ce..99baff31f 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Constants.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Constants.hpp @@ -21,6 +21,17 @@ # elif defined(__GXX_EXPERIMENTAL_CXX0X__) # define STATIC_ASSERT static_assert # elif defined(_MSC_VER) && _MSC_VER >= 1600 +// For reference, here is a table of Visual Studio and _MSC_VER +// correspondences: +// +// _MSC_VER Visual Studio +// 1300 vc7 +// 1311 vc7.1 (2003) +// 1400 vc8 (2005) +// 1500 vc9 (2008) +// 1600 vc10 (2010) +// 1700 vc11 (2012) +// 1800 vc12 (2013) # define STATIC_ASSERT static_assert # else # define STATIC_ASSERT(cond,reason) \ diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Geodesic.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Geodesic.hpp index cd687acce..6cec279a2 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Geodesic.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Geodesic.hpp @@ -162,8 +162,8 @@ namespace GeographicLib { * Example of use: * \include example-Geodesic.cpp * - * Geod is a command-line utility providing access - * to the functionality of Geodesic and GeodesicLine. + * GeodSolve is a command-line utility + * providing access to the functionality of Geodesic and GeodesicLine. **********************************************************************/ class GEOGRAPHICLIB_EXPORT Geodesic { diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicExact.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicExact.hpp index c0444f70e..e76daee4c 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicExact.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicExact.hpp @@ -25,7 +25,7 @@ namespace GeographicLib { class GeodesicLineExact; /** - * \brief Exact %Geodesic calculations + * \brief Exact geodesic calculations * * The equations for geodesics on an ellipsoid can be expressed in terms of * incomplete elliptic integrals. The Geodesic class expands these integrals @@ -67,14 +67,14 @@ namespace GeographicLib { * about 8 decimal digits for \e b/\e a ∈ [1/4, 4]. * * See \ref geodellip for the formulation. See the documentation on the - * Geodesic class for additional information on the geodesics problems. + * Geodesic class for additional information on the geodesic problems. * * Example of use: * \include example-GeodesicExact.cpp * - * Geod is a command-line utility providing access - * to the functionality of GeodesicExact and GeodesicLineExact (via the -E - * option). + * GeodSolve is a command-line utility + * providing access to the functionality of GeodesicExact and + * GeodesicLineExact (via the -E option). **********************************************************************/ class GEOGRAPHICLIB_EXPORT GeodesicExact { diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLine.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLine.hpp index fd968e2cf..c901b3007 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLine.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLine.hpp @@ -52,8 +52,8 @@ namespace GeographicLib { * Example of use: * \include example-GeodesicLine.cpp * - * Geod is a command-line utility providing access - * to the functionality of Geodesic and GeodesicLine. + * GeodSolve is a command-line utility + * providing access to the functionality of Geodesic and GeodesicLine. **********************************************************************/ class GEOGRAPHICLIB_EXPORT GeodesicLine { diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLineExact.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLineExact.hpp index ca7d0d211..a2e19f845 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLineExact.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/GeodesicLineExact.hpp @@ -27,9 +27,9 @@ namespace GeographicLib { * Example of use: * \include example-GeodesicLineExact.cpp * - * Geod is a command-line utility providing access - * to the functionality of GeodesicExact and GeodesicLineExact (via the -E - * option). + * GeodSolve is a command-line utility + * providing access to the functionality of GeodesicExact and + * GeodesicLineExact (via the -E option). **********************************************************************/ class GEOGRAPHICLIB_EXPORT GeodesicLineExact { diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Math.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Math.hpp index 9971ebdf0..2ef649e96 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Math.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Math.hpp @@ -18,8 +18,11 @@ * Are C++11 math functions available? **********************************************************************/ #if !defined(GEOGRAPHICLIB_CPLUSPLUS11_MATH) -# if defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ >= 8 \ - && __cplusplus >= 201103 +# if defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ >= 7 \ + && __cplusplus >= 201103 && !(defined(__ANDROID__) || defined(ANDROID)) +// The android toolchain uses g++ and supports C++11, but not, apparently, the +// new mathematical functions introduced with C++11. Android toolchains might +// define __ANDROID__ or ANDROID; so need to check both. # define GEOGRAPHICLIB_CPLUSPLUS11_MATH 1 # elif defined(_MSC_VER) && _MSC_VER >= 1800 # define GEOGRAPHICLIB_CPLUSPLUS11_MATH 1 diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/UTMUPS.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/UTMUPS.hpp index 512ba5c43..9fc19c66e 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/UTMUPS.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/UTMUPS.hpp @@ -70,7 +70,7 @@ namespace GeographicLib { static const int epsg01N = 32601; // EPSG code for UTM 01N static const int epsg60N = 32660; // EPSG code for UTM 60N static const int epsgN = 32661; // EPSG code for UPS N - static const int epsg01S = 32701; // EPSG code for UTM 01N + static const int epsg01S = 32701; // EPSG code for UTM 01S static const int epsg60S = 32760; // EPSG code for UTM 60S static const int epsgS = 32761; // EPSG code for UPS S static real CentralMeridian(int zone) throw() diff --git a/gtsam/3rdparty/GeographicLib/legacy/C/geodesic.c b/gtsam/3rdparty/GeographicLib/legacy/C/geodesic.c index bd9fc960f..f46a6ff41 100644 --- a/gtsam/3rdparty/GeographicLib/legacy/C/geodesic.c +++ b/gtsam/3rdparty/GeographicLib/legacy/C/geodesic.c @@ -700,7 +700,7 @@ real geod_geninverse(const struct geod_geodesic* g, /* Add the check for sig12 since zero length geodesics might yield m12 < * 0. Test case was * - * echo 20.001 0 20.001 0 | Geod -i + * echo 20.001 0 20.001 0 | GeodSolve -i * * In fact, we will have sig12 > pi/2 for meridional geodesic which is * not a shortest path. */ diff --git a/gtsam/3rdparty/GeographicLib/legacy/Fortran/geodesic.for b/gtsam/3rdparty/GeographicLib/legacy/Fortran/geodesic.for index 6501363c1..aaef081c5 100644 --- a/gtsam/3rdparty/GeographicLib/legacy/Fortran/geodesic.for +++ b/gtsam/3rdparty/GeographicLib/legacy/Fortran/geodesic.for @@ -685,7 +685,7 @@ * Add the check for sig12 since zero length geodesics might yield m12 < * 0. Test case was * -* echo 20.001 0 20.001 0 | Geod -i +* echo 20.001 0 20.001 0 | GeodSolve -i * * In fact, we will have sig12 > pi/2 for meridional geodesic which is * not a shortest path. diff --git a/gtsam/3rdparty/GeographicLib/man/CMakeLists.txt b/gtsam/3rdparty/GeographicLib/man/CMakeLists.txt index d8af12998..ccdf213c3 100644 --- a/gtsam/3rdparty/GeographicLib/man/CMakeLists.txt +++ b/gtsam/3rdparty/GeographicLib/man/CMakeLists.txt @@ -34,7 +34,7 @@ foreach (TOOL ${TOOLS}) MAIN_DEPENDENCY ${TOOL}.pod) add_custom_command (OUTPUT ${TOOL}.1.html COMMAND - pod2html --noindex ${CMAKE_CURRENT_SOURCE_DIR}/${TOOL}.pod | + pod2html --title "'${TOOL}(1)'" --noindex ${CMAKE_CURRENT_SOURCE_DIR}/${TOOL}.pod | sed -e 's%%%' -e 's%\\\([^<>]*\\\)\(\\\(.\\\)\)%&%'g > ${TOOL}.1.html && cp ${TOOL}.1.html ../doc/html-stage/ diff --git a/gtsam/3rdparty/GeographicLib/man/CartConvert.1 b/gtsam/3rdparty/GeographicLib/man/CartConvert.1 index 02dfbfa5a..9a2f74660 100644 --- a/gtsam/3rdparty/GeographicLib/man/CartConvert.1 +++ b/gtsam/3rdparty/GeographicLib/man/CartConvert.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "CARTCONVERT 1" -.TH CARTCONVERT 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH CARTCONVERT 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/CartConvert.1.html b/gtsam/3rdparty/GeographicLib/man/CartConvert.1.html index 38dcd6af1..17ec77507 100644 --- a/gtsam/3rdparty/GeographicLib/man/CartConvert.1.html +++ b/gtsam/3rdparty/GeographicLib/man/CartConvert.1.html @@ -2,7 +2,7 @@ - +CartConvert(1) diff --git a/gtsam/3rdparty/GeographicLib/man/CartConvert.usage b/gtsam/3rdparty/GeographicLib/man/CartConvert.usage index f9f6f05ac..57374a78f 100644 --- a/gtsam/3rdparty/GeographicLib/man/CartConvert.usage +++ b/gtsam/3rdparty/GeographicLib/man/CartConvert.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " CartConvert --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/CartConvert.1.html\n"; +" http://geographiclib.sf.net/1.35/CartConvert.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/ConicProj.1 b/gtsam/3rdparty/GeographicLib/man/ConicProj.1 index 85f65f9a6..997017d7f 100644 --- a/gtsam/3rdparty/GeographicLib/man/ConicProj.1 +++ b/gtsam/3rdparty/GeographicLib/man/ConicProj.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "CONICPROJ 1" -.TH CONICPROJ 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH CONICPROJ 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/ConicProj.1.html b/gtsam/3rdparty/GeographicLib/man/ConicProj.1.html index e4af0e6b5..9de3fd683 100644 --- a/gtsam/3rdparty/GeographicLib/man/ConicProj.1.html +++ b/gtsam/3rdparty/GeographicLib/man/ConicProj.1.html @@ -2,7 +2,7 @@ - +ConicProj(1) diff --git a/gtsam/3rdparty/GeographicLib/man/ConicProj.usage b/gtsam/3rdparty/GeographicLib/man/ConicProj.usage index 3fbcc4481..a84aa4fde 100644 --- a/gtsam/3rdparty/GeographicLib/man/ConicProj.usage +++ b/gtsam/3rdparty/GeographicLib/man/ConicProj.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " ConicProj --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/ConicProj.1.html\n"; +" http://geographiclib.sf.net/1.35/ConicProj.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/GeoConvert.1 b/gtsam/3rdparty/GeographicLib/man/GeoConvert.1 index 0dfa11283..747b8cd3f 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoConvert.1 +++ b/gtsam/3rdparty/GeographicLib/man/GeoConvert.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "GEOCONVERT 1" -.TH GEOCONVERT 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH GEOCONVERT 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/GeoConvert.1.html b/gtsam/3rdparty/GeographicLib/man/GeoConvert.1.html index c46e2e7c0..5c117b49e 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoConvert.1.html +++ b/gtsam/3rdparty/GeographicLib/man/GeoConvert.1.html @@ -2,7 +2,7 @@ - +GeoConvert(1) diff --git a/gtsam/3rdparty/GeographicLib/man/GeoConvert.usage b/gtsam/3rdparty/GeographicLib/man/GeoConvert.usage index 7a0f8a852..19d78a3ef 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoConvert.usage +++ b/gtsam/3rdparty/GeographicLib/man/GeoConvert.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " GeoConvert --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/GeoConvert.1.html\n"; +" http://geographiclib.sf.net/1.35/GeoConvert.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/GeodSolve.1 b/gtsam/3rdparty/GeographicLib/man/GeodSolve.1 index 06ca388e4..3bd025aa4 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodSolve.1 +++ b/gtsam/3rdparty/GeographicLib/man/GeodSolve.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "GEODSOLVE 1" -.TH GEODSOLVE 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH GEODSOLVE 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/GeodSolve.1.html b/gtsam/3rdparty/GeographicLib/man/GeodSolve.1.html index 262bccf3b..8af2af9e7 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodSolve.1.html +++ b/gtsam/3rdparty/GeographicLib/man/GeodSolve.1.html @@ -2,7 +2,7 @@ - +GeodSolve(1) diff --git a/gtsam/3rdparty/GeographicLib/man/GeodSolve.usage b/gtsam/3rdparty/GeographicLib/man/GeodSolve.usage index 48bc7261e..78ec73ec7 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodSolve.usage +++ b/gtsam/3rdparty/GeographicLib/man/GeodSolve.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " GeodSolve --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/GeodSolve.1.html\n"; +" http://geographiclib.sf.net/1.35/GeodSolve.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1 b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1 index a4cc6680e..9b87078a3 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1 +++ b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "GEODESICPROJ 1" -.TH GEODESICPROJ 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH GEODESICPROJ 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1.html b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1.html index c40ca5a4d..2b63cf7de 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1.html +++ b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.1.html @@ -2,7 +2,7 @@ - +GeodesicProj(1) diff --git a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.usage b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.usage index 9a5ab2517..013fb7acf 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeodesicProj.usage +++ b/gtsam/3rdparty/GeographicLib/man/GeodesicProj.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " GeodesicProj --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/GeodesicProj.1.html\n"; +" http://geographiclib.sf.net/1.35/GeodesicProj.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/GeoidEval.1 b/gtsam/3rdparty/GeographicLib/man/GeoidEval.1 index 681a096f0..7c5086e1c 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoidEval.1 +++ b/gtsam/3rdparty/GeographicLib/man/GeoidEval.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "GEOIDEVAL 1" -.TH GEOIDEVAL 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH GEOIDEVAL 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/GeoidEval.1.html b/gtsam/3rdparty/GeographicLib/man/GeoidEval.1.html index 91f8f3610..ea131d8a7 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoidEval.1.html +++ b/gtsam/3rdparty/GeographicLib/man/GeoidEval.1.html @@ -2,7 +2,7 @@ - +GeoidEval(1) diff --git a/gtsam/3rdparty/GeographicLib/man/GeoidEval.usage b/gtsam/3rdparty/GeographicLib/man/GeoidEval.usage index 727811108..4fa144ca7 100644 --- a/gtsam/3rdparty/GeographicLib/man/GeoidEval.usage +++ b/gtsam/3rdparty/GeographicLib/man/GeoidEval.usage @@ -10,7 +10,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " GeoidEval --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/GeoidEval.1.html\n"; +" http://geographiclib.sf.net/1.35/GeoidEval.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/Gravity.1 b/gtsam/3rdparty/GeographicLib/man/Gravity.1 index 97e247b7f..96462b244 100644 --- a/gtsam/3rdparty/GeographicLib/man/Gravity.1 +++ b/gtsam/3rdparty/GeographicLib/man/Gravity.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "GRAVITY 1" -.TH GRAVITY 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH GRAVITY 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/Gravity.1.html b/gtsam/3rdparty/GeographicLib/man/Gravity.1.html index 3fbdba19b..cca8ad941 100644 --- a/gtsam/3rdparty/GeographicLib/man/Gravity.1.html +++ b/gtsam/3rdparty/GeographicLib/man/Gravity.1.html @@ -2,7 +2,7 @@ - +Gravity(1) diff --git a/gtsam/3rdparty/GeographicLib/man/Gravity.usage b/gtsam/3rdparty/GeographicLib/man/Gravity.usage index 20b522307..d60547f09 100644 --- a/gtsam/3rdparty/GeographicLib/man/Gravity.usage +++ b/gtsam/3rdparty/GeographicLib/man/Gravity.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " Gravity --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/Gravity.1.html\n"; +" http://geographiclib.sf.net/1.35/Gravity.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/MagneticField.1 b/gtsam/3rdparty/GeographicLib/man/MagneticField.1 index 0659f2325..3d520cb6b 100644 --- a/gtsam/3rdparty/GeographicLib/man/MagneticField.1 +++ b/gtsam/3rdparty/GeographicLib/man/MagneticField.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "MAGNETICFIELD 1" -.TH MAGNETICFIELD 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH MAGNETICFIELD 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/MagneticField.1.html b/gtsam/3rdparty/GeographicLib/man/MagneticField.1.html index cac6794c8..dd1884602 100644 --- a/gtsam/3rdparty/GeographicLib/man/MagneticField.1.html +++ b/gtsam/3rdparty/GeographicLib/man/MagneticField.1.html @@ -2,7 +2,7 @@ - +MagneticField(1) diff --git a/gtsam/3rdparty/GeographicLib/man/MagneticField.usage b/gtsam/3rdparty/GeographicLib/man/MagneticField.usage index b6b89d0de..9f1ca8b9b 100644 --- a/gtsam/3rdparty/GeographicLib/man/MagneticField.usage +++ b/gtsam/3rdparty/GeographicLib/man/MagneticField.usage @@ -10,7 +10,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " MagneticField --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/MagneticField.1.html\n"; +" http://geographiclib.sf.net/1.35/MagneticField.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/man/Makefile.am b/gtsam/3rdparty/GeographicLib/man/Makefile.am index 8fae122f6..cad9e45ce 100644 --- a/gtsam/3rdparty/GeographicLib/man/Makefile.am +++ b/gtsam/3rdparty/GeographicLib/man/Makefile.am @@ -62,7 +62,7 @@ if HAVE_PODPROGS $(POD2MAN) $^ > $@ .pod.1.html: - pod2html --noindex $^ | $(PODFIX) > $@ + pod2html --noindex --tile "$*(1)" $^ | $(PODFIX) > $@ else diff --git a/gtsam/3rdparty/GeographicLib/man/Makefile.in b/gtsam/3rdparty/GeographicLib/man/Makefile.in index b434fd523..3399f753c 100644 --- a/gtsam/3rdparty/GeographicLib/man/Makefile.in +++ b/gtsam/3rdparty/GeographicLib/man/Makefile.in @@ -567,7 +567,7 @@ htmlman: $(HTMLMAN) @HAVE_PODPROGS_TRUE@ $(POD2MAN) $^ > $@ @HAVE_PODPROGS_TRUE@.pod.1.html: -@HAVE_PODPROGS_TRUE@ pod2html --noindex $^ | $(PODFIX) > $@ +@HAVE_PODPROGS_TRUE@ pod2html --noindex --tile "$*(1)" $^ | $(PODFIX) > $@ @HAVE_PODPROGS_FALSE@CartConvert.usage: @HAVE_PODPROGS_FALSE@ $(USAGECMD) diff --git a/gtsam/3rdparty/GeographicLib/man/Planimeter.1 b/gtsam/3rdparty/GeographicLib/man/Planimeter.1 index 28bbbf46f..07e1563cd 100644 --- a/gtsam/3rdparty/GeographicLib/man/Planimeter.1 +++ b/gtsam/3rdparty/GeographicLib/man/Planimeter.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "PLANIMETER 1" -.TH PLANIMETER 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH PLANIMETER 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l @@ -200,7 +200,7 @@ is allowed for \fIf\fR. (Also, if \fIf\fR > 1, the flattening is set to 1/\fIf\fR.) By default, the \s-1WGS84\s0 ellipsoid is used, \fIa\fR = 6378137 m, \&\fIf\fR = 1/298.257223563. If entering vertices as \s-1UTM/UPS\s0 or \s-1MGRS\s0 coordinates, use the default ellipsoid, since the conversion of these -coordinates to latitude and longitude uses the \s-1WGS84\s0 parameters. +coordinates to latitude and longitude always uses the \s-1WGS84\s0 parameters. .IP "\fB\-\-comment\-delimiter\fR" 4 .IX Item "--comment-delimiter" set the comment delimiter to \fIcommentdelim\fR (e.g., \*(L"#\*(R" or \*(L"//\*(R"). If diff --git a/gtsam/3rdparty/GeographicLib/man/Planimeter.1.html b/gtsam/3rdparty/GeographicLib/man/Planimeter.1.html index b86a5ecd7..7a6eb52c4 100644 --- a/gtsam/3rdparty/GeographicLib/man/Planimeter.1.html +++ b/gtsam/3rdparty/GeographicLib/man/Planimeter.1.html @@ -2,7 +2,7 @@ - +Planimeter(1) @@ -56,7 +56,7 @@
-e
-

specify the ellipsoid via a f; the equatorial radius is a and the flattening is f. Setting f = 0 results in a sphere. Specify f < 0 for a prolate ellipsoid. A simple fraction, e.g., 1/297, is allowed for f. (Also, if f > 1, the flattening is set to 1/f.) By default, the WGS84 ellipsoid is used, a = 6378137 m, f = 1/298.257223563. If entering vertices as UTM/UPS or MGRS coordinates, use the default ellipsoid, since the conversion of these coordinates to latitude and longitude uses the WGS84 parameters.

+

specify the ellipsoid via a f; the equatorial radius is a and the flattening is f. Setting f = 0 results in a sphere. Specify f < 0 for a prolate ellipsoid. A simple fraction, e.g., 1/297, is allowed for f. (Also, if f > 1, the flattening is set to 1/f.) By default, the WGS84 ellipsoid is used, a = 6378137 m, f = 1/298.257223563. If entering vertices as UTM/UPS or MGRS coordinates, use the default ellipsoid, since the conversion of these coordinates to latitude and longitude always uses the WGS84 parameters.

--comment-delimiter
diff --git a/gtsam/3rdparty/GeographicLib/man/Planimeter.pod b/gtsam/3rdparty/GeographicLib/man/Planimeter.pod index 828ddc100..d78afca8c 100644 --- a/gtsam/3rdparty/GeographicLib/man/Planimeter.pod +++ b/gtsam/3rdparty/GeographicLib/man/Planimeter.pod @@ -78,7 +78,7 @@ is allowed for I. (Also, if I E 1, the flattening is set to 1/I.) By default, the WGS84 ellipsoid is used, I = 6378137 m, I = 1/298.257223563. If entering vertices as UTM/UPS or MGRS coordinates, use the default ellipsoid, since the conversion of these -coordinates to latitude and longitude uses the WGS84 parameters. +coordinates to latitude and longitude always uses the WGS84 parameters. =item B<--comment-delimiter> diff --git a/gtsam/3rdparty/GeographicLib/man/Planimeter.usage b/gtsam/3rdparty/GeographicLib/man/Planimeter.usage index 3a45ad85d..b96b12f9f 100644 --- a/gtsam/3rdparty/GeographicLib/man/Planimeter.usage +++ b/gtsam/3rdparty/GeographicLib/man/Planimeter.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " Planimeter --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/Planimeter.1.html\n"; +" http://geographiclib.sf.net/1.35/Planimeter.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" @@ -76,7 +76,7 @@ int usage(int retval, bool brief) { " default, the WGS84 ellipsoid is used, a = 6378137 m, f =\n" " 1/298.257223563. If entering vertices as UTM/UPS or MGRS\n" " coordinates, use the default ellipsoid, since the conversion of\n" -" these coordinates to latitude and longitude uses the WGS84\n" +" these coordinates to latitude and longitude always uses the WGS84\n" " parameters.\n" "\n" " --comment-delimiter\n" diff --git a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1 b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1 index 0b03ff6ca..3a5d3d115 100644 --- a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1 +++ b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1 @@ -124,7 +124,7 @@ .\" ======================================================================== .\" .IX Title "TRANSVERSEMERCATORPROJ 1" -.TH TRANSVERSEMERCATORPROJ 1 "2013-12-11" "GeographicLib 1.34" "GeographicLib Utilities" +.TH TRANSVERSEMERCATORPROJ 1 "2014-03-13" "GeographicLib 1.35" "GeographicLib Utilities" .\" For nroff, turn off justification. Always turn off hyphenation; it makes .\" way too many mistakes in technical documents. .if n .ad l diff --git a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1.html b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1.html index c43bf74da..8b28a7d87 100644 --- a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1.html +++ b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.1.html @@ -2,7 +2,7 @@ - +TransverseMercatorProj(1) diff --git a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.usage b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.usage index 189744e8b..62d6045a7 100644 --- a/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.usage +++ b/gtsam/3rdparty/GeographicLib/man/TransverseMercatorProj.usage @@ -9,7 +9,7 @@ int usage(int retval, bool brief) { "For full documentation type:\n" " TransverseMercatorProj --help\n" "or visit:\n" -" http://geographiclib.sf.net/1.34/TransverseMercatorProj.1.html\n"; +" http://geographiclib.sf.net/1.35/TransverseMercatorProj.1.html\n"; else ( retval ? std::cerr : std::cout ) << "Man page:\n" "NAME\n" diff --git a/gtsam/3rdparty/GeographicLib/matlab/geodesicdirect.cpp b/gtsam/3rdparty/GeographicLib/matlab/geodesicdirect.cpp index 770feecd0..06a35e607 100644 --- a/gtsam/3rdparty/GeographicLib/matlab/geodesicdirect.cpp +++ b/gtsam/3rdparty/GeographicLib/matlab/geodesicdirect.cpp @@ -2,7 +2,7 @@ * \file geodesicdirect.cpp * \brief Matlab mex file for geographic to UTM/UPS conversions * - * Copyright (c) Charles Karney (2010-2011) and licensed + * Copyright (c) Charles Karney (2010-2013) and licensed * under the MIT/X11 License. For more information, see * http://geographiclib.sourceforge.net/ **********************************************************************/ @@ -17,11 +17,51 @@ #include #include +#include #include using namespace std; using namespace GeographicLib; +template void +compute(double a, double f, mwSize m, const double* geodesic, + double* latlong, double* aux) { + const double* lat1 = geodesic; + const double* lon1 = geodesic + m; + const double* azi1 = geodesic + 2*m; + const double* s12 = geodesic + 3*m; + double* lat2 = latlong; + double* lon2 = latlong + m; + double* azi2 = latlong + 2*m; + double* a12 = NULL; + double* m12 = NULL; + double* M12 = NULL; + double* M21 = NULL; + double* S12 = NULL; + if (aux) { + a12 = aux; + m12 = aux + m; + M12 = aux + 2*m; + M21 = aux + 3*m; + S12 = aux + 4*m; + } + + const G g(a, f); + for (mwIndex i = 0; i < m; ++i) { + if (abs(lat1[i]) <= 90 && + lon1[i] >= -540 && lon1[i] < 540 && + azi1[i] >= -540 && azi1[i] < 540) { + if (aux) + a12[i] = g.Direct(lat1[i], lon1[i], azi1[i], s12[i], + lat2[i], lon2[i], azi2[i], + m12[i], M12[i], M21[i], S12[i]); + else + g.Direct(lat1[i], lon1[i], azi1[i], s12[i], + lat2[i], lon2[i], azi2[i]); + } + } +} + void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] ) { @@ -54,48 +94,22 @@ void mexFunction( int nlhs, mxArray* plhs[], mwSize m = mxGetM(prhs[0]); - double* lat1 = mxGetPr(prhs[0]); - double* lon1 = lat1 + m; - double* azi1 = lat1 + 2*m; - double* s12 = lat1 + 3*m; + const double* geodesic = mxGetPr(prhs[0]); - plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL); - double* lat2 = mxGetPr(plhs[0]); - std::fill(lat2, lat2 + 3*m, Math::NaN()); - double* lon2 = lat2 + m; - double* azi2 = lat2 + 2*m; - double* a12 = NULL; - double* m12 = NULL; - double* M12 = NULL; - double* M21 = NULL; - double* S12 = NULL; - bool aux = nlhs == 2; + double* latlong = mxGetPr(plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL)); + std::fill(latlong, latlong + 3*m, Math::NaN()); - if (aux) { - plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL); - a12 = mxGetPr(plhs[1]); - std::fill(a12, a12 + 5*m, Math::NaN()); - m12 = a12 + m; - M12 = a12 + 2*m; - M21 = a12 + 3*m; - S12 = a12 + 4*m; - } + double* aux = + nlhs == 2 ? mxGetPr(plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL)) : + NULL; + if (aux) + std::fill(aux, aux + 5*m, Math::NaN()); try { - const Geodesic g(a, f); - for (mwIndex i = 0; i < m; ++i) { - if (abs(lat1[i]) <= 90 && - lon1[i] >= -540 && lon1[i] < 540 && - azi1[i] >= -540 && azi1[i] < 540) { - if (aux) - a12[i] = g.Direct(lat1[i], lon1[i], azi1[i], s12[i], - lat2[i], lon2[i], azi2[i], m12[i], - M12[i], M21[i], S12[i]); - else - g.Direct(lat1[i], lon1[i], azi1[i], s12[i], - lat2[i], lon2[i], azi2[i]); - } - } + if (std::abs(f) <= 0.02) + compute(a, f, m, geodesic, latlong, aux); + else + compute(a, f, m, geodesic, latlong, aux); } catch (const std::exception& e) { mexErrMsgTxt(e.what()); diff --git a/gtsam/3rdparty/GeographicLib/matlab/geodesicinverse.cpp b/gtsam/3rdparty/GeographicLib/matlab/geodesicinverse.cpp index c3b1b2ace..f03877f18 100644 --- a/gtsam/3rdparty/GeographicLib/matlab/geodesicinverse.cpp +++ b/gtsam/3rdparty/GeographicLib/matlab/geodesicinverse.cpp @@ -2,7 +2,7 @@ * \file geodesicinverse.cpp * \brief Matlab mex file for geographic to UTM/UPS conversions * - * Copyright (c) Charles Karney (2010-2011) and licensed + * Copyright (c) Charles Karney (2010-2013) and licensed * under the MIT/X11 License. For more information, see * http://geographiclib.sourceforge.net/ **********************************************************************/ @@ -17,11 +17,50 @@ #include #include +#include #include using namespace std; using namespace GeographicLib; +template void +compute(double a, double f, mwSize m, const double* latlong, + double* geodesic, double* aux) { + const double* lat1 = latlong; + const double* lon1 = latlong + m; + const double* lat2 = latlong + 2*m; + const double* lon2 = latlong + 3*m; + double* azi1 = geodesic; + double* azi2 = geodesic + m; + double* s12 = geodesic + 2*m; + double* a12 = NULL; + double* m12 = NULL; + double* M12 = NULL; + double* M21 = NULL; + double* S12 = NULL; + if (aux) { + a12 = aux; + m12 = aux + m; + M12 = aux + 2*m; + M21 = aux + 3*m; + S12 = aux + 4*m; + } + + const G g(a, f); + for (mwIndex i = 0; i < m; ++i) { + if (abs(lat1[i]) <= 90 && lon1[i] >= -540 && lon1[i] < 540 && + abs(lat2[i]) <= 90 && lon2[i] >= -540 && lon2[i] < 540) { + if (aux) + a12[i] = g.Inverse(lat1[i], lon1[i], lat2[i], lon2[i], + s12[i], azi1[i], azi2[i], + m12[i], M12[i], M21[i], S12[i]); + else + g.Inverse(lat1[i], lon1[i], lat2[i], lon2[i], + s12[i], azi1[i], azi2[i]); + } + } +} + void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] ) { @@ -54,47 +93,22 @@ void mexFunction( int nlhs, mxArray* plhs[], mwSize m = mxGetM(prhs[0]); - double* lat1 = mxGetPr(prhs[0]); - double* lon1 = lat1 + m; - double* lat2 = lat1 + 2*m; - double* lon2 = lat1 + 3*m; + const double* latlong = mxGetPr(prhs[0]); - plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL); - double* azi1 = mxGetPr(plhs[0]); - std::fill(azi1, azi1 + 3*m, Math::NaN()); - double* azi2 = azi1 + m; - double* s12 = azi1 + 2*m; - double* a12 = NULL; - double* m12 = NULL; - double* M12 = NULL; - double* M21 = NULL; - double* S12 = NULL; - bool aux = nlhs == 2; + double* geodesic = mxGetPr(plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL)); + std::fill(geodesic, geodesic + 3*m, Math::NaN()); - if (aux) { - plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL); - a12 = mxGetPr(plhs[1]); - std::fill(a12, a12 + 5*m, Math::NaN()); - m12 = a12 + m; - M12 = a12 + 2*m; - M21 = a12 + 3*m; - S12 = a12 + 4*m; - } + double* aux = + nlhs == 2 ? mxGetPr(plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL)) : + NULL; + if (aux) + std::fill(aux, aux + 5*m, Math::NaN()); try { - const Geodesic g(a, f); - for (mwIndex i = 0; i < m; ++i) { - if (abs(lat1[i]) <= 90 && lon1[i] >= -540 && lon1[i] < 540 && - abs(lat2[i]) <= 90 && lon2[i] >= -540 && lon2[i] < 540) { - if (aux) - a12[i] = g.Inverse(lat1[i], lon1[i], lat2[i], lon2[i], - s12[i], azi1[i], azi2[i], - m12[i], M12[i], M21[i], S12[i]); - else - g.Inverse(lat1[i], lon1[i], lat2[i], lon2[i], - s12[i], azi1[i], azi2[i]); - } - } + if (std::abs(f) <= 0.02) + compute(a, f, m, latlong, geodesic, aux); + else + compute(a, f, m, latlong, geodesic, aux); } catch (const std::exception& e) { mexErrMsgTxt(e.what()); diff --git a/gtsam/3rdparty/GeographicLib/matlab/geodesicline.cpp b/gtsam/3rdparty/GeographicLib/matlab/geodesicline.cpp index 4de50e476..7382cc03a 100644 --- a/gtsam/3rdparty/GeographicLib/matlab/geodesicline.cpp +++ b/gtsam/3rdparty/GeographicLib/matlab/geodesicline.cpp @@ -2,7 +2,7 @@ * \file geodesicline.cpp * \brief Matlab mex file for geographic to UTM/UPS conversions * - * Copyright (c) Charles Karney (2010-2011) and licensed + * Copyright (c) Charles Karney (2010-2013) and licensed * under the MIT/X11 License. For more information, see * http://geographiclib.sourceforge.net/ **********************************************************************/ @@ -16,11 +16,40 @@ // -lGeographic geodesicline.cpp #include +#include #include using namespace std; using namespace GeographicLib; +template void +compute(double a, double f, double lat1, double lon1, double azi1, + mwSize m, const double* s12, double* latlong, double* aux) { + double* lat2 = latlong; + double* lon2 = latlong + m; + double* azi2 = latlong + 2*m; + double* a12 = NULL; + double* m12 = NULL; + double* M12 = NULL; + double* M21 = NULL; + double* S12 = NULL; + if (aux) { + a12 = aux; + m12 = aux + m; + M12 = aux + 2*m; + M21 = aux + 3*m; + S12 = aux + 4*m; + } + const G g(a, f); + const L l(g, lat1, lon1, azi1); + for (mwIndex i = 0; i < m; ++i) + if (aux) + a12[i] = l.Position(s12[i], lat2[i], lon2[i], azi2[i], + m12[i], M12[i], M21[i], S12[i]); + else + l.Position(s12[i], lat2[i], lon2[i], azi2[i]); +} + void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] ) { @@ -70,41 +99,24 @@ void mexFunction( int nlhs, mxArray* plhs[], double* s12 = mxGetPr(prhs[3]); - plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL); - double* lat2 = mxGetPr(plhs[0]); - double* lon2 = lat2 + m; - double* azi2 = lat2 + 2*m; - double* a12 = NULL; - double* m12 = NULL; - double* M12 = NULL; - double* M21 = NULL; - double* S12 = NULL; - bool aux = nlhs == 2; - - if (aux) { - plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL); - a12 = mxGetPr(plhs[1]); - m12 = a12 + m; - M12 = a12 + 2*m; - M21 = a12 + 3*m; - S12 = a12 + 4*m; - } + double* latlong = mxGetPr(plhs[0] = mxCreateDoubleMatrix(m, 3, mxREAL)); + double* aux = + nlhs == 2 ? mxGetPr(plhs[1] = mxCreateDoubleMatrix(m, 5, mxREAL)) : + NULL; try { - const Geodesic g(a, f); if (!(abs(lat1) <= 90)) throw GeographicErr("Invalid latitude"); if (!(lon1 >= -540 || lon1 < 540)) throw GeographicErr("Invalid longitude"); if (!(azi1 >= -540 || azi1 < 540)) throw GeographicErr("Invalid azimuth"); - const GeodesicLine l(g, lat1, lon1, azi1); - for (mwIndex i = 0; i < m; ++i) - if (aux) - a12[i] = l.Position(s12[i], lat2[i], lon2[i], azi2[i], - m12[i], M12[i], M21[i], S12[i]); - else - l.Position(s12[i], lat2[i], lon2[i], azi2[i]); + if (std::abs(f) <= 0.02) + compute + (a, f, lat1, lon1, azi1, m, s12, latlong, aux); + else + compute + (a, f, lat1, lon1, azi1, m, s12, latlong, aux); } catch (const std::exception& e) { mexErrMsgTxt(e.what()); diff --git a/gtsam/3rdparty/GeographicLib/pom.xml b/gtsam/3rdparty/GeographicLib/pom.xml new file mode 100644 index 000000000..2d40845a3 --- /dev/null +++ b/gtsam/3rdparty/GeographicLib/pom.xml @@ -0,0 +1,82 @@ + + 4.0.0 + + + com.sri.vt.majic + majic-parent + 0.1.9-SNAPSHOT + + + com.sri.vt + geographiclib + 1.35-SNAPSHOT + majic-cmake + GeographicLib + + + + os-windows + + + Windows + + + + ON + + + + os-linux + + + Linux + + + + OFF + + + + + + + artifactory-vt + SRI VT Repository + https://artifactory-vt.sarnoff.internal/artifactory/repo + + + + + + + com.sri.vt.majic + build-helper-maven-plugin + + + default-cmake-configure + + + ON + BOTH + OFF + OFF + ${build.netgeographiclib} + + + + + default-cmake-test + + + Release + + + + + + + + diff --git a/gtsam/3rdparty/GeographicLib/src/UTMUPS.cpp b/gtsam/3rdparty/GeographicLib/src/UTMUPS.cpp index 2abf43e11..88c151a6a 100644 --- a/gtsam/3rdparty/GeographicLib/src/UTMUPS.cpp +++ b/gtsam/3rdparty/GeographicLib/src/UTMUPS.cpp @@ -269,21 +269,19 @@ namespace GeographicLib { } void UTMUPS::DecodeEPSG(int epsg, int& zone, bool& northp) throw() { + northp = false; if (epsg >= epsg01N && epsg <= epsg60N) { - zone = epsg - epsg01N + 1; + zone = (epsg - epsg01N) + MINUTMZONE; northp = true; } else if (epsg == epsgN) { zone = UPS; northp = true; } else if (epsg >= epsg01S && epsg <= epsg60S) { - zone = epsg - epsg01S + 1; - northp = false; + zone = (epsg - epsg01S) + MINUTMZONE; } else if (epsg == epsgS) { zone = UPS; - northp = false; } else { zone = INVALID; - northp = false; } } @@ -292,7 +290,7 @@ namespace GeographicLib { if (zone == UPS) epsg = epsgS; else if (zone >= MINUTMZONE && zone <= MAXUTMZONE) - epsg = epsg + (zone - MINUTMZONE) + epsg01S; + epsg = (zone - MINUTMZONE) + epsg01S; if (epsg >= 0 && northp) epsg += epsgN - epsgS; return epsg; diff --git a/gtsam/3rdparty/GeographicLib/tools/CMakeLists.txt b/gtsam/3rdparty/GeographicLib/tools/CMakeLists.txt index 98233fe29..987505817 100644 --- a/gtsam/3rdparty/GeographicLib/tools/CMakeLists.txt +++ b/gtsam/3rdparty/GeographicLib/tools/CMakeLists.txt @@ -22,6 +22,12 @@ foreach (TOOL ${TOOLS}) endforeach () +if (MSVC OR CMAKE_CONFIGURATION_TYPES) + # Add _d suffix for your debug versions of the tools + set_target_properties (${TOOLS} PROPERTIES + DEBUG_POSTFIX ${CMAKE_DEBUG_POSTFIX}) +endif () + if (MSVC) get_target_property (_LIBTYPE ${PROJECT_LIBRARIES} TYPE) if (_LIBTYPE STREQUAL "SHARED_LIBRARY") @@ -62,21 +68,23 @@ enable_testing () # Here are the tests. They consists of calling the various tools with # --input-string and matching the output against regular expressions. -add_test (GeoConvert0 GeoConvert -p -3 -m --input-string "33.3 44.4") +add_test (NAME GeoConvert0 + COMMAND GeoConvert -p -3 -m --input-string "33.3 44.4") set_tests_properties (GeoConvert0 PROPERTIES PASS_REGULAR_EXPRESSION "38SMB4484") -add_test (GeoConvert1 GeoConvert -d --input-string "38smb") +add_test (NAME GeoConvert1 COMMAND GeoConvert -d --input-string "38smb") set_tests_properties (GeoConvert1 PROPERTIES PASS_REGULAR_EXPRESSION "32d59'14\\.1\"N 044d27'53\\.4\"E") -add_test (GeoConvert2 GeoConvert -p -2 --input-string "30d30'30\" 30.50833") +add_test (NAME GeoConvert2 + COMMAND GeoConvert -p -2 --input-string "30d30'30\" 30.50833") set_tests_properties (GeoConvert2 PROPERTIES PASS_REGULAR_EXPRESSION "30\\.508 30\\.508") -add_test (GeoConvert3 GeoConvert --junk) +add_test (NAME GeoConvert3 COMMAND GeoConvert --junk) set_tests_properties (GeoConvert3 PROPERTIES WILL_FAIL ON) -add_test (GeoConvert4 GeoConvert --input-string garbage) +add_test (NAME GeoConvert4 COMMAND GeoConvert --input-string garbage) set_tests_properties (GeoConvert4 PROPERTIES WILL_FAIL ON) # Check fix for DMS::Decode bug fixed on 2011-03-22 -add_test (GeoConvert5 GeoConvert --input-string "5d. 0") +add_test (NAME GeoConvert5 COMMAND GeoConvert --input-string "5d. 0") set_tests_properties (GeoConvert5 PROPERTIES WILL_FAIL ON) if (NOT (MSVC AND MSVC_VERSION MATCHES "1[78]..")) # Check fix for DMS::Decode double rounding bug fixed on 2012-11-15 @@ -84,48 +92,49 @@ if (NOT (MSVC AND MSVC_VERSION MATCHES "1[78]..")) # http://connect.microsoft.com/VisualStudio/feedback/details/776287 # OK to skip this test for these compilers because this is a question # of accuracy of the least significant bit. - add_test (GeoConvert6 GeoConvert -p 9 + add_test (NAME GeoConvert6 COMMAND GeoConvert -p 9 --input-string "0 179.99999999999998578") set_tests_properties (GeoConvert6 PROPERTIES PASS_REGULAR_EXPRESSION "179\\.9999999999999[7-9]") endif () -add_test (GeodSolve0 +add_test (NAME GeodSolve0 COMMAND GeodSolve -i -p 0 --input-string "40.6 -73.8 49d01'N 2d33'E") set_tests_properties (GeodSolve0 PROPERTIES PASS_REGULAR_EXPRESSION "53\\.47022 111\\.59367 5853226") -add_test (GeodSolve1 +add_test (NAME GeodSolve1 COMMAND GeodSolve -p 0 --input-string "40d38'23\"N 073d46'44\"W 53d30' 5850e3") set_tests_properties (GeodSolve1 PROPERTIES PASS_REGULAR_EXPRESSION "49\\.01467 2\\.56106 111\\.62947") # Check fix for antipodal prolate bug found 2010-09-04 -add_test (GeodSolve2 +add_test (NAME GeodSolve2 COMMAND GeodSolve -i -p 0 -e 6.4e6 -1/150 --input-string "0.07476 0 -0.07476 180") set_tests_properties (GeodSolve2 PROPERTIES PASS_REGULAR_EXPRESSION "90\\.00078 90\\.00078 20106193") # Another check for similar bug -add_test (GeodSolve3 +add_test (NAME GeodSolve3 COMMAND GeodSolve -i -p 0 -e 6.4e6 -1/150 --input-string "0.1 0 -0.1 180") set_tests_properties (GeodSolve3 PROPERTIES PASS_REGULAR_EXPRESSION "90\\.00105 90\\.00105 20106193") # Check fix for short line bug found 2010-05-21 -add_test (GeodSolve4 +add_test (NAME GeodSolve4 COMMAND GeodSolve -i --input-string "36.493349428792 0 36.49334942879201 .0000008") set_tests_properties (GeodSolve4 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 0\\.072") # Check fix for point2=pole bug found 2010-05-03 (but only with long double) -add_test (GeodSolve5 GeodSolve -p 0 --input-string "0.01777745589997 30 0 10e6") +add_test (NAME GeodSolve5 + COMMAND GeodSolve -p 0 --input-string "0.01777745589997 30 0 10e6") set_tests_properties (GeodSolve5 PROPERTIES PASS_REGULAR_EXPRESSION "90\\.00000 -150\\.00000 -180\\.00000;90\\.00000 30\\.00000 0\\.00000") # Check fix for volatile sbet12a bug found 2011-06-25 (gcc 4.4.4 x86 -O3) # Found again on 2012-03-27 with tdm-mingw32 (g++ 4.6.1). -add_test (GeodSolve6 GeodSolve -i --input-string +add_test (NAME GeodSolve6 COMMAND GeodSolve -i --input-string "88.202499451857 0 -88.202499451857 179.981022032992859592") -add_test (GeodSolve7 GeodSolve -i --input-string +add_test (NAME GeodSolve7 COMMAND GeodSolve -i --input-string "89.262080389218 0 -89.262080389218 179.992207982775375662") -add_test (GeodSolve8 GeodSolve -i --input-string +add_test (NAME GeodSolve8 COMMAND GeodSolve -i --input-string "89.333123580033 0 -89.333123580032997687 179.99295812360148422") set_tests_properties (GeodSolve6 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 20003898.214") @@ -135,39 +144,42 @@ set_tests_properties (GeodSolve8 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 20003926.881") # Check fix for volatile x bug found 2011-06-25 (gcc 4.4.4 x86 -O3) -add_test (GeodSolve9 GeodSolve -i --input-string +add_test (NAME GeodSolve9 COMMAND GeodSolve -i --input-string "56.320923501171 0 -56.320923501171 179.664747671772880215") set_tests_properties (GeodSolve9 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 19993558.287") # Check fix for adjust tol1_ bug found 2011-06-25 (Visual Studio 10 rel + debug) -add_test (GeodSolve10 GeodSolve -i --input-string +add_test (NAME GeodSolve10 COMMAND GeodSolve -i --input-string "52.784459512564 0 -52.784459512563990912 179.634407464943777557") set_tests_properties (GeodSolve10 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 19991596.095") # Check fix for bet2 = -bet1 bug found 2011-06-25 (Visual Studio 10 rel + debug) -add_test (GeodSolve11 GeodSolve -i --input-string +add_test (NAME GeodSolve11 COMMAND GeodSolve -i --input-string "48.522876735459 0 -48.52287673545898293 179.599720456223079643") set_tests_properties (GeodSolve11 PROPERTIES PASS_REGULAR_EXPRESSION ".* .* 19989144.774") # Check fix for inverse geodesics on extreme prolate/oblate ellipsoids # Reported 2012-08-29 Stefan Guenther ; fixed 2012-10-07 -add_test (GeodSolve12 +add_test (NAME GeodSolve12 COMMAND GeodSolve -i -e 89.8 -1.83 -p 0 --input-string "0 0 -10 160") -add_test (GeodSolve13 +add_test (NAME GeodSolve13 COMMAND GeodSolve -i -e 89.8 -1.83 -p 0 --input-string "0 0 -10 160" -E) set_tests_properties (GeodSolve12 GeodSolve13 PROPERTIES PASS_REGULAR_EXPRESSION "120\\.27.* 105\\.15.* 267") # Check fix for pole-encircling bug found 2011-03-16 -add_test (Planimeter0 Planimeter --input-string "89 0;89 90;89 180;89 270") -add_test (Planimeter1 +add_test (NAME Planimeter0 + COMMAND Planimeter --input-string "89 0;89 90;89 180;89 270") +add_test (NAME Planimeter1 COMMAND Planimeter -r --input-string "-89 0;-89 90;-89 180;-89 270") -add_test (Planimeter2 Planimeter --input-string "0 -1;-1 0;0 1;1 0") -add_test (Planimeter3 Planimeter --input-string "90 0; 0 0; 0 90") -add_test (Planimeter4 Planimeter -l --input-string "90 0; 0 0; 0 90") +add_test (NAME Planimeter2 + COMMAND Planimeter --input-string "0 -1;-1 0;0 1;1 0") +add_test (NAME Planimeter3 COMMAND Planimeter --input-string "90 0; 0 0; 0 90") +add_test (NAME Planimeter4 + COMMAND Planimeter -l --input-string "90 0; 0 0; 0 90") set_tests_properties (Planimeter0 PROPERTIES PASS_REGULAR_EXPRESSION "4 631819\\.8745[0-9]+ 2495230567[78]\\.[0-9]+") @@ -182,56 +194,65 @@ set_tests_properties (Planimeter3 set_tests_properties (Planimeter4 PROPERTIES PASS_REGULAR_EXPRESSION "3 20020719\\.[0-9]+") # Check fix for Planimeter pole crossing bug found 2011-06-24 -add_test (Planimeter5 Planimeter --input-string "89,0.1;89,90.1;89,-179.9") +add_test (NAME Planimeter5 + COMMAND Planimeter --input-string "89,0.1;89,90.1;89,-179.9") set_tests_properties (Planimeter5 PROPERTIES PASS_REGULAR_EXPRESSION "3 539297\\.[0-9]+ 1247615283[89]\\.[0-9]+") # Check fix for Planimeter lon12 rounding bug found 2012-12-03 -add_test (Planimeter6 Planimeter --input-string "9 -0.00000000000001;9 180;9 0") -add_test (Planimeter7 Planimeter --input-string "9 0.00000000000001;9 0;9 180") -add_test (Planimeter8 Planimeter --input-string "9 0.00000000000001;9 180;9 0") -add_test (Planimeter9 Planimeter --input-string "9 -0.00000000000001;9 0;9 180") +add_test (NAME Planimeter6 + COMMAND Planimeter --input-string "9 -0.00000000000001;9 180;9 0") +add_test (NAME Planimeter7 + COMMAND Planimeter --input-string "9 0.00000000000001;9 0;9 180") +add_test (NAME Planimeter8 + COMMAND Planimeter --input-string "9 0.00000000000001;9 180;9 0") +add_test (NAME Planimeter9 + COMMAND Planimeter --input-string "9 -0.00000000000001;9 0;9 180") set_tests_properties (Planimeter6 Planimeter7 Planimeter8 Planimeter9 PROPERTIES PASS_REGULAR_EXPRESSION "3 36026861\\.[0-9]+ -?0.0[0-9]+") # Check fix for AlbersEqualArea::Reverse bug found 2011-05-01 -add_test (ConicProj0 +add_test (NAME ConicProj0 COMMAND ConicProj -a 40d58 39d56 -l 77d45W -r --input-string "220e3 -52e3") set_tests_properties (ConicProj0 PROPERTIES PASS_REGULAR_EXPRESSION "39\\.95[0-9]+ -75\\.17[0-9]+ 1\\.67[0-9]+ 0\\.99[0-9]+") # Check fix for AlbersEqualArea prolate bug found 2012-05-15 -add_test (ConicProj1 +add_test (NAME ConicProj1 COMMAND ConicProj -a 0 0 -e 6.4e6 -0.5 -r --input-string "0 8605508") set_tests_properties (ConicProj1 PROPERTIES PASS_REGULAR_EXPRESSION "^85\\.00") # Check fix for LambertConformalConic::Forward bug found 2012-07-14 -add_test (ConicProj2 ConicProj -c -30 -30 --input-string "-30 0") +add_test (NAME ConicProj2 COMMAND ConicProj -c -30 -30 --input-string "-30 0") set_tests_properties (ConicProj2 PROPERTIES PASS_REGULAR_EXPRESSION "^-?0\\.0+ -?0\\.0+ -?0\\.0+ 1\\.0+") # Check fixes for LambertConformalConic::Reverse overflow bugs found 2012-07-14 -add_test (ConicProj3 ConicProj -r -c 0 0 --input-string "1113195 -1e10") +add_test (NAME ConicProj3 + COMMAND ConicProj -r -c 0 0 --input-string "1113195 -1e10") set_tests_properties (ConicProj3 PROPERTIES PASS_REGULAR_EXPRESSION "^-90\\.0+ 10\\.00[0-9]+ ") -add_test (ConicProj4 ConicProj -r -c 0 0 --input-string "1113195 inf") +add_test (NAME ConicProj4 + COMMAND ConicProj -r -c 0 0 --input-string "1113195 inf") set_tests_properties (ConicProj4 PROPERTIES PASS_REGULAR_EXPRESSION "^90\\.0+ 10\\.00[0-9]+ ") -add_test (ConicProj5 ConicProj -r -c 45 45 --input-string "0 -1e100") +add_test (NAME ConicProj5 + COMMAND ConicProj -r -c 45 45 --input-string "0 -1e100") set_tests_properties (ConicProj5 PROPERTIES PASS_REGULAR_EXPRESSION "^-90\\.0+ -?0\\.00[0-9]+ ") -add_test (ConicProj6 ConicProj -r -c 45 45 --input-string "0 -inf") +add_test (NAME ConicProj6 COMMAND ConicProj -r -c 45 45 --input-string "0 -inf") set_tests_properties (ConicProj6 PROPERTIES PASS_REGULAR_EXPRESSION "^-90\\.0+ -?0\\.00[0-9]+ ") -add_test (ConicProj7 ConicProj -r -c 90 90 --input-string "0 -1e150") +add_test (NAME ConicProj7 + COMMAND ConicProj -r -c 90 90 --input-string "0 -1e150") set_tests_properties (ConicProj7 PROPERTIES PASS_REGULAR_EXPRESSION "^-90\\.0+ -?0\\.00[0-9]+ ") -add_test (ConicProj8 ConicProj -r -c 90 90 --input-string "0 -inf") +add_test (NAME ConicProj8 COMMAND ConicProj -r -c 90 90 --input-string "0 -inf") set_tests_properties (ConicProj8 PROPERTIES PASS_REGULAR_EXPRESSION "^-90\\.0+ -?0\\.00[0-9]+ ") -add_test (CartConvert0 +add_test (NAME CartConvert0 COMMAND CartConvert -e 6.4e6 1/100 -r --input-string "10e3 0 1e3") -add_test (CartConvert1 +add_test (NAME CartConvert1 COMMAND CartConvert -e 6.4e6 -1/100 -r --input-string "1e3 0 10e3") set_tests_properties (CartConvert0 PROPERTIES PASS_REGULAR_EXPRESSION @@ -242,60 +263,60 @@ set_tests_properties (CartConvert1 # Test fix to bad meridian convergence at pole with # TransverseMercatorExact found 2013-06-26 -add_test (TransverseMercatorProj0 +add_test (NAME TransverseMercatorProj0 COMMAND TransverseMercatorProj -k 1 --input-string "90 75") set_tests_properties (TransverseMercatorProj0 PROPERTIES PASS_REGULAR_EXPRESSION "^0\\.0+ 10001965\\.7293[0-9]+ 75\\.0+ 1\\.0+") # Test fix to bad scale at pole with TransverseMercatorExact # found 2013-06-30 (quarter meridian = 10001965.7293127228128889202m) -add_test (TransverseMercatorProj1 +add_test (NAME TransverseMercatorProj1 COMMAND TransverseMercatorProj -k 1 -r --input-string "0 10001965.7293127228") set_tests_properties (TransverseMercatorProj1 PROPERTIES PASS_REGULAR_EXPRESSION "90\\.0+ 0\\.0+ 0\\.0+ (1\\.0+|0\\.9999+)") if (EXISTS ${GEOGRAPHICLIB_DATA}/geoids/egm96-5.pgm) # Check fix for single-cell cache bug found 2010-11-23 - add_test (GeoidEval0 GeoidEval -n egm96-5 --input-string "0d1 0d1;0d4 0d4") + add_test (NAME GeoidEval0 + COMMAND GeoidEval -n egm96-5 --input-string "0d1 0d1;0d4 0d4") set_tests_properties (GeoidEval0 - PROPERTIES PASS_REGULAR_EXPRESSION "^17\\.1[56].. -17\\.1[45]..") + PROPERTIES PASS_REGULAR_EXPRESSION "^17\\.1[56]..\n17\\.1[45]..") endif () if (EXISTS ${GEOGRAPHICLIB_DATA}/magnetic/wmm2010.wmm) # Test case from WMM2010_Report.pdf, Sec 1.5, pp 14-15: # t = 2012.5, lat = -80, lon = 240, h = 100e3 - add_test (MagneticField0 + add_test (NAME MagneticField0 COMMAND MagneticField -n wmm2010 -p 10 -r --input-string "2012.5 -80 240 100e3") - add_test (MagneticField1 + add_test (NAME MagneticField1 COMMAND MagneticField -n wmm2010 -p 10 -r -t 2012.5 --input-string "-80 240 100e3") - add_test (MagneticField2 + add_test (NAME MagneticField2 COMMAND MagneticField -n wmm2010 -p 10 -r -c 2012.5 -80 100e3 --input-string "240") set_tests_properties (MagneticField0 PROPERTIES PASS_REGULAR_EXPRESSION - " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .* -.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") + " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .*\n.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") set_tests_properties (MagneticField1 PROPERTIES PASS_REGULAR_EXPRESSION - " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .* -.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") + " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .*\n.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") set_tests_properties (MagneticField2 PROPERTIES PASS_REGULAR_EXPRESSION - " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .* -.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") + " 5535\\.5249148687 14765\\.3703243050 -50625\\.9305478794 .*\n.* 20\\.4904268023 1\\.0272592716 83\\.5313962281 ") endif () if (EXISTS ${GEOGRAPHICLIB_DATA}/gravity/egm2008.egm) # Verify no overflow at poles with high degree model - add_test (Gravity0 Gravity -n egm2008 -p 6 --input-string "90 110 0") + add_test (NAME Gravity0 + COMMAND Gravity -n egm2008 -p 6 --input-string "90 110 0") set_tests_properties (Gravity0 PROPERTIES PASS_REGULAR_EXPRESSION "-0\\.000146 0\\.000078 -9\\.832294") # Check fix for invR bug in GravityCircle found by Mathieu Peyrega on # 2013-04-09 - add_test (Gravity1 Gravity -n egm2008 -A -c -18 4000 --input-string "-86") + add_test (NAME Gravity1 + COMMAND Gravity -n egm2008 -A -c -18 4000 --input-string "-86") set_tests_properties (Gravity1 PROPERTIES PASS_REGULAR_EXPRESSION "-7\\.438 1\\.305 -1\\.563") - add_test (Gravity2 Gravity -n egm2008 -D -c -18 4000 --input-string "-86") + add_test (NAME Gravity2 + COMMAND Gravity -n egm2008 -D -c -18 4000 --input-string "-86") set_tests_properties (Gravity2 PROPERTIES PASS_REGULAR_EXPRESSION "7\\.404 -6\\.168 7\\.616") endif () From 219b2b4db74d1079e07880f403b772b6c1c7f2e3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 3 May 2014 15:50:21 -0400 Subject: [PATCH 024/101] Updated to 3.0 --- matlab/gtsam_tests/testJacobianFactor.m | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/matlab/gtsam_tests/testJacobianFactor.m b/matlab/gtsam_tests/testJacobianFactor.m index 3bc8f4edd..bba6ca5ac 100644 --- a/matlab/gtsam_tests/testJacobianFactor.m +++ b/matlab/gtsam_tests/testJacobianFactor.m @@ -36,7 +36,9 @@ model4 = noiseModel.Diagonal.Sigmas(sigmas); combined = JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4); % eliminate the first variable (x2) in the combined factor, destructive ! -actualCG = combined.eliminateFirst(); +ord=Ordering; +ord.push_back(x2); +actualCG = combined.eliminate(ord); % create expected Conditional Gaussian R11 = [ @@ -52,7 +54,7 @@ S13 = [ +0.00,-8.94427 ]; d=[2.23607;-1.56525]; -expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13,[1;1]); +expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13); % check if the result matches CHECK('actualCG.equals(expectedCG,1e-5)',actualCG.equals(expectedCG,1e-4)); From f27cb3f317e7ff57e915ffca0fc2cea7d5e10fc4 Mon Sep 17 00:00:00 2001 From: jing Date: Sat, 3 May 2014 17:04:07 -0400 Subject: [PATCH 025/101] few style bugs fixed --- gtsam/geometry/Cal3Unified.cpp | 5 --- gtsam/geometry/Cal3Unified.h | 33 +++---------------- ...{testCal3Unify.cpp => testCal3Unified.cpp} | 0 3 files changed, 4 insertions(+), 34 deletions(-) rename gtsam/geometry/tests/{testCal3Unify.cpp => testCal3Unified.cpp} (100%) diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index d2a2be287..39ad221cb 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -28,11 +28,6 @@ namespace gtsam { Cal3Unified::Cal3Unified(const Vector &v): Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} -/* ************************************************************************* */ -Matrix Cal3Unified::K() const { - return Base::K(); -} - /* ************************************************************************* */ Vector Cal3Unified::vector() const { return (Vector(10) << Base::vector(), xi_); diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index d8c0251f0..6961757bd 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -32,7 +32,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3Unified : protected Cal3DS2 { +class GTSAM_EXPORT Cal3Unified : public Cal3DS2 { typedef Cal3Unified This; typedef Cal3DS2 Base; @@ -49,8 +49,8 @@ private: // pi = K*pn public: - Matrix K() const ; - Eigen::Vector4d k() const { return Base::k(); } + //Matrix K() const ; + //Eigen::Vector4d k() const { return Base::k(); } Vector vector() const ; /// @name Standard Constructors @@ -86,21 +86,6 @@ public: /// mirror parameter inline double xi() const { return xi_;} - /// focal length x - inline double fx() const { return fx_;} - - /// focal length y - inline double fy() const { return fy_;} - - /// skew - inline double skew() const { return s_;} - - /// image center in x - inline double px() const { return u0_;} - - /// image center in y - inline double py() const { return v0_;} - /** * convert intrinsic coordinates xy to image coordinates uv * @param p point in intrinsic coordinates @@ -149,20 +134,10 @@ private: void serialize(Archive & ar, const unsigned int version) { ar & boost::serialization::make_nvp("Cal3Unified", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(fx_); - ar & BOOST_SERIALIZATION_NVP(fy_); - ar & BOOST_SERIALIZATION_NVP(s_); - ar & BOOST_SERIALIZATION_NVP(u0_); - ar & BOOST_SERIALIZATION_NVP(v0_); - ar & BOOST_SERIALIZATION_NVP(k1_); - ar & BOOST_SERIALIZATION_NVP(k2_); - ar & BOOST_SERIALIZATION_NVP(k3_); - ar & BOOST_SERIALIZATION_NVP(k4_); + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(xi_); } - /// @} }; diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unified.cpp similarity index 100% rename from gtsam/geometry/tests/testCal3Unify.cpp rename to gtsam/geometry/tests/testCal3Unified.cpp From 2daeae243885da79603fc03077a88b1e4c58520f Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Sat, 3 May 2014 17:26:07 -0400 Subject: [PATCH 026/101] GeographicLib now works on Windows --- gtsam/navigation/GPSFactor.h | 2 +- gtsam/navigation/tests/CMakeLists.txt | 14 ++++++++++++-- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index f4ef6c49a..6902f81f1 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -31,7 +31,7 @@ namespace gtsam { * See Farrell08book or e.g. http://www.dirsig.org/docs/new/coordinates.html * @addtogroup Navigation */ -class GPSFactor: public NoiseModelFactor1 { +class GTSAM_EXPORT GPSFactor: public NoiseModelFactor1 { private: diff --git a/gtsam/navigation/tests/CMakeLists.txt b/gtsam/navigation/tests/CMakeLists.txt index 2f3c0883a..b03b8167c 100644 --- a/gtsam/navigation/tests/CMakeLists.txt +++ b/gtsam/navigation/tests/CMakeLists.txt @@ -7,12 +7,22 @@ set(tests_excluded "") if(GTSAM_INSTALL_GEOGRAPHICLIB) # If we're installing GeographicLib, use the one we're compiling include_directories(${PROJECT_SOURCE_DIR}/gtsam/3rdparty/GeographicLib/include) - list(APPEND test_link_libraries GeographicLib) + if(MSVC) + list(APPEND test_link_libraries GeographicLib_STATIC) + else() + list(APPEND test_link_libraries GeographicLib) + endif() + else() if(GEOGRAPHICLIB_FOUND) # If we're not installing, but it's already installed, use the installed one include_directories(${GeographicLib_INCLUDE_DIRS}) - list(APPEND test_link_libraries ${GeographicLib_LIBRARIES}) + list(APPEND test_link_libraries ) + #if(MSVC) + # list(APPEND test_link_libraries GeographicLib_STATIC) + #else() + list(APPEND test_link_libraries ${GeographicLib_LIBRARIES}) + #endif() else() # We don't have GeographicLib set(tests_excluded testGeographicLib.cpp testGPSFactor.cpp testMagFactor.cpp) From 0704e8219023bb4bc28f13a095951de8e741519c Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Sat, 3 May 2014 19:17:52 -0400 Subject: [PATCH 027/101] Fix GCC 4.6+ specific warning --- gtsam/3rdparty/metis-5.1.0/CMakeLists.txt | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt b/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt index 827e50b34..8497973b2 100644 --- a/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt +++ b/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt @@ -8,7 +8,12 @@ if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") endif() add_definitions(-Wno-unknown-pragmas) -add_definitions(-Wunused-but-set-variable) + +if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.6 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 4.6) + add_definitions(-Wno-unused-but-set-variable) + endif() +endif() set(GKLIB_PATH ${PROJECT_SOURCE_DIR}/GKlib CACHE PATH "path to GKlib") set(SHARED FALSE CACHE BOOL "build a shared library") From 05c1e572b6b8c2d0de9326626a1debaf7f475cff Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 5 May 2014 10:14:36 -0400 Subject: [PATCH 028/101] Moved TSAMFactors.h from tsam to gtsam --- gtsam_unstable/slam/TSAMFactors.h | 167 ++++++++++++++++++ gtsam_unstable/slam/tests/testTSAMFactors.cpp | 149 ++++++++++++++++ 2 files changed, 316 insertions(+) create mode 100644 gtsam_unstable/slam/TSAMFactors.h create mode 100644 gtsam_unstable/slam/tests/testTSAMFactors.cpp diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h new file mode 100644 index 000000000..d4c5f8172 --- /dev/null +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -0,0 +1,167 @@ +/* ---------------------------------------------------------------------------- + + * 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 TSAMFactors.h + * @brief TSAM 1 Factors, simpler than the hierarchical TSAM 2 + * @author Frank Dellaert + * @date May 2014 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * DeltaFactor: relative 2D measurement between Pose2 and Point2 + */ +class DeltaFactor: public NoiseModelFactor2 { + +public: + typedef DeltaFactor This; + typedef NoiseModelFactor2 Base; + typedef boost::shared_ptr shared_ptr; + +private: + Point2 measured_; ///< the measurement + +public: + + /// Constructor + DeltaFactor(Key i, Key j, const Point2& measured, + const SharedNoiseModel& model) : + Base(model, i, j), measured_(measured) { + } + + /// Evaluate measurement error h(x)-z + Vector evaluateError(const Pose2& pose, const Point2& point, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + Point2 d = pose.transform_to(point, H1, H2); + Point2 e = measured_.between(d); + return e.vector(); + } +}; + +/** + * DeltaFactorBase: relative 2D measurement between Pose2 and Point2, with Basenodes + */ +class DeltaFactorBase: public NoiseModelFactor4 { + +public: + typedef DeltaFactorBase This; + typedef NoiseModelFactor4 Base; + typedef boost::shared_ptr shared_ptr; + +private: + Point2 measured_; ///< the measurement + +public: + + /// Constructor + DeltaFactorBase(Key b1, Key i, Key b2, Key j, const Point2& measured, + const SharedNoiseModel& model) : + Base(model, b1, i, b2, j), measured_(measured) { + } + + /// Evaluate measurement error h(x)-z + Vector evaluateError(const Pose2& base1, const Pose2& pose, + const Pose2& base2, const Point2& point, // + boost::optional H1 = boost::none, // + boost::optional H2 = boost::none, // + boost::optional H3 = boost::none, // + boost::optional H4 = boost::none) const { + if (H1 || H2 || H3 || H4) { + // TODO use fixed-size matrices + Matrix D_pose_g_base1, D_pose_g_pose; + Pose2 pose_g = base1.compose(pose, D_pose_g_base1, D_pose_g_pose); + Matrix D_point_g_base2, D_point_g_point; + Point2 point_g = base2.transform_from(point, D_point_g_base2, + D_point_g_point); + Matrix D_e_pose_g, D_e_point_g; + Point2 d = pose_g.transform_to(point_g, D_e_pose_g, D_e_point_g); + if (H1) + *H1 = D_e_pose_g * D_pose_g_base1; + if (H2) + *H2 = D_e_pose_g * D_pose_g_pose; + if (H3) + *H3 = D_e_point_g * D_point_g_base2; + if (H4) + *H4 = D_e_point_g * D_point_g_point; + return measured_.localCoordinates(d); + } else { + Pose2 pose_g = base1.compose(pose); + Point2 point_g = base2.transform_from(point); + Point2 d = pose_g.transform_to(point_g); + return measured_.localCoordinates(d); + } + } +}; + +/** + * OdometryFactorBase: Pose2 odometry, with Basenodes + */ +class OdometryFactorBase: public NoiseModelFactor4 { + +public: + typedef OdometryFactorBase This; + typedef NoiseModelFactor4 Base; + typedef boost::shared_ptr shared_ptr; + +private: + Pose2 measured_; ///< the measurement + +public: + + /// Constructor + OdometryFactorBase(Key b1, Key i, Key b2, Key j, const Pose2& measured, + const SharedNoiseModel& model) : + Base(model, b1, i, b2, j), measured_(measured) { + } + + /// Evaluate measurement error h(x)-z + Vector evaluateError(const Pose2& base1, const Pose2& pose1, + const Pose2& base2, const Pose2& pose2, // + boost::optional H1 = boost::none, // + boost::optional H2 = boost::none, // + boost::optional H3 = boost::none, // + boost::optional H4 = boost::none) const { + if (H1 || H2 || H3 || H4) { + // TODO use fixed-size matrices + Matrix D_pose1_g_base1, D_pose1_g_pose1; + Pose2 pose1_g = base1.compose(pose1, D_pose1_g_base1, D_pose1_g_pose1); + Matrix D_pose2_g_base2, D_pose2_g_pose2; + Pose2 pose2_g = base2.compose(pose2, D_pose2_g_base2, D_pose2_g_pose2); + Matrix D_e_pose1_g, D_e_pose2_g; + Pose2 d = pose1_g.between(pose2_g, D_e_pose1_g, D_e_pose2_g); + if (H1) + *H1 = D_e_pose1_g * D_pose1_g_base1; + if (H2) + *H2 = D_e_pose1_g * D_pose1_g_pose1; + if (H3) + *H3 = D_e_pose2_g * D_pose2_g_base2; + if (H4) + *H4 = D_e_pose2_g * D_pose2_g_pose2; + return measured_.localCoordinates(d); + } else { + Pose2 pose1_g = base1.compose(pose1); + Pose2 pose2_g = base2.compose(pose2); + Pose2 d = pose1_g.between(pose2_g); + return measured_.localCoordinates(d); + } + } +}; + +} + diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp new file mode 100644 index 000000000..44dca9b8e --- /dev/null +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -0,0 +1,149 @@ +/* ---------------------------------------------------------------------------- + + * 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 testTSAMFactors.cpp + * @brief Unit tests for TSAM 1 Factors + * @author Frank Dellaert + * @date May 2014 + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +Key i(1), j(2); // Key for pose and point + +//************************************************************************* +TEST( DeltaFactor, all ) { + // Create a factor + Point2 measurement(1, 1); + static SharedNoiseModel model(noiseModel::Unit::Create(2)); + DeltaFactor factor(i, j, measurement, model); + + // Set the linearization point + Pose2 pose(1, 2, 0); + Point2 point(4, 11); + Vector2 expected(4 - 1 - 1, 11 - 2 - 1); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual; + Vector actual = factor.evaluateError(pose, point, H1Actual, H2Actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected; + H1Expected = numericalDerivative11( + boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none, + boost::none), pose); + H2Expected = numericalDerivative11( + boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none, + boost::none), point); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); +} + +//************************************************************************* +TEST( DeltaFactorBase, all ) { + // Create a factor + Key b1(10), b2(20); + Point2 measurement(1, 1); + static SharedNoiseModel model(noiseModel::Unit::Create(2)); + DeltaFactorBase factor(b1, i, b2, j, measurement, model); + + // Set the linearization point + Pose2 base1, base2(1, 0, 0); + Pose2 pose(1, 2, 0); + Point2 point(4, 11); + Vector2 expected(4 + 1 - 1 - 1, 11 - 2 - 1); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual, H4Actual; + Vector actual = factor.evaluateError(base1, pose, base2, point, H1Actual, + H2Actual, H3Actual, H4Actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected, H3Expected, H4Expected; + H1Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2, + point, boost::none, boost::none, boost::none, boost::none), base1); + H2Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2, + point, boost::none, boost::none, boost::none, boost::none), pose); + H3Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1, + point, boost::none, boost::none, boost::none, boost::none), base2); + H4Expected = numericalDerivative11( + boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, + _1, boost::none, boost::none, boost::none, boost::none), point); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); + EXPECT(assert_equal(H3Expected, H3Actual, 1e-9)); + EXPECT(assert_equal(H4Expected, H4Actual, 1e-9)); +} + +//************************************************************************* +TEST( OdometryFactorBase, all ) { + // Create a factor + Key b1(10), b2(20); + Pose2 measurement(1, 1, 0); + static SharedNoiseModel model(noiseModel::Unit::Create(2)); + OdometryFactorBase factor(b1, i, b2, j, measurement, model); + + // Set the linearization pose2 + Pose2 base1, base2(1, 0, 0); + Pose2 pose1(1, 2, 0), pose2(4, 11, 0); + Vector3 expected(4 + 1 - 1 - 1, 11 - 2 - 1, 0); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual, H4Actual; + Vector actual = factor.evaluateError(base1, pose1, base2, pose2, H1Actual, + H2Actual, H3Actual, H4Actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected, H3Expected, H4Expected; + H1Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2, + pose2, boost::none, boost::none, boost::none, boost::none), base1); + H2Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2, + pose2, boost::none, boost::none, boost::none, boost::none), pose1); + H3Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1, + pose2, boost::none, boost::none, boost::none, boost::none), base2); + H4Expected = numericalDerivative11( + boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, + base2, _1, boost::none, boost::none, boost::none, boost::none), + pose2); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); + EXPECT(assert_equal(H3Expected, H3Actual, 1e-9)); + EXPECT(assert_equal(H4Expected, H4Actual, 1e-9)); +} + +//************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//************************************************************************* + From 0a2385711b2a2dc9df85eb29a691ef7ade1d2990 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 5 May 2014 10:14:56 -0400 Subject: [PATCH 029/101] MATLAB wrapping of TSAMFactors --- .cproject | 438 +++++++++--------- gtsam_unstable/gtsam_unstable.h | 33 +- matlab/unstable_examples/TSAMFactorsExample.m | 44 ++ 3 files changed, 305 insertions(+), 210 deletions(-) create mode 100644 matlab/unstable_examples/TSAMFactorsExample.m diff --git a/.cproject b/.cproject index 97e36d81f..31d4d9143 100644 --- a/.cproject +++ b/.cproject @@ -1,17 +1,19 @@ - + + + + + - - @@ -60,13 +62,13 @@ + + - - @@ -116,13 +118,13 @@ + + - - @@ -540,14 +542,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -574,6 +568,7 @@ make + tests/testBayesTree.run true false @@ -581,6 +576,7 @@ make + testBinaryBayesNet.run true false @@ -628,6 +624,7 @@ make + testSymbolicBayesNet.run true false @@ -635,6 +632,7 @@ make + tests/testSymbolicFactor.run true false @@ -642,6 +640,7 @@ make + testSymbolicFactorGraph.run true false @@ -657,11 +656,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -758,22 +766,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -790,6 +782,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -814,42 +822,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j6 -j8 - testWhiteNoiseFactor.run + -j2 + clean true true true @@ -918,26 +910,42 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j6 -j8 + testWhiteNoiseFactor.run true true true @@ -1086,6 +1094,14 @@ true true + + make + -j5 + testTSAMFactors.run + true + true + true + make -j5 @@ -1328,6 +1344,7 @@ make + testGraph.run true false @@ -1335,6 +1352,7 @@ make + testJunctionTree.run true false @@ -1342,6 +1360,7 @@ make + testSymbolicBayesNetB.run true false @@ -1509,6 +1528,7 @@ make + testErrors.run true false @@ -1554,22 +1574,6 @@ true true - - make - -j5 - testParticleFactor.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1650,6 +1654,22 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j5 + testParticleFactor.run + true + true + true + make -j2 @@ -1930,22 +1950,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j2 @@ -2028,7 +2032,6 @@ make - testSimulated2DOriented.run true false @@ -2068,7 +2071,6 @@ make - testSimulated2D.run true false @@ -2076,7 +2078,6 @@ make - testSimulated3D.run true false @@ -2090,6 +2091,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j5 @@ -2364,7 +2381,6 @@ make - tests/testGaussianISAM2 true false @@ -2386,102 +2402,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2683,6 +2603,7 @@ cpack + -G DEB true false @@ -2690,6 +2611,7 @@ cpack + -G RPM true false @@ -2697,6 +2619,7 @@ cpack + -G TGZ true false @@ -2704,6 +2627,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2877,34 +2801,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2948,6 +2936,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 80ee41b22..1aa840825 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -15,8 +15,11 @@ virtual class gtsam::Pose3; virtual class gtsam::noiseModel::Base; virtual class gtsam::noiseModel::Gaussian; virtual class gtsam::imuBias::ConstantBias; -virtual class gtsam::NoiseModelFactor; virtual class gtsam::NonlinearFactor; +virtual class gtsam::NoiseModelFactor; +virtual class gtsam::NoiseModelFactor2; +virtual class gtsam::NoiseModelFactor3; +virtual class gtsam::NoiseModelFactor4; virtual class gtsam::GaussianFactor; virtual class gtsam::HessianFactor; virtual class gtsam::JacobianFactor; @@ -709,4 +712,32 @@ class AHRS { void print(string s) const; }; +// Tectonic SAM Factors + +#include +#include + +//typedef gtsam::NoiseModelFactor2 NLPosePose; +virtual class DeltaFactor : gtsam::NoiseModelFactor { + DeltaFactor(size_t i, size_t j, const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel); + void print(string s) const; +}; + +//typedef gtsam::NoiseModelFactor4 NLPosePosePosePoint; +virtual class DeltaFactorBase : gtsam::NoiseModelFactor { + DeltaFactorBase(size_t b1, size_t i, size_t b2, size_t j, + const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel); + void print(string s) const; +}; + +//typedef gtsam::NoiseModelFactor4 NLPosePosePosePose; +virtual class OdometryFactorBase : gtsam::NoiseModelFactor { + OdometryFactorBase(size_t b1, size_t i, size_t b2, size_t j, + const gtsam::Pose2& measured, const gtsam::noiseModel::Base* noiseModel); + void print(string s) const; +}; + } //\namespace gtsam diff --git a/matlab/unstable_examples/TSAMFactorsExample.m b/matlab/unstable_examples/TSAMFactorsExample.m new file mode 100644 index 000000000..5ee9f3909 --- /dev/null +++ b/matlab/unstable_examples/TSAMFactorsExample.m @@ -0,0 +1,44 @@ +% TSAMFactorsExample +% Frank Dellaert, May 2014 + +import gtsam.*; + +% noise models +noisePrior = noiseModel.Diagonal.Sigmas([0; 0; 0]); +noiseDelta = noiseModel.Isotropic.Sigma(2, 0.1); +noiseOdom = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.05]); + +% Example is 1, landmark, 2 poses 10 and 20, 2 base nodes 100 and 200 +% + + +% - b - p - l - p - b +% +---+-------+---+ +% Create a graph +graph = NonlinearFactorGraph +origin = Pose2; +graph.add(gtsam.PriorFactorPose2(100, origin, noisePrior)) +graph.add(gtsam.PriorFactorPose2(10, origin, noisePrior)) +graph.add(gtsam.PriorFactorPose2(20, origin, noisePrior)) +graph.add(DeltaFactor(10, 1, Point2(1,0), noiseDelta)) +graph.add(DeltaFactor(20, 1, Point2(-1,0), noiseDelta)) +graph.add(OdometryFactorBase(100,10,200,20, Pose2(2,0,0), noiseOdom)) +graph + +% Initial values +initial = Values; +initial.insert(100,origin); +initial.insert(10,origin); +initial.insert(1,Point2(2,0)); +initial.insert(20,origin); +initial.insert(200,origin); + +graph.error(initial) +graph.at(0).error(initial) +graph.at(1).error(initial) +graph.at(2).error(initial) +graph.at(3).error(initial) + +% optimize +params = LevenbergMarquardtParams; +params.setVerbosity('ERROR'); +optimizer = LevenbergMarquardtOptimizer(graph, initial, params); +result = optimizer.optimize() \ No newline at end of file From f5e0a7f2b19f03cbabf97f7dcbcc78089434df85 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 5 May 2014 10:15:23 -0400 Subject: [PATCH 030/101] ignore --- matlab/unstable_examples/.gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 matlab/unstable_examples/.gitignore diff --git a/matlab/unstable_examples/.gitignore b/matlab/unstable_examples/.gitignore new file mode 100644 index 000000000..6d725d9bc --- /dev/null +++ b/matlab/unstable_examples/.gitignore @@ -0,0 +1 @@ +*.m~ From 9409357fe76b1ef4b6483fda76c91041eec4aede Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 5 May 2014 11:26:16 -0400 Subject: [PATCH 031/101] Works, but is really a test, not an example --- matlab/unstable_examples/TSAMFactorsExample.m | 54 ++++++++++++------- 1 file changed, 36 insertions(+), 18 deletions(-) diff --git a/matlab/unstable_examples/TSAMFactorsExample.m b/matlab/unstable_examples/TSAMFactorsExample.m index 5ee9f3909..abdfc5922 100644 --- a/matlab/unstable_examples/TSAMFactorsExample.m +++ b/matlab/unstable_examples/TSAMFactorsExample.m @@ -4,41 +4,59 @@ import gtsam.*; % noise models -noisePrior = noiseModel.Diagonal.Sigmas([0; 0; 0]); +noisePrior = noiseModel.Diagonal.Sigmas([100; 100; 100]); noiseDelta = noiseModel.Isotropic.Sigma(2, 0.1); noiseOdom = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.05]); -% Example is 1, landmark, 2 poses 10 and 20, 2 base nodes 100 and 200 +% Example is 2 landmarks 1 and 2, 2 poses 10 and 20, 2 base nodes 100 and 200 +% l1 l2 % + + -% - b - p - l - p - b -% +---+-------+---+ +% - b - p - p - b +% +---+---+---+ + +% True values +b1 = Pose2(0,0,0); +b2 = Pose2(2,0,0); +l1 = Point2(0,1); +l2 = Point2(2,1); + % Create a graph -graph = NonlinearFactorGraph +graph = NonlinearFactorGraph; origin = Pose2; -graph.add(gtsam.PriorFactorPose2(100, origin, noisePrior)) graph.add(gtsam.PriorFactorPose2(10, origin, noisePrior)) graph.add(gtsam.PriorFactorPose2(20, origin, noisePrior)) -graph.add(DeltaFactor(10, 1, Point2(1,0), noiseDelta)) -graph.add(DeltaFactor(20, 1, Point2(-1,0), noiseDelta)) +graph.add(gtsam.PriorFactorPose2(100, origin, noisePrior)) +graph.add(DeltaFactor(10, 1, b1.transform_to(l1), noiseDelta)) +graph.add(DeltaFactor(20, 1, b2.transform_to(l2), noiseDelta)) +graph.add(DeltaFactorBase(100,10, 200,2, b1.transform_to(l2), noiseDelta)) +graph.add(DeltaFactorBase(200,20, 100,1, b2.transform_to(l1), noiseDelta)) graph.add(OdometryFactorBase(100,10,200,20, Pose2(2,0,0), noiseOdom)) -graph % Initial values initial = Values; initial.insert(100,origin); initial.insert(10,origin); -initial.insert(1,Point2(2,0)); +initial.insert(1,l1); +initial.insert(2,l2); initial.insert(20,origin); initial.insert(200,origin); -graph.error(initial) -graph.at(0).error(initial) -graph.at(1).error(initial) -graph.at(2).error(initial) -graph.at(3).error(initial) - % optimize params = LevenbergMarquardtParams; -params.setVerbosity('ERROR'); +% params.setVerbosity('ERROR'); optimizer = LevenbergMarquardtOptimizer(graph, initial, params); -result = optimizer.optimize() \ No newline at end of file +result = optimizer.optimize(); + +% Check result +CHECK('error',result.at(100).equals(b1,1e-5)) +CHECK('error',result.at(10).equals(origin,1e-5)) +CHECK('error',result.at(1).equals(Point2(0,1),1e-5)) +CHECK('error',result.at(2).equals(Point2(0,1),1e-5)) +CHECK('error',result.at(20).equals(origin,1e-5)) +CHECK('error',result.at(200).equals(b2,1e-5)) + +% Check error +CHECK('error',abs(graph.error(result))<1e-9) +for i=0:7 + CHECK('error',abs(graph.at(i).error(result))<1e-9) +end From 3b1f947909c4fc141f72606b540a7f105f1f949b Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 5 May 2014 11:29:02 -0400 Subject: [PATCH 032/101] Renamed to test --- .../unstable_examples/{TSAMFactorsExample.m => testTSAMFactors.m} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename matlab/unstable_examples/{TSAMFactorsExample.m => testTSAMFactors.m} (100%) diff --git a/matlab/unstable_examples/TSAMFactorsExample.m b/matlab/unstable_examples/testTSAMFactors.m similarity index 100% rename from matlab/unstable_examples/TSAMFactorsExample.m rename to matlab/unstable_examples/testTSAMFactors.m From cf77189d71f020b450f9629164f66f245c9517c7 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 5 May 2014 14:42:46 -0400 Subject: [PATCH 033/101] Start a DEVELOP document and update INSTALL --- DEVELOP | 19 +++++++++++++++++++ INSTALL | 9 +++++++-- 2 files changed, 26 insertions(+), 2 deletions(-) create mode 100644 DEVELOP diff --git a/DEVELOP b/DEVELOP new file mode 100644 index 000000000..483197bc8 --- /dev/null +++ b/DEVELOP @@ -0,0 +1,19 @@ +Information for developers + +Coding Conventions: + +* Classes are Uppercase, methods and functions lowerMixedCase +* We use a modified K&R Style, with 2-space tabs, inserting spaces for tabs +* Use meaningful variable names, e.g., measurement not msm + + +Windows: + +On Windows it is necessary to explicitly export all functions from the library +which should be externally accessible. To do this, include the macro +GTSAM_EXPORT in your class or function definition. + +For example: +class GTSAM_EXPORT MyClass { ... }; + +GTSAM_EXPORT myFunction(); \ No newline at end of file diff --git a/INSTALL b/INSTALL index 75277e815..c71dcd4f9 100644 --- a/INSTALL +++ b/INSTALL @@ -24,7 +24,7 @@ Optional dependent libraries: may be installed from the Ubuntu repositories, and for other platforms it may be downloaded from https://www.threadingbuildingblocks.org/ -Tested compilers +Tested compilers: - GCC 4.2-4.7 - OSX Clang 2.9-5.0 @@ -35,7 +35,12 @@ Tested systems: - Ubuntu 11.04 - 13.10 - MacOS 10.6 - 10.9 -- Windows 7, 8 +- Windows 7, 8, 8.1 + +Known issues: + +- MSVC 2013 is not yet supported because it cannot build the serialization module + of Boost 1.55 (or earlier). 2) GTSAM makes extensive use of debug assertions, and we highly recommend you work From 2f180d1d02f0eec65199f36c1219223173371568 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 5 May 2014 16:35:22 -0400 Subject: [PATCH 034/101] Remove using namespace std from header files --- gtsam_unstable/partition/FindSeparator-inl.h | 166 +++++++++--------- .../partition/NestedDissection-inl.h | 2 - 2 files changed, 82 insertions(+), 86 deletions(-) diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index 6eb39cfaa..dacbebc76 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -24,8 +24,6 @@ extern "C" { #include "FindSeparator.h" -using namespace std; - namespace gtsam { namespace partition { typedef boost::shared_array sharedInts; @@ -37,7 +35,7 @@ namespace gtsam { namespace partition { * whether node j is in the left part of the graph, the right part, or the * separator, respectively */ - pair separatorMetis(idx_t n, const sharedInts& xadj, + std::pair separatorMetis(idx_t n, const sharedInts& xadj, const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) { // control parameters @@ -72,7 +70,7 @@ namespace gtsam { namespace partition { printf("**********************************************************************\n"); } - return make_pair(sepsize, part_); + return std::make_pair(sepsize, part_); } /* ************************************************************************* */ @@ -112,7 +110,7 @@ namespace gtsam { namespace partition { // *edgecut = graph->mincut; // *sepsize = graph.pwgts[2]; icopy(*nvtxs, graph->where, part); - cout << "Finished bisection:" << *edgecut << endl; + std::cout << "Finished bisection:" << *edgecut << std::endl; FreeGraph(&graph); FreeCtrl(&ctrl); @@ -124,7 +122,7 @@ namespace gtsam { namespace partition { * Part [j] is 0 or 1, depending on * whether node j is in the left part of the graph or the right part respectively */ - pair edgeMetis(idx_t n, const sharedInts& xadj, const sharedInts& adjncy, + std::pair edgeMetis(idx_t n, const sharedInts& xadj, const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) { // control parameters @@ -163,7 +161,7 @@ namespace gtsam { namespace partition { printf("**********************************************************************\n"); } - return make_pair(edgecut, part_); + return std::make_pair(edgecut, part_); } /* ************************************************************************* */ @@ -173,13 +171,13 @@ namespace gtsam { namespace partition { * adjncy always has the size equal to two times of the no. of the edges in the Metis graph */ template - void prepareMetisGraph(const GenericGraph& graph, const vector& keys, WorkSpace& workspace, + void prepareMetisGraph(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) { typedef int Weight; - typedef vector Weights; - typedef vector Neighbors; - typedef pair NeighborsInfo; + typedef std::vector Weights; + typedef std::vector Neighbors; + typedef std::pair NeighborsInfo; // set up dictionary std::vector& dictionary = workspace.dictionary; @@ -188,27 +186,27 @@ namespace gtsam { namespace partition { // prepare for {adjacencyMap}, a pair of neighbor indices and the correponding edge weights int numNodes = keys.size(); int numEdges = 0; - vector adjacencyMap; + std::vector adjacencyMap; adjacencyMap.resize(numNodes); - cout << "Number of nodes: " << adjacencyMap.size() << endl; + std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl; int index1, index2; BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ index1 = dictionary[factor->key1.index]; index2 = dictionary[factor->key2.index]; - cout << "index1: " << index1 << endl; - cout << "index2: " << index2 << endl; + std::cout << "index1: " << index1 << std::endl; + std::cout << "index2: " << index2 << std::endl; // if both nodes are in the current graph, i.e. not a joint factor between frontal and separator if (index1 >= 0 && index2 >= 0) { - pair& adjacencyMap1 = adjacencyMap[index1]; - pair& adjacencyMap2 = adjacencyMap[index2]; + std::pair& adjacencyMap1 = adjacencyMap[index1]; + std::pair& adjacencyMap2 = adjacencyMap[index2]; try{ adjacencyMap1.first.push_back(index2); adjacencyMap1.second.push_back(factor->weight); adjacencyMap2.first.push_back(index1); adjacencyMap2.second.push_back(factor->weight); }catch(std::exception& e){ - cout << e.what() << endl; + std::cout << e.what() << std::endl; } numEdges++; } @@ -237,11 +235,11 @@ namespace gtsam { namespace partition { /* ************************************************************************* */ template boost::optional separatorPartitionByMetis(const GenericGraph& graph, - const vector& keys, WorkSpace& workspace, bool verbose) { + const std::vector& keys, WorkSpace& workspace, bool verbose) { // create a metis graph size_t numKeys = keys.size(); if (verbose) - cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << endl; + std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; sharedInts xadj, adjncy, adjwgt; @@ -260,29 +258,29 @@ namespace gtsam { namespace partition { result.A.reserve(numKeys - sepsize); result.B.reserve(numKeys - sepsize); int* ptr_part = part.get(); - vector::const_iterator itKey = keys.begin(); - vector::const_iterator itKeyLast = keys.end(); + std::vector::const_iterator itKey = keys.begin(); + std::vector::const_iterator itKeyLast = keys.end(); while(itKey != itKeyLast) { switch(*(ptr_part++)) { case 0: result.A.push_back(*(itKey++)); break; case 1: result.B.push_back(*(itKey++)); break; case 2: result.C.push_back(*(itKey++)); break; - default: throw runtime_error("separatorPartitionByMetis: invalid results from Metis ND!"); + default: throw std::runtime_error("separatorPartitionByMetis: invalid results from Metis ND!"); } } if (verbose) { - cout << "total key: " << keys.size() + std::cout << "total key: " << keys.size() << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " - << result.C.size() << "; sepsize from Metis = " << sepsize << endl; + << result.C.size() << "; sepsize from Metis = " << sepsize << std::endl; //throw runtime_error("separatorPartitionByMetis:stop for debug"); } if(result.C.size() != sepsize) { - cout << "total key: " << keys.size() + std::cout << "total key: " << keys.size() << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size() - << "; sepsize from Metis = " << sepsize << endl; - throw runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!"); + << "; sepsize from Metis = " << sepsize << std::endl; + throw std::runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!"); } return boost::make_optional(result); @@ -291,7 +289,7 @@ namespace gtsam { namespace partition { /* *************************************************************************/ template boost::optional edgePartitionByMetis(const GenericGraph& graph, - const vector& keys, WorkSpace& workspace, bool verbose) { + const std::vector& keys, WorkSpace& workspace, bool verbose) { // a small hack for handling the camera1-camera2 case used in the unit tests if (graph.size() == 1 && keys.size() == 2) { @@ -303,7 +301,7 @@ namespace gtsam { namespace partition { // create a metis graph size_t numKeys = keys.size(); - if (verbose) cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << endl; + if (verbose) std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; sharedInts xadj, adjncy, adjwgt; prepareMetisGraph(graph, keys, workspace, &xadj, &adjncy, &adjwgt); @@ -317,35 +315,35 @@ namespace gtsam { namespace partition { result.A.reserve(numKeys); result.B.reserve(numKeys); int* ptr_part = part.get(); - vector::const_iterator itKey = keys.begin(); - vector::const_iterator itKeyLast = keys.end(); + std::vector::const_iterator itKey = keys.begin(); + std::vector::const_iterator itKeyLast = keys.end(); while(itKey != itKeyLast) { if (*ptr_part != 0 && *ptr_part != 1) - cout << *ptr_part << "!!!" << endl; + std::cout << *ptr_part << "!!!" << std::endl; switch(*(ptr_part++)) { case 0: result.A.push_back(*(itKey++)); break; case 1: result.B.push_back(*(itKey++)); break; - default: throw runtime_error("edgePartitionByMetis: invalid results from Metis ND!"); + default: throw std::runtime_error("edgePartitionByMetis: invalid results from Metis ND!"); } } if (verbose) { - cout << "the size of two submaps in the reduced graph: " << result.A.size() - << " " << result.B.size() << endl; + std::cout << "the size of two submaps in the reduced graph: " << result.A.size() + << " " << result.B.size() << std::endl; int edgeCut = 0; BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ int key1 = factor->key1.index; int key2 = factor->key2.index; // print keys and their subgraph assignment - cout << key1; - if (std::find(result.A.begin(), result.A.end(), key1) != result.A.end()) cout <<"A "; - if (std::find(result.B.begin(), result.B.end(), key1) != result.B.end()) cout <<"B "; + std::cout << key1; + if (std::find(result.A.begin(), result.A.end(), key1) != result.A.end()) std::cout <<"A "; + if (std::find(result.B.begin(), result.B.end(), key1) != result.B.end()) std::cout <<"B "; - cout << key2; - if (std::find(result.A.begin(), result.A.end(), key2) != result.A.end()) cout <<"A "; - if (std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) cout <<"B "; - cout << "weight " << factor->weight;; + std::cout << key2; + if (std::find(result.A.begin(), result.A.end(), key2) != result.A.end()) std::cout <<"A "; + if (std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) std::cout <<"B "; + std::cout << "weight " << factor->weight;; // find vertices that were assigned to sets A & B. Their edge will be cut if ((std::find(result.A.begin(), result.A.end(), key1) != result.A.end() && @@ -353,48 +351,48 @@ namespace gtsam { namespace partition { (std::find(result.B.begin(), result.B.end(), key1) != result.B.end() && std::find(result.A.begin(), result.A.end(), key2) != result.A.end())){ edgeCut ++; - cout << " CUT "; + std::cout << " CUT "; } - cout << endl; + std::cout << std::endl; } - cout << "edgeCut: " << edgeCut << endl; + std::cout << "edgeCut: " << edgeCut << std::endl; } return boost::make_optional(result); } /* ************************************************************************* */ - bool isLargerIsland(const vector& island1, const vector& island2) { + bool isLargerIsland(const std::vector& island1, const std::vector& island2) { return island1.size() > island2.size(); } /* ************************************************************************* */ // debug functions - void printIsland(const vector& island) { - cout << "island: "; + void printIsland(const std::vector& island) { + std::cout << "island: "; BOOST_FOREACH(const size_t key, island) - cout << key << " "; - cout << endl; + std::cout << key << " "; + std::cout << std::endl; } - void printIslands(const list >& islands) { - BOOST_FOREACH(const vector& island, islands) + void printIslands(const std::list >& islands) { + BOOST_FOREACH(const std::vector& island, islands) printIsland(island); } - void printNumCamerasLandmarks(const vector& keys, const vector& int2symbol) { + void printNumCamerasLandmarks(const std::vector& keys, const std::vector& int2symbol) { int numCamera = 0, numLandmark = 0; BOOST_FOREACH(const size_t key, keys) if (int2symbol[key].chr() == 'x') numCamera++; else numLandmark++; - cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << endl; + std::cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << std::endl; } /* ************************************************************************* */ template - void addLandmarkToPartitionResult(const GenericGraph& graph, const vector& landmarkKeys, + void addLandmarkToPartitionResult(const GenericGraph& graph, const std::vector& landmarkKeys, MetisResult& partitionResult, WorkSpace& workspace) { // set up cameras in the dictionary @@ -408,7 +406,7 @@ namespace gtsam { namespace partition { BOOST_FOREACH(const size_t b, B) dictionary[b] = 2; if (!C.empty()) - throw runtime_error("addLandmarkToPartitionResult: C is not empty"); + throw std::runtime_error("addLandmarkToPartitionResult: C is not empty"); // set up landmarks size_t i,j; @@ -424,7 +422,7 @@ namespace gtsam { namespace partition { dictionary[j] = 0; } // if (j == 67980) -// cout << "dictionary[67980]" << dictionary[j] << endl; +// std::cout << "dictionary[67980]" << dictionary[j] << std::endl; } BOOST_FOREACH(const size_t j, landmarkKeys) { @@ -432,8 +430,8 @@ namespace gtsam { namespace partition { case 0: C.push_back(j); break; case 1: A.push_back(j); break; case 2: B.push_back(j); break; - default: cout << j << ": " << dictionary[j] << endl; - throw runtime_error("addLandmarkToPartitionResult: wrong status for landmark"); + default: std::cout << j << ": " << dictionary[j] << std::endl; + throw std::runtime_error("addLandmarkToPartitionResult: wrong status for landmark"); } } } @@ -442,13 +440,13 @@ namespace gtsam { namespace partition { /* ************************************************************************* */ template - boost::optional findPartitoning(const GenericGraph& graph, const vector& keys, + boost::optional findPartitoning(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, bool verbose, - const boost::optional >& int2symbol, const bool reduceGraph) { + const boost::optional >& int2symbol, const bool reduceGraph) { boost::optional result; GenericGraph reducedGraph; - vector keyToPartition; - vector cameraKeys, landmarkKeys; + std::vector keyToPartition; + std::vector cameraKeys, landmarkKeys; if (reduceGraph) { if (!int2symbol.is_initialized()) throw std::invalid_argument("findSeparator: int2symbol must be valid!"); @@ -467,20 +465,20 @@ namespace gtsam { namespace partition { workspace.prepareDictionary(keyToPartition); const std::vector& dictionary = workspace.dictionary; reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph); - cout << "original graph: V" << keys.size() << ", E" << graph.size() - << " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << endl; + std::cout << "original graph: V" << keys.size() << ", E" << graph.size() + << " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << std::endl; result = edgePartitionByMetis(reducedGraph, keyToPartition, workspace, verbose); } else // call Metis to partition the graph to A, B, C result = separatorPartitionByMetis(graph, keys, workspace, verbose); if (!result.is_initialized()) { - cout << "metis failed!" << endl; + std::cout << "metis failed!" << std::endl; return 0; } if (reduceGraph) { addLandmarkToPartitionResult(graph, landmarkKeys, *result, workspace); - cout << "the separator size: " << result->C.size() << " landmarks" << endl; + std::cout << "the separator size: " << result->C.size() << " landmarks" << std::endl; } return result; @@ -488,22 +486,22 @@ namespace gtsam { namespace partition { /* ************************************************************************* */ template - int findSeparator(const GenericGraph& graph, const vector& keys, + int findSeparator(const GenericGraph& graph, const std::vector& keys, const int minNodesPerMap, WorkSpace& workspace, bool verbose, - const boost::optional >& int2symbol, const bool reduceGraph, + const boost::optional >& int2symbol, const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { boost::optional result = findPartitoning(graph, keys, workspace, verbose, int2symbol, reduceGraph); // find the island in A and B, and make them separated submaps - typedef vector Island; - list islands; + typedef std::vector Island; + std::list islands; - list islands_in_A = findIslands(graph, result->A, workspace, + std::list islands_in_A = findIslands(graph, result->A, workspace, minNrConstraintsPerCamera, minNrConstraintsPerLandmark); - list islands_in_B = findIslands(graph, result->B, workspace, + std::list islands_in_B = findIslands(graph, result->B, workspace, minNrConstraintsPerCamera, minNrConstraintsPerLandmark); islands.insert(islands.end(), islands_in_A.begin(), islands_in_A.end()); @@ -514,13 +512,13 @@ namespace gtsam { namespace partition { #ifdef NDEBUG // verbose = true; // if (!int2symbol) throw std::invalid_argument("findSeparator: int2symbol is not set!"); -// cout << "sep size: " << result->C.size() << "; "; +// std::cout << "sep size: " << result->C.size() << "; "; // printNumCamerasLandmarks(result->C, *int2symbol); -// cout << "no. of island: " << islands.size() << "; "; -// cout << "island size: "; +// std::cout << "no. of island: " << islands.size() << "; "; +// std::cout << "island size: "; // BOOST_FOREACH(const Island& island, islands) -// cout << island.size() << " "; -// cout << endl; +// std::cout << island.size() << " "; +// std::cout << std::endl; // BOOST_FOREACH(const Island& island, islands) { // printNumCamerasLandmarks(island, int2symbol); @@ -531,20 +529,20 @@ namespace gtsam { namespace partition { size_t oldSize = islands.size(); while(true) { if (islands.size() < 2) { - cout << "numIsland: " << numIsland0 << endl; - throw runtime_error("findSeparator: found fewer than 2 submaps!"); + std::cout << "numIsland: " << numIsland0 << std::endl; + throw std::runtime_error("findSeparator: found fewer than 2 submaps!"); } - list::reference island = islands.back(); + std::list::reference island = islands.back(); if ((int)island.size() >= minNodesPerMap) break; result->C.insert(result->C.end(), island.begin(), island.end()); islands.pop_back(); } if (islands.size() != oldSize){ - if (verbose) cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << endl; + if (verbose) std::cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << std::endl; } else{ - if (verbose) cout << oldSize << " submap(s);\t" << endl; + if (verbose) std::cout << oldSize << " submap(s);\t" << std::endl; } // generate the node map diff --git a/gtsam_unstable/partition/NestedDissection-inl.h b/gtsam_unstable/partition/NestedDissection-inl.h index e8f3fa34a..d6dafce49 100644 --- a/gtsam_unstable/partition/NestedDissection-inl.h +++ b/gtsam_unstable/partition/NestedDissection-inl.h @@ -14,8 +14,6 @@ #include "OrderedSymbols.h" #include "NestedDissection.h" -using namespace std; - namespace gtsam { namespace partition { /* ************************************************************************* */ From cea4aef9f207475a929b0083cb645d6cef533fda Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 6 May 2014 00:23:20 -0400 Subject: [PATCH 035/101] New perturbPose2 utility --- gtsam.h | 1 + matlab.h | 20 +++++++++++++++----- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/gtsam.h b/gtsam.h index 56a1a601d..05cf7ffb5 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2363,6 +2363,7 @@ namespace utilities { gtsam::Values allPose3s(gtsam::Values& values); Matrix extractPose3(const gtsam::Values& values); void perturbPoint2(gtsam::Values& values, double sigma, int seed); + void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); void perturbPoint3(gtsam::Values& values, double sigma, int seed); void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); diff --git a/matlab.h b/matlab.h index 6c2b137d7..100f5a242 100644 --- a/matlab.h +++ b/matlab.h @@ -84,7 +84,7 @@ namespace gtsam { return result; } - /// perturb all Point2 using normally distributed noise + /// Perturb all Point2 values using normally distributed noise void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma); Sampler sampler(model, seed); @@ -93,7 +93,17 @@ namespace gtsam { } } - /// perturb all Point3 using normally distributed noise + /// Perturb all Pose2 values using normally distributed noise + void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = 42u) { + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( + Vector3(sigmaT, sigmaT, sigmaR)); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } + } + + /// Perturb all Point3 values using normally distributed noise void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); Sampler sampler(model, seed); @@ -102,7 +112,7 @@ namespace gtsam { } } - /// insert a number of initial point values by backprojecting + /// Insert a number of initial point values by backprojecting void insertBackprojections(Values& values, const SimpleCamera& camera, const Vector& J, const Matrix& Z, double depth) { if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); if (Z.cols() != J.size()) throw std::invalid_argument("insertBackProjections: J and Z must have same number of entries"); @@ -113,7 +123,7 @@ namespace gtsam { } } - /// insert multiple projection factors for a single pose key + /// Insert multiple projection factors for a single pose key void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, const Vector& J, const Matrix& Z, const SharedNoiseModel& model, const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) { if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K"); @@ -126,7 +136,7 @@ namespace gtsam { } } - /// calculate the errors of all projection factors in a graph + /// Calculate the errors of all projection factors in a graph Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) { // first count size_t K = 0, k=0; From 779d6ad2afcdf41a76a116dbda27d47e7737b340 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 6 May 2014 00:23:34 -0400 Subject: [PATCH 036/101] Added utilities to Contents.m --- matlab/+gtsam/Contents.m | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 8d15bc913..5fec7721c 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -173,3 +173,16 @@ % symbol - create a Symbol key % symbolChr - get character from a symbol key % symbolIndex - get index from a symbol key +% +%% Wrapped C++ Convenience Functions for use within MATLAB +% utilities.extractPoint2 - Extract all Point2 values into a single matrix [x y] +% utilities.extractPoint3 - Extract all Point3 values into a single matrix [x y z] +% utilities.extractPose2 - Extract all Pose2 values into a single matrix [x y theta] +% utilities.allPose3s - Extract all Pose3 values +% utilities.extractPose3 - Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] +% utilities.perturbPoint2 - Perturb all Point2 values using normally distributed noise +% utilities.perturbPose2 - Perturb all Pose2 values using normally distributed noise +% utilities.perturbPoint3 - Perturb all Point3 values using normally distributed noise +% utilities.insertBackprojections - Insert a number of initial point values by backprojecting +% utilities.insertProjectionFactors - Insert multiple projection factors for a single pose key +% utilities.reprojectionErrors - Calculate the errors of all projection factors in a graph From 6c6c545d99f2542864e7fe3ea6bce3319f44905c Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 6 May 2014 01:33:09 -0400 Subject: [PATCH 037/101] Comments --- gtsam/linear/GaussianConditional.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index c6bd3967b..a5c651a44 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -119,14 +119,20 @@ namespace gtsam { /* ************************************************************************* */ VectorValues GaussianConditional::solve(const VectorValues& x) const { - // Solve matrix + // Concatenate all vector values that correspond to parent variables Vector xS = x.vector(FastVector(beginParents(), endParents())); + + // Update right-hand-side xS = getb() - get_S() * xS; + + // Solve matrix Vector soln = get_R().triangularView().solve(xS); // Check for indeterminant solution if(soln.hasNaN()) throw IndeterminantLinearSystemException(keys().front()); + // TODO, do we not have to scale by sigmas here? Copy/paste bug + // Insert solution into a VectorValues VectorValues result; DenseIndex vectorPosition = 0; @@ -142,9 +148,14 @@ namespace gtsam { VectorValues GaussianConditional::solveOtherRHS( const VectorValues& parents, const VectorValues& rhs) const { + // Concatenate all vector values that correspond to parent variables Vector xS = parents.vector(FastVector(beginParents(), endParents())); + + // Instead of updating getb(), update the right-hand-side from the given rhs const Vector rhsR = rhs.vector(FastVector(beginFrontals(), endFrontals())); xS = rhsR - get_S() * xS; + + // Solve Matrix Vector soln = get_R().triangularView().solve(xS); // Scale by sigmas From 108357992c0f61d63665939f6fe948f667b4e5af Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 6 May 2014 02:18:52 -0400 Subject: [PATCH 038/101] Optimize an incomplete BayesNet --- .cproject | 184 +++++++++--------- gtsam.h | 1 + gtsam/linear/GaussianBayesNet.cpp | 9 +- gtsam/linear/GaussianBayesNet.h | 8 +- .../tests/testGaussianBayesNetUnordered.cpp | 18 ++ 5 files changed, 127 insertions(+), 93 deletions(-) diff --git a/.cproject b/.cproject index 31d4d9143..5d6801d19 100644 --- a/.cproject +++ b/.cproject @@ -998,6 +998,102 @@ true true + + make + -j5 + testGaussianFactorGraphUnordered.run + true + true + true + + + make + -j5 + testGaussianBayesNetUnordered.run + true + true + true + + + make + -j5 + testGaussianConditional.run + true + true + true + + + make + -j5 + testGaussianDensity.run + true + true + true + + + make + -j5 + testGaussianJunctionTree.run + true + true + true + + + make + -j5 + testHessianFactor.run + true + true + true + + + make + -j5 + testJacobianFactor.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 -j2 @@ -2123,94 +2219,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 -j2 diff --git a/gtsam.h b/gtsam.h index 05cf7ffb5..8d61487a5 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1423,6 +1423,7 @@ virtual class GaussianBayesNet { void push_back(const gtsam::GaussianBayesNet& bayesNet); gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; gtsam::VectorValues optimizeGradientSearch() const; gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; gtsam::VectorValues gradientAtZero() const; diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 8b3dc0084..b3b8b9a41 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -42,7 +42,14 @@ namespace gtsam { /* ************************************************************************* */ VectorValues GaussianBayesNet::optimize() const { - VectorValues soln; + VectorValues soln; // no missing variables -> just create an empty vector + return optimize(soln); + } + + /* ************************************************************************* */ + VectorValues GaussianBayesNet::optimize( + const VectorValues& solutionForMissing) const { + VectorValues soln(solutionForMissing); // possibly empty // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) /** solve each node in turn in topological sort order (parents first)*/ BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) { diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 26b187369..69a70a5e4 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -68,12 +68,12 @@ namespace gtsam { /// @name Standard Interface /// @{ - /** - * Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by - * back-substitution. - */ + /// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by back-substitution VectorValues optimize() const; + /// Version of optimize for incomplete BayesNet, needs solution for missing variables + VectorValues optimize(const VectorValues& solutionForMissing) const; + ///@} ///@name Linear Algebra diff --git a/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp b/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp index ca2b4c3d2..608e9b1e1 100644 --- a/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp +++ b/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp @@ -69,6 +69,24 @@ TEST( GaussianBayesNet, optimize ) EXPECT(assert_equal(expected,actual)); } +/* ************************************************************************* */ +TEST( GaussianBayesNet, optimizeIncomplete ) +{ + static GaussianBayesNet incompleteBayesNet = list_of + (GaussianConditional(_x_, (Vector(1) << 9.0), (Matrix(1, 1) << 1.0), _y_, (Matrix(1, 1) << 1.0))); + + VectorValues solutionForMissing = map_list_of + (_y_, (Vector(1) << 5.0)); + + VectorValues actual = incompleteBayesNet.optimize(solutionForMissing); + + VectorValues expected = map_list_of + (_x_, (Vector(1) << 4.0)) + (_y_, (Vector(1) << 5.0)); + + EXPECT(assert_equal(expected,actual)); +} + /* ************************************************************************* */ TEST( GaussianBayesNet, optimize3 ) { From 3f125b46dfeaadaf111b8d207b89ae3b98fbe4d6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 6 May 2014 11:08:33 -0400 Subject: [PATCH 039/101] New method merge (mainly for MATLAB) --- gtsam.h | 1 + gtsam/base/FastSet.h | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/gtsam.h b/gtsam.h index 05cf7ffb5..261a962d0 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1725,6 +1725,7 @@ class KeySet { // structure specific methods void insert(size_t key); + void merge(gtsam::KeySet& other); bool erase(size_t key); // returns true if value was removed bool count(size_t key) const; // returns true if value exists diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 7ed6c7cd7..69e841e57 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -103,6 +103,11 @@ public: /** Check for equality within tolerance to implement Testable */ bool equals(const FastSet& other, double tol = 1e-9) const { return FastSetTestableHelper::equals(*this, other, tol); } + /** insert another set: handy for MATLAB access */ + void merge(const FastSet& other) { + Base::insert(other.begin(),other.end()); + } + private: /** Serialization function */ friend class boost::serialization::access; From 0450c58d6647350c2c510d8380115fd10e4bed94 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 6 May 2014 11:54:45 -0400 Subject: [PATCH 040/101] silence warning --- gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 2761e3af6..8606538bf 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -350,7 +350,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { std::cout << "using lambda = " << lambda << std::endl; result.iterations++; - } while(result.iterations < parameters_.maxIterations && + } while(result.iterations < (size_t)parameters_.maxIterations && !checkConvergence(parameters_.relativeErrorTol, parameters_.absoluteErrorTol, parameters_.errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT)); return result; From ba8db019da7c9469c4014e4f5621e8f1e7c733e7 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 6 May 2014 11:56:47 -0400 Subject: [PATCH 041/101] Adding unit test for skewSymmetric because there's a strange warning elsewhere --- gtsam/base/tests/testMatrix.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 828208361..9d84e5ab7 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -459,6 +459,22 @@ TEST( matrix, scale_rows_mask ) EXPECT(assert_equal(actual, expected)); } +/* ************************************************************************* */ +TEST( matrix, skewSymmetric ) +{ + double wx = 1, wy = 2, wz = 3; + Matrix3 actual = skewSymmetric(wx,wy,wz); + + Matrix expected(3,3); + expected << 0, -3, 2, + 3, 0, -1, + -2, 1, 0; + + EXPECT(assert_equal(actual, expected)); + +} + + /* ************************************************************************* */ TEST( matrix, equal ) { From 9395bb0913e3b79cff464b5487292edd7d763292 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 6 May 2014 12:31:03 -0400 Subject: [PATCH 042/101] typos --- gtsam/geometry/Cal3Unified.cpp | 2 +- gtsam/geometry/Cal3Unified.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index 39ad221cb..808cb84a4 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file Cal3Unify.cpp + * @file Cal3Unified.cpp * @date Mar 8, 2014 * @author Jing Dong */ diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 6961757bd..a1d47332a 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file Cal3Unify.h + * @file Cal3Unified.h * @brief Unified Calibration Model, see Mei07icra for details * @date Mar 8, 2014 * @author Jing Dong From 2649b0fd7a5ff015e6c474a0bd66b3d6093685a8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 6 May 2014 13:21:49 -0400 Subject: [PATCH 043/101] print variants for KeyList, KeyVector --- .cproject | 1862 ++++++++++++++++++++------------------- gtsam.h | 8 +- gtsam/inference/Key.cpp | 33 +- gtsam/inference/Key.h | 8 + 4 files changed, 974 insertions(+), 937 deletions(-) diff --git a/.cproject b/.cproject index 5d6801d19..49afd8e90 100644 --- a/.cproject +++ b/.cproject @@ -662,14 +662,6 @@ false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -726,46 +718,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - make -j2 @@ -782,134 +734,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j6 -j8 - testAntiFactor.run - true - true - true - - - make - -j6 -j8 - testBetweenFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - make -j5 @@ -950,6 +774,30 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + make -j5 @@ -1094,38 +942,6 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean - true - true - true - make -j5 @@ -1198,326 +1014,6 @@ true true - - make - -j5 - testDiscreteFactor.run - true - true - true - - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - 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 - vSFMexample.run - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j2 - testVSLAMGraph - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j5 - testMarginals.run - true - true - true - - - make - -j5 - testGaussianISAM2.run - true - true - true - - - make - -j5 - testSymbolicFactorGraphB.run - true - true - true - - - make - -j2 - timeSequentialOnDataset.run - true - true - true - - - make - -j5 - testGradientDescentOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - -j2 - testNonlinearISAM.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testExtendedKalmanFilter.run - true - true - true - - - make - -j5 - timing.tests - true - true - true - - - make - -j5 - testNonlinearFactor.run - true - true - true - - - make - -j5 - clean - true - true - true - - - make - -j5 - testGaussianJunctionTreeB.run - true - true - true - - - make - - testGraph.run - true - false - true - - - make - - 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 - testSummarization.run - true - true - true - make -j5 @@ -1670,6 +1166,14 @@ true true + + make + -j5 + testParticleFactor.run + true + true + true + make -j2 @@ -1750,22 +1254,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j5 - testParticleFactor.run - true - true - true - make -j2 @@ -1862,54 +1350,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -1918,130 +1358,18 @@ true true - + make -j5 - testStereoCamera.run + testImuFactor.run true true true - + make -j5 - testRot3M.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testEssentialMatrix.run + testCombinedImuFactor.run true true true @@ -2187,209 +1515,42 @@ true true - + make -j5 - testImuFactor.run + testEliminationTree.run true true true - + make -j5 - testCombinedImuFactor.run + testInference.run true true true - + make -j5 - testVector.run + testKey.run true true true - + make - -j5 - testMatrix.run + -j1 + testSymbolicBayesTree.run true - true + false true - + make - -j2 - SimpleRotation.run - true - true - true - - - make - -j5 - CameraResectioning.run - true - true - true - - - make - -j5 - PlanarSLAMExample.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j5 - Pose2SLAMExample.run - true - true - true - - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - - - make - -j5 - UGM_small.run - true - true - true - - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - OdometryExample.run - true - true - true - - - make - -j5 - RangeISAMExample_plaza2.run - true - true - true - - - make - -j5 - SelfCalibrationExample.run - true - true - true - - - make - -j5 - SFMExample.run - true - true - true - - - make - -j5 - VisualISAMExample.run - true - true - true - - - make - -j5 - VisualISAM2Example.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graphviz.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graph.run - true - true - true - - - make - -j5 - SFMExample_bal.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - tests/testGaussianISAM2 + -j1 + testSymbolicSequentialSolver.run true false true @@ -2809,6 +1970,892 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + clean all + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j6 -j8 + testAntiFactor.run + true + true + true + + + make + -j6 -j8 + testBetweenFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j2 + testVSLAMGraph + true + true + true + + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j5 + testMarginals.run + true + true + true + + + make + -j5 + testGaussianISAM2.run + true + true + true + + + make + -j5 + testSymbolicFactorGraphB.run + true + true + true + + + make + -j2 + timeSequentialOnDataset.run + true + true + true + + + make + -j5 + testGradientDescentOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + -j2 + testNonlinearISAM.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + + + make + -j5 + timing.tests + true + true + true + + + make + -j5 + testNonlinearFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + + testGraph.run + true + false + true + + + make + + 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 + testSummarization.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + + + make + -j2 + SimpleRotation.run + true + true + true + + + make + -j5 + CameraResectioning.run + true + true + true + + + make + -j5 + PlanarSLAMExample.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + easyPoint2KalmanFilter.run + true + true + true + + + make + -j2 + elaboratePoint2KalmanFilter.run + true + true + true + + + make + -j5 + Pose2SLAMExample.run + true + true + true + + + make + -j2 + Pose2SLAMwSPCG_easy.run + true + true + true + + + make + -j5 + UGM_small.run + true + true + true + + + make + -j5 + LocalizationExample.run + true + true + true + + + make + -j5 + OdometryExample.run + true + true + true + + + make + -j5 + RangeISAMExample_plaza2.run + true + true + true + + + make + -j5 + SelfCalibrationExample.run + true + true + true + + + make + -j5 + SFMExample.run + true + true + true + + + make + -j5 + VisualISAMExample.run + true + true + true + + + make + -j5 + VisualISAM2Example.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graphviz.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graph.run + true + true + true + + + make + -j5 + SFMExample_bal.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + tests/testGaussianISAM2 + true + false + true + make -j2 @@ -2905,45 +2952,6 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. - true - false - true - make -j5 diff --git a/gtsam.h b/gtsam.h index fa3c3fbcf..2d181d350 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1551,8 +1551,12 @@ char symbolChr(size_t key); size_t symbolIndex(size_t key); // Default keyformatter -void printKeySet(const gtsam::KeySet& keys); -void printKeySet(const gtsam::KeySet& keys, string s); +void printKeyList (const gtsam::KeyList& keys); +void printKeyList (const gtsam::KeyList& keys, string s); +void printKeyVector(const gtsam::KeyVector& keys); +void printKeyVector(const gtsam::KeyVector& keys, string s); +void printKeySet (const gtsam::KeySet& keys); +void printKeySet (const gtsam::KeySet& keys, string s); #include class LabeledSymbol { diff --git a/gtsam/inference/Key.cpp b/gtsam/inference/Key.cpp index 3b73725bc..0b9be2f1c 100644 --- a/gtsam/inference/Key.cpp +++ b/gtsam/inference/Key.cpp @@ -27,31 +27,48 @@ namespace gtsam { - /* ************************************************************************* */ -std::string _multirobotKeyFormatter(gtsam::Key key) { +std::string _multirobotKeyFormatter(Key key) { const LabeledSymbol asLabeledSymbol(key); - if(asLabeledSymbol.chr() > 0 && asLabeledSymbol.label() > 0) - return (std::string)asLabeledSymbol; + if (asLabeledSymbol.chr() > 0 && asLabeledSymbol.label() > 0) + return (std::string) asLabeledSymbol; - const gtsam::Symbol asSymbol(key); + const Symbol asSymbol(key); if (asLabeledSymbol.chr() > 0) - return (std::string)asSymbol; + return (std::string) asSymbol; else return boost::lexical_cast(key); } /* ************************************************************************* */ -void printKeySet(const gtsam::KeySet& keys, const std::string& s, const KeyFormatter& keyFormatter) { +template +static void print(const CONTAINER& keys, const std::string& s, + const KeyFormatter& keyFormatter) { std::cout << s << " "; if (keys.empty()) std::cout << "(none)" << std::endl; else { - BOOST_FOREACH(const gtsam::Key& key, keys) + BOOST_FOREACH(const Key& key, keys) std::cout << keyFormatter(key) << " "; std::cout << std::endl; } } + +/* ************************************************************************* */ +void printKeyList(const KeyList& keys, const std::string& s, + const KeyFormatter& keyFormatter) { + print(keys, s, keyFormatter); +} +/* ************************************************************************* */ +void printKeyVector(const KeyVector& keys, const std::string& s, + const KeyFormatter& keyFormatter) { + print(keys, s, keyFormatter); +} +/* ************************************************************************* */ +void printKeySet(const KeySet& keys, const std::string& s, + const KeyFormatter& keyFormatter) { + print(keys, s, keyFormatter); +} /* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index 3f573fcce..e2be79dc7 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -46,6 +46,14 @@ namespace gtsam { typedef FastSet KeySet; typedef FastMap KeyGroupMap; + /// Utility function to print sets of keys with optional prefix + GTSAM_EXPORT void printKeyList(const KeyList& keys, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + + /// Utility function to print sets of keys with optional prefix + GTSAM_EXPORT void printKeyVector(const KeyVector& keys, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + /// Utility function to print sets of keys with optional prefix GTSAM_EXPORT void printKeySet(const KeySet& keys, const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); From 842554a23099f22d92a6d5dfbe041fe198e29715 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 6 May 2014 20:03:45 -0400 Subject: [PATCH 044/101] Unit test fixes for quaternion mode and also rotation matrix + full expmap mode. --- gtsam/geometry/tests/testEssentialMatrix.cpp | 4 +-- gtsam/slam/tests/testBetweenFactor.cpp | 4 +-- .../tests/testEssentialMatrixConstraint.cpp | 4 +-- .../slam/tests/testEssentialMatrixFactor.cpp | 9 ++++++ gtsam/slam/tests/testRotateFactor.cpp | 20 +++++++++++++ .../slam/tests/testPoseBetweenFactor.cpp | 28 ++++++++++++++++--- .../slam/tests/testPosePriorFactor.cpp | 25 +++++++++++++++-- 7 files changed, 82 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index b3ff907d9..9ad30bc41 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -125,7 +125,7 @@ TEST (EssentialMatrix, rotate) { } // avoid exception Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), // expH2 = numericalDerivative22(rotate_, bodyE, cRb); - EXPECT(assert_equal(expH1, actH1, 1e-8)); + EXPECT(assert_equal(expH1, actH1, 1e-7)); // Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8)); } @@ -149,7 +149,7 @@ TEST (EssentialMatrix, FromPose3_b) { EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); - EXPECT(assert_equal(expectedH, actualH, 1e-8)); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } //************************************************************************* diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index fac2164d8..d0dbe7908 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -35,13 +35,13 @@ TEST(BetweenFactor, Rot3) { boost::function(boost::bind( &BetweenFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), R1, R2, 1e-5); - EXPECT(assert_equal(numericalH1,actualH1)); + EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); Matrix numericalH2 = numericalDerivative22( boost::function(boost::bind( &BetweenFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), R1, R2, 1e-5); - EXPECT(assert_equal(numericalH2,actualH2)); + EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 8fcb5b6e5..5184393ac 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -63,8 +63,8 @@ TEST( EssentialMatrixConstraint, test ) { factor.evaluateError(pose1, pose2, actualH1, actualH2); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); - CHECK(assert_equal(expectedH2, actualH2, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); + CHECK(assert_equal(expectedH2, actualH2, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 0766dbf8f..1e5674599 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -128,7 +128,11 @@ TEST (EssentialMatrixFactor, minimization) { EssentialMatrix initialE = trueE.retract( (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1)); initial.insert(1, initialE); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); +#else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); +#endif // Optimize LevenbergMarquardtParams parameters; @@ -339,7 +343,12 @@ TEST (EssentialMatrixFactor, extraMinimization) { EssentialMatrix initialE = trueE.retract( (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1)); initial.insert(1, initialE); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); +#else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); +#endif // Optimize LevenbergMarquardtParams parameters; diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index ca924aaae..f36405318 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -59,7 +59,11 @@ TEST (RotateFactor, test) { EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8)); Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1)); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + Vector expectedE = (Vector(3) << -0.0248752, 0.202981, -0.0890529); +#else Vector expectedE = (Vector(3) << -0.0246305, 0.20197, -0.08867); +#endif EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); Matrix actual, expected; @@ -97,7 +101,12 @@ TEST (RotateFactor, minimization) { double degree = M_PI / 180; Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20)); initial.insert(1, initialE); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(3545.40, graph.error(initial), 1); +#else EXPECT_DOUBLES_EQUAL(3349, graph.error(initial), 1); +#endif // Optimize LevenbergMarquardtParams parameters; @@ -120,7 +129,13 @@ TEST (RotateDirectionsFactor, test) { EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8)); Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1)); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + Vector expectedE = (Vector(2) << -0.0890529, -0.202981); +#else Vector expectedE = (Vector(2) << -0.08867, -0.20197); +#endif + EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); Matrix actual, expected; @@ -160,7 +175,12 @@ TEST (RotateDirectionsFactor, minimization) { double degree = M_PI / 180; Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20)); initial.insert(1, initialE); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(3335.9, graph.error(initial), 1); +#else EXPECT_DOUBLES_EQUAL(3162, graph.error(initial), 1); +#endif // Optimize LevenbergMarquardtParams parameters; diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index 2fc471eab..bf9dc0e8e 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -95,12 +95,21 @@ TEST( PoseBetweenFactor, Error ) { // The expected error Vector expectedError(6); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + expectedError << -0.0298135267953815, + 0.0131341515747393, + 0.0968868439682154, + -0.145701634472172, + -0.134898525569125, + -0.0421026389164264; +#else expectedError << -0.029839512616488, 0.013145599455949, 0.096971291682578, -0.139187549519821, -0.142346243063553, -0.039088532100977; +#endif // Create a factor and calculate the error Key poseKey1(1); @@ -123,12 +132,23 @@ TEST( PoseBetweenFactor, ErrorWithTransform ) { // The expected error Vector expectedError(6); + // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! +#if defined(GTSAM_ROT3_EXPMAP) + expectedError << 0.0173358202010741, + 0.0222210698409755, + -0.0125032003886145, + 0.0263800787416566, + 0.00540285006310398, + 0.000175859555693563; +#else expectedError << 0.017337193670445, 0.022222830355243, -0.012504190982804, 0.026413288603739, 0.005237695303536, -0.000071612703633; +#endif + // Create a factor and calculate the error Key poseKey1(1); @@ -165,8 +185,8 @@ TEST( PoseBetweenFactor, Jacobian ) { factor.evaluateError(pose1, pose2, actualH1, actualH2); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); - CHECK(assert_equal(expectedH2, actualH2, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); + CHECK(assert_equal(expectedH2, actualH2, 1e-6)); } /* ************************************************************************* */ @@ -194,8 +214,8 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) { Vector error = factor.evaluateError(pose1, pose2, actualH1, actualH2); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); - CHECK(assert_equal(expectedH2, actualH2, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-6)); + CHECK(assert_equal(expectedH2, actualH2, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index 27bcd55ce..cbfa45431 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -90,12 +90,23 @@ TEST( PosePriorFactor, Error ) { // The expected error Vector expectedError(6); + // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! +#if defined(GTSAM_ROT3_EXPMAP) + expectedError << -0.182948257976108, + 0.13851858011118, + -0.157375974517456, + 0.766913166076379, + -1.22976117053126, + 0.949345561430261; +#else expectedError << -0.184137861505414, 0.139419283914526, -0.158399296722242, 0.740211733817804, -1.198210282560946, 1.008156093015192; +#endif + // Create a factor and calculate the error Key poseKey(1); @@ -116,12 +127,22 @@ TEST( PosePriorFactor, ErrorWithTransform ) { // The expected error Vector expectedError(6); + // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! +#if defined(GTSAM_ROT3_EXPMAP) + expectedError << -0.0224998729281528, + 0.191947887288328, + 0.273826035236257, + 1.36483391560855, + -0.754590051075035, + 0.585710674473659; +#else expectedError << -0.022712885347328, 0.193765110165872, 0.276418420819283, 1.497519863757366, -0.549375791422721, 0.452761203187666; +#endif // Create a factor and calculate the error Key poseKey(1); @@ -152,7 +173,7 @@ TEST( PosePriorFactor, Jacobian ) { factor.evaluateError(pose, actualH1); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } /* ************************************************************************* */ @@ -176,7 +197,7 @@ TEST( PosePriorFactor, JacobianWithTransform ) { factor.evaluateError(pose, actualH1); // Verify we get the expected error - CHECK(assert_equal(expectedH1, actualH1, 1e-9)); + CHECK(assert_equal(expectedH1, actualH1, 1e-5)); } /* ************************************************************************* */ From d45fe95b8d4f99213238ef6342dee1e9f1cd97e9 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 6 May 2014 19:05:59 -0700 Subject: [PATCH 045/101] Better and up-to-date example project --- cmake/example_project/CMakeLists.txt | 169 ++---------------- cmake/example_project/SayGoodbye.cpp | 23 +++ cmake/example_project/SayHello.cpp | 23 +++ cmake/example_project/example.h | 28 +++ .../example_project/example/PrintExamples.cpp | 44 +++++ cmake/example_project/example/PrintExamples.h | 41 +++++ cmake/example_project/tests/testExample.cpp | 43 +++++ cmake/example_project_simple/CMakeLists.txt | 38 ---- 8 files changed, 221 insertions(+), 188 deletions(-) create mode 100644 cmake/example_project/SayGoodbye.cpp create mode 100644 cmake/example_project/SayHello.cpp create mode 100644 cmake/example_project/example.h create mode 100644 cmake/example_project/example/PrintExamples.cpp create mode 100644 cmake/example_project/example/PrintExamples.h create mode 100644 cmake/example_project/tests/testExample.cpp delete mode 100644 cmake/example_project_simple/CMakeLists.txt diff --git a/cmake/example_project/CMakeLists.txt b/cmake/example_project/CMakeLists.txt index 8dc124b2f..8090c5974 100644 --- a/cmake/example_project/CMakeLists.txt +++ b/cmake/example_project/CMakeLists.txt @@ -1,173 +1,42 @@ # This file should be used as a template for creating new projects using the CMake tools # This project has the following features # - GTSAM linking -# - Boost linking # - Unit tests via CppUnitLite -# - Automatic detection of sources and headers in subfolders -# - Installation of library and headers -# - Matlab wrap interface with within-project building -# - Use of GTSAM cmake macros +# - Scripts ################################################################################### -# To create your own project, replace "myproject" with the actual name of your project +# To create your own project, replace "example" with the actual name of your project cmake_minimum_required(VERSION 2.6) -enable_testing() -project(myproject CXX C) +project(example CXX C) -# Add the cmake subfolder to the cmake module path - necessary to use macros -set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${PROJECT_SOURCE_DIR}/cmake") +# Include GTSAM CMake tools +find_package(GTSAMCMakeTools) +include(GtsamBuildTypes) # Load build type flags and default to Debug mode +include(GtsamTesting) # Easy functions for creating unit tests and scripts +include(GtsamMatlabWrap) # Automatic MATLAB wrapper generation # Ensure that local folder is searched before library folders include_directories(BEFORE "${PROJECT_SOURCE_DIR}") -# Load build type flags and default to Debug mode -include(GtsamBuildTypes) - ################################################################################### -# Create a list of library dependencies -# These will be linked with executables -set(library_deps "") -set(linking_mode "static") - # Find GTSAM components find_package(GTSAM REQUIRED) # Uses installed package -list(APPEND library_deps gtsam-${linking_mode} gtsam_unstable-${linking_mode}) - -# Include ransac -find_package(ransac REQUIRED) # Uses installed package -list(APPEND library_deps ransac-${linking_mode}) - -# Boost - same requirement as gtsam -find_package(Boost 1.43 COMPONENTS - serialization - system - filesystem - thread - date_time - REQUIRED) -list(APPEND library_deps - ${Boost_SERIALIZATION_LIBRARY} - ${Boost_SYSTEM_LIBRARY} - ${Boost_FILESYSTEM_LIBRARY} - ${Boost_THREAD_LIBRARY} - ${Boost_DATE_TIME_LIBRARY}) - -include_directories(${Boost_INCLUDE_DIR} ${GTSAM_INCLUDE_DIR} ${ransac_INCLUDE_DIR}) +include_directories(${GTSAM_INCLUDE_DIR}) ################################################################################### -# List subdirs to process tests/sources -# Each of these will be scanned for new files -set (myproject_subdirs - "." # ensure root folder gets included - stuff - things - ) - -# loop through subdirs to install and build up source lists -set(myproject_lib_source "") -set(myproject_tests_source "") -set(myproject_scripts_source "") -foreach(subdir ${myproject_subdirs}) - # Installing headers - message(STATUS "Installing ${subdir}") - file(GLOB sub_myproject_headers "myproject/${subdir}/*.h") - install(FILES ${sub_myproject_headers} DESTINATION include/myproject/${subdir}) - - # add sources to main sources list - file(GLOB subdir_srcs "myproject/${subdir}/*.cpp") - list(APPEND myproject_lib_source ${subdir_srcs}) - - # add tests to main tests list - file(GLOB subdir_test_srcs "myproject/${subdir}/tests/*.cpp") - list(APPEND myproject_tests_source ${subdir_test_srcs}) - - # add scripts to main tests list - file(GLOB subdir_scripts_srcs "myproject/${subdir}/scripts/*.cpp") - list(APPEND myproject_scripts_source ${subdir_scripts_srcs}) -endforeach(subdir) - -set(myproject_version ${myproject_VERSION_MAJOR}.${myproject_VERSION_MINOR}.${myproject_VERSION_PATCH}) -set(myproject_soversion ${myproject_VERSION_MAJOR}) -message(STATUS "GTSAM Version: ${gtsam_version}") -message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") - -# Build library (static and shared versions) -# Include installed versions -SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) -add_library(${PROJECT_NAME}-shared SHARED ${myproject_lib_source}) -set_target_properties(${PROJECT_NAME}-shared PROPERTIES - OUTPUT_NAME ${PROJECT_NAME} - CLEAN_DIRECT_OUTPUT 1) -install(TARGETS myproject-shared EXPORT myproject-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) -list(APPEND myproject_EXPORTED_TARGETS myproject-shared) - -add_library(${PROJECT_NAME}-static STATIC ${myproject_lib_source}) -set_target_properties(${PROJECT_NAME}-static PROPERTIES - OUTPUT_NAME ${PROJECT_NAME} - CLEAN_DIRECT_OUTPUT 1) -install(TARGETS myproject-static EXPORT myproject-exports ARCHIVE DESTINATION lib) -list(APPEND myproject_EXPORTED_TARGETS myproject-static) - -install(TARGETS ${PROJECT_NAME}-shared LIBRARY DESTINATION lib ) -install(TARGETS ${PROJECT_NAME}-static ARCHIVE DESTINATION lib ) - -# Disabled tests - subtract these from the test files -# Note the need for a full path -set(disabled_tests - "dummy" - #"${PROJECT_SOURCE_DIR}/myproject/geometry/tests/testCovarianceEllipse.cpp" -) -list(REMOVE_ITEM myproject_tests_source ${disabled_tests}) +# Build static library from common sources +set(CONVENIENCE_LIB_NAME ${PROJECT_NAME}) +add_library(${CONVENIENCE_LIB_NAME} STATIC example/PrintExamples.h example/PrintExamples.cpp) +target_link_libraries(${CONVENIENCE_LIB_NAME} gtsam) ################################################################################### -# Build tests -add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND}) -foreach(test_src_file ${myproject_tests_source}) - get_filename_component(test_base ${test_src_file} NAME_WE) - message(STATUS "Adding test ${test_src_file} with base name ${test_base}" ) - add_executable(${test_base} ${test_src_file}) - target_link_libraries(${test_base} ${PROJECT_NAME}-${linking_mode} ${library_deps} CppUnitLite) - add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}/${test_base}) - add_custom_target(${test_base}.run ${test_base} ${ARGN}) - add_dependencies(check ${test_base}) -endforeach(test_src_file) - -# Build scripts -foreach(script_src_file ${myproject_scripts_source}) - get_filename_component(script_base ${script_src_file} NAME_WE) - message(STATUS "Adding script ${script_src_file} with base name ${script_base}" ) - add_executable(${script_base} ${script_src_file}) - target_link_libraries(${script_base} ${PROJECT_NAME}-${linking_mode} ${library_deps} CppUnitLite) - add_custom_target(${script_base}.run ${script_base} ${ARGN}) -endforeach(script_src_file) +# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library) +gtsamAddTestsGlob("example" "tests/test*.cpp" "" "${CONVENIENCE_LIB_NAME}") ################################################################################### -# Matlab wrapping -include(GtsamMatlabWrap) -set(MEX_COMMAND "mex" CACHE STRING "Command to use for executing mex (if on path, 'mex' will work)") -set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation") -set(MYPROJECT_TOOLBOX_DIR "../matlab/myproject" CACHE PATH "Install folder for matlab toolbox - defaults to inside project") -set(WRAP_HEADER_PATH "${GTSAM_DIR}/../../../include") -set(MYPROJECT_TOOLBOX_FLAGS - ${GTSAM_BUILD_MEX_BINARY_FLAGS} -I${PROJECT_SOURCE_DIR} -I${PROJECT_SOURCE_DIR}/myproject -I${Boost_INCLUDE_DIR} -I${MEX_INCLUDE_ROOT} -I${GTSAM_INCLUDE_DIR} -I${WRAP_HEADER_PATH} -Wl,-rpath,${CMAKE_BINARY_DIR}:${CMAKE_INSTALL_PREFIX}/lib) -set(MYPROJECT_LIBRARY_DEPS gtsam gtsam_unstable ransac myproject) -set(GTSAM_BUILD_MEX_BIN ON) - -# Function to setup codegen, building and installation of the wrap toolbox -# This wrap setup function assumes that the toolbox will be installed directly, -# with predictable matlab.h sourcing -# params: -# moduleName : the name of the module, interface file must be called moduleName.h -# mexFlags : Compilation flags to be passed to the mex compiler -# modulePath : relative path to module markup header file (called moduleName.h) -# otherLibraries : list of library targets this should depend on -# toolboxPath : the directory in which to generate/build wrappers -# wrap_header_path : path to the installed wrap header -wrap_library_generic(myproject "${MYPROJECT_TOOLBOX_FLAGS}" "" "${MYPROJECT_LIBRARY_DEPS}" "${MYPROJECT_TOOLBOX_DIR}" "${WRAP_HEADER_PATH}") +# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library) +gtsamAddExamplesGlob("*.cpp" "" "${CONVENIENCE_LIB_NAME}") ################################################################################### -# Create Install config and export files -# This config file takes the place of FindXXX.cmake scripts -include(GtsamMakeConfigFile) -GtsamMakeConfigFile(myproject) -export(TARGETS ${myproject_EXPORTED_TARGETS} FILE myproject-exports.cmake) \ No newline at end of file +# Build MATLAB wrapper +wrap_and_install_library("example.h" "${CONVENIENCE_LIB_NAME}" "" "") diff --git a/cmake/example_project/SayGoodbye.cpp b/cmake/example_project/SayGoodbye.cpp new file mode 100644 index 000000000..be1165ef6 --- /dev/null +++ b/cmake/example_project/SayGoodbye.cpp @@ -0,0 +1,23 @@ +/* ---------------------------------------------------------------------------- + + * 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 SayGoodbye.cpp + * @brief Example script for example project + * @author Richard Roberts + */ + +#include + +int main(int argc, char *argv[]) { + example::PrintExamples().sayGoodbye(); + return 0; +} diff --git a/cmake/example_project/SayHello.cpp b/cmake/example_project/SayHello.cpp new file mode 100644 index 000000000..2da06ab32 --- /dev/null +++ b/cmake/example_project/SayHello.cpp @@ -0,0 +1,23 @@ +/* ---------------------------------------------------------------------------- + + * 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 SayHello.cpp + * @brief Example script for example project + * @author Richard Roberts + */ + +#include + +int main(int argc, char *argv[]) { + example::PrintExamples().sayHello(); + return 0; +} diff --git a/cmake/example_project/example.h b/cmake/example_project/example.h new file mode 100644 index 000000000..47b9a0aa2 --- /dev/null +++ b/cmake/example_project/example.h @@ -0,0 +1,28 @@ +/* ---------------------------------------------------------------------------- + + * 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 example.h + * @brief Example wrapper interface file + * @author Richard Roberts + */ + +// This is an interface file for automatic MATLAB wrapper generation. See +// gtsam.h for full documentation and more examples. + +namespace example { + +class PrintExamples { + void sayHello() const; + void sayGoodbye() const; +}; + +} diff --git a/cmake/example_project/example/PrintExamples.cpp b/cmake/example_project/example/PrintExamples.cpp new file mode 100644 index 000000000..1e9f10713 --- /dev/null +++ b/cmake/example_project/example/PrintExamples.cpp @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + + * 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 print_examples.cpp + * @brief Example library file + * @author Richard Roberts + */ + +#include + +#include + +namespace example { + +void PrintExamples::sayHello() const { + std::cout << internal::getHelloString() << std::endl; +} + +void PrintExamples::sayGoodbye() const { + std::cout << internal::getGoodbyeString() << std::endl; +} + +namespace internal { + +std::string getHelloString() { + return "Hello!"; +} + +std::string getGoodbyeString() { + return "See you soon!"; +} + +} // namespace internal + +} // namespace example diff --git a/cmake/example_project/example/PrintExamples.h b/cmake/example_project/example/PrintExamples.h new file mode 100644 index 000000000..b151dfbc7 --- /dev/null +++ b/cmake/example_project/example/PrintExamples.h @@ -0,0 +1,41 @@ +/* ---------------------------------------------------------------------------- + + * 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 print_examples.h + * @brief Example library file + * @author Richard Roberts + */ + +#pragma once + +#include + +namespace example { + +class PrintExamples { +public: + /// Print a greeting + void sayHello() const; + + /// Print a farewell + void sayGoodbye() const; +}; + +namespace internal { + +std::string getHelloString(); + +std::string getGoodbyeString(); + +} // namespace internal + +} // namespace example diff --git a/cmake/example_project/tests/testExample.cpp b/cmake/example_project/tests/testExample.cpp new file mode 100644 index 000000000..c2a5a173b --- /dev/null +++ b/cmake/example_project/tests/testExample.cpp @@ -0,0 +1,43 @@ +/* ---------------------------------------------------------------------------- + + * 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 testExample.cpp + * @brief Unit tests for example + * @author Richard Roberts + */ + +#include + +#include + +#include + +using namespace gtsam; + +TEST(Example, HelloString) { + const std::string expectedString = "Hello!"; + EXPECT(assert_equal(expectedString, example::internal::getHelloString())); +} + +TEST(Example, GoodbyeString) { + const std::string expectedString = "See you soon!"; + EXPECT(assert_equal(expectedString, example::internal::getGoodbyeString())); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + + diff --git a/cmake/example_project_simple/CMakeLists.txt b/cmake/example_project_simple/CMakeLists.txt deleted file mode 100644 index e8bea909c..000000000 --- a/cmake/example_project_simple/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -# This file should be used as a template for creating new projects using the CMake tools -# This project has the following features -# - GTSAM linking -# - Unit tests via CppUnitLite -# - Scripts - -################################################################################### -# To create your own project, replace "myproject" with the actual name of your project -cmake_minimum_required(VERSION 2.6) -enable_testing() -project(myproject CXX C) - -# Add the cmake subfolder to the cmake module path - necessary to use macros -list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake") - -# Ensure that local folder is searched before library folders -include_directories(BEFORE "${PROJECT_SOURCE_DIR}") - -# Load build type flags and default to Debug mode -include(GtsamBuildTypes) - -################################################################################### -# Find GTSAM components -find_package(GTSAM REQUIRED) # Uses installed package -include_directories(${GTSAM_INCLUDE_DIR}) - -################################################################################### -# Build static library from common sources -add_library(${PROJECT_NAME} STATIC ${PROJECT_NAME}/MySourceFiles.cpp) -target_link_libraries(${PROJECT_NAME} gtsam-shared) - -################################################################################### -# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library) -gtsam_add_subdir_tests(${PROJECT_NAME} "${PROJECT_NAME}" "${PROJECT_NAME}" "") - -################################################################################### -# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library) -gtsam_add_executables("${PROJECT_NAME}/myScripts.cpp" "${PROJECT_NAME}" "${PROJECT_NAME}" "") From bd6b7a63d90352d733010da69c87233cd39f4cd8 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 6 May 2014 19:12:59 -0700 Subject: [PATCH 046/101] Fixed comment --- cmake/example_project/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/example_project/CMakeLists.txt b/cmake/example_project/CMakeLists.txt index 8090c5974..0700d0262 100644 --- a/cmake/example_project/CMakeLists.txt +++ b/cmake/example_project/CMakeLists.txt @@ -3,6 +3,7 @@ # - GTSAM linking # - Unit tests via CppUnitLite # - Scripts +# - Automatic MATLAB wrapper generation ################################################################################### # To create your own project, replace "example" with the actual name of your project From fe3ecc9f9709c2bf287803f928c70dd1668506ab Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 6 May 2014 19:14:03 -0700 Subject: [PATCH 047/101] Fixed another comment --- cmake/example_project/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/example_project/CMakeLists.txt b/cmake/example_project/CMakeLists.txt index 0700d0262..548d56acd 100644 --- a/cmake/example_project/CMakeLists.txt +++ b/cmake/example_project/CMakeLists.txt @@ -39,5 +39,5 @@ gtsamAddTestsGlob("example" "tests/test*.cpp" "" "${CONVENIENCE_LIB_NAME}") gtsamAddExamplesGlob("*.cpp" "" "${CONVENIENCE_LIB_NAME}") ################################################################################### -# Build MATLAB wrapper +# Build MATLAB wrapper (CMake tracks the dependecy to link with GTSAM through our project's static library) wrap_and_install_library("example.h" "${CONVENIENCE_LIB_NAME}" "" "") From 1062e41de38db89317660e1ad2fb8ccce8b54869 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Tue, 6 May 2014 22:57:26 -0400 Subject: [PATCH 048/101] shut up some warnings on Windows --- gtsam_unstable/slam/SmartFactorBase.h | 8 ++++---- gtsam_unstable/slam/SmartProjectionFactor.h | 10 +++++----- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index ebef221bb..d24a878bb 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -220,7 +220,7 @@ public: Point2 reprojectionError(camera.project(point) - zi); overallError += 0.5 * this->noise_.at(i)->distance(reprojectionError.vector()); - } catch (CheiralityException& e) { + } catch (CheiralityException&) { std::cout << "Cheirality exception " << std::endl; exit (EXIT_FAILURE); } @@ -260,7 +260,7 @@ public: double computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, const Cameras& cameras, const Point3& point) const { - int numKeys = this->keys_.size(); + size_t numKeys = this->keys_.size(); E = zeros(2 * numKeys, 3); b = zero(2 * numKeys); double f = 0; @@ -272,7 +272,7 @@ public: try { bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); - } catch (CheiralityException& e) { + } catch (CheiralityException&) { std::cout << "Cheirality exception " << std::endl; exit (EXIT_FAILURE); } @@ -321,7 +321,7 @@ public: const Cameras& cameras, const Point3& point, const double lambda = 0.0) const { - int numKeys = this->keys_.size(); + size_t numKeys = this->keys_.size(); std::vector < KeyMatrix2D > Fblocks; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda); diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index a1b85e2c3..59a841813 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -234,14 +234,14 @@ public: rankTolerance_, enableEPI_); degenerate_ = false; cheiralityException_ = false; - } catch (TriangulationUnderconstrainedException& e) { + } catch (TriangulationUnderconstrainedException&) { // if TriangulationUnderconstrainedException can be // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) // in the second case we want to use a rotation-only smart factor degenerate_ = true; cheiralityException_ = false; - } catch (TriangulationCheiralityException& e) { + } catch (TriangulationCheiralityException&) { // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint cheiralityException_ = true; @@ -279,7 +279,7 @@ public: const Cameras& cameras, const double lambda = 0.0) const { bool isDebug = false; - int numKeys = this->keys_.size(); + size_t numKeys = this->keys_.size(); // Create structures for Hessian Factors std::vector < Key > js; std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); @@ -351,10 +351,10 @@ public: // Populate Gs and gs int GsCount2 = 0; - for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera + for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera DenseIndex i1D = i1 * D; gs.at(i1) = gs_vector.segment < D > (i1D); - for (DenseIndex i2 = 0; i2 < numKeys; i2++) { + for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) { if (i2 >= i1) { Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D); GsCount2++; From b108e1c94f96a97eb51b9c42ca4f22fca266042f Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 7 May 2014 00:04:22 -0400 Subject: [PATCH 049/101] Upgrade to Eigen 3.2.1 - minor fixes, Richard's patches carried forward --- gtsam/3rdparty/Eigen/CMakeLists.txt | 2 +- gtsam/3rdparty/Eigen/Eigen/Eigen2Support | 15 +- .../3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h | 52 ++-- .../Eigen/src/CholmodSupport/CholmodSupport.h | 23 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h | 2 +- .../Eigen/Eigen/src/Core/BooleanRedux.h | 8 +- .../3rdparty/Eigen/Eigen/src/Core/EigenBase.h | 30 --- gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h | 9 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h | 2 +- .../Eigen/Eigen/src/Core/MatrixBase.h | 45 ++++ .../Eigen/Eigen/src/Core/PermutationMatrix.h | 3 +- .../Eigen/Eigen/src/Core/PlainObjectBase.h | 16 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h | 5 +- .../Eigen/Eigen/src/Core/StableNorm.h | 27 +- .../3rdparty/Eigen/Eigen/src/Core/Transpose.h | 4 +- .../Eigen/Eigen/src/Core/VectorwiseOp.h | 5 +- .../Eigen/src/Core/arch/SSE/MathFunctions.h | 11 + .../Eigen/src/Core/arch/SSE/PacketMath.h | 11 +- .../Core/products/GeneralBlockPanelKernel.h | 6 + .../src/Core/products/GeneralMatrixVector.h | 15 +- .../Core/products/SelfadjointMatrixVector.h | 6 +- .../Eigen/Eigen/src/Core/util/Macros.h | 9 +- .../Eigen/Eigen/src/Core/util/Memory.h | 13 +- .../Eigen/Eigen/src/Eigen2Support/SVD.h | 3 +- .../Eigen/Eigen/src/Geometry/EulerAngles.h | 2 +- .../Eigen/Eigen/src/Geometry/Quaternion.h | 27 +- .../Eigen/Eigen/src/Geometry/Transform.h | 4 +- .../Eigen/Eigen/src/QR/ColPivHouseholderQR.h | 2 +- .../Eigen/Eigen/src/QR/FullPivHouseholderQR.h | 53 ++-- .../src/SPQRSupport/SuiteSparseQRSupport.h | 18 +- .../3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h | 5 +- .../ConservativeSparseSparseProduct.h | 34 +-- .../Eigen/src/SparseCore/MappedSparseMatrix.h | 2 + .../Eigen/Eigen/src/SparseCore/SparseBlock.h | 4 + .../Eigen/src/SparseCore/SparseColEtree.h | 6 +- .../Eigen/src/SparseCore/SparseDenseProduct.h | 4 +- .../Eigen/Eigen/src/SparseCore/SparseMatrix.h | 21 +- .../Eigen/src/SparseCore/SparseMatrixBase.h | 4 +- .../Eigen/src/SparseCore/SparsePermutation.h | 4 +- .../Eigen/src/SparseCore/SparseProduct.h | 5 +- .../SparseSparseProductWithPruning.h | 13 +- .../Eigen/Eigen/src/SparseCore/SparseUtil.h | 2 +- .../Eigen/Eigen/src/SparseLU/SparseLU.h | 8 +- .../Eigen/src/SparseLU/SparseLU_Memory.h | 43 ++-- .../Eigen/Eigen/src/SparseQR/SparseQR.h | 13 +- .../Eigen/Eigen/src/plugins/BlockMethods.h | 240 ++++++++++-------- .../Eigen/src/plugins/MatrixCwiseBinaryOps.h | 4 +- gtsam/3rdparty/Eigen/blas/CMakeLists.txt | 3 + .../Eigen/cmake/language_support.cmake | 2 +- .../Eigen/doc/A05_PortingFrom2To3.dox | 2 + .../Eigen/doc/A10_Eigen2SupportModes.dox | 2 + .../Eigen/doc/AsciiQuickReference.txt | 62 +++-- .../Eigen/doc/PreprocessorDirectives.dox | 6 +- gtsam/3rdparty/Eigen/doc/QuickReference.dox | 20 +- gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox | 2 +- gtsam/3rdparty/Eigen/doc/TutorialSparse.dox | 2 +- gtsam/3rdparty/Eigen/doc/eigendoxy.css | 10 + .../Eigen/doc/eigendoxy_footer.html.in | 4 +- .../snippets/MatrixBase_applyOnTheLeft.cpp | 7 + .../snippets/MatrixBase_applyOnTheRight.cpp | 9 + .../Eigen/doc/special_examples/CMakeLists.txt | 6 +- .../Tutorial_sparse_example_details.cpp | 4 +- gtsam/3rdparty/Eigen/lapack/CMakeLists.txt | 3 + gtsam/3rdparty/Eigen/scripts/eigen_gen_docs | 5 +- gtsam/3rdparty/Eigen/test/array.cpp | 13 +- .../3rdparty/Eigen/test/array_for_matrix.cpp | 15 +- gtsam/3rdparty/Eigen/test/cholesky.cpp | 24 +- .../Eigen/test/conservative_resize.cpp | 31 ++- gtsam/3rdparty/Eigen/test/geo_eulerangles.cpp | 67 +++-- .../Eigen/test/geo_transformations.cpp | 7 + gtsam/3rdparty/Eigen/test/product_trmm.cpp | 1 + gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp | 4 + gtsam/3rdparty/Eigen/test/ref.cpp | 4 +- gtsam/3rdparty/Eigen/test/sparse.h | 10 +- gtsam/3rdparty/Eigen/test/sparse_product.cpp | 17 +- gtsam/3rdparty/Eigen/test/sparse_vector.cpp | 13 +- gtsam/3rdparty/Eigen/test/stable_norm.cpp | 14 +- gtsam/3rdparty/Eigen/test/umeyama.cpp | 9 +- gtsam/3rdparty/Eigen/test/vectorwiseop.cpp | 10 + gtsam/3rdparty/Eigen/test/zerosized.cpp | 31 ++- .../Eigen/unsupported/Eigen/OpenGLSupport | 7 +- .../IterativeSolvers/ConstrainedConjGrad.h | 10 +- .../3rdparty/Eigen/unsupported/test/FFTW.cpp | 13 +- 83 files changed, 804 insertions(+), 485 deletions(-) create mode 100644 gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheLeft.cpp create mode 100644 gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheRight.cpp diff --git a/gtsam/3rdparty/Eigen/CMakeLists.txt b/gtsam/3rdparty/Eigen/CMakeLists.txt index ad0269ea6..76a11b9d2 100644 --- a/gtsam/3rdparty/Eigen/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/CMakeLists.txt @@ -204,7 +204,7 @@ if(NOT MSVC) option(EIGEN_TEST_NEON "Enable/Disable Neon in tests/examples" OFF) if(EIGEN_TEST_NEON) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a"8) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a8") message(STATUS "Enabling NEON in tests/examples") endif() diff --git a/gtsam/3rdparty/Eigen/Eigen/Eigen2Support b/gtsam/3rdparty/Eigen/Eigen/Eigen2Support index 36156d29a..6aa009d20 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Eigen2Support +++ b/gtsam/3rdparty/Eigen/Eigen/Eigen2Support @@ -14,12 +14,25 @@ #error Eigen2 support must be enabled by defining EIGEN2_SUPPORT before including any Eigen header #endif +#ifndef EIGEN_NO_EIGEN2_DEPRECATED_WARNING + +#if defined(__GNUC__) || defined(__INTEL_COMPILER) || defined(__clang__) +#warning "Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)" +#else +#pragma message ("Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)") +#endif + +#endif // EIGEN_NO_EIGEN2_DEPRECATED_WARNING + #include "src/Core/util/DisableStupidWarnings.h" /** \ingroup Support_modules * \defgroup Eigen2Support_Module Eigen2 support module - * This module provides a couple of deprecated functions improving the compatibility with Eigen2. * + * \warning Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. + * + * This module provides a couple of deprecated functions improving the compatibility with Eigen2. + * * To use it, define EIGEN2_SUPPORT before including any Eigen header * \code * #define EIGEN2_SUPPORT diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h index d19cb3968..d026418f8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h @@ -16,7 +16,10 @@ namespace Eigen { namespace internal { -template struct LDLT_Traits; + template struct LDLT_Traits; + + // PositiveSemiDef means positive semi-definite and non-zero; same for NegativeSemiDef + enum SignMatrix { PositiveSemiDef, NegativeSemiDef, ZeroSign, Indefinite }; } /** \ingroup Cholesky_Module @@ -69,7 +72,12 @@ template class LDLT * The default constructor is useful in cases in which the user intends to * perform decompositions via LDLT::compute(const MatrixType&). */ - LDLT() : m_matrix(), m_transpositions(), m_isInitialized(false) {} + LDLT() + : m_matrix(), + m_transpositions(), + m_sign(internal::ZeroSign), + m_isInitialized(false) + {} /** \brief Default Constructor with memory preallocation * @@ -81,6 +89,7 @@ template class LDLT : m_matrix(size, size), m_transpositions(size), m_temporary(size), + m_sign(internal::ZeroSign), m_isInitialized(false) {} @@ -93,6 +102,7 @@ template class LDLT : m_matrix(matrix.rows(), matrix.cols()), m_transpositions(matrix.rows()), m_temporary(matrix.rows()), + m_sign(internal::ZeroSign), m_isInitialized(false) { compute(matrix); @@ -139,7 +149,7 @@ template class LDLT inline bool isPositive() const { eigen_assert(m_isInitialized && "LDLT is not initialized."); - return m_sign == 1; + return m_sign == internal::PositiveSemiDef || m_sign == internal::ZeroSign; } #ifdef EIGEN2_SUPPORT @@ -153,7 +163,7 @@ template class LDLT inline bool isNegative(void) const { eigen_assert(m_isInitialized && "LDLT is not initialized."); - return m_sign == -1; + return m_sign == internal::NegativeSemiDef || m_sign == internal::ZeroSign; } /** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A. @@ -235,7 +245,7 @@ template class LDLT MatrixType m_matrix; TranspositionType m_transpositions; TmpMatrixType m_temporary; - int m_sign; + internal::SignMatrix m_sign; bool m_isInitialized; }; @@ -246,7 +256,7 @@ template struct ldlt_inplace; template<> struct ldlt_inplace { template - static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0) + static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign) { using std::abs; typedef typename MatrixType::Scalar Scalar; @@ -258,8 +268,9 @@ template<> struct ldlt_inplace if (size <= 1) { transpositions.setIdentity(); - if(sign) - *sign = numext::real(mat.coeff(0,0))>0 ? 1:-1; + if (numext::real(mat.coeff(0,0)) > 0) sign = PositiveSemiDef; + else if (numext::real(mat.coeff(0,0)) < 0) sign = NegativeSemiDef; + else sign = ZeroSign; return true; } @@ -284,7 +295,6 @@ template<> struct ldlt_inplace if(biggest_in_corner < cutoff) { for(Index i = k; i < size; i++) transpositions.coeffRef(i) = i; - if(sign) *sign = 0; break; } @@ -325,15 +335,15 @@ template<> struct ldlt_inplace } if((rs>0) && (abs(mat.coeffRef(k,k)) > cutoff)) A21 /= mat.coeffRef(k,k); - - if(sign) - { - // LDLT is not guaranteed to work for indefinite matrices, but let's try to get the sign right - int newSign = numext::real(mat.diagonal().coeff(index_of_biggest_in_corner)) > 0; - if(k == 0) - *sign = newSign; - else if(*sign != newSign) - *sign = 0; + + RealScalar realAkk = numext::real(mat.coeffRef(k,k)); + if (sign == PositiveSemiDef) { + if (realAkk < 0) sign = Indefinite; + } else if (sign == NegativeSemiDef) { + if (realAkk > 0) sign = Indefinite; + } else if (sign == ZeroSign) { + if (realAkk > 0) sign = PositiveSemiDef; + else if (realAkk < 0) sign = NegativeSemiDef; } } @@ -399,7 +409,7 @@ template<> struct ldlt_inplace template<> struct ldlt_inplace { template - static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0) + static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign) { Transpose matt(mat); return ldlt_inplace::unblocked(matt, transpositions, temp, sign); @@ -445,7 +455,7 @@ LDLT& LDLT::compute(const MatrixType& a) m_isInitialized = false; m_temporary.resize(size); - internal::ldlt_inplace::unblocked(m_matrix, m_transpositions, m_temporary, &m_sign); + internal::ldlt_inplace::unblocked(m_matrix, m_transpositions, m_temporary, m_sign); m_isInitialized = true; return *this; @@ -473,7 +483,7 @@ LDLT& LDLT::rankUpdate(const MatrixBase=0 ? 1 : -1; + m_sign = sigma>=0 ? internal::PositiveSemiDef : internal::NegativeSemiDef; m_isInitialized = true; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h index 783324b0b..c449960de 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h @@ -58,10 +58,12 @@ cholmod_sparse viewAsCholmod(SparseMatrix<_Scalar,_Options,_Index>& mat) res.p = mat.outerIndexPtr(); res.i = mat.innerIndexPtr(); res.x = mat.valuePtr(); + res.z = 0; res.sorted = 1; if(mat.isCompressed()) { res.packed = 1; + res.nz = 0; } else { @@ -170,6 +172,7 @@ class CholmodBase : internal::noncopyable CholmodBase() : m_cholmodFactor(0), m_info(Success), m_isInitialized(false) { + m_shiftOffset[0] = m_shiftOffset[1] = RealScalar(0.0); cholmod_start(&m_cholmod); } @@ -241,7 +244,7 @@ class CholmodBase : internal::noncopyable return internal::sparse_solve_retval(*this, b.derived()); } - /** Performs a symbolic decomposition on the sparcity of \a matrix. + /** Performs a symbolic decomposition on the sparsity pattern of \a matrix. * * This function is particularly useful when solving for several problems having the same structure. * @@ -265,7 +268,7 @@ class CholmodBase : internal::noncopyable /** Performs a numeric decomposition of \a matrix * - * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed. + * The given matrix must have the same sparsity pattern as the matrix on which the symbolic decomposition has been performed. * * \sa analyzePattern() */ @@ -302,7 +305,7 @@ class CholmodBase : internal::noncopyable { this->m_info = NumericalIssue; } - // TODO optimize this copy by swapping when possible (be carreful with alignment, etc.) + // TODO optimize this copy by swapping when possible (be careful with alignment, etc.) dest = Matrix::Map(reinterpret_cast(x_cd->x),b.rows(),b.cols()); cholmod_free_dense(&x_cd, &m_cholmod); } @@ -323,7 +326,7 @@ class CholmodBase : internal::noncopyable { this->m_info = NumericalIssue; } - // TODO optimize this copy by swapping when possible (be carreful with alignment, etc.) + // TODO optimize this copy by swapping when possible (be careful with alignment, etc.) dest = viewAsEigen(*x_cs); cholmod_free_sparse(&x_cs, &m_cholmod); } @@ -365,8 +368,8 @@ class CholmodBase : internal::noncopyable * * This class allows to solve for A.X = B sparse linear problems via a simplicial LL^T Cholesky factorization * using the Cholmod library. - * This simplicial variant is equivalent to Eigen's built-in SimplicialLLT class. Thefore, it has little practical interest. - * The sparse matrix A must be selfajoint and positive definite. The vectors or matrices + * This simplicial variant is equivalent to Eigen's built-in SimplicialLLT class. Therefore, it has little practical interest. + * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices * X and B can be either dense or sparse. * * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> @@ -412,8 +415,8 @@ class CholmodSimplicialLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimpl * * This class allows to solve for A.X = B sparse linear problems via a simplicial LDL^T Cholesky factorization * using the Cholmod library. - * This simplicial variant is equivalent to Eigen's built-in SimplicialLDLT class. Thefore, it has little practical interest. - * The sparse matrix A must be selfajoint and positive definite. The vectors or matrices + * This simplicial variant is equivalent to Eigen's built-in SimplicialLDLT class. Therefore, it has little practical interest. + * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices * X and B can be either dense or sparse. * * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> @@ -458,7 +461,7 @@ class CholmodSimplicialLDLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimp * This class allows to solve for A.X = B sparse linear problems via a supernodal LL^T Cholesky factorization * using the Cholmod library. * This supernodal variant performs best on dense enough problems, e.g., 3D FEM, or very high order 2D FEM. - * The sparse matrix A must be selfajoint and positive definite. The vectors or matrices + * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices * X and B can be either dense or sparse. * * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> @@ -501,7 +504,7 @@ class CholmodSupernodalLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSuper * \brief A general Cholesky factorization and solver based on Cholmod * * This class allows to solve for A.X = B sparse linear problems via a LL^T or LDL^T Cholesky factorization - * using the Cholmod library. The sparse matrix A must be selfajoint and positive definite. The vectors or matrices + * using the Cholmod library. The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices * X and B can be either dense or sparse. * * This variant permits to change the underlying Cholesky method at runtime. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h index 497efff66..0ab03eff0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h @@ -210,7 +210,7 @@ class Array : Base(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols()) { Base::_check_template_params(); - Base::resize(other.rows(), other.cols()); + Base::_resize_to_match(other); *this = other; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h index 6e37e031a..be9f48a8c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h @@ -29,9 +29,9 @@ struct all_unroller }; template -struct all_unroller +struct all_unroller { - static inline bool run(const Derived &mat) { return mat.coeff(0, 0); } + static inline bool run(const Derived &/*mat*/) { return true; } }; template @@ -55,9 +55,9 @@ struct any_unroller }; template -struct any_unroller +struct any_unroller { - static inline bool run(const Derived &mat) { return mat.coeff(0, 0); } + static inline bool run(const Derived & /*mat*/) { return false; } }; template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h index 2b8dd1b70..fadb45852 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h @@ -126,36 +126,6 @@ Derived& DenseBase::operator-=(const EigenBase &other) return derived(); } -/** replaces \c *this by \c *this * \a other. - * - * \returns a reference to \c *this - */ -template -template -inline Derived& -MatrixBase::operator*=(const EigenBase &other) -{ - other.derived().applyThisOnTheRight(derived()); - return derived(); -} - -/** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=(). - */ -template -template -inline void MatrixBase::applyOnTheRight(const EigenBase &other) -{ - other.derived().applyThisOnTheRight(derived()); -} - -/** replaces \c *this by \c *this * \a other. */ -template -template -inline void MatrixBase::applyOnTheLeft(const EigenBase &other) -{ - other.derived().applyThisOnTheLeft(derived()); -} - } // end namespace Eigen #endif // EIGEN_EIGENBASE_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h index c8d5f6379..8d4bc59e9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h @@ -185,21 +185,22 @@ std::ostream & print_matrix(std::ostream & s, const Derived& _m, const IOFormat& explicit_precision = fmt.precision; } + std::streamsize old_precision = 0; + if(explicit_precision) old_precision = s.precision(explicit_precision); + bool align_cols = !(fmt.flags & DontAlignCols); if(align_cols) { // compute the largest width - for(Index j = 1; j < m.cols(); ++j) + for(Index j = 0; j < m.cols(); ++j) for(Index i = 0; i < m.rows(); ++i) { std::stringstream sstr; - if(explicit_precision) sstr.precision(explicit_precision); + sstr.copyfmt(s); sstr << m.coeff(i,j); width = std::max(width, Index(sstr.str().length())); } } - std::streamsize old_precision = 0; - if(explicit_precision) old_precision = s.precision(explicit_precision); s << fmt.matPrefix; for(Index i = 0; i < m.rows(); ++i) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h index 0ba5d90cc..d7d0b5b9a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h @@ -304,7 +304,7 @@ class Matrix : Base(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols()) { Base::_check_template_params(); - Base::resize(other.rows(), other.cols()); + Base::_resize_to_match(other); // FIXME/CHECK: isn't *this = other.derived() more efficient. it allows to // go for pure _set() implementations, right? *this = other; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h index 9193b6abb..344b38f2f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h @@ -510,6 +510,51 @@ template class MatrixBase {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;} }; + +/*************************************************************************** +* Implementation of matrix base methods +***************************************************************************/ + +/** replaces \c *this by \c *this * \a other. + * + * \returns a reference to \c *this + * + * Example: \include MatrixBase_applyOnTheRight.cpp + * Output: \verbinclude MatrixBase_applyOnTheRight.out + */ +template +template +inline Derived& +MatrixBase::operator*=(const EigenBase &other) +{ + other.derived().applyThisOnTheRight(derived()); + return derived(); +} + +/** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=(). + * + * Example: \include MatrixBase_applyOnTheRight.cpp + * Output: \verbinclude MatrixBase_applyOnTheRight.out + */ +template +template +inline void MatrixBase::applyOnTheRight(const EigenBase &other) +{ + other.derived().applyThisOnTheRight(derived()); +} + +/** replaces \c *this by \a other * \c *this. + * + * Example: \include MatrixBase_applyOnTheLeft.cpp + * Output: \verbinclude MatrixBase_applyOnTheLeft.out + */ +template +template +inline void MatrixBase::applyOnTheLeft(const EigenBase &other) +{ + other.derived().applyThisOnTheLeft(derived()); +} + } // end namespace Eigen #endif // EIGEN_MATRIXBASE_H diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h index 4fc5dd318..1297b8413 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h @@ -553,7 +553,8 @@ struct permut_matrix_product_retval template inline void evalTo(Dest& dst) const { const Index n = Side==OnTheLeft ? rows() : cols(); - + // FIXME we need an is_same for expression that is not sensitive to constness. For instance + // is_same_xpr, Block >::value should be true. if(is_same::value && extract_data(dst) == extract_data(m_matrix)) { // apply the permutation inplace diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h index af0a479c7..dd34b59e5 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h @@ -47,7 +47,10 @@ template<> struct check_rows_cols_for_overflow { } }; -template struct conservative_resize_like_impl; +template +struct conservative_resize_like_impl; template struct matrix_swap_impl; @@ -668,8 +671,10 @@ private: enum { ThisConstantIsPrivateInPlainObjectBase }; }; +namespace internal { + template -struct internal::conservative_resize_like_impl +struct conservative_resize_like_impl { typedef typename Derived::Index Index; static void run(DenseBase& _this, Index rows, Index cols) @@ -729,11 +734,14 @@ struct internal::conservative_resize_like_impl } }; -namespace internal { - +// Here, the specialization for vectors inherits from the general matrix case +// to allow calling .conservativeResize(rows,cols) on vectors. template struct conservative_resize_like_impl + : conservative_resize_like_impl { + using conservative_resize_like_impl::run; + typedef typename Derived::Index Index; static void run(DenseBase& _this, Index size) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h index aba795bdb..00d9e6d2b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h @@ -94,7 +94,8 @@ struct traits > typedef _PlainObjectType PlainObjectType; typedef _StrideType StrideType; enum { - Options = _Options + Options = _Options, + Flags = traits >::Flags | NestByRefBit }; template struct match { @@ -111,7 +112,7 @@ struct traits > }; typedef typename internal::conditional::type type; }; - + }; template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h index c83e955ee..389d94275 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h @@ -17,16 +17,29 @@ namespace internal { template inline void stable_norm_kernel(const ExpressionType& bl, Scalar& ssq, Scalar& scale, Scalar& invScale) { - Scalar max = bl.cwiseAbs().maxCoeff(); - if (max>scale) + using std::max; + Scalar maxCoeff = bl.cwiseAbs().maxCoeff(); + + if (maxCoeff>scale) { - ssq = ssq * numext::abs2(scale/max); - scale = max; - invScale = Scalar(1)/scale; + ssq = ssq * numext::abs2(scale/maxCoeff); + Scalar tmp = Scalar(1)/maxCoeff; + if(tmp > NumTraits::highest()) + { + invScale = NumTraits::highest(); + scale = Scalar(1)/invScale; + } + else + { + scale = maxCoeff; + invScale = tmp; + } } - // TODO if the max is much much smaller than the current scale, + + // TODO if the maxCoeff is much much smaller than the current scale, // then we can neglect this sub vector - ssq += (bl*invScale).squaredNorm(); + if(scale>Scalar(0)) // if scale==0, then bl is 0 + ssq += (bl*invScale).squaredNorm(); } template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h index f21b3aa65..22096ea2f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h @@ -284,7 +284,8 @@ struct inplace_transpose_selector { // non square matrix * Notice however that this method is only useful if you want to replace a matrix by its own transpose. * If you just need the transpose of a matrix, use transpose(). * - * \note if the matrix is not square, then \c *this must be a resizable matrix. + * \note if the matrix is not square, then \c *this must be a resizable matrix. + * This excludes (non-square) fixed-size matrices, block-expressions and maps. * * \sa transpose(), adjoint(), adjointInPlace() */ template @@ -315,6 +316,7 @@ inline void DenseBase::transposeInPlace() * If you just need the adjoint of a matrix, use adjoint(). * * \note if the matrix is not square, then \c *this must be a resizable matrix. + * This excludes (non-square) fixed-size matrices, block-expressions and maps. * * \sa transpose(), adjoint(), transposeInPlace() */ template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h index 511564875..d5ab03664 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h @@ -50,7 +50,7 @@ struct traits > MaxColsAtCompileTime = Direction==Horizontal ? 1 : MatrixType::MaxColsAtCompileTime, Flags0 = (unsigned int)_MatrixTypeNested::Flags & HereditaryBits, Flags = (Flags0 & ~RowMajorBit) | (RowsAtCompileTime == 1 ? RowMajorBit : 0), - TraversalSize = Direction==Vertical ? RowsAtCompileTime : ColsAtCompileTime + TraversalSize = Direction==Vertical ? MatrixType::RowsAtCompileTime : MatrixType::ColsAtCompileTime }; #if EIGEN_GNUC_AT_LEAST(3,4) typedef typename MemberOp::template Cost CostOpType; @@ -58,7 +58,8 @@ struct traits > typedef typename MemberOp::template Cost CostOpType; #endif enum { - CoeffReadCost = TraversalSize * traits<_MatrixTypeNested>::CoeffReadCost + int(CostOpType::value) + CoeffReadCost = TraversalSize==Dynamic ? Dynamic + : TraversalSize * traits<_MatrixTypeNested>::CoeffReadCost + int(CostOpType::value) }; }; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h index 3376a984e..99cbd0d95 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h @@ -442,8 +442,11 @@ Packet4f pcos(const Packet4f& _x) return _mm_xor_ps(y, sign_bit); } +#if EIGEN_FAST_MATH + // This is based on Quake3's fast inverse square root. // For detail see here: http://www.beyond3d.com/content/articles/8/ +// It lacks 1 (or 2 bits in some rare cases) of precision, and does not handle negative, +inf, or denormalized numbers correctly. template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f psqrt(const Packet4f& _x) { @@ -457,6 +460,14 @@ Packet4f psqrt(const Packet4f& _x) return pmul(_x,x); } +#else + +template<> EIGEN_STRONG_INLINE Packet4f psqrt(const Packet4f& x) { return _mm_sqrt_ps(x); } + +#endif + +template<> EIGEN_STRONG_INLINE Packet2d psqrt(const Packet2d& x) { return _mm_sqrt_pd(x); } + } // end namespace internal } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h index e256f4bac..fc8ae50fe 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h @@ -83,7 +83,8 @@ template<> struct packet_traits : default_packet_traits size=2, HasDiv = 1, - HasExp = 1 + HasExp = 1, + HasSqrt = 1 }; }; template<> struct packet_traits : default_packet_traits @@ -507,8 +508,8 @@ template<> EIGEN_STRONG_INLINE int predux_min(const Packet4i& a) // for GCC (eg., it does not like using std::min after the pstore !!) EIGEN_ALIGN16 int aux[4]; pstore(aux, a); - register int aux0 = aux[0] EIGEN_STRONG_INLINE int predux_max(const Packet4i& a) // for GCC (eg., it does not like using std::min after the pstore !!) EIGEN_ALIGN16 int aux[4]; pstore(aux, a); - register int aux0 = aux[0]>aux[1] ? aux[0] : aux[1]; - register int aux2 = aux[2]>aux[3] ? aux[2] : aux[3]; + int aux0 = aux[0]>aux[1] ? aux[0] : aux[1]; + int aux2 = aux[2]>aux[3] ? aux[2] : aux[3]; return aux0>aux2 ? aux0 : aux2; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h index 780fa74d3..bcdca5b0d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h @@ -1128,6 +1128,8 @@ EIGEN_DONT_INLINE void gemm_pack_lhs::size }; EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK LHS"); + EIGEN_UNUSED_VARIABLE(stride) + EIGEN_UNUSED_VARIABLE(offset) eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride)); eigen_assert( (StorageOrder==RowMajor) || ((Pack1%PacketSize)==0 && Pack1<=4*PacketSize) ); conj_if::IsComplex && Conjugate> cj; @@ -1215,6 +1217,8 @@ EIGEN_DONT_INLINE void gemm_pack_rhs=depth && offset<=stride)); conj_if::IsComplex && Conjugate> cj; Index packet_cols = (cols/nr) * nr; @@ -1266,6 +1270,8 @@ EIGEN_DONT_INLINE void gemm_pack_rhs=depth && offset<=stride)); conj_if::IsComplex && Conjugate> cj; Index packet_cols = (cols/nr) * nr; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h index c1cb78498..09387703e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h @@ -52,11 +52,7 @@ EIGEN_DONT_INLINE static void run( Index rows, Index cols, const LhsScalar* lhs, Index lhsStride, const RhsScalar* rhs, Index rhsIncr, - ResScalar* res, Index - #ifdef EIGEN_INTERNAL_DEBUGGING - resIncr - #endif - , RhsScalar alpha); + ResScalar* res, Index resIncr, RhsScalar alpha); }; template @@ -64,12 +60,9 @@ EIGEN_DONT_INLINE void general_matrix_vector_product(&lhs0[i]), ptmp0, pload(&res[i]))); + pstore(&res[i], pcj.pmadd(pload(&lhs0[i]), ptmp0, pload(&res[i]))); else for (Index i = alignedStart;i(&lhs0[i]), ptmp0, pload(&res[i]))); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h index c40e80f53..f698f67f9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h @@ -79,8 +79,8 @@ EIGEN_DONT_INLINE void selfadjoint_matrix_vector_product(t0); @@ -147,7 +147,7 @@ EIGEN_DONT_INLINE void selfadjoint_matrix_vector_productx || (EIGEN_WORLD_VERSION>=x && \ (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \ @@ -238,7 +238,12 @@ #endif // Suppresses 'unused variable' warnings. -#define EIGEN_UNUSED_VARIABLE(var) (void)var; +namespace Eigen { + namespace internal { + template void ignore_unused_variable(const T&) {} + } +} +#define EIGEN_UNUSED_VARIABLE(var) Eigen::internal::ignore_unused_variable(var); #if !defined(EIGEN_ASM_COMMENT) #if (defined __GNUC__) && ( defined(__i386__) || defined(__x86_64__) ) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h index 451535a0c..cacbd02fd 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h @@ -578,7 +578,7 @@ template class aligned_stack_memory_handler */ #ifdef EIGEN_ALLOCA - #ifdef __arm__ + #if defined(__arm__) || defined(_WIN32) #define EIGEN_ALIGNED_ALLOCA(SIZE) reinterpret_cast((reinterpret_cast(EIGEN_ALLOCA(SIZE+16)) & ~(size_t(15))) + 16) #else #define EIGEN_ALIGNED_ALLOCA EIGEN_ALLOCA @@ -634,7 +634,9 @@ template class aligned_stack_memory_handler /* memory allocated we can safely let the default implementation handle */ \ /* this particular case. */ \ static void *operator new(size_t size, void *ptr) { return ::operator new(size,ptr); } \ + static void *operator new[](size_t size, void* ptr) { return ::operator new[](size,ptr); } \ void operator delete(void * memory, void *ptr) throw() { return ::operator delete(memory,ptr); } \ + void operator delete[](void * memory, void *ptr) throw() { return ::operator delete[](memory,ptr); } \ /* nothrow-new (returns zero instead of std::bad_alloc) */ \ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \ void operator delete(void *ptr, const std::nothrow_t&) throw() { \ @@ -729,15 +731,6 @@ public: ::new( p ) T( value ); } - // Support for c++11 -#if (__cplusplus >= 201103L) - template - void construct(pointer p, Args&&... args) - { - ::new(p) T(std::forward(args)...); - } -#endif - void destroy( pointer p ) { p->~T(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/SVD.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/SVD.h index 077d26d54..3d03d2288 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/SVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/SVD.h @@ -512,8 +512,7 @@ template template bool SVD::solve(const MatrixBase &b, ResultType* result) const { - const int rows = m_matU.rows(); - ei_assert(b.rows() == rows); + ei_assert(b.rows() == m_matU.rows()); Scalar maxVal = m_sigma.cwise().abs().maxCoeff(); for (int j=0; j conjugate() const; - /** \returns an interpolation for a constant motion between \a other and \c *this - * \a t in [0;1] - * see http://en.wikipedia.org/wiki/Slerp - */ template Quaternion slerp(const Scalar& t, const QuaternionBase& other) const; /** \returns \c true if \c *this is approximately equal to \a other, within the precision @@ -194,11 +190,11 @@ public: * \brief The quaternion class used to represent 3D orientations and rotations * * \tparam _Scalar the scalar type, i.e., the type of the coefficients - * \tparam _Options controls the memory alignement of the coeffecients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign. + * \tparam _Options controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign. * * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of * orientations and rotations of objects in three dimensions. Compared to other representations - * like Euler angles or 3x3 matrices, quatertions offer the following advantages: + * like Euler angles or 3x3 matrices, quaternions offer the following advantages: * \li \b compact storage (4 scalars) * \li \b efficient to compose (28 flops), * \li \b stable spherical interpolation @@ -385,7 +381,7 @@ class Map, _Options > /** Constructs a Mapped Quaternion object from the pointer \a coeffs * - * The pointer \a coeffs must reference the four coeffecients of Quaternion in the following order: + * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order: * \code *coeffs == {x, y, z, w} \endcode * * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */ @@ -399,16 +395,16 @@ class Map, _Options > }; /** \ingroup Geometry_Module - * Map an unaligned array of single precision scalar as a quaternion */ + * Map an unaligned array of single precision scalars as a quaternion */ typedef Map, 0> QuaternionMapf; /** \ingroup Geometry_Module - * Map an unaligned array of double precision scalar as a quaternion */ + * Map an unaligned array of double precision scalars as a quaternion */ typedef Map, 0> QuaternionMapd; /** \ingroup Geometry_Module - * Map a 16-bits aligned array of double precision scalars as a quaternion */ + * Map a 16-byte aligned array of single precision scalars as a quaternion */ typedef Map, Aligned> QuaternionMapAlignedf; /** \ingroup Geometry_Module - * Map a 16-bits aligned array of double precision scalars as a quaternion */ + * Map a 16-byte aligned array of double precision scalars as a quaternion */ typedef Map, Aligned> QuaternionMapAlignedd; /*************************************************************************** @@ -579,7 +575,7 @@ inline Derived& QuaternionBase::setFromTwoVectors(const MatrixBase accuraletly compute the rotation axis by computing the + // => accurately compute the rotation axis by computing the // intersection of the two planes. This is done by solving: // x^T v0 = 0 // x^T v1 = 0 @@ -677,8 +673,13 @@ QuaternionBase::angularDistance(const QuaternionBase& oth return static_cast(2 * acos(d)); } + + /** \returns the spherical linear interpolation between the two quaternions - * \c *this and \a other at the parameter \a t + * \c *this and \a other at the parameter \a t in [0;1]. + * + * This represents an interpolation for a constant motion between \c *this and \a other, + * see also http://en.wikipedia.org/wiki/Slerp. */ template template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h index 887e718d6..498fea41a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h @@ -530,9 +530,9 @@ public: inline Transform& operator=(const UniformScaling& t); inline Transform& operator*=(const UniformScaling& s) { return scale(s.factor()); } - inline Transform operator*(const UniformScaling& s) const + inline Transform operator*(const UniformScaling& s) const { - Transform res = *this; + Transform res = *this; res.scale(s.factor()); return res; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h index 8b01f8179..bec85810c 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h @@ -349,7 +349,7 @@ template class ColPivHouseholderQR return m_usePrescribedThreshold ? m_prescribedThreshold // this formula comes from experimenting (see "LU precision tuning" thread on the list) // and turns out to be identical to Higham's formula used already in LDLt. - : NumTraits::epsilon() * m_qr.diagonalSize(); + : NumTraits::epsilon() * RealScalar(m_qr.diagonalSize()); } /** \returns the number of nonzero pivots in the QR decomposition. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h index 0dd5ad347..6168e7abf 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h @@ -63,9 +63,10 @@ template class FullPivHouseholderQR typedef typename MatrixType::Index Index; typedef internal::FullPivHouseholderQRMatrixQReturnType MatrixQReturnType; typedef typename internal::plain_diag_type::type HCoeffsType; - typedef Matrix IntRowVectorType; + typedef Matrix IntDiagSizeVectorType; typedef PermutationMatrix PermutationType; - typedef typename internal::plain_col_type::type IntColVectorType; typedef typename internal::plain_row_type::type RowVectorType; typedef typename internal::plain_col_type::type ColVectorType; @@ -93,10 +94,10 @@ template class FullPivHouseholderQR FullPivHouseholderQR(Index rows, Index cols) : m_qr(rows, cols), m_hCoeffs((std::min)(rows,cols)), - m_rows_transpositions(rows), - m_cols_transpositions(cols), + m_rows_transpositions((std::min)(rows,cols)), + m_cols_transpositions((std::min)(rows,cols)), m_cols_permutation(cols), - m_temp((std::min)(rows,cols)), + m_temp(cols), m_isInitialized(false), m_usePrescribedThreshold(false) {} @@ -115,10 +116,10 @@ template class FullPivHouseholderQR FullPivHouseholderQR(const MatrixType& matrix) : m_qr(matrix.rows(), matrix.cols()), m_hCoeffs((std::min)(matrix.rows(), matrix.cols())), - m_rows_transpositions(matrix.rows()), - m_cols_transpositions(matrix.cols()), + m_rows_transpositions((std::min)(matrix.rows(), matrix.cols())), + m_cols_transpositions((std::min)(matrix.rows(), matrix.cols())), m_cols_permutation(matrix.cols()), - m_temp((std::min)(matrix.rows(), matrix.cols())), + m_temp(matrix.cols()), m_isInitialized(false), m_usePrescribedThreshold(false) { @@ -126,11 +127,12 @@ template class FullPivHouseholderQR } /** This method finds a solution x to the equation Ax=b, where A is the matrix of which - * *this is the QR decomposition, if any exists. + * \c *this is the QR decomposition. * * \param b the right-hand-side of the equation to solve. * - * \returns a solution. + * \returns the exact or least-square solution if the rank is greater or equal to the number of columns of A, + * and an arbitrary solution otherwise. * * \note The case where b is a matrix is not yet implemented. Also, this * code is space inefficient. @@ -172,7 +174,7 @@ template class FullPivHouseholderQR } /** \returns a const reference to the vector of indices representing the rows transpositions */ - const IntColVectorType& rowsTranspositions() const + const IntDiagSizeVectorType& rowsTranspositions() const { eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized."); return m_rows_transpositions; @@ -344,7 +346,7 @@ template class FullPivHouseholderQR return m_usePrescribedThreshold ? m_prescribedThreshold // this formula comes from experimenting (see "LU precision tuning" thread on the list) // and turns out to be identical to Higham's formula used already in LDLt. - : NumTraits::epsilon() * m_qr.diagonalSize(); + : NumTraits::epsilon() * RealScalar(m_qr.diagonalSize()); } /** \returns the number of nonzero pivots in the QR decomposition. @@ -368,8 +370,8 @@ template class FullPivHouseholderQR protected: MatrixType m_qr; HCoeffsType m_hCoeffs; - IntColVectorType m_rows_transpositions; - IntRowVectorType m_cols_transpositions; + IntDiagSizeVectorType m_rows_transpositions; + IntDiagSizeVectorType m_cols_transpositions; PermutationType m_cols_permutation; RowVectorType m_temp; bool m_isInitialized, m_usePrescribedThreshold; @@ -415,10 +417,10 @@ FullPivHouseholderQR& FullPivHouseholderQR::compute(cons m_temp.resize(cols); - m_precision = NumTraits::epsilon() * size; + m_precision = NumTraits::epsilon() * RealScalar(size); - m_rows_transpositions.resize(matrix.rows()); - m_cols_transpositions.resize(matrix.cols()); + m_rows_transpositions.resize(size); + m_cols_transpositions.resize(size); Index number_of_transpositions = 0; RealScalar biggest(0); @@ -516,17 +518,6 @@ struct solve_retval, Rhs> dec().hCoeffs().coeff(k), &temp.coeffRef(0)); } - if(!dec().isSurjective()) - { - // is c is in the image of R ? - RealScalar biggest_in_upper_part_of_c = c.topRows( dec().rank() ).cwiseAbs().maxCoeff(); - RealScalar biggest_in_lower_part_of_c = c.bottomRows(rows-dec().rank()).cwiseAbs().maxCoeff(); - // FIXME brain dead - const RealScalar m_precision = NumTraits::epsilon() * (std::min)(rows,cols); - // this internal:: prefix is needed by at least gcc 3.4 and ICC - if(!internal::isMuchSmallerThan(biggest_in_lower_part_of_c, biggest_in_upper_part_of_c, m_precision)) - return; - } dec().matrixQR() .topLeftCorner(dec().rank(), dec().rank()) .template triangularView() @@ -548,14 +539,14 @@ template struct FullPivHouseholderQRMatrixQReturnType { public: typedef typename MatrixType::Index Index; - typedef typename internal::plain_col_type::type IntColVectorType; + typedef typename FullPivHouseholderQR::IntDiagSizeVectorType IntDiagSizeVectorType; typedef typename internal::plain_diag_type::type HCoeffsType; typedef Matrix WorkVectorType; FullPivHouseholderQRMatrixQReturnType(const MatrixType& qr, const HCoeffsType& hCoeffs, - const IntColVectorType& rowsTranspositions) + const IntDiagSizeVectorType& rowsTranspositions) : m_qr(qr), m_hCoeffs(hCoeffs), m_rowsTranspositions(rowsTranspositions) @@ -595,7 +586,7 @@ public: protected: typename MatrixType::Nested m_qr; typename HCoeffsType::Nested m_hCoeffs; - typename IntColVectorType::Nested m_rowsTranspositions; + typename IntDiagSizeVectorType::Nested m_rowsTranspositions; }; } // end namespace internal diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h index aa41f434c..a2cc2a9e2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h @@ -64,7 +64,8 @@ class SPQR typedef PermutationMatrix PermutationType; public: SPQR() - : m_ordering(SPQR_ORDERING_DEFAULT), + : m_isInitialized(false), + m_ordering(SPQR_ORDERING_DEFAULT), m_allow_tol(SPQR_DEFAULT_TOL), m_tolerance (NumTraits::epsilon()) { @@ -72,7 +73,8 @@ class SPQR } SPQR(const _MatrixType& matrix) - : m_ordering(SPQR_ORDERING_DEFAULT), + : m_isInitialized(false), + m_ordering(SPQR_ORDERING_DEFAULT), m_allow_tol(SPQR_DEFAULT_TOL), m_tolerance (NumTraits::epsilon()) { @@ -82,16 +84,22 @@ class SPQR ~SPQR() { - // Calls SuiteSparseQR_free() + SPQR_free(); + cholmod_l_finish(&m_cc); + } + void SPQR_free() + { cholmod_l_free_sparse(&m_H, &m_cc); cholmod_l_free_sparse(&m_cR, &m_cc); cholmod_l_free_dense(&m_HTau, &m_cc); std::free(m_E); std::free(m_HPinv); - cholmod_l_finish(&m_cc); } + void compute(const _MatrixType& matrix) { + if(m_isInitialized) SPQR_free(); + MatrixType mat(matrix); cholmod_sparse A; A = viewAsCholmod(mat); @@ -139,7 +147,7 @@ class SPQR eigen_assert(b.cols()==1 && "This method is for vectors only"); //Compute Q^T * b - Dest y; + typename Dest::PlainObject y; y = matrixQ().transpose() * b; // Solves with the triangular matrix R Index rk = this->rank(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h index 4786768ff..f44995cd3 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h @@ -380,7 +380,10 @@ struct svd_precondition_2x2_block_to_be_real z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q); work_matrix.row(p) *= z; if(svd.computeU()) svd.m_matrixU.col(p) *= conj(z); - z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q); + if(work_matrix.coeff(q,q)!=Scalar(0)) + z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q); + else + z = Scalar(0); work_matrix.row(q) *= z; if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h index 4b13f08d4..5c320e2d2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h @@ -66,9 +66,9 @@ static void conservative_sparse_sparse_product_impl(const Lhs& lhs, const Rhs& r } // unordered insertion - for(int k=0; k1) std::sort(indices.data(),indices.data()+nnz); - for(int k=0; k RowMajorMatrix; - typedef SparseMatrix ColMajorMatrix; + typedef SparseMatrix RowMajorMatrix; + typedef SparseMatrix ColMajorMatrix; ColMajorMatrix resCol(lhs.rows(),rhs.cols()); internal::conservative_sparse_sparse_product_impl(lhs, rhs, resCol); // sort the non zeros: @@ -149,7 +149,7 @@ struct conservative_sparse_sparse_product_selector RowMajorMatrix; + typedef SparseMatrix RowMajorMatrix; RowMajorMatrix rhsRow = rhs; RowMajorMatrix resRow(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(rhsRow, lhs, resRow); @@ -162,7 +162,7 @@ struct conservative_sparse_sparse_product_selector RowMajorMatrix; + typedef SparseMatrix RowMajorMatrix; RowMajorMatrix lhsRow = lhs; RowMajorMatrix resRow(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(rhs, lhsRow, resRow); @@ -175,7 +175,7 @@ struct conservative_sparse_sparse_product_selector RowMajorMatrix; + typedef SparseMatrix RowMajorMatrix; RowMajorMatrix resRow(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(rhs, lhs, resRow); res = resRow; @@ -190,7 +190,7 @@ struct conservative_sparse_sparse_product_selector ColMajorMatrix; + typedef SparseMatrix ColMajorMatrix; ColMajorMatrix resCol(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(lhs, rhs, resCol); res = resCol; @@ -202,7 +202,7 @@ struct conservative_sparse_sparse_product_selector ColMajorMatrix; + typedef SparseMatrix ColMajorMatrix; ColMajorMatrix lhsCol = lhs; ColMajorMatrix resCol(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(lhsCol, rhs, resCol); @@ -215,7 +215,7 @@ struct conservative_sparse_sparse_product_selector ColMajorMatrix; + typedef SparseMatrix ColMajorMatrix; ColMajorMatrix rhsCol = rhs; ColMajorMatrix resCol(lhs.rows(), rhs.cols()); internal::conservative_sparse_sparse_product_impl(lhs, rhsCol, resCol); @@ -228,8 +228,8 @@ struct conservative_sparse_sparse_product_selector RowMajorMatrix; - typedef SparseMatrix ColMajorMatrix; + typedef SparseMatrix RowMajorMatrix; + typedef SparseMatrix ColMajorMatrix; RowMajorMatrix resRow(lhs.rows(),rhs.cols()); internal::conservative_sparse_sparse_product_impl(rhs, lhs, resRow); // sort the non zeros: diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h index 93cd4832d..ab1a266a9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h @@ -50,6 +50,8 @@ class MappedSparseMatrix inline Index cols() const { return IsRowMajor ? m_innerSize : m_outerSize; } inline Index innerSize() const { return m_innerSize; } inline Index outerSize() const { return m_outerSize; } + + bool isCompressed() const { return true; } //---------------------------------------- // direct access interface diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h index 0b3e193db..16a20a574 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h @@ -66,6 +66,8 @@ public: typename XprType::Nested m_matrix; Index m_outerStart; const internal::variable_if_dynamic m_outerSize; + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) }; @@ -391,6 +393,8 @@ public: protected: friend class InnerIterator; friend class ReverseInnerIterator; + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) typename XprType::Nested m_matrix; const internal::variable_if_dynamic m_startRow; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseColEtree.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseColEtree.h index f89ca3814..f8745f461 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseColEtree.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseColEtree.h @@ -63,6 +63,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl typedef typename MatrixType::Index Index; Index nc = mat.cols(); // Number of columns Index m = mat.rows(); + Index diagSize = (std::min)(nc,m); IndexVector root(nc); // root of subtree of etree root.setZero(); IndexVector pp(nc); // disjoint sets @@ -72,7 +73,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl Index row,col; firstRowElt.resize(m); firstRowElt.setConstant(nc); - firstRowElt.segment(0, nc).setLinSpaced(nc, 0, nc-1); + firstRowElt.segment(0, diagSize).setLinSpaced(diagSize, 0, diagSize-1); bool found_diag; for (col = 0; col < nc; col++) { @@ -91,7 +92,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl Index rset, cset, rroot; for (col = 0; col < nc; col++) { - found_diag = false; + found_diag = col>=m; pp(col) = col; cset = col; root(cset) = col; @@ -105,6 +106,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl Index i = col; if(it) i = it.index(); if (i == col) found_diag = true; + row = firstRowElt(i); if (row >= col) continue; rset = internal::etree_find(row, pp); // Find the name of the set containing row diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h index 30975c29c..54fd633a1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h @@ -125,7 +125,7 @@ class SparseDenseOuterProduct::InnerIterator : public _LhsNes inline Scalar value() const { return Base::value() * m_factor; } protected: - int m_outer; + Index m_outer; Scalar m_factor; }; @@ -155,7 +155,7 @@ struct sparse_time_dense_product_impl::Constant(outerSize(), 2)); } return insertUncompressed(row,col); } @@ -402,7 +402,7 @@ class SparseMatrix * \sa insertBack, insertBackByOuterInner */ inline void startVec(Index outer) { - eigen_assert(m_outerIndex[outer]==int(m_data.size()) && "You must call startVec for each inner vector sequentially"); + eigen_assert(m_outerIndex[outer]==Index(m_data.size()) && "You must call startVec for each inner vector sequentially"); eigen_assert(m_outerIndex[outer+1]==0 && "You must call startVec for each inner vector sequentially"); m_outerIndex[outer+1] = m_outerIndex[outer]; } @@ -480,7 +480,7 @@ class SparseMatrix if(m_innerNonZeros != 0) return; m_innerNonZeros = static_cast(std::malloc(m_outerSize * sizeof(Index))); - for (int i = 0; i < m_outerSize; i++) + for (Index i = 0; i < m_outerSize; i++) { m_innerNonZeros[i] = m_outerIndex[i+1] - m_outerIndex[i]; } @@ -752,8 +752,8 @@ class SparseMatrix else for (Index i=0; i trMat(mat.rows(),mat.cols()); - if(begin wi(trMat.outerSize()); wi.setZero(); for(InputIterator it(begin); it!=end; ++it) { @@ -1018,11 +1019,11 @@ void SparseMatrix::sumupDuplicates() { eigen_assert(!isCompressed()); // TODO, in practice we should be able to use m_innerNonZeros for that task - VectorXi wi(innerSize()); + Matrix wi(innerSize()); wi.fill(-1); Index count = 0; // for each inner-vector, wi[inner_index] will hold the position of first element into the index/value buffers - for(int j=0; j& SparseMatrix positions(dest.outerSize()); for (Index j=0; j class SparseMatrixBase : public EigenBase } else { - SparseMatrix trans = m; - s << static_cast >&>(trans); + SparseMatrix trans = m; + s << static_cast >&>(trans); } } return s; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h index b897b7595..b85be93f6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h @@ -57,7 +57,7 @@ struct permut_sparsematrix_product_retval if(MoveOuter) { SparseMatrix tmp(m_matrix.rows(), m_matrix.cols()); - VectorXi sizes(m_matrix.outerSize()); + Matrix sizes(m_matrix.outerSize()); for(Index j=0; j tmp(m_matrix.rows(), m_matrix.cols()); - VectorXi sizes(tmp.outerSize()); + Matrix sizes(tmp.outerSize()); sizes.setZero(); PermutationMatrix perm; if((Side==OnTheLeft) ^ Transposed) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseProduct.h index 70b6480ef..cf7663070 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseProduct.h @@ -16,6 +16,7 @@ template struct SparseSparseProductReturnType { typedef typename internal::traits::Scalar Scalar; + typedef typename internal::traits::Index Index; enum { LhsRowMajor = internal::traits::Flags & RowMajorBit, RhsRowMajor = internal::traits::Flags & RowMajorBit, @@ -24,11 +25,11 @@ struct SparseSparseProductReturnType }; typedef typename internal::conditional, + SparseMatrix, typename internal::nested::type>::type LhsNested; typedef typename internal::conditional, + SparseMatrix, typename internal::nested::type>::type RhsNested; typedef SparseSparseProduct Type; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h index 70857c7b6..fcc18f5c9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h @@ -27,7 +27,7 @@ static void sparse_sparse_product_with_pruning_impl(const Lhs& lhs, const Rhs& r // make sure to call innerSize/outerSize since we fake the storage order. Index rows = lhs.innerSize(); Index cols = rhs.outerSize(); - //int size = lhs.outerSize(); + //Index size = lhs.outerSize(); eigen_assert(lhs.outerSize() == rhs.innerSize()); // allocate a temporary buffer @@ -100,7 +100,7 @@ struct sparse_sparse_product_with_pruning_selector SparseTemporaryType; + typedef SparseMatrix SparseTemporaryType; SparseTemporaryType _res(res.rows(), res.cols()); internal::sparse_sparse_product_with_pruning_impl(lhs, rhs, _res, tolerance); res = _res; @@ -126,10 +126,11 @@ struct sparse_sparse_product_with_pruning_selector ColMajorMatrix; - ColMajorMatrix colLhs(lhs); - ColMajorMatrix colRhs(rhs); - internal::sparse_sparse_product_with_pruning_impl(colLhs, colRhs, res, tolerance); + typedef SparseMatrix ColMajorMatrixLhs; + typedef SparseMatrix ColMajorMatrixRhs; + ColMajorMatrixLhs colLhs(lhs); + ColMajorMatrixRhs colRhs(rhs); + internal::sparse_sparse_product_with_pruning_impl(colLhs, colRhs, res, tolerance); // let's transpose the product to get a column x column product // typedef SparseMatrix SparseTemporaryType; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h index 064a40707..05023858b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h @@ -143,7 +143,7 @@ template struct plain_matrix_type * * \sa SparseMatrix::setFromTriplets() */ -template +template::Index > class Triplet { public: diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h index dd9eab2c2..1d592f2c8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h @@ -219,9 +219,9 @@ class SparseLU : public internal::SparseLUImpl - bool _solve(const MatrixBase &B, MatrixBase &_X) const + bool _solve(const MatrixBase &B, MatrixBase &X_base) const { - Dest& X(_X.derived()); + Dest& X(X_base.derived()); eigen_assert(m_factorizationIsOk && "The matrix should be factorized first"); EIGEN_STATIC_ASSERT((Dest::Flags&RowMajorBit)==0, THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES); @@ -229,8 +229,10 @@ class SparseLU : public internal::SparseLUImplmatrixL().solveInPlace(X); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h index a5158025c..1ffa7d54e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h @@ -70,23 +70,30 @@ Index SparseLUImpl::expand(VectorType& vec, Index& length, Index if(num_expansions == 0 || keep_prev) new_len = length ; // First time allocate requested else - new_len = Index(alpha * length); + new_len = (std::max)(length+1,Index(alpha * length)); VectorType old_vec; // Temporary vector to hold the previous values if (nbElts > 0 ) old_vec = vec.segment(0,nbElts); //Allocate or expand the current vector - try +#ifdef EIGEN_EXCEPTIONS + try +#endif { vec.resize(new_len); } +#ifdef EIGEN_EXCEPTIONS catch(std::bad_alloc& ) +#else + if(!vec.size()) +#endif { - if ( !num_expansions ) + if (!num_expansions) { // First time to allocate from LUMemInit() - throw; // Pass the exception to LUMemInit() which has a try... catch block + // Let LUMemInit() deals with it. + return -1; } if (keep_prev) { @@ -100,12 +107,18 @@ Index SparseLUImpl::expand(VectorType& vec, Index& length, Index do { alpha = (alpha + 1)/2; - new_len = Index(alpha * length); + new_len = (std::max)(length+1,Index(alpha * length)); +#ifdef EIGEN_EXCEPTIONS try +#endif { vec.resize(new_len); } +#ifdef EIGEN_EXCEPTIONS catch(std::bad_alloc& ) +#else + if (!vec.size()) +#endif { tries += 1; if ( tries > 10) return new_len; @@ -139,10 +152,9 @@ template Index SparseLUImpl::memInit(Index m, Index n, Index annz, Index lwork, Index fillratio, Index panel_size, GlobalLU_t& glu) { Index& num_expansions = glu.num_expansions; //No memory expansions so far - num_expansions = 0; - glu.nzumax = glu.nzlumax = (std::max)(fillratio * annz, m*n); // estimated number of nonzeros in U + num_expansions = 0; + glu.nzumax = glu.nzlumax = (std::min)(fillratio * annz / n, m) * n; // estimated number of nonzeros in U glu.nzlmax = (std::max)(Index(4), fillratio) * annz / 4; // estimated nnz in L factor - // Return the estimated size to the user if necessary Index tempSpace; tempSpace = (2*panel_size + 4 + LUNoMarker) * m * sizeof(Index) + (panel_size + 1) * m * sizeof(Scalar); @@ -166,14 +178,10 @@ Index SparseLUImpl::memInit(Index m, Index n, Index annz, Index lw // Reserve memory for L/U factors do { - try - { - expand(glu.lusup, glu.nzlumax, 0, 0, num_expansions); - expand(glu.ucol,glu.nzumax, 0, 0, num_expansions); - expand(glu.lsub,glu.nzlmax, 0, 0, num_expansions); - expand(glu.usub,glu.nzumax, 0, 1, num_expansions); - } - catch(std::bad_alloc& ) + if( (expand(glu.lusup, glu.nzlumax, 0, 0, num_expansions)<0) + || (expand(glu.ucol, glu.nzumax, 0, 0, num_expansions)<0) + || (expand (glu.lsub, glu.nzlmax, 0, 0, num_expansions)<0) + || (expand (glu.usub, glu.nzumax, 0, 1, num_expansions)<0) ) { //Reduce the estimated size and retry glu.nzlumax /= 2; @@ -181,10 +189,7 @@ Index SparseLUImpl::memInit(Index m, Index n, Index annz, Index lw glu.nzlmax /= 2; if (glu.nzlumax < annz ) return glu.nzlumax; } - } while (!glu.lusup.size() || !glu.ucol.size() || !glu.lsub.size() || !glu.usub.size()); - - ++num_expansions; return 0; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h index 07c46e4b9..afda43bfc 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h @@ -161,8 +161,9 @@ class SparseQR b = y; // Solve with the triangular matrix R + y.resize((std::max)(cols(),Index(y.rows())),y.cols()); y.topRows(rank) = this->matrixR().topLeftCorner(rank, rank).template triangularView().solve(b.topRows(rank)); - y.bottomRows(y.size()-rank).setZero(); + y.bottomRows(y.rows()-rank).setZero(); // Apply the column permutation if (m_perm_c.size()) dest.topRows(cols()) = colsPermutation() * y.topRows(cols()); @@ -246,7 +247,7 @@ class SparseQR Index m_nonzeropivots; // Number of non zero pivots found IndexVector m_etree; // Column elimination tree IndexVector m_firstRowElt; // First element in each row - bool m_isQSorted; // whether Q is sorted or not + bool m_isQSorted; // whether Q is sorted or not template friend struct SparseQR_QProduct; template friend struct SparseQRMatrixQReturnType; @@ -338,7 +339,7 @@ void SparseQR::factorize(const MatrixType& mat) Index nonzeroCol = 0; // Record the number of valid pivots // Left looking rank-revealing QR factorization: compute a column of R and Q at a time - for (Index col = 0; col < n; ++col) + for (Index col = 0; col < (std::min)(n,m); ++col) { mark.setConstant(-1); m_R.startVec(col); @@ -346,7 +347,7 @@ void SparseQR::factorize(const MatrixType& mat) mark(nonzeroCol) = col; Qidx(0) = nonzeroCol; nzcolR = 0; nzcolQ = 1; - found_diag = false; + found_diag = col>=m; tval.setZero(); // Symbolic factorization: find the nonzero locations of the column k of the factors R and Q, i.e., @@ -571,6 +572,7 @@ struct SparseQR_QProduct : ReturnByValue topRightCorner() const /** \returns an expression of a top-right corner of *this. * - * \tparam CRows number of rows in corner as specified at compile time - * \tparam CCols number of columns in corner as specified at compile time - * \param cRows number of rows in corner as specified at run time - * \param cCols number of columns in corner as specified at run time + * \tparam CRows number of rows in corner as specified at compile-time + * \tparam CCols number of columns in corner as specified at compile-time + * \param cRows number of rows in corner as specified at run-time + * \param cCols number of columns in corner as specified at run-time * - * This function is mainly useful for corners where the number of rows is specified at compile time - * and the number of columns is specified at run time, or vice versa. The compile-time and run-time + * This function is mainly useful for corners where the number of rows is specified at compile-time + * and the number of columns is specified at run-time, or vice versa. The compile-time and run-time * information should not contradict. In other words, \a cRows should equal \a CRows unless * \a CRows is \a Dynamic, and the same for the number of columns. * @@ -188,13 +188,13 @@ inline const Block topLeftCorner() const /** \returns an expression of a top-left corner of *this. * - * \tparam CRows number of rows in corner as specified at compile time - * \tparam CCols number of columns in corner as specified at compile time - * \param cRows number of rows in corner as specified at run time - * \param cCols number of columns in corner as specified at run time + * \tparam CRows number of rows in corner as specified at compile-time + * \tparam CCols number of columns in corner as specified at compile-time + * \param cRows number of rows in corner as specified at run-time + * \param cCols number of columns in corner as specified at run-time * - * This function is mainly useful for corners where the number of rows is specified at compile time - * and the number of columns is specified at run time, or vice versa. The compile-time and run-time + * This function is mainly useful for corners where the number of rows is specified at compile-time + * and the number of columns is specified at run-time, or vice versa. The compile-time and run-time * information should not contradict. In other words, \a cRows should equal \a CRows unless * \a CRows is \a Dynamic, and the same for the number of columns. * @@ -263,13 +263,13 @@ inline const Block bottomRightCorner() const /** \returns an expression of a bottom-right corner of *this. * - * \tparam CRows number of rows in corner as specified at compile time - * \tparam CCols number of columns in corner as specified at compile time - * \param cRows number of rows in corner as specified at run time - * \param cCols number of columns in corner as specified at run time + * \tparam CRows number of rows in corner as specified at compile-time + * \tparam CCols number of columns in corner as specified at compile-time + * \param cRows number of rows in corner as specified at run-time + * \param cCols number of columns in corner as specified at run-time * - * This function is mainly useful for corners where the number of rows is specified at compile time - * and the number of columns is specified at run time, or vice versa. The compile-time and run-time + * This function is mainly useful for corners where the number of rows is specified at compile-time + * and the number of columns is specified at run-time, or vice versa. The compile-time and run-time * information should not contradict. In other words, \a cRows should equal \a CRows unless * \a CRows is \a Dynamic, and the same for the number of columns. * @@ -338,13 +338,13 @@ inline const Block bottomLeftCorner() const /** \returns an expression of a bottom-left corner of *this. * - * \tparam CRows number of rows in corner as specified at compile time - * \tparam CCols number of columns in corner as specified at compile time - * \param cRows number of rows in corner as specified at run time - * \param cCols number of columns in corner as specified at run time + * \tparam CRows number of rows in corner as specified at compile-time + * \tparam CCols number of columns in corner as specified at compile-time + * \param cRows number of rows in corner as specified at run-time + * \param cCols number of columns in corner as specified at run-time * - * This function is mainly useful for corners where the number of rows is specified at compile time - * and the number of columns is specified at run time, or vice versa. The compile-time and run-time + * This function is mainly useful for corners where the number of rows is specified at compile-time + * and the number of columns is specified at run-time, or vice versa. The compile-time and run-time * information should not contradict. In other words, \a cRows should equal \a CRows unless * \a CRows is \a Dynamic, and the same for the number of columns. * @@ -390,7 +390,11 @@ inline ConstRowsBlockXpr topRows(Index n) const /** \returns a block consisting of the top rows of *this. * - * \tparam N the number of rows in the block + * \tparam N the number of rows in the block as specified at compile-time + * \param n the number of rows in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_topRows.cpp * Output: \verbinclude MatrixBase_template_int_topRows.out @@ -398,16 +402,16 @@ inline ConstRowsBlockXpr topRows(Index n) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NRowsBlockXpr::Type topRows() +inline typename NRowsBlockXpr::Type topRows(Index n = N) { - return typename NRowsBlockXpr::Type(derived(), 0, 0, N, cols()); + return typename NRowsBlockXpr::Type(derived(), 0, 0, n, cols()); } /** This is the const version of topRows().*/ template -inline typename ConstNRowsBlockXpr::Type topRows() const +inline typename ConstNRowsBlockXpr::Type topRows(Index n = N) const { - return typename ConstNRowsBlockXpr::Type(derived(), 0, 0, N, cols()); + return typename ConstNRowsBlockXpr::Type(derived(), 0, 0, n, cols()); } @@ -434,7 +438,11 @@ inline ConstRowsBlockXpr bottomRows(Index n) const /** \returns a block consisting of the bottom rows of *this. * - * \tparam N the number of rows in the block + * \tparam N the number of rows in the block as specified at compile-time + * \param n the number of rows in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_bottomRows.cpp * Output: \verbinclude MatrixBase_template_int_bottomRows.out @@ -442,16 +450,16 @@ inline ConstRowsBlockXpr bottomRows(Index n) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NRowsBlockXpr::Type bottomRows() +inline typename NRowsBlockXpr::Type bottomRows(Index n = N) { - return typename NRowsBlockXpr::Type(derived(), rows() - N, 0, N, cols()); + return typename NRowsBlockXpr::Type(derived(), rows() - n, 0, n, cols()); } /** This is the const version of bottomRows().*/ template -inline typename ConstNRowsBlockXpr::Type bottomRows() const +inline typename ConstNRowsBlockXpr::Type bottomRows(Index n = N) const { - return typename ConstNRowsBlockXpr::Type(derived(), rows() - N, 0, N, cols()); + return typename ConstNRowsBlockXpr::Type(derived(), rows() - n, 0, n, cols()); } @@ -459,28 +467,32 @@ inline typename ConstNRowsBlockXpr::Type bottomRows() const /** \returns a block consisting of a range of rows of *this. * * \param startRow the index of the first row in the block - * \param numRows the number of rows in the block + * \param n the number of rows in the block * * Example: \include DenseBase_middleRows_int.cpp * Output: \verbinclude DenseBase_middleRows_int.out * * \sa class Block, block(Index,Index,Index,Index) */ -inline RowsBlockXpr middleRows(Index startRow, Index numRows) +inline RowsBlockXpr middleRows(Index startRow, Index n) { - return RowsBlockXpr(derived(), startRow, 0, numRows, cols()); + return RowsBlockXpr(derived(), startRow, 0, n, cols()); } /** This is the const version of middleRows(Index,Index).*/ -inline ConstRowsBlockXpr middleRows(Index startRow, Index numRows) const +inline ConstRowsBlockXpr middleRows(Index startRow, Index n) const { - return ConstRowsBlockXpr(derived(), startRow, 0, numRows, cols()); + return ConstRowsBlockXpr(derived(), startRow, 0, n, cols()); } /** \returns a block consisting of a range of rows of *this. * - * \tparam N the number of rows in the block + * \tparam N the number of rows in the block as specified at compile-time * \param startRow the index of the first row in the block + * \param n the number of rows in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include DenseBase_template_int_middleRows.cpp * Output: \verbinclude DenseBase_template_int_middleRows.out @@ -488,16 +500,16 @@ inline ConstRowsBlockXpr middleRows(Index startRow, Index numRows) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NRowsBlockXpr::Type middleRows(Index startRow) +inline typename NRowsBlockXpr::Type middleRows(Index startRow, Index n = N) { - return typename NRowsBlockXpr::Type(derived(), startRow, 0, N, cols()); + return typename NRowsBlockXpr::Type(derived(), startRow, 0, n, cols()); } /** This is the const version of middleRows().*/ template -inline typename ConstNRowsBlockXpr::Type middleRows(Index startRow) const +inline typename ConstNRowsBlockXpr::Type middleRows(Index startRow, Index n = N) const { - return typename ConstNRowsBlockXpr::Type(derived(), startRow, 0, N, cols()); + return typename ConstNRowsBlockXpr::Type(derived(), startRow, 0, n, cols()); } @@ -524,7 +536,11 @@ inline ConstColsBlockXpr leftCols(Index n) const /** \returns a block consisting of the left columns of *this. * - * \tparam N the number of columns in the block + * \tparam N the number of columns in the block as specified at compile-time + * \param n the number of columns in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_leftCols.cpp * Output: \verbinclude MatrixBase_template_int_leftCols.out @@ -532,16 +548,16 @@ inline ConstColsBlockXpr leftCols(Index n) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NColsBlockXpr::Type leftCols() +inline typename NColsBlockXpr::Type leftCols(Index n = N) { - return typename NColsBlockXpr::Type(derived(), 0, 0, rows(), N); + return typename NColsBlockXpr::Type(derived(), 0, 0, rows(), n); } /** This is the const version of leftCols().*/ template -inline typename ConstNColsBlockXpr::Type leftCols() const +inline typename ConstNColsBlockXpr::Type leftCols(Index n = N) const { - return typename ConstNColsBlockXpr::Type(derived(), 0, 0, rows(), N); + return typename ConstNColsBlockXpr::Type(derived(), 0, 0, rows(), n); } @@ -568,7 +584,11 @@ inline ConstColsBlockXpr rightCols(Index n) const /** \returns a block consisting of the right columns of *this. * - * \tparam N the number of columns in the block + * \tparam N the number of columns in the block as specified at compile-time + * \param n the number of columns in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_rightCols.cpp * Output: \verbinclude MatrixBase_template_int_rightCols.out @@ -576,16 +596,16 @@ inline ConstColsBlockXpr rightCols(Index n) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NColsBlockXpr::Type rightCols() +inline typename NColsBlockXpr::Type rightCols(Index n = N) { - return typename NColsBlockXpr::Type(derived(), 0, cols() - N, rows(), N); + return typename NColsBlockXpr::Type(derived(), 0, cols() - n, rows(), n); } /** This is the const version of rightCols().*/ template -inline typename ConstNColsBlockXpr::Type rightCols() const +inline typename ConstNColsBlockXpr::Type rightCols(Index n = N) const { - return typename ConstNColsBlockXpr::Type(derived(), 0, cols() - N, rows(), N); + return typename ConstNColsBlockXpr::Type(derived(), 0, cols() - n, rows(), n); } @@ -613,8 +633,12 @@ inline ConstColsBlockXpr middleCols(Index startCol, Index numCols) const /** \returns a block consisting of a range of columns of *this. * - * \tparam N the number of columns in the block + * \tparam N the number of columns in the block as specified at compile-time * \param startCol the index of the first column in the block + * \param n the number of columns in the block as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include DenseBase_template_int_middleCols.cpp * Output: \verbinclude DenseBase_template_int_middleCols.out @@ -622,16 +646,16 @@ inline ConstColsBlockXpr middleCols(Index startCol, Index numCols) const * \sa class Block, block(Index,Index,Index,Index) */ template -inline typename NColsBlockXpr::Type middleCols(Index startCol) +inline typename NColsBlockXpr::Type middleCols(Index startCol, Index n = N) { - return typename NColsBlockXpr::Type(derived(), 0, startCol, rows(), N); + return typename NColsBlockXpr::Type(derived(), 0, startCol, rows(), n); } /** This is the const version of middleCols().*/ template -inline typename ConstNColsBlockXpr::Type middleCols(Index startCol) const +inline typename ConstNColsBlockXpr::Type middleCols(Index startCol, Index n = N) const { - return typename ConstNColsBlockXpr::Type(derived(), 0, startCol, rows(), N); + return typename ConstNColsBlockXpr::Type(derived(), 0, startCol, rows(), n); } @@ -667,15 +691,15 @@ inline const Block block(Index startRow, In /** \returns an expression of a block in *this. * - * \tparam BlockRows number of rows in block as specified at compile time - * \tparam BlockCols number of columns in block as specified at compile time + * \tparam BlockRows number of rows in block as specified at compile-time + * \tparam BlockCols number of columns in block as specified at compile-time * \param startRow the first row in the block * \param startCol the first column in the block - * \param blockRows number of rows in block as specified at run time - * \param blockCols number of columns in block as specified at run time + * \param blockRows number of rows in block as specified at run-time + * \param blockCols number of columns in block as specified at run-time * - * This function is mainly useful for blocks where the number of rows is specified at compile time - * and the number of columns is specified at run time, or vice versa. The compile-time and run-time + * This function is mainly useful for blocks where the number of rows is specified at compile-time + * and the number of columns is specified at run-time, or vice versa. The compile-time and run-time * information should not contradict. In other words, \a blockRows should equal \a BlockRows unless * \a BlockRows is \a Dynamic, and the same for the number of columns. * @@ -738,7 +762,7 @@ inline ConstRowXpr row(Index i) const * \only_for_vectors * * \param start the first coefficient in the segment - * \param vecSize the number of coefficients in the segment + * \param n the number of coefficients in the segment * * Example: \include MatrixBase_segment_int_int.cpp * Output: \verbinclude MatrixBase_segment_int_int.out @@ -749,25 +773,25 @@ inline ConstRowXpr row(Index i) const * * \sa class Block, segment(Index) */ -inline SegmentReturnType segment(Index start, Index vecSize) +inline SegmentReturnType segment(Index start, Index n) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return SegmentReturnType(derived(), start, vecSize); + return SegmentReturnType(derived(), start, n); } /** This is the const version of segment(Index,Index).*/ -inline ConstSegmentReturnType segment(Index start, Index vecSize) const +inline ConstSegmentReturnType segment(Index start, Index n) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return ConstSegmentReturnType(derived(), start, vecSize); + return ConstSegmentReturnType(derived(), start, n); } /** \returns a dynamic-size expression of the first coefficients of *this. * * \only_for_vectors * - * \param vecSize the number of coefficients in the block + * \param n the number of coefficients in the segment * * Example: \include MatrixBase_start_int.cpp * Output: \verbinclude MatrixBase_start_int.out @@ -778,25 +802,24 @@ inline ConstSegmentReturnType segment(Index start, Index vecSize) const * * \sa class Block, block(Index,Index) */ -inline SegmentReturnType head(Index vecSize) +inline SegmentReturnType head(Index n) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return SegmentReturnType(derived(), 0, vecSize); + return SegmentReturnType(derived(), 0, n); } /** This is the const version of head(Index).*/ -inline ConstSegmentReturnType - head(Index vecSize) const +inline ConstSegmentReturnType head(Index n) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return ConstSegmentReturnType(derived(), 0, vecSize); + return ConstSegmentReturnType(derived(), 0, n); } /** \returns a dynamic-size expression of the last coefficients of *this. * * \only_for_vectors * - * \param vecSize the number of coefficients in the block + * \param n the number of coefficients in the segment * * Example: \include MatrixBase_end_int.cpp * Output: \verbinclude MatrixBase_end_int.out @@ -807,95 +830,106 @@ inline ConstSegmentReturnType * * \sa class Block, block(Index,Index) */ -inline SegmentReturnType tail(Index vecSize) +inline SegmentReturnType tail(Index n) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return SegmentReturnType(derived(), this->size() - vecSize, vecSize); + return SegmentReturnType(derived(), this->size() - n, n); } /** This is the const version of tail(Index).*/ -inline ConstSegmentReturnType tail(Index vecSize) const +inline ConstSegmentReturnType tail(Index n) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return ConstSegmentReturnType(derived(), this->size() - vecSize, vecSize); + return ConstSegmentReturnType(derived(), this->size() - n, n); } /** \returns a fixed-size expression of a segment (i.e. a vector block) in \c *this * * \only_for_vectors * - * The template parameter \a Size is the number of coefficients in the block + * \tparam N the number of coefficients in the segment as specified at compile-time + * \param start the index of the first element in the segment + * \param n the number of coefficients in the segment as specified at compile-time * - * \param start the index of the first element of the sub-vector + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_segment.cpp * Output: \verbinclude MatrixBase_template_int_segment.out * * \sa class Block */ -template -inline typename FixedSegmentReturnType::Type segment(Index start) +template +inline typename FixedSegmentReturnType::Type segment(Index start, Index n = N) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename FixedSegmentReturnType::Type(derived(), start); + return typename FixedSegmentReturnType::Type(derived(), start, n); } /** This is the const version of segment(Index).*/ -template -inline typename ConstFixedSegmentReturnType::Type segment(Index start) const +template +inline typename ConstFixedSegmentReturnType::Type segment(Index start, Index n = N) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename ConstFixedSegmentReturnType::Type(derived(), start); + return typename ConstFixedSegmentReturnType::Type(derived(), start, n); } /** \returns a fixed-size expression of the first coefficients of *this. * * \only_for_vectors * - * The template parameter \a Size is the number of coefficients in the block + * \tparam N the number of coefficients in the segment as specified at compile-time + * \param n the number of coefficients in the segment as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_start.cpp * Output: \verbinclude MatrixBase_template_int_start.out * * \sa class Block */ -template -inline typename FixedSegmentReturnType::Type head() +template +inline typename FixedSegmentReturnType::Type head(Index n = N) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename FixedSegmentReturnType::Type(derived(), 0); + return typename FixedSegmentReturnType::Type(derived(), 0, n); } /** This is the const version of head().*/ -template -inline typename ConstFixedSegmentReturnType::Type head() const +template +inline typename ConstFixedSegmentReturnType::Type head(Index n = N) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename ConstFixedSegmentReturnType::Type(derived(), 0); + return typename ConstFixedSegmentReturnType::Type(derived(), 0, n); } /** \returns a fixed-size expression of the last coefficients of *this. * * \only_for_vectors * - * The template parameter \a Size is the number of coefficients in the block + * \tparam N the number of coefficients in the segment as specified at compile-time + * \param n the number of coefficients in the segment as specified at run-time + * + * The compile-time and run-time information should not contradict. In other words, + * \a n should equal \a N unless \a N is \a Dynamic. * * Example: \include MatrixBase_template_int_end.cpp * Output: \verbinclude MatrixBase_template_int_end.out * * \sa class Block */ -template -inline typename FixedSegmentReturnType::Type tail() +template +inline typename FixedSegmentReturnType::Type tail(Index n = N) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename FixedSegmentReturnType::Type(derived(), size() - Size); + return typename FixedSegmentReturnType::Type(derived(), size() - n); } /** This is the const version of tail.*/ -template -inline typename ConstFixedSegmentReturnType::Type tail() const +template +inline typename ConstFixedSegmentReturnType::Type tail(Index n = N) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) - return typename ConstFixedSegmentReturnType::Type(derived(), size() - Size); + return typename ConstFixedSegmentReturnType::Type(derived(), size() - n); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h b/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h index 3a737df7b..7f62149e0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h @@ -83,7 +83,7 @@ cwiseMin(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const EIGEN_STRONG_INLINE const CwiseBinaryOp, const Derived, const ConstantReturnType> cwiseMin(const Scalar &other) const { - return cwiseMin(Derived::PlainObject::Constant(rows(), cols(), other)); + return cwiseMin(Derived::Constant(rows(), cols(), other)); } /** \returns an expression of the coefficient-wise max of *this and \a other @@ -107,7 +107,7 @@ cwiseMax(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const EIGEN_STRONG_INLINE const CwiseBinaryOp, const Derived, const ConstantReturnType> cwiseMax(const Scalar &other) const { - return cwiseMax(Derived::PlainObject::Constant(rows(), cols(), other)); + return cwiseMax(Derived::Constant(rows(), cols(), other)); } diff --git a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt index c35a2fdbe..a9bc05137 100644 --- a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt @@ -7,6 +7,9 @@ workaround_9220(Fortran EIGEN_Fortran_COMPILER_WORKS) if(EIGEN_Fortran_COMPILER_WORKS) enable_language(Fortran OPTIONAL) + if(NOT CMAKE_Fortran_COMPILER) + set(EIGEN_Fortran_COMPILER_WORKS OFF) + endif() endif() add_custom_target(blas) diff --git a/gtsam/3rdparty/Eigen/cmake/language_support.cmake b/gtsam/3rdparty/Eigen/cmake/language_support.cmake index 2ca303c92..d687b71f6 100644 --- a/gtsam/3rdparty/Eigen/cmake/language_support.cmake +++ b/gtsam/3rdparty/Eigen/cmake/language_support.cmake @@ -23,7 +23,7 @@ function(workaround_9220 language language_works) #message("DEBUG: language = ${language}") set(text "project(test NONE) - cmake_minimum_required(VERSION 2.6.0) + cmake_minimum_required(VERSION 2.8.0) set (CMAKE_Fortran_FLAGS \"${CMAKE_Fortran_FLAGS}\") set (CMAKE_EXE_LINKER_FLAGS \"${CMAKE_EXE_LINKER_FLAGS}\") enable_language(${language} OPTIONAL) diff --git a/gtsam/3rdparty/Eigen/doc/A05_PortingFrom2To3.dox b/gtsam/3rdparty/Eigen/doc/A05_PortingFrom2To3.dox index d885b4f6d..4d5f3ae1f 100644 --- a/gtsam/3rdparty/Eigen/doc/A05_PortingFrom2To3.dox +++ b/gtsam/3rdparty/Eigen/doc/A05_PortingFrom2To3.dox @@ -2,6 +2,8 @@ namespace Eigen { /** \page Eigen2ToEigen3 Porting from Eigen2 to Eigen3 +
Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3.
+ This page lists the most important API changes between Eigen2 and Eigen3, and gives tips to help porting your application from Eigen2 to Eigen3. diff --git a/gtsam/3rdparty/Eigen/doc/A10_Eigen2SupportModes.dox b/gtsam/3rdparty/Eigen/doc/A10_Eigen2SupportModes.dox index abfdb4683..f3df91515 100644 --- a/gtsam/3rdparty/Eigen/doc/A10_Eigen2SupportModes.dox +++ b/gtsam/3rdparty/Eigen/doc/A10_Eigen2SupportModes.dox @@ -2,6 +2,8 @@ namespace Eigen { /** \page Eigen2SupportModes Eigen 2 support modes +
Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3.
+ This page documents the Eigen2 support modes, a powerful tool to help migrating your project from Eigen 2 to Eigen 3. Don't miss our page on \ref Eigen2ToEigen3 "API changes" between Eigen 2 and Eigen 3. diff --git a/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt b/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt index 6b0a7cd6a..694d32484 100644 --- a/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt +++ b/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt @@ -16,8 +16,8 @@ double s; // Basic usage // Eigen // Matlab // comments x.size() // length(x) // vector size -C.rows() // size(C)(1) // number of rows -C.cols() // size(C)(2) // number of columns +C.rows() // size(C,1) // number of rows +C.cols() // size(C,2) // number of columns x(i) // x(i+1) // Matlab is 1-based C(i,j) // C(i+1,j+1) // @@ -51,20 +51,34 @@ v.setLinSpace(size,low,high) // v = linspace(low,high,size)' // Eigen // Matlab x.head(n) // x(1:n) x.head() // x(1:n) -x.tail(n) // N = rows(x); x(N - n: N) -x.tail() // N = rows(x); x(N - n: N) +x.tail(n) // x(end - n + 1: end) +x.tail() // x(end - n + 1: end) x.segment(i, n) // x(i+1 : i+n) x.segment(i) // x(i+1 : i+n) P.block(i, j, rows, cols) // P(i+1 : i+rows, j+1 : j+cols) P.block(i, j) // P(i+1 : i+rows, j+1 : j+cols) +P.row(i) // P(i+1, :) +P.col(j) // P(:, j+1) +P.leftCols() // P(:, 1:cols) +P.leftCols(cols) // P(:, 1:cols) +P.middleCols(j) // P(:, j+1:j+cols) +P.middleCols(j, cols) // P(:, j+1:j+cols) +P.rightCols() // P(:, end-cols+1:end) +P.rightCols(cols) // P(:, end-cols+1:end) +P.topRows() // P(1:rows, :) +P.topRows(rows) // P(1:rows, :) +P.middleRows(i) // P(:, i+1:i+rows) +P.middleRows(i, rows) // P(:, i+1:i+rows) +P.bottomRows() // P(:, end-rows+1:end) +P.bottomRows(rows) // P(:, end-rows+1:end) P.topLeftCorner(rows, cols) // P(1:rows, 1:cols) -P.topRightCorner(rows, cols) // [m n]=size(P); P(1:rows, n-cols+1:n) -P.bottomLeftCorner(rows, cols) // [m n]=size(P); P(m-rows+1:m, 1:cols) -P.bottomRightCorner(rows, cols) // [m n]=size(P); P(m-rows+1:m, n-cols+1:n) +P.topRightCorner(rows, cols) // P(1:rows, end-cols+1:end) +P.bottomLeftCorner(rows, cols) // P(end-rows+1:end, 1:cols) +P.bottomRightCorner(rows, cols) // P(end-rows+1:end, end-cols+1:end) P.topLeftCorner() // P(1:rows, 1:cols) -P.topRightCorner() // [m n]=size(P); P(1:rows, n-cols+1:n) -P.bottomLeftCorner() // [m n]=size(P); P(m-rows+1:m, 1:cols) -P.bottomRightCorner() // [m n]=size(P); P(m-rows+1:m, n-cols+1:n) +P.topRightCorner() // P(1:rows, end-cols+1:end) +P.bottomLeftCorner() // P(end-rows+1:end, 1:cols) +P.bottomRightCorner() // P(end-rows+1:end, end-cols+1:end) // Of particular note is Eigen's swap function which is highly optimized. // Eigen // Matlab @@ -125,10 +139,8 @@ int r, c; // Eigen // Matlab R.minCoeff() // min(R(:)) R.maxCoeff() // max(R(:)) -s = R.minCoeff(&r, &c) // [aa, bb] = min(R); [cc, dd] = min(aa); - // r = bb(dd); c = dd; s = cc -s = R.maxCoeff(&r, &c) // [aa, bb] = max(R); [cc, dd] = max(aa); - // row = bb(dd); col = dd; s = cc +s = R.minCoeff(&r, &c) // [s, i] = min(R(:)); [r, c] = ind2sub(size(R), i); +s = R.maxCoeff(&r, &c) // [s, i] = max(R(:)); [r, c] = ind2sub(size(R), i); R.sum() // sum(R(:)) R.colwise().sum() // sum(R) R.rowwise().sum() // sum(R, 2) or sum(R')' @@ -150,13 +162,25 @@ x.squaredNorm() // dot(x, x) Note the equivalence is not true for co x.dot(y) // dot(x, y) x.cross(y) // cross(x, y) Requires #include +//// Type conversion +// Eigen // Matlab +A.cast(); // double(A) +A.cast(); // single(A) +A.cast(); // int32(A) +// if the original type equals destination type, no work is done + +// Note that for most operations Eigen requires all operands to have the same type: +MatrixXf F = MatrixXf::Zero(3,3); +A += F; // illegal in Eigen. In Matlab A = A+F is allowed +A += F.cast(); // F converted to double and then added (generally, conversion happens on-the-fly) + // Eigen can map existing memory into Eigen matrices. float array[3]; -Map(array, 3).fill(10); -int data[4] = 1, 2, 3, 4; -Matrix2i mat2x2(data); -MatrixXi mat2x2 = Map(data); -MatrixXi mat2x2 = Map(data, 2, 2); +Vector3f::Map(array).fill(10); // create a temporary Map over array and sets entries to 10 +int data[4] = {1, 2, 3, 4}; +Matrix2i mat2x2(data); // copies data into mat2x2 +Matrix2i::Map(data) = 2*mat2x2; // overwrite elements of data with 2*mat2x2 +MatrixXi::Map(data, 2, 2) += mat2x2; // adds mat2x2 to elements of data (alternative syntax if size is not know at compile time) // Solve Ax = b. Result stored in x. Matlab: x = A \ b. x = A.ldlt().solve(b)); // A sym. p.s.d. #include diff --git a/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox b/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox index eedd5524a..981083e96 100644 --- a/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox +++ b/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox @@ -64,9 +64,9 @@ run time. However, these assertions do cost time and can thus be turned off. \c EIGEN_DONT_ALIGN is defined. - \b EIGEN_DONT_VECTORIZE - disables explicit vectorization when defined. Not defined by default, unless alignment is disabled by %Eigen's platform test or the user defining \c EIGEN_DONT_ALIGN. - - \b EIGEN_FAST_MATH - enables some optimizations which might affect the accuracy of the result. The only - optimization this currently includes is single precision sin() and cos() in the present of SSE - vectorization. Defined by default. + - \b EIGEN_FAST_MATH - enables some optimizations which might affect the accuracy of the result. This currently + enables the SSE vectorization of sin() and cos(), and speedups sqrt() for single precision. Defined to 1 by default. + Define it to 0 to disable. - \b EIGEN_UNROLLING_LIMIT - defines the size of a loop to enable meta unrolling. Set it to zero to disable unrolling. The size of a loop here is expressed in %Eigen's own notion of "number of FLOPS", it does not correspond to the number of iterations or the number of instructions. The default is value 100. diff --git a/gtsam/3rdparty/Eigen/doc/QuickReference.dox b/gtsam/3rdparty/Eigen/doc/QuickReference.dox index 5e0168649..a4be0f68a 100644 --- a/gtsam/3rdparty/Eigen/doc/QuickReference.dox +++ b/gtsam/3rdparty/Eigen/doc/QuickReference.dox @@ -405,19 +405,19 @@ array1 == array2 array1 != array2 array1 == scalar array1 != scalar array1.min(array2) array1.max(array2) array1.abs2() -array1.abs() std::abs(array1) -array1.sqrt() std::sqrt(array1) -array1.log() std::log(array1) -array1.exp() std::exp(array1) -array1.pow(exponent) std::pow(array1,exponent) +array1.abs() abs(array1) +array1.sqrt() sqrt(array1) +array1.log() log(array1) +array1.exp() exp(array1) +array1.pow(exponent) pow(array1,exponent) array1.square() array1.cube() array1.inverse() -array1.sin() std::sin(array1) -array1.cos() std::cos(array1) -array1.tan() std::tan(array1) -array1.asin() std::asin(array1) -array1.acos() std::acos(array1) +array1.sin() sin(array1) +array1.cos() cos(array1) +array1.tan() tan(array1) +array1.asin() asin(array1) +array1.acos() acos(array1) \endcode diff --git a/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox b/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox index 81aeec978..372a275de 100644 --- a/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox +++ b/gtsam/3rdparty/Eigen/doc/TutorialGeometry.dox @@ -127,7 +127,7 @@ VectorNf vec1, vec2; vec2 = t.linear() * vec1;\endcode Apply a \em general transformation \n to a \b normal \b vector -(explanations)\code +(explanations)\code VectorNf n1, n2; MatrixNf normalMatrix = t.linear().inverse().transpose(); n2 = (normalMatrix * n1).normalized();\endcode diff --git a/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox b/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox index dbfb4a9eb..835c59354 100644 --- a/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox +++ b/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox @@ -83,7 +83,7 @@ There is no notion of compressed/uncompressed mode for a SparseVector. \section TutorialSparseExample First example -Before describing each individual class, let's start with the following typical example: solving the Lapace equation \f$ \nabla u = 0 \f$ on a regular 2D grid using a finite difference scheme and Dirichlet boundary conditions. +Before describing each individual class, let's start with the following typical example: solving the Laplace equation \f$ \nabla u = 0 \f$ on a regular 2D grid using a finite difference scheme and Dirichlet boundary conditions. Such problem can be mathematically expressed as a linear problem of the form \f$ Ax=b \f$ where \f$ x \f$ is the vector of \c m unknowns (in our case, the values of the pixels), \f$ b \f$ is the right hand side vector resulting from the boundary conditions, and \f$ A \f$ is an \f$ m \times m \f$ matrix containing only a few non-zero elements resulting from the discretization of the Laplacian operator. diff --git a/gtsam/3rdparty/Eigen/doc/eigendoxy.css b/gtsam/3rdparty/Eigen/doc/eigendoxy.css index 60243d870..efaeb46dc 100644 --- a/gtsam/3rdparty/Eigen/doc/eigendoxy.css +++ b/gtsam/3rdparty/Eigen/doc/eigendoxy.css @@ -199,3 +199,13 @@ h3.version { td.width20em p.endtd { width: 20em; } + +.bigwarning { + font-size:2em; + font-weight:bold; + margin:1em; + padding:1em; + color:red; + border:solid; +} + diff --git a/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in b/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in index e62af8ed5..878244a19 100644 --- a/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in +++ b/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in @@ -17,8 +17,7 @@ $generatedby   - - ---> diff --git a/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheLeft.cpp b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheLeft.cpp new file mode 100644 index 000000000..6398c873a --- /dev/null +++ b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheLeft.cpp @@ -0,0 +1,7 @@ +Matrix3f A = Matrix3f::Random(3,3), B; +B << 0,1,0, + 0,0,1, + 1,0,0; +cout << "At start, A = " << endl << A << endl; +A.applyOnTheLeft(B); +cout << "After applyOnTheLeft, A = " << endl << A << endl; diff --git a/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheRight.cpp b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheRight.cpp new file mode 100644 index 000000000..e4b71b2d8 --- /dev/null +++ b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_applyOnTheRight.cpp @@ -0,0 +1,9 @@ +Matrix3f A = Matrix3f::Random(3,3), B; +B << 0,1,0, + 0,0,1, + 1,0,0; +cout << "At start, A = " << endl << A << endl; +A *= B; +cout << "After A *= B, A = " << endl << A << endl; +A.applyOnTheRight(B); // equivalent to A *= B +cout << "After applyOnTheRight, A = " << endl << A << endl; diff --git a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt index 138a2f6ef..0c9b3c3ba 100644 --- a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt @@ -10,12 +10,12 @@ endif(NOT EIGEN_TEST_NOQT) if(QT4_FOUND) add_executable(Tutorial_sparse_example Tutorial_sparse_example.cpp Tutorial_sparse_example_details.cpp) target_link_libraries(Tutorial_sparse_example ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}) - + add_custom_command( TARGET Tutorial_sparse_example POST_BUILD - COMMAND Tutorial_sparse_example - ARGS ${CMAKE_CURRENT_BINARY_DIR}/../html/Tutorial_sparse_example.jpeg + COMMAND Tutorial_sparse_example ARGS ${CMAKE_CURRENT_BINARY_DIR}/../html/Tutorial_sparse_example.jpeg ) + add_dependencies(all_examples Tutorial_sparse_example) endif(QT4_FOUND) diff --git a/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example_details.cpp b/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example_details.cpp index 8c3020b63..7d820b44a 100644 --- a/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example_details.cpp +++ b/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example_details.cpp @@ -11,8 +11,8 @@ void insertCoefficient(int id, int i, int j, double w, std::vector& coeffs, int n = boundary.size(); int id1 = i+j*n; - if(i==-1 || i==n) b(id) -= w * boundary(j); // constrained coeffcieint - else if(j==-1 || j==n) b(id) -= w * boundary(i); // constrained coeffcieint + if(i==-1 || i==n) b(id) -= w * boundary(j); // constrained coefficient + else if(j==-1 || j==n) b(id) -= w * boundary(i); // constrained coefficient else coeffs.push_back(T(id,id1,w)); // unknown coefficient } diff --git a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt index 7e7444326..9883d4c72 100644 --- a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt @@ -7,6 +7,9 @@ workaround_9220(Fortran EIGEN_Fortran_COMPILER_WORKS) if(EIGEN_Fortran_COMPILER_WORKS) enable_language(Fortran OPTIONAL) + if(NOT CMAKE_Fortran_COMPILER) + set(EIGEN_Fortran_COMPILER_WORKS OFF) + endif() endif() add_custom_target(lapack) diff --git a/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs b/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs index 9b71cd8e0..0e6f9ada2 100644 --- a/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs +++ b/gtsam/3rdparty/Eigen/scripts/eigen_gen_docs @@ -4,6 +4,7 @@ # You should call this script with USER set as you want, else some default # will be used USER=${USER:-'orzel'} +UPLOAD_DIR=dox #ulimit -v 1024000 @@ -14,10 +15,10 @@ mkdir build -p #step 2 : upload # (the '/' at the end of path is very important, see rsync documentation) -rsync -az --no-p --delete build/doc/html/ $USER@ssh.tuxfamily.org:eigen/eigen.tuxfamily.org-web/htdocs/dox-devel/ || { echo "upload failed"; exit 1; } +rsync -az --no-p --delete build/doc/html/ $USER@ssh.tuxfamily.org:eigen/eigen.tuxfamily.org-web/htdocs/$UPLOAD_DIR/ || { echo "upload failed"; exit 1; } #step 3 : fix the perm -ssh $USER@ssh.tuxfamily.org 'chmod -R g+w /home/eigen/eigen.tuxfamily.org-web/htdocs/dox-devel' || { echo "perm failed"; exit 1; } +ssh $USER@ssh.tuxfamily.org "chmod -R g+w /home/eigen/eigen.tuxfamily.org-web/htdocs/$UPLOAD_DIR" || { echo "perm failed"; exit 1; } echo "Uploaded successfully" diff --git a/gtsam/3rdparty/Eigen/test/array.cpp b/gtsam/3rdparty/Eigen/test/array.cpp index 8960e49f8..c607da631 100644 --- a/gtsam/3rdparty/Eigen/test/array.cpp +++ b/gtsam/3rdparty/Eigen/test/array.cpp @@ -167,21 +167,14 @@ template void array_real(const ArrayType& m) Scalar s1 = internal::random(); // these tests are mostly to check possible compilation issues. -// VERIFY_IS_APPROX(m1.sin(), std::sin(m1)); VERIFY_IS_APPROX(m1.sin(), sin(m1)); -// VERIFY_IS_APPROX(m1.cos(), std::cos(m1)); VERIFY_IS_APPROX(m1.cos(), cos(m1)); -// VERIFY_IS_APPROX(m1.asin(), std::asin(m1)); VERIFY_IS_APPROX(m1.asin(), asin(m1)); -// VERIFY_IS_APPROX(m1.acos(), std::acos(m1)); VERIFY_IS_APPROX(m1.acos(), acos(m1)); -// VERIFY_IS_APPROX(m1.tan(), std::tan(m1)); VERIFY_IS_APPROX(m1.tan(), tan(m1)); VERIFY_IS_APPROX(cos(m1+RealScalar(3)*m2), cos((m1+RealScalar(3)*m2).eval())); -// VERIFY_IS_APPROX(std::cos(m1+RealScalar(3)*m2), std::cos((m1+RealScalar(3)*m2).eval())); -// VERIFY_IS_APPROX(m1.abs().sqrt(), std::sqrt(std::abs(m1))); VERIFY_IS_APPROX(m1.abs().sqrt(), sqrt(abs(m1))); VERIFY_IS_APPROX(m1.abs(), sqrt(numext::abs2(m1))); @@ -190,9 +183,10 @@ template void array_real(const ArrayType& m) if(!NumTraits::IsComplex) VERIFY_IS_APPROX(numext::real(m1), m1); - VERIFY_IS_APPROX(m1.abs().log() , log(abs(m1))); + // shift argument of logarithm so that it is not zero + Scalar smallNumber = NumTraits::dummy_precision(); + VERIFY_IS_APPROX((m1.abs() + smallNumber).log() , log(abs(m1) + smallNumber)); -// VERIFY_IS_APPROX(m1.exp(), std::exp(m1)); VERIFY_IS_APPROX(m1.exp() * m2.exp(), exp(m1+m2)); VERIFY_IS_APPROX(m1.exp(), exp(m1)); VERIFY_IS_APPROX(m1.exp() / m2.exp(),(m1-m2).exp()); @@ -236,7 +230,6 @@ template void array_complex(const ArrayType& m) m2(i,j) = sqrt(m1(i,j)); VERIFY_IS_APPROX(m1.sqrt(), m2); -// VERIFY_IS_APPROX(m1.sqrt(), std::sqrt(m1)); VERIFY_IS_APPROX(m1.sqrt(), Eigen::sqrt(m1)); } diff --git a/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp b/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp index 1250c9ff5..9a50f99ab 100644 --- a/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp +++ b/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp @@ -163,10 +163,14 @@ template void cwise_min_max(const MatrixType& m) // min/max with scalar input VERIFY_IS_APPROX(MatrixType::Constant(rows,cols, minM1), m1.cwiseMin( minM1)); - VERIFY_IS_APPROX(m1, m1.cwiseMin( maxM1)); + VERIFY_IS_APPROX(m1, m1.cwiseMin(maxM1)); + VERIFY_IS_APPROX(-m1, (-m1).cwiseMin(-minM1)); + VERIFY_IS_APPROX(-m1.array(), ((-m1).array().min)( -minM1)); VERIFY_IS_APPROX(MatrixType::Constant(rows,cols, maxM1), m1.cwiseMax( maxM1)); - VERIFY_IS_APPROX(m1, m1.cwiseMax( minM1)); + VERIFY_IS_APPROX(m1, m1.cwiseMax(minM1)); + VERIFY_IS_APPROX(-m1, (-m1).cwiseMax(-maxM1)); + VERIFY_IS_APPROX(-m1.array(), ((-m1).array().max)(-maxM1)); VERIFY_IS_APPROX(MatrixType::Constant(rows,cols, minM1).array(), (m1.array().min)( minM1)); VERIFY_IS_APPROX(m1.array(), (m1.array().min)( maxM1)); @@ -202,6 +206,12 @@ template void resize(const MatrixTraits& t) VERIFY(a1.size()==cols); } +void regression_bug_654() +{ + ArrayXf a = RowVectorXf(3); + VectorXf v = Array(3); +} + void test_array_for_matrix() { for(int i = 0; i < g_repeat; i++) { @@ -239,4 +249,5 @@ void test_array_for_matrix() CALL_SUBTEST_5( resize(MatrixXf(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); CALL_SUBTEST_6( resize(MatrixXi(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); } + CALL_SUBTEST_6( regression_bug_654() ); } diff --git a/gtsam/3rdparty/Eigen/test/cholesky.cpp b/gtsam/3rdparty/Eigen/test/cholesky.cpp index 378525a83..b980dc572 100644 --- a/gtsam/3rdparty/Eigen/test/cholesky.cpp +++ b/gtsam/3rdparty/Eigen/test/cholesky.cpp @@ -263,8 +263,8 @@ template void cholesky_bug241(const MatrixType& m) // LDLT is not guaranteed to work for indefinite matrices, but happens to work fine if matrix is diagonal. // This test checks that LDLT reports correctly that matrix is indefinite. -// See http://forum.kde.org/viewtopic.php?f=74&t=106942 -template void cholesky_indefinite(const MatrixType& m) +// See http://forum.kde.org/viewtopic.php?f=74&t=106942 and bug 736 +template void cholesky_definiteness(const MatrixType& m) { eigen_assert(m.rows() == 2 && m.cols() == 2); MatrixType mat; @@ -280,6 +280,24 @@ template void cholesky_indefinite(const MatrixType& m) VERIFY(!ldlt.isNegative()); VERIFY(!ldlt.isPositive()); } + { + mat << 0, 0, 0, 0; + LDLT ldlt(mat); + VERIFY(ldlt.isNegative()); + VERIFY(ldlt.isPositive()); + } + { + mat << 0, 0, 0, 1; + LDLT ldlt(mat); + VERIFY(!ldlt.isNegative()); + VERIFY(ldlt.isPositive()); + } + { + mat << -1, 0, 0, 0; + LDLT ldlt(mat); + VERIFY(ldlt.isNegative()); + VERIFY(!ldlt.isPositive()); + } } template void cholesky_verify_assert() @@ -309,7 +327,7 @@ void test_cholesky() CALL_SUBTEST_1( cholesky(Matrix()) ); CALL_SUBTEST_3( cholesky(Matrix2d()) ); CALL_SUBTEST_3( cholesky_bug241(Matrix2d()) ); - CALL_SUBTEST_3( cholesky_indefinite(Matrix2d()) ); + CALL_SUBTEST_3( cholesky_definiteness(Matrix2d()) ); CALL_SUBTEST_4( cholesky(Matrix3f()) ); CALL_SUBTEST_5( cholesky(Matrix4d()) ); s = internal::random(1,EIGEN_TEST_MAX_SIZE); diff --git a/gtsam/3rdparty/Eigen/test/conservative_resize.cpp b/gtsam/3rdparty/Eigen/test/conservative_resize.cpp index 2d1ab3f03..498421b4c 100644 --- a/gtsam/3rdparty/Eigen/test/conservative_resize.cpp +++ b/gtsam/3rdparty/Eigen/test/conservative_resize.cpp @@ -60,34 +60,51 @@ void run_matrix_tests() template void run_vector_tests() { - typedef Matrix MatrixType; + typedef Matrix VectorType; - MatrixType m, n; + VectorType m, n; // boundary cases ... - m = n = MatrixType::Random(50); + m = n = VectorType::Random(50); m.conservativeResize(1); VERIFY_IS_APPROX(m, n.segment(0,1)); - m = n = MatrixType::Random(50); + m = n = VectorType::Random(50); m.conservativeResize(50); VERIFY_IS_APPROX(m, n.segment(0,50)); + + m = n = VectorType::Random(50); + m.conservativeResize(m.rows(),1); + VERIFY_IS_APPROX(m, n.segment(0,1)); + + m = n = VectorType::Random(50); + m.conservativeResize(m.rows(),50); + VERIFY_IS_APPROX(m, n.segment(0,50)); // random shrinking ... for (int i=0; i<50; ++i) { const int size = internal::random(1,50); - m = n = MatrixType::Random(50); + m = n = VectorType::Random(50); m.conservativeResize(size); VERIFY_IS_APPROX(m, n.segment(0,size)); + + m = n = VectorType::Random(50); + m.conservativeResize(m.rows(), size); + VERIFY_IS_APPROX(m, n.segment(0,size)); } // random growing with zeroing ... for (int i=0; i<50; ++i) { const int size = internal::random(50,100); - m = n = MatrixType::Random(50); - m.conservativeResizeLike(MatrixType::Zero(size)); + m = n = VectorType::Random(50); + m.conservativeResizeLike(VectorType::Zero(size)); + VERIFY_IS_APPROX(m.segment(0,50), n); + VERIFY( size<=50 || m.segment(50,size-50).sum() == Scalar(0) ); + + m = n = VectorType::Random(50); + m.conservativeResizeLike(Matrix::Zero(1,size)); VERIFY_IS_APPROX(m.segment(0,50), n); VERIFY( size<=50 || m.segment(50,size-50).sum() == Scalar(0) ); } diff --git a/gtsam/3rdparty/Eigen/test/geo_eulerangles.cpp b/gtsam/3rdparty/Eigen/test/geo_eulerangles.cpp index 5445cd81a..b4830bd41 100644 --- a/gtsam/3rdparty/Eigen/test/geo_eulerangles.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_eulerangles.cpp @@ -12,36 +12,48 @@ #include #include -template void check_all_var(const Matrix& ea) + +template +void verify_euler(const Matrix& ea, int i, int j, int k) { typedef Matrix Matrix3; typedef Matrix Vector3; typedef AngleAxis AngleAxisx; using std::abs; + Matrix3 m(AngleAxisx(ea[0], Vector3::Unit(i)) * AngleAxisx(ea[1], Vector3::Unit(j)) * AngleAxisx(ea[2], Vector3::Unit(k))); + Vector3 eabis = m.eulerAngles(i, j, k); + Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) * AngleAxisx(eabis[2], Vector3::Unit(k))); + VERIFY_IS_APPROX(m, mbis); + /* If I==K, and ea[1]==0, then there no unique solution. */ + /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */ + if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(M_PI/2),test_precision())) ) + VERIFY((ea-eabis).norm() <= test_precision()); - #define VERIFY_EULER(I,J,K, X,Y,Z) { \ - Matrix3 m(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \ - Vector3 eabis = m.eulerAngles(I,J,K); \ - Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit##X()) * AngleAxisx(eabis[1], Vector3::Unit##Y()) * AngleAxisx(eabis[2], Vector3::Unit##Z())); \ - VERIFY_IS_APPROX(m, mbis); \ - /* If I==K, and ea[1]==0, then there no unique solution. */ \ - /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */ \ - if( (I!=K || ea[1]!=0) && (I==K || !internal::isApprox(abs(ea[1]),Scalar(M_PI/2),test_precision())) ) VERIFY((ea-eabis).norm() <= test_precision()); \ - } - VERIFY_EULER(0,1,2, X,Y,Z); - VERIFY_EULER(0,1,0, X,Y,X); - VERIFY_EULER(0,2,1, X,Z,Y); - VERIFY_EULER(0,2,0, X,Z,X); + // approx_or_less_than does not work for 0 + VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1))); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(M_PI)); + VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(M_PI), eabis[1]); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(M_PI)); + VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(M_PI), eabis[2]); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(M_PI)); +} - VERIFY_EULER(1,2,0, Y,Z,X); - VERIFY_EULER(1,2,1, Y,Z,Y); - VERIFY_EULER(1,0,2, Y,X,Z); - VERIFY_EULER(1,0,1, Y,X,Y); +template void check_all_var(const Matrix& ea) +{ + verify_euler(ea, 0,1,2); + verify_euler(ea, 0,1,0); + verify_euler(ea, 0,2,1); + verify_euler(ea, 0,2,0); - VERIFY_EULER(2,0,1, Z,X,Y); - VERIFY_EULER(2,0,2, Z,X,Z); - VERIFY_EULER(2,1,0, Z,Y,X); - VERIFY_EULER(2,1,2, Z,Y,Z); + verify_euler(ea, 1,2,0); + verify_euler(ea, 1,2,1); + verify_euler(ea, 1,0,2); + verify_euler(ea, 1,0,1); + + verify_euler(ea, 2,0,1); + verify_euler(ea, 2,0,2); + verify_euler(ea, 2,1,0); + verify_euler(ea, 2,1,2); } template void eulerangles() @@ -63,7 +75,16 @@ template void eulerangles() ea = m.eulerAngles(0,1,0); check_all_var(ea); - ea = (Array3::Random() + Array3(1,1,0))*Scalar(M_PI)*Array3(0.5,0.5,1); + // Check with purely random Quaternion: + q1.coeffs() = Quaternionx::Coefficients::Random().normalized(); + m = q1; + ea = m.eulerAngles(0,1,2); + check_all_var(ea); + ea = m.eulerAngles(0,1,0); + check_all_var(ea); + + // Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi]. + ea = (Array3::Random() + Array3(1,0,0))*Scalar(M_PI)*Array3(0.5,1,1); check_all_var(ea); ea[2] = ea[0] = internal::random(0,Scalar(M_PI)); diff --git a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp index 35ae67ebe..ee3030b5d 100644 --- a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp @@ -279,6 +279,13 @@ template void transformations() t1 = Eigen::Scaling(s0,s0,s0) * t1; VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + t0 = t3; + t0.scale(s0); + t1 = t3 * Eigen::Scaling(s0); + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + t0.prescale(s0); + t1 = Eigen::Scaling(s0) * t1; + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); t0.setIdentity(); t0.prerotate(q1).prescale(v0).pretranslate(v0); diff --git a/gtsam/3rdparty/Eigen/test/product_trmm.cpp b/gtsam/3rdparty/Eigen/test/product_trmm.cpp index 506a1aeb9..d715b9a36 100644 --- a/gtsam/3rdparty/Eigen/test/product_trmm.cpp +++ b/gtsam/3rdparty/Eigen/test/product_trmm.cpp @@ -51,6 +51,7 @@ void trmm(int rows=internal::random(1,EIGEN_TEST_MAX_SIZE), ge_xs_save = ge_xs; VERIFY_IS_APPROX( (ge_xs_save + s1*triTr.conjugate() * (s2*ge_left.adjoint())).eval(), ge_xs.noalias() += (s1*mat.adjoint()).template triangularView() * (s2*ge_left.adjoint()) ); + ge_sx.setRandom(); ge_sx_save = ge_sx; VERIFY_IS_APPROX( ge_sx_save - (ge_right.adjoint() * (-s1 * triTr).conjugate()).eval(), ge_sx.noalias() -= (ge_right.adjoint() * (-s1 * mat).adjoint().template triangularView()).eval()); diff --git a/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp b/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp index 15d7299d7..511f2473f 100644 --- a/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp +++ b/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp @@ -130,4 +130,8 @@ void test_qr_fullpivoting() // Test problem size constructors CALL_SUBTEST_7(FullPivHouseholderQR(10, 20)); + CALL_SUBTEST_7((FullPivHouseholderQR >(10,20))); + CALL_SUBTEST_7((FullPivHouseholderQR >(Matrix::Random()))); + CALL_SUBTEST_7((FullPivHouseholderQR >(20,10))); + CALL_SUBTEST_7((FullPivHouseholderQR >(Matrix::Random()))); } diff --git a/gtsam/3rdparty/Eigen/test/ref.cpp b/gtsam/3rdparty/Eigen/test/ref.cpp index 65b4f5a3e..f639d900b 100644 --- a/gtsam/3rdparty/Eigen/test/ref.cpp +++ b/gtsam/3rdparty/Eigen/test/ref.cpp @@ -14,9 +14,9 @@ static int nb_temporaries; -inline void on_temporary_creation(int size) { +inline void on_temporary_creation(int) { // here's a great place to set a breakpoint when debugging failures in this test! - if(size!=0) nb_temporaries++; + nb_temporaries++; } diff --git a/gtsam/3rdparty/Eigen/test/sparse.h b/gtsam/3rdparty/Eigen/test/sparse.h index a09c65e5f..e19a76316 100644 --- a/gtsam/3rdparty/Eigen/test/sparse.h +++ b/gtsam/3rdparty/Eigen/test/sparse.h @@ -154,16 +154,16 @@ initSparse(double density, sparseMat.finalize(); } -template void +template void initSparse(double density, Matrix& refVec, - SparseVector& sparseVec, + SparseVector& sparseVec, std::vector* zeroCoords = 0, std::vector* nonzeroCoords = 0) { sparseVec.reserve(int(refVec.size()*density)); sparseVec.setZero(); - for(int i=0; i(0,1) < density) ? internal::random() : Scalar(0); if (v!=Scalar(0)) @@ -178,10 +178,10 @@ initSparse(double density, } } -template void +template void initSparse(double density, Matrix& refVec, - SparseVector& sparseVec, + SparseVector& sparseVec, std::vector* zeroCoords = 0, std::vector* nonzeroCoords = 0) { diff --git a/gtsam/3rdparty/Eigen/test/sparse_product.cpp b/gtsam/3rdparty/Eigen/test/sparse_product.cpp index 664e33887..a2ea9d5b7 100644 --- a/gtsam/3rdparty/Eigen/test/sparse_product.cpp +++ b/gtsam/3rdparty/Eigen/test/sparse_product.cpp @@ -13,8 +13,9 @@ template struct test_outer { static void run(SparseMatrixType& m2, SparseMatrixType& m4, DenseMatrix& refMat2, DenseMatrix& refMat4) { - int c = internal::random(0,m2.cols()-1); - int c1 = internal::random(0,m2.cols()-1); + typedef typename SparseMatrixType::Index Index; + Index c = internal::random(0,m2.cols()-1); + Index c1 = internal::random(0,m2.cols()-1); VERIFY_IS_APPROX(m4=m2.col(c)*refMat2.col(c1).transpose(), refMat4=refMat2.col(c)*refMat2.col(c1).transpose()); VERIFY_IS_APPROX(m4=refMat2.col(c1)*m2.col(c).transpose(), refMat4=refMat2.col(c1)*refMat2.col(c).transpose()); } @@ -22,8 +23,9 @@ template struct test_outer struct test_outer { static void run(SparseMatrixType& m2, SparseMatrixType& m4, DenseMatrix& refMat2, DenseMatrix& refMat4) { - int r = internal::random(0,m2.rows()-1); - int c1 = internal::random(0,m2.cols()-1); + typedef typename SparseMatrixType::Index Index; + Index r = internal::random(0,m2.rows()-1); + Index c1 = internal::random(0,m2.cols()-1); VERIFY_IS_APPROX(m4=m2.row(r).transpose()*refMat2.col(c1).transpose(), refMat4=refMat2.row(r).transpose()*refMat2.col(c1).transpose()); VERIFY_IS_APPROX(m4=refMat2.col(c1)*m2.row(r), refMat4=refMat2.col(c1)*refMat2.row(r)); } @@ -37,9 +39,9 @@ template void sparse_product() { typedef typename SparseMatrixType::Index Index; Index n = 100; - const Index rows = internal::random(1,n); - const Index cols = internal::random(1,n); - const Index depth = internal::random(1,n); + const Index rows = internal::random(1,n); + const Index cols = internal::random(1,n); + const Index depth = internal::random(1,n); typedef typename SparseMatrixType::Scalar Scalar; enum { Flags = SparseMatrixType::Flags }; @@ -244,6 +246,7 @@ void test_sparse_product() CALL_SUBTEST_1( (sparse_product >()) ); CALL_SUBTEST_2( (sparse_product, ColMajor > >()) ); CALL_SUBTEST_2( (sparse_product, RowMajor > >()) ); + CALL_SUBTEST_3( (sparse_product >()) ); CALL_SUBTEST_4( (sparse_product_regression_test, Matrix >()) ); } } diff --git a/gtsam/3rdparty/Eigen/test/sparse_vector.cpp b/gtsam/3rdparty/Eigen/test/sparse_vector.cpp index ec5877b6a..0c9476803 100644 --- a/gtsam/3rdparty/Eigen/test/sparse_vector.cpp +++ b/gtsam/3rdparty/Eigen/test/sparse_vector.cpp @@ -9,14 +9,14 @@ #include "sparse.h" -template void sparse_vector(int rows, int cols) +template void sparse_vector(int rows, int cols) { double densityMat = (std::max)(8./(rows*cols), 0.01); double densityVec = (std::max)(8./float(rows), 0.1); typedef Matrix DenseMatrix; typedef Matrix DenseVector; - typedef SparseVector SparseVectorType; - typedef SparseMatrix SparseMatrixType; + typedef SparseVector SparseVectorType; + typedef SparseMatrix SparseMatrixType; Scalar eps = 1e-6; SparseMatrixType m1(rows,rows); @@ -101,9 +101,10 @@ template void sparse_vector(int rows, int cols) void test_sparse_vector() { for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( sparse_vector(8, 8) ); - CALL_SUBTEST_2( sparse_vector >(16, 16) ); - CALL_SUBTEST_1( sparse_vector(299, 535) ); + CALL_SUBTEST_1(( sparse_vector(8, 8) )); + CALL_SUBTEST_2(( sparse_vector, int>(16, 16) )); + CALL_SUBTEST_1(( sparse_vector(299, 535) )); + CALL_SUBTEST_1(( sparse_vector(299, 535) )); } } diff --git a/gtsam/3rdparty/Eigen/test/stable_norm.cpp b/gtsam/3rdparty/Eigen/test/stable_norm.cpp index c09fc17b7..549f91fbf 100644 --- a/gtsam/3rdparty/Eigen/test/stable_norm.cpp +++ b/gtsam/3rdparty/Eigen/test/stable_norm.cpp @@ -55,8 +55,16 @@ template void stable_norm(const MatrixType& m) Index rows = m.rows(); Index cols = m.cols(); - Scalar big = internal::random() * ((std::numeric_limits::max)() * RealScalar(1e-4)); - Scalar small = internal::random() * ((std::numeric_limits::min)() * RealScalar(1e4)); + // get a non-zero random factor + Scalar factor = internal::random(); + while(numext::abs2(factor)(); + Scalar big = factor * ((std::numeric_limits::max)() * RealScalar(1e-4)); + + factor = internal::random(); + while(numext::abs2(factor)(); + Scalar small = factor * ((std::numeric_limits::min)() * RealScalar(1e4)); MatrixType vzero = MatrixType::Zero(rows, cols), vrand = MatrixType::Random(rows, cols), @@ -91,7 +99,7 @@ template void stable_norm(const MatrixType& m) VERIFY_IS_APPROX(vsmall.blueNorm(), sqrt(size)*abs(small)); VERIFY_IS_APPROX(vsmall.hypotNorm(), sqrt(size)*abs(small)); -// Test compilation of cwise() version + // Test compilation of cwise() version VERIFY_IS_APPROX(vrand.colwise().stableNorm(), vrand.colwise().norm()); VERIFY_IS_APPROX(vrand.colwise().blueNorm(), vrand.colwise().norm()); VERIFY_IS_APPROX(vrand.colwise().hypotNorm(), vrand.colwise().norm()); diff --git a/gtsam/3rdparty/Eigen/test/umeyama.cpp b/gtsam/3rdparty/Eigen/test/umeyama.cpp index 738d0af70..2e8092434 100644 --- a/gtsam/3rdparty/Eigen/test/umeyama.cpp +++ b/gtsam/3rdparty/Eigen/test/umeyama.cpp @@ -130,10 +130,11 @@ void run_fixed_size_test(int num_elements) // MUST be positive because in any other case det(cR_t) may become negative for // odd dimensions! - const Scalar c = abs(internal::random()); + // Also if c is to small compared to t.norm(), problem is ill-posed (cf. Bug 744) + const Scalar c = internal::random(0.5, 2.0); FixedMatrix R = randMatrixSpecialUnitary(dim); - FixedVector t = Scalar(50)*FixedVector::Random(dim,1); + FixedVector t = Scalar(32)*FixedVector::Random(dim,1); HomMatrix cR_t = HomMatrix::Identity(dim+1,dim+1); cR_t.block(0,0,dim,dim) = c*R; @@ -149,9 +150,9 @@ void run_fixed_size_test(int num_elements) HomMatrix cR_t_umeyama = umeyama(src_block, dst_block); - const Scalar error = ( cR_t_umeyama*src - dst ).array().square().sum(); + const Scalar error = ( cR_t_umeyama*src - dst ).squaredNorm(); - VERIFY(error < Scalar(10)*std::numeric_limits::epsilon()); + VERIFY(error < Scalar(16)*std::numeric_limits::epsilon()); } void test_umeyama() diff --git a/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp b/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp index 9d60b0388..6cd1acdda 100644 --- a/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp +++ b/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp @@ -101,6 +101,16 @@ template void vectorwiseop_array(const ArrayType& m) VERIFY_RAISES_ASSERT(m2.rowwise() /= rowvec.transpose()); VERIFY_RAISES_ASSERT(m1.rowwise() / rowvec.transpose()); + + m2 = m1; + // yes, there might be an aliasing issue there but ".rowwise() /=" + // is suppposed to evaluate " m2.colwise().sum()" into to temporary to avoid + // evaluating the reducions multiple times + if(ArrayType::RowsAtCompileTime>2 || ArrayType::RowsAtCompileTime==Dynamic) + { + m2.rowwise() /= m2.colwise().sum(); + VERIFY_IS_APPROX(m2, m1.rowwise() / m1.colwise().sum()); + } } template void vectorwiseop_matrix(const MatrixType& m) diff --git a/gtsam/3rdparty/Eigen/test/zerosized.cpp b/gtsam/3rdparty/Eigen/test/zerosized.cpp index 6905e584e..da7dd0481 100644 --- a/gtsam/3rdparty/Eigen/test/zerosized.cpp +++ b/gtsam/3rdparty/Eigen/test/zerosized.cpp @@ -9,12 +9,26 @@ #include "main.h" + +template void zeroReduction(const MatrixType& m) { + // Reductions that must hold for zero sized objects + VERIFY(m.all()); + VERIFY(!m.any()); + VERIFY(m.prod()==1); + VERIFY(m.sum()==0); + VERIFY(m.count()==0); + VERIFY(m.allFinite()); + VERIFY(!m.hasNaN()); +} + + template void zeroSizedMatrix() { MatrixType t1; - if (MatrixType::SizeAtCompileTime == Dynamic) + if (MatrixType::SizeAtCompileTime == Dynamic || MatrixType::SizeAtCompileTime == 0) { + zeroReduction(t1); if (MatrixType::RowsAtCompileTime == Dynamic) VERIFY(t1.rows() == 0); if (MatrixType::ColsAtCompileTime == Dynamic) @@ -22,9 +36,13 @@ template void zeroSizedMatrix() if (MatrixType::RowsAtCompileTime == Dynamic && MatrixType::ColsAtCompileTime == Dynamic) { + MatrixType t2(0, 0); VERIFY(t2.rows() == 0); VERIFY(t2.cols() == 0); + + zeroReduction(t2); + VERIFY(t1==t2); } } } @@ -33,11 +51,15 @@ template void zeroSizedVector() { VectorType t1; - if (VectorType::SizeAtCompileTime == Dynamic) + if (VectorType::SizeAtCompileTime == Dynamic || VectorType::SizeAtCompileTime==0) { + zeroReduction(t1); VERIFY(t1.size() == 0); VectorType t2(DenseIndex(0)); // DenseIndex disambiguates with 0-the-null-pointer (error with gcc 4.4 and MSVC8) VERIFY(t2.size() == 0); + zeroReduction(t2); + + VERIFY(t1==t2); } } @@ -51,9 +73,12 @@ void test_zerosized() zeroSizedMatrix >(); zeroSizedMatrix >(); zeroSizedMatrix >(); - + zeroSizedMatrix >(); + zeroSizedMatrix >(); + zeroSizedVector(); zeroSizedVector(); zeroSizedVector(); zeroSizedVector >(); + zeroSizedVector >(); } diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport b/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport index 4454bf820..c4090ab11 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport @@ -11,7 +11,12 @@ #define EIGEN_OPENGL_MODULE #include -#include + +#if defined(__APPLE_CC__) + #include +#else + #include +#endif namespace Eigen { diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h index 3f18beeeb..dc0093eb9 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h @@ -58,7 +58,9 @@ void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV) Scalar rho, rho_1, alpha; d.setZero(); - CINV.startFill(); // FIXME estimate the number of non-zeros + typedef Triplet T; + std::vector tripletList; + for (Index i = 0; i < rows; ++i) { d[i] = 1.0; @@ -84,11 +86,12 @@ void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV) // FIXME add a generic "prune/filter" expression for both dense and sparse object to sparse for (Index j=0; j TmpVec; diff --git a/gtsam/3rdparty/Eigen/unsupported/test/FFTW.cpp b/gtsam/3rdparty/Eigen/unsupported/test/FFTW.cpp index a07bf274b..d3718e2d2 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/FFTW.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/FFTW.cpp @@ -16,9 +16,6 @@ std::complex RandomCpx() { return std::complex( (T)(rand()/(T)RAND_MAX - . using namespace std; using namespace Eigen; -float norm(float x) {return x*x;} -double norm(double x) {return x*x;} -long double norm(long double x) {return x*x;} template < typename T> complex promote(complex x) { return complex(x.real(),x.imag()); } @@ -40,11 +37,11 @@ complex promote(long double x) { return complex( x); for (size_t k1=0;k1<(size_t)timebuf.size();++k1) { acc += promote( timebuf[k1] ) * exp( complex(0,k1*phinc) ); } - totalpower += norm(acc); + totalpower += numext::abs2(acc); complex x = promote(fftbuf[k0]); complex dif = acc - x; - difpower += norm(dif); - //cerr << k0 << "\t" << acc << "\t" << x << "\t" << sqrt(norm(dif)) << endl; + difpower += numext::abs2(dif); + //cerr << k0 << "\t" << acc << "\t" << x << "\t" << sqrt(numext::abs2(dif)) << endl; } cerr << "rmse:" << sqrt(difpower/totalpower) << endl; return sqrt(difpower/totalpower); @@ -57,8 +54,8 @@ complex promote(long double x) { return complex( x); long double difpower=0; size_t n = (min)( buf1.size(),buf2.size() ); for (size_t k=0;k Date: Wed, 7 May 2014 10:42:15 -0400 Subject: [PATCH 050/101] Warning for Clang 5.0+ --- gtsam/3rdparty/metis-5.1.0/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt b/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt index 8497973b2..4c4e2c03f 100644 --- a/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt +++ b/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt @@ -4,7 +4,9 @@ project(METIS) # Add flags for currect directory and below if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") add_definitions(-Wno-unused-variable) - add_definitions(-Wno-sometimes-uninitialized) + if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 5.0 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 5.0) + add_definitions(-Wno-sometimes-uninitialized) + endif() endif() add_definitions(-Wno-unknown-pragmas) From d725dd481641ce7c01593107c55770ba0397edda Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 7 May 2014 18:17:12 -0700 Subject: [PATCH 051/101] Renamed unit tests that still had 'Unordered' in the name --- ...testGaussianBayesNetUnordered.cpp => testGaussianBayesNet.cpp} | 0 ...stGaussianBayesTreeUnordered.cpp => testGaussianBayesTree.cpp} | 0 ...ussianConditionalUnordered.cpp => testGaussianConditional.cpp} | 0 ...ussianFactorGraphUnordered.cpp => testGaussianFactorGraph.cpp} | 0 .../{testHessianFactorUnordered.cpp => testHessianFactor.cpp} | 0 .../{testJacobianFactorUnordered.cpp => testJacobianFactor.cpp} | 0 .../tests/{testVectorValuesUnordered.cpp => testVectorValues.cpp} | 0 .../{testVariableIndexUnordered.cpp => testVariableIndex.cpp} | 0 tests/{testGaussianBayesTree.cpp => testGaussianBayesTreeB.cpp} | 0 9 files changed, 0 insertions(+), 0 deletions(-) rename gtsam/linear/tests/{testGaussianBayesNetUnordered.cpp => testGaussianBayesNet.cpp} (100%) rename gtsam/linear/tests/{testGaussianBayesTreeUnordered.cpp => testGaussianBayesTree.cpp} (100%) rename gtsam/linear/tests/{testGaussianConditionalUnordered.cpp => testGaussianConditional.cpp} (100%) rename gtsam/linear/tests/{testGaussianFactorGraphUnordered.cpp => testGaussianFactorGraph.cpp} (100%) rename gtsam/linear/tests/{testHessianFactorUnordered.cpp => testHessianFactor.cpp} (100%) rename gtsam/linear/tests/{testJacobianFactorUnordered.cpp => testJacobianFactor.cpp} (100%) rename gtsam/linear/tests/{testVectorValuesUnordered.cpp => testVectorValues.cpp} (100%) rename gtsam/symbolic/tests/{testVariableIndexUnordered.cpp => testVariableIndex.cpp} (100%) rename tests/{testGaussianBayesTree.cpp => testGaussianBayesTreeB.cpp} (100%) diff --git a/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp similarity index 100% rename from gtsam/linear/tests/testGaussianBayesNetUnordered.cpp rename to gtsam/linear/tests/testGaussianBayesNet.cpp diff --git a/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp similarity index 100% rename from gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp rename to gtsam/linear/tests/testGaussianBayesTree.cpp diff --git a/gtsam/linear/tests/testGaussianConditionalUnordered.cpp b/gtsam/linear/tests/testGaussianConditional.cpp similarity index 100% rename from gtsam/linear/tests/testGaussianConditionalUnordered.cpp rename to gtsam/linear/tests/testGaussianConditional.cpp diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp similarity index 100% rename from gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp rename to gtsam/linear/tests/testGaussianFactorGraph.cpp diff --git a/gtsam/linear/tests/testHessianFactorUnordered.cpp b/gtsam/linear/tests/testHessianFactor.cpp similarity index 100% rename from gtsam/linear/tests/testHessianFactorUnordered.cpp rename to gtsam/linear/tests/testHessianFactor.cpp diff --git a/gtsam/linear/tests/testJacobianFactorUnordered.cpp b/gtsam/linear/tests/testJacobianFactor.cpp similarity index 100% rename from gtsam/linear/tests/testJacobianFactorUnordered.cpp rename to gtsam/linear/tests/testJacobianFactor.cpp diff --git a/gtsam/linear/tests/testVectorValuesUnordered.cpp b/gtsam/linear/tests/testVectorValues.cpp similarity index 100% rename from gtsam/linear/tests/testVectorValuesUnordered.cpp rename to gtsam/linear/tests/testVectorValues.cpp diff --git a/gtsam/symbolic/tests/testVariableIndexUnordered.cpp b/gtsam/symbolic/tests/testVariableIndex.cpp similarity index 100% rename from gtsam/symbolic/tests/testVariableIndexUnordered.cpp rename to gtsam/symbolic/tests/testVariableIndex.cpp diff --git a/tests/testGaussianBayesTree.cpp b/tests/testGaussianBayesTreeB.cpp similarity index 100% rename from tests/testGaussianBayesTree.cpp rename to tests/testGaussianBayesTreeB.cpp From 473052748182f74464df8f87631828682479617a Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Thu, 8 May 2014 11:46:42 -0400 Subject: [PATCH 052/101] testSerializationLinear passes. Fill in the lower triangle part of matrix_ so that boost::serialization won't throw the input_stream exception due to uninitialized data --- gtsam/base/SymmetricBlockMatrix.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 45edbf6af..5dcc79a68 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -236,6 +236,10 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { + // Fill in the lower triangle part of the matrix, so boost::serialization won't + // complain about uninitialized data with an input_stream_error exception + // http://www.boost.org/doc/libs/1_37_0/libs/serialization/doc/exceptions.html#stream_error + matrix_.triangularView() = matrix_.triangularView().transpose(); ar & BOOST_SERIALIZATION_NVP(matrix_); ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); ar & BOOST_SERIALIZATION_NVP(blockStart_); From aa942a664b88065c68d9ef905dc8c5841e273e16 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 12 May 2014 12:41:39 -0400 Subject: [PATCH 053/101] Fix for static linking to boost --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d03f11ede..196b7e4df 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -91,10 +91,10 @@ set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator") # If using Boost shared libs, disable auto linking if(MSVC) # Some libraries, at least Boost Program Options, rely on this to export DLL symbols - add_definitions(-DBOOST_ALL_DYN_LINK) # Disable autolinking if(NOT Boost_USE_STATIC_LIBS) add_definitions(-DBOOST_ALL_NO_LIB) + add_definitions(-DBOOST_ALL_DYN_LINK) endif() endif() From 5b717c801ff94f71f0ab025ca440e51c8c85928b Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 13 May 2014 21:13:16 -0400 Subject: [PATCH 054/101] Moved eclipse targets into tests folder --- .cproject | 2010 ++++++++++++++++++++++++++--------------------------- 1 file changed, 1000 insertions(+), 1010 deletions(-) diff --git a/.cproject b/.cproject index 49afd8e90..7a99a565d 100644 --- a/.cproject +++ b/.cproject @@ -542,6 +542,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -568,7 +576,6 @@ make - tests/testBayesTree.run true false @@ -576,7 +583,6 @@ make - testBinaryBayesNet.run true false @@ -624,7 +630,6 @@ make - testSymbolicBayesNet.run true false @@ -632,7 +637,6 @@ make - tests/testSymbolicFactor.run true false @@ -640,7 +644,6 @@ make - testSymbolicFactorGraph.run true false @@ -656,7 +659,6 @@ make - tests/testBayesTree true false @@ -678,6 +680,134 @@ true true + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + 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 @@ -718,6 +848,62 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -734,6 +920,118 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + clean all + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j6 -j8 + testAntiFactor.run + true + true + true + + + make + -j6 -j8 + testBetweenFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + make -j5 @@ -774,30 +1072,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - make -j5 @@ -846,6 +1120,38 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + clean + true + true + true + make -j5 @@ -1014,6 +1320,291 @@ true true + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + + make + -j2 + testVSLAMGraph + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j5 + testMarginals.run + true + true + true + + + make + -j5 + testGaussianISAM2.run + true + true + true + + + make + -j5 + testSymbolicFactorGraphB.run + true + true + true + + + make + -j2 + timeSequentialOnDataset.run + true + true + true + + + make + -j5 + testGradientDescentOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + -j2 + testNonlinearISAM.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + + + make + -j5 + timing.tests + true + true + true + + + make + -j5 + testNonlinearFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + testGraph.run + true + false + true + + + make + 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 + testSummarization.run + true + true + true + make -j5 @@ -1120,7 +1711,6 @@ make - testErrors.run true false @@ -1166,6 +1756,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -1350,6 +1948,54 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -1456,6 +2102,7 @@ make + testSimulated2DOriented.run true false @@ -1495,6 +2142,7 @@ make + testSimulated2D.run true false @@ -1502,6 +2150,7 @@ make + testSimulated3D.run true false @@ -1515,6 +2164,22 @@ true true + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + make -j5 @@ -1555,6 +2220,182 @@ false true + + make + -j2 + SimpleRotation.run + true + true + true + + + make + -j5 + CameraResectioning.run + true + true + true + + + make + -j5 + PlanarSLAMExample.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + easyPoint2KalmanFilter.run + true + true + true + + + make + -j2 + elaboratePoint2KalmanFilter.run + true + true + true + + + make + -j5 + Pose2SLAMExample.run + true + true + true + + + make + -j2 + Pose2SLAMwSPCG_easy.run + true + true + true + + + make + -j5 + UGM_small.run + true + true + true + + + make + -j5 + LocalizationExample.run + true + true + true + + + make + -j5 + OdometryExample.run + true + true + true + + + make + -j5 + RangeISAMExample_plaza2.run + true + true + true + + + make + -j5 + SelfCalibrationExample.run + true + true + true + + + make + -j5 + SFMExample.run + true + true + true + + + make + -j5 + VisualISAMExample.run + true + true + true + + + make + -j5 + VisualISAM2Example.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graphviz.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graph.run + true + true + true + + + make + -j5 + SFMExample_bal.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + + tests/testGaussianISAM2 + true + false + true + make -j2 @@ -1571,6 +2412,102 @@ true true + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + make -j3 @@ -1772,7 +2709,6 @@ cpack - -G DEB true false @@ -1780,7 +2716,6 @@ cpack - -G RPM true false @@ -1788,7 +2723,6 @@ cpack - -G TGZ true false @@ -1796,7 +2730,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -1970,6 +2903,38 @@ true true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + make -j2 @@ -2009,981 +2974,6 @@ false true - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j6 -j8 - testAntiFactor.run - true - true - true - - - make - -j6 -j8 - testBetweenFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testDiscreteFactor.run - true - true - true - - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - true - true - - - make - -j2 - vSFMexample.run - true - true - true - - - make - -j2 - testVSLAMGraph - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j5 - testMarginals.run - true - true - true - - - make - -j5 - testGaussianISAM2.run - true - true - true - - - make - -j5 - testSymbolicFactorGraphB.run - true - true - true - - - make - -j2 - timeSequentialOnDataset.run - true - true - true - - - make - -j5 - testGradientDescentOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - -j2 - testNonlinearISAM.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testExtendedKalmanFilter.run - true - true - true - - - make - -j5 - timing.tests - true - true - true - - - make - -j5 - testNonlinearFactor.run - true - true - true - - - make - -j5 - clean - true - true - true - - - make - -j5 - testGaussianJunctionTreeB.run - true - true - true - - - make - - testGraph.run - true - false - true - - - make - - 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 - testSummarization.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j5 - testVector.run - true - true - true - - - make - -j5 - testMatrix.run - true - true - true - - - make - -j2 - SimpleRotation.run - true - true - true - - - make - -j5 - CameraResectioning.run - true - true - true - - - make - -j5 - PlanarSLAMExample.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j5 - Pose2SLAMExample.run - true - true - true - - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - - - make - -j5 - UGM_small.run - true - true - true - - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - OdometryExample.run - true - true - true - - - make - -j5 - RangeISAMExample_plaza2.run - true - true - true - - - make - -j5 - SelfCalibrationExample.run - true - true - true - - - make - -j5 - SFMExample.run - true - true - true - - - make - -j5 - VisualISAMExample.run - true - true - true - - - make - -j5 - VisualISAM2Example.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graphviz.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graph.run - true - true - true - - - make - -j5 - SFMExample_bal.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - tests/testGaussianISAM2 - true - false - true - - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap - true - true - true - From e1c0ad83359e38fcdaf6670ee47da9314dab6ccc Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 14 May 2014 14:08:29 -0400 Subject: [PATCH 055/101] Changed 1e-20 to 1e-16. 1e-20 was cutting it too close on 32-bit system, resulting in divide by zero later --- gtsam/geometry/Unit3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 88359e0f8..db4c8da83 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -151,9 +151,9 @@ Vector Unit3::localCoordinates(const Unit3& y) const { double dot = p.dot(q); // Check for special cases - if (std::abs(dot - 1.0) < 1e-20) + if (std::abs(dot - 1.0) < 1e-16) return (Vector(2) << 0, 0); - else if (std::abs(dot + 1.0) < 1e-20) + else if (std::abs(dot + 1.0) < 1e-16) return (Vector(2) << M_PI, 0); else { // no special case From d0a2db6646b31d8e06bcf95a4f2a84f7dc96de0f Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 14 May 2014 19:14:38 -0400 Subject: [PATCH 056/101] Fix for infinite loop on 32-bit Linux. --- gtsam/nonlinear/DoglegOptimizerImpl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index a6c04681b..dea4113f7 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -191,7 +191,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY) stay = false; // If not searching, just return with the new Delta else if(mode == SEARCH_EACH_ITERATION) { - if(newDelta == Delta || lastAction == DECREASED_DELTA) + if(fabs(newDelta - Delta) < 1e-15 || lastAction == DECREASED_DELTA) stay = false; // Searching, but Newton's solution is within trust region so keep the same trust region else { stay = true; // Searching and increased Delta, so try again to increase Delta From 814b01a995f54911d1c35d61aa4e5b7cd2162c0d Mon Sep 17 00:00:00 2001 From: Natesh Srinivasan Date: Thu, 22 May 2014 12:09:31 -0400 Subject: [PATCH 057/101] Added a fix to install unsupported Eigen --- gtsam/3rdparty/CMakeLists.txt | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 5822a51f5..666c340f5 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -16,13 +16,30 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) endif() endforeach(eigen_dir) + # do the same for the unsupported eigen folder + file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h") + + file(GLOB unsupported_eigen_dir_headers_all "Eigen/unsupported/Eigen/*") + foreach(unsupported_eigen_dir ${unsupported_eigen_dir_headers_all}) + get_filename_component(filename ${unsupported_eigen_dir} NAME) + if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn"))) + list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}") + install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/unsupported/Eigen) + endif() + endforeach(unsupported_eigen_dir) + # Add to project source set(eigen_headers ${eigen_headers} PARENT_SCOPE) + # set(unsupported_eigen_headers ${unsupported_eigen_headers} PARENT_SCOPE) # install Eigen - only the headers in our 3rdparty directory install(DIRECTORY Eigen/Eigen DESTINATION include/gtsam/3rdparty/Eigen FILES_MATCHING PATTERN "*.h") + + install(DIRECTORY Eigen/unsupported/Eigen + DESTINATION include/gtsam/3rdparty/unsupported/ + FILES_MATCHING PATTERN "*.h") endif() option(GTSAM_BUILD_METIS "Build metis library" ON) From b08894564146d252c4a5de019ad40783c5cdd6b3 Mon Sep 17 00:00:00 2001 From: Natesh Srinivasan Date: Thu, 22 May 2014 13:33:02 -0400 Subject: [PATCH 058/101] In response to Chris's comment --- gtsam/3rdparty/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 666c340f5..576da93bd 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -24,7 +24,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) get_filename_component(filename ${unsupported_eigen_dir} NAME) if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn"))) list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}") - install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/unsupported/Eigen) + install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/unsupported/Eigen) endif() endforeach(unsupported_eigen_dir) @@ -38,7 +38,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) FILES_MATCHING PATTERN "*.h") install(DIRECTORY Eigen/unsupported/Eigen - DESTINATION include/gtsam/3rdparty/unsupported/ + DESTINATION include/gtsam/3rdparty/Eigen/unsupported/ FILES_MATCHING PATTERN "*.h") endif() From 4f91f94d1e8f03d29385ab7830a7dcbb331704fe Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 22 May 2014 21:27:46 -0400 Subject: [PATCH 059/101] included possibility to linearize to Jacobian for smart Pose factors --- gtsam_unstable/slam/SmartFactorBase.h | 12 ++++++++++ gtsam_unstable/slam/SmartProjectionFactor.h | 13 +++++++++++ .../slam/SmartProjectionPoseFactor.h | 22 +++++++++++++++---- 3 files changed, 43 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index d24a878bb..265df5ecd 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -20,6 +20,7 @@ #pragma once #include "JacobianFactorQ.h" +#include "JacobianFactorSVD.h" #include "ImplicitSchurFactor.h" #include "RegularHessianFactor.h" @@ -629,6 +630,17 @@ public: return boost::make_shared < JacobianFactorQ > (Fblocks, E, PointCov, b); } + // **************************************************************************************************** + boost::shared_ptr createJacobianSVDFactor( + const Cameras& cameras, const Point3& point, double lambda = 0.0) const { + size_t numKeys = this->keys_.size(); + std::vector < KeyMatrix2D > Fblocks; + Vector b; + Matrix Enull(2*numKeys, 2*numKeys-3); + computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); + return boost::make_shared< JacobianFactorSVD<6> >(Fblocks, Enull, b); + } + private: /// Serialization function diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 59a841813..786c578fd 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -54,6 +54,10 @@ public: double f; }; +enum linearizationType { + HESSIAN, JACOBIAN_SVD, JACOBIAN_Q +}; + /** * SmartProjectionFactor: triangulates point * TODO: why LANDMARK parameter? @@ -400,6 +404,15 @@ public: return boost::shared_ptr >(); } + /// different (faster) way to compute Jacobian factor + boost::shared_ptr createJacobianSVDFactor(const Cameras& cameras, + double lambda) const { + if (triangulateForLinearize(cameras)) + return Base::createJacobianQFactor(cameras, point_, lambda); + else + return boost::shared_ptr >(); + } + /// Returns true if nonDegenerate bool computeCamerasAndTriangulate(const Values& values, Cameras& myCameras) const { diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactor.h b/gtsam_unstable/slam/SmartProjectionPoseFactor.h index 879e5ab80..4a565d7dd 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactor.h @@ -31,6 +31,8 @@ template class SmartProjectionPoseFactor: public SmartProjectionFactor { protected: + linearizationType linearizeTo_; + // Known calibration std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) @@ -56,8 +58,9 @@ public: */ SmartProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor) {} + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + linearizationType linearizeTo = HESSIAN) : + Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor), linearizeTo_(linearizeTo) {} /** Virtual destructor */ virtual ~SmartProjectionPoseFactor() {} @@ -139,11 +142,22 @@ public: } /** - * linearize returns a Hessian factor contraining the poses + * linear factor on the poses */ virtual boost::shared_ptr linearize( const Values& values) const { - return this->createHessianFactor(cameras(values)); + // depending on flag set on construction we may linearize to different linear factors + switch(linearizeTo_){ + case JACOBIAN_SVD : + return this->createJacobianSVDFactor(cameras(values), 0.0); + break; + case JACOBIAN_Q : + return this->createJacobianQFactor(cameras(values), 0.0); + break; + default: + return this->createHessianFactor(cameras(values)); + break; + } } /** From e0500f7b186c8c770e4d77c6504c86022c7607e2 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 22 May 2014 21:28:00 -0400 Subject: [PATCH 060/101] added and renamed unit tests --- .cproject | 378 +++++++++--------- ....cpp => testSmartProjectionPoseFactor.cpp} | 124 ++++++ 2 files changed, 317 insertions(+), 185 deletions(-) rename gtsam_unstable/slam/tests/{testSmartProjectionHessianPoseFactor.cpp => testSmartProjectionPoseFactor.cpp} (89%) diff --git a/.cproject b/.cproject index 7a99a565d..8e20710ec 100644 --- a/.cproject +++ b/.cproject @@ -1,19 +1,17 @@ - - - + - - + + @@ -62,13 +60,13 @@ - - + + @@ -118,13 +116,13 @@ - - + + @@ -542,14 +540,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -576,6 +566,7 @@ make + tests/testBayesTree.run true false @@ -583,6 +574,7 @@ make + testBinaryBayesNet.run true false @@ -630,6 +622,7 @@ make + testSymbolicBayesNet.run true false @@ -637,6 +630,7 @@ make + tests/testSymbolicFactor.run true false @@ -644,6 +638,7 @@ make + testSymbolicFactorGraph.run true false @@ -659,11 +654,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -888,22 +892,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -920,6 +908,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -1120,6 +1124,14 @@ true true + + make + -j5 + testSmartProjectionPoseFactor.run + true + true + true + make -j2 @@ -1304,14 +1316,6 @@ true true - - make - -j5 - testSmartProjectionFactor.run - true - true - true - make -j5 @@ -1530,6 +1534,7 @@ make + testGraph.run true false @@ -1537,6 +1542,7 @@ make + testJunctionTree.run true false @@ -1544,6 +1550,7 @@ make + testSymbolicBayesNetB.run true false @@ -1711,6 +1718,7 @@ make + testErrors.run true false @@ -1756,22 +1764,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j5 - testParticleFactor.run - true - true - true - make -j2 @@ -1852,6 +1844,22 @@ true true + + make + -j5 + testParticleFactor.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -2004,22 +2012,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j2 @@ -2102,7 +2094,6 @@ make - testSimulated2DOriented.run true false @@ -2142,7 +2133,6 @@ make - testSimulated2D.run true false @@ -2150,7 +2140,6 @@ make - testSimulated3D.run true false @@ -2164,6 +2153,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j5 @@ -2390,7 +2395,6 @@ make - tests/testGaussianISAM2 true false @@ -2412,102 +2416,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2709,6 +2617,7 @@ cpack + -G DEB true false @@ -2716,6 +2625,7 @@ cpack + -G RPM true false @@ -2723,6 +2633,7 @@ cpack + -G TGZ true false @@ -2730,6 +2641,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2903,34 +2815,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2974,6 +2950,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp similarity index 89% rename from gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp rename to gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp index 9a3fe7f62..e55084ed7 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp @@ -369,6 +369,130 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ if(isDebugTest) tictoc_print_(); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, jacobianSVD ){ + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3*noise_pose); + + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, jacobianQ ){ + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3*noise_pose); + + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; From c2705902cb05a01795098d13b48a3eef8e663810 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 22 May 2014 22:32:12 -0400 Subject: [PATCH 061/101] added dynamic outlier rejection (with unit tests) for smart pose factors --- gtsam_unstable/slam/SmartProjectionFactor.h | 41 ++++- .../slam/SmartProjectionPoseFactor.h | 6 +- .../tests/testSmartProjectionPoseFactor.cpp | 141 ++++++++++++++++++ 3 files changed, 183 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 786c578fd..33730db8a 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -95,6 +95,13 @@ protected: /// shorthand for base class type typedef SmartFactorBase Base; + double landmarkDistanceThreshold_; // if the landmark is triangulated at a + // distance larger than that the factor is considered degenerate + + double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the + // average reprojection error is smaller than this threshold after triangulation, + // and the factor is disregarded if the error is large + /// shorthand for this class typedef SmartProjectionFactor This; @@ -119,12 +126,15 @@ public: SmartProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, boost::optional body_P_sensor = boost::none, - SmartFactorStatePtr state = SmartFactorStatePtr( - new SmartProjectionFactorState())) : + double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1, + SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( - false), verboseCheirality_(false), state_(state) { + false), verboseCheirality_(false), state_(state), + landmarkDistanceThreshold_(landmarkDistanceThreshold), + dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) { } /** Virtual destructor */ @@ -238,6 +248,31 @@ public: rankTolerance_, enableEPI_); degenerate_ = false; cheiralityException_ = false; + + // Check landmark distance and reprojection errors to avoid outliers + double totalReprojError = 0.0; + size_t i=0; + BOOST_FOREACH(const Camera& camera, cameras) { + Point3 cameraTranslation = camera.pose().translation(); + // we discard smart factors corresponding to points that are far away + if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){ + degenerate_ = true; + break; + } + const Point2& zi = this->measured_.at(i); + try { + Point2 reprojectionError(camera.project(point_) - zi); + totalReprojError += reprojectionError.vector().norm(); + } catch (CheiralityException& e) { + cheiralityException_ = true; + } + i += 1; + } + // we discard smart factors that have large reprojection error + if(dynamicOutlierRejectionThreshold_ > 0 && + totalReprojError/m > dynamicOutlierRejectionThreshold_) + degenerate_ = true; + } catch (TriangulationUnderconstrainedException&) { // if TriangulationUnderconstrainedException can be // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactor.h b/gtsam_unstable/slam/SmartProjectionPoseFactor.h index 4a565d7dd..9548e10c8 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactor.h @@ -59,8 +59,10 @@ public: SmartProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, const bool enableEPI = false, boost::optional body_P_sensor = boost::none, - linearizationType linearizeTo = HESSIAN) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor), linearizeTo_(linearizeTo) {} + linearizationType linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1) : + Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, + landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {} /** Virtual destructor */ virtual ~SmartProjectionPoseFactor() {} diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp index e55084ed7..ee8db1267 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp @@ -431,6 +431,147 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){ EXPECT(assert_equal(pose3,result.at(x3))); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, landmarkDistance ){ + + double excludeLandmarksFutherThanDist = 2; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3*noise_pose); + + // All factors are disabled and pose should remain where it is + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3),result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + Point3 landmark4(5, -0.5, 1); + + vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10,10); // add outlier + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, + JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor3->add(measurements_cam3, views, model, K); + + SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + smartFactor4->add(measurements_cam4, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3); + + // All factors are disabled and pose should remain where it is + LevenbergMarquardtParams params; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3,result.at(x3))); +} + /* *************************************************************************/ TEST( SmartProjectionPoseFactor, jacobianQ ){ From 8806c15b36d94d3bfd96fe5b91e5c9f41a192022 Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 23 May 2014 17:49:42 -0400 Subject: [PATCH 062/101] fixed possible connectivity issue when smart factors are degenerate --- .cproject | 354 ++++++++++---------- gtsam_unstable/slam/JacobianFactorQ.h | 13 + gtsam_unstable/slam/JacobianFactorSVD.h | 14 +- gtsam_unstable/slam/SmartProjectionFactor.h | 10 +- 4 files changed, 207 insertions(+), 184 deletions(-) diff --git a/.cproject b/.cproject index 8e20710ec..fa8c24875 100644 --- a/.cproject +++ b/.cproject @@ -540,6 +540,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -566,7 +574,6 @@ make - tests/testBayesTree.run true false @@ -574,7 +581,6 @@ make - testBinaryBayesNet.run true false @@ -622,7 +628,6 @@ make - testSymbolicBayesNet.run true false @@ -630,7 +635,6 @@ make - tests/testSymbolicFactor.run true false @@ -638,7 +642,6 @@ make - testSymbolicFactorGraph.run true false @@ -654,20 +657,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -892,22 +886,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -924,6 +902,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -1260,6 +1254,14 @@ true true + + make + -j5 + testAttitudeFactor.run + true + true + true + make -j5 @@ -1534,7 +1536,6 @@ make - testGraph.run true false @@ -1542,7 +1543,6 @@ make - testJunctionTree.run true false @@ -1550,7 +1550,6 @@ make - testSymbolicBayesNetB.run true false @@ -1718,7 +1717,6 @@ make - testErrors.run true false @@ -1764,6 +1762,22 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j5 + testParticleFactor.run + true + true + true + make -j2 @@ -1844,22 +1858,6 @@ true true - - make - -j5 - testParticleFactor.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -2012,6 +2010,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j2 @@ -2094,6 +2108,7 @@ make + testSimulated2DOriented.run true false @@ -2133,6 +2148,7 @@ make + testSimulated2D.run true false @@ -2140,6 +2156,7 @@ make + testSimulated3D.run true false @@ -2153,22 +2170,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j5 @@ -2395,6 +2396,7 @@ make + tests/testGaussianISAM2 true false @@ -2416,6 +2418,102 @@ true true + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + make -j3 @@ -2617,7 +2715,6 @@ cpack - -G DEB true false @@ -2625,7 +2722,6 @@ cpack - -G RPM true false @@ -2633,7 +2729,6 @@ cpack - -G TGZ true false @@ -2641,7 +2736,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2815,98 +2909,34 @@ true true - + make - -j2 - testRot3.run + -j5 + testSpirit.run true true true - + make - -j2 - testRot2.run + -j5 + testWrap.run true true true - + make - -j2 - testPose3.run + -j5 + check.wrap true true true - + make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run + -j5 + wrap true true true @@ -2950,38 +2980,6 @@ false true - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap - true - true - true - diff --git a/gtsam_unstable/slam/JacobianFactorQ.h b/gtsam_unstable/slam/JacobianFactorQ.h index fdbe0e231..f4dfb9422 100644 --- a/gtsam_unstable/slam/JacobianFactorQ.h +++ b/gtsam_unstable/slam/JacobianFactorQ.h @@ -23,6 +23,19 @@ public: JacobianFactorQ() { } + /// Empty constructor with keys + JacobianFactorQ(const FastVector& keys, + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + Matrix zeroMatrix = Matrix::Zero(0,D); + Vector zeroVector = Vector::Zero(0); + typedef std::pair KeyMatrix; + std::vector QF; + QF.reserve(keys.size()); + BOOST_FOREACH(const Key& key, keys) + QF.push_back(KeyMatrix(key, zeroMatrix)); + JacobianFactor::fillTerms(QF, zeroVector, model); + } + /// Constructor JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, diff --git a/gtsam_unstable/slam/JacobianFactorSVD.h b/gtsam_unstable/slam/JacobianFactorSVD.h index 752a9f48e..e8ade3b1b 100644 --- a/gtsam_unstable/slam/JacobianFactorSVD.h +++ b/gtsam_unstable/slam/JacobianFactorSVD.h @@ -18,10 +18,23 @@ public: typedef Eigen::Matrix Matrix2D; typedef std::pair KeyMatrix2D; + typedef std::pair KeyMatrix; /// Default constructor JacobianFactorSVD() {} + /// Empty constructor with keys + JacobianFactorSVD(const FastVector& keys, + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + Matrix zeroMatrix = Matrix::Zero(0,D); + Vector zeroVector = Vector::Zero(0); + std::vector QF; + QF.reserve(keys.size()); + BOOST_FOREACH(const Key& key, keys) + QF.push_back(KeyMatrix(key, zeroMatrix)); + JacobianFactor::fillTerms(QF, zeroVector, model); + } + /// Constructor JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { @@ -32,7 +45,6 @@ public: // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // JacobianFactor factor(QF, Q * b); - typedef std::pair KeyMatrix; std::vector QF; QF.reserve(numKeys); BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 33730db8a..2124dd6f6 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -424,7 +424,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianQFactor(cameras, point_, lambda); else - return boost::shared_ptr >(); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// Create a factor, takes values @@ -436,16 +436,16 @@ public: if (nonDegenerate) return createJacobianQFactor(myCameras, lambda); else - return boost::shared_ptr >(); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// different (faster) way to compute Jacobian factor - boost::shared_ptr createJacobianSVDFactor(const Cameras& cameras, + boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) - return Base::createJacobianQFactor(cameras, point_, lambda); + return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::shared_ptr >(); + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate From 87c386d77fd67f6b6b4f18173513ae587167162a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 02:03:33 -0400 Subject: [PATCH 063/101] Fast creation of large key sets from within MATLAB --- gtsam.h | 1 + matlab.h | 8 ++++++++ matlab/+gtsam/Contents.m | 1 + 3 files changed, 10 insertions(+) diff --git a/gtsam.h b/gtsam.h index 2d181d350..71fcaeb17 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2375,6 +2375,7 @@ namespace utilities { void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); + gtsam::KeySet createKeySet(string s, Vector I); } //\namespace utilities diff --git a/matlab.h b/matlab.h index 100f5a242..261fd2d48 100644 --- a/matlab.h +++ b/matlab.h @@ -151,6 +151,14 @@ namespace gtsam { return errors; } + // Create a Keyset from indices + FastSet createKeySet(string s, const Vector& I) { + FastSet set; + char c = s[0]; + for(int i=0;i Date: Sun, 25 May 2014 10:49:59 -0400 Subject: [PATCH 064/101] Formatting --- matlab.h | 272 +++++++++++++++++++++++++++++-------------------------- 1 file changed, 144 insertions(+), 128 deletions(-) diff --git a/matlab.h b/matlab.h index 261fd2d48..b62c9866a 100644 --- a/matlab.h +++ b/matlab.h @@ -32,134 +32,150 @@ namespace gtsam { - namespace utilities { - - /// Extract all Point2 values into a single matrix [x y] - Matrix extractPoint2(const Values& values) { - size_t j=0; - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(),2); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) - result.row(j++) = key_value.value.vector(); - return result; - } - - /// Extract all Point3 values into a single matrix [x y z] - Matrix extractPoint3(const Values& values) { - Values::ConstFiltered points = values.filter(); - Matrix result(points.size(),3); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) - result.row(j++) = key_value.value.vector(); - return result; - } - - /// Extract all Pose2 values into a single matrix [x y theta] - Matrix extractPose2(const Values& values) { - Values::ConstFiltered poses = values.filter(); - Matrix result(poses.size(),3); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) - result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); - return result; - } - - /// Extract all Pose3 values - Values allPose3s(const Values& values) { - return values.filter(); - } - - /// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] - Matrix extractPose3(const Values& values) { - Values::ConstFiltered poses = values.filter(); - Matrix result(poses.size(),12); - size_t j=0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { - result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); - result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); - result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); - result.row(j).tail(3) = key_value.value.translation().vector(); - j++; - } - return result; - } - - /// Perturb all Point2 values using normally distributed noise - void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// Perturb all Pose2 values using normally distributed noise - void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = 42u) { - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( - Vector3(sigmaT, sigmaT, sigmaR)); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// Perturb all Point3 values using normally distributed noise - void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); - Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); - } - } - - /// Insert a number of initial point values by backprojecting - void insertBackprojections(Values& values, const SimpleCamera& camera, const Vector& J, const Matrix& Z, double depth) { - if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); - if (Z.cols() != J.size()) throw std::invalid_argument("insertBackProjections: J and Z must have same number of entries"); - for(int k=0;k > - (Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); - } - } - - /// Calculate the errors of all projection factors in a graph - Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) { - // first count - size_t K = 0, k=0; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) - if (boost::dynamic_pointer_cast >(f)) ++K; - // now fill - Matrix errors(2,K); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { - boost::shared_ptr > p = boost::dynamic_pointer_cast >(f); - if (p) errors.col(k++) = p->unwhitenedError(values); - } - return errors; - } - - // Create a Keyset from indices - FastSet createKeySet(string s, const Vector& I) { - FastSet set; - char c = s[0]; - for(int i=0;i points = values.filter(); + Matrix result(points.size(), 2); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; +} + +/// Extract all Point3 values into a single matrix [x y z] +Matrix extractPoint3(const Values& values) { + Values::ConstFiltered points = values.filter(); + Matrix result(points.size(), 3); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; +} + +/// Extract all Pose2 values into a single matrix [x y theta] +Matrix extractPose2(const Values& values) { + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(), 3); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) + result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); + return result; +} + +/// Extract all Pose3 values +Values allPose3s(const Values& values) { + return values.filter(); +} + +/// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] +Matrix extractPose3(const Values& values) { + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(), 12); + size_t j = 0; + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { + result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); + result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); + result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); + result.row(j).tail(3) = key_value.value.translation().vector(); + j++; + } + return result; +} + +/// Perturb all Point2 values using normally distributed noise +void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, + sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Perturb all Pose2 values using normally distributed noise +void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = + 42u) { + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( + Vector3(sigmaT, sigmaT, sigmaR)); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Perturb all Point3 values using normally distributed noise +void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3, + sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } +} + +/// Insert a number of initial point values by backprojecting +void insertBackprojections(Values& values, const SimpleCamera& camera, + const Vector& J, const Matrix& Z, double depth) { + if (Z.rows() != 2) + throw std::invalid_argument("insertBackProjections: Z must be 2*K"); + if (Z.cols() != J.size()) + throw std::invalid_argument( + "insertBackProjections: J and Z must have same number of entries"); + for (int k = 0; k < Z.cols(); k++) { + Point2 p(Z(0, k), Z(1, k)); + Point3 P = camera.backproject(p, depth); + values.insert(J(k), P); + } +} + +/// Insert multiple projection factors for a single pose key +void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, + const Vector& J, const Matrix& Z, const SharedNoiseModel& model, + const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) { + if (Z.rows() != 2) + throw std::invalid_argument("addMeasurements: Z must be 2*K"); + if (Z.cols() != J.size()) + throw std::invalid_argument( + "addMeasurements: J and Z must have same number of entries"); + for (int k = 0; k < Z.cols(); k++) { + graph.push_back( + boost::make_shared >( + Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); + } +} + +/// Calculate the errors of all projection factors in a graph +Matrix reprojectionErrors(const NonlinearFactorGraph& graph, + const Values& values) { + // first count + size_t K = 0, k = 0; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) + if (boost::dynamic_pointer_cast >( + f)) + ++K; + // now fill + Matrix errors(2, K); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { + boost::shared_ptr > p = + boost::dynamic_pointer_cast >( + f); + if (p) + errors.col(k++) = p->unwhitenedError(values); + } + return errors; +} + +// Create a Keyset from indices +FastSet createKeySet(string s, const Vector& I) { + FastSet set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.insert(symbol(c, I[i])); + return set; +} + +} } From ddcf9c0efb622515a7079e36614500d8e33096a7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 11:15:17 -0400 Subject: [PATCH 065/101] New routines to create Key collections --- gtsam.h | 7 ++++++- matlab.h | 60 +++++++++++++++++++++++++++++++++++++++++++++++--------- 2 files changed, 57 insertions(+), 10 deletions(-) diff --git a/gtsam.h b/gtsam.h index 71fcaeb17..99aec3f86 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2363,6 +2363,12 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor { namespace utilities { #include + gtsam::KeyList createKeyList(Vector I); + gtsam::KeyList createKeyList(string s, Vector I); + gtsam::KeyVector createKeyVector(Vector I); + gtsam::KeyVector createKeyVector(string s, Vector I); + gtsam::KeySet createKeySet(Vector I); + gtsam::KeySet createKeySet(string s, Vector I); Matrix extractPoint2(const gtsam::Values& values); Matrix extractPoint3(const gtsam::Values& values); Matrix extractPose2(const gtsam::Values& values); @@ -2375,7 +2381,6 @@ namespace utilities { void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); - gtsam::KeySet createKeySet(string s, Vector I); } //\namespace utilities diff --git a/matlab.h b/matlab.h index b62c9866a..e9ad519bd 100644 --- a/matlab.h +++ b/matlab.h @@ -34,6 +34,57 @@ namespace gtsam { namespace utilities { +// Create a KeyList from indices +FastList createKeyList(const Vector& I) { + FastList set; + for (int i = 0; i < I.size(); i++) + set.push_back(I[i]); + return set; +} + +// Create a KeyList from indices using symbol +FastList createKeyList(string s, const Vector& I) { + FastList set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.push_back(Symbol(c, I[i])); + return set; +} + +// Create a KeyVector from indices +FastVector createKeyVector(const Vector& I) { + FastVector set; + for (int i = 0; i < I.size(); i++) + set.push_back(I[i]); + return set; +} + +// Create a KeyVector from indices using symbol +FastVector createKeyVector(string s, const Vector& I) { + FastVector set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.push_back(Symbol(c, I[i])); + return set; +} + +// Create a KeySet from indices +FastSet createKeySet(const Vector& I) { + FastSet set; + for (int i = 0; i < I.size(); i++) + set.insert(I[i]); + return set; +} + +// Create a KeySet from indices using symbol +FastSet createKeySet(string s, const Vector& I) { + FastSet set; + char c = s[0]; + for (int i = 0; i < I.size(); i++) + set.insert(symbol(c, I[i])); + return set; +} + /// Extract all Point2 values into a single matrix [x y] Matrix extractPoint2(const Values& values) { size_t j = 0; @@ -167,15 +218,6 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph, return errors; } -// Create a Keyset from indices -FastSet createKeySet(string s, const Vector& I) { - FastSet set; - char c = s[0]; - for (int i = 0; i < I.size(); i++) - set.insert(symbol(c, I[i])); - return set; -} - } } From ab4bb159e8871b4e4dc51d0774c4c421026f1973 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 11:15:49 -0400 Subject: [PATCH 066/101] Unit tests for new routines (and they *failed* at first, as I had a bug) --- matlab/gtsam_tests/testUtilities.m | 47 ++++++++++++++++++++++++++++++ matlab/gtsam_tests/test_gtsam.m | 3 ++ 2 files changed, 50 insertions(+) create mode 100644 matlab/gtsam_tests/testUtilities.m diff --git a/matlab/gtsam_tests/testUtilities.m b/matlab/gtsam_tests/testUtilities.m new file mode 100644 index 000000000..da8dec789 --- /dev/null +++ b/matlab/gtsam_tests/testUtilities.m @@ -0,0 +1,47 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 +% +% @brief Checks for results of functions in utilities namespace +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +import gtsam.* + +%% Create keys for variables +x1 = symbol('x',1); x2 = symbol('x',2); x3 = symbol('x',3); + +actual = utilities.createKeyList([1;2;3]); +CHECK('KeyList', isa(actual,'gtsam.KeyList')); +CHECK('size==3', actual.size==3); +CHECK('actual.front==1', actual.front==1); + +actual = utilities.createKeyList('x',[1;2;3]); +CHECK('KeyList', isa(actual,'gtsam.KeyList')); +CHECK('size==3', actual.size==3); +CHECK('actual.front==x1', actual.front==x1); + +actual = utilities.createKeyVector([1;2;3]); +CHECK('KeyVector', isa(actual,'gtsam.KeyVector')); +CHECK('size==3', actual.size==3); +CHECK('actual.at(0)==1', actual.at(0)==1); + +actual = utilities.createKeyVector('x',[1;2;3]); +CHECK('KeyVector', isa(actual,'gtsam.KeyVector')); +CHECK('size==3', actual.size==3); +CHECK('actual.at(0)==x1', actual.at(0)==x1); + +actual = utilities.createKeySet([1;2;3]); +CHECK('KeySet', isa(actual,'gtsam.KeySet')); +CHECK('size==3', actual.size==3); +CHECK('actual.count(1)', actual.count(1)); + +actual = utilities.createKeySet('x',[1;2;3]); +CHECK('KeySet', isa(actual,'gtsam.KeySet')); +CHECK('size==3', actual.size==3); +CHECK('actual.count(x1)', actual.count(x1)); + diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index 2cad40df8..c379179c5 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -33,5 +33,8 @@ testVisualISAMExample display 'Starting: testSerialization' testSerialization +display 'Starting: testUtilities' +testUtilities + % end of tests display 'Tests complete!' From 1762825c289c3d579fd5a0201d90f31419385cc1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 11:17:49 -0400 Subject: [PATCH 067/101] Forgot to update docs --- matlab/+gtsam/Contents.m | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index ac6842e8e..7a7491462 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -175,6 +175,9 @@ % symbolIndex - get index from a symbol key % %% Wrapped C++ Convenience Functions for use within MATLAB +% utilities.createKeyList - Create KeyList from indices +% utilities.createKeyVector - Create KeyVector from indices +% utilities.createKeySet - Create KeySet from indices % utilities.extractPoint2 - Extract all Point2 values into a single matrix [x y] % utilities.extractPoint3 - Extract all Point3 values into a single matrix [x y z] % utilities.extractPose2 - Extract all Pose2 values into a single matrix [x y theta] @@ -186,4 +189,3 @@ % utilities.insertBackprojections - Insert a number of initial point values by backprojecting % utilities.insertProjectionFactors - Insert multiple projection factors for a single pose key % utilities.reprojectionErrors - Calculate the errors of all projection factors in a graph -% utilities.createKeySet - Create keys from indices From 02c3fe9516e128dd50ec3ea55a2bfec6de846861 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 12:06:34 -0400 Subject: [PATCH 068/101] Standard BORG formatting --- wrap/Method.cpp | 168 +++++++++++++++++++++++++----------------------- 1 file changed, 88 insertions(+), 80 deletions(-) diff --git a/wrap/Method.cpp b/wrap/Method.cpp index b327ac7dc..3fbf6a9c0 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -38,57 +38,54 @@ void Method::addOverload(bool verbose, bool is_const, const std::string& name, } /* ************************************************************************* */ -void Method::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const string& cppClassName, - const std::string& matlabQualName, - const std::string& matlabUniqueName, - const string& wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { +void Method::proxy_wrapper_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const string& cppClassName, + const std::string& matlabQualName, const std::string& matlabUniqueName, + const string& wrapperName, const TypeAttributesTable& typeAttributes, + vector& functionNames) const { proxyFile.oss << " function varargout = " << name << "(this, varargin)\n"; //Comments for documentation - string up_name = boost::to_upper_copy(name); + string up_name = boost::to_upper_copy(name); proxyFile.oss << " % " << up_name << " usage:"; unsigned int argLCount = 0; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - if(argLCount != argLists.size()-1) - proxyFile.oss << "), "; - else - proxyFile.oss << ") : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; - argLCount++; + BOOST_FOREACH(ArgumentList argList, argLists) { + proxyFile.oss << " " << name << "("; + unsigned int i = 0; + BOOST_FOREACH(const Argument& arg, argList) { + if (i != argList.size() - 1) + proxyFile.oss << arg.type << " " << arg.name << ", "; + else + proxyFile.oss << arg.type << " " << arg.name; + i++; } + if (argLCount != argLists.size() - 1) + proxyFile.oss << "), "; + else + proxyFile.oss << ") : returns " + << returnVals[0].return_type(false, returnVals[0].pair) << endl; + argLCount++; + } - proxyFile.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; + proxyFile.oss << " % " + << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" + << endl; proxyFile.oss << " % " << "" << endl; proxyFile.oss << " % " << "Method Overloads" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " % " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")" << endl; + BOOST_FOREACH(ArgumentList argList, argLists) { + proxyFile.oss << " % " << name << "("; + unsigned int i = 0; + BOOST_FOREACH(const Argument& arg, argList) { + if (i != argList.size() - 1) + proxyFile.oss << arg.type << " " << arg.name << ", "; + else + proxyFile.oss << arg.type << " " << arg.name; + i++; } + proxyFile.oss << ")" << endl; + } - for(size_t overload = 0; overload < argLists.size(); ++overload) { + for (size_t overload = 0; overload < argLists.size(); ++overload) { const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; size_t nrArgs = args.size(); @@ -98,95 +95,106 @@ void Method::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperF // Output proxy matlab code // check for number of arguments... - proxyFile.oss << " " << (overload==0?"":"else") << "if length(varargin) == " << nrArgs; - if (nrArgs>0) proxyFile.oss << " && "; + proxyFile.oss << " " << (overload == 0 ? "" : "else") + << "if length(varargin) == " << nrArgs; + if (nrArgs > 0) + proxyFile.oss << " && "; // ...and their types bool first = true; - for(size_t i=0;i(id); + const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + + boost::lexical_cast(id); const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; // call - file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start file.oss << "{\n"; - if(returnVal.isPair) - { - if(returnVal.category1 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; - if(returnVal.category2 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 << ";"<< endl; - } - else - if(returnVal.category1 == ReturnValue::CLASS) - file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; + if (returnVal.isPair) { + if (returnVal.category1 == ReturnValue::CLASS) + file.oss << " typedef boost::shared_ptr<" + << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 + << ";" << endl; + if (returnVal.category2 == ReturnValue::CLASS) + file.oss << " typedef boost::shared_ptr<" + << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 + << ";" << endl; + } else if (returnVal.category1 == ReturnValue::CLASS) + file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") + << "> Shared" << returnVal.type1 << ";" << endl; - file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" + << endl; // check arguments // extra argument obj -> nargin-1 is passed ! // example: checkArguments("equals",nargout,nargin-1,2); - file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1," << args.size() << ");\n"; + file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1," + << args.size() << ");\n"; // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); - file.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; + file.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName + << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; // unwrap arguments, see Argument.cpp - args.matlab_unwrap(file,1); + args.matlab_unwrap(file, 1); // call method and wrap result // example: out[0]=wrap(self->return_field(t)); - if (returnVal.type1!="void") - returnVal.wrap_result("obj->"+name+"("+args.names()+")", file, typeAttributes); + if (returnVal.type1 != "void") + returnVal.wrap_result("obj->" + name + "(" + args.names() + ")", file, + typeAttributes); else - file.oss << " obj->"+name+"("+args.names()+");\n"; + file.oss << " obj->" + name + "(" + args.names() + ");\n"; // finish file.oss << "}\n"; From 82d6bae4b9150967035825ee50d3a84db4c95ae8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 12:43:19 -0400 Subject: [PATCH 069/101] Standard BORG formatting --- wrap/Class.cpp | 719 +++++++++++++++++++++++------------------- wrap/StaticMethod.cpp | 150 ++++----- 2 files changed, 474 insertions(+), 395 deletions(-) diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 3b8c41041..8788e740f 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -7,62 +7,62 @@ * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - + * -------------------------------------------------------------------------- */ + /** * @file Class.cpp * @author Frank Dellaert * @author Andrew Melim * @author Richard Roberts - **/ - + **/ + #include #include #include //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above #include // works on Linux GCC - #include #include - + #include "Class.h" #include "utilities.h" #include "Argument.h" - -using namespace std; -using namespace wrap; - -/* ************************************************************************* */ -void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, - const TypeAttributesTable& typeAttributes, - FileWriter& wrapperFile, vector& functionNames) const { - + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, + const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, + vector& functionNames) const { + // Create namespace folders - createNamespaceStructure(namespaces, toolboxPath); - + createNamespaceStructure(namespaces, toolboxPath); + // open destination classFile - string classFile = toolboxPath; - if(!namespaces.empty()) - classFile += "/+" + wrap::qualifiedName("/+", namespaces); - classFile += "/" + name + ".m"; - FileWriter proxyFile(classFile, verbose_, "%"); - + string classFile = toolboxPath; + if (!namespaces.empty()) + classFile += "/+" + wrap::qualifiedName("/+", namespaces); + classFile += "/" + name + ".m"; + FileWriter proxyFile(classFile, verbose_, "%"); + // get the name of actual matlab object - const string matlabQualName = qualifiedName("."), matlabUniqueName = qualifiedName(), cppName = qualifiedName("::"); - const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent); - const string cppBaseName = wrap::qualifiedName("::", qualifiedParent); - + const string matlabQualName = qualifiedName("."), matlabUniqueName = + qualifiedName(), cppName = qualifiedName("::"); + const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent); + const string cppBaseName = wrap::qualifiedName("::", qualifiedParent); + // emit class proxy code // we want our class to inherit the handle class for memory purposes - const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName; + const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName; comment_fragment(proxyFile); - proxyFile.oss << "classdef " << name << " < " << parent << endl; + proxyFile.oss << "classdef " << name << " < " << parent << endl; proxyFile.oss << " properties\n"; proxyFile.oss << " ptr_" << matlabUniqueName << " = 0\n"; proxyFile.oss << " end\n"; proxyFile.oss << " methods\n"; - + // Constructor proxyFile.oss << " function obj = " << name << "(varargin)\n"; // Special pointer constructors - one in MATLAB to create an object and @@ -72,267 +72,316 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName, // other wrap modules - to add these to their collectors the pointer is // passed from one C++ module into matlab then back into the other C++ // module. - pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, functionNames); - wrapperFile.oss << "\n"; + pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, + functionNames); + wrapperFile.oss << "\n"; // Regular constructors - BOOST_FOREACH(ArgumentList a, constructor.args_list) - { - const int id = (int)functionNames.size(); - constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), id, a); - const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, - cppName, matlabUniqueName, cppBaseName, id, a); - wrapperFile.oss << "\n"; - functionNames.push_back(wrapFunctionName); - } - proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of " << matlabQualName << " constructor');\n"; - proxyFile.oss << " end\n"; - if(!qualifiedParent.empty()) - proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" << ptr_constructor_key << "), base_ptr);\n"; - proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; - proxyFile.oss << " end\n\n"; - + BOOST_FOREACH(ArgumentList a, constructor.args_list) { + const int id = (int) functionNames.size(); + constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), + id, a); + const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, + cppName, matlabUniqueName, cppBaseName, id, a); + wrapperFile.oss << "\n"; + functionNames.push_back(wrapFunctionName); + } + proxyFile.oss << " else\n"; + proxyFile.oss << " error('Arguments do not match any overload of " + << matlabQualName << " constructor');\n"; + proxyFile.oss << " end\n"; + if (!qualifiedParent.empty()) + proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" + << ptr_constructor_key << "), base_ptr);\n"; + proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; + proxyFile.oss << " end\n\n"; + // Deconstructor - { - const int id = (int)functionNames.size(); - deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id); - proxyFile.oss << "\n"; - const string functionName = deconstructor.wrapper_fragment(wrapperFile, cppName, matlabUniqueName, id); - wrapperFile.oss << "\n"; - functionNames.push_back(functionName); - } - proxyFile.oss << " function display(obj), obj.print(''); end\n %DISPLAY Calls print on the object\n"; - proxyFile.oss << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; - + { + const int id = (int) functionNames.size(); + deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id); + proxyFile.oss << "\n"; + const string functionName = deconstructor.wrapper_fragment(wrapperFile, + cppName, matlabUniqueName, id); + wrapperFile.oss << "\n"; + functionNames.push_back(functionName); + } + proxyFile.oss + << " function display(obj), obj.print(''); end\n %DISPLAY Calls print on the object\n"; + proxyFile.oss + << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; + // Methods - BOOST_FOREACH(const Methods::value_type& name_m, methods) { - const Method& m = name_m.second; - m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); - proxyFile.oss << "\n"; - wrapperFile.oss << "\n"; - } + BOOST_FOREACH(const Methods::value_type& name_m, methods) { + const Method& m = name_m.second; + m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, + matlabUniqueName, wrapperName, typeAttributes, functionNames); + proxyFile.oss << "\n"; + wrapperFile.oss << "\n"; + } if (hasSerialization) serialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames); - - proxyFile.oss << " end\n"; - proxyFile.oss << "\n"; - proxyFile.oss << " methods(Static = true)\n"; - + + proxyFile.oss << " end\n"; + proxyFile.oss << "\n"; + proxyFile.oss << " methods(Static = true)\n"; + // Static methods - BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { - const StaticMethod& m = name_m.second; - m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); - proxyFile.oss << "\n"; - wrapperFile.oss << "\n"; - } + BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { + const StaticMethod& m = name_m.second; + m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, + matlabUniqueName, wrapperName, typeAttributes, functionNames); + proxyFile.oss << "\n"; + wrapperFile.oss << "\n"; + } if (hasSerialization) - deserialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames); + deserialization_fragments(proxyFile, wrapperFile, wrapperName, + functionNames); proxyFile.oss << " end\n"; proxyFile.oss << "end\n"; - + // Close file - proxyFile.emit(true); -} - -/* ************************************************************************* */ -string Class::qualifiedName(const string& delim) const { - return ::wrap::qualifiedName(delim, namespaces, name); -} - -/* ************************************************************************* */ -void Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const string& wrapperName, vector& functionNames) const { - - const string matlabUniqueName = qualifiedName(), cppName = qualifiedName("::"); - const string baseCppName = wrap::qualifiedName("::", qualifiedParent); - - const int collectorInsertId = (int)functionNames.size(); - const string collectorInsertFunctionName = matlabUniqueName + "_collectorInsertAndMakeBase_" + boost::lexical_cast(collectorInsertId); - functionNames.push_back(collectorInsertFunctionName); - - int upcastFromVoidId; - string upcastFromVoidFunctionName; - if(isVirtual) { - upcastFromVoidId = (int)functionNames.size(); - upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_" + boost::lexical_cast(upcastFromVoidId); - functionNames.push_back(upcastFromVoidFunctionName); - } - + proxyFile.emit(true); +} + +/* ************************************************************************* */ +string Class::qualifiedName(const string& delim) const { + return ::wrap::qualifiedName(delim, namespaces, name); +} + +/* ************************************************************************* */ +void Class::pointer_constructor_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const string& wrapperName, + vector& functionNames) const { + + const string matlabUniqueName = qualifiedName(), cppName = qualifiedName( + "::"); + const string baseCppName = wrap::qualifiedName("::", qualifiedParent); + + const int collectorInsertId = (int) functionNames.size(); + const string collectorInsertFunctionName = matlabUniqueName + + "_collectorInsertAndMakeBase_" + + boost::lexical_cast(collectorInsertId); + functionNames.push_back(collectorInsertFunctionName); + + int upcastFromVoidId; + string upcastFromVoidFunctionName; + if (isVirtual) { + upcastFromVoidId = (int) functionNames.size(); + upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_" + + boost::lexical_cast(upcastFromVoidId); + functionNames.push_back(upcastFromVoidFunctionName); + } + // MATLAB constructor that assigns pointer to matlab object then calls c++ // function to add the object to the collector. - if(isVirtual) { - proxyFile.oss << " if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))"; - } else { - proxyFile.oss << " if nargin == 2"; - } - proxyFile.oss << " && isa(varargin{1}, 'uint64') && varargin{1} == uint64(" << ptr_constructor_key << ")\n"; - if(isVirtual) { - proxyFile.oss << " if nargin == 2\n"; - proxyFile.oss << " my_ptr = varargin{2};\n"; - proxyFile.oss << " else\n"; - proxyFile.oss << " my_ptr = " << wrapperName << "(" << upcastFromVoidId << ", varargin{2});\n"; - proxyFile.oss << " end\n"; - } else { - proxyFile.oss << " my_ptr = varargin{2};\n"; - } - if(qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back - proxyFile.oss << " "; - else - proxyFile.oss << " base_ptr = "; + if (isVirtual) { + proxyFile.oss + << " if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))"; + } else { + proxyFile.oss << " if nargin == 2"; + } + proxyFile.oss << " && isa(varargin{1}, 'uint64') && varargin{1} == uint64(" + << ptr_constructor_key << ")\n"; + if (isVirtual) { + proxyFile.oss << " if nargin == 2\n"; + proxyFile.oss << " my_ptr = varargin{2};\n"; + proxyFile.oss << " else\n"; + proxyFile.oss << " my_ptr = " << wrapperName << "(" + << upcastFromVoidId << ", varargin{2});\n"; + proxyFile.oss << " end\n"; + } else { + proxyFile.oss << " my_ptr = varargin{2};\n"; + } + if (qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back + proxyFile.oss << " "; + else + proxyFile.oss << " base_ptr = "; proxyFile.oss << wrapperName << "(" << collectorInsertId << ", my_ptr);\n"; // Call collector insert and get base class ptr - + // C++ function to add pointer from MATLAB to collector. The pointer always // comes from a C++ return value; this mechanism allows the object to be added // to a collector in a different wrap module. If this class has a base class, // a new pointer to the base class is allocated and returned. - wrapperFile.oss << "void " << collectorInsertFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - wrapperFile.oss << "{\n"; - wrapperFile.oss << " mexAtExit(&_deleteAllObjects);\n"; + wrapperFile.oss << "void " << collectorInsertFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + wrapperFile.oss << "{\n"; + wrapperFile.oss << " mexAtExit(&_deleteAllObjects);\n"; // Typedef boost::shared_ptr - wrapperFile.oss << " typedef boost::shared_ptr<" << cppName << "> Shared;\n"; - wrapperFile.oss << "\n"; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppName << "> Shared;\n"; + wrapperFile.oss << "\n"; // Get self pointer passed in - wrapperFile.oss << " Shared *self = *reinterpret_cast (mxGetData(in[0]));\n"; + wrapperFile.oss + << " Shared *self = *reinterpret_cast (mxGetData(in[0]));\n"; // Add to collector - wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; + wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) - if(!qualifiedParent.empty()) { - wrapperFile.oss << "\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName << "> SharedBase;\n"; - wrapperFile.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; - wrapperFile.oss << " *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self);\n"; - } - wrapperFile.oss << "}\n"; - + if (!qualifiedParent.empty()) { + wrapperFile.oss << "\n"; + wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName + << "> SharedBase;\n"; + wrapperFile.oss + << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; + wrapperFile.oss + << " *reinterpret_cast(mxGetData(out[0])) = new SharedBase(*self);\n"; + } + wrapperFile.oss << "}\n"; + // If this is a virtual function, C++ function to dynamic upcast it from a // shared_ptr. This mechanism allows automatic dynamic creation of the // real underlying derived-most class when a C++ method returns a virtual // base class. - if(isVirtual) - wrapperFile.oss << - "\n" - "void " << upcastFromVoidFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {\n" - " mexAtExit(&_deleteAllObjects);\n" - " typedef boost::shared_ptr<" << cppName << "> Shared;\n" - " boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0]));\n" - " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n" - " Shared *self = new Shared(boost::static_pointer_cast<" << cppName << ">(*asVoid));\n" - " *reinterpret_cast(mxGetData(out[0])) = self;\n" - "}\n"; -} - -/* ************************************************************************* */ -vector expandArgumentListsTemplate(const vector& argLists, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { - vector result; - BOOST_FOREACH(const ArgumentList& argList, argLists) { - ArgumentList instArgList; - BOOST_FOREACH(const Argument& arg, argList) { - Argument instArg = arg; - if(arg.type == templateArg) { - instArg.namespaces.assign(instName.begin(), instName.end()-1); - instArg.type = instName.back(); - } else if(arg.type == "This") { - instArg.namespaces.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); - instArg.type = expandedClassName; - } - instArgList.push_back(instArg); - } - result.push_back(instArgList); - } - return result; -} - -/* ************************************************************************* */ -template -map expandMethodTemplate(const map& methods, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { - map result; - typedef pair Name_Method; - BOOST_FOREACH(const Name_Method& name_method, methods) { - const METHOD& method = name_method.second; - METHOD instMethod = method; - instMethod.argLists = expandArgumentListsTemplate(method.argLists, templateArg, instName, expandedClassNamespace, expandedClassName); - instMethod.returnVals.clear(); - BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) { - ReturnValue instRetVal = retVal; - if(retVal.type1 == templateArg) { - instRetVal.namespaces1.assign(instName.begin(), instName.end()-1); - instRetVal.type1 = instName.back(); - } else if(retVal.type1 == "This") { - instRetVal.namespaces1.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); - instRetVal.type1 = expandedClassName; - } - if(retVal.type2 == templateArg) { - instRetVal.namespaces2.assign(instName.begin(), instName.end()-1); - instRetVal.type2 = instName.back(); - } else if(retVal.type1 == "This") { - instRetVal.namespaces2.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); - instRetVal.type2 = expandedClassName; - } - instMethod.returnVals.push_back(instRetVal); - } - result.insert(make_pair(name_method.first, instMethod)); - } - return result; -} - -/* ************************************************************************* */ -Class expandClassTemplate(const Class& cls, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { - Class inst; - inst.name = cls.name; - inst.templateArgs = cls.templateArgs; - inst.typedefName = cls.typedefName; - inst.isVirtual = cls.isVirtual; + if (isVirtual) + wrapperFile.oss << "\n" + "void " << upcastFromVoidFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {\n" + " mexAtExit(&_deleteAllObjects);\n" + " typedef boost::shared_ptr<" << cppName + << "> Shared;\n" + " boost::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0]));\n" + " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n" + " Shared *self = new Shared(boost::static_pointer_cast<" << cppName + << ">(*asVoid));\n" + " *reinterpret_cast(mxGetData(out[0])) = self;\n" + "}\n"; +} + +/* ************************************************************************* */ +vector expandArgumentListsTemplate( + const vector& argLists, const string& templateArg, + const vector& instName, + const std::vector& expandedClassNamespace, + const string& expandedClassName) { + vector result; + BOOST_FOREACH(const ArgumentList& argList, argLists) { + ArgumentList instArgList; + BOOST_FOREACH(const Argument& arg, argList) { + Argument instArg = arg; + if (arg.type == templateArg) { + instArg.namespaces.assign(instName.begin(), instName.end() - 1); + instArg.type = instName.back(); + } else if (arg.type == "This") { + instArg.namespaces.assign(expandedClassNamespace.begin(), + expandedClassNamespace.end()); + instArg.type = expandedClassName; + } + instArgList.push_back(instArg); + } + result.push_back(instArgList); + } + return result; +} + +/* ************************************************************************* */ +template +map expandMethodTemplate(const map& methods, + const string& templateArg, const vector& instName, + const std::vector& expandedClassNamespace, + const string& expandedClassName) { + map result; + typedef pair Name_Method; + BOOST_FOREACH(const Name_Method& name_method, methods) { + const METHOD& method = name_method.second; + METHOD instMethod = method; + instMethod.argLists = expandArgumentListsTemplate(method.argLists, + templateArg, instName, expandedClassNamespace, expandedClassName); + instMethod.returnVals.clear(); + BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) { + ReturnValue instRetVal = retVal; + if (retVal.type1 == templateArg) { + instRetVal.namespaces1.assign(instName.begin(), instName.end() - 1); + instRetVal.type1 = instName.back(); + } else if (retVal.type1 == "This") { + instRetVal.namespaces1.assign(expandedClassNamespace.begin(), + expandedClassNamespace.end()); + instRetVal.type1 = expandedClassName; + } + if (retVal.type2 == templateArg) { + instRetVal.namespaces2.assign(instName.begin(), instName.end() - 1); + instRetVal.type2 = instName.back(); + } else if (retVal.type1 == "This") { + instRetVal.namespaces2.assign(expandedClassNamespace.begin(), + expandedClassNamespace.end()); + instRetVal.type2 = expandedClassName; + } + instMethod.returnVals.push_back(instRetVal); + } + result.insert(make_pair(name_method.first, instMethod)); + } + return result; +} + +/* ************************************************************************* */ +Class expandClassTemplate(const Class& cls, const string& templateArg, + const vector& instName, + const std::vector& expandedClassNamespace, + const string& expandedClassName) { + Class inst; + inst.name = cls.name; + inst.templateArgs = cls.templateArgs; + inst.typedefName = cls.typedefName; + inst.isVirtual = cls.isVirtual; inst.isSerializable = cls.isSerializable; - inst.qualifiedParent = cls.qualifiedParent; - inst.methods = expandMethodTemplate(cls.methods, templateArg, instName, expandedClassNamespace, expandedClassName); - inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg, instName, expandedClassNamespace, expandedClassName); - inst.namespaces = cls.namespaces; - inst.constructor = cls.constructor; - inst.constructor.args_list = expandArgumentListsTemplate(cls.constructor.args_list, templateArg, instName, expandedClassNamespace, expandedClassName); - inst.constructor.name = inst.name; - inst.deconstructor = cls.deconstructor; - inst.deconstructor.name = inst.name; - inst.verbose_ = cls.verbose_; - return inst; -} - -/* ************************************************************************* */ -vector Class::expandTemplate(const string& templateArg, const vector >& instantiations) const { - vector result; - BOOST_FOREACH(const vector& instName, instantiations) { - const string expandedName = name + instName.back(); - Class inst = expandClassTemplate(*this, templateArg, instName, this->namespaces, expandedName); - inst.name = expandedName; - inst.templateArgs.clear(); - inst.typedefName = qualifiedName("::") + "<" + wrap::qualifiedName("::", instName) + ">"; - result.push_back(inst); - } - return result; -} - -/* ************************************************************************* */ -Class Class::expandTemplate(const string& templateArg, const vector& instantiation, const std::vector& expandedClassNamespace, const string& expandedClassName) const { - return expandClassTemplate(*this, templateArg, instantiation, expandedClassNamespace, expandedClassName); -} - -/* ************************************************************************* */ -std::string Class::getTypedef() const { - string result; - BOOST_FOREACH(const string& namesp, namespaces) { - result += ("namespace " + namesp + " { "); - } - result += ("typedef " + typedefName + " " + name + ";"); - for (size_t i = 0; i Class::expandTemplate(const string& templateArg, + const vector >& instantiations) const { + vector result; + BOOST_FOREACH(const vector& instName, instantiations) { + const string expandedName = name + instName.back(); + Class inst = expandClassTemplate(*this, templateArg, instName, + this->namespaces, expandedName); + inst.name = expandedName; + inst.templateArgs.clear(); + inst.typedefName = qualifiedName("::") + "<" + + wrap::qualifiedName("::", instName) + ">"; + result.push_back(inst); + } + return result; +} + +/* ************************************************************************* */ +Class Class::expandTemplate(const string& templateArg, + const vector& instantiation, + const std::vector& expandedClassNamespace, + const string& expandedClassName) const { + return expandClassTemplate(*this, templateArg, instantiation, + expandedClassNamespace, expandedClassName); +} + +/* ************************************************************************* */ +std::string Class::getTypedef() const { + string result; + BOOST_FOREACH(const string& namesp, namespaces) { + result += ("namespace " + namesp + " { "); + } + result += ("typedef " + typedefName + " " + name + ";"); + for (size_t i = 0; i < namespaces.size(); ++i) { + result += " }"; + } + return result; +} + +/* ************************************************************************* */ void Class::comment_fragment(FileWriter& proxyFile) const { - proxyFile.oss << "%class " << name - << ", see Doxygen page for details\n"; + proxyFile.oss << "%class " << name << ", see Doxygen page for details\n"; proxyFile.oss << "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; @@ -396,15 +445,17 @@ void Class::comment_fragment(FileWriter& proxyFile) const { if (hasSerialization) { proxyFile.oss << "%\n%-------Serialization Interface-------\n"; proxyFile.oss << "%string_serialize() : returns string\n"; - proxyFile.oss << "%string_deserialize(string serialized) : returns " << this->name << "\n"; + proxyFile.oss << "%string_deserialize(string serialized) : returns " + << this->name << "\n"; } proxyFile.oss << "%\n"; } -/* ************************************************************************* */ -void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& wrapperName, std::vector& functionNames) const { +/* ************************************************************************* */ +void Class::serialization_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const std::string& wrapperName, + std::vector& functionNames) const { //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //{ @@ -418,30 +469,34 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi //} int serialize_id = functionNames.size(); - const string - matlabQualName = qualifiedName("."), - matlabUniqueName = qualifiedName(), - cppClassName = qualifiedName("::"); - const string wrapFunctionNameSerialize = matlabUniqueName + "_string_serialize_" + boost::lexical_cast(serialize_id); + const string matlabQualName = qualifiedName("."), matlabUniqueName = + qualifiedName(), cppClassName = qualifiedName("::"); + const string wrapFunctionNameSerialize = matlabUniqueName + + "_string_serialize_" + boost::lexical_cast(serialize_id); functionNames.push_back(wrapFunctionNameSerialize); // call //void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) - wrapperFile.oss << "void " << wrapFunctionNameSerialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + wrapperFile.oss << "void " << wrapFunctionNameSerialize + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; wrapperFile.oss << "{\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; // check arguments - for serialize, no arguments // example: checkArguments("string_serialize",nargout,nargin-1,0); - wrapperFile.oss << " checkArguments(\"string_serialize\",nargout,nargin-1,0);\n"; + wrapperFile.oss + << " checkArguments(\"string_serialize\",nargout,nargin-1,0);\n"; // get class pointer // example: Shared obj = unwrap_shared_ptr(in[0], "ptr_Point3"); - wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; + wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName + << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; // Serialization boilerplate wrapperFile.oss << " std::ostringstream out_archive_stream;\n"; - wrapperFile.oss << " boost::archive::text_oarchive out_archive(out_archive_stream);\n"; + wrapperFile.oss + << " boost::archive::text_oarchive out_archive(out_archive_stream);\n"; wrapperFile.oss << " out_archive << *obj;\n"; wrapperFile.oss << " out[0] = wrap< string >(out_archive_stream.str());\n"; @@ -459,13 +514,19 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi // end // end - proxyFile.oss << " function varargout = string_serialize(this, varargin)\n"; - proxyFile.oss << " % STRING_SERIALIZE usage: string_serialize() : returns string\n"; - proxyFile.oss << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; + proxyFile.oss + << " function varargout = string_serialize(this, varargin)\n"; + proxyFile.oss + << " % STRING_SERIALIZE usage: string_serialize() : returns string\n"; + proxyFile.oss + << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; proxyFile.oss << " if length(varargin) == 0\n"; - proxyFile.oss << " varargout{1} = " << wrapperName << "(" << boost::lexical_cast(serialize_id) << ", this, varargin{:});\n"; + proxyFile.oss << " varargout{1} = " << wrapperName << "(" + << boost::lexical_cast(serialize_id) << ", this, varargin{:});\n"; proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << ".string_serialize');\n"; + proxyFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << ".string_serialize');\n"; proxyFile.oss << " end\n"; proxyFile.oss << " end\n\n"; @@ -476,14 +537,16 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi // end proxyFile.oss << " function sobj = saveobj(obj)\n"; - proxyFile.oss << " % SAVEOBJ Saves the object to a matlab-readable format\n"; + proxyFile.oss + << " % SAVEOBJ Saves the object to a matlab-readable format\n"; proxyFile.oss << " sobj = obj.string_serialize();\n"; proxyFile.oss << " end\n"; } /* ************************************************************************* */ -void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& wrapperName, std::vector& functionNames) const { +void Class::deserialization_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const std::string& wrapperName, + std::vector& functionNames) const { //void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) //{ // typedef boost::shared_ptr Shared; @@ -495,32 +558,36 @@ void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapper // in_archive >> *output; // out[0] = wrap_shared_ptr(output,"Point3", false); //} - int deserialize_id = functionNames.size(); - const string - matlabQualName = qualifiedName("."), - matlabUniqueName = qualifiedName(), - cppClassName = qualifiedName("::"); - const string wrapFunctionNameDeserialize = matlabUniqueName + "_string_deserialize_" + boost::lexical_cast(deserialize_id); - functionNames.push_back(wrapFunctionNameDeserialize); + int deserialize_id = functionNames.size(); + const string matlabQualName = qualifiedName("."), matlabUniqueName = + qualifiedName(), cppClassName = qualifiedName("::"); + const string wrapFunctionNameDeserialize = matlabUniqueName + + "_string_deserialize_" + boost::lexical_cast(deserialize_id); + functionNames.push_back(wrapFunctionNameDeserialize); - // call - wrapperFile.oss << "void " << wrapFunctionNameDeserialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - wrapperFile.oss << "{\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + // call + wrapperFile.oss << "void " << wrapFunctionNameDeserialize + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + wrapperFile.oss << "{\n"; + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; - // check arguments - for deserialize, 1 string argument - wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << ".string_deserialize\",nargout,nargin,1);\n"; + // check arguments - for deserialize, 1 string argument + wrapperFile.oss << " checkArguments(\"" << matlabUniqueName + << ".string_deserialize\",nargout,nargin,1);\n"; - // string argument with deserialization boilerplate - wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n"; - wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n"; - wrapperFile.oss << " boost::archive::text_iarchive in_archive(in_archive_stream);\n"; - wrapperFile.oss << " Shared output(new " << cppClassName << "());\n"; - wrapperFile.oss << " in_archive >> *output;\n"; - wrapperFile.oss << " out[0] = wrap_shared_ptr(output,\"" << matlabQualName << "\", false);\n"; - wrapperFile.oss << "}\n"; + // string argument with deserialization boilerplate + wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n"; + wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n"; + wrapperFile.oss + << " boost::archive::text_iarchive in_archive(in_archive_stream);\n"; + wrapperFile.oss << " Shared output(new " << cppClassName << "());\n"; + wrapperFile.oss << " in_archive >> *output;\n"; + wrapperFile.oss << " out[0] = wrap_shared_ptr(output,\"" << matlabQualName + << "\", false);\n"; + wrapperFile.oss << "}\n"; - // Generate matlab function + // Generate matlab function // function varargout = string_deserialize(varargin) // % STRING_DESERIALIZE usage: string_deserialize() : returns Point3 // % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html @@ -531,32 +598,40 @@ void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapper // end // end - proxyFile.oss << " function varargout = string_deserialize(varargin)\n"; - proxyFile.oss << " % STRING_DESERIALIZE usage: string_deserialize() : returns " << matlabQualName << "\n"; - proxyFile.oss << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; - proxyFile.oss << " if length(varargin) == 1\n"; - proxyFile.oss << " varargout{1} = " << wrapperName << "(" << boost::lexical_cast(deserialize_id) << ", varargin{:});\n"; - proxyFile.oss << " else\n"; - proxyFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << ".string_deserialize');\n"; - proxyFile.oss << " end\n"; - proxyFile.oss << " end\n\n"; + proxyFile.oss << " function varargout = string_deserialize(varargin)\n"; + proxyFile.oss + << " % STRING_DESERIALIZE usage: string_deserialize() : returns " + << matlabQualName << "\n"; + proxyFile.oss + << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; + proxyFile.oss << " if length(varargin) == 1\n"; + proxyFile.oss << " varargout{1} = " << wrapperName << "(" + << boost::lexical_cast(deserialize_id) << ", varargin{:});\n"; + proxyFile.oss << " else\n"; + proxyFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << ".string_deserialize');\n"; + proxyFile.oss << " end\n"; + proxyFile.oss << " end\n\n"; - - // Generate matlab load function + // Generate matlab load function // function obj = loadobj(sobj) // % LOADOBJ Saves the object to a matlab-readable format // obj = Point3.string_deserialize(sobj); // end - proxyFile.oss << " function obj = loadobj(sobj)\n"; - proxyFile.oss << " % LOADOBJ Saves the object to a matlab-readable format\n"; - proxyFile.oss << " obj = " << matlabQualName << ".string_deserialize(sobj);\n"; - proxyFile.oss << " end" << endl; + proxyFile.oss << " function obj = loadobj(sobj)\n"; + proxyFile.oss + << " % LOADOBJ Saves the object to a matlab-readable format\n"; + proxyFile.oss << " obj = " << matlabQualName + << ".string_deserialize(sobj);\n"; + proxyFile.oss << " end" << endl; } /* ************************************************************************* */ std::string Class::getSerializationExport() const { //BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsamSharedDiagonal"); - return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" + qualifiedName() + "\");"; + return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" + + qualifiedName() + "\");"; } /* ************************************************************************* */ diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index e5c10b4c8..8c84030e7 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -28,7 +28,6 @@ using namespace std; using namespace wrap; - /* ************************************************************************* */ void StaticMethod::addOverload(bool verbose, const std::string& name, const ArgumentList& args, const ReturnValue& retVal) { @@ -39,144 +38,149 @@ void StaticMethod::addOverload(bool verbose, const std::string& name, } /* ************************************************************************* */ -void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const string& cppClassName, - const std::string& matlabQualName, - const std::string& matlabUniqueName, - const string& wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { +void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, const string& cppClassName, + const std::string& matlabQualName, const std::string& matlabUniqueName, + const string& wrapperName, const TypeAttributesTable& typeAttributes, + vector& functionNames) const { - string upperName = name; upperName[0] = std::toupper(upperName[0], std::locale()); + string upperName = name; + upperName[0] = std::toupper(upperName[0], std::locale()); proxyFile.oss << " function varargout = " << upperName << "(varargin)\n"; //Comments for documentation - string up_name = boost::to_upper_copy(name); + string up_name = boost::to_upper_copy(name); proxyFile.oss << " % " << up_name << " usage:"; unsigned int argLCount = 0; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - if(argLCount != argLists.size()-1) - proxyFile.oss << "), "; - else - proxyFile.oss << ") : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; - argLCount++; + BOOST_FOREACH(ArgumentList argList, argLists) { + proxyFile.oss << " " << name << "("; + unsigned int i = 0; + BOOST_FOREACH(const Argument& arg, argList) { + if (i != argList.size() - 1) + proxyFile.oss << arg.type << " " << arg.name << ", "; + else + proxyFile.oss << arg.type << " " << arg.name; + i++; } + if (argLCount != argLists.size() - 1) + proxyFile.oss << "), "; + else + proxyFile.oss << ") : returns " + << returnVals[0].return_type(false, returnVals[0].pair) << endl; + argLCount++; + } - proxyFile.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; + proxyFile.oss << " % " + << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" + << endl; proxyFile.oss << " % " << "" << endl; proxyFile.oss << " % " << "Usage" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) - { - proxyFile.oss << " % " << up_name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) - { - if(i != argList.size()-1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")" << endl; + BOOST_FOREACH(ArgumentList argList, argLists) { + proxyFile.oss << " % " << up_name << "("; + unsigned int i = 0; + BOOST_FOREACH(const Argument& arg, argList) { + if (i != argList.size() - 1) + proxyFile.oss << arg.type << " " << arg.name << ", "; + else + proxyFile.oss << arg.type << " " << arg.name; + i++; } + proxyFile.oss << ")" << endl; + } - - for(size_t overload = 0; overload < argLists.size(); ++overload) { + for (size_t overload = 0; overload < argLists.size(); ++overload) { const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; size_t nrArgs = args.size(); - const int id = (int)functionNames.size(); + const int id = (int) functionNames.size(); // Output proxy matlab code // check for number of arguments... - proxyFile.oss << " " << (overload==0?"":"else") << "if length(varargin) == " << nrArgs; - if (nrArgs>0) proxyFile.oss << " && "; + proxyFile.oss << " " << (overload == 0 ? "" : "else") + << "if length(varargin) == " << nrArgs; + if (nrArgs > 0) + proxyFile.oss << " && "; // ...and their types bool first = true; - for(size_t i=0;i(id); + const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + + boost::lexical_cast(id); const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; // call - file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start file.oss << "{\n"; returnVal.wrapTypeUnwrap(file); - file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" + << endl; // check arguments // NOTE: for static functions, there is no object passed - file.oss << " checkArguments(\"" << matlabUniqueName << "." << name << "\",nargout,nargin," << args.size() << ");\n"; + file.oss << " checkArguments(\"" << matlabUniqueName << "." << name + << "\",nargout,nargin," << args.size() << ");\n"; // unwrap arguments, see Argument.cpp - args.matlab_unwrap(file,0); // We start at 0 because there is no self object + args.matlab_unwrap(file, 0); // We start at 0 because there is no self object // call method with default type and wrap result - if (returnVal.type1!="void") - returnVal.wrap_result(cppClassName+"::"+name+"("+args.names()+")", file, typeAttributes); + if (returnVal.type1 != "void") + returnVal.wrap_result(cppClassName + "::" + name + "(" + args.names() + ")", + file, typeAttributes); else - file.oss << cppClassName+"::"+name+"("+args.names()+");\n"; + file.oss << cppClassName + "::" + name + "(" + args.names() + ");\n"; // finish file.oss << "}\n"; From 80b7d91264bbe5fe0c2505fcaee0c4fe7fd82b5b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:01:36 -0400 Subject: [PATCH 070/101] emit prototype method (to eliminate horrible copy/paste mess someone thought was good programming style) --- wrap/Argument.cpp | 22 ++++++++++++++++++---- wrap/Argument.h | 7 +++++++ 2 files changed, 25 insertions(+), 4 deletions(-) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index c3798e5ce..0f5961a48 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -16,13 +16,14 @@ * @author Richard Roberts **/ -#include -#include -#include +#include "Argument.h" + #include #include -#include "Argument.h" +#include +#include +#include using namespace std; using namespace wrap; @@ -128,4 +129,17 @@ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const { } /* ************************************************************************* */ +void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { + file.oss << name << "("; + unsigned int i = 0; + BOOST_FOREACH(Argument arg, *this) { + if (i != size() - 1) + file.oss << arg.type << " " << arg.name << ", "; + else + file.oss << arg.type << " " << arg.name; + i++; + } + file.oss << ")"; +} +/* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h index f46eaa427..da15cda36 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -68,6 +68,13 @@ struct ArgumentList: public std::vector { */ void matlab_unwrap(FileWriter& file, int start = 0) const; // MATLAB to C++ + /** + * emit MATLAB prototype + * @param file output stream + * @param name of method or function + */ + void emit_prototype(FileWriter& file, const std::string& name) const; + }; } // \namespace wrap From 26cae48338c8b350f0c85d72dbd43fdc7c4fd1a8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:02:29 -0400 Subject: [PATCH 071/101] First successful use of new ArgumentList::emit_prototype method --- wrap/Class.cpp | 27 ++++++++++----------------- wrap/Class.h | 1 - 2 files changed, 10 insertions(+), 18 deletions(-) diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 8788e740f..e8b26efb2 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -16,18 +16,19 @@ * @author Richard Roberts **/ +#include "Class.h" +#include "utilities.h" +#include "Argument.h" + +#include +#include + #include #include #include //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above #include // works on Linux GCC -#include -#include - -#include "Class.h" -#include "utilities.h" -#include "Argument.h" using namespace std; using namespace wrap; @@ -406,17 +407,9 @@ void Class::comment_fragment(FileWriter& proxyFile) const { BOOST_FOREACH(const Methods::value_type& name_m, methods) { const Method& m = name_m.second; BOOST_FOREACH(ArgumentList argList, m.argLists) { - string up_name = boost::to_upper_copy(m.name); - proxyFile.oss << "%" << m.name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ") : returns " + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, m.name); + proxyFile.oss << " : returns " << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl; } } diff --git a/wrap/Class.h b/wrap/Class.h index f5267cee2..2ca976f66 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -27,7 +27,6 @@ #include "Method.h" #include "StaticMethod.h" #include "TypeAttributesTable.h" -#include namespace wrap { From 47e90166938a5c80b2562a464974454967aa45d8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:02:54 -0400 Subject: [PATCH 072/101] Fixed so prints expected file then actual, as expected (no pun intended) --- wrap/utilities.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 1acc50db1..870ba3101 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -89,7 +89,7 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) bool equal = actual_contents == expected_contents; if (!equal) { stringstream command; - command << "diff " << actual << " " << expected << endl; + command << "diff " << expected << " " << actual << endl; system(command.str().c_str()); } return equal; From ededc01ec149899bc3671b5bd1b9f90c23851659 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:03:05 -0400 Subject: [PATCH 073/101] moved target --- .cproject | 422 +++++++++++++++++++++++++++--------------------------- 1 file changed, 213 insertions(+), 209 deletions(-) diff --git a/.cproject b/.cproject index fa8c24875..da776f062 100644 --- a/.cproject +++ b/.cproject @@ -1,17 +1,19 @@ - + + + + + - - @@ -60,13 +62,13 @@ + + - - @@ -116,13 +118,13 @@ + + - - @@ -540,14 +542,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -574,6 +568,7 @@ make + tests/testBayesTree.run true false @@ -581,6 +576,7 @@ make + testBinaryBayesNet.run true false @@ -628,6 +624,7 @@ make + testSymbolicBayesNet.run true false @@ -635,6 +632,7 @@ make + tests/testSymbolicFactor.run true false @@ -642,6 +640,7 @@ make + testSymbolicFactorGraph.run true false @@ -657,11 +656,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -886,22 +894,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -918,6 +910,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -942,6 +950,30 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + make -j5 @@ -1006,30 +1038,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - make -j5 @@ -1118,10 +1126,10 @@ true true - + make -j5 - testSmartProjectionPoseFactor.run + testWrap.run true true true @@ -1254,14 +1262,6 @@ true true - - make - -j5 - testAttitudeFactor.run - true - true - true - make -j5 @@ -1318,6 +1318,14 @@ true true + + make + -j5 + testSmartProjectionFactor.run + true + true + true + make -j5 @@ -1536,6 +1544,7 @@ make + testGraph.run true false @@ -1543,6 +1552,7 @@ make + testJunctionTree.run true false @@ -1550,6 +1560,7 @@ make + testSymbolicBayesNetB.run true false @@ -1717,6 +1728,7 @@ make + testErrors.run true false @@ -1762,22 +1774,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j5 - testParticleFactor.run - true - true - true - make -j2 @@ -1858,6 +1854,22 @@ true true + + make + -j5 + testParticleFactor.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -2010,22 +2022,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j2 @@ -2108,7 +2104,6 @@ make - testSimulated2DOriented.run true false @@ -2148,7 +2143,6 @@ make - testSimulated2D.run true false @@ -2156,7 +2150,6 @@ make - testSimulated3D.run true false @@ -2170,6 +2163,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j5 @@ -2396,7 +2405,6 @@ make - tests/testGaussianISAM2 true false @@ -2418,102 +2426,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2715,6 +2627,7 @@ cpack + -G DEB true false @@ -2722,6 +2635,7 @@ cpack + -G RPM true false @@ -2729,6 +2643,7 @@ cpack + -G TGZ true false @@ -2736,6 +2651,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2909,34 +2825,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2980,6 +2960,30 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + From 4403d47865566abda9cb2b5e1a571abc4e1029f1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:12:48 -0400 Subject: [PATCH 074/101] Slight refactor --- wrap/Argument.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 0f5961a48..689ca6d88 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -131,13 +131,11 @@ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const { /* ************************************************************************* */ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { file.oss << name << "("; - unsigned int i = 0; + bool first = true; BOOST_FOREACH(Argument arg, *this) { - if (i != size() - 1) - file.oss << arg.type << " " << arg.name << ", "; - else - file.oss << arg.type << " " << arg.name; - i++; + if (!first) file.oss << ", "; + file.oss << arg.type << " " << arg.name; + first = false; } file.oss << ")"; } From 95b85e8494671aea570a18816858e440cea98d5b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:22:10 -0400 Subject: [PATCH 075/101] Now using ArgumentList::emit_prototype everywhere, for non copy/paste code --- wrap/Class.cpp | 30 ++++++---------------------- wrap/Method.cpp | 46 ++++++++++++++++++------------------------- wrap/Method.h | 7 +++---- wrap/StaticMethod.cpp | 38 +++++++++++------------------------ wrap/StaticMethod.h | 7 +++---- 5 files changed, 43 insertions(+), 85 deletions(-) diff --git a/wrap/Class.cpp b/wrap/Class.cpp index e8b26efb2..075c98811 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -29,7 +29,6 @@ //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above #include // works on Linux GCC - using namespace std; using namespace wrap; @@ -389,17 +388,9 @@ void Class::comment_fragment(FileWriter& proxyFile) const { if (!constructor.args_list.empty()) proxyFile.oss << "%\n%-------Constructors-------\n"; BOOST_FOREACH(ArgumentList argList, constructor.args_list) { - string up_name = boost::to_upper_copy(name); - proxyFile.oss << "%" << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")\n"; + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, name); + proxyFile.oss << "\n"; } if (!methods.empty()) @@ -419,18 +410,9 @@ void Class::comment_fragment(FileWriter& proxyFile) const { BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) { const StaticMethod& m = name_m.second; BOOST_FOREACH(ArgumentList argList, m.argLists) { - string up_name = boost::to_upper_copy(m.name); - proxyFile.oss << "%" << m.name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - - proxyFile.oss << ") : returns " + proxyFile.oss << "%"; + argList.emit_prototype(proxyFile, m.name); + proxyFile.oss << " : returns " << m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl; } } diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 3fbf6a9c0..edfb80268 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -15,14 +15,15 @@ * @author Richard Roberts **/ -#include -#include +#include "Method.h" +#include "utilities.h" #include #include +#include -#include "Method.h" -#include "utilities.h" +#include +#include using namespace std; using namespace wrap; @@ -44,47 +45,38 @@ void Method::proxy_wrapper_fragments(FileWriter& proxyFile, const string& wrapperName, const TypeAttributesTable& typeAttributes, vector& functionNames) const { + // Create function header proxyFile.oss << " function varargout = " << name << "(this, varargin)\n"; - //Comments for documentation + + // Emit comments for documentation string up_name = boost::to_upper_copy(name); - proxyFile.oss << " % " << up_name << " usage:"; + proxyFile.oss << " % " << up_name << " usage: "; unsigned int argLCount = 0; BOOST_FOREACH(ArgumentList argList, argLists) { - proxyFile.oss << " " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } + argList.emit_prototype(proxyFile, name); if (argLCount != argLists.size() - 1) - proxyFile.oss << "), "; + proxyFile.oss << ", "; else - proxyFile.oss << ") : returns " + proxyFile.oss << " : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; argLCount++; } + // Emit URL to Doxygen page proxyFile.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; + + // Document all overloads, if any proxyFile.oss << " % " << "" << endl; proxyFile.oss << " % " << "Method Overloads" << endl; BOOST_FOREACH(ArgumentList argList, argLists) { - proxyFile.oss << " % " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")" << endl; + proxyFile.oss << " % "; + argList.emit_prototype(proxyFile, name); + proxyFile.oss << endl; } + // For all overloads, check the number of arguments for (size_t overload = 0; overload < argLists.size(); ++overload) { const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; diff --git a/wrap/Method.h b/wrap/Method.h index f2a7ed190..9926a5179 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -18,13 +18,12 @@ #pragma once -#include -#include - #include "Argument.h" #include "ReturnValue.h" #include "TypeAttributesTable.h" -#include + +#include +#include namespace wrap { diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 8c84030e7..29b3b12df 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -16,14 +16,15 @@ * @author Richard Roberts **/ -#include -#include +#include "StaticMethod.h" +#include "utilities.h" #include #include +#include -#include "StaticMethod.h" -#include "utilities.h" +#include +#include using namespace std; using namespace wrap; @@ -50,22 +51,14 @@ void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, proxyFile.oss << " function varargout = " << upperName << "(varargin)\n"; //Comments for documentation string up_name = boost::to_upper_copy(name); - proxyFile.oss << " % " << up_name << " usage:"; + proxyFile.oss << " % " << up_name << " usage: "; unsigned int argLCount = 0; BOOST_FOREACH(ArgumentList argList, argLists) { - proxyFile.oss << " " << name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } + argList.emit_prototype(proxyFile, name); if (argLCount != argLists.size() - 1) - proxyFile.oss << "), "; + proxyFile.oss << ", "; else - proxyFile.oss << ") : returns " + proxyFile.oss << " : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; argLCount++; } @@ -76,16 +69,9 @@ void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, proxyFile.oss << " % " << "" << endl; proxyFile.oss << " % " << "Usage" << endl; BOOST_FOREACH(ArgumentList argList, argLists) { - proxyFile.oss << " % " << up_name << "("; - unsigned int i = 0; - BOOST_FOREACH(const Argument& arg, argList) { - if (i != argList.size() - 1) - proxyFile.oss << arg.type << " " << arg.name << ", "; - else - proxyFile.oss << arg.type << " " << arg.name; - i++; - } - proxyFile.oss << ")" << endl; + proxyFile.oss << " % "; + argList.emit_prototype(proxyFile, up_name); + proxyFile.oss << endl; } for (size_t overload = 0; overload < argLists.size(); ++overload) { diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index 3e8dc08cf..576232346 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -19,13 +19,12 @@ #pragma once -#include -#include - #include "Argument.h" #include "ReturnValue.h" #include "TypeAttributesTable.h" -#include + +#include +#include namespace wrap { From 43e238c6ab4f4a10b2e50918862d517154ecfbfb Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:26:14 -0400 Subject: [PATCH 076/101] Formatting and headers --- wrap/Constructor.h | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 96d1030a1..5438c515c 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -18,11 +18,11 @@ #pragma once +#include "Argument.h" + #include #include -#include "Argument.h" - namespace wrap { // Constructor class @@ -34,7 +34,7 @@ struct Constructor { } // Then the instance variables are set directly by the Module constructor - std::vector args_list; + std::vector args_list; std::string name; bool verbose_; @@ -50,21 +50,18 @@ struct Constructor { * if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end */ void proxy_fragment(FileWriter& file, const std::string& wrapperName, - bool hasParent, const int id, const ArgumentList args) const; + bool hasParent, const int id, const ArgumentList args) const; /// cpp wrapper std::string wrapper_fragment(FileWriter& file, - const std::string& cppClassName, - const std::string& matlabUniqueName, - const std::string& cppBaseClassName, - int id, - const ArgumentList& al) const; + const std::string& cppClassName, const std::string& matlabUniqueName, + const std::string& cppBaseClassName, int id, + const ArgumentList& al) const; /// constructor function void generate_construct(FileWriter& file, const std::string& cppClassName, - std::vector& args_list) const; - + std::vector& args_list) const; + }; - } // \namespace wrap From 1002696f83176c510a7eaef3a04312a29fc61c3a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:29:06 -0400 Subject: [PATCH 077/101] Formatting and headers --- wrap/Argument.h | 4 ++-- wrap/ReturnValue.h | 21 +++++++++------------ wrap/StaticMethod.h | 22 +++++++++------------- 3 files changed, 20 insertions(+), 27 deletions(-) diff --git a/wrap/Argument.h b/wrap/Argument.h index da15cda36..2a989713b 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -19,11 +19,11 @@ #pragma once +#include "FileWriter.h" + #include #include -#include "FileWriter.h" - namespace wrap { /// Argument class diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index 7a677532f..ea78923f6 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -8,12 +8,11 @@ * @author Richard Roberts */ -#include -#include - #include "FileWriter.h" #include "TypeAttributesTable.h" +#include + #pragma once namespace wrap { @@ -21,16 +20,13 @@ namespace wrap { struct ReturnValue { typedef enum { - CLASS = 1, - EIGEN = 2, - BASIS = 3, - VOID = 4 + CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 } return_category; - ReturnValue(bool enable_verbosity = true) - : verbose(enable_verbosity), isPtr1(false), isPtr2(false), - isPair(false), category1(CLASS), category2(CLASS) - {} + ReturnValue(bool enable_verbosity = true) : + verbose(enable_verbosity), isPtr1(false), isPtr2(false), isPair(false), category1( + CLASS), category2(CLASS) { + } bool verbose; bool isPtr1, isPtr2, isPair; @@ -49,7 +45,8 @@ struct ReturnValue { std::string matlab_returnType() const; - void wrap_result(const std::string& result, FileWriter& file, const TypeAttributesTable& typeAttributes) const; + void wrap_result(const std::string& result, FileWriter& file, + const TypeAttributesTable& typeAttributes) const; void wrapTypeUnwrap(FileWriter& wrapperFile) const; diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index 576232346..e1855f4c2 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -23,9 +23,6 @@ #include "ReturnValue.h" #include "TypeAttributesTable.h" -#include -#include - namespace wrap { /// StaticMethod class @@ -33,7 +30,8 @@ struct StaticMethod { /// Constructor creates empty object StaticMethod(bool verbosity = true) : - verbose(verbosity) {} + verbose(verbosity) { + } // Then the instance variables are set directly by the Module constructor bool verbose; @@ -45,22 +43,20 @@ struct StaticMethod { // with those in rhs, but in subsequent calls it adds additional argument // lists as function overloads. void addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal); + const ArgumentList& args, const ReturnValue& retVal); // MATLAB code generation // classPath is class directory, e.g., ../matlab/@Point2 void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - const std::string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName, - const std::string& wrapperName, const TypeAttributesTable& typeAttributes, - std::vector& functionNames) const; + const std::string& cppClassName, const std::string& matlabQualName, + const std::string& matlabUniqueName, const std::string& wrapperName, + const TypeAttributesTable& typeAttributes, + std::vector& functionNames) const; private: std::string wrapper_fragment(FileWriter& file, - const std::string& cppClassName, - const std::string& matlabUniqueName, - int overload, - int id, - const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper + const std::string& cppClassName, const std::string& matlabUniqueName, + int overload, int id, const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper }; } // \namespace wrap From 61baef3be0c6875fb70bb27946425ef1ac588566 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 13:37:44 -0400 Subject: [PATCH 078/101] Don't emit overloads unless there are any --- wrap/Method.cpp | 14 ++--- wrap/tests/expected/Point2.m | 21 -------- wrap/tests/expected/Point3.m | 3 -- wrap/tests/expected/Test.m | 57 -------------------- wrap/tests/expected_namespaces/+ns2/ClassA.m | 9 ---- 5 files changed, 8 insertions(+), 96 deletions(-) diff --git a/wrap/Method.cpp b/wrap/Method.cpp index edfb80268..ae50a36ae 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -68,12 +68,14 @@ void Method::proxy_wrapper_fragments(FileWriter& proxyFile, << endl; // Document all overloads, if any - proxyFile.oss << " % " << "" << endl; - proxyFile.oss << " % " << "Method Overloads" << endl; - BOOST_FOREACH(ArgumentList argList, argLists) { - proxyFile.oss << " % "; - argList.emit_prototype(proxyFile, name); - proxyFile.oss << endl; + if (argLists.size() > 1) { + proxyFile.oss << " % " << "" << endl; + proxyFile.oss << " % " << "Method Overloads" << endl; + BOOST_FOREACH(ArgumentList argList, argLists) { + proxyFile.oss << " % "; + argList.emit_prototype(proxyFile, name); + proxyFile.oss << endl; + } } // For all overloads, check the number of arguments diff --git a/wrap/tests/expected/Point2.m b/wrap/tests/expected/Point2.m index 22dec9641..fa0741fe2 100644 --- a/wrap/tests/expected/Point2.m +++ b/wrap/tests/expected/Point2.m @@ -44,9 +44,6 @@ classdef Point2 < handle function varargout = argChar(this, varargin) % ARGCHAR usage: argChar(char a) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % argChar(char a) if length(varargin) == 1 && isa(varargin{1},'char') geometry_wrapper(4, this, varargin{:}); else @@ -57,9 +54,6 @@ classdef Point2 < handle function varargout = argUChar(this, varargin) % ARGUCHAR usage: argUChar(unsigned char a) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % argUChar(unsigned char a) if length(varargin) == 1 && isa(varargin{1},'char') geometry_wrapper(5, this, varargin{:}); else @@ -70,9 +64,6 @@ classdef Point2 < handle function varargout = dim(this, varargin) % DIM usage: dim() : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % dim() if length(varargin) == 0 varargout{1} = geometry_wrapper(6, this, varargin{:}); else @@ -83,9 +74,6 @@ classdef Point2 < handle function varargout = returnChar(this, varargin) % RETURNCHAR usage: returnChar() : returns char % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % returnChar() if length(varargin) == 0 varargout{1} = geometry_wrapper(7, this, varargin{:}); else @@ -96,9 +84,6 @@ classdef Point2 < handle function varargout = vectorConfusion(this, varargin) % VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % vectorConfusion() if length(varargin) == 0 varargout{1} = geometry_wrapper(8, this, varargin{:}); else @@ -109,9 +94,6 @@ classdef Point2 < handle function varargout = x(this, varargin) % X usage: x() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % x() if length(varargin) == 0 varargout{1} = geometry_wrapper(9, this, varargin{:}); else @@ -122,9 +104,6 @@ classdef Point2 < handle function varargout = y(this, varargin) % Y usage: y() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % y() if length(varargin) == 0 varargout{1} = geometry_wrapper(10, this, varargin{:}); else diff --git a/wrap/tests/expected/Point3.m b/wrap/tests/expected/Point3.m index 94d9c25b8..fd17df60b 100644 --- a/wrap/tests/expected/Point3.m +++ b/wrap/tests/expected/Point3.m @@ -43,9 +43,6 @@ classdef Point3 < handle function varargout = norm(this, varargin) % NORM usage: norm() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % norm() if length(varargin) == 0 varargout{1} = geometry_wrapper(14, this, varargin{:}); else diff --git a/wrap/tests/expected/Test.m b/wrap/tests/expected/Test.m index 8e56df6fc..a82ae3ed3 100644 --- a/wrap/tests/expected/Test.m +++ b/wrap/tests/expected/Test.m @@ -56,9 +56,6 @@ classdef Test < handle function varargout = arg_EigenConstRef(this, varargin) % ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % arg_EigenConstRef(Matrix value) if length(varargin) == 1 && isa(varargin{1},'double') geometry_wrapper(23, this, varargin{:}); else @@ -69,9 +66,6 @@ classdef Test < handle function varargout = create_MixedPtrs(this, varargin) % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % create_MixedPtrs() if length(varargin) == 0 [ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:}); else @@ -82,9 +76,6 @@ classdef Test < handle function varargout = create_ptrs(this, varargin) % CREATE_PTRS usage: create_ptrs() : returns pair< SharedTest, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % create_ptrs() if length(varargin) == 0 [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:}); else @@ -95,9 +86,6 @@ classdef Test < handle function varargout = print(this, varargin) % PRINT usage: print() : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % print() if length(varargin) == 0 geometry_wrapper(26, this, varargin{:}); else @@ -108,9 +96,6 @@ classdef Test < handle function varargout = return_Point2Ptr(this, varargin) % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2 % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_Point2Ptr(bool value) if length(varargin) == 1 && isa(varargin{1},'logical') varargout{1} = geometry_wrapper(27, this, varargin{:}); else @@ -121,9 +106,6 @@ classdef Test < handle function varargout = return_Test(this, varargin) % RETURN_TEST usage: return_Test(Test value) : returns Test % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_Test(Test value) if length(varargin) == 1 && isa(varargin{1},'Test') varargout{1} = geometry_wrapper(28, this, varargin{:}); else @@ -134,9 +116,6 @@ classdef Test < handle function varargout = return_TestPtr(this, varargin) % RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_TestPtr(Test value) if length(varargin) == 1 && isa(varargin{1},'Test') varargout{1} = geometry_wrapper(29, this, varargin{:}); else @@ -147,9 +126,6 @@ classdef Test < handle function varargout = return_bool(this, varargin) % RETURN_BOOL usage: return_bool(bool value) : returns bool % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_bool(bool value) if length(varargin) == 1 && isa(varargin{1},'logical') varargout{1} = geometry_wrapper(30, this, varargin{:}); else @@ -160,9 +136,6 @@ classdef Test < handle function varargout = return_double(this, varargin) % RETURN_DOUBLE usage: return_double(double value) : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_double(double value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(31, this, varargin{:}); else @@ -173,9 +146,6 @@ classdef Test < handle function varargout = return_field(this, varargin) % RETURN_FIELD usage: return_field(Test t) : returns bool % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_field(Test t) if length(varargin) == 1 && isa(varargin{1},'Test') varargout{1} = geometry_wrapper(32, this, varargin{:}); else @@ -186,9 +156,6 @@ classdef Test < handle function varargout = return_int(this, varargin) % RETURN_INT usage: return_int(int value) : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_int(int value) if length(varargin) == 1 && isa(varargin{1},'numeric') varargout{1} = geometry_wrapper(33, this, varargin{:}); else @@ -199,9 +166,6 @@ classdef Test < handle function varargout = return_matrix1(this, varargin) % RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_matrix1(Matrix value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(34, this, varargin{:}); else @@ -212,9 +176,6 @@ classdef Test < handle function varargout = return_matrix2(this, varargin) % RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_matrix2(Matrix value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(35, this, varargin{:}); else @@ -225,9 +186,6 @@ classdef Test < handle function varargout = return_pair(this, varargin) % RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_pair(Vector v, Matrix A) if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double') [ varargout{1} varargout{2} ] = geometry_wrapper(36, this, varargin{:}); else @@ -238,9 +196,6 @@ classdef Test < handle function varargout = return_ptrs(this, varargin) % RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< SharedTest, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_ptrs(Test p1, Test p2) if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test') [ varargout{1} varargout{2} ] = geometry_wrapper(37, this, varargin{:}); else @@ -251,9 +206,6 @@ classdef Test < handle function varargout = return_size_t(this, varargin) % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_size_t(size_t value) if length(varargin) == 1 && isa(varargin{1},'numeric') varargout{1} = geometry_wrapper(38, this, varargin{:}); else @@ -264,9 +216,6 @@ classdef Test < handle function varargout = return_string(this, varargin) % RETURN_STRING usage: return_string(string value) : returns string % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_string(string value) if length(varargin) == 1 && isa(varargin{1},'char') varargout{1} = geometry_wrapper(39, this, varargin{:}); else @@ -277,9 +226,6 @@ classdef Test < handle function varargout = return_vector1(this, varargin) % RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_vector1(Vector value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(40, this, varargin{:}); else @@ -290,9 +236,6 @@ classdef Test < handle function varargout = return_vector2(this, varargin) % RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % return_vector2(Vector value) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = geometry_wrapper(41, this, varargin{:}); else diff --git a/wrap/tests/expected_namespaces/+ns2/ClassA.m b/wrap/tests/expected_namespaces/+ns2/ClassA.m index c7ab901ae..f31825851 100644 --- a/wrap/tests/expected_namespaces/+ns2/ClassA.m +++ b/wrap/tests/expected_namespaces/+ns2/ClassA.m @@ -40,9 +40,6 @@ classdef ClassA < handle function varargout = memberFunction(this, varargin) % MEMBERFUNCTION usage: memberFunction() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % memberFunction() if length(varargin) == 0 varargout{1} = testNamespaces_wrapper(9, this, varargin{:}); else @@ -53,9 +50,6 @@ classdef ClassA < handle function varargout = nsArg(this, varargin) % NSARG usage: nsArg(ClassB arg) : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % nsArg(ClassB arg) if length(varargin) == 1 && isa(varargin{1},'ns1.ClassB') varargout{1} = testNamespaces_wrapper(10, this, varargin{:}); else @@ -66,9 +60,6 @@ classdef ClassA < handle function varargout = nsReturn(this, varargin) % NSRETURN usage: nsReturn(double q) : returns ns2::ns3::ClassB % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - % - % Method Overloads - % nsReturn(double q) if length(varargin) == 1 && isa(varargin{1},'double') varargout{1} = testNamespaces_wrapper(11, this, varargin{:}); else From 406419317fb7a119bc497c0a6ca6b4e325e0f85b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 14:35:07 -0400 Subject: [PATCH 079/101] eliminated more copy/paste mess between Method and StaticMethod --- wrap/Argument.cpp | 85 ++++++++++++++++++++++++++++++++----------- wrap/Argument.h | 9 +++++ wrap/Method.cpp | 80 ++++++++++++---------------------------- wrap/StaticMethod.cpp | 73 +++++++++++-------------------------- 4 files changed, 118 insertions(+), 129 deletions(-) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 689ca6d88..2ee705c5b 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -32,14 +32,14 @@ using namespace wrap; string Argument::matlabClass(const string& delim) const { string result; BOOST_FOREACH(const string& ns, namespaces) - result += ns + delim; - if (type=="string" || type=="unsigned char" || type=="char") + result += ns + delim; + if (type == "string" || type == "unsigned char" || type == "char") return result + "char"; - if (type=="Vector" || type=="Matrix") + if (type == "Vector" || type == "Matrix") return result + "double"; - if (type=="int" || type=="size_t") + if (type == "int" || type == "size_t") return result + "numeric"; - if (type=="bool") + if (type == "bool") return result + "logical"; return result + type; } @@ -53,7 +53,8 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { if (is_ptr) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer - file.oss << "boost::shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< "; + file.oss << "boost::shared_ptr<" << cppType << "> " << name + << " = unwrap_shared_ptr< "; else if (is_ref) // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< "; @@ -66,23 +67,28 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << cppType << " " << name << " = unwrap< "; file.oss << cppType << " >(" << matlabName; - if (is_ptr || is_ref) file.oss << ", \"ptr_" << matlabUniqueType << "\""; + if (is_ptr || is_ref) + file.oss << ", \"ptr_" << matlabUniqueType << "\""; file.oss << ");" << endl; } /* ************************************************************************* */ string Argument::qualifiedType(const string& delim) const { string result; - BOOST_FOREACH(const string& ns, namespaces) result += ns + delim; + BOOST_FOREACH(const string& ns, namespaces) + result += ns + delim; return result + type; } /* ************************************************************************* */ string ArgumentList::types() const { string str; - bool first=true; + bool first = true; BOOST_FOREACH(Argument arg, *this) { - if (!first) str += ","; str += arg.type; first=false; + if (!first) + str += ","; + str += arg.type; + first = false; } return str; } @@ -90,16 +96,17 @@ string ArgumentList::types() const { /* ************************************************************************* */ string ArgumentList::signature() const { string sig; - bool cap=false; + bool cap = false; BOOST_FOREACH(Argument arg, *this) { BOOST_FOREACH(char ch, arg.type) - if(isupper(ch)) { - sig += ch; - //If there is a capital letter, we don't want to read it below - cap=true; - } - if(!cap) sig += arg.type[0]; + if (isupper(ch)) { + sig += ch; + //If there is a capital letter, we don't want to read it below + cap = true; + } + if (!cap) + sig += arg.type[0]; //Reset to default cap = false; } @@ -110,9 +117,12 @@ string ArgumentList::signature() const { /* ************************************************************************* */ string ArgumentList::names() const { string str; - bool first=true; + bool first = true; BOOST_FOREACH(Argument arg, *this) { - if (!first) str += ","; str += arg.name; first=false; + if (!first) + str += ","; + str += arg.name; + first = false; } return str; } @@ -123,7 +133,7 @@ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const { BOOST_FOREACH(Argument arg, *this) { stringstream buf; buf << "in[" << index << "]"; - arg.matlab_unwrap(file,buf.str()); + arg.matlab_unwrap(file, buf.str()); index++; } } @@ -133,11 +143,44 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { file.oss << name << "("; bool first = true; BOOST_FOREACH(Argument arg, *this) { - if (!first) file.oss << ", "; + if (!first) + file.oss << ", "; file.oss << arg.type << " " << arg.name; first = false; } file.oss << ")"; } /* ************************************************************************* */ +void ArgumentList::emit_conditional_call(FileWriter& file, + const ReturnValue& returnVal, const string& wrapperName, int id, + bool staticMethod) const { + // Check nr of arguments + file.oss << "if length(varargin) == " << size(); + if (size() > 0) + file.oss << " && "; + // ...and their types + bool first = true; + for (size_t i = 0; i < size(); i++) { + if (!first) + file.oss << " && "; + file.oss << "isa(varargin{" << i + 1 << "},'" << (*this)[i].matlabClass(".") + << "')"; + first = false; + } + file.oss << "\n"; + + // output call to C++ wrapper + string output; + if (returnVal.isPair) + output = "[ varargout{1} varargout{2} ] = "; + else if (returnVal.category1 == ReturnValue::VOID) + output = ""; + else + output = "varargout{1} = "; + file.oss << " " << output << wrapperName << "(" << id; + if (!staticMethod) + file.oss << ", this"; + file.oss << ", varargin{:});\n"; +} +/* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h index 2a989713b..da6a733bb 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -20,6 +20,7 @@ #pragma once #include "FileWriter.h" +#include "ReturnValue.h" #include #include @@ -75,6 +76,14 @@ struct ArgumentList: public std::vector { */ void emit_prototype(FileWriter& file, const std::string& name) const; + /** + * emit conditional MATLAB call (checking arguments first) + * @param file output stream + * @param returnVal the return value + * @param wrapperName of method or function + */ + void emit_conditional_call(FileWriter& file, const ReturnValue& returnVal, + const std::string& wrapperName, int id, bool staticMethod=false) const; }; } // \namespace wrap diff --git a/wrap/Method.cpp b/wrap/Method.cpp index ae50a36ae..7018ad138 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -39,99 +39,67 @@ void Method::addOverload(bool verbose, bool is_const, const std::string& name, } /* ************************************************************************* */ -void Method::proxy_wrapper_fragments(FileWriter& proxyFile, - FileWriter& wrapperFile, const string& cppClassName, - const std::string& matlabQualName, const std::string& matlabUniqueName, - const string& wrapperName, const TypeAttributesTable& typeAttributes, +void Method::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile, + const string& cppClassName, const std::string& matlabQualName, + const std::string& matlabUniqueName, const string& wrapperName, + const TypeAttributesTable& typeAttributes, vector& functionNames) const { // Create function header - proxyFile.oss << " function varargout = " << name << "(this, varargin)\n"; + file.oss << " function varargout = " << name << "(this, varargin)\n"; // Emit comments for documentation string up_name = boost::to_upper_copy(name); - proxyFile.oss << " % " << up_name << " usage: "; + file.oss << " % " << up_name << " usage: "; unsigned int argLCount = 0; BOOST_FOREACH(ArgumentList argList, argLists) { - argList.emit_prototype(proxyFile, name); + argList.emit_prototype(file, name); if (argLCount != argLists.size() - 1) - proxyFile.oss << ", "; + file.oss << ", "; else - proxyFile.oss << " : returns " + file.oss << " : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; argLCount++; } // Emit URL to Doxygen page - proxyFile.oss << " % " + file.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; // Document all overloads, if any if (argLists.size() > 1) { - proxyFile.oss << " % " << "" << endl; - proxyFile.oss << " % " << "Method Overloads" << endl; + file.oss << " % " << "" << endl; + file.oss << " % " << "Method Overloads" << endl; BOOST_FOREACH(ArgumentList argList, argLists) { - proxyFile.oss << " % "; - argList.emit_prototype(proxyFile, name); - proxyFile.oss << endl; + file.oss << " % "; + argList.emit_prototype(file, name); + file.oss << endl; } } - // For all overloads, check the number of arguments + // Check arguments for all overloads for (size_t overload = 0; overload < argLists.size(); ++overload) { - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; - size_t nrArgs = args.size(); - - const int id = functionNames.size(); // Output proxy matlab code - - // check for number of arguments... - proxyFile.oss << " " << (overload == 0 ? "" : "else") - << "if length(varargin) == " << nrArgs; - if (nrArgs > 0) - proxyFile.oss << " && "; - // ...and their types - bool first = true; - for (size_t i = 0; i < nrArgs; i++) { - if (!first) - proxyFile.oss << " && "; - proxyFile.oss << "isa(varargin{" << i + 1 << "},'" - << args[i].matlabClass(".") << "')"; - first = false; - } - proxyFile.oss << "\n"; - - // output call to C++ wrapper - string output; - if (returnVal.isPair) - output = "[ varargout{1} varargout{2} ] = "; - else if (returnVal.category1 == ReturnValue::VOID) - output = ""; - else - output = "varargout{1} = "; - proxyFile.oss << " " << output << wrapperName << "(" << id - << ", this, varargin{:});\n"; + file.oss << " " << (overload == 0 ? "" : "else"); + const int id = (int) functionNames.size(); + argLists[overload].emit_conditional_call(file, returnVals[overload], + wrapperName, id); // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, matlabUniqueName, overload, id, typeAttributes); // Add to function list functionNames.push_back(wrapFunctionName); - } - - proxyFile.oss << " else\n"; - proxyFile.oss - << " error('Arguments do not match any overload of function " + file.oss << " else\n"; + file.oss << " error('Arguments do not match any overload of function " << matlabQualName << "." << name << "');" << endl; + file.oss << " end\n"; - proxyFile.oss << " end\n"; - proxyFile.oss << " end\n"; + file.oss << " end\n"; } /* ************************************************************************* */ diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 29b3b12df..0c4cc7d75 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -39,7 +39,7 @@ void StaticMethod::addOverload(bool verbose, const std::string& name, } /* ************************************************************************* */ -void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, +void StaticMethod::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile, const string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName, const string& wrapperName, const TypeAttributesTable& typeAttributes, @@ -48,85 +48,54 @@ void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, string upperName = name; upperName[0] = std::toupper(upperName[0], std::locale()); - proxyFile.oss << " function varargout = " << upperName << "(varargin)\n"; + file.oss << " function varargout = " << upperName << "(varargin)\n"; //Comments for documentation string up_name = boost::to_upper_copy(name); - proxyFile.oss << " % " << up_name << " usage: "; + file.oss << " % " << up_name << " usage: "; unsigned int argLCount = 0; BOOST_FOREACH(ArgumentList argList, argLists) { - argList.emit_prototype(proxyFile, name); + argList.emit_prototype(file, name); if (argLCount != argLists.size() - 1) - proxyFile.oss << ", "; + file.oss << ", "; else - proxyFile.oss << " : returns " + file.oss << " : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl; argLCount++; } - proxyFile.oss << " % " + file.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl; - proxyFile.oss << " % " << "" << endl; - proxyFile.oss << " % " << "Usage" << endl; + file.oss << " % " << "" << endl; + file.oss << " % " << "Usage" << endl; BOOST_FOREACH(ArgumentList argList, argLists) { - proxyFile.oss << " % "; - argList.emit_prototype(proxyFile, up_name); - proxyFile.oss << endl; + file.oss << " % "; + argList.emit_prototype(file, up_name); + file.oss << endl; } + // Check arguments for all overloads for (size_t overload = 0; overload < argLists.size(); ++overload) { - const ArgumentList& args = argLists[overload]; - const ReturnValue& returnVal = returnVals[overload]; - size_t nrArgs = args.size(); - - const int id = (int) functionNames.size(); // Output proxy matlab code - - // check for number of arguments... - proxyFile.oss << " " << (overload == 0 ? "" : "else") - << "if length(varargin) == " << nrArgs; - if (nrArgs > 0) - proxyFile.oss << " && "; - // ...and their types - bool first = true; - for (size_t i = 0; i < nrArgs; i++) { - if (!first) - proxyFile.oss << " && "; - proxyFile.oss << "isa(varargin{" << i + 1 << "},'" - << args[i].matlabClass(".") << "')"; - first = false; - } - proxyFile.oss << "\n"; - - // output call to C++ wrapper - string output; - if (returnVal.isPair) - output = "[ varargout{1} varargout{2} ] = "; - else if (returnVal.category1 == ReturnValue::VOID) - output = ""; - else - output = "varargout{1} = "; - proxyFile.oss << " " << output << wrapperName << "(" << id - << ", varargin{:});\n"; + file.oss << " " << (overload == 0 ? "" : "else"); + const int id = (int) functionNames.size(); + argLists[overload].emit_conditional_call(file, returnVals[overload], + wrapperName, id, true); // last bool is to indicate static ! // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, matlabUniqueName, (int) overload, id, typeAttributes); // Add to function list functionNames.push_back(wrapFunctionName); - } - - proxyFile.oss << " else\n"; - proxyFile.oss - << " error('Arguments do not match any overload of function " + file.oss << " else\n"; + file.oss << " error('Arguments do not match any overload of function " << matlabQualName << "." << upperName << "');" << endl; + file.oss << " end\n"; - proxyFile.oss << " end\n"; - proxyFile.oss << " end\n"; + file.oss << " end\n"; } /* ************************************************************************* */ From 05008ecaa30d7a14d2110f69b750a33b55862312 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 14:40:39 -0400 Subject: [PATCH 080/101] Comments --- wrap/ReturnValue.h | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index ea78923f6..2bc5aa82b 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -17,23 +17,28 @@ namespace wrap { +/** + * Encapsulates return value of a method or function + */ struct ReturnValue { + /// the different supported return value categories typedef enum { CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 } return_category; - ReturnValue(bool enable_verbosity = true) : - verbose(enable_verbosity), isPtr1(false), isPtr2(false), isPair(false), category1( - CLASS), category2(CLASS) { - } - bool verbose; bool isPtr1, isPtr2, isPair; return_category category1, category2; std::string type1, type2; std::vector namespaces1, namespaces2; + /// Constructor + ReturnValue(bool enable_verbosity = true) : + verbose(enable_verbosity), isPtr1(false), isPtr2(false), isPair(false), category1( + CLASS), category2(CLASS) { + } + typedef enum { arg1, arg2, pair } pairing; From 52cd2007181d2561abefd6d4e4e95db5a9e5d6ad Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 14:52:49 -0400 Subject: [PATCH 081/101] ReturnValue now emits, eliminated some copy/paste. Also removed unused verbose field/argument in ReturnValue --- wrap/Argument.cpp | 11 +++-------- wrap/GlobalFunction.cpp | 11 +++-------- wrap/Method.cpp | 2 ++ wrap/Module.cpp | 2 +- wrap/ReturnValue.cpp | 8 ++++++++ wrap/ReturnValue.h | 8 ++++---- 6 files changed, 21 insertions(+), 21 deletions(-) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 2ee705c5b..1d7c43809 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -170,14 +170,9 @@ void ArgumentList::emit_conditional_call(FileWriter& file, file.oss << "\n"; // output call to C++ wrapper - string output; - if (returnVal.isPair) - output = "[ varargout{1} varargout{2} ] = "; - else if (returnVal.category1 == ReturnValue::VOID) - output = ""; - else - output = "varargout{1} = "; - file.oss << " " << output << wrapperName << "(" << id; + file.oss << " "; + returnVal.emit_matlab(file); + file.oss << wrapperName << "(" << id; if (!staticMethod) file.oss << ", this"; file.oss << ", varargin{:});\n"; diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index 16f6d48f1..588cbf7c8 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -103,14 +103,9 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons mfunctionFile.oss << "\n"; // output call to C++ wrapper - string output; - if(returnVal.isPair) - output = "[ varargout{1} varargout{2} ] = "; - else if(returnVal.category1 == ReturnValue::VOID) - output = ""; - else - output = "varargout{1} = "; - mfunctionFile.oss << " " << output << wrapperName << "(" << id << ", varargin{:});\n"; + mfunctionFile.oss << " "; + returnVal.emit_matlab(mfunctionFile); + mfunctionFile.oss << wrapperName << "(" << id << ", varargin{:});\n"; // Output C++ wrapper code diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 7018ad138..c0ec79e91 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -78,6 +78,8 @@ void Method::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile, } } + // Handle special case of single overload with numeric + // Check arguments for all overloads for (size_t overload = 0; overload < argLists.size(); ++overload) { diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 6870d5178..2cb5ea7ed 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -98,7 +98,7 @@ void Module::parseMarkup(const std::string& data) { // The one with postfix 0 are used to reset the variables after parse. string methodName, methodName0; bool isConst, isConst0 = false; - ReturnValue retVal0(verbose), retVal(verbose); + ReturnValue retVal0, retVal; Argument arg0, arg; ArgumentList args0, args; vector arg_dup; ///keep track of duplicates diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 3cb68181b..78e36d4da 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -141,5 +141,13 @@ void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const { } } /* ************************************************************************* */ +void ReturnValue::emit_matlab(FileWriter& file) const { + string output; + if (isPair) + file.oss << "[ varargout{1} varargout{2} ] = "; + else if (category1 != ReturnValue::VOID) + file.oss << "varargout{1} = "; +} +/* ************************************************************************* */ diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index 2bc5aa82b..989f81109 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -27,16 +27,15 @@ struct ReturnValue { CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 } return_category; - bool verbose; bool isPtr1, isPtr2, isPair; return_category category1, category2; std::string type1, type2; std::vector namespaces1, namespaces2; /// Constructor - ReturnValue(bool enable_verbosity = true) : - verbose(enable_verbosity), isPtr1(false), isPtr2(false), isPair(false), category1( - CLASS), category2(CLASS) { + ReturnValue() : + isPtr1(false), isPtr2(false), isPair(false), category1(CLASS), category2( + CLASS) { } typedef enum { @@ -55,6 +54,7 @@ struct ReturnValue { void wrapTypeUnwrap(FileWriter& wrapperFile) const; + void emit_matlab(FileWriter& file) const; }; } // \namespace wrap From 9d9614d18552a627dc2949eeab7e3b652b26f1bd Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 14:59:20 -0400 Subject: [PATCH 082/101] Split up into two calls --- wrap/Argument.cpp | 15 ++++++++++----- wrap/Argument.h | 15 +++++++++++++-- 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 1d7c43809..fb3d962ed 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -151,6 +151,15 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { file.oss << ")"; } /* ************************************************************************* */ +void ArgumentList::emit_call(FileWriter& file, const ReturnValue& returnVal, + const string& wrapperName, int id, bool staticMethod) const { + returnVal.emit_matlab(file); + file.oss << wrapperName << "(" << id; + if (!staticMethod) + file.oss << ", this"; + file.oss << ", varargin{:});\n"; +} +/* ************************************************************************* */ void ArgumentList::emit_conditional_call(FileWriter& file, const ReturnValue& returnVal, const string& wrapperName, int id, bool staticMethod) const { @@ -171,11 +180,7 @@ void ArgumentList::emit_conditional_call(FileWriter& file, // output call to C++ wrapper file.oss << " "; - returnVal.emit_matlab(file); - file.oss << wrapperName << "(" << id; - if (!staticMethod) - file.oss << ", this"; - file.oss << ", varargin{:});\n"; + emit_call(file, returnVal, wrapperName, id, staticMethod); } /* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h index da6a733bb..9d57f2853 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -77,13 +77,24 @@ struct ArgumentList: public std::vector { void emit_prototype(FileWriter& file, const std::string& name) const; /** - * emit conditional MATLAB call (checking arguments first) + * emit emit MATLAB call to wrapper * @param file output stream * @param returnVal the return value * @param wrapperName of method or function + * @param staticMethod flag to emit "this" in call + */ + void emit_call(FileWriter& file, const ReturnValue& returnVal, + const std::string& wrapperName, int id, bool staticMethod = false) const; + + /** + * emit conditional MATLAB call to wrapper (checking arguments first) + * @param file output stream + * @param returnVal the return value + * @param wrapperName of method or function + * @param staticMethod flag to emit "this" in call */ void emit_conditional_call(FileWriter& file, const ReturnValue& returnVal, - const std::string& wrapperName, int id, bool staticMethod=false) const; + const std::string& wrapperName, int id, bool staticMethod = false) const; }; } // \namespace wrap From 5987f78be34effcd5ba70fa303e3911c59e8459e Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 15:21:13 -0400 Subject: [PATCH 083/101] Methods to check whether arguments are scalar --- wrap/Argument.cpp | 13 +++++++++++++ wrap/Argument.h | 6 ++++++ 2 files changed, 19 insertions(+) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index fb3d962ed..d76556e4a 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -44,6 +44,12 @@ string Argument::matlabClass(const string& delim) const { return result + type; } +/* ************************************************************************* */ +bool Argument::isScalar() const { + return (type == "bool" || type == "char" || type == "unsigned char" + || type == "int" || type == "size_t" || type == "double"); +} + /* ************************************************************************* */ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << " "; @@ -127,6 +133,13 @@ string ArgumentList::names() const { return str; } +/* ************************************************************************* */ +bool ArgumentList::allScalar() const { + BOOST_FOREACH(Argument arg, *this) + if (!arg.isScalar()) return false; + return true; +} + /* ************************************************************************* */ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const { int index = start; diff --git a/wrap/Argument.h b/wrap/Argument.h index 9d57f2853..6f791978a 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -41,6 +41,9 @@ struct Argument { /// return MATLAB class for use in isa(x,class) std::string matlabClass(const std::string& delim = "") const; + /// Check if will be unwrapped using scalar login in wrap/matlab.h + bool isScalar() const; + /// adds namespaces to type std::string qualifiedType(const std::string& delim = "") const; @@ -60,6 +63,9 @@ struct ArgumentList: public std::vector { /// create a comma-separated string listing all argument names, used in m-files std::string names() const; + /// Check if all arguments scalar + bool allScalar() const; + // MATLAB code generation: /** From a38504dc6a615a2479f5a7f9c4331f058d510d78 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 15:21:33 -0400 Subject: [PATCH 084/101] Ignore some files --- wrap/tests/expected/.gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 wrap/tests/expected/.gitignore diff --git a/wrap/tests/expected/.gitignore b/wrap/tests/expected/.gitignore new file mode 100644 index 000000000..6d725d9bc --- /dev/null +++ b/wrap/tests/expected/.gitignore @@ -0,0 +1 @@ +*.m~ From 9a102e8c5976b7af629f3bf377fc17dbd0211ac4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 15:21:49 -0400 Subject: [PATCH 085/101] Handle special case of single overload with all numeric arguments --- wrap/Method.cpp | 40 ++++++++++------ wrap/tests/expected/Point2.m | 42 +++-------------- wrap/tests/expected/Point3.m | 6 +-- wrap/tests/expected/Test.m | 48 ++++---------------- wrap/tests/expected_namespaces/+ns2/ClassA.m | 12 +---- 5 files changed, 45 insertions(+), 103 deletions(-) diff --git a/wrap/Method.cpp b/wrap/Method.cpp index c0ec79e91..7b88b9cdc 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -78,28 +78,42 @@ void Method::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile, } } - // Handle special case of single overload with numeric - - // Check arguments for all overloads - for (size_t overload = 0; overload < argLists.size(); ++overload) { - + // Handle special case of single overload with all numeric arguments + if (argLists.size() == 1 && argLists[0].allScalar()) { // Output proxy matlab code - file.oss << " " << (overload == 0 ? "" : "else"); + file.oss << " "; const int id = (int) functionNames.size(); - argLists[overload].emit_conditional_call(file, returnVals[overload], - wrapperName, id); + argLists[0].emit_call(file, returnVals[0], wrapperName, id); // Output C++ wrapper code const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, - matlabUniqueName, overload, id, typeAttributes); + matlabUniqueName, 0, id, typeAttributes); // Add to function list functionNames.push_back(wrapFunctionName); + } else { + // Check arguments for all overloads + for (size_t overload = 0; overload < argLists.size(); ++overload) { + + // Output proxy matlab code + file.oss << " " << (overload == 0 ? "" : "else"); + const int id = (int) functionNames.size(); + argLists[overload].emit_conditional_call(file, returnVals[overload], + wrapperName, id); + + // Output C++ wrapper code + const string wrapFunctionName = wrapper_fragment(wrapperFile, + cppClassName, matlabUniqueName, overload, id, typeAttributes); + + // Add to function list + functionNames.push_back(wrapFunctionName); + } + file.oss << " else\n"; + file.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << "." << name << "');" << endl; + file.oss << " end\n"; } - file.oss << " else\n"; - file.oss << " error('Arguments do not match any overload of function " - << matlabQualName << "." << name << "');" << endl; - file.oss << " end\n"; file.oss << " end\n"; } diff --git a/wrap/tests/expected/Point2.m b/wrap/tests/expected/Point2.m index fa0741fe2..9f64f2d10 100644 --- a/wrap/tests/expected/Point2.m +++ b/wrap/tests/expected/Point2.m @@ -44,71 +44,43 @@ classdef Point2 < handle function varargout = argChar(this, varargin) % ARGCHAR usage: argChar(char a) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'char') - geometry_wrapper(4, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.argChar'); - end + geometry_wrapper(4, this, varargin{:}); end function varargout = argUChar(this, varargin) % ARGUCHAR usage: argUChar(unsigned char a) : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'char') - geometry_wrapper(5, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.argUChar'); - end + geometry_wrapper(5, this, varargin{:}); end function varargout = dim(this, varargin) % DIM usage: dim() : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - varargout{1} = geometry_wrapper(6, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.dim'); - end + varargout{1} = geometry_wrapper(6, this, varargin{:}); end function varargout = returnChar(this, varargin) % RETURNCHAR usage: returnChar() : returns char % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - varargout{1} = geometry_wrapper(7, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.returnChar'); - end + varargout{1} = geometry_wrapper(7, this, varargin{:}); end function varargout = vectorConfusion(this, varargin) % VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - varargout{1} = geometry_wrapper(8, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.vectorConfusion'); - end + varargout{1} = geometry_wrapper(8, this, varargin{:}); end function varargout = x(this, varargin) % X usage: x() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - varargout{1} = geometry_wrapper(9, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.x'); - end + varargout{1} = geometry_wrapper(9, this, varargin{:}); end function varargout = y(this, varargin) % Y usage: y() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - varargout{1} = geometry_wrapper(10, this, varargin{:}); - else - error('Arguments do not match any overload of function Point2.y'); - end + varargout{1} = geometry_wrapper(10, this, varargin{:}); end end diff --git a/wrap/tests/expected/Point3.m b/wrap/tests/expected/Point3.m index fd17df60b..8a43987b9 100644 --- a/wrap/tests/expected/Point3.m +++ b/wrap/tests/expected/Point3.m @@ -43,11 +43,7 @@ classdef Point3 < handle function varargout = norm(this, varargin) % NORM usage: norm() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - varargout{1} = geometry_wrapper(14, this, varargin{:}); - else - error('Arguments do not match any overload of function Point3.norm'); - end + varargout{1} = geometry_wrapper(14, this, varargin{:}); end function varargout = string_serialize(this, varargin) diff --git a/wrap/tests/expected/Test.m b/wrap/tests/expected/Test.m index a82ae3ed3..1afd15efe 100644 --- a/wrap/tests/expected/Test.m +++ b/wrap/tests/expected/Test.m @@ -66,41 +66,25 @@ classdef Test < handle function varargout = create_MixedPtrs(this, varargin) % CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - [ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.create_MixedPtrs'); - end + [ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:}); end function varargout = create_ptrs(this, varargin) % CREATE_PTRS usage: create_ptrs() : returns pair< SharedTest, SharedTest > % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.create_ptrs'); - end + [ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:}); end function varargout = print(this, varargin) % PRINT usage: print() : returns void % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - geometry_wrapper(26, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.print'); - end + geometry_wrapper(26, this, varargin{:}); end function varargout = return_Point2Ptr(this, varargin) % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2 % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = geometry_wrapper(27, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_Point2Ptr'); - end + varargout{1} = geometry_wrapper(27, this, varargin{:}); end function varargout = return_Test(this, varargin) @@ -126,21 +110,13 @@ classdef Test < handle function varargout = return_bool(this, varargin) % RETURN_BOOL usage: return_bool(bool value) : returns bool % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = geometry_wrapper(30, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_bool'); - end + varargout{1} = geometry_wrapper(30, this, varargin{:}); end function varargout = return_double(this, varargin) % RETURN_DOUBLE usage: return_double(double value) : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = geometry_wrapper(31, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_double'); - end + varargout{1} = geometry_wrapper(31, this, varargin{:}); end function varargout = return_field(this, varargin) @@ -156,11 +132,7 @@ classdef Test < handle function varargout = return_int(this, varargin) % RETURN_INT usage: return_int(int value) : returns int % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = geometry_wrapper(33, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_int'); - end + varargout{1} = geometry_wrapper(33, this, varargin{:}); end function varargout = return_matrix1(this, varargin) @@ -206,11 +178,7 @@ classdef Test < handle function varargout = return_size_t(this, varargin) % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = geometry_wrapper(38, this, varargin{:}); - else - error('Arguments do not match any overload of function Test.return_size_t'); - end + varargout{1} = geometry_wrapper(38, this, varargin{:}); end function varargout = return_string(this, varargin) diff --git a/wrap/tests/expected_namespaces/+ns2/ClassA.m b/wrap/tests/expected_namespaces/+ns2/ClassA.m index f31825851..9c064734e 100644 --- a/wrap/tests/expected_namespaces/+ns2/ClassA.m +++ b/wrap/tests/expected_namespaces/+ns2/ClassA.m @@ -40,11 +40,7 @@ classdef ClassA < handle function varargout = memberFunction(this, varargin) % MEMBERFUNCTION usage: memberFunction() : returns double % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 0 - varargout{1} = testNamespaces_wrapper(9, this, varargin{:}); - else - error('Arguments do not match any overload of function ns2.ClassA.memberFunction'); - end + varargout{1} = testNamespaces_wrapper(9, this, varargin{:}); end function varargout = nsArg(this, varargin) @@ -60,11 +56,7 @@ classdef ClassA < handle function varargout = nsReturn(this, varargin) % NSRETURN usage: nsReturn(double q) : returns ns2::ns3::ClassB % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html - if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = testNamespaces_wrapper(11, this, varargin{:}); - else - error('Arguments do not match any overload of function ns2.ClassA.nsReturn'); - end + varargout{1} = testNamespaces_wrapper(11, this, varargin{:}); end end From 05a38ca263cf6dbea6ab29cc060f99ea42c967c5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 16:01:30 -0400 Subject: [PATCH 086/101] Standard BORG formatting --- wrap/GlobalFunction.cpp | 80 ++++++++++++++++++++++------------------- wrap/GlobalFunction.h | 33 ++++++++--------- 2 files changed, 60 insertions(+), 53 deletions(-) diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index 588cbf7c8..26d52018e 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -17,7 +17,8 @@ using namespace std; /* ************************************************************************* */ void GlobalFunction::addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal, const StrVec& ns_stack) { + const ArgumentList& args, const ReturnValue& retVal, + const StrVec& ns_stack) { this->verbose_ = verbose; this->name = name; this->argLists.push_back(args); @@ -26,16 +27,16 @@ void GlobalFunction::addOverload(bool verbose, const std::string& name, } /* ************************************************************************* */ -void GlobalFunction::matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, - std::vector& functionNames) const { +void GlobalFunction::matlab_proxy(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const { // cluster overloads with same namespace // create new GlobalFunction structures around namespaces - same namespaces and names are overloads // map of namespace to global function typedef map GlobalFunctionMap; GlobalFunctionMap grouped_functions; - for (size_t i=0; i& functionNames) const { +void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const { // create the folder for the namespace const StrVec& ns = namespaces.front(); @@ -68,20 +70,18 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons // open destination mfunctionFileName string mfunctionFileName = toolboxPath; - if(!ns.empty()) + if (!ns.empty()) mfunctionFileName += "/+" + wrap::qualifiedName("/+", ns); mfunctionFileName += "/" + name + ".m"; FileWriter mfunctionFile(mfunctionFileName, verbose_, "%"); // get the name of actual matlab object - const string - matlabQualName = qualifiedName(".", ns, name), - matlabUniqueName = qualifiedName("", ns, name), - cppName = qualifiedName("::", ns, name); + const string matlabQualName = qualifiedName(".", ns, name), matlabUniqueName = + qualifiedName("", ns, name), cppName = qualifiedName("::", ns, name); mfunctionFile.oss << "function varargout = " << name << "(varargin)\n"; - for(size_t overload = 0; overload < argLists.size(); ++overload) { + for (size_t overload = 0; overload < argLists.size(); ++overload) { const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; size_t nrArgs = args.size(); @@ -91,14 +91,18 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons // Output proxy matlab code // check for number of arguments... - mfunctionFile.oss << (overload==0?"":"else") << "if length(varargin) == " << nrArgs; - if (nrArgs>0) mfunctionFile.oss << " && "; + mfunctionFile.oss << (overload == 0 ? "" : "else") + << "if length(varargin) == " << nrArgs; + if (nrArgs > 0) + mfunctionFile.oss << " && "; // ...and their types bool first = true; - for(size_t i=0;i(id); + const string wrapFunctionName = matlabUniqueName + "_" + + boost::lexical_cast(id); // call - wrapperFile.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + file.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start - wrapperFile.oss << "{\n"; + file.oss << "{\n"; - returnVal.wrapTypeUnwrap(wrapperFile); + returnVal.wrapTypeUnwrap(file); // check arguments // NOTE: for static functions, there is no object passed - wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << "\",nargout,nargin," << args.size() << ");\n"; + file.oss << " checkArguments(\"" << matlabUniqueName + << "\",nargout,nargin," << args.size() << ");\n"; // unwrap arguments, see Argument.cpp - args.matlab_unwrap(wrapperFile,0); // We start at 0 because there is no self object + args.matlab_unwrap(file, 0); // We start at 0 because there is no self object // call method with default type and wrap result - if (returnVal.type1!="void") - returnVal.wrap_result(cppName+"("+args.names()+")", wrapperFile, typeAttributes); + if (returnVal.type1 != "void") + returnVal.wrap_result(cppName + "(" + args.names() + ")", file, + typeAttributes); else - wrapperFile.oss << cppName+"("+args.names()+");\n"; + file.oss << cppName + "(" + args.names() + ");\n"; // finish - wrapperFile.oss << "}\n"; + file.oss << "}\n"; // Add to function list functionNames.push_back(wrapFunctionName); } mfunctionFile.oss << "else\n"; - mfunctionFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << "');" << endl; + mfunctionFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << "');" << endl; mfunctionFile.oss << "end" << endl; // Close file @@ -148,9 +158,5 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons /* ************************************************************************* */ - } // \namespace wrap - - - diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index 2ecaf4998..6c8ad0c86 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -22,37 +22,38 @@ struct GlobalFunction { std::string name; // each overload, regardless of namespace - std::vector argLists; ///< arugments for each overload - std::vector returnVals; ///< returnVals for each overload - std::vector namespaces; ///< Stack of namespaces + std::vector argLists; ///< arugments for each overload + std::vector returnVals; ///< returnVals for each overload + std::vector namespaces; ///< Stack of namespaces // Constructor only used in Module - GlobalFunction(bool verbose = true) : verbose_(verbose) {} + GlobalFunction(bool verbose = true) : + verbose_(verbose) { + } // Used to reconstruct - GlobalFunction(const std::string& name_, bool verbose = true) - : verbose_(verbose), name(name_) {} + GlobalFunction(const std::string& name_, bool verbose = true) : + verbose_(verbose), name(name_) { + } // adds an overloaded version of this function void addOverload(bool verbose, const std::string& name, - const ArgumentList& args, const ReturnValue& retVal, const StrVec& ns_stack); + const ArgumentList& args, const ReturnValue& retVal, + const StrVec& ns_stack); // codegen function called from Module to build the cpp and matlab versions of the function - void matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, - std::vector& functionNames) const; + void matlab_proxy(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const; private: // Creates a single global function - all in same namespace - void generateSingleFunction(const std::string& toolboxPath, const std::string& wrapperName, - const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, - std::vector& functionNames) const; + void generateSingleFunction(const std::string& toolboxPath, + const std::string& wrapperName, const TypeAttributesTable& typeAttributes, + FileWriter& file, std::vector& functionNames) const; }; } // \namespace wrap - - - From 1129b384b2010c76c94fc5f04b00e46a9d864476 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 16:03:28 -0400 Subject: [PATCH 087/101] Header order --- wrap/tests/testWrap.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 8bf2c1412..2cf152a7d 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -15,16 +15,18 @@ * @author Frank Dellaert **/ +#include +#include + +#include + +#include +#include + #include #include #include #include -#include -#include -#include - -#include -#include using namespace std; using namespace boost::assign; From 925f23515d0103bbf01b8eef64703cb8f024abd1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 16:27:29 -0400 Subject: [PATCH 088/101] Better reporting of whitespace changes only --- wrap/utilities.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 870ba3101..47f7d10a6 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -88,9 +88,11 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) string actual_contents = file_contents(actual, skipheader); bool equal = actual_contents == expected_contents; if (!equal) { + cerr << "<<< DIFF OUTPUT (if none, white-space differences only):\n"; stringstream command; - command << "diff " << expected << " " << actual << endl; + command << "diff --ignore-all-space " << expected << " " << actual << endl; system(command.str().c_str()); + cerr << ">>>\n"; } return equal; } From 5e9632e7813328a00f4bac27c3aafedb4a3f8a47 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 16:28:39 -0400 Subject: [PATCH 089/101] Now using emit_conditional_call (changed indenting) --- wrap/GlobalFunction.cpp | 31 ++++++--------------------- wrap/tests/expected/aGlobalFunction.m | 10 ++++----- 2 files changed, 11 insertions(+), 30 deletions(-) diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index 26d52018e..25e1dcedb 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -84,32 +84,13 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, for (size_t overload = 0; overload < argLists.size(); ++overload) { const ArgumentList& args = argLists[overload]; const ReturnValue& returnVal = returnVals[overload]; - size_t nrArgs = args.size(); const int id = functionNames.size(); // Output proxy matlab code - - // check for number of arguments... - mfunctionFile.oss << (overload == 0 ? "" : "else") - << "if length(varargin) == " << nrArgs; - if (nrArgs > 0) - mfunctionFile.oss << " && "; - // ...and their types - bool first = true; - for (size_t i = 0; i < nrArgs; i++) { - if (!first) - mfunctionFile.oss << " && "; - mfunctionFile.oss << "isa(varargin{" << i + 1 << "},'" - << args[i].matlabClass(".") << "')"; - first = false; - } - mfunctionFile.oss << "\n"; - - // output call to C++ wrapper - mfunctionFile.oss << " "; - returnVal.emit_matlab(mfunctionFile); - mfunctionFile.oss << wrapperName << "(" << id << ", varargin{:});\n"; + mfunctionFile.oss << " " << (overload == 0 ? "" : "else"); + argLists[overload].emit_conditional_call(mfunctionFile, + returnVals[overload], wrapperName, id, true); // true omits "this" // Output C++ wrapper code @@ -146,11 +127,11 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, functionNames.push_back(wrapFunctionName); } - mfunctionFile.oss << "else\n"; + mfunctionFile.oss << " else\n"; mfunctionFile.oss - << " error('Arguments do not match any overload of function " + << " error('Arguments do not match any overload of function " << matlabQualName << "');" << endl; - mfunctionFile.oss << "end" << endl; + mfunctionFile.oss << " end" << endl; // Close file mfunctionFile.emit(true); diff --git a/wrap/tests/expected/aGlobalFunction.m b/wrap/tests/expected/aGlobalFunction.m index 09ece0b83..94e9b4a67 100644 --- a/wrap/tests/expected/aGlobalFunction.m +++ b/wrap/tests/expected/aGlobalFunction.m @@ -1,6 +1,6 @@ function varargout = aGlobalFunction(varargin) -if length(varargin) == 0 - varargout{1} = geometry_wrapper(42, varargin{:}); -else - error('Arguments do not match any overload of function aGlobalFunction'); -end + if length(varargin) == 0 + varargout{1} = geometry_wrapper(42, varargin{:}); + else + error('Arguments do not match any overload of function aGlobalFunction'); + end From 399c5e555167df20ea8845a37dccaf27bcab07d2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 16:28:59 -0400 Subject: [PATCH 090/101] Added test for overloaded global functions --- wrap/tests/expected/geometry_wrapper.cpp | 19 +++++++++++++++++++ .../tests/expected/overloadedGlobalFunction.m | 8 ++++++++ wrap/tests/geometry.h | 4 ++++ wrap/tests/testWrap.cpp | 5 +++-- 4 files changed, 34 insertions(+), 2 deletions(-) create mode 100644 wrap/tests/expected/overloadedGlobalFunction.m diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index e00004d57..b34112718 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -502,6 +502,19 @@ void aGlobalFunction_42(int nargout, mxArray *out[], int nargin, const mxArray * checkArguments("aGlobalFunction",nargout,nargin,0); out[0] = wrap< Vector >(aGlobalFunction()); } +void overloadedGlobalFunction_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("overloadedGlobalFunction",nargout,nargin,1); + int a = unwrap< int >(in[0]); + out[0] = wrap< Vector >(overloadedGlobalFunction(a)); +} +void overloadedGlobalFunction_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("overloadedGlobalFunction",nargout,nargin,2); + int a = unwrap< int >(in[0]); + double b = unwrap< double >(in[1]); + out[0] = wrap< Vector >(overloadedGlobalFunction(a,b)); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { @@ -643,6 +656,12 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) case 42: aGlobalFunction_42(nargout, out, nargin-1, in+1); break; + case 43: + overloadedGlobalFunction_43(nargout, out, nargin-1, in+1); + break; + case 44: + overloadedGlobalFunction_44(nargout, out, nargin-1, in+1); + break; } } catch(const std::exception& e) { mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); diff --git a/wrap/tests/expected/overloadedGlobalFunction.m b/wrap/tests/expected/overloadedGlobalFunction.m new file mode 100644 index 000000000..5b086b15e --- /dev/null +++ b/wrap/tests/expected/overloadedGlobalFunction.m @@ -0,0 +1,8 @@ +function varargout = overloadedGlobalFunction(varargin) + if length(varargin) == 1 && isa(varargin{1},'numeric') + varargout{1} = geometry_wrapper(43, varargin{:}); + elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double') + varargout{1} = geometry_wrapper(44, varargin{:}); + else + error('Arguments do not match any overload of function overloadedGlobalFunction'); + end diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index bdced45ed..bc233763d 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -87,6 +87,10 @@ class Test { Vector aGlobalFunction(); +// An overloaded global function +Vector overloadedGlobalFunction(int a); +Vector overloadedGlobalFunction(int a, double b); + // comments at the end! // even more comments at the end! diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 2cf152a7d..290372c09 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -307,8 +307,8 @@ TEST( wrap, parse_geometry ) { } // evaluate global functions -// Vector aGlobalFunction(); - LONGS_EQUAL(1, module.global_functions.size()); + // Vector aGlobalFunction(); + LONGS_EQUAL(2, module.global_functions.size()); CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end()); { GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); @@ -447,6 +447,7 @@ TEST( wrap, matlab_code_geometry ) { EXPECT(files_equal(epath + "Point3.m" , apath + "Point3.m" )); EXPECT(files_equal(epath + "Test.m" , apath + "Test.m" )); EXPECT(files_equal(epath + "aGlobalFunction.m" , apath + "aGlobalFunction.m" )); + EXPECT(files_equal(epath + "overloadedGlobalFunction.m" , apath + "overloadedGlobalFunction.m" )); } /* ************************************************************************* */ From 5048946ae945a22dd6185107bc4cdc22ea359da7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 16:37:43 -0400 Subject: [PATCH 091/101] Make sure it works for namespaces as well --- .../+ns2/overloadedGlobalFunction.m | 8 +++++++ .../testNamespaces_wrapper.cpp | 21 +++++++++++++++++++ wrap/tests/testNamespaces.h | 4 ++++ wrap/tests/testWrap.cpp | 20 +++++++++++------- 4 files changed, 45 insertions(+), 8 deletions(-) create mode 100644 wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m diff --git a/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m b/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m new file mode 100644 index 000000000..84f3b8f47 --- /dev/null +++ b/wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m @@ -0,0 +1,8 @@ +function varargout = overloadedGlobalFunction(varargin) + if length(varargin) == 1 && isa(varargin{1},'ns1.ClassA') + varargout{1} = testNamespaces_wrapper(24, varargin{:}); + elseif length(varargin) == 2 && isa(varargin{1},'ns1.ClassA') && isa(varargin{2},'double') + varargout{1} = testNamespaces_wrapper(25, varargin{:}); + else + error('Arguments do not match any overload of function ns2.overloadedGlobalFunction'); + end diff --git a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp index 29739a2f6..6d22e1625 100644 --- a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp +++ b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp @@ -342,6 +342,21 @@ void ns2aGlobalFunction_23(int nargout, mxArray *out[], int nargin, const mxArra checkArguments("ns2aGlobalFunction",nargout,nargin,0); out[0] = wrap< Vector >(ns2::aGlobalFunction()); } +void ns2overloadedGlobalFunction_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedClassA; + checkArguments("ns2overloadedGlobalFunction",nargout,nargin,1); + ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA"); + out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a))),"ns1.ClassA", false); +} +void ns2overloadedGlobalFunction_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr SharedClassA; + checkArguments("ns2overloadedGlobalFunction",nargout,nargin,2); + ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA"); + double b = unwrap< double >(in[1]); + out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a,b))),"ns1.ClassA", false); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { @@ -426,6 +441,12 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) case 23: ns2aGlobalFunction_23(nargout, out, nargin-1, in+1); break; + case 24: + ns2overloadedGlobalFunction_24(nargout, out, nargin-1, in+1); + break; + case 25: + ns2overloadedGlobalFunction_25(nargout, out, nargin-1, in+1); + break; } } catch(const std::exception& e) { mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); diff --git a/wrap/tests/testNamespaces.h b/wrap/tests/testNamespaces.h index 8e6ef51d6..a9b23cad1 100644 --- a/wrap/tests/testNamespaces.h +++ b/wrap/tests/testNamespaces.h @@ -47,6 +47,10 @@ class ClassC { // separate namespace global function, same name Vector aGlobalFunction(); +// An overloaded global function +ns1::ClassA overloadedGlobalFunction(const ns1::ClassA& a); +ns1::ClassA overloadedGlobalFunction(const ns1::ClassA& a, double b); + } //\namespace ns2 class ClassD { diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 290372c09..c6ce0903a 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -382,7 +382,7 @@ TEST( wrap, parse_namespaces ) { // evaluate global functions // Vector ns1::aGlobalFunction(); // Vector ns2::aGlobalFunction(); - LONGS_EQUAL(1, module.global_functions.size()); + LONGS_EQUAL(2, module.global_functions.size()); CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end()); { GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); @@ -417,13 +417,17 @@ TEST( wrap, matlab_code_namespaces ) { module.matlab_code("actual_namespaces", headerPath); - EXPECT(files_equal(exp_path + "ClassD.m" , act_path + "ClassD.m" )); - EXPECT(files_equal(exp_path + "+ns1/ClassA.m" , act_path + "+ns1/ClassA.m" )); - EXPECT(files_equal(exp_path + "+ns1/ClassB.m" , act_path + "+ns1/ClassB.m" )); - EXPECT(files_equal(exp_path + "+ns2/ClassA.m" , act_path + "+ns2/ClassA.m" )); - EXPECT(files_equal(exp_path + "+ns2/ClassC.m" , act_path + "+ns2/ClassC.m" )); - EXPECT(files_equal(exp_path + "+ns2/+ns3/ClassB.m" , act_path + "+ns2/+ns3/ClassB.m" )); - EXPECT(files_equal(exp_path + "testNamespaces_wrapper.cpp" , act_path + "testNamespaces_wrapper.cpp" )); + EXPECT(files_equal(exp_path + "ClassD.m", act_path + "ClassD.m" )); + EXPECT(files_equal(exp_path + "+ns1/ClassA.m", act_path + "+ns1/ClassA.m" )); + EXPECT(files_equal(exp_path + "+ns1/ClassB.m", act_path + "+ns1/ClassB.m" )); + EXPECT(files_equal(exp_path + "+ns2/ClassA.m", act_path + "+ns2/ClassA.m" )); + EXPECT(files_equal(exp_path + "+ns2/ClassC.m", act_path + "+ns2/ClassC.m" )); + EXPECT( + files_equal(exp_path + "+ns2/overloadedGlobalFunction.m", exp_path + "+ns2/overloadedGlobalFunction.m" )); + EXPECT( + files_equal(exp_path + "+ns2/+ns3/ClassB.m", act_path + "+ns2/+ns3/ClassB.m" )); + EXPECT( + files_equal(exp_path + "testNamespaces_wrapper.cpp", act_path + "testNamespaces_wrapper.cpp" )); } /* ************************************************************************* */ From 852e1e1f2f17419d78deda410d44b13bfc3f9ec3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 17:46:30 -0400 Subject: [PATCH 092/101] Drastic speedup of plotting --- matlab/+gtsam/plot2DTrajectory.m | 54 +++++++++++++------------------- 1 file changed, 21 insertions(+), 33 deletions(-) diff --git a/matlab/+gtsam/plot2DTrajectory.m b/matlab/+gtsam/plot2DTrajectory.m index c6b0f85aa..45e7fe467 100644 --- a/matlab/+gtsam/plot2DTrajectory.m +++ b/matlab/+gtsam/plot2DTrajectory.m @@ -8,53 +8,41 @@ function plot2DTrajectory(values, linespec, marginals) import gtsam.* if ~exist('linespec', 'var') || isempty(linespec) - linespec = 'k*-'; + linespec = 'k*-'; end haveMarginals = exist('marginals', 'var'); -keys = KeyVector(values.keys); holdstate = ishold; hold on -% Plot poses and covariance matrices -lastIndex = []; -for i = 0:keys.size-1 +% Do something very efficient to draw trajectory +poses = utilities.extractPose2(values); +X = poses(:,1); +Y = poses(:,2); +theta = poses(:,3); +plot(X,Y,linespec); + +% Quiver can also be vectorized if no marginals asked +if ~haveMarginals + C = cos(theta); + S = sin(theta); + quiver(X,Y,C,S,0.1,linespec); +else + % plotPose2 does both quiver and covariance matrix + keys = KeyVector(values.keys); + for i = 0:keys.size-1 key = keys.at(i); x = values.at(key); if isa(x, 'gtsam.Pose2') - if ~isempty(lastIndex) - % Draw line from last pose then covariance ellipse on top of - % last pose. - lastKey = keys.at(lastIndex); - lastPose = values.at(lastKey); - plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - gtsam.plotPose2(lastPose, 'g', P); - else - gtsam.plotPose2(lastPose, 'g', []); - end - - end - lastIndex = i; - end -end - -% Draw final covariance ellipse -if ~isempty(lastIndex) - lastKey = keys.at(lastIndex); - lastPose = values.at(lastKey); - if haveMarginals - P = marginals.marginalCovariance(lastKey); - gtsam.plotPose2(lastPose, 'g', P); - else - gtsam.plotPose2(lastPose, 'g', []); + P = marginals.marginalCovariance(key); + gtsam.plotPose2(x,linespec(1), P); end + end end if ~holdstate - hold off + hold off end end From c2e748b362e34ab07835a9cceb9358a53228d469 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 17:46:47 -0400 Subject: [PATCH 093/101] This example stopped working: now fixed --- matlab/gtsam_examples/Pose2SLAMExample_advanced.m | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/matlab/gtsam_examples/Pose2SLAMExample_advanced.m b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m index 343dee05b..64cda5afc 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_advanced.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m @@ -59,16 +59,16 @@ params.setAbsoluteErrorTol(1e-15); params.setRelativeErrorTol(1e-15); params.setVerbosity('ERROR'); params.setVerbosityDL('VERBOSE'); -params.setOrdering(graph.orderingCOLAMD(initialEstimate)); +params.setOrdering(graph.orderingCOLAMD()); optimizer = DoglegOptimizer(graph, initialEstimate, params); result = optimizer.optimizeSafely(); result.print('final result'); %% Get the corresponding dense matrix -ord = graph.orderingCOLAMD(result); -gfg = graph.linearize(result,ord); -denseAb = gfg.denseJacobian; +ord = graph.orderingCOLAMD(); +gfg = graph.linearize(result); +denseAb = gfg.augmentedJacobian; %% Get sparse matrix A and RHS b IJS = gfg.sparseJacobian_(); From 499f2f291855368e53bec198df7b9d8307f63cc5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 25 May 2014 17:47:12 -0400 Subject: [PATCH 094/101] Small comments in examples (and timing of marginals) --- matlab/gtsam_examples/Pose2SLAMExample_graph.m | 2 ++ matlab/gtsam_examples/Pose3SLAMExample_graph.m | 2 ++ 2 files changed, 4 insertions(+) diff --git a/matlab/gtsam_examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m index b4957cce3..83ec949cc 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m @@ -36,7 +36,9 @@ toc hold on; plot2DTrajectory(result, 'b-*'); %% Plot Covariance Ellipses +tic marginals = Marginals(graph, result); +toc P={}; for i=1:result.size()-1 pose_i = result.at(i); diff --git a/matlab/gtsam_examples/Pose3SLAMExample_graph.m b/matlab/gtsam_examples/Pose3SLAMExample_graph.m index 01df4fc33..39e48c204 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose3SLAMExample_graph.m @@ -12,6 +12,8 @@ import gtsam.* +%% PLEASE NOTE THAT PLOTTING TAKES A VERY LONG TIME HERE + %% Find data file N = 2500; % dataset = 'sphere_smallnoise.graph'; From 020d2e43f8f782b3ba20511e19383c45bf8ceec7 Mon Sep 17 00:00:00 2001 From: jing Date: Sun, 25 May 2014 20:09:49 -0400 Subject: [PATCH 095/101] change cmake option into correct way: use set CMAKE_CXX_FLAGS replace add_definition --- cmake/GtsamBuildTypes.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index ee2bbd42a..9a0b297ab 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -56,7 +56,7 @@ endif() # Clang on Mac uses a template depth that is less than standard and is too small if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0") - add_definitions(-ftemplate-depth=1024) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024") endif() endif() From f4cad73b73234b1851aaa0d49c27edf75948fee3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 26 May 2014 23:29:14 -0400 Subject: [PATCH 096/101] Fixed a problem with initialization (exposed by victoria_park.txt) and cleaned up a bit. Also fixed variances -> std deviation. --- gtsam/slam/dataset.cpp | 75 +++++++++++++++++++++++------------------- 1 file changed, 41 insertions(+), 34 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 4a7166f38..772c0ec97 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -112,14 +112,14 @@ pair load2D( // Create a sampler with random number generator Sampler sampler(42u); - // load the factors + // Parse the pose constraints + int id1, id2; bool haveLandmark = false; while (is) { if(! (is >> tag)) break; if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { - int id1, id2; double x, y, yaw; double v1, v2, v3, v4, v5, v6; @@ -168,68 +168,75 @@ pair load2D( new BetweenFactor(id1, id2, l1Xl2, *model)); graph->push_back(factor); } + + // Parse measurements + double bearing, range, bearing_std, range_std; + + // A bearing-range measurement if (tag == "BR") { - int id1, id2; - double bearing, range, bearing_std, range_std; - is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std; - - // optional filter - if (maxID && (id1 >= maxID || id2 >= maxID)) - continue; - - noiseModel::Diagonal::shared_ptr measurementNoise = - noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); - *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); - - // Insert poses or points if they do not exist yet - if (!initial->exists(id1)) - initial->insert(id1, Pose2()); - if (!initial->exists(id2)) { - Pose2 pose = initial->at(id1); - Point2 local(cos(bearing)*range,sin(bearing)*range); - Point2 global = pose.transform_from(local); - initial->insert(id2, global); - } } + + // A landmark measurement, TODO Frank says: don't know why is converted to bearing-range if (tag == "LANDMARK") { - int id1, id2; double lmx, lmy; double v1, v2, v3; is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3; // Convert x,y to bearing,range - double bearing = std::atan2(lmy, lmx); - double range = std::sqrt(lmx*lmx + lmy*lmy); + bearing = std::atan2(lmy, lmx); + range = std::sqrt(lmx*lmx + lmy*lmy); // In our experience, the x-y covariance on landmark sightings is not very good, so assume - // that it describes the uncertainty at a range of 10m, and convert that to bearing/range - // uncertainty. - SharedDiagonal measurementNoise; + // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty. if(std::abs(v1 - v3) < 1e-4) { - double rangeVar = v1; - double bearingVar = v1 / 10.0; - measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearingVar, rangeVar)); + bearing_std = sqrt(v1 / 10.0); + range_std = sqrt(v1); } else { + bearing_std = 1; + range_std = 1; if(!haveLandmark) { cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n" "non-uniform covariance on LANDMARK measurements in this file." << endl; haveLandmark = true; } } + } + + // Do some common stuff for bearing-range measurements + if (tag == "LANDMARK" || tag == "BR") { + + // optional filter + if (maxID && id1 >= maxID) + continue; + + // Create noise model + noiseModel::Diagonal::shared_ptr measurementNoise = + noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); // Add to graph - *graph += BearingRangeFactor(id1, L(id2), bearing, range, measurementNoise); + *graph += BearingRangeFactor(id1, L(id2), bearing, range, + measurementNoise); + + // Insert poses or points if they do not exist yet + if (!initial->exists(id1)) + initial->insert(id1, Pose2()); + if (!initial->exists(L(id2))) { + Pose2 pose = initial->at(id1); + Point2 local(cos(bearing)*range,sin(bearing)*range); + Point2 global = pose.transform_from(local); + initial->insert(L(id2), global); + } } is.ignore(LINESIZE, '\n'); } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } From 3b10f61e5c426bc1e57493de1701d7ccd4c51ab0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 27 May 2014 00:42:03 -0400 Subject: [PATCH 097/101] utilities.localToWorld --- gtsam.h | 2 ++ matlab.h | 32 ++++++++++++++++++++++++++++++++ matlab/+gtsam/Contents.m | 1 + 3 files changed, 35 insertions(+) diff --git a/gtsam.h b/gtsam.h index 99aec3f86..96a778acf 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2381,6 +2381,8 @@ namespace utilities { void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); } //\namespace utilities diff --git a/matlab.h b/matlab.h index e9ad519bd..c4891a615 100644 --- a/matlab.h +++ b/matlab.h @@ -28,6 +28,8 @@ #include #include +#include + #include namespace gtsam { @@ -218,6 +220,36 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph, return errors; } +/// Convert from local to world coordinates +Values localToWorld(const Values& local, const Pose2& base, + const FastVector user_keys = FastVector()) { + + Values world; + + // if no keys given, get all keys from local values + FastVector keys(user_keys); + if (keys.size()==0) + keys = FastVector(local.keys()); + + // Loop over all keys + BOOST_FOREACH(Key key, keys) { + try { + // if value is a Pose2, compose it with base pose + Pose2 pose = local.at(key); + world.insert(key, base.compose(pose)); + } catch (std::exception e1) { + try { + // if value is a Point2, transform it from base pose + Point2 point = local.at(key); + world.insert(key, base.transform_from(point)); + } catch (std::exception e2) { + // if not Pose2 or Point2, do nothing + } + } + } + return world; +} + } } diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 7a7491462..023c61dbe 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -189,3 +189,4 @@ % utilities.insertBackprojections - Insert a number of initial point values by backprojecting % utilities.insertProjectionFactors - Insert multiple projection factors for a single pose key % utilities.reprojectionErrors - Calculate the errors of all projection factors in a graph +% utilities.localToWorld - Convert from local to world coordinates From e730da95c4e67e6bd97b30e2b684e1bce5b53d0a Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 28 May 2014 09:17:31 -0400 Subject: [PATCH 098/101] Smarter noise models: Diagonal::Sigmas is now actually smart, and Gaussian::SqrtInformation now has a smart flag (default is true) --- gtsam/linear/NoiseModel.cpp | 63 ++++++++++++++++++++------- gtsam/linear/NoiseModel.h | 6 +-- gtsam/linear/tests/testNoiseModel.cpp | 18 ++++++++ 3 files changed, 69 insertions(+), 18 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 641b47640..4bce597a1 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -47,22 +47,50 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) { Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose(); } +/* ************************************************************************* */ +// check *above the diagonal* for non-zero entries +static boost::optional checkIfDiagonal(const Matrix M) { + size_t m = M.rows(), n = M.cols(); + // check all non-diagonal entries + bool full = false; + size_t i, j; + for (i = 0; i < m; i++) + if (!full) + for (j = i + 1; j < n; j++) + if (fabs(M(i, j)) > 1e-9) { + full = true; + break; + } + if (full) { + return boost::none; + } else { + Vector diagonal(n); + for (j = 0; j < n; j++) + diagonal(j) = M(j, j); + return diagonal; + } +} + +/* ************************************************************************* */ +Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { + size_t m = R.rows(), n = R.cols(); + if (m != n) throw invalid_argument("Gaussian::SqrtInformation: R not square"); + boost::optional diagonal = boost::none; + if (smart) + diagonal = checkIfDiagonal(R); + if (diagonal) return Diagonal::Sigmas(reciprocal(*diagonal),true); + else return shared_ptr(new Gaussian(R.rows(),R)); +} /* ************************************************************************* */ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) { size_t m = covariance.rows(), n = covariance.cols(); if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square"); - if (smart) { - // check all non-diagonal entries - size_t i,j; - for (i = 0; i < m; i++) - for (j = 0; j < n; j++) - if (i != j && fabs(covariance(i, j)) > 1e-9) goto full; - Vector variances(n); - for (j = 0; j < n; j++) variances(j) = covariance(j,j); - return Diagonal::Variances(variances,true); - } - full: return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); + boost::optional variances = boost::none; + if (smart) + variances = checkIfDiagonal(covariance); + if (variances) return Diagonal::Variances(*variances,true); + else return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); } /* ************************************************************************* */ @@ -166,7 +194,7 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const // Diagonal /* ************************************************************************* */ Diagonal::Diagonal() : - Gaussian(1)//, sigmas_(ones(1)), invsigmas_(ones(1)), precisions_(ones(1)) + Gaussian(1) // TODO: Frank asks: really sure about this? { } @@ -191,12 +219,17 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { /* ************************************************************************* */ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) { if (smart) { + DenseIndex j, n = sigmas.size(); // look for zeros to make a constraint - for (size_t i=0; i< (size_t) sigmas.size(); ++i) - if (sigmas(i)<1e-8) + for (j=0; j< n; ++j) + if (sigmas(j)<1e-8) return Constrained::MixedSigmas(sigmas); + // check whether all the same entry + for (j = 1; j < n; j++) + if (sigmas(j) != sigmas(0)) goto full; + return Isotropic::Sigma(n, sigmas(0), true); } - return Diagonal::shared_ptr(new Diagonal(sigmas)); + full: return Diagonal::shared_ptr(new Diagonal(sigmas)); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index a87f426fa..e709ea543 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -159,10 +159,10 @@ namespace gtsam { /** * A Gaussian noise model created by specifying a square root information matrix. + * @param R The (upper-triangular) square root information matrix + * @param smart check if can be simplified to derived class */ - static shared_ptr SqrtInformation(const Matrix& R) { - return shared_ptr(new Gaussian(R.rows(),R)); - } + static shared_ptr SqrtInformation(const Matrix& R, bool smart = true); /** * A Gaussian noise model created by specifying a covariance matrix. diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 9d87a163b..d68caeabe 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -267,6 +267,24 @@ TEST(NoiseModel, QRNan ) EXPECT(assert_equal(expectedAb,Ab)); } +/* ************************************************************************* */ +TEST(NoiseModel, SmartSqrtInformation ) +{ + bool smart = true; + gtsam::SharedGaussian expected = Unit::Create(3); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(eye(3), smart); + EXPECT(assert_equal(*expected,*actual)); +} + +/* ************************************************************************* */ +TEST(NoiseModel, SmartSqrtInformation2 ) +{ + bool smart = true; + gtsam::SharedGaussian expected = Unit::Isotropic::Sigma(3,2); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*eye(3), smart); + EXPECT(assert_equal(*expected,*actual)); +} + /* ************************************************************************* */ TEST(NoiseModel, SmartCovariance ) { From b7fc6762af9667a9654ffe7c190304f3d3506dec Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 28 May 2014 10:27:01 -0400 Subject: [PATCH 099/101] Changes every time ? --- examples/Data/dubrovnik-3-7-pre-rewritten.txt | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/examples/Data/dubrovnik-3-7-pre-rewritten.txt b/examples/Data/dubrovnik-3-7-pre-rewritten.txt index 12c9f4db4..7dea43c9e 100644 --- a/examples/Data/dubrovnik-3-7-pre-rewritten.txt +++ b/examples/Data/dubrovnik-3-7-pre-rewritten.txt @@ -20,9 +20,9 @@ 1 6 543.18011474609375 294.80999755859375 2 6 -58.419979095458984375 110.8300018310546875 -0.29656188120312942935 + 0.29656188120312942935 -0.035318354384285870207 -0.31252101755032046793 + 0.31252101755032046793 0.47230274932665988752 -0.3572340863744113415 -2.0517704282499575896 @@ -30,21 +30,21 @@ -7.5572756941255647689e-08 3.2377570134516087119e-14 -0.28532097381985194184 + 0.28532097381985194184 -0.27699838370789808817 0.048601169984112867206 --1.2598695987143850861 + -1.2598695987143850861 -0.049063798188844320869 --1.9586867140445654023 + -1.9586867140445654023 1432.137451171875 -7.3171918302250560373e-08 3.1759419042137054801e-14 0.057491325683772541433 -0.34853090049579965592 -0.47985129303736057116 -8.1963904289063389541 -6.5146840788718787252 + 0.34853090049579965592 + 0.47985129303736057116 + 8.1963904289063389541 + 6.5146840788718787252 -3.8392804395897406344 1572.047119140625 -1.5962623223231275915e-08 From 8dba25f532000e274815b3995f89f483a46eecce Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 28 May 2014 10:36:26 -0400 Subject: [PATCH 100/101] Rationalized some cholesky-related code as I was looking at it. --- .cproject | 1848 +++++++++++++++---------------- gtsam/base/Matrix.cpp | 47 +- gtsam/base/tests/testMatrix.cpp | 36 +- 3 files changed, 952 insertions(+), 979 deletions(-) diff --git a/.cproject b/.cproject index da776f062..af6b32cd2 100644 --- a/.cproject +++ b/.cproject @@ -568,7 +568,6 @@ make - tests/testBayesTree.run true false @@ -576,7 +575,6 @@ make - testBinaryBayesNet.run true false @@ -624,7 +622,6 @@ make - testSymbolicBayesNet.run true false @@ -632,7 +629,6 @@ make - tests/testSymbolicFactor.run true false @@ -640,7 +636,6 @@ make - testSymbolicFactorGraph.run true false @@ -656,20 +651,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -686,134 +672,6 @@ true true - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testStereoCamera.run - true - 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 @@ -854,46 +712,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - make -j2 @@ -910,134 +728,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j6 -j8 - testAntiFactor.run - true - true - true - - - make - -j6 -j8 - testBetweenFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - make -j5 @@ -1078,6 +768,30 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + make -j5 @@ -1126,46 +840,6 @@ true true - - make - -j5 - testWrap.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean - true - true - true - make -j5 @@ -1334,294 +1008,6 @@ true true - - make - -j5 - testDiscreteFactor.run - true - true - true - - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - true - true - - - make - -j2 - vSFMexample.run - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j2 - testVSLAMGraph - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j5 - testMarginals.run - true - true - true - - - make - -j5 - testGaussianISAM2.run - true - true - true - - - make - -j5 - testSymbolicFactorGraphB.run - true - true - true - - - make - -j2 - timeSequentialOnDataset.run - true - true - true - - - make - -j5 - testGradientDescentOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - -j2 - testNonlinearISAM.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testExtendedKalmanFilter.run - true - true - true - - - make - -j5 - timing.tests - true - true - true - - - make - -j5 - testNonlinearFactor.run - true - true - true - - - make - -j5 - clean - true - true - true - - - make - -j5 - testGaussianJunctionTreeB.run - true - true - true - - - make - - testGraph.run - true - false - true - - - make - - 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 - testSummarization.run - true - true - true - make -j5 @@ -1728,7 +1114,6 @@ make - testErrors.run true false @@ -1862,14 +1247,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1966,54 +1343,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -2104,6 +1433,7 @@ make + testSimulated2DOriented.run true false @@ -2143,6 +1473,7 @@ make + testSimulated2D.run true false @@ -2150,6 +1481,7 @@ make + testSimulated3D.run true false @@ -2179,22 +1511,6 @@ true true - - make - -j5 - testVector.run - true - true - true - - - make - -j5 - testMatrix.run - true - true - true - make -j5 @@ -2235,181 +1551,6 @@ false true - - make - -j2 - SimpleRotation.run - true - true - true - - - make - -j5 - CameraResectioning.run - true - true - true - - - make - -j5 - PlanarSLAMExample.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j5 - Pose2SLAMExample.run - true - true - true - - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - - - make - -j5 - UGM_small.run - true - true - true - - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - OdometryExample.run - true - true - true - - - make - -j5 - RangeISAMExample_plaza2.run - true - true - true - - - make - -j5 - SelfCalibrationExample.run - true - true - true - - - make - -j5 - SFMExample.run - true - true - true - - - make - -j5 - VisualISAMExample.run - true - true - true - - - make - -j5 - VisualISAM2Example.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graphviz.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graph.run - true - true - true - - - make - -j5 - SFMExample_bal.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - tests/testGaussianISAM2 - true - false - true - make -j2 @@ -2627,7 +1768,6 @@ cpack - -G DEB true false @@ -2635,7 +1775,6 @@ cpack - -G RPM true false @@ -2643,7 +1782,6 @@ cpack - -G TGZ true false @@ -2651,7 +1789,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2825,6 +1962,898 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + clean all + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j6 -j8 + testAntiFactor.run + true + true + true + + + make + -j6 -j8 + testBetweenFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j2 + testVSLAMGraph + true + true + true + + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j5 + testMarginals.run + true + true + true + + + make + -j5 + testGaussianISAM2.run + true + true + true + + + make + -j5 + testSymbolicFactorGraphB.run + true + true + true + + + make + -j2 + timeSequentialOnDataset.run + true + true + true + + + make + -j5 + testGradientDescentOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + -j2 + testNonlinearISAM.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + + + make + -j5 + timing.tests + true + true + true + + + make + -j5 + testNonlinearFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + testGraph.run + true + false + true + + + make + 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 + testSummarization.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + SimpleRotation.run + true + true + true + + + make + -j5 + CameraResectioning.run + true + true + true + + + make + -j5 + PlanarSLAMExample.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + easyPoint2KalmanFilter.run + true + true + true + + + make + -j2 + elaboratePoint2KalmanFilter.run + true + true + true + + + make + -j5 + Pose2SLAMExample.run + true + true + true + + + make + -j2 + Pose2SLAMwSPCG_easy.run + true + true + true + + + make + -j5 + UGM_small.run + true + true + true + + + make + -j5 + LocalizationExample.run + true + true + true + + + make + -j5 + OdometryExample.run + true + true + true + + + make + -j5 + RangeISAMExample_plaza2.run + true + true + true + + + make + -j5 + SelfCalibrationExample.run + true + true + true + + + make + -j5 + SFMExample.run + true + true + true + + + make + -j5 + VisualISAMExample.run + true + true + true + + + make + -j5 + VisualISAM2Example.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graphviz.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graph.run + true + true + true + + + make + -j5 + SFMExample_bal.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + + tests/testGaussianISAM2 + true + false + true + make -j2 @@ -2921,45 +2950,6 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. - true - false - true - make -j5 diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index d9ab7d71c..f51197cf2 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -591,41 +591,17 @@ Matrix3 skewSymmetric(double wx, double wy, double wz) } /* ************************************************************************* */ -/** Numerical Recipes in C wrappers - * create Numerical Recipes in C structure - * pointers are subtracted by one to provide base 1 access - */ -/* ************************************************************************* */ -// FIXME: assumes row major, rather than column major -//double** createNRC(Matrix& A) { -// const size_t m=A.rows(); -// double** a = new double* [m]; -// for(size_t i = 0; i < m; i++) -// a[i] = &A(i,0)-1; -// return a; -//} - -/* ****************************************** - * - * Modified from Justin's codebase - * - * Idea came from other public domain code. Takes a S.P.D. matrix - * and computes the LL^t decomposition. returns L, which is lower - * triangular. Note this is the opposite convention from Matlab, - * which calculates Q'Q where Q is upper triangular. - * - * ******************************************/ Matrix LLt(const Matrix& A) { - Matrix L = zeros(A.rows(), A.rows()); - Eigen::LLT llt; - llt.compute(A); + Eigen::LLT llt(A); return llt.matrixL(); } +/* ************************************************************************* */ Matrix RtR(const Matrix &A) { - return LLt(A).transpose(); + Eigen::LLT llt(A); + return llt.matrixU(); } /* @@ -633,13 +609,10 @@ Matrix RtR(const Matrix &A) */ Matrix cholesky_inverse(const Matrix &A) { - // FIXME: replace with real algorithm - return A.inverse(); - -// Matrix L = LLt(A); -// Matrix inv(eye(A.rows())); -// inplace_solve (L, inv, BNU::lower_tag ()); -// return BNU::prod(trans(inv), inv); + Eigen::LLT llt(A); + Matrix inv = eye(A.rows()); + llt.matrixU().solveInPlace(inv); + return inv*inv.transpose(); } /* ************************************************************************* */ @@ -648,9 +621,9 @@ Matrix cholesky_inverse(const Matrix &A) // inv(B) * inv(B)' == A // inv(B' * B) == A Matrix inverse_square_root(const Matrix& A) { - Matrix R = RtR(A); + Eigen::LLT llt(A); Matrix inv = eye(A.rows()); - R.triangularView().solveInPlace(inv); + llt.matrixU().solveInPlace(inv); return inv.transpose(); } diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 9d84e5ab7..4e857b143 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1022,22 +1022,32 @@ TEST( matrix, inverse_square_root ) /* *********************************************************************** */ // M was generated as the covariance of a set of random numbers. L that // we are checking against was generated via chol(M)' on octave +namespace cholesky { +Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463, + 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, + 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, + 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, + -0.0021113, 0.0036415, 0.0909464); + +Matrix expected = (Matrix(5, 5) << + 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, + -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, + 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, + 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, + 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); +} TEST( matrix, LLt ) { - Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463, - 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, - 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, - 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363, - -0.0021113, 0.0036415, 0.0909464); + EQUALITY(cholesky::expected, LLt(cholesky::M)); +} +TEST( matrix, RtR ) +{ + EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M)); +} - Matrix expected = (Matrix(5, 5) << - 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, - -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, - 0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, - 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, - 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); - - EQUALITY(expected, LLt(M)); +TEST( matrix, cholesky_inverse ) +{ + EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M)); } /* ************************************************************************* */ From 26996059437fcdc831111700d58b27e490460e0a Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 28 May 2014 11:31:17 -0400 Subject: [PATCH 101/101] Fix return type mismatch --- gtsam_unstable/slam/SmartFactorBase.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index 265df5ecd..974a37706 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -136,12 +136,12 @@ public: } /** return the measurements */ - const Vector& measured() const { + const std::vector& measured() const { return measured_; } /** return the noise model */ - const SharedNoiseModel& noise() const { + const std::vector& noise() const { return noise_; }