diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 07801df7a..c9a960a89 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -20,6 +20,8 @@ #pragma once #include // Configuration from CMake #include +#include +#include #ifndef OPTIONALJACOBIAN_NOBOOST #include @@ -96,6 +98,24 @@ public: usurp(dynamic->data()); } + /** + * @brief Constructor from an Eigen::Ref *value*. Will not usurp if dimension is wrong + * @note This is important so we don't overwrite someone else's memory! + */ + template + OptionalJacobian(Eigen::Ref dynamic_ref) : + map_(nullptr) { + if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) { + usurp(dynamic_ref.data()); + } else { + throw std::invalid_argument( + std::string("OptionalJacobian called with wrong dimensions or " + "storage order.\n" + "Expected: ") + + "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")"); + } + } + #ifndef OPTIONALJACOBIAN_NOBOOST /// Constructor with boost::none just makes empty diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index d8060cfcf..06c32526b 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -113,6 +113,18 @@ list circleCircleIntersection(Point2 c1, double r1, Point2 c2, return circleCircleIntersection(c1, c2, fh); } +Point2Pair means(const std::vector &abPointPairs) { + const size_t n = abPointPairs.size(); + if (n == 0) throw std::invalid_argument("Point2::mean input Point2Pair vector is empty"); + Point2 aSum(0, 0), bSum(0, 0); + for (const Point2Pair &abPair : abPointPairs) { + aSum += abPair.first; + bSum += abPair.second; + } + const double f = 1.0 / n; + return {aSum * f, bSum * f}; +} + /* ************************************************************************* */ ostream &operator<<(ostream &os, const gtsam::Point2Pair &p) { os << p.first << " <-> " << p.second; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index cdb9f4480..d8b6daca8 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -71,6 +71,9 @@ GTSAM_EXPORT boost::optional circleCircleIntersection(double R_d, double * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well. */ GTSAM_EXPORT std::list circleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh); + +/// Calculate the two means of a set of Point2 pairs +GTSAM_EXPORT Point2Pair means(const std::vector &abPointPairs); /** * @brief Intersect 2 circles diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 96bdce69b..b37674b92 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -365,7 +365,7 @@ boost::optional Pose2::Align(const Matrix& a, const Matrix& b) { "Pose2:Align expects 2*N matrices of equal shape."); } Point2Pairs ab_pairs; - for (size_t j=0; j < a.cols(); j++) { + for (Eigen::Index j = 0; j < a.cols(); j++) { ab_pairs.emplace_back(a.col(j), b.col(j)); } return Pose2::Align(ab_pairs); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 0d75113a2..466c5a42a 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -353,6 +353,10 @@ GTSAM_EXPORT boost::optional GTSAM_DEPRECATED align(const Point2Pairs& pairs); #endif +// Convenience typedef +using Pose2Pair = std::pair; +using Pose2Pairs = std::vector; + template <> struct traits : public internal::LieGroup {}; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index b88d812a4..2da51a625 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -473,7 +473,7 @@ boost::optional Pose3::Align(const Matrix& a, const Matrix& b) { "Pose3:Align expects 3*N matrices of equal shape."); } Point3Pairs abPointPairs; - for (size_t j=0; j < a.cols(); j++) { + for (Eigen::Index j = 0; j < a.cols(); j++) { abPointPairs.emplace_back(a.col(j), b.col(j)); } return Pose3::Align(abPointPairs); diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 283147e4c..9bf631e50 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -129,6 +129,19 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) { } } +/* ************************************************************************* */ +Rot2 Rot2::ClosestTo(const Matrix2& M) { + Eigen::JacobiSVD svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV); + const Matrix2& U = svd.matrixU(); + const Matrix2& V = svd.matrixV(); + const double det = (U * V.transpose()).determinant(); + Matrix2 M_prime = (U * Vector2(1, det).asDiagonal() * V.transpose()); + + double c = M_prime(0, 0); + double s = M_prime(1, 0); + return Rot2::fromCosSin(c, s); +} + /* ************************************************************************* */ } // gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index ec30c6657..2690ca248 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -14,6 +14,7 @@ * @brief 2D rotation * @date Dec 9, 2009 * @author Frank Dellaert + * @author John Lambert */ #pragma once @@ -209,6 +210,9 @@ namespace gtsam { /** return 2*2 transpose (inverse) rotation matrix */ Matrix2 transpose() const; + /** Find closest valid rotation matrix, given a 2x2 matrix */ + static Rot2 ClosestTo(const Matrix2& M); + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp new file mode 100644 index 000000000..4ed3351f8 --- /dev/null +++ b/gtsam/geometry/Similarity2.cpp @@ -0,0 +1,242 @@ +/* ---------------------------------------------------------------------------- + + * 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 Similarity2.cpp + * @brief Implementation of Similarity2 transform + * @author John Lambert, Varun Agrawal + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + +using std::vector; + +namespace internal { + +/// Subtract centroids from point pairs. +static Point2Pairs SubtractCentroids(const Point2Pairs& abPointPairs, + const Point2Pair& centroids) { + Point2Pairs d_abPointPairs; + for (const Point2Pair& abPair : abPointPairs) { + Point2 da = abPair.first - centroids.first; + Point2 db = abPair.second - centroids.second; + d_abPointPairs.emplace_back(da, db); + } + return d_abPointPairs; +} + +/// Form inner products x and y and calculate scale. +static double CalculateScale(const Point2Pairs& d_abPointPairs, + const Rot2& aRb) { + double x = 0, y = 0; + Point2 da, db; + + for (const Point2Pair& d_abPair : d_abPointPairs) { + std::tie(da, db) = d_abPair; + const Vector2 da_prime = aRb * db; + y += da.transpose() * da_prime; + x += da_prime.transpose() * da_prime; + } + const double s = y / x; + return s; +} + +/// Form outer product H. +static Matrix2 CalculateH(const Point2Pairs& d_abPointPairs) { + Matrix2 H = Z_2x2; + for (const Point2Pair& d_abPair : d_abPointPairs) { + H += d_abPair.first * d_abPair.second.transpose(); + } + return H; +} + +/** + * @brief This method estimates the similarity transform from differences point + * pairs, given a known or estimated rotation and point centroids. + * + * @param d_abPointPairs + * @param aRb + * @param centroids + * @return Similarity2 + */ +static Similarity2 Align(const Point2Pairs& d_abPointPairs, const Rot2& aRb, + const Point2Pair& centroids) { + const double s = CalculateScale(d_abPointPairs, aRb); + // dividing aTb by s is required because the registration cost function + // minimizes ||a - sRb - t||, whereas Sim(2) computes s(Rb + t) + const Point2 aTb = (centroids.first - s * (aRb * centroids.second)) / s; + return Similarity2(aRb, aTb, s); +} + +/** + * @brief This method estimates the similarity transform from point pairs, + * given a known or estimated rotation. + * Refer to: + * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf + * Chapter 3 + * + * @param abPointPairs + * @param aRb + * @return Similarity2 + */ +static Similarity2 AlignGivenR(const Point2Pairs& abPointPairs, + const Rot2& aRb) { + auto centroids = means(abPointPairs); + auto d_abPointPairs = internal::SubtractCentroids(abPointPairs, centroids); + return internal::Align(d_abPointPairs, aRb, centroids); +} +} // namespace internal + +Similarity2::Similarity2() : t_(0, 0), s_(1) {} + +Similarity2::Similarity2(double s) : t_(0, 0), s_(s) {} + +Similarity2::Similarity2(const Rot2& R, const Point2& t, double s) + : R_(R), t_(t), s_(s) {} + +Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s) + : R_(Rot2::ClosestTo(R)), t_(t), s_(s) {} + +Similarity2::Similarity2(const Matrix3& T) + : R_(Rot2::ClosestTo(T.topLeftCorner<2, 2>())), + t_(T.topRightCorner<2, 1>()), + s_(1.0 / T(2, 2)) {} + +bool Similarity2::equals(const Similarity2& other, double tol) const { + return R_.equals(other.R_, tol) && + traits::Equals(t_, other.t_, tol) && s_ < (other.s_ + tol) && + s_ > (other.s_ - tol); +} + +bool Similarity2::operator==(const Similarity2& other) const { + return R_.matrix() == other.R_.matrix() && t_ == other.t_ && s_ == other.s_; +} + +void Similarity2::print(const std::string& s) const { + std::cout << std::endl; + std::cout << s; + rotation().print("\nR:\n"); + std::cout << "t: " << translation().transpose() << " s: " << scale() + << std::endl; +} + +Similarity2 Similarity2::identity() { return Similarity2(); } + +Similarity2 Similarity2::operator*(const Similarity2& S) const { + return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_); +} + +Similarity2 Similarity2::inverse() const { + const Rot2 Rt = R_.inverse(); + const Point2 sRt = Rt * (-s_ * t_); + return Similarity2(Rt, sRt, 1.0 / s_); +} + +Point2 Similarity2::transformFrom(const Point2& p) const { + const Point2 q = R_ * p + t_; + return s_ * q; +} + +Pose2 Similarity2::transformFrom(const Pose2& T) const { + Rot2 R = R_.compose(T.rotation()); + Point2 t = Point2(s_ * (R_ * T.translation() + t_)); + return Pose2(R, t); +} + +Point2 Similarity2::operator*(const Point2& p) const { + return transformFrom(p); +} + +Similarity2 Similarity2::Align(const Point2Pairs& abPointPairs) { + // Refer to Chapter 3 of + // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf + if (abPointPairs.size() < 2) + throw std::runtime_error("input should have at least 2 pairs of points"); + auto centroids = means(abPointPairs); + auto d_abPointPairs = internal::SubtractCentroids(abPointPairs, centroids); + Matrix2 H = internal::CalculateH(d_abPointPairs); + // ClosestTo finds rotation matrix closest to H in Frobenius sense + Rot2 aRb = Rot2::ClosestTo(H); + return internal::Align(d_abPointPairs, aRb, centroids); +} + +Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) { + const size_t n = abPosePairs.size(); + if (n < 2) + throw std::runtime_error("input should have at least 2 pairs of poses"); + + // calculate rotation + vector rotations; + Point2Pairs abPointPairs; + rotations.reserve(n); + abPointPairs.reserve(n); + // Below denotes the pose of the i'th object/camera/etc + // in frame "a" or frame "b". + Pose2 aTi, bTi; + for (const Pose2Pair& abPair : abPosePairs) { + std::tie(aTi, bTi) = abPair; + const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse()); + rotations.emplace_back(aRb); + abPointPairs.emplace_back(aTi.translation(), bTi.translation()); + } + const Rot2 aRb_estimate = FindKarcherMean(rotations); + + return internal::AlignGivenR(abPointPairs, aRb_estimate); +} + +Vector4 Similarity2::Logmap(const Similarity2& S, // + OptionalJacobian<4, 4> Hm) { + const Vector2 u = S.t_; + const Vector1 w = Rot2::Logmap(S.R_); + const double s = log(S.s_); + Vector4 result; + result << u, w, s; + if (Hm) { + throw std::runtime_error("Similarity2::Logmap: derivative not implemented"); + } + return result; +} + +Similarity2 Similarity2::Expmap(const Vector4& v, // + OptionalJacobian<4, 4> Hm) { + const Vector2 t = v.head<2>(); + const Rot2 R = Rot2::Expmap(v.segment<1>(2)); + const double s = v[3]; + if (Hm) { + throw std::runtime_error("Similarity2::Expmap: derivative not implemented"); + } + return Similarity2(R, t, s); +} + +Matrix4 Similarity2::AdjointMap() const { + throw std::runtime_error("Similarity2::AdjointMap not implemented"); +} + +std::ostream& operator<<(std::ostream& os, const Similarity2& p) { + os << "[" << p.rotation().theta() << " " << p.translation().transpose() << " " + << p.scale() << "]\';"; + return os; +} + +Matrix3 Similarity2::matrix() const { + Matrix3 T; + T.topRows<2>() << R_.matrix(), t_; + T.bottomRows<1>() << 0, 0, 1.0 / s_; + return T; +} + +} // namespace gtsam diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h new file mode 100644 index 000000000..05f10d149 --- /dev/null +++ b/gtsam/geometry/Similarity2.h @@ -0,0 +1,200 @@ +/* ---------------------------------------------------------------------------- + + * 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 Similarity2.h + * @brief Implementation of Similarity2 transform + * @author John Lambert, Varun Agrawal + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +// Forward declarations +class Pose2; + +/** + * 2D similarity transform + */ +class GTSAM_EXPORT Similarity2 : public LieGroup { + /// @name Pose Concept + /// @{ + typedef Rot2 Rotation; + typedef Point2 Translation; + /// @} + + private: + Rot2 R_; + Point2 t_; + double s_; + + public: + /// @name Constructors + /// @{ + + /// Default constructor + Similarity2(); + + /// Construct pure scaling + Similarity2(double s); + + /// Construct from GTSAM types + Similarity2(const Rot2& R, const Point2& t, double s); + + /// Construct from Eigen types + Similarity2(const Matrix2& R, const Vector2& t, double s); + + /// Construct from matrix [R t; 0 s^-1] + Similarity2(const Matrix3& T); + + /// @} + /// @name Testable + /// @{ + + /// Compare with tolerance + bool equals(const Similarity2& sim, double tol) const; + + /// Exact equality + bool operator==(const Similarity2& other) const; + + /// Print with optional string + void print(const std::string& s) const; + + friend std::ostream& operator<<(std::ostream& os, const Similarity2& p); + + /// @} + /// @name Group + /// @{ + + /// Return an identity transform + static Similarity2 identity(); + + /// Composition + Similarity2 operator*(const Similarity2& S) const; + + /// Return the inverse + Similarity2 inverse() const; + + /// @} + /// @name Group action on Point2 + /// @{ + + /// Action on a point p is s*(R*p+t) + Point2 transformFrom(const Point2& p) const; + + /** + * Action on a pose T. + * |Rs ts| |R t| |Rs*R Rs*t+ts| + * |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim2 object. + * To retrieve a Pose2, we normalized the scale value into 1. + * |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)| + * | 0 1/s | = | 0 1 | + * + * This group action satisfies the compatibility condition. + * For more details, refer to: https://en.wikipedia.org/wiki/Group_action + */ + Pose2 transformFrom(const Pose2& T) const; + + /* syntactic sugar for transformFrom */ + Point2 operator*(const Point2& p) const; + + /** + * Create Similarity2 by aligning at least two point pairs + */ + static Similarity2 Align(const Point2Pairs& abPointPairs); + + /** + * Create the Similarity2 object that aligns at least two pose pairs. + * Each pair is of the form (aTi, bTi). + * Given a list of pairs in frame a, and a list of pairs in frame b, + Align() + * will compute the best-fit Similarity2 aSb transformation to align them. + * First, the rotation aRb will be computed as the average (Karcher mean) + of + * many estimates aRb (from each pair). Afterwards, the scale factor will + be computed + * using the algorithm described here: + * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf + */ + static Similarity2 Align(const std::vector& abPosePairs); + + /// @} + /// @name Lie Group + /// @{ + + /** + * Log map at the identity + * \f$ [t_x, t_y, \delta, \lambda] \f$ + */ + static Vector4 Logmap(const Similarity2& S, // + OptionalJacobian<4, 4> Hm = boost::none); + + /// Exponential map at the identity + static Similarity2 Expmap(const Vector4& v, // + OptionalJacobian<4, 4> Hm = boost::none); + + /// Chart at the origin + struct ChartAtOrigin { + static Similarity2 Retract(const Vector4& v, + ChartJacobian H = boost::none) { + return Similarity2::Expmap(v, H); + } + static Vector4 Local(const Similarity2& other, + ChartJacobian H = boost::none) { + return Similarity2::Logmap(other, H); + } + }; + + /// Project from one tangent space to another + Matrix4 AdjointMap() const; + + using LieGroup::inverse; + + /// @} + /// @name Standard interface + /// @{ + + /// Calculate 4*4 matrix group equivalent + Matrix3 matrix() const; + + /// Return a GTSAM rotation + Rot2 rotation() const { return R_; } + + /// Return a GTSAM translation + Point2 translation() const { return t_; } + + /// Return the scale + double scale() const { return s_; } + + /// Dimensionality of tangent space = 4 DOF - used to autodetect sizes + inline static size_t Dim() { return 4; } + + /// Dimensionality of tangent space = 4 DOF + inline size_t dim() const { return 4; } + + /// @} +}; + +template <> +struct traits : public internal::LieGroup {}; + +template <> +struct traits : public internal::LieGroup {}; + +} // namespace gtsam diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index e8d6e7510..7fde974c5 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -26,7 +26,7 @@ namespace gtsam { using std::vector; -namespace { +namespace internal { /// Subtract centroids from point pairs. static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs, const Point3Pair ¢roids) { @@ -81,10 +81,10 @@ static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb, static Similarity3 alignGivenR(const Point3Pairs &abPointPairs, const Rot3 &aRb) { auto centroids = means(abPointPairs); - auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); + auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids); return align(d_abPointPairs, aRb, centroids); } -} // namespace +} // namespace internal Similarity3::Similarity3() : t_(0,0,0), s_(1) { @@ -165,11 +165,11 @@ Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) { if (abPointPairs.size() < 3) throw std::runtime_error("input should have at least 3 pairs of points"); auto centroids = means(abPointPairs); - auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); - Matrix3 H = calculateH(d_abPointPairs); + auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids); + Matrix3 H = internal::calculateH(d_abPointPairs); // ClosestTo finds rotation matrix closest to H in Frobenius sense Rot3 aRb = Rot3::ClosestTo(H); - return align(d_abPointPairs, aRb, centroids); + return internal::align(d_abPointPairs, aRb, centroids); } Similarity3 Similarity3::Align(const vector &abPosePairs) { @@ -192,7 +192,7 @@ Similarity3 Similarity3::Align(const vector &abPosePairs) { } const Rot3 aRb_estimate = FindKarcherMean(rotations); - return alignGivenR(abPointPairs, aRb_estimate); + return internal::alignGivenR(abPointPairs, aRb_estimate); } Matrix4 Similarity3::wedge(const Vector7 &xi) { @@ -283,15 +283,11 @@ std::ostream &operator<<(std::ostream &os, const Similarity3& p) { return os; } -const Matrix4 Similarity3::matrix() const { +Matrix4 Similarity3::matrix() const { Matrix4 T; T.topRows<3>() << R_.matrix(), t_; T.bottomRows<1>() << 0, 0, 0, 1.0 / s_; return T; } -Similarity3::operator Pose3() const { - return Pose3(R_, s_ * t_); -} - } // namespace gtsam diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index 0ef787b05..845d4c810 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -18,13 +18,12 @@ #pragma once -#include -#include -#include #include #include #include - +#include +#include +#include namespace gtsam { @@ -34,108 +33,106 @@ class Pose3; /** * 3D similarity transform */ -class Similarity3: public LieGroup { - +class GTSAM_EXPORT Similarity3 : public LieGroup { /// @name Pose Concept /// @{ typedef Rot3 Rotation; typedef Point3 Translation; /// @} -private: + private: Rot3 R_; Point3 t_; double s_; -public: - + public: /// @name Constructors /// @{ /// Default constructor - GTSAM_EXPORT Similarity3(); + Similarity3(); /// Construct pure scaling - GTSAM_EXPORT Similarity3(double s); + Similarity3(double s); /// Construct from GTSAM types - GTSAM_EXPORT Similarity3(const Rot3& R, const Point3& t, double s); + Similarity3(const Rot3& R, const Point3& t, double s); /// Construct from Eigen types - GTSAM_EXPORT Similarity3(const Matrix3& R, const Vector3& t, double s); + Similarity3(const Matrix3& R, const Vector3& t, double s); /// Construct from matrix [R t; 0 s^-1] - GTSAM_EXPORT Similarity3(const Matrix4& T); + Similarity3(const Matrix4& T); /// @} /// @name Testable /// @{ /// Compare with tolerance - GTSAM_EXPORT bool equals(const Similarity3& sim, double tol) const; + bool equals(const Similarity3& sim, double tol) const; /// Exact equality - GTSAM_EXPORT bool operator==(const Similarity3& other) const; + bool operator==(const Similarity3& other) const; /// Print with optional string - GTSAM_EXPORT void print(const std::string& s) const; + void print(const std::string& s) const; - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p); + friend std::ostream& operator<<(std::ostream& os, const Similarity3& p); /// @} /// @name Group /// @{ /// Return an identity transform - GTSAM_EXPORT static Similarity3 identity(); + static Similarity3 identity(); /// Composition - GTSAM_EXPORT Similarity3 operator*(const Similarity3& S) const; + Similarity3 operator*(const Similarity3& S) const; /// Return the inverse - GTSAM_EXPORT Similarity3 inverse() const; + Similarity3 inverse() const; /// @} /// @name Group action on Point3 /// @{ /// Action on a point p is s*(R*p+t) - GTSAM_EXPORT Point3 transformFrom(const Point3& p, // - OptionalJacobian<3, 7> H1 = boost::none, // - OptionalJacobian<3, 3> H2 = boost::none) const; + Point3 transformFrom(const Point3& p, // + OptionalJacobian<3, 7> H1 = boost::none, // + OptionalJacobian<3, 3> H2 = boost::none) const; - /** + /** * Action on a pose T. - * |Rs ts| |R t| |Rs*R Rs*t+ts| + * |Rs ts| |R t| |Rs*R Rs*t+ts| * |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim3 object. * To retrieve a Pose3, we normalized the scale value into 1. * |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)| * | 0 1/s | = | 0 1 | - * - * This group action satisfies the compatibility condition. + * + * This group action satisfies the compatibility condition. * For more details, refer to: https://en.wikipedia.org/wiki/Group_action */ - GTSAM_EXPORT Pose3 transformFrom(const Pose3& T) const; + Pose3 transformFrom(const Pose3& T) const; /** syntactic sugar for transformFrom */ - GTSAM_EXPORT Point3 operator*(const Point3& p) const; + Point3 operator*(const Point3& p) const; /** * Create Similarity3 by aligning at least three point pairs */ - GTSAM_EXPORT static Similarity3 Align(const std::vector& abPointPairs); - + static Similarity3 Align(const std::vector& abPointPairs); + /** * Create the Similarity3 object that aligns at least two pose pairs. * Each pair is of the form (aTi, bTi). * Given a list of pairs in frame a, and a list of pairs in frame b, Align() * will compute the best-fit Similarity3 aSb transformation to align them. * First, the rotation aRb will be computed as the average (Karcher mean) of - * many estimates aRb (from each pair). Afterwards, the scale factor will be computed - * using the algorithm described here: + * many estimates aRb (from each pair). Afterwards, the scale factor will be + * computed using the algorithm described here: * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf */ - GTSAM_EXPORT static Similarity3 Align(const std::vector& abPosePairs); + static Similarity3 Align(const std::vector& abPosePairs); /// @} /// @name Lie Group @@ -144,20 +141,22 @@ public: /** Log map at the identity * \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$ */ - GTSAM_EXPORT static Vector7 Logmap(const Similarity3& s, // - OptionalJacobian<7, 7> Hm = boost::none); + static Vector7 Logmap(const Similarity3& s, // + OptionalJacobian<7, 7> Hm = boost::none); /** Exponential map at the identity */ - GTSAM_EXPORT static Similarity3 Expmap(const Vector7& v, // - OptionalJacobian<7, 7> Hm = boost::none); + static Similarity3 Expmap(const Vector7& v, // + OptionalJacobian<7, 7> Hm = boost::none); /// Chart at the origin struct ChartAtOrigin { - static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) { + static Similarity3 Retract(const Vector7& v, + ChartJacobian H = boost::none) { return Similarity3::Expmap(v, H); } - static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) { + static Vector7 Local(const Similarity3& other, + ChartJacobian H = boost::none) { return Similarity3::Logmap(other, H); } }; @@ -170,67 +169,53 @@ public: * @return 4*4 element of Lie algebra that can be exponentiated * TODO(frank): rename to Hat, make part of traits */ - GTSAM_EXPORT static Matrix4 wedge(const Vector7& xi); + static Matrix4 wedge(const Vector7& xi); /// Project from one tangent space to another - GTSAM_EXPORT Matrix7 AdjointMap() const; + Matrix7 AdjointMap() const; /// @} /// @name Standard interface /// @{ /// Calculate 4*4 matrix group equivalent - GTSAM_EXPORT const Matrix4 matrix() const; + Matrix4 matrix() const; /// Return a GTSAM rotation - const Rot3& rotation() const { - return R_; - } + Rot3 rotation() const { return R_; } /// Return a GTSAM translation - const Point3& translation() const { - return t_; - } + Point3 translation() const { return t_; } /// Return the scale - double scale() const { - return s_; - } - - /// Convert to a rigid body pose (R, s*t) - /// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast. - GTSAM_EXPORT operator Pose3() const; + double scale() const { return s_; } /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes - inline static size_t Dim() { - return 7; - } + inline static size_t Dim() { return 7; } /// Dimensionality of tangent space = 7 DOF - inline size_t dim() const { - return 7; - } + inline size_t dim() const { return 7; } /// @} /// @name Helper functions /// @{ -private: + private: /// Calculate expmap and logmap coefficients. static Matrix3 GetV(Vector3 w, double lambda); /// @} }; -template<> +template <> inline Matrix wedge(const Vector& xi) { return Similarity3::wedge(xi); } -template<> +template <> struct traits : public internal::LieGroup {}; -template<> +template <> struct traits : public internal::LieGroup {}; -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 858270c00..0dc23c160 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -547,6 +547,12 @@ class EssentialMatrix { // Standard Constructors EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); + // Constructors from Pose3 + gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_); + + gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_, + Eigen::Ref H); + // Testable void print(string s = "") const; bool equals(const gtsam::EssentialMatrix& pose, double tol) const; @@ -584,7 +590,13 @@ class Cal3_S2 { // Action on Point2 gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; // Standard Interface double fx() const; @@ -623,7 +635,13 @@ virtual class Cal3DS2_Base { // Action on Point2 gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; // enabling serialization functionality void serialize() const; @@ -680,7 +698,13 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base { // Note: the signature of this functions differ from the functions // with equal name in the base class. gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; // enabling serialization functionality void serialize() const; @@ -706,7 +730,13 @@ class Cal3Fisheye { // Action on Point2 gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; // Standard Interface double fx() const; @@ -769,7 +799,13 @@ class Cal3Bundler { // Action on Point2 gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; // Standard Interface double fx() const; @@ -807,12 +843,25 @@ class CalibratedCamera { // Action on Point3 gtsam::Point2 project(const gtsam::Point3& point) const; + gtsam::Point2 project(const gtsam::Point3& point, + Eigen::Ref Dcamera, + Eigen::Ref Dpoint); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + gtsam::Point3 backproject(const gtsam::Point2& p, double depth, + Eigen::Ref Dresult_dpose, + Eigen::Ref Dresult_dp, + Eigen::Ref Dresult_ddepth); + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); // Standard Interface gtsam::Pose3 pose() const; double range(const gtsam::Point3& point) const; + double range(const gtsam::Point3& point, Eigen::Ref Dcamera, + Eigen::Ref Dpoint); double range(const gtsam::Pose3& pose) const; + double range(const gtsam::Pose3& point, Eigen::Ref Dcamera, + Eigen::Ref Dpose); double range(const gtsam::CalibratedCamera& camera) const; // enabling serialization functionality @@ -824,6 +873,7 @@ template class PinholeCamera { // Standard Constructors and Named Constructors PinholeCamera(); + PinholeCamera(const gtsam::PinholeCamera other); PinholeCamera(const gtsam::Pose3& pose); PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, @@ -850,14 +900,123 @@ class PinholeCamera { static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); pair projectSafe(const gtsam::Point3& pw) const; gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point2 project(const gtsam::Point3& point, + Eigen::Ref Dpose, + Eigen::Ref Dpoint, + Eigen::Ref Dcal); gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + gtsam::Point3 backproject(const gtsam::Point2& p, double depth, + Eigen::Ref Dresult_dpose, + Eigen::Ref Dresult_dp, + Eigen::Ref Dresult_ddepth, + Eigen::Ref Dresult_dcal); + + gtsam::Point2 reprojectionError(const gtsam::Point3& pw, const gtsam::Point2& measured, + Eigen::Ref Dpose, + Eigen::Ref Dpoint, + Eigen::Ref Dcal); + double range(const gtsam::Point3& point); + double range(const gtsam::Point3& point, Eigen::Ref Dcamera, + Eigen::Ref Dpoint); double range(const gtsam::Pose3& pose); + double range(const gtsam::Pose3& pose, Eigen::Ref Dcamera, + Eigen::Ref Dpose); // enabling serialization functionality void serialize() const; }; +// Forward declaration of PinholeCameraCalX is defined here. +#include +// Some typedefs for common camera types +// PinholeCameraCal3_S2 is the same as SimpleCamera above +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; +typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +typedef gtsam::PinholeCamera PinholeCameraCal3Unified; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; + +#include +template +class PinholePose { + // Standard Constructors and Named Constructors + PinholePose(); + PinholePose(const gtsam::PinholePose other); + PinholePose(const gtsam::Pose3& pose); + PinholePose(const gtsam::Pose3& pose, const CALIBRATION* K); + static This Level(const gtsam::Pose2& pose, double height); + static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const CALIBRATION* K); + + // Testable + void print(string s = "PinholePose") const; + bool equals(const This& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + CALIBRATION calibration() const; + + // Manifold + This retract(Vector d) const; + Vector localCoordinates(const This& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point2 project(const gtsam::Point3& point, + Eigen::Ref Dpose, + Eigen::Ref Dpoint, + Eigen::Ref Dcal); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + gtsam::Point3 backproject(const gtsam::Point2& p, double depth, + Eigen::Ref Dresult_dpose, + Eigen::Ref Dresult_dp, + Eigen::Ref Dresult_ddepth, + Eigen::Ref Dresult_dcal); + double range(const gtsam::Point3& point); + double range(const gtsam::Point3& point, Eigen::Ref Dcamera, + Eigen::Ref Dpoint); + double range(const gtsam::Pose3& pose); + double range(const gtsam::Pose3& pose, Eigen::Ref Dcamera, + Eigen::Ref Dpose); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::PinholePose PinholePoseCal3_S2; +typedef gtsam::PinholePose PinholePoseCal3DS2; +typedef gtsam::PinholePose PinholePoseCal3Unified; +typedef gtsam::PinholePose PinholePoseCal3Bundler; +typedef gtsam::PinholePose PinholePoseCal3Fisheye; + +#include +class Similarity2 { + // Standard Constructors + Similarity2(); + Similarity2(double s); + Similarity2(const gtsam::Rot2& R, const gtsam::Point2& t, double s); + Similarity2(const Matrix& R, const Vector& t, double s); + Similarity2(const Matrix& T); + + gtsam::Point2 transformFrom(const gtsam::Point2& p) const; + gtsam::Pose2 transformFrom(const gtsam::Pose2& T); + + static gtsam::Similarity2 Align(const gtsam::Point2Pairs& abPointPairs); + static gtsam::Similarity2 Align(const gtsam::Pose2Pairs& abPosePairs); + + // Standard Interface + bool equals(const gtsam::Similarity2& sim, double tol) const; + Matrix matrix() const; + gtsam::Rot2& rotation(); + gtsam::Point2& translation(); + double scale() const; +}; + #include class Similarity3 { // Standard Constructors @@ -874,22 +1033,13 @@ class Similarity3 { static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs); // Standard Interface - const Matrix matrix() const; - const gtsam::Rot3& rotation(); - const gtsam::Point3& translation(); + bool equals(const gtsam::Similarity3& sim, double tol) const; + Matrix matrix() const; + gtsam::Rot3& rotation(); + gtsam::Point3& translation(); double scale() const; }; -// Forward declaration of PinholeCameraCalX is defined here. -#include -// Some typedefs for common camera types -// PinholeCameraCal3_S2 is the same as SimpleCamera above -typedef gtsam::PinholeCamera PinholeCameraCal3_S2; -typedef gtsam::PinholeCamera PinholeCameraCal3DS2; -typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; -typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; - template class CameraSet { CameraSet(); @@ -921,9 +1071,18 @@ class StereoCamera { static size_t Dim(); // Transformations and measurement functions - gtsam::StereoPoint2 project(const gtsam::Point3& point); + gtsam::StereoPoint2 project(const gtsam::Point3& point) const; gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; + // project with Jacobian + gtsam::StereoPoint2 project2(const gtsam::Point3& point, + Eigen::Ref H1, + Eigen::Ref H2) const; + + gtsam::Point3 backproject2(const gtsam::StereoPoint2& p, + Eigen::Ref H1, + Eigen::Ref H2) const; + // enabling serialization functionality void serialize() const; }; diff --git a/gtsam/geometry/tests/testSimilarity2.cpp b/gtsam/geometry/tests/testSimilarity2.cpp new file mode 100644 index 000000000..dd4fd0efd --- /dev/null +++ b/gtsam/geometry/tests/testSimilarity2.cpp @@ -0,0 +1,66 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSimilarity2.cpp + * @brief Unit tests for Similarity2 class + * @author Varun Agrawal + */ + +#include +#include +#include +#include +#include + +#include + +using namespace std::placeholders; +using namespace gtsam; +using namespace std; + +GTSAM_CONCEPT_TESTABLE_INST(Similarity2) + +static const Point2 P(0.2, 0.7); +static const Rot2 R = Rot2::fromAngle(0.3); +static const double s = 4; + +const double degree = M_PI / 180; + +//****************************************************************************** +TEST(Similarity2, Concepts) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(Similarity2, Constructors) { + Similarity2 sim2_Construct1; + Similarity2 sim2_Construct2(s); + Similarity2 sim2_Construct3(R, P, s); + Similarity2 sim2_Construct4(R.matrix(), P, s); +} + +//****************************************************************************** +TEST(Similarity2, Getters) { + Similarity2 sim2_default; + EXPECT(assert_equal(Rot2(), sim2_default.rotation())); + EXPECT(assert_equal(Point2(0, 0), sim2_default.translation())); + EXPECT_DOUBLES_EQUAL(1.0, sim2_default.scale(), 1e-9); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp index 428422072..7a134f6ef 100644 --- a/gtsam/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -458,18 +458,18 @@ TEST(Similarity3, Optimization2) { Values result; result = LevenbergMarquardtOptimizer(graph, initial).optimize(); //result.print("Optimized Estimate\n"); - Pose3 p1, p2, p3, p4, p5; - p1 = Pose3(result.at(X(1))); - p2 = Pose3(result.at(X(2))); - p3 = Pose3(result.at(X(3))); - p4 = Pose3(result.at(X(4))); - p5 = Pose3(result.at(X(5))); + Similarity3 p1, p2, p3, p4, p5; + p1 = result.at(X(1)); + p2 = result.at(X(2)); + p3 = result.at(X(3)); + p4 = result.at(X(4)); + p5 = result.at(X(5)); - //p1.print("Pose1"); - //p2.print("Pose2"); - //p3.print("Pose3"); - //p4.print("Pose4"); - //p5.print("Pose5"); + //p1.print("Similarity1"); + //p2.print("Similarity2"); + //p3.print("Similarity3"); + //p4.print("Similarity4"); + //p5.print("Similarity5"); Similarity3 expected(0.7); EXPECT(assert_equal(expected, result.at(X(5)), 0.4)); diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 1699b833f..d9cfc1f3c 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -54,23 +54,31 @@ namespace noiseModel { // clang-format on namespace mEstimator { -//--------------------------------------------------------------------------------------- - +/** + * Pure virtual class for all robust error function classes. + * + * It provides the machinery for block vs scalar reweighting strategies, in + * addition to defining the interface of derived classes. + */ class GTSAM_EXPORT Base { public: + /** the rows can be weighted independently according to the error + * or uniformly with the norm of the right hand side */ enum ReweightScheme { Scalar, Block }; typedef boost::shared_ptr shared_ptr; protected: - /** the rows can be weighted independently according to the error - * or uniformly with the norm of the right hand side */ + /// Strategy for reweighting \sa ReweightScheme ReweightScheme reweight_; public: Base(const ReweightScheme reweight = Block) : reweight_(reweight) {} virtual ~Base() {} - /* + /// Returns the reweight scheme, as explained in ReweightScheme + ReweightScheme reweightScheme() const { return reweight_; } + + /** * This method is responsible for returning the total penalty for a given * amount of error. For example, this method is responsible for implementing * the quadratic function for an L2 penalty, the absolute value function for @@ -80,16 +88,20 @@ class GTSAM_EXPORT Base { * error vector, then it prevents implementations of asymmeric loss * functions. It would be better for this function to accept the vector and * internally call the norm if necessary. + * + * This returns \rho(x) in \ref mEstimator */ - virtual double loss(double distance) const { return 0; }; + virtual double loss(double distance) const { return 0; } - /* + /** * This method is responsible for returning the weight function for a given * amount of error. The weight function is related to the analytic derivative * of the loss function. See * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf * for details. This method is required when optimizing cost functions with * robust penalties using iteratively re-weighted least squares. + * + * This returns w(x) in \ref mEstimator */ virtual double weight(double distance) const = 0; @@ -124,7 +136,15 @@ class GTSAM_EXPORT Base { } }; -/// Null class should behave as Gaussian +/** "Null" robust loss function, equivalent to a Gaussian pdf noise model, or + * plain least-squares (non-robust). + * + * This model has no additional parameters. + * + * - Loss \rho(x) = 0.5 x² + * - Derivative \phi(x) = x + * - Weight w(x) = \phi(x)/x = 1 + */ class GTSAM_EXPORT Null : public Base { public: typedef boost::shared_ptr shared_ptr; @@ -146,7 +166,14 @@ class GTSAM_EXPORT Null : public Base { } }; -/// Fair implements the "Fair" robust error model (Zhang97ivc) +/** Implementation of the "Fair" robust error model (Zhang97ivc) + * + * This model has a scalar parameter "c". + * + * - Loss \rho(x) = c² (|x|/c - log(1+|x|/c)) + * - Derivative \phi(x) = x/(1+|x|/c) + * - Weight w(x) = \phi(x)/x = 1/(1+|x|/c) + */ class GTSAM_EXPORT Fair : public Base { protected: double c_; @@ -160,6 +187,7 @@ class GTSAM_EXPORT Fair : public Base { void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double c, const ReweightScheme reweight = Block); + double modelParameter() const { return c_; } private: /** Serialization function */ @@ -171,7 +199,14 @@ class GTSAM_EXPORT Fair : public Base { } }; -/// Huber implements the "Huber" robust error model (Zhang97ivc) +/** The "Huber" robust error model (Zhang97ivc). + * + * This model has a scalar parameter "k". + * + * - Loss \rho(x) = 0.5 x² if |x| shared_ptr; @@ -295,6 +359,7 @@ class GTSAM_EXPORT GemanMcClure : public Base { void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); + double modelParameter() const { return c_; } protected: double c_; @@ -309,11 +374,18 @@ class GTSAM_EXPORT GemanMcClure : public Base { } }; -/// DCS implements the Dynamic Covariance Scaling robust error model -/// from the paper Robust Map Optimization (Agarwal13icra). -/// -/// Under the special condition of the parameter c == 1.0 and not -/// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure. +/** DCS implements the Dynamic Covariance Scaling robust error model + * from the paper Robust Map Optimization (Agarwal13icra). + * + * Under the special condition of the parameter c == 1.0 and not + * forcing the output weight s <= 1.0, DCS is similar to Geman-McClure. + * + * This model has a scalar parameter "c" (with "units" of squared error). + * + * - Loss \rho(x) = (c²x² + cx⁴)/(x²+c)² (for any "x") + * - Derivative \phi(x) = 2c²x/(x²+c)² + * - Weight w(x) = \phi(x)/x = 2c²/(x²+c)² if x²>c, 1 otherwise + */ class GTSAM_EXPORT DCS : public Base { public: typedef boost::shared_ptr shared_ptr; @@ -325,6 +397,7 @@ class GTSAM_EXPORT DCS : public Base { void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); + double modelParameter() const { return c_; } protected: double c_; @@ -339,12 +412,19 @@ class GTSAM_EXPORT DCS : public Base { } }; -/// L2WithDeadZone implements a standard L2 penalty, but with a dead zone of -/// width 2*k, centered at the origin. The resulting penalty within the dead -/// zone is always zero, and grows quadratically outside the dead zone. In this -/// sense, the L2WithDeadZone penalty is "robust to inliers", rather than being -/// robust to outliers. This penalty can be used to create barrier functions in -/// a general way. +/** L2WithDeadZone implements a standard L2 penalty, but with a dead zone of + * width 2*k, centered at the origin. The resulting penalty within the dead + * zone is always zero, and grows quadratically outside the dead zone. In this + * sense, the L2WithDeadZone penalty is "robust to inliers", rather than being + * robust to outliers. This penalty can be used to create barrier functions in + * a general way. + * + * This model has a scalar parameter "k". + * + * - Loss \rho(x) = 0 if |x|k, (k+x) if x<-k + * - Weight w(x) = \phi(x)/x = 0 if |x|k, (k+x)/x if x<-k + */ class GTSAM_EXPORT L2WithDeadZone : public Base { protected: double k_; @@ -358,6 +438,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base { void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); + double modelParameter() const { return k_; } private: /** Serialization function */ diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index f1bc92f69..943b661d8 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -671,6 +671,10 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters { DummyPreconditionerParameters(); }; +virtual class BlockJacobiPreconditionerParameters : gtsam::PreconditionerParameters { + BlockJacobiPreconditionerParameters(); +}; + #include virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { PCGSolverParameters(); diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 10c33d101..c7d82975a 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -90,7 +90,11 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation /** * Add a single Gyroscope measurement to the preintegration. - * @param measureOmedga Measured angular velocity (in body frame) + * Measurements are taken to be in the sensor + * frame and conversion to the body frame is handled by `body_P_sensor` in + * `PreintegratedRotationParams` (if provided). + * + * @param measuredOmega Measured angular velocity (as given by the sensor) * @param deltaT Time step */ void integrateMeasurement(const Vector3& measuredOmega, double deltaT); diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 068a17ca4..213f5f223 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -208,8 +208,11 @@ public: /** * Add a single IMU measurement to the preintegration. - * @param measuredAcc Measured acceleration (in body frame, as given by the - * sensor) + * Both accelerometer and gyroscope measurements are taken to be in the sensor + * frame and conversion to the body frame is handled by `body_P_sensor` in + * `PreintegrationParams`. + * + * @param measuredAcc Measured acceleration (as given by the sensor) * @param measuredOmega Measured angular velocity (as given by the sensor) * @param dt Time interval between two consecutive IMU measurements */ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 35207e452..740827162 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -121,7 +121,11 @@ public: /** * Add a single IMU measurement to the preintegration. - * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) + * Both accelerometer and gyroscope measurements are taken to be in the sensor + * frame and conversion to the body frame is handled by `body_P_sensor` in + * `PreintegrationParams`. + * + * @param measuredAcc Measured acceleration (as given by the sensor) * @param measuredOmega Measured angular velocity (as given by the sensor) * @param dt Time interval between this and the last IMU measurement */ diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index c94e1d3d5..5607add16 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -27,7 +27,6 @@ #include #include -using namespace std::placeholders; using namespace std; using namespace gtsam; using namespace GeographicLib; @@ -71,7 +70,7 @@ TEST( GPSFactor, Constructor ) { EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T); // Use the factor to calculate the derivative @@ -100,7 +99,7 @@ TEST( GPSFactor2, Constructor ) { EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T); // Use the factor to calculate the derivative diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index e2a623710..971803dbf 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -26,7 +26,6 @@ #include -using namespace std::placeholders; using namespace std; using namespace gtsam; using namespace GeographicLib; @@ -64,7 +63,7 @@ TEST( MagFactor, unrotate ) { Matrix H; Point3 expected(22735.5, 314.502, 44202.5); EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6)); } @@ -75,27 +74,27 @@ TEST( MagFactor, Factors ) { // MagFactor MagFactor f(1, measured, s, dir, bias, model); - EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); - EXPECT( assert_equal((Matrix)numericalDerivative11 // + EXPECT(assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); + EXPECT(assert_equal((Matrix)numericalDerivative11 // (std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7)); -// MagFactor1 + // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); - EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT(assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); + EXPECT(assert_equal(numericalDerivative11 // (std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7)); -// MagFactor2 + // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); - EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT(assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); + EXPECT(assert_equal(numericalDerivative11 // (std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),// H1, 1e-7)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),// H2, 1e-7)); -// MagFactor2 + // MagFactor3 MagFactor3 f3(1, 2, 3, measured, nRb, model); EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal((Matrix)numericalDerivative11 // diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 3fff71978..30181e08d 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -226,6 +226,10 @@ class Values { void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); void insert(size_t j, double c); @@ -269,6 +273,10 @@ class Values { void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); @@ -310,6 +318,10 @@ class Values { void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert_or_assign(size_t j, const gtsam::NavState& nav_state); void insert_or_assign(size_t j, Vector vector); @@ -351,6 +363,10 @@ class Values { gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::PinholePose, + gtsam::PinholePose, + gtsam::PinholePose, + gtsam::PinholePose, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h index afce12205..430e107ad 100644 --- a/gtsam/sfm/SfmData.h +++ b/gtsam/sfm/SfmData.h @@ -77,10 +77,14 @@ struct GTSAM_EXPORT SfmData { size_t numberCameras() const { return cameras.size(); } /// The track formed by series of landmark measurements - SfmTrack track(size_t idx) const { return tracks[idx]; } + const SfmTrack& track(size_t idx) const { return tracks[idx]; } /// The camera pose at frame index `idx` - SfmCamera camera(size_t idx) const { return cameras[idx]; } + const SfmCamera& camera(size_t idx) const { return cameras[idx]; } + + /// Getters + const std::vector& cameraList() const { return cameras; } + const std::vector& trackList() const { return tracks; } /** * @brief Create projection factors using keys i and P(j) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index bf9a73ac5..19e21b10c 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -9,6 +9,8 @@ class SfmTrack { SfmTrack(); SfmTrack(const gtsam::Point3& pt); const Point3& point3() const; + + Point3 p; double r; double g; @@ -34,12 +36,15 @@ class SfmData { static gtsam::SfmData FromBundlerFile(string filename); static gtsam::SfmData FromBalFile(string filename); + std::vector& trackList() const; + std::vector>& cameraList() const; + void addTrack(const gtsam::SfmTrack& t); void addCamera(const gtsam::SfmCamera& cam); size_t numberTracks() const; size_t numberCameras() const; - gtsam::SfmTrack track(size_t idx) const; - gtsam::PinholeCamera camera(size_t idx) const; + gtsam::SfmTrack& track(size_t idx) const; + gtsam::PinholeCamera& camera(size_t idx) const; gtsam::NonlinearFactorGraph generalSfmFactors( const gtsam::SharedNoiseModel& model = @@ -91,6 +96,13 @@ class BinaryMeasurementsUnit3 { void push_back(const gtsam::BinaryMeasurement& measurement); }; +class BinaryMeasurementsRot3 { + BinaryMeasurementsRot3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; + #include // TODO(frank): copy/pasta below until we have integer template paremeters in @@ -184,6 +196,10 @@ class ShonanAveraging2 { }; class ShonanAveraging3 { + ShonanAveraging3( + const std::vector>& measurements, + const gtsam::ShonanAveragingParameters3& parameters = + gtsam::ShonanAveragingParameters3()); ShonanAveraging3(string g2oFile); ShonanAveraging3(string g2oFile, const gtsam::ShonanAveragingParameters3& parameters); diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h index c81a9adc5..00f741705 100644 --- a/gtsam/slam/KarcherMeanFactor-inl.h +++ b/gtsam/slam/KarcherMeanFactor-inl.h @@ -40,8 +40,7 @@ T FindKarcherMeanImpl(const vector& rotations) { return result.at(kKey); } -template ::value >::type > +template T FindKarcherMean(const std::vector& rotations) { return FindKarcherMeanImpl(rotations); } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index dc450a9f7..7c4d7fea7 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename, size_t maxIndex = 0); using BinaryMeasurementsUnit3 = std::vector>; +using BinaryMeasurementsRot3 = std::vector>; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 inline boost::optional GTSAM_DEPRECATED diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 4e943253e..8e1e06d5b 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -90,6 +90,22 @@ typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Unified; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorPoseCal3_S2; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorPoseCal3DS2; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorPoseCal3Bundler; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorPoseCal3Fisheye; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorPoseCal3Unified; + template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 31f89bd0c..249f2d7d7 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -48,6 +48,7 @@ set(ignore gtsam::Rot3Vector gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 + gtsam::BinaryMeasurementsRot3 gtsam::DiscreteKey gtsam::KeyPairDoubleMap) @@ -99,11 +100,23 @@ set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam) copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" "${GTSAM_MODULE_PATH}") -# Hack to get python test files copied every time they are modified +# Hack to get python test and util files copied every time they are modified file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py") foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY) endforeach() +file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py") +foreach(util_file ${GTSAM_PYTHON_UTIL_FILES}) + configure_file(${util_file} "${GTSAM_MODULE_PATH}/utils/${test_file}" COPYONLY) +endforeach() +file(GLOB GTSAM_PYTHON_PREAMBLE_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/preamble/*.h") +foreach(util_file ${GTSAM_PYTHON_PREAMBLE_FILES}) + configure_file(${util_file} "${GTSAM_MODULE_PATH}/preamble/${test_file}" COPYONLY) +endforeach() +file(GLOB GTSAM_PYTHON_SPECIALIZATION_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/specializations/*.h") +foreach(util_file ${GTSAM_PYTHON_SPECIALIZATION_FILES}) + configure_file(${util_file} "${GTSAM_MODULE_PATH}/specializations/${test_file}" COPYONLY) +endforeach() # Common directory for data/datasets stored with the package. # This will store the data in the Python site package directly. @@ -126,6 +139,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::KeyVector gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::BinaryMeasurementsUnit3 + gtsam::BinaryMeasurementsRot3 gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler gtsam::CameraSetCal3Unified @@ -161,7 +175,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) # Hack to get python test files copied every time they are modified file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py") - foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) + foreach(test_file ${GTSAM_UNSTABLE_PYTHON_TEST_FILES}) configure_file(${test_file} "${GTSAM_UNSTABLE_MODULE_PATH}/tests/${test_file}" COPYONLY) endforeach() diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h index 35fe2a577..bd0441d06 100644 --- a/python/gtsam/preamble/geometry.h +++ b/python/gtsam/preamble/geometry.h @@ -23,8 +23,8 @@ PYBIND11_MAKE_OPAQUE( std::vector>); PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); +PYBIND11_MAKE_OPAQUE(gtsam::Pose2Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE( - gtsam::CameraSet>); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index a34e73058..8ff0ea82e 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -9,4 +9,18 @@ * automatic STL binding, such that the raw objects can be accessed in Python. * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. - */ \ No newline at end of file + */ + +// Including can cause some serious hard-to-debug bugs!!! +// #include +#include + +PYBIND11_MAKE_OPAQUE( + std::vector); + +PYBIND11_MAKE_OPAQUE( + std::vector); +PYBIND11_MAKE_OPAQUE( + std::vector>); +PYBIND11_MAKE_OPAQUE( + std::vector>); diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h index a492ce8eb..5a0ea35ef 100644 --- a/python/gtsam/specializations/geometry.h +++ b/python/gtsam/specializations/geometry.h @@ -16,6 +16,7 @@ py::bind_vector< m_, "Point2Vector"); py::bind_vector>(m_, "Point2Pairs"); py::bind_vector>(m_, "Point3Pairs"); +py::bind_vector>(m_, "Pose2Pairs"); py::bind_vector>(m_, "Pose3Pairs"); py::bind_vector>(m_, "Pose3Vector"); py::bind_vector>>( diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 6de15217f..90a2b8417 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -13,4 +13,19 @@ py::bind_vector > >( m_, "BinaryMeasurementsUnit3"); +py::bind_vector > >( + m_, "BinaryMeasurementsRot3"); py::bind_map(m_, "KeyPairDoubleMap"); + +py::bind_vector< + std::vector >( + m_, "SfmTracks"); + +py::bind_vector< + std::vector >( + m_, "SfmCameras"); + +py::bind_vector< + std::vector>>( + m_, "SfmMeasurementVector" + ); diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index bafbacfa4..630109d66 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -139,6 +139,17 @@ class TestCal3Unified(GtsamTestCase): self.gtsamAssertEquals(z, np.zeros(2)) self.gtsamAssertEquals(H @ H.T, 4*np.eye(2)) + Dcal = np.zeros((2, 10), order='F') + Dp = np.zeros((2, 2), order='F') + camera.calibrate(img_point, Dcal, Dp) + + self.gtsamAssertEquals(Dcal, np.array( + [[ 0., 0., 0., -1., 0., 0., 0., 0., 0., 0.], + [ 0., 0., 0., 0., -1., 0., 0., 0., 0., 0.]])) + self.gtsamAssertEquals(Dp, np.array( + [[ 1., -0.], + [-0., 1.]])) + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation(self): """Estimate spatial point from image measurements""" diff --git a/python/gtsam/tests/test_PinholeCamera.py b/python/gtsam/tests/test_PinholeCamera.py new file mode 100644 index 000000000..392d48d3f --- /dev/null +++ b/python/gtsam/tests/test_PinholeCamera.py @@ -0,0 +1,46 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +PinholeCamera unit tests. +Author: Fan Jiang +""" +import unittest +from math import pi + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestPinholeCamera(GtsamTestCase): + """ + Tests if we can correctly get the camera Jacobians in Python + """ + def test_jacobian(self): + cam1 = gtsam.PinholeCameraCal3Bundler() + + # order is important because Eigen is column major! + Dpose = np.zeros((2, 6), order='F') + Dpoint = np.zeros((2, 3), order='F') + Dcal = np.zeros((2, 3), order='F') + cam1.project(np.array([1, 1, 1]), Dpose, Dpoint, Dcal) + + self.gtsamAssertEquals(Dpoint, np.array([[1, 0, -1], [0, 1, -1]])) + + self.gtsamAssertEquals( + Dpose, + np.array([ + [1., -2., 1., -1., 0., 1.], # + [2., -1., -1., 0., -1., 1.] + ])) + + self.gtsamAssertEquals(Dcal, np.array([[1., 2., 4.], [1., 2., 4.]])) + + +if __name__ == "__main__": + unittest.main() diff --git a/python/gtsam/tests/test_Sim2.py b/python/gtsam/tests/test_Sim2.py new file mode 100644 index 000000000..ea809b965 --- /dev/null +++ b/python/gtsam/tests/test_Sim2.py @@ -0,0 +1,194 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Sim3 unit tests. +Author: John Lambert +""" +# pylint: disable=no-name-in-module +import unittest + +import numpy as np +from gtsam import Pose2, Pose2Pairs, Rot2, Similarity2 +from gtsam.utils.test_case import GtsamTestCase + + +class TestSim2(GtsamTestCase): + """Test selected Sim2 methods.""" + + def test_align_poses_along_straight_line(self) -> None: + """Test Align Pose2Pairs method. + + Scenario: + 3 object poses + same scale (no gauge ambiguity) + world frame has poses rotated about 180 degrees. + world and egovehicle frame translated by 15 meters w.r.t. each other + """ + R180 = Rot2.fromDegrees(180) + + # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame) + # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame + eTo0 = Pose2(Rot2(), np.array([5, 0])) + eTo1 = Pose2(Rot2(), np.array([10, 0])) + eTo2 = Pose2(Rot2(), np.array([15, 0])) + + eToi_list = [eTo0, eTo1, eTo2] + + # Create destination poses + # (same three objects, but instead living in the world "w" frame) + wTo0 = Pose2(R180, np.array([-10, 0])) + wTo1 = Pose2(R180, np.array([-5, 0])) + wTo2 = Pose2(R180, np.array([0, 0])) + + wToi_list = [wTo0, wTo1, wTo2] + + we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + wSe = Similarity2.Align(we_pairs) + + for wToi, eToi in zip(wToi_list, eToi_list): + self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) + + def test_align_poses_along_straight_line_gauge(self): + """Test if Align Pose3Pairs method can account for gauge ambiguity. + + Scenario: + 3 object poses + with gauge ambiguity (2x scale) + world frame has poses rotated by 90 degrees. + world and egovehicle frame translated by 11 meters w.r.t. each other + """ + R90 = Rot2.fromDegrees(90) + + # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame) + # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame + eTo0 = Pose2(Rot2(), np.array([1, 0])) + eTo1 = Pose2(Rot2(), np.array([2, 0])) + eTo2 = Pose2(Rot2(), np.array([4, 0])) + + eToi_list = [eTo0, eTo1, eTo2] + + # Create destination poses + # (same three objects, but instead living in the world/city "w" frame) + wTo0 = Pose2(R90, np.array([0, 12])) + wTo1 = Pose2(R90, np.array([0, 14])) + wTo2 = Pose2(R90, np.array([0, 18])) + + wToi_list = [wTo0, wTo1, wTo2] + + we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + wSe = Similarity2.Align(we_pairs) + + for wToi, eToi in zip(wToi_list, eToi_list): + self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) + + def test_align_poses_scaled_squares(self): + """Test if Align Pose2Pairs method can account for gauge ambiguity. + + Make sure a big and small square can be aligned. + The u's represent a big square (10x10), and v's represents a small square (4x4). + + Scenario: + 4 object poses + with gauge ambiguity (2.5x scale) + """ + # 0, 90, 180, and 270 degrees yaw + R0 = Rot2.fromDegrees(0) + R90 = Rot2.fromDegrees(90) + R180 = Rot2.fromDegrees(180) + R270 = Rot2.fromDegrees(270) + + aTi0 = Pose2(R0, np.array([2, 3])) + aTi1 = Pose2(R90, np.array([12, 3])) + aTi2 = Pose2(R180, np.array([12, 13])) + aTi3 = Pose2(R270, np.array([2, 13])) + + aTi_list = [aTi0, aTi1, aTi2, aTi3] + + bTi0 = Pose2(R0, np.array([4, 3])) + bTi1 = Pose2(R90, np.array([8, 3])) + bTi2 = Pose2(R180, np.array([8, 7])) + bTi3 = Pose2(R270, np.array([4, 7])) + + bTi_list = [bTi0, bTi1, bTi2, bTi3] + + ab_pairs = Pose2Pairs(list(zip(aTi_list, bTi_list))) + + # Recover the transformation wSe (i.e. world_S_egovehicle) + aSb = Similarity2.Align(ab_pairs) + + for aTi, bTi in zip(aTi_list, bTi_list): + self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi)) + + def test_constructor(self) -> None: + """Sim(2) to perform p_b = bSa * p_a""" + bRa = Rot2() + bta = np.array([1, 2]) + bsa = 3.0 + bSa = Similarity2(R=bRa, t=bta, s=bsa) + self.assertIsInstance(bSa, Similarity2) + np.testing.assert_allclose(bSa.rotation().matrix(), bRa.matrix()) + np.testing.assert_allclose(bSa.translation(), bta) + np.testing.assert_allclose(bSa.scale(), bsa) + + def test_is_eq(self) -> None: + """Ensure object equality works properly (are equal).""" + bSa = Similarity2(R=Rot2(), t=np.array([1, 2]), s=3.0) + bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3) + self.gtsamAssertEquals(bSa, bSa_) + + def test_not_eq_translation(self) -> None: + """Ensure object equality works properly (not equal translation).""" + bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0) + bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3) + self.assertNotEqual(bSa, bSa_) + + def test_not_eq_rotation(self) -> None: + """Ensure object equality works properly (not equal rotation).""" + bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0) + bSa_ = Similarity2(R=Rot2.fromDegrees(180), t=np.array([2.0, 1.0]), s=3) + self.assertNotEqual(bSa, bSa_) + + def test_not_eq_scale(self) -> None: + """Ensure object equality works properly (not equal scale).""" + bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0) + bSa_ = Similarity2(R=Rot2(), t=np.array([2.0, 1.0]), s=1.0) + self.assertNotEqual(bSa, bSa_) + + def test_rotation(self) -> None: + """Ensure rotation component is returned properly.""" + R = Rot2.fromDegrees(90) + t = np.array([1, 2]) + bSa = Similarity2(R=R, t=t, s=3.0) + + # evaluates to [[0, -1], [1, 0]] + expected_R = Rot2.fromDegrees(90) + np.testing.assert_allclose(expected_R.matrix(), bSa.rotation().matrix()) + + def test_translation(self) -> None: + """Ensure translation component is returned properly.""" + R = Rot2.fromDegrees(90) + t = np.array([1, 2]) + bSa = Similarity2(R=R, t=t, s=3.0) + + expected_t = np.array([1, 2]) + np.testing.assert_allclose(expected_t, bSa.translation()) + + def test_scale(self) -> None: + """Ensure the scale factor is returned properly.""" + bRa = Rot2() + bta = np.array([1, 2]) + bsa = 3.0 + bSa = Similarity2(R=bRa, t=bta, s=bsa) + self.assertEqual(bSa.scale(), 3.0) + + +if __name__ == "__main__": + unittest.main()