diff --git a/cpp/Cal3_S2.cpp b/cpp/Cal3_S2.cpp index fff63da81..7ee9457e5 100644 --- a/cpp/Cal3_S2.cpp +++ b/cpp/Cal3_S2.cpp @@ -43,18 +43,6 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { /* ************************************************************************* */ -bool assert_equal(const Cal3_S2& actual, const Cal3_S2& expected, double tol) { - bool ret = actual.equals(expected, tol); - if (!ret) { - cout << "Not Equal:" << endl; - actual.print("Actual"); - expected.print("Expected"); - } - return ret; -} - -/* ************************************************************************* */ - Point2 uncalibrate(const Cal3_S2& K, const Point2& p) { return K.uncalibrate(p); } diff --git a/cpp/Cal3_S2.h b/cpp/Cal3_S2.h index 04fe70d0e..bc4143dcb 100644 --- a/cpp/Cal3_S2.h +++ b/cpp/Cal3_S2.h @@ -12,7 +12,7 @@ namespace gtsam { /** The most common 5DOF 3D->2D calibration */ - class Cal3_S2 { + class Cal3_S2: Testable { private: double fx_, fy_, s_, u0_, v0_; @@ -31,6 +31,15 @@ namespace gtsam { fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) { } + void print(const std::string& s = "") const { + gtsam::print(matrix(), s); + } + + /** + * Check if equal up to specified tolerance + */ + bool equals(const Cal3_S2& K, double tol = 10e-9) const; + /** * return DOF, dimensionality of tangent space */ @@ -82,10 +91,6 @@ namespace gtsam { return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); } - void print(const std::string& s = "") const { - gtsam::print(matrix(), s); - } - std::string dump() const { char buffer[100]; buffer[0] = 0; @@ -93,11 +98,6 @@ namespace gtsam { return std::string(buffer); } - /** - * Check if equal up to specified tolerance - */ - bool equals(const Cal3_S2& K, double tol = 10e-9) const; - /** friends */ friend Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p); @@ -115,11 +115,6 @@ namespace gtsam { } }; - /** - * assert for unit tests - */ - bool assert_equal(const Cal3_S2& actual, const Cal3_S2& expected, double tol = 1e-8); - /** * convert intrinsic coordinates xy to image coordinates uv */ diff --git a/cpp/LinearConstraint.cpp b/cpp/LinearConstraint.cpp index 4a26c36c7..34c5955e4 100644 --- a/cpp/LinearConstraint.cpp +++ b/cpp/LinearConstraint.cpp @@ -38,24 +38,6 @@ LinearConstraint::LinearConstraint(const std::map& matrices { } -ConstrainedConditionalGaussian::shared_ptr LinearConstraint::eliminate(const std::string& key) { - // check to ensure key is one of constraint nodes - const_iterator it = As.find(key); - if (it == As.end()) - throw invalid_argument("Node " + key + " is not in LinearConstraint"); - - // extract the leading matrix - Matrix A1 = it->second; - - // assemble parents - map parents = As; - parents.erase(key); - - // construct resulting CCG with parts - ConstrainedConditionalGaussian::shared_ptr ccg(new ConstrainedConditionalGaussian(A1, parents, b)); - return ccg; -} - void LinearConstraint::print(const string& s) const { cout << s << ": " << endl; string key; Matrix A; @@ -85,6 +67,24 @@ bool LinearConstraint::equals(const LinearConstraint& f, double tol) const { return true; } +ConstrainedConditionalGaussian::shared_ptr LinearConstraint::eliminate(const std::string& key) { + // check to ensure key is one of constraint nodes + const_iterator it = As.find(key); + if (it == As.end()) + throw invalid_argument("Node " + key + " is not in LinearConstraint"); + + // extract the leading matrix + Matrix A1 = it->second; + + // assemble parents + map parents = As; + parents.erase(key); + + // construct resulting CCG with parts + ConstrainedConditionalGaussian::shared_ptr ccg(new ConstrainedConditionalGaussian(A1, parents, b)); + return ccg; +} + bool LinearConstraint::involves(const std::string& key) const { return As.find(key) != As.end(); } @@ -104,17 +104,6 @@ string LinearConstraint::dump() const { return ret; } -bool assert_equal(const LinearConstraint& actual, - const LinearConstraint& expected, double tol) { - bool ret = actual.equals(expected, tol); - if (!ret) { - cout << "Not Equal:" << endl; - actual.print("Actual"); - expected.print("Expected"); - } - return ret; -} - LinearConstraint::shared_ptr combineConstraints( const set& constraints) { map blocks; diff --git a/cpp/LinearConstraint.h b/cpp/LinearConstraint.h index cf7174be3..2aafd95f4 100644 --- a/cpp/LinearConstraint.h +++ b/cpp/LinearConstraint.h @@ -18,7 +18,7 @@ namespace gtsam { * Linear constraints are similar to factors in structure, but represent * a different problem */ -class LinearConstraint { +class LinearConstraint: Testable { public: typedef boost::shared_ptr shared_ptr; @@ -69,17 +69,6 @@ public: ~LinearConstraint() { } - /** - * Eliminates the constraint - * Note: Assumes that the constraint will be completely eliminated - * and that the matrix associated with the key is invertible - * @param key is the variable to eliminate - * @return a constrained conditional gaussian for the variable that is a - * function of its parents - */ - ConstrainedConditionalGaussian::shared_ptr - eliminate(const std::string& key); - /** * print * @param s optional string naming the factor @@ -91,6 +80,16 @@ public: */ bool equals(const LinearConstraint& f, double tol = 1e-9) const; + /** + * Eliminates the constraint + * Note: Assumes that the constraint will be completely eliminated + * and that the matrix associated with the key is invertible + * @param key is the variable to eliminate + * @return a constrained conditional gaussian for the variable that is a + * function of its parents + */ + ConstrainedConditionalGaussian::shared_ptr + eliminate(const std::string& key); /** * returns a version of the factor as a string */ @@ -138,10 +137,6 @@ public: */ LinearConstraint::shared_ptr combineConstraints(const std::set& constraints); -/** assert equals for testing - prints when not equal */ -bool assert_equal(const LinearConstraint& actual, - const LinearConstraint& expected, double tol = 1e-9); - } #endif /* EQUALITYFACTOR_H_ */ diff --git a/cpp/Point3.cpp b/cpp/Point3.cpp index d1ad9a3b3..0854b202a 100644 --- a/cpp/Point3.cpp +++ b/cpp/Point3.cpp @@ -8,6 +8,11 @@ namespace gtsam { +/* ************************************************************************* */ +bool Point3::equals(const Point3 & q, double tol) const { + return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol); +} + /* ************************************************************************* */ bool Point3::operator== (const Point3& q) const { @@ -58,11 +63,6 @@ Matrix Dsub1(const Point3 &p, const Point3 &q) { Matrix Dsub2(const Point3 &p, const Point3 &q) { return -eye(3,3); } - -/* ************************************************************************* */ -bool Point3::equals(const Point3 & q, double tol) const { - return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol); -} /* ************************************************************************* */ Point3 cross(const Point3 &p, const Point3 &q) { @@ -81,14 +81,5 @@ double norm(const Point3 &p) return sqrt( p.x_*p.x_ + p.y_*p.y_ + p.z_*p.z_ ); } /* ************************************************************************* */ -bool assert_equal(const Point3& p, const Point3& q, double tol) { - if(p.equals(q,tol)) return true; - printf("not equal:\n"); - p.print("p = "); - q.print("q = "); - (p-q).print("p-q = "); - return false; -} -/* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/Point3.h b/cpp/Point3.h index f832b4f8e..9cadc0403 100644 --- a/cpp/Point3.h +++ b/cpp/Point3.h @@ -13,11 +13,12 @@ #include #include "Matrix.h" +#include "Testable.h" namespace gtsam { /** A 3D point */ - class Point3 { + class Point3: Testable { private: double x_, y_, z_; @@ -27,6 +28,14 @@ namespace gtsam { Point3(double x, double y, double z): x_(x), y_(y), z_(z) {} Point3(const Vector& v) : x_(v(0)), y_(v(1)), z_(v(2)) {} + /** print with optional string */ + void print(const std::string& s = "") const { + std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl; + } + + /** equals with an tolerance */ + bool equals(const Point3& p, double tol = 1e-9) const; + /** return DOF, dimensionality of tangent space */ size_t dim() const { return 3;} @@ -53,19 +62,11 @@ namespace gtsam { Point3 operator * (double s) const; Point3 operator / (double s) const; - /** print with optional string */ - void print(const std::string& s = "") const { - std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl; - } - /** distance between two points */ double dist(const Point3& p2) const { return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0)); } - /** equals with an tolerance */ - bool equals(const Point3& p, double tol = 1e-9) const; - /** friends */ friend Point3 cross(const Point3 &p1, const Point3 &p2); friend double dot(const Point3 &p1, const Point3 &p2); @@ -103,7 +104,4 @@ namespace gtsam { /** dot product */ double norm(const Point3 &p); - - /** equals with an tolerance, prints out message if unequal */ - bool assert_equal(const Point3& p, const Point3& q, double tol = 1e-9); } diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index a66afad87..be74751d5 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -14,11 +14,6 @@ namespace gtsam { cout << s << "(" << x_ << ", " << y_ << ", " << theta_ << ")" << endl; } - /* ************************************************************************* */ - Pose2 Pose2::exmap(const Vector& v) const { - return Pose2(x_ + v(0), y_ + v(1), theta_ + v(2)); - } - /* ************************************************************************* */ bool Pose2::equals(const Pose2& q, double tol) const { return (fabs(x_ - q.x_) < tol && fabs(y_ - q.y_) < tol && fabs(theta_ @@ -26,12 +21,8 @@ namespace gtsam { } /* ************************************************************************* */ - bool assert_equal(const Pose2& A, const Pose2& B, double tol) { - if (A.equals(B, tol)) return true; - printf("not equal:\n"); - A.print("A"); - B.print("B"); - return false; + Pose2 Pose2::exmap(const Vector& v) const { + return Pose2(x_ + v(0), y_ + v(1), theta_ + v(2)); } /* ************************************************************************* */ diff --git a/cpp/Pose2.h b/cpp/Pose2.h index d3d267052..ccc8c7709 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -9,13 +9,14 @@ #pragma once #include "Point2.h" +#include "Testable.h" namespace gtsam { /** * A 2D pose (x,y,theta) */ - class Pose2 { + class Pose2: Testable { private: double x_, y_, theta_; @@ -47,21 +48,18 @@ namespace gtsam { x_(t.x()), y_(t.y()), theta_(theta) { } + /** print with optional string */ + void print(const std::string& s = "") const; + + /** assert equality up to a tolerance */ + bool equals(const Pose2& pose, double tol = 1e-9) const; + /** get functions for x, y, theta */ double x() const { return x_;} double y() const { return y_;} double theta() const { return theta_;} - /** print with optional string */ - void print(const std::string& s = "") const; - Pose2 exmap(const Vector& v) const; - - /** assert equality up to a tolerance */ - bool equals(const Pose2& pose, double tol = 1e-9) const; }; - /** assert equality up to a tolerance */ - bool assert_equal(const Pose2& A, const Pose2& B, double tol = 1e-9); - } // namespace gtsam diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index 69fa82847..0d3b4fc1f 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -9,6 +9,18 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +void Pose3::print(const string& s) const { + R_.print(s + ".R"); + t_.print(s + ".t"); +} + +/* ************************************************************************* */ +bool Pose3::equals(const Pose3& pose, double tol) const +{ + return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol); +} + /* ************************************************************************* */ // Agrawal06iros, formula (6), seems to suggest this could be wrong: Pose3 Pose3::exmap(const Vector& v) const { @@ -28,12 +40,6 @@ Matrix Pose3::matrix() const { return stack(2, &A34, &A14); } -/* ************************************************************************* */ -void Pose3::print(const string& s) const { - R_.print(s + ".R"); - t_.print(s + ".t"); -} - /* ************************************************************************* */ Point3 transform_from(const Pose3& pose, const Point3& p) { return pose.R_ * p + pose.t_; @@ -106,22 +112,6 @@ Pose3 Pose3::inverse() const return Pose3(Rt,-(Rt*t_)); } -/* ************************************************************************* */ -bool Pose3::equals(const Pose3& pose, double tol) const -{ - return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol); -} - -/* ************************************************************************* */ -bool assert_equal(const Pose3& A, const Pose3& B, double tol) -{ - if(A.equals(B,tol)) return true; - printf("not equal:\n"); - A.print("A"); - B.print("B"); - return false; -} - /* ************************************************************************* */ Pose3 Pose3::transformPose_to(const Pose3& pose) const { diff --git a/cpp/Pose3.h b/cpp/Pose3.h index e8eca75c5..b4c4bba53 100644 --- a/cpp/Pose3.h +++ b/cpp/Pose3.h @@ -9,11 +9,12 @@ #include "Point3.h" #include "Rot3.h" +#include "Testable.h" namespace gtsam { /** A 3D pose (R,t) : (Rot3,Point3) */ - class Pose3 { + class Pose3 : Testable { private: Rot3 R_; Point3 t_; @@ -51,6 +52,12 @@ namespace gtsam { t_(V(9), V(10),V(11)) { } + /** print with optional string */ + void print(const std::string& s = "") const; + + /** assert equality up to a tolerance */ + bool equals(const Pose3& pose, double tol = 1e-9) const; + const Rot3& rotation() const { return R_; } @@ -81,15 +88,9 @@ namespace gtsam { /** convert to 4*4 matrix */ Matrix matrix() const; - /** print with optional string */ - void print(const std::string& s = "") const; - /** transforms */ Pose3 transformPose_to(const Pose3& transform) const; - /** assert equality up to a tolerance */ - bool equals(const Pose3& pose, double tol = 1e-9) const; - /** friends */ friend Point3 transform_from(const Pose3& pose, const Point3& p); friend Point3 transform_to(const Pose3& pose, const Point3& p); @@ -131,7 +132,4 @@ namespace gtsam { */ Matrix DhPose(const Vector& x); - /** assert equality up to a tolerance */ - bool assert_equal(const Pose3& A, const Pose3& B, double tol = 1e-9); - } // namespace gtsam diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp index 7d410cfe5..3a365828c 100644 --- a/cpp/Rot3.cpp +++ b/cpp/Rot3.cpp @@ -12,6 +12,11 @@ using namespace std; namespace gtsam { + /* ************************************************************************* */ + bool Rot3::equals(const Rot3 & R, double tol) const { + return equal_with_abs_tol(matrix(), R.matrix(), tol); + } + /* ************************************************************************* */ /** faster than below ? */ /* ************************************************************************* */ @@ -77,11 +82,6 @@ namespace gtsam { return R.unrotate(p); } - /* ************************************************************************* */ - bool Rot3::equals(const Rot3 & R, double tol) const { - return equal_with_abs_tol(matrix(), R.matrix(), tol); - } - /* ************************************************************************* */ /** see libraries/caml/geometry/math.lyx, derivative of unrotate */ /* ************************************************************************* */ @@ -95,15 +95,6 @@ namespace gtsam { return R.transpose(); } - /* ************************************************************************* */ - bool assert_equal(const Rot3 & A, const Rot3 & B, double tol) { - if(A.equals(B,tol)) return true; - printf("not equal:\n"); - A.print("A"); - B.print("B"); - return false; - } - /* ************************************************************************* */ /** This function receives a rotation 3 by 3 matrix and returns 3 rotation angles. * The implementation is based on the algorithm in multiple view geometry diff --git a/cpp/Rot3.h b/cpp/Rot3.h index 49895e983..bc7ad3247 100644 --- a/cpp/Rot3.h +++ b/cpp/Rot3.h @@ -11,11 +11,12 @@ #pragma once #include "Point3.h" +#include "Testable.h" namespace gtsam { /* Rotation matrix */ - class Rot3{ + class Rot3: Testable { private: /** we store columns ! */ Point3 r1_, r2_, r3_; @@ -54,6 +55,12 @@ namespace gtsam { r2_(Point3(R(0,1), R(1,1), R(2,1))), r3_(Point3(R(0,2), R(1,2), R(2,2))) {} + /** print */ + void print(const std::string& s="R") const { gtsam::print(matrix(), s);} + + /** equals with an tolerance */ + bool equals(const Rot3& p, double tol = 1e-9) const; + /** return DOF, dimensionality of tangent space */ size_t dim() const { return 3;} @@ -121,12 +128,6 @@ namespace gtsam { ); } - /** print */ - void print(const std::string& s="R") const { gtsam::print(matrix(), s);} - - /** equals with an tolerance */ - bool equals(const Rot3& p, double tol = 1e-9) const; - /** friends */ friend Matrix Dunrotate1(const Rot3& R, const Point3& p); @@ -190,8 +191,6 @@ namespace gtsam { Matrix Dunrotate1(const Rot3& R, const Point3& p); Matrix Dunrotate2(const Rot3& R); // does not depend on p ! - bool assert_equal(const Rot3& A, const Rot3& B, double tol=1e-9); - /** receives a rotation 3 by 3 matrix and returns 3 rotation angles.*/ Vector RQ(Matrix R);