From e5c3f4228a6a1e86ddbdbabaa23b99dc820c2146 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 6 Oct 2014 14:22:18 +0200 Subject: [PATCH 01/24] Some fixed size in UnaryExpression --- gtsam_unstable/nonlinear/Expression-inl.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 786ee2b21..ab58dbf4c 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -310,7 +310,8 @@ class UnaryExpression: public ExpressionNode { public: - typedef boost::function)> Function; + typedef Eigen::Matrix JacobianTA; + typedef boost::function)> Function; private: @@ -344,16 +345,16 @@ public: virtual Augmented forward(const Values& values) const { using boost::none; Augmented argument = this->expressionA_->forward(values); - Matrix dTdA; + JacobianTA dTdA; T t = function_(argument.value(), - argument.constant() ? none : boost::optional(dTdA)); + argument.constant() ? none : boost::optional(dTdA)); return Augmented(t, dTdA, argument.jacobians()); } /// Trace structure for reverse AD struct Trace: public JacobianTrace { boost::shared_ptr > trace1; - Matrix dTdA; + JacobianTA dTdA; /// Start the reverse AD process virtual void reverseAD(JacobianMap& jacobians) const { trace1->reverseAD(dTdA, jacobians); From f9695f058e1f7db07de3e736c5f60cfb6612ca9f Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 6 Oct 2014 14:22:49 +0200 Subject: [PATCH 02/24] Fixed size matrix in project_to_camera --- gtsam/base/Matrix.h | 4 ++++ gtsam/geometry/PinholeCamera.h | 8 +++----- gtsam/slam/EssentialMatrixFactor.h | 6 ++++-- 3 files changed, 11 insertions(+), 7 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 16884f4c1..689f36baa 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -41,6 +41,10 @@ typedef Eigen::Matrix3d Matrix3; typedef Eigen::Matrix4d Matrix4; typedef Eigen::Matrix Matrix6; +typedef Eigen::Matrix Matrix23; +typedef Eigen::Matrix Matrix25; +typedef Eigen::Matrix Matrix36; + // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 4f81750a5..baf450365 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -270,17 +270,15 @@ public: * @param Dpoint is the 2*3 Jacobian w.r.t. P */ inline static Point2 project_to_camera(const Point3& P, - boost::optional Dpoint = boost::none) { + boost::optional Dpoint = boost::none) { #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (P.z() <= 0) throw CheiralityException(); #endif double d = 1.0 / P.z(); const double u = P.x() * d, v = P.y() * d; - if (Dpoint) { - Dpoint->resize(2,3); + if (Dpoint) *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; - } return Point2(u, v); } @@ -356,7 +354,7 @@ public: Dpc_pose.block(0, 0, 3, 3) = Dpc_rot; // camera to normalized image coordinate - Matrix Dpn_pc; // 2*3 + Matrix23 Dpn_pc; // 2*3 const Point2 pn = project_to_camera(pc, Dpn_pc); // uncalibration diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 9e6f3f8ba..557565a6e 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -174,12 +174,14 @@ public: } else { // Calculate derivatives. TODO if slow: optimize with Mathematica - // 3*2 3*3 3*3 2*3 - Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2; + // 3*2 3*3 3*3 + Matrix D_1T2_dir, DdP2_rot, DP2_point; Point3 _1T2 = E.direction().point3(D_1T2_dir); Point3 d1T2 = d * _1T2; Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point); + + Matrix23 Dpn_dP2; pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); if (DE) { From e48b38ca21da7fd5ad75f1f8638705365abf9f54 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 6 Oct 2014 15:45:26 +0200 Subject: [PATCH 03/24] Fixing uncalibrate (does not yet compile) --- gtsam/base/Matrix.h | 2 ++ gtsam/geometry/Cal3_S2.cpp | 9 +++++++++ gtsam/geometry/Cal3_S2.h | 9 +++++++-- gtsam_unstable/nonlinear/Expression-inl.h | 2 ++ 4 files changed, 20 insertions(+), 2 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 689f36baa..44ff1703f 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -37,8 +37,10 @@ namespace gtsam { typedef Eigen::MatrixXd Matrix; typedef Eigen::Matrix MatrixRowMajor; +typedef Eigen::Matrix2d Matrix2; typedef Eigen::Matrix3d Matrix3; typedef Eigen::Matrix4d Matrix4; +typedef Eigen::Matrix Matrix5; typedef Eigen::Matrix Matrix6; typedef Eigen::Matrix Matrix23; diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index c82b36a83..133f1821c 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -86,6 +86,15 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); } +/* ************************************************************************* */ +Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, + boost::optional Dp) const { + const double x = p.x(), y = p.y(); + if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; + if (Dp) *Dp << fx_, s_, 0.0, fy_; + return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); +} + /* ************************************************************************* */ Point2 Cal3_S2::calibrate(const Point2& p) const { const double u = p.x(), v = p.y(); diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 03c6bff3f..c7996f5d2 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -36,6 +36,8 @@ private: double fx_, fy_, s_, u0_, v0_; public: + /// dimension of the variable - used to autodetect sizes + static const size_t dimension = 5; typedef boost::shared_ptr shared_ptr; ///< shared pointer to calibration object @@ -151,6 +153,9 @@ public: Point2 uncalibrate(const Point2& p, boost::optional Dcal = boost::none, boost::optional Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, boost::optional Dcal = + boost::none, boost::optional Dp = boost::none) const; + /** * convert image coordinates uv to intrinsic coordinates xy * @param p point in image coordinates @@ -181,12 +186,12 @@ public: /// return DOF, dimensionality of tangent space inline size_t dim() const { - return 5; + return dimension; } /// return DOF, dimensionality of tangent space static size_t Dim() { - return 5; + return dimension; } /// Given 5-dim tangent vector, create new calibration diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index ab58dbf4c..fb0af0d54 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -383,6 +383,8 @@ class BinaryExpression: public ExpressionNode { public: + typedef Eigen::Matrix JacobianTA1; + typedef Eigen::Matrix JacobianTA2; typedef boost::function< T(const A1&, const A2&, boost::optional, boost::optional)> Function; From ea40de6758fa8c1d8096a402c5f4539b43b885ef Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 6 Oct 2014 21:37:05 +0200 Subject: [PATCH 04/24] Fixed Jacobian versions --- gtsam/geometry/Cal3_S2.cpp | 6 ++++++ gtsam/geometry/Cal3_S2.h | 10 ++++++---- gtsam/geometry/PinholeCamera.h | 2 +- gtsam/geometry/Pose2.cpp | 21 +++++++++++++++++++++ gtsam/geometry/Pose2.h | 12 ++++++++++-- gtsam/geometry/Pose3.cpp | 28 ++++++++++++++++++++++++++++ gtsam/geometry/Pose3.h | 7 ++++++- gtsam/geometry/Rot3.h | 11 +++++++++-- gtsam/geometry/Rot3M.cpp | 13 +++++++++++++ gtsam/geometry/Rot3Q.cpp | 13 +++++++++++++ 10 files changed, 113 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 133f1821c..aece1ded1 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -95,6 +95,12 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); } +/* ************************************************************************* */ +Point2 Cal3_S2::uncalibrate(const Point2& p) const { + const double x = p.x(), y = p.y(); + return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); +} + /* ************************************************************************* */ Point2 Cal3_S2::calibrate(const Point2& p) const { const double u = p.x(), v = p.y(); diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index c7996f5d2..45ef782d7 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -150,11 +150,13 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal = - boost::none, boost::optional Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, boost::optional Dcal, + boost::optional Dp) const; - Point2 uncalibrate(const Point2& p, boost::optional Dcal = - boost::none, boost::optional Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, boost::optional Dcal, + boost::optional Dp) const; + + Point2 uncalibrate(const Point2& p) const; /** * convert image coordinates uv to intrinsic coordinates xy diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index baf450365..095da4daa 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -325,7 +325,7 @@ public: } return pi; } else - return K_.uncalibrate(pn, Dcal); + return K_.uncalibrate(pn, Dcal, boost::none); } /** project a point at infinity from world coordinate to the image diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 85307e322..99c81b488 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -123,6 +123,27 @@ Pose2 Pose2::inverse(boost::optional H1) const { return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } +/* ************************************************************************* */ +// see doc/math.lyx, SE(2) section +Point2 Pose2::transform_to(const Point2& point) const { + Point2 d = point - t_; + return r_.unrotate(d); +} + +/* ************************************************************************* */ +// see doc/math.lyx, SE(2) section +Point2 Pose2::transform_to(const Point2& point, + boost::optional H1, boost::optional H2) const { + Point2 d = point - t_; + Point2 q = r_.unrotate(d); + if (!H1 && !H2) return q; + if (H1) *H1 << + -1.0, 0.0, q.y(), + 0.0, -1.0, -q.x(); + if (H2) *H2 = r_.transpose(); + return q; +} + /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_to(const Point2& point, diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 26244877b..91b131bcb 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -182,10 +182,18 @@ public: /// @name Group Action on Point2 /// @{ + /** Return point coordinates in pose coordinate frame */ + Point2 transform_to(const Point2& point) const; + /** Return point coordinates in pose coordinate frame */ Point2 transform_to(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + boost::optional H1, + boost::optional H2) const; + + /** Return point coordinates in pose coordinate frame */ + Point2 transform_to(const Point2& point, + boost::optional H1, + boost::optional H2) const; /** Return point coordinates in global frame */ Point2 transform_from(const Point2& point, diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index ea04c1d44..f82c8e588 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -253,6 +253,34 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, return R_ * p + t_; } +/* ************************************************************************* */ +Point3 Pose3::transform_to(const Point3& p) const { + // Only get transpose once, to avoid multiple allocations, + // as well as multiple conversions in the Quaternion case + return R_.unrotate(p - t_); +} + +/* ************************************************************************* */ +Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, + boost::optional Dpoint) const { + // Only get transpose once, to avoid multiple allocations, + // as well as multiple conversions in the Quaternion case + Matrix3 Rt(R_.transpose()); + const Point3 q(Rt*(p - t_).vector()); + if (Dpose) { + double wx = q.x(); + double wy = q.y(); + double wz = q.z(); + (*Dpose) << + 0.0, -wz, +wy,-1.0, 0.0, 0.0, + +wz, 0.0, -wx, 0.0,-1.0, 0.0, + -wy, +wx, 0.0, 0.0, 0.0,-1.0; + } + if (Dpoint) + *Dpoint = Rt; + return q; +} + /* ************************************************************************* */ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 825389243..55cda05a8 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -250,8 +250,13 @@ public: * @param Dpoint optional 3*3 Jacobian wrpt point * @return point in Pose coordinates */ + Point3 transform_to(const Point3& p) const; + Point3 transform_to(const Point3& p, - boost::optional Dpose=boost::none, boost::optional Dpoint=boost::none) const; + boost::optional Dpose, boost::optional Dpoint) const; + + Point3 transform_to(const Point3& p, + boost::optional Dpose, boost::optional Dpoint) const; /// @} /// @name Standard Interface diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index c8aeae51b..fa9787076 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -219,8 +219,15 @@ namespace gtsam { Rot3 inverse(boost::optional H1=boost::none) const; /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + Rot3 compose(const Rot3& R2) const; + + /// Compose two rotations i.e., R= (*this) * R2 + Rot3 compose(const Rot3& R2, boost::optional H1, + boost::optional H2) const; + + /// Compose two rotations i.e., R= (*this) * R2 + Rot3 compose(const Rot3& R2, boost::optional H1, + boost::optional H2) const; /** compose two rotations */ Rot3 operator*(const Rot3& R2) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 118d8546e..7db3acaa2 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -143,6 +143,19 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) { -swy + C02, swx + C12, c + C22); } +/* ************************************************************************* */ +Rot3 Rot3::compose (const Rot3& R2) const { + return *this * R2; +} + +/* ************************************************************************* */ +Rot3 Rot3::compose (const Rot3& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return *this * R2; +} + /* ************************************************************************* */ Rot3 Rot3::compose (const Rot3& R2, boost::optional H1, boost::optional H2) const { diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index c5990153a..dd0d39ffa 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -82,6 +82,19 @@ namespace gtsam { Rot3 Rot3::rodriguez(const Vector& w, double theta) { return Quaternion(Eigen::AngleAxisd(theta, w)); } + /* ************************************************************************* */ + Rot3 Rot3::compose(const Rot3& R2) const { + return Rot3(quaternion_ * R2.quaternion_); + } + + /* ************************************************************************* */ + Rot3 Rot3::compose(const Rot3& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return Rot3(quaternion_ * R2.quaternion_); + } + /* ************************************************************************* */ Rot3 Rot3::compose(const Rot3& R2, boost::optional H1, boost::optional H2) const { From a38ac5a107750f40d641f7686ecc306a2ff4946e Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 00:29:43 +0200 Subject: [PATCH 05/24] More fixed-size definitions --- gtsam/base/Matrix.h | 12 ++++++++++++ gtsam/base/Vector.h | 5 +++++ 2 files changed, 17 insertions(+) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 44ff1703f..f3d5be9e8 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -44,8 +44,20 @@ typedef Eigen::Matrix Matrix5; typedef Eigen::Matrix Matrix6; typedef Eigen::Matrix Matrix23; +typedef Eigen::Matrix Matrix24; typedef Eigen::Matrix Matrix25; +typedef Eigen::Matrix Matrix26; +typedef Eigen::Matrix Matrix27; +typedef Eigen::Matrix Matrix28; +typedef Eigen::Matrix Matrix29; + +typedef Eigen::Matrix Matrix32; +typedef Eigen::Matrix Matrix34; +typedef Eigen::Matrix Matrix35; typedef Eigen::Matrix Matrix36; +typedef Eigen::Matrix Matrix37; +typedef Eigen::Matrix Matrix38; +typedef Eigen::Matrix Matrix39; // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index b35afccdb..bf7d1733a 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -36,7 +36,12 @@ typedef Eigen::VectorXd Vector; // Commonly used fixed size vectors typedef Eigen::Vector2d Vector2; typedef Eigen::Vector3d Vector3; +typedef Eigen::Matrix Vector4; +typedef Eigen::Matrix Vector5; typedef Eigen::Matrix Vector6; +typedef Eigen::Matrix Vector7; +typedef Eigen::Matrix Vector8; +typedef Eigen::Matrix Vector9; typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; From 885a9235a639fb1cfa9bf4fe61f4e52039d6eb39 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 00:30:24 +0200 Subject: [PATCH 06/24] Definition now in Matrix.h --- gtsam/slam/ImplicitSchurFactor.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/slam/ImplicitSchurFactor.h b/gtsam/slam/ImplicitSchurFactor.h index c0f339b30..e579d38a1 100644 --- a/gtsam/slam/ImplicitSchurFactor.h +++ b/gtsam/slam/ImplicitSchurFactor.h @@ -29,7 +29,6 @@ public: protected: typedef Eigen::Matrix Matrix2D; ///< type of an F block - typedef Eigen::Matrix Matrix23; typedef Eigen::Matrix MatrixDD; ///< camera hessian typedef std::pair KeyMatrix2D; ///< named F block @@ -203,7 +202,7 @@ public: // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( // static const Eigen::Matrix I2 = eye(2); - // Eigen::Matrix Q = // + // Matrix2 Q = // // I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); // blocks[j] = Fj.transpose() * Q * Fj; } From 399bf7c9937ee27a9de5df10e9c7be86ef6bc0f0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 00:30:42 +0200 Subject: [PATCH 07/24] dimension --- gtsam/geometry/Cal3Bundler.h | 2 ++ gtsam/geometry/Cal3DS2.h | 3 +++ gtsam/geometry/Cal3Unified.h | 5 +++-- 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index fff9a6e6d..e508710cd 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -36,6 +36,8 @@ private: double u0_, v0_; ///< image center, not a parameter to be optimized but a constant public: + /// dimension of the variable - used to autodetect sizes + static const size_t dimension = 3; /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 51fe958d6..82bfa4c5f 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -46,6 +46,9 @@ protected: double p1_, p2_ ; // tangential distortion public: + /// dimension of the variable - used to autodetect sizes + static const size_t dimension = 9; + Matrix K() const ; Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); } Vector vector() const ; diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 58e024c27..eacbf7053 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -50,8 +50,9 @@ private: double xi_; // mirror parameter public: - //Matrix K() const ; - //Eigen::Vector4d k() const { return Base::k(); } + /// dimension of the variable - used to autodetect sizes + static const size_t dimension = 10; + Vector vector() const ; /// @name Standard Constructors From e28953931234026a0f1ecdeb199d3250af84524f Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 00:31:13 +0200 Subject: [PATCH 08/24] New matrix definitions --- gtsam/geometry/Cal3DS2.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index c75eff022..fe2acaf29 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -53,23 +53,23 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { } /* ************************************************************************* */ -static Eigen::Matrix D2dcalibration(double x, double y, double xx, +static Matrix29 D2dcalibration(double x, double y, double xx, double yy, double xy, double rr, double r4, double pnx, double pny, - const Eigen::Matrix& DK) { - Eigen::Matrix DR1; + const Matrix2& DK) { + Matrix25 DR1; DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; - Eigen::Matrix DR2; + Matrix24 DR2; DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // y * rr, y * r4, rr + 2 * yy, 2 * xy; - Eigen::Matrix D; + Matrix29 D; D << DR1, DK * DR2; return D; } /* ************************************************************************* */ -static Eigen::Matrix D2dintrinsic(double x, double y, double rr, +static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1, double k2, double p1, double p2, - const Eigen::Matrix& DK) { + const Matrix2& DK) { const double drdx = 2. * x; const double drdy = 2. * y; const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; @@ -82,7 +82,7 @@ static Eigen::Matrix D2dintrinsic(double x, double y, double rr, const double dDydx = 2. * p2 * y + p1 * drdx; const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); - Eigen::Matrix DR; + Matrix2 DR; DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // y * dgdx + dDydx, g + y * dgdy + dDydy; @@ -110,7 +110,7 @@ Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional H1, const double pnx = g * x + dx; const double pny = g * y + dy; - Eigen::Matrix DK; + Matrix2 DK; if (H1 || H2) DK << fx_, s_, 0.0, fy_; // Derivative for calibration @@ -161,7 +161,7 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { const double rr = xx + yy; const double r4 = rr * rr; const double g = (1 + k1_ * rr + k2_ * r4); - Eigen::Matrix DK; + Matrix2 DK; DK << fx_, s_, 0.0, fy_; return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); } @@ -176,7 +176,7 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const { const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); const double pnx = g * x + dx; const double pny = g * y + dy; - Eigen::Matrix DK; + Matrix2 DK; DK << fx_, s_, 0.0, fy_; return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); } From be3d39b3955dc15ee4331614e7c2a42b66745424 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 00:31:48 +0200 Subject: [PATCH 09/24] Documentation --- gtsam/geometry/Cal3_S2.h | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 45ef782d7..4e17c64f4 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -146,6 +146,23 @@ public: /** * convert intrinsic coordinates xy to image coordinates uv * @param p point in intrinsic coordinates + * @return point in image coordinates + */ + Point2 uncalibrate(const Point2& p) const; + + /** + * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves + * @param p point in intrinsic coordinates + * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates + */ + Point2 uncalibrate(const Point2& p, boost::optional Dcal, + boost::optional Dp) const; + + /** + * convert intrinsic coordinates xy to image coordinates uv, dynamic derivaitves + * @param p point in intrinsic coordinates * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates @@ -153,11 +170,6 @@ public: Point2 uncalibrate(const Point2& p, boost::optional Dcal, boost::optional Dp) const; - Point2 uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const; - - Point2 uncalibrate(const Point2& p) const; - /** * convert image coordinates uv to intrinsic coordinates xy * @param p point in image coordinates From 0a6fe0f0a8aca2c665b3cb93f37997358ca8b527 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 00:44:40 +0200 Subject: [PATCH 10/24] No more default argument --- gtsam/geometry/TriangulationFactor.h | 4 ++-- gtsam/geometry/tests/testSimpleCamera.cpp | 2 +- gtsam/slam/ProjectionFactor.h | 6 +++--- gtsam_unstable/geometry/InvDepthCamera3.h | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/TriangulationFactor.h b/gtsam/geometry/TriangulationFactor.h index 24b7918e3..fc8d546d3 100644 --- a/gtsam/geometry/TriangulationFactor.h +++ b/gtsam/geometry/TriangulationFactor.h @@ -115,7 +115,7 @@ public: Vector evaluateError(const Point3& point, boost::optional H2 = boost::none) const { try { - Point2 error(camera_.project(point, boost::none, H2) - measured_); + Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_); return error.vector(); } catch (CheiralityException& e) { if (H2) @@ -155,7 +155,7 @@ public: // Would be even better if we could pass blocks to project const Point3& point = x.at(key()); - b = -(camera_.project(point, boost::none, A) - measured_).vector(); + b = -(camera_.project(point, boost::none, A, boost::none) - measured_).vector(); if (noiseModel_) this->noiseModel_->WhitenSystem(A, b); diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 9ced28dca..366d09d49 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -125,7 +125,7 @@ static Point2 project2(const Pose3& pose, const Point3& point) { TEST( SimpleCamera, Dproject_point_pose) { Matrix Dpose, Dpoint; - Point2 result = camera.project(point1, Dpose, Dpoint); + Point2 result = camera.project(point1, Dpose, Dpoint, boost::none); Matrix numerical_pose = numericalDerivative21(project2, pose1, point1); Matrix numerical_point = numericalDerivative22(project2, pose1, point1); CHECK(assert_equal(result, Point2(-100, 100) )); diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 3e093c7c4..db37e6b8d 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -132,17 +132,17 @@ namespace gtsam { if(H1) { gtsam::Matrix H0; PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); - Point2 reprojectionError(camera.project(point, H1, H2) - measured_); + Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); *H1 = *H1 * H0; return reprojectionError.vector(); } else { PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); - Point2 reprojectionError(camera.project(point, H1, H2) - measured_); + Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); return reprojectionError.vector(); } } else { PinholeCamera camera(pose, *K_); - Point2 reprojectionError(camera.project(point, H1, H2) - measured_); + Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); return reprojectionError.vector(); } } catch( CheiralityException& e) { diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 4eb7992a2..30f17fb7a 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -104,7 +104,7 @@ public: } else { gtsam::Matrix J2; - gtsam::Point2 uv= camera.project(landmark,H1, J2); + gtsam::Point2 uv= camera.project(landmark,H1, J2, boost::none); if (H1) { *H1 = (*H1) * gtsam::eye(6); } From 467c94a01a3d3aca44ce6c83c24f99cf2c26dd5b Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 01:01:00 +0200 Subject: [PATCH 11/24] Fixed Jacobian version (copy/paste!) --- gtsam/geometry/PinholeCamera.h | 59 ++++++++++++++++++++++++++++++++-- 1 file changed, 56 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 095da4daa..b7e9df31b 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -289,6 +289,22 @@ public: return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); } + /** project a point from world coordinate to the image + * @param pw is a point in world coordinates + */ + inline Point2 project(const Point3& pw) const { + + // Transform to camera coordinates and check cheirality + const Point3 pc = pose_.transform_to(pw); + + // Project to normalized image coordinates + const Point2 pn = project_to_camera(pc); + + return K_.uncalibrate(pn); + } + + typedef Eigen::Matrix Matrix2K; + /** project a point from world coordinate to the image * @param pw is a point in world coordinates * @param Dpose is the Jacobian w.r.t. pose3 @@ -297,9 +313,46 @@ public: */ inline Point2 project( const Point3& pw, // - boost::optional Dpose = boost::none, - boost::optional Dpoint = boost::none, - boost::optional Dcal = boost::none) const { + boost::optional Dpose, + boost::optional Dpoint, + boost::optional Dcal) const { + + // Transform to camera coordinates and check cheirality + const Point3 pc = pose_.transform_to(pw); + + // Project to normalized image coordinates + const Point2 pn = project_to_camera(pc); + + if (Dpose || Dpoint) { + const double z = pc.z(), d = 1.0 / z; + + // uncalibration + Matrix2 Dpi_pn; + const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); + + // chain the Jacobian matrices + if (Dpose) { + calculateDpose(pn, d, Dpi_pn, *Dpose); + } + if (Dpoint) { + calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); + } + return pi; + } else + return K_.uncalibrate(pn, Dcal, boost::none); + } + + /** project a point from world coordinate to the image + * @param pw is a point in world coordinates + * @param Dpose is the Jacobian w.r.t. pose3 + * @param Dpoint is the Jacobian w.r.t. point3 + * @param Dcal is the Jacobian w.r.t. calibration + */ + inline Point2 project( + const Point3& pw, // + boost::optional Dpose, + boost::optional Dpoint, + boost::optional Dcal) const { // Transform to camera coordinates and check cheirality const Point3 pc = pose_.transform_to(pw); From 921b79f446b1d4408ef28808ec490cd944243aa3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 01:01:16 +0200 Subject: [PATCH 12/24] No more default argument --- gtsam_unstable/slam/ProjectionFactorPPP.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index e7060dcd4..b69f852b4 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -127,13 +127,13 @@ namespace gtsam { if(H1 || H2 || H3) { gtsam::Matrix H0, H02; PinholeCamera camera(pose.compose(transform, H0, H02), *K_); - Point2 reprojectionError(camera.project(point, H1, H3) - measured_); + Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_); *H2 = *H1 * H02; *H1 = *H1 * H0; return reprojectionError.vector(); } else { PinholeCamera camera(pose.compose(transform), *K_); - Point2 reprojectionError(camera.project(point, H1, H3) - measured_); + Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_); return reprojectionError.vector(); } } catch( CheiralityException& e) { From 613cb0bb129750584f3a41707a9e3ef6ca64f5e3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 01:01:46 +0200 Subject: [PATCH 13/24] Binary functions now take fixed Jacobians --- gtsam_unstable/nonlinear/Expression-inl.h | 14 ++++++++------ gtsam_unstable/nonlinear/Expression.h | 17 +++++++++++------ .../nonlinear/tests/testExpression.cpp | 4 ++-- gtsam_unstable/timing/timeCameraExpression.cpp | 4 ++-- 4 files changed, 23 insertions(+), 16 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index fb0af0d54..5a16895d0 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -386,8 +386,8 @@ public: typedef Eigen::Matrix JacobianTA1; typedef Eigen::Matrix JacobianTA2; typedef boost::function< - T(const A1&, const A2&, boost::optional, - boost::optional)> Function; + T(const A1&, const A2&, boost::optional, + boost::optional)> Function; private: @@ -429,10 +429,11 @@ public: using boost::none; Augmented a1 = this->expressionA1_->forward(values); Augmented a2 = this->expressionA2_->forward(values); - Matrix dTdA1, dTdA2; + JacobianTA1 dTdA1; + JacobianTA2 dTdA2; T t = function_(a1.value(), a2.value(), - a1.constant() ? none : boost::optional(dTdA1), - a2.constant() ? none : boost::optional(dTdA2)); + a1.constant() ? none : boost::optional(dTdA1), + a2.constant() ? none : boost::optional(dTdA2)); return Augmented(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians()); } @@ -440,7 +441,8 @@ public: struct Trace: public JacobianTrace { boost::shared_ptr > trace1; boost::shared_ptr > trace2; - Matrix dTdA1, dTdA2; + JacobianTA1 dTdA1; + JacobianTA2 dTdA2; /// Start the reverse AD process virtual void reverseAD(JacobianMap& jacobians) const { trace1->reverseAD(dTdA1, jacobians); diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 18e6c35e1..1c16fab25 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -76,8 +76,10 @@ public: /// Construct a unary method expression template Expression(const Expression& expression1, - T (A1::*method)(const A2&, boost::optional, - boost::optional) const, const Expression& expression2) { + T (A1::*method)(const A2&, + boost::optional::JacobianTA1&>, + boost::optional::JacobianTA2&>) const, + const Expression& expression2) { root_.reset( new BinaryExpression(boost::bind(method, _1, _2, _3, _4), expression1, expression2)); @@ -94,9 +96,11 @@ public: /// Construct a ternary function expression template Expression(typename TernaryExpression::Function function, - const Expression& expression1, const Expression& expression2, const Expression& expression3) { + const Expression& expression1, const Expression& expression2, + const Expression& expression3) { root_.reset( - new TernaryExpression(function, expression1, expression2, expression3)); + new TernaryExpression(function, expression1, expression2, + expression3)); } /// Return keys that play in this expression @@ -132,8 +136,9 @@ public: template struct apply_compose { typedef T result_type; - T operator()(const T& x, const T& y, boost::optional H1, - boost::optional H2) const { + typedef Eigen::Matrix Jacobian; + T operator()(const T& x, const T& y, boost::optional H1, + boost::optional H2) const { return x.compose(y, H1, H2); } }; diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 51a7ecbc7..5532b0da3 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -32,8 +32,8 @@ using namespace gtsam; /* ************************************************************************* */ template -Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional Dcal, - boost::optional Dp) { +Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional Dcal, + boost::optional Dp) { return K.uncalibrate(p, Dcal, Dp); } diff --git a/gtsam_unstable/timing/timeCameraExpression.cpp b/gtsam_unstable/timing/timeCameraExpression.cpp index baa515029..4e4e31d18 100644 --- a/gtsam_unstable/timing/timeCameraExpression.cpp +++ b/gtsam_unstable/timing/timeCameraExpression.cpp @@ -47,9 +47,9 @@ void time(const NonlinearFactor& f, const Values& values) { boost::shared_ptr fixedK(new Cal3_S2()); Point2 myProject(const Pose3& pose, const Point3& point, - boost::optional H1, boost::optional H2) { + boost::optional H1, boost::optional H2) { PinholeCamera camera(pose, *fixedK); - return camera.project(point, H1, H2); + return camera.project(point, H1, H2, boost::none); } int main() { From 155f64e1bf95d06ba3b29117064aa34d019cfacb Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 01:16:10 +0200 Subject: [PATCH 14/24] No more default --- examples/CameraResectioning.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 2048a84c8..f1e1a0010 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -48,7 +48,7 @@ public: virtual Vector evaluateError(const Pose3& pose, boost::optional H = boost::none) const { SimpleCamera camera(pose, *K_); - Point2 reprojectionError(camera.project(P_, H) - p_); + Point2 reprojectionError(camera.project(P_, H, boost::none, boost::none) - p_); return reprojectionError.vector(); } }; From 8b37da54c9bee4833b421f6723247df2f3e92cce Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 01:20:57 +0200 Subject: [PATCH 15/24] between copy/paste :-( --- gtsam/geometry/Pose2.cpp | 56 ++++++++++++++++++++++++++++++++++++++++ gtsam/geometry/Pose2.h | 15 ++++++++--- 2 files changed, 68 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 99c81b488..90c3f5f8c 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -182,6 +182,62 @@ Point2 Pose2::transform_from(const Point2& p, return q + t_; } +/* ************************************************************************* */ +Pose2 Pose2::between(const Pose2& p2) const { + // get cosines and sines from rotation matrices + const Rot2& R1 = r_, R2 = p2.r(); + double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); + + // Assert that R1 and R2 are normalized + assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); + + // Calculate delta rotation = between(R1,R2) + double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; + Rot2 R(Rot2::atan2(s,c)); // normalizes + + // Calculate delta translation = unrotate(R1, dt); + Point2 dt = p2.t() - t_; + double x = dt.x(), y = dt.y(); + // t = R1' * (t2-t1) + Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); + + return Pose2(R,t); +} + +/* ************************************************************************* */ +Pose2 Pose2::between(const Pose2& p2, boost::optional H1, + boost::optional H2) const { + // get cosines and sines from rotation matrices + const Rot2& R1 = r_, R2 = p2.r(); + double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); + + // Assert that R1 and R2 are normalized + assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); + + // Calculate delta rotation = between(R1,R2) + double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; + Rot2 R(Rot2::atan2(s,c)); // normalizes + + // Calculate delta translation = unrotate(R1, dt); + Point2 dt = p2.t() - t_; + double x = dt.x(), y = dt.y(); + // t = R1' * (t2-t1) + Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); + + // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above + if (H1) { + double dt1 = -s2 * x + c2 * y; + double dt2 = -c2 * x - s2 * y; + *H1 << + -c, -s, dt1, + s, -c, dt2, + 0.0, 0.0,-1.0; + } + if (H2) *H2 = I3; + + return Pose2(R,t); +} + /* ************************************************************************* */ Pose2 Pose2::between(const Pose2& p2, boost::optional H1, boost::optional H2) const { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 91b131bcb..13773ddb4 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -123,10 +123,19 @@ public: /** * Return relative pose between p1 and p2, in p1 coordinate frame */ - Pose2 between(const Pose2& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + Pose2 between(const Pose2& p2) const; + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + */ + Pose2 between(const Pose2& p2, boost::optional H1, + boost::optional H2) const; + + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + */ + Pose2 between(const Pose2& p2, boost::optional H1, + boost::optional H2) const; /// @} /// @name Manifold From 83d77271d9f410d1f70795cb864ea7a7e3abf3f7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 13:04:04 +0200 Subject: [PATCH 16/24] Ternary now fixed --- gtsam_unstable/nonlinear/Expression-inl.h | 27 +- .../nonlinear/tests/testBADFactor.cpp | 305 +++++++++++------- gtsam_unstable/slam/expressions.h | 11 + .../timing/timeCameraExpression.cpp | 27 +- 4 files changed, 224 insertions(+), 146 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 5a16895d0..869bff2ea 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -310,7 +310,7 @@ class UnaryExpression: public ExpressionNode { public: - typedef Eigen::Matrix JacobianTA; + typedef Eigen::Matrix JacobianTA; typedef boost::function)> Function; private: @@ -383,8 +383,8 @@ class BinaryExpression: public ExpressionNode { public: - typedef Eigen::Matrix JacobianTA1; - typedef Eigen::Matrix JacobianTA2; + typedef Eigen::Matrix JacobianTA1; + typedef Eigen::Matrix JacobianTA2; typedef boost::function< T(const A1&, const A2&, boost::optional, boost::optional)> Function; @@ -476,9 +476,12 @@ class TernaryExpression: public ExpressionNode { public: + typedef Eigen::Matrix JacobianTA1; + typedef Eigen::Matrix JacobianTA2; + typedef Eigen::Matrix JacobianTA3; typedef boost::function< - T(const A1&, const A2&, const A3&, boost::optional, - boost::optional, boost::optional)> Function; + T(const A1&, const A2&, const A3&, boost::optional, + boost::optional, boost::optional)> Function; private: @@ -528,11 +531,13 @@ public: Augmented a1 = this->expressionA1_->forward(values); Augmented a2 = this->expressionA2_->forward(values); Augmented a3 = this->expressionA3_->forward(values); - Matrix dTdA1, dTdA2, dTdA3; + JacobianTA1 dTdA1; + JacobianTA2 dTdA2; + JacobianTA3 dTdA3; T t = function_(a1.value(), a2.value(), a3.value(), - a1.constant() ? none : boost::optional(dTdA1), - a2.constant() ? none : boost::optional(dTdA2), - a3.constant() ? none : boost::optional(dTdA3)); + a1.constant() ? none : boost::optional(dTdA1), + a2.constant() ? none : boost::optional(dTdA2), + a3.constant() ? none : boost::optional(dTdA3)); return Augmented(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians(), dTdA3, a3.jacobians()); } @@ -542,7 +547,9 @@ public: boost::shared_ptr > trace1; boost::shared_ptr > trace2; boost::shared_ptr > trace3; - Matrix dTdA1, dTdA2, dTdA3; + JacobianTA1 dTdA1; + JacobianTA2 dTdA2; + JacobianTA3 dTdA3; /// Start the reverse AD process virtual void reverseAD(JacobianMap& jacobians) const { trace1->reverseAD(dTdA1, jacobians); diff --git a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp index 4a5b1a76f..0eb806c98 100644 --- a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp @@ -58,162 +58,217 @@ TEST(BADFactor, test) { } /* ************************************************************************* */ - // Unary(Binary(Leaf,Leaf)) - TEST(BADFactor, test1) { +// Unary(Binary(Leaf,Leaf)) +TEST(BADFactor, test1) { - // Create some values - Values values; - values.insert(1, Pose3()); - values.insert(2, Point3(0, 0, 1)); + // Create some values + Values values; + values.insert(1, Pose3()); + values.insert(2, Point3(0, 0, 1)); - // Create old-style factor to create expected value and derivatives - GenericProjectionFactor old(measured, model, 1, 2, - boost::make_shared()); - double expected_error = old.error(values); - GaussianFactor::shared_ptr expected = old.linearize(values); + // Create old-style factor to create expected value and derivatives + GenericProjectionFactor old(measured, model, 1, 2, + boost::make_shared()); + double expected_error = old.error(values); + GaussianFactor::shared_ptr expected = old.linearize(values); - // Create leaves - Pose3_ x(1); - Point3_ p(2); + // Create leaves + Pose3_ x(1); + Point3_ p(2); - // Try concise version - BADFactor f2(model, measured, project(transform_to(x, p))); - EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); - EXPECT_LONGS_EQUAL(2, f2.dim()); - boost::shared_ptr gf2 = f2.linearize(values); - EXPECT( assert_equal(*expected, *gf2, 1e-9)); - } + // Try concise version + BADFactor f2(model, measured, project(transform_to(x, p))); + EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f2.dim()); + boost::shared_ptr gf2 = f2.linearize(values); + EXPECT( assert_equal(*expected, *gf2, 1e-9)); +} - /* ************************************************************************* */ - // Binary(Leaf,Unary(Binary(Leaf,Leaf))) - TEST(BADFactor, test2) { +/* ************************************************************************* */ +// Binary(Leaf,Unary(Binary(Leaf,Leaf))) +TEST(BADFactor, test2) { - // Create some values - Values values; - values.insert(1, Pose3()); - values.insert(2, Point3(0, 0, 1)); - values.insert(3, Cal3_S2()); + // Create some values + Values values; + values.insert(1, Pose3()); + values.insert(2, Point3(0, 0, 1)); + values.insert(3, Cal3_S2()); - // Create old-style factor to create expected value and derivatives - GeneralSFMFactor2 old(measured, model, 1, 2, 3); - double expected_error = old.error(values); - GaussianFactor::shared_ptr expected = old.linearize(values); + // Create old-style factor to create expected value and derivatives + GeneralSFMFactor2 old(measured, model, 1, 2, 3); + double expected_error = old.error(values); + GaussianFactor::shared_ptr expected = old.linearize(values); - // Create leaves - Pose3_ x(1); - Point3_ p(2); - Cal3_S2_ K(3); + // Create leaves + Pose3_ x(1); + Point3_ p(2); + Cal3_S2_ K(3); - // Create expression tree - Point3_ p_cam(x, &Pose3::transform_to, p); - Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); - Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); + // Create expression tree + Point3_ p_cam(x, &Pose3::transform_to, p); + Point2_ xy_hat(PinholeCamera::project_to_camera, p_cam); + Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); - // Create factor and check value, dimension, linearization - BADFactor f(model, measured, uv_hat); - EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); - EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf = f.linearize(values); - EXPECT( assert_equal(*expected, *gf, 1e-9)); + // Create factor and check value, dimension, linearization + BADFactor f(model, measured, uv_hat); + EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f.dim()); + boost::shared_ptr gf = f.linearize(values); + EXPECT( assert_equal(*expected, *gf, 1e-9)); - // Try concise version - BADFactor f2(model, measured, - uncalibrate(K, project(transform_to(x, p)))); - EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); - EXPECT_LONGS_EQUAL(2, f2.dim()); - boost::shared_ptr gf2 = f2.linearize(values); - EXPECT( assert_equal(*expected, *gf2, 1e-9)); - } + // Try concise version + BADFactor f2(model, measured, + uncalibrate(K, project(transform_to(x, p)))); + EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f2.dim()); + boost::shared_ptr gf2 = f2.linearize(values); + EXPECT( assert_equal(*expected, *gf2, 1e-9)); - /* ************************************************************************* */ + TernaryExpression::Function fff = project6; - TEST(BADFactor, compose1) { + // Try ternary version + BADFactor f3(model, measured, project3(x, p, K)); + EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); + EXPECT_LONGS_EQUAL(2, f3.dim()); + boost::shared_ptr gf3 = f3.linearize(values); + EXPECT( assert_equal(*expected, *gf3, 1e-9)); +} - // Create expression - Rot3_ R1(1), R2(2); - Rot3_ R3 = R1 * R2; +/* ************************************************************************* */ - // Create factor - BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); +TEST(BADFactor, compose1) { - // Create some values - Values values; - values.insert(1, Rot3()); - values.insert(2, Rot3()); + // Create expression + Rot3_ R1(1), R2(2); + Rot3_ R3 = R1 * R2; - // Check unwhitenedError - std::vector H(2); - Vector actual = f.unwhitenedError(values, H); - EXPECT( assert_equal(eye(3), H[0],1e-9)); - EXPECT( assert_equal(eye(3), H[1],1e-9)); + // Create factor + BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); - // Check linearization - JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); - } + // Create some values + Values values; + values.insert(1, Rot3()); + values.insert(2, Rot3()); - /* ************************************************************************* */ - // Test compose with arguments referring to the same rotation - TEST(BADFactor, compose2) { + // Check unwhitenedError + std::vector H(2); + Vector actual = f.unwhitenedError(values, H); + EXPECT( assert_equal(eye(3), H[0],1e-9)); + EXPECT( assert_equal(eye(3), H[1],1e-9)); - // Create expression - Rot3_ R1(1), R2(1); - Rot3_ R3 = R1 * R2; + // Check linearization + JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} - // Create factor - BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); +/* ************************************************************************* */ +// Test compose with arguments referring to the same rotation +TEST(BADFactor, compose2) { - // Create some values - Values values; - values.insert(1, Rot3()); + // Create expression + Rot3_ R1(1), R2(1); + Rot3_ R3 = R1 * R2; - // Check unwhitenedError - std::vector H(1); - Vector actual = f.unwhitenedError(values, H); - EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(2*eye(3), H[0],1e-9)); + // Create factor + BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); - // Check linearization - JacobianFactor expected(1, 2 * eye(3), zero(3)); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); - } + // Create some values + Values values; + values.insert(1, Rot3()); - /* ************************************************************************* */ - // Test compose with one arguments referring to a constant same rotation - TEST(BADFactor, compose3) { + // Check unwhitenedError + std::vector H(1); + Vector actual = f.unwhitenedError(values, H); + EXPECT_LONGS_EQUAL(1, H.size()); + EXPECT( assert_equal(2*eye(3), H[0],1e-9)); - // Create expression - Rot3_ R1(Rot3::identity()), R2(3); - Rot3_ R3 = R1 * R2; + // Check linearization + JacobianFactor expected(1, 2 * eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} - // Create factor - BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); +/* ************************************************************************* */ +// Test compose with one arguments referring to a constant same rotation +TEST(BADFactor, compose3) { - // Create some values - Values values; - values.insert(3, Rot3()); + // Create expression + Rot3_ R1(Rot3::identity()), R2(3); + Rot3_ R3 = R1 * R2; - // Check unwhitenedError - std::vector H(1); - Vector actual = f.unwhitenedError(values, H); - EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(eye(3), H[0],1e-9)); + // Create factor + BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); - // Check linearization - JacobianFactor expected(3, eye(3), zero(3)); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); - } + // Create some values + Values values; + values.insert(3, Rot3()); - /* ************************************************************************* */ + // Check unwhitenedError + std::vector H(1); + Vector actual = f.unwhitenedError(values, H); + EXPECT_LONGS_EQUAL(1, H.size()); + EXPECT( assert_equal(eye(3), H[0],1e-9)); + + // Check linearization + JacobianFactor expected(3, eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} + +/* ************************************************************************* */ +// Test compose with three arguments +Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, + boost::optional H1, boost::optional H2, + boost::optional H3) { + // return dummy derivatives (not correct, but that's ok for testing here) + if (H1) + *H1 = eye(3); + if (H2) + *H2 = eye(3); + if (H3) + *H3 = eye(3); + return R1 * (R2 * R3); +} + +TEST(BADFactor, composeTernary) { + + // Create expression + Rot3_ A(1), B(2), C(3); + Rot3_ ABC(composeThree, A, B, C); + + // Create factor + BADFactor f(noiseModel::Unit::Create(3), Rot3(), ABC); + + // Create some values + Values values; + values.insert(1, Rot3()); + values.insert(2, Rot3()); + values.insert(3, Rot3()); + + // Check unwhitenedError + std::vector H(3); + Vector actual = f.unwhitenedError(values, H); + EXPECT_LONGS_EQUAL(3, H.size()); + EXPECT( assert_equal(eye(3), H[0],1e-9)); + EXPECT( assert_equal(eye(3), H[1],1e-9)); + EXPECT( assert_equal(eye(3), H[2],1e-9)); + + // Check linearization + JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} + +/* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); diff --git a/gtsam_unstable/slam/expressions.h b/gtsam_unstable/slam/expressions.h index 1acde67d3..d9cbd5716 100644 --- a/gtsam_unstable/slam/expressions.h +++ b/gtsam_unstable/slam/expressions.h @@ -10,6 +10,7 @@ #include #include #include +#include namespace gtsam { @@ -48,6 +49,16 @@ Point2_ project(const Point3_& p_cam) { return Point2_(PinholeCamera::project_to_camera, p_cam); } +Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, + boost::optional Dpose, boost::optional Dpoint, + boost::optional Dcal) { + return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); +} + +Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) { + return Point2_(project6, x, p, K); +} + template Point2_ uncalibrate(const Expression& K, const Point2_& xy_hat) { return Point2_(K, &CAL::uncalibrate, xy_hat); diff --git a/gtsam_unstable/timing/timeCameraExpression.cpp b/gtsam_unstable/timing/timeCameraExpression.cpp index 4e4e31d18..3b5d85b72 100644 --- a/gtsam_unstable/timing/timeCameraExpression.cpp +++ b/gtsam_unstable/timing/timeCameraExpression.cpp @@ -26,7 +26,6 @@ #include #include #include // std::setprecision - using namespace std; using namespace gtsam; @@ -74,37 +73,43 @@ int main() { // Dedicated factor // Oct 3, 2014, Macbook Air // 4.2 musecs/call - GeneralSFMFactor2 oldFactor2(z, model, 1, 2, 3); - time(oldFactor2, values); + GeneralSFMFactor2 f1(z, model, 1, 2, 3); + time(f1, values); // BADFactor // Oct 3, 2014, Macbook Air // 20.3 musecs/call - BADFactor newFactor2(model, z, + BADFactor f2(model, z, uncalibrate(K, project(transform_to(x, p)))); - time(newFactor2, values); + time(f2, values); + + // BADFactor ternary + // Oct 3, 2014, Macbook Air + // 20.3 musecs/call + BADFactor f3(model, z, project3(x, p, K)); + time(f3, values); // CALIBRATED // Dedicated factor // Oct 3, 2014, Macbook Air // 3.4 musecs/call - GenericProjectionFactor oldFactor1(z, model, 1, 2, fixedK); - time(oldFactor1, values); + GenericProjectionFactor g1(z, model, 1, 2, fixedK); + time(g1, values); // BADFactor // Oct 3, 2014, Macbook Air // 16.0 musecs/call - BADFactor newFactor1(model, z, + BADFactor g2(model, z, uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p)))); - time(newFactor1, values); + time(g2, values); // BADFactor, optimized // Oct 3, 2014, Macbook Air // 9.0 musecs/call typedef PinholeCamera Camera; typedef Expression Camera_; - BADFactor newFactor3(model, z, Point2_(myProject, x, p)); - time(newFactor3, values); + BADFactor g3(model, z, Point2_(myProject, x, p)); + time(g3, values); return 0; } From e4392c0a3b7c839160ff1f02b3826b169330dde1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 16:11:55 +0200 Subject: [PATCH 17/24] JacobianTrace no longer templated --- gtsam_unstable/nonlinear/Expression-inl.h | 100 +++++++++--------- gtsam_unstable/nonlinear/Expression.h | 6 +- .../nonlinear/tests/testBADFactor.cpp | 2 +- .../nonlinear/tests/testExpression.cpp | 13 ++- 4 files changed, 61 insertions(+), 60 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 869bff2ea..2dcf5b43f 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { @@ -134,16 +135,23 @@ public: }; //----------------------------------------------------------------------------- -template struct JacobianTrace { - T t; - T value() const { - return t; - } virtual void reverseAD(JacobianMap& jacobians) const = 0; virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; +// template +// void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const { }; +typedef boost::shared_ptr TracePtr; + +//template +//struct TypedTrace { +// virtual void reverseAD(JacobianMap& jacobians) const = 0; +// virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; +//// template +//// void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const { +//}; + //----------------------------------------------------------------------------- /** * Expression node. The superclass for objects that do the heavy lifting @@ -175,8 +183,7 @@ public: virtual Augmented forward(const Values& values) const = 0; /// Construct an execution trace for reverse AD - virtual boost::shared_ptr > traceExecution( - const Values& values) const = 0; + virtual std::pair traceExecution(const Values& values) const = 0; }; //----------------------------------------------------------------------------- @@ -217,7 +224,7 @@ public: } /// Trace structure for reverse AD - struct Trace: public JacobianTrace { + struct Trace: public JacobianTrace { /// If the expression is just a constant, we do nothing virtual void reverseAD(JacobianMap& jacobians) const { } @@ -227,11 +234,9 @@ public: }; /// Construct an execution trace for reverse AD - virtual boost::shared_ptr > traceExecution( - const Values& values) const { + virtual std::pair traceExecution(const Values& values) const { boost::shared_ptr trace = boost::make_shared(); - trace->t = constant_; - return trace; + return std::make_pair(constant_, trace); } }; @@ -270,12 +275,11 @@ public: /// Return value and derivatives virtual Augmented forward(const Values& values) const { - T t = value(values); - return Augmented(t, key_); + return Augmented(values.at(key_), key_); } /// Trace structure for reverse AD - struct Trace: public JacobianTrace { + struct Trace: public JacobianTrace { Key key; /// If the expression is just a leaf, we just insert an identity matrix virtual void reverseAD(JacobianMap& jacobians) const { @@ -293,12 +297,10 @@ public: }; /// Construct an execution trace for reverse AD - virtual boost::shared_ptr > traceExecution( - const Values& values) const { + virtual std::pair traceExecution(const Values& values) const { boost::shared_ptr trace = boost::make_shared(); - trace->t = value(values); trace->key = key_; - return trace; + return std::make_pair(values.at(key_), trace); } }; @@ -352,26 +354,25 @@ public: } /// Trace structure for reverse AD - struct Trace: public JacobianTrace { - boost::shared_ptr > trace1; + struct Trace: public JacobianTrace { + TracePtr trace; JacobianTA dTdA; /// Start the reverse AD process virtual void reverseAD(JacobianMap& jacobians) const { - trace1->reverseAD(dTdA, jacobians); + trace->reverseAD(dTdA, jacobians); } /// Given df/dT, multiply in dT/dA and continue reverse AD process virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { - trace1->reverseAD(dFdT * dTdA, jacobians); + trace->reverseAD(dFdT * dTdA, jacobians); } }; /// Construct an execution trace for reverse AD - virtual boost::shared_ptr > traceExecution( - const Values& values) const { + virtual std::pair traceExecution(const Values& values) const { + A a; boost::shared_ptr trace = boost::make_shared(); - trace->trace1 = this->expressionA_->traceExecution(values); - trace->t = function_(trace->trace1->value(), trace->dTdA); - return trace; + boost::tie(a, trace->trace) = this->expressionA_->traceExecution(values); + return std::make_pair(function_(a, trace->dTdA),trace); } }; @@ -438,9 +439,8 @@ public: } /// Trace structure for reverse AD - struct Trace: public JacobianTrace { - boost::shared_ptr > trace1; - boost::shared_ptr > trace2; + struct Trace: public JacobianTrace { + TracePtr trace1, trace2; JacobianTA1 dTdA1; JacobianTA2 dTdA2; /// Start the reverse AD process @@ -456,14 +456,13 @@ public: }; /// Construct an execution trace for reverse AD - virtual boost::shared_ptr > traceExecution( - const Values& values) const { + virtual std::pair traceExecution(const Values& values) const { + A1 a1; + A2 a2; boost::shared_ptr trace = boost::make_shared(); - trace->trace1 = this->expressionA1_->traceExecution(values); - trace->trace2 = this->expressionA2_->traceExecution(values); - trace->t = function_(trace->trace1->value(), trace->trace2->value(), - trace->dTdA1, trace->dTdA2); - return trace; + boost::tie(a1, trace->trace1) = this->expressionA1_->traceExecution(values); + boost::tie(a2, trace->trace2) = this->expressionA2_->traceExecution(values); + return std::make_pair(function_(a1, a2, trace->dTdA1, trace->dTdA2), trace); } }; @@ -543,10 +542,10 @@ public: } /// Trace structure for reverse AD - struct Trace: public JacobianTrace { - boost::shared_ptr > trace1; - boost::shared_ptr > trace2; - boost::shared_ptr > trace3; + struct Trace: public JacobianTrace { + TracePtr trace1; + TracePtr trace2; + TracePtr trace3; JacobianTA1 dTdA1; JacobianTA2 dTdA2; JacobianTA3 dTdA3; @@ -565,15 +564,16 @@ public: }; /// Construct an execution trace for reverse AD - virtual boost::shared_ptr > traceExecution( - const Values& values) const { + virtual std::pair traceExecution(const Values& values) const { + A1 a1; + A2 a2; + A3 a3; boost::shared_ptr trace = boost::make_shared(); - trace->trace1 = this->expressionA1_->traceExecution(values); - trace->trace2 = this->expressionA2_->traceExecution(values); - trace->trace3 = this->expressionA3_->traceExecution(values); - trace->t = function_(trace->trace1->value(), trace->trace2->value(), - trace->trace3->value(), trace->dTdA1, trace->dTdA2, trace->dTdA3); - return trace; + boost::tie(a1, trace->trace1) = this->expressionA1_->traceExecution(values); + boost::tie(a2, trace->trace2) = this->expressionA2_->traceExecution(values); + boost::tie(a3, trace->trace3) = this->expressionA3_->traceExecution(values); + return std::make_pair( + function_(a1, a2, a3, trace->dTdA1, trace->dTdA2, trace->dTdA3), trace); } }; diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 1c16fab25..968a0f814 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -117,8 +117,10 @@ public: Augmented augmented(const Values& values) const { #define REVERSE_AD #ifdef REVERSE_AD - boost::shared_ptr > trace(root_->traceExecution(values)); - Augmented augmented(trace->value()); + T value; + TracePtr trace; + boost::tie(value,trace) = root_->traceExecution(values); + Augmented augmented(value); trace->reverseAD(augmented.jacobians()); return augmented; #else diff --git a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp index 0eb806c98..44a7eab3f 100644 --- a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp @@ -124,7 +124,7 @@ TEST(BADFactor, test2) { boost::shared_ptr gf2 = f2.linearize(values); EXPECT( assert_equal(*expected, *gf2, 1e-9)); - TernaryExpression::Function fff = project6; + TernaryExpression::Function fff = project6; // Try ternary version BADFactor f3(model, measured, project3(x, p, K)); diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 5532b0da3..188394e0a 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -32,12 +32,12 @@ using namespace gtsam; /* ************************************************************************* */ template -Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional Dcal, - boost::optional Dp) { +Point2 uncalibrate(const CAL& K, const Point2& p, + boost::optional Dcal, boost::optional Dp) { return K.uncalibrate(p, Dcal, Dp); } -static const Rot3 someR = Rot3::RzRyRx(1,2,3); +static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); /* ************************************************************************* */ @@ -55,7 +55,7 @@ TEST(Expression, constant) { TEST(Expression, leaf) { Expression R(100); Values values; - values.insert(100,someR); + values.insert(100, someR); Augmented a = R.augmented(values); EXPECT(assert_equal(someR, a.value())); JacobianMap expected; @@ -76,7 +76,6 @@ TEST(Expression, leaf) { // expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50)); // EXPECT(assert_equal(expected.at(67),a.jacobians().at(67))); //} - /* ************************************************************************* */ TEST(Expression, test) { @@ -149,8 +148,8 @@ TEST(Expression, compose3) { /* ************************************************************************* */ // Test with ternary function Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - boost::optional H1, boost::optional H2, - boost::optional H3) { + boost::optional H1, boost::optional H2, + boost::optional H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); From 982dc29d2fb47a626edd3ec400b04cc201721f43 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 18:10:53 +0200 Subject: [PATCH 18/24] Time ternary version as well --- gtsam_unstable/timing/timeOneCameraExpression.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/timing/timeOneCameraExpression.cpp b/gtsam_unstable/timing/timeOneCameraExpression.cpp index a76200812..822ccbb2b 100644 --- a/gtsam_unstable/timing/timeOneCameraExpression.cpp +++ b/gtsam_unstable/timing/timeOneCameraExpression.cpp @@ -58,9 +58,13 @@ int main() { // BADFactor // Oct 3, 2014, Macbook Air // 20.3 musecs/call - BADFactor newFactor2(model, z, - uncalibrate(K, project(transform_to(x, p)))); - time(newFactor2, values); +#define TERNARY +#ifdef TERNARY + BADFactor f(model, z, project3(x, p, K)); +#else + BADFactor f(model, z, uncalibrate(K, project(transform_to(x, p)))); +#endif + time(f, values); return 0; } From 3c1c9c6d125eb2415b929605a5356f43de3cd4eb Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 18:11:12 +0200 Subject: [PATCH 19/24] Switch to pointers - nice improvement --- gtsam_unstable/nonlinear/Expression-inl.h | 32 ++++++++++++++++------- gtsam_unstable/nonlinear/Expression.h | 1 + 2 files changed, 23 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 2dcf5b43f..c2f51ea96 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -136,13 +136,15 @@ public: //----------------------------------------------------------------------------- struct JacobianTrace { + virtual ~JacobianTrace() { + } virtual void reverseAD(JacobianMap& jacobians) const = 0; virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; // template // void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const { }; -typedef boost::shared_ptr TracePtr; +typedef JacobianTrace* TracePtr; //template //struct TypedTrace { @@ -235,7 +237,7 @@ public: /// Construct an execution trace for reverse AD virtual std::pair traceExecution(const Values& values) const { - boost::shared_ptr trace = boost::make_shared(); + Trace* trace = new Trace(); return std::make_pair(constant_, trace); } }; @@ -298,7 +300,7 @@ public: /// Construct an execution trace for reverse AD virtual std::pair traceExecution(const Values& values) const { - boost::shared_ptr trace = boost::make_shared(); + Trace* trace = new Trace(); trace->key = key_; return std::make_pair(values.at(key_), trace); } @@ -357,6 +359,9 @@ public: struct Trace: public JacobianTrace { TracePtr trace; JacobianTA dTdA; + virtual ~Trace() { + delete trace; + } /// Start the reverse AD process virtual void reverseAD(JacobianMap& jacobians) const { trace->reverseAD(dTdA, jacobians); @@ -370,9 +375,9 @@ public: /// Construct an execution trace for reverse AD virtual std::pair traceExecution(const Values& values) const { A a; - boost::shared_ptr trace = boost::make_shared(); + Trace* trace = new Trace(); boost::tie(a, trace->trace) = this->expressionA_->traceExecution(values); - return std::make_pair(function_(a, trace->dTdA),trace); + return std::make_pair(function_(a, trace->dTdA), trace); } }; @@ -443,6 +448,10 @@ public: TracePtr trace1, trace2; JacobianTA1 dTdA1; JacobianTA2 dTdA2; + virtual ~Trace() { + delete trace1; + delete trace2; + } /// Start the reverse AD process virtual void reverseAD(JacobianMap& jacobians) const { trace1->reverseAD(dTdA1, jacobians); @@ -459,7 +468,7 @@ public: virtual std::pair traceExecution(const Values& values) const { A1 a1; A2 a2; - boost::shared_ptr trace = boost::make_shared(); + Trace* trace = new Trace(); boost::tie(a1, trace->trace1) = this->expressionA1_->traceExecution(values); boost::tie(a2, trace->trace2) = this->expressionA2_->traceExecution(values); return std::make_pair(function_(a1, a2, trace->dTdA1, trace->dTdA2), trace); @@ -543,12 +552,15 @@ public: /// Trace structure for reverse AD struct Trace: public JacobianTrace { - TracePtr trace1; - TracePtr trace2; - TracePtr trace3; + TracePtr trace1, trace2, trace3; JacobianTA1 dTdA1; JacobianTA2 dTdA2; JacobianTA3 dTdA3; + virtual ~Trace() { + delete trace1; + delete trace2; + delete trace3; + } /// Start the reverse AD process virtual void reverseAD(JacobianMap& jacobians) const { trace1->reverseAD(dTdA1, jacobians); @@ -568,7 +580,7 @@ public: A1 a1; A2 a2; A3 a3; - boost::shared_ptr trace = boost::make_shared(); + Trace* trace = new Trace(); boost::tie(a1, trace->trace1) = this->expressionA1_->traceExecution(values); boost::tie(a2, trace->trace2) = this->expressionA2_->traceExecution(values); boost::tie(a3, trace->trace3) = this->expressionA3_->traceExecution(values); diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 968a0f814..cc7977a23 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -122,6 +122,7 @@ public: boost::tie(value,trace) = root_->traceExecution(values); Augmented augmented(value); trace->reverseAD(augmented.jacobians()); + delete trace; return augmented; #else return root_->forward(values); From b704b7b1a1e054e964b96901643076eab0f158a6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 19:34:45 +0200 Subject: [PATCH 20/24] Faster versions --- gtsam/geometry/Pose3.cpp | 12 ++---------- gtsam/geometry/Rot3.cpp | 26 +++++++++++++++++++++++--- gtsam/geometry/Rot3.h | 15 ++++++++++----- gtsam/geometry/Rot3M.cpp | 6 ++++++ gtsam/geometry/Rot3Q.cpp | 5 +++++ 5 files changed, 46 insertions(+), 18 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f82c8e588..b7a0c1714 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -255,8 +255,6 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, /* ************************************************************************* */ Point3 Pose3::transform_to(const Point3& p) const { - // Only get transpose once, to avoid multiple allocations, - // as well as multiple conversions in the Quaternion case return R_.unrotate(p - t_); } @@ -268,9 +266,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, Matrix3 Rt(R_.transpose()); const Point3 q(Rt*(p - t_).vector()); if (Dpose) { - double wx = q.x(); - double wy = q.y(); - double wz = q.z(); + const double wx = q.x(), wy = q.y(), wz = q.z(); (*Dpose) << 0.0, -wz, +wy,-1.0, 0.0, 0.0, +wz, 0.0, -wx, 0.0,-1.0, 0.0, @@ -284,14 +280,10 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, /* ************************************************************************* */ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { - // Only get transpose once, to avoid multiple allocations, - // as well as multiple conversions in the Quaternion case Matrix3 Rt(R_.transpose()); const Point3 q(Rt*(p - t_).vector()); if (Dpose) { - double wx = q.x(); - double wy = q.y(); - double wz = q.z(); + const double wx = q.x(), wy = q.y(), wz = q.z(); Dpose->resize(3, 6); (*Dpose) << 0.0, -wz, +wy,-1.0, 0.0, 0.0, diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 37aa78a78..daa8eeda1 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -97,13 +97,33 @@ Unit3 Rot3::operator*(const Unit3& p) const { return rotate(p); } +/* ************************************************************************* */ +// see doc/math.lyx, SO(3) section +Point3 Rot3::unrotate(const Point3& p, boost::optional H1, + boost::optional H2) const { + Matrix3 Rt(transpose()); + Point3 q(Rt * p.vector()); // q = Rt*p + const double wx = q.x(), wy = q.y(), wz = q.z(); + if (H1) + *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; + if (H2) + *H2 = Rt; + return q; +} + /* ************************************************************************* */ // see doc/math.lyx, SO(3) section Point3 Rot3::unrotate(const Point3& p, boost::optional H1, boost::optional H2) const { - Point3 q(transpose()*p.vector()); // q = Rt*p - if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); - if (H2) *H2 = transpose(); + Matrix3 Rt(transpose()); + Point3 q(Rt * p.vector()); // q = Rt*p + const double wx = q.x(), wy = q.y(), wz = q.z(); + if (H1) { + H1->resize(3,3); + *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; + } + if (H2) + *H2 = Rt; return q; } diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index fa9787076..115f288e3 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -335,11 +335,16 @@ namespace gtsam { /// rotate point from rotated coordinate frame to world = R*p Point3 operator*(const Point3& p) const; - /** - * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - */ - Point3 unrotate(const Point3& p, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ + Point3 unrotate(const Point3& p) const; + + /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ + Point3 unrotate(const Point3& p, boost::optional H1, + boost::optional H2) const; + + /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ + Point3 unrotate(const Point3& p, boost::optional H1, + boost::optional H2) const; /// @} /// @name Group Action on Unit3 diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 7db3acaa2..96ebcac08 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -314,6 +314,12 @@ Quaternion Rot3::toQuaternion() const { return Quaternion(rot_); } +/* ************************************************************************* */ +Point3 Rot3::unrotate(const Point3& p) const { + // Eigen expression + return Point3(rot_.transpose()*p.vector()); // q = Rt*p +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index dd0d39ffa..4344a623c 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -171,6 +171,11 @@ namespace gtsam { /* ************************************************************************* */ Quaternion Rot3::toQuaternion() const { return quaternion_; } + /* ************************************************************************* */ + Point3 Rot3::unrotate(const Point3& p) const { + return Point3(transpose()*p.vector()); // q = Rt*p + } + /* ************************************************************************* */ } // namespace gtsam From e1c9ae95cb0a8f3308630c5b60de51468f635c7d Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 19:35:06 +0200 Subject: [PATCH 21/24] Some fixed matrices --- gtsam/geometry/PinholeCamera.h | 24 +++++++++++------------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index b7e9df31b..86e6a9097 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -331,12 +331,10 @@ public: const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); // chain the Jacobian matrices - if (Dpose) { + if (Dpose) calculateDpose(pn, d, Dpi_pn, *Dpose); - } - if (Dpoint) { + if (Dpoint) calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - } return pi; } else return K_.uncalibrate(pn, Dcal, boost::none); @@ -442,12 +440,12 @@ public: const double z = pc.z(), d = 1.0 / z; // uncalibration - Matrix Dcal, Dpi_pn(2, 2); + Matrix Dcal, Dpi_pn(2,2); const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); if (Dcamera) { Dcamera->resize(2, this->dim()); - calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>()); + calculateDpose(pn, d, Dpi_pn.block<2,2>(0,0), Dcamera->leftCols<6>()); Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib } if (Dpoint) { @@ -569,16 +567,16 @@ private: * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html */ template - static void calculateDpose(const Point2& pn, double d, const Matrix& Dpi_pn, + static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn, Eigen::MatrixBase const & Dpose) { // optimized version of derivatives, see CalibratedCamera.nb const double u = pn.x(), v = pn.y(); double uv = u * v, uu = u * u, vv = v * v; - Eigen::Matrix Dpn_pose; + Matrix26 Dpn_pose; Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; assert(Dpose.rows()==2 && Dpose.cols()==6); const_cast&>(Dpose) = // - Dpi_pn.block<2, 2>(0, 0) * Dpn_pose; + Dpi_pn * Dpn_pose; } /** @@ -590,18 +588,18 @@ private: * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html */ template - static void calculateDpoint(const Point2& pn, double d, const Matrix& R, - const Matrix& Dpi_pn, Eigen::MatrixBase const & Dpoint) { + static void calculateDpoint(const Point2& pn, double d, const Matrix3& R, + const Matrix2& Dpi_pn, Eigen::MatrixBase const & Dpoint) { // optimized version of derivatives, see CalibratedCamera.nb const double u = pn.x(), v = pn.y(); - Eigen::Matrix Dpn_point; + Matrix23 Dpn_point; Dpn_point << // R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), // /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); Dpn_point *= d; assert(Dpoint.rows()==2 && Dpoint.cols()==3); const_cast&>(Dpoint) = // - Dpi_pn.block<2, 2>(0, 0) * Dpn_point; + Dpi_pn * Dpn_point; } /// @} From 84987aa35169d44c350daf4c1c756bb85fc63513 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 19:35:20 +0200 Subject: [PATCH 22/24] Deal with Rot3 changes --- gtsam/navigation/MagFactor.h | 4 ++-- gtsam_unstable/dynamics/SimpleHelicopter.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 44f543bc9..bcd33a095 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -53,7 +53,7 @@ public: static Point3 unrotate(const Rot2& R, const Point3& p, boost::optional HR = boost::none) { - Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR); + Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR, boost::none); if (HR) { // assign to temporary first to avoid error in Win-Debug mode Matrix H = HR->col(2); @@ -106,7 +106,7 @@ public: Vector evaluateError(const Rot3& nRb, boost::optional H = boost::none) const { // measured bM = nRbÕ * nM + b - Point3 hx = nRb.unrotate(nM_, H) + bias_; + Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_; return (hx - measured_).vector(); } }; diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index ebc430277..52dcea2db 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -125,7 +125,7 @@ public: Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(-h_*xik_1, muk_1, D_adjThxik1_muk1); Matrix D_gravityBody_gk; - Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk); + Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk, boost::none); Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext; From c4a92acde11c7a82092bd141dd0b8f3117764073 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 19:35:44 +0200 Subject: [PATCH 23/24] Avoid argument temporaries --- gtsam_unstable/nonlinear/Expression-inl.h | 46 +++++++++++------------ gtsam_unstable/nonlinear/Expression.h | 3 +- 2 files changed, 23 insertions(+), 26 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index c2f51ea96..06405579e 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -185,7 +185,7 @@ public: virtual Augmented forward(const Values& values) const = 0; /// Construct an execution trace for reverse AD - virtual std::pair traceExecution(const Values& values) const = 0; + virtual T traceExecution(const Values& values, TracePtr& p) const = 0; }; //----------------------------------------------------------------------------- @@ -236,9 +236,10 @@ public: }; /// Construct an execution trace for reverse AD - virtual std::pair traceExecution(const Values& values) const { + virtual T traceExecution(const Values& values, TracePtr& p) const { Trace* trace = new Trace(); - return std::make_pair(constant_, trace); + p = static_cast(trace); + return constant_; } }; @@ -299,10 +300,11 @@ public: }; /// Construct an execution trace for reverse AD - virtual std::pair traceExecution(const Values& values) const { + virtual T traceExecution(const Values& values, TracePtr& p) const { Trace* trace = new Trace(); + p = static_cast(trace); trace->key = key_; - return std::make_pair(values.at(key_), trace); + return values.at(key_); } }; @@ -373,11 +375,11 @@ public: }; /// Construct an execution trace for reverse AD - virtual std::pair traceExecution(const Values& values) const { - A a; + virtual T traceExecution(const Values& values, TracePtr& p) const { Trace* trace = new Trace(); - boost::tie(a, trace->trace) = this->expressionA_->traceExecution(values); - return std::make_pair(function_(a, trace->dTdA), trace); + p = static_cast(trace); + A a = this->expressionA_->traceExecution(values,trace->trace); + return function_(a, trace->dTdA); } }; @@ -465,13 +467,12 @@ public: }; /// Construct an execution trace for reverse AD - virtual std::pair traceExecution(const Values& values) const { - A1 a1; - A2 a2; + virtual T traceExecution(const Values& values, TracePtr& p) const { Trace* trace = new Trace(); - boost::tie(a1, trace->trace1) = this->expressionA1_->traceExecution(values); - boost::tie(a2, trace->trace2) = this->expressionA2_->traceExecution(values); - return std::make_pair(function_(a1, a2, trace->dTdA1, trace->dTdA2), trace); + p = static_cast(trace); + A1 a1 = this->expressionA1_->traceExecution(values,trace->trace1); + A2 a2 = this->expressionA2_->traceExecution(values,trace->trace2); + return function_(a1, a2, trace->dTdA1, trace->dTdA2); } }; @@ -576,16 +577,13 @@ public: }; /// Construct an execution trace for reverse AD - virtual std::pair traceExecution(const Values& values) const { - A1 a1; - A2 a2; - A3 a3; + virtual T traceExecution(const Values& values, TracePtr& p) const { Trace* trace = new Trace(); - boost::tie(a1, trace->trace1) = this->expressionA1_->traceExecution(values); - boost::tie(a2, trace->trace2) = this->expressionA2_->traceExecution(values); - boost::tie(a3, trace->trace3) = this->expressionA3_->traceExecution(values); - return std::make_pair( - function_(a1, a2, a3, trace->dTdA1, trace->dTdA2, trace->dTdA3), trace); + p = static_cast(trace); + A1 a1 = this->expressionA1_->traceExecution(values,trace->trace1); + A2 a2 = this->expressionA2_->traceExecution(values,trace->trace2); + A3 a3 = this->expressionA3_->traceExecution(values,trace->trace3); + return function_(a1, a2, a3, trace->dTdA1, trace->dTdA2, trace->dTdA3); } }; diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index cc7977a23..06265a9fb 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -117,9 +117,8 @@ public: Augmented augmented(const Values& values) const { #define REVERSE_AD #ifdef REVERSE_AD - T value; TracePtr trace; - boost::tie(value,trace) = root_->traceExecution(values); + T value = root_->traceExecution(values,trace); Augmented augmented(value); trace->reverseAD(augmented.jacobians()); delete trace; From d7022a21c7603f7c5af28d59beb0039fcbf56910 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 7 Oct 2014 20:11:19 +0200 Subject: [PATCH 24/24] More samples to average --- gtsam_unstable/timing/timeOneCameraExpression.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/timing/timeOneCameraExpression.cpp b/gtsam_unstable/timing/timeOneCameraExpression.cpp index 822ccbb2b..11bf65709 100644 --- a/gtsam_unstable/timing/timeOneCameraExpression.cpp +++ b/gtsam_unstable/timing/timeOneCameraExpression.cpp @@ -25,7 +25,7 @@ using namespace std; using namespace gtsam; -static const int n = 500000; +static const int n = 1000000; void time(const NonlinearFactor& f, const Values& values) { long timeLog = clock();