From a97502f5a11ccc4deff78396c19513cf72dea862 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 6 Jun 2016 21:57:52 -0700 Subject: [PATCH] Resolved all issues with typedefs --- examples/CameraResectioning.cpp | 3 +- examples/easyPoint2KalmanFilter.cpp | 12 +++--- gtsam/geometry/Cal3Bundler.cpp | 2 +- gtsam/geometry/Cal3DS2_Base.cpp | 2 +- gtsam/geometry/Point2.cpp | 12 +++--- gtsam/geometry/Point2.h | 8 ++-- gtsam/geometry/Point3.cpp | 8 ++-- gtsam/geometry/Point3.h | 10 ++--- gtsam/geometry/Pose2.cpp | 4 +- gtsam/geometry/Pose3.cpp | 2 +- gtsam/geometry/tests/testCal3Bundler.cpp | 2 +- gtsam/geometry/tests/testCal3DS2.cpp | 2 +- gtsam/geometry/tests/testCal3Unified.cpp | 2 +- gtsam/geometry/tests/testPinholeCamera.cpp | 2 +- gtsam/geometry/tests/testPinholePose.cpp | 2 +- gtsam/geometry/tests/testPoint2.cpp | 40 ++++++++++--------- gtsam/geometry/tests/testPoint3.cpp | 6 +-- gtsam/geometry/tests/testPose2.cpp | 4 +- gtsam/geometry/triangulation.h | 5 ++- gtsam/nonlinear/NonlinearEquality.h | 2 +- gtsam/nonlinear/tests/testExpression.cpp | 33 +++++++++++---- gtsam/slam/GeneralSFMFactor.h | 8 ++-- gtsam/slam/ProjectionFactor.h | 4 +- gtsam/slam/ReferenceFrameFactor.h | 4 +- gtsam/slam/SmartFactorBase.h | 2 +- gtsam/slam/TriangulationFactor.h | 18 ++++----- .../slam/tests/testEssentialMatrixFactor.cpp | 12 ++---- gtsam_unstable/dynamics/PoseRTV.cpp | 2 +- gtsam_unstable/geometry/Event.h | 2 +- gtsam_unstable/geometry/SimPolygon2D.cpp | 10 ++--- gtsam_unstable/geometry/SimWall2D.cpp | 19 ++++----- gtsam_unstable/geometry/SimWall2D.h | 2 +- gtsam_unstable/slam/InvDepthFactor3.h | 4 +- gtsam_unstable/slam/InvDepthFactorVariant1.h | 4 +- gtsam_unstable/slam/InvDepthFactorVariant2.h | 4 +- gtsam_unstable/slam/InvDepthFactorVariant3.h | 8 ++-- gtsam_unstable/slam/ProjectionFactorPPP.h | 4 +- gtsam_unstable/slam/ProjectionFactorPPPC.h | 4 +- gtsam_unstable/slam/SmartRangeFactor.h | 10 ++--- tests/simulated2DConstraints.h | 2 +- tests/testLie.cpp | 2 +- 41 files changed, 153 insertions(+), 135 deletions(-) diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 43814731f..676dd42ec 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -48,8 +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, boost::none, boost::none) - p_); - return reprojectionError.vector(); + return camera.project(P_, H, boost::none, boost::none) - p_; } }; diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index 201ec188b..d68cedb74 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -70,7 +70,7 @@ int main() { // Predict the new value with the EKF class Point2 x1_predict = ekf.predict(factor1); - x1_predict.print("X1 Predict"); + traits::Print(x1_predict, "X1 Predict"); @@ -91,7 +91,7 @@ int main() { // Update the Kalman Filter with the measurement Point2 x1_update = ekf.update(factor2); - x1_update.print("X1 Update"); + traits::Print(x1_update, "X1 Update"); @@ -101,13 +101,13 @@ int main() { difference = Point2(1,0); BetweenFactor factor3(x1, x2, difference, Q); Point2 x2_predict = ekf.predict(factor1); - x2_predict.print("X2 Predict"); + traits::Print(x2_predict, "X2 Predict"); // Update Point2 z2(2.0, 0.0); PriorFactor factor4(x2, z2, R); Point2 x2_update = ekf.update(factor4); - x2_update.print("X2 Update"); + traits::Print(x2_update, "X2 Update"); @@ -117,13 +117,13 @@ int main() { difference = Point2(1,0); BetweenFactor factor5(x2, x3, difference, Q); Point2 x3_predict = ekf.predict(factor5); - x3_predict.print("X3 Predict"); + traits::Print(x3_predict, "X3 Predict"); // Update Point2 z3(3.0, 0.0); PriorFactor factor6(x3, z3, R); Point2 x3_update = ekf.update(factor6); - x3_update.print("X3 Update"); + traits::Print(x3_update, "X3 Update"); return 0; } diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 2ee2e5581..4ad1dffa2 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -106,7 +106,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { const int maxIterations = 10; int iteration; for (iteration = 0; iteration < maxIterations; ++iteration) { - if (distance(uncalibrate(pn), pi) <= tol) + if (distance2(uncalibrate(pn), pi) <= tol) break; const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y; const double rr = xx + yy; diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index 232f442f6..2071b8792 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -144,7 +144,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { const int maxIterations = 10; int iteration; for (iteration = 0; iteration < maxIterations; ++iteration) { - if (distance(uncalibrate(pn), pi) <= tol) break; + if (distance2(uncalibrate(pn), pi) <= tol) break; const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y; const double rr = xx + yy; const double g = (1 + k1_ * rr + k2_ * rr * rr); diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index efe54766d..dbbe898ef 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -23,7 +23,7 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -double norm(const Point2& p, OptionalJacobian<1,2> H) { +double norm2(const Point2& p, OptionalJacobian<1,2> H) { double r = std::sqrt(p.x() * p.x() + p.y() * p.y()); if (H) { if (fabs(r) > 1e-10) @@ -35,17 +35,17 @@ double norm(const Point2& p, OptionalJacobian<1,2> H) { } /* ************************************************************************* */ -double distance(const Point2& p, const Point2& q, OptionalJacobian<1,2> H1, - OptionalJacobian<1,2> H2) { +double distance2(const Point2& p, const Point2& q, OptionalJacobian<1, 2> H1, + OptionalJacobian<1, 2> H2) { Point2 d = q - p; if (H1 || H2) { Matrix12 H; - double r = norm(d, H); + double r = norm2(d, H); if (H1) *H1 = -H; if (H2) *H2 = H; return r; } else { - return norm(d); + return d.norm(); } } @@ -130,7 +130,7 @@ list circleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol) { // distance between circle centers. - double d = distance(c1, c2); + double d = distance2(c1, c2); // centers coincide, either no solution or infinite number of solutions. if (d<1e-9) return list(); diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 1ae426afd..8d75c805b 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -166,12 +166,12 @@ struct traits : public internal::VectorSpace { #endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS /// Distance of the point from the origin, with Jacobian -double norm(const Point2& p, OptionalJacobian<1, 2> H = boost::none); +double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none); /// distance between two points -double distance(const Point2& p1, const Point2& q, - OptionalJacobian<1, 2> H1 = boost::none, - OptionalJacobian<1, 2> H2 = boost::none); +double distance2(const Point2& p1, const Point2& q, + OptionalJacobian<1, 2> H1 = boost::none, + OptionalJacobian<1, 2> H2 = boost::none); // Convenience typedef typedef std::pair Point2Pair; diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 99136e31d..d158c91f0 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -80,8 +80,8 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, #endif /* ************************************************************************* */ -double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, - OptionalJacobian<1, 3> H2) { +double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, + OptionalJacobian<1, 3> H2) { double d = (q - p1).norm(); if (H1) { *H1 << p1.x() - q.x(), p1.y() - q.y(), p1.z() - q.z(); @@ -94,7 +94,7 @@ double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, return d; } -double norm(const Point3 &p, OptionalJacobian<1, 3> H) { +double norm3(const Point3 &p, OptionalJacobian<1, 3> H) { double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z()); if (H) { if (fabs(r) > 1e-10) @@ -106,7 +106,7 @@ double norm(const Point3 &p, OptionalJacobian<1, 3> H) { } Point3 normalize(const Point3 &p, OptionalJacobian<3, 3> H) { - Point3 normalized = p / norm(p); + Point3 normalized = p / p.norm(); if (H) { // 3*3 Derivative double x2 = p.x() * p.x(), y2 = p.y() * p.y(), z2 = p.z() * p.z(); diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 31ce985c0..7787b346e 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -160,12 +160,12 @@ typedef std::pair Point3Pair; std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); /// distance between two points -double distance(const Point3& p1, const Point3& q, - OptionalJacobian<1, 3> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none); +double distance3(const Point3& p1, const Point3& q, + OptionalJacobian<1, 3> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none); /// Distance of the point from the origin, with Jacobian -double norm(const Point3& p, OptionalJacobian<1, 3> H = boost::none); +double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none); /// normalize, with optional Jacobian Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none); @@ -193,7 +193,7 @@ struct Range { double operator()(const Point3& p, const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H2 = boost::none) { - return distance(p, q, H1, H2); + return distance3(p, q, H1, H2); } }; diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index ec4c3dce4..2a52e98ba 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -249,7 +249,7 @@ double Pose2::range(const Point2& point, Point2 d = point - t_; if (!Hpose && !Hpoint) return d.norm(); Matrix12 D_r_d; - double r = norm(d, D_r_d); + double r = norm2(d, D_r_d); if (Hpose) { Matrix23 D_d_pose; D_d_pose << -r_.c(), r_.s(), 0.0, @@ -267,7 +267,7 @@ double Pose2::range(const Pose2& pose, Point2 d = pose.t() - t_; if (!Hpose && !Hother) return d.norm(); Matrix12 D_r_d; - double r = norm(d, D_r_d); + double r = norm2(d, D_r_d); if (Hpose) { Matrix23 D_d_pose; D_d_pose << diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 3138dfd64..25cd661e8 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -345,7 +345,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, return local.norm(); } else { Matrix13 D_r_local; - const double r = norm(local, D_r_local); + const double r = norm3(local, D_r_local); if (H1) *H1 = D_r_local * D_local_pose; if (H2) *H2 = D_r_local * D_local_point; return r; diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index e9eb689e1..8de049fa4 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -52,7 +52,7 @@ TEST( Cal3Bundler, calibrate ) Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); Point2 pn_hat = K.calibrate(pi); - CHECK( pn.equals(pn_hat, 1e-5)); + CHECK( traits::Equals(pn, pn_hat, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index c5a6be2d6..416665d46 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -48,7 +48,7 @@ TEST( Cal3DS2, calibrate ) Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); Point2 pn_hat = K.calibrate(pi); - CHECK( pn.equals(pn_hat, 1e-5)); + CHECK( traits::Equals(pn, pn_hat, 1e-5)); } Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); } diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp index de9a8b739..2c5ffd7fb 100644 --- a/gtsam/geometry/tests/testCal3Unified.cpp +++ b/gtsam/geometry/tests/testCal3Unified.cpp @@ -59,7 +59,7 @@ TEST( Cal3Unified, calibrate) { Point2 pi = K.uncalibrate(p); Point2 pn_hat = K.calibrate(pi); - CHECK( p.equals(pn_hat, 1e-8)); + CHECK( traits::Equals(p, pn_hat, 1e-8)); } Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); } diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 5cca1d723..a9b68bdec 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -278,7 +278,7 @@ TEST( PinholeCamera, range0) { double result = camera.range(point1, D1, D2); Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); - EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, + EXPECT_DOUBLES_EQUAL(distance3(point1, camera.pose().translation()), result, 1e-9); EXPECT(assert_equal(Hexpected1, D1, 1e-7)); EXPECT(assert_equal(Hexpected2, D2, 1e-7)); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 0caca3fda..ecbb92061 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -212,7 +212,7 @@ TEST( PinholePose, range0) { double result = camera.range(point1, D1, D2); Matrix expectedDcamera = numericalDerivative21(range0, camera, point1); Matrix expectedDpoint = numericalDerivative22(range0, camera, point1); - EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, 1e-9); + EXPECT_DOUBLES_EQUAL(distance3(point1, camera.pose().translation()), result, 1e-9); EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); } diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 04bab71ee..7ed8cfc6e 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -111,9 +111,9 @@ TEST( Point2, arithmetic) { /* ************************************************************************* */ TEST( Point2, unit) { Point2 p0(10, 0), p1(0, -10), p2(10, 10); - EXPECT(assert_equal(Point2(1, 0), p0.unit(), 1e-6)); - EXPECT(assert_equal(Point2(0,-1), p1.unit(), 1e-6)); - EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.unit(), 1e-6)); + EXPECT(assert_equal(Point2(1, 0), p0.normalized(), 1e-6)); + EXPECT(assert_equal(Point2(0,-1), p1.normalized(), 1e-6)); + EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.normalized(), 1e-6)); } namespace { @@ -131,19 +131,19 @@ TEST( Point2, norm ) { Point2 p0(cos(5.0), sin(5.0)); DOUBLES_EQUAL(1, p0.norm(), 1e-6); Point2 p1(4, 5), p2(1, 1); - DOUBLES_EQUAL( 5, p1.distance(p2), 1e-6); + DOUBLES_EQUAL( 5, distance2(p1, p2), 1e-6); DOUBLES_EQUAL( 5, (p2-p1).norm(), 1e-6); Matrix expectedH, actualH; double actual; // exception, for (0,0) derivative is [Inf,Inf] but we return [1,1] - actual = x1.norm(actualH); + actual = norm2(x1, actualH); EXPECT_DOUBLES_EQUAL(0, actual, 1e-9); expectedH = (Matrix(1, 2) << 1.0, 1.0).finished(); EXPECT(assert_equal(expectedH,actualH)); - actual = x2.norm(actualH); + actual = norm2(x2, actualH); EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual, 1e-9); expectedH = numericalDerivative11(norm_proxy, x2); EXPECT(assert_equal(expectedH,actualH)); @@ -156,20 +156,20 @@ TEST( Point2, norm ) { /* ************************************************************************* */ namespace { double distance_proxy(const Point2& location, const Point2& point) { - return location.distance(point); + return distance2(location, point); } } TEST( Point2, distance ) { Matrix expectedH1, actualH1, expectedH2, actualH2; // establish distance is indeed zero - EXPECT_DOUBLES_EQUAL(1, x1.distance(l1), 1e-9); + EXPECT_DOUBLES_EQUAL(1, distance2(x1, l1), 1e-9); // establish distance is indeed 45 degrees - EXPECT_DOUBLES_EQUAL(sqrt(2.0), x1.distance(l2), 1e-9); + EXPECT_DOUBLES_EQUAL(sqrt(2.0), distance2(x1, l2), 1e-9); // Another pair - double actual23 = x2.distance(l3, actualH1, actualH2); + double actual23 = distance2(x2, l3, actualH1, actualH2); EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual23, 1e-9); // Check numerical derivatives @@ -179,7 +179,7 @@ TEST( Point2, distance ) { EXPECT(assert_equal(expectedH2,actualH2)); // Another test - double actual34 = x3.distance(l4, actualH1, actualH2); + double actual34 = distance2(x3, l4, actualH1, actualH2); EXPECT_DOUBLES_EQUAL(2, actual34, 1e-9); // Check numerical derivatives @@ -195,42 +195,42 @@ TEST( Point2, circleCircleIntersection) { double offset = 0.994987; // Test intersections of circle moving from inside to outside - list inside = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,0),1); + list inside = circleCircleIntersection(Point2(0,0),5,Point2(0,0),1); EXPECT_LONGS_EQUAL(0,inside.size()); - list touching1 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(4,0),1); + list touching1 = circleCircleIntersection(Point2(0,0),5,Point2(4,0),1); EXPECT_LONGS_EQUAL(1,touching1.size()); EXPECT(assert_equal(Point2(5,0), touching1.front())); - list common = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(5,0),1); + list common = circleCircleIntersection(Point2(0,0),5,Point2(5,0),1); EXPECT_LONGS_EQUAL(2,common.size()); EXPECT(assert_equal(Point2(4.9, offset), common.front(), 1e-6)); EXPECT(assert_equal(Point2(4.9, -offset), common.back(), 1e-6)); - list touching2 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(6,0),1); + list touching2 = circleCircleIntersection(Point2(0,0),5,Point2(6,0),1); EXPECT_LONGS_EQUAL(1,touching2.size()); EXPECT(assert_equal(Point2(5,0), touching2.front())); // test rotated case - list rotated = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,5),1); + list rotated = circleCircleIntersection(Point2(0,0),5,Point2(0,5),1); EXPECT_LONGS_EQUAL(2,rotated.size()); EXPECT(assert_equal(Point2(-offset, 4.9), rotated.front(), 1e-6)); EXPECT(assert_equal(Point2( offset, 4.9), rotated.back(), 1e-6)); // test r1 smaller = Point2::CircleCircleIntersection(Point2(0,0),1,Point2(5,0),5); + list smaller = circleCircleIntersection(Point2(0,0),1,Point2(5,0),5); EXPECT_LONGS_EQUAL(2,smaller.size()); EXPECT(assert_equal(Point2(0.1, offset), smaller.front(), 1e-6)); EXPECT(assert_equal(Point2(0.1, -offset), smaller.back(), 1e-6)); // test offset case, r1>r2 - list offset1 = Point2::CircleCircleIntersection(Point2(1,1),5,Point2(6,1),1); + list offset1 = circleCircleIntersection(Point2(1,1),5,Point2(6,1),1); EXPECT_LONGS_EQUAL(2,offset1.size()); EXPECT(assert_equal(Point2(5.9, 1+offset), offset1.front(), 1e-6)); EXPECT(assert_equal(Point2(5.9, 1-offset), offset1.back(), 1e-6)); // test offset case, r1 offset2 = Point2::CircleCircleIntersection(Point2(6,1),1,Point2(1,1),5); + list offset2 = circleCircleIntersection(Point2(6,1),1,Point2(1,1),5); EXPECT_LONGS_EQUAL(2,offset2.size()); EXPECT(assert_equal(Point2(5.9, 1-offset), offset2.front(), 1e-6)); EXPECT(assert_equal(Point2(5.9, 1+offset), offset2.back(), 1e-6)); @@ -238,12 +238,14 @@ TEST( Point2, circleCircleIntersection) { } /* ************************************************************************* */ +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS TEST( Point2, stream) { Point2 p(1, 2); std::ostringstream os; os << p; EXPECT(os.str() == "(1, 2)"); } +#endif /* ************************************************************************* */ int main () { diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 405f52521..8dde2b5aa 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -183,20 +183,20 @@ TEST (Point3, norm) { Matrix actualH; Point3 point(3,4,5); // arbitrary point double expected = sqrt(50); - EXPECT_DOUBLES_EQUAL(expected, norm(point, actualH), 1e-8); + EXPECT_DOUBLES_EQUAL(expected, norm3(point, actualH), 1e-8); Matrix expectedH = numericalDerivative11(norm_proxy, point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } /* ************************************************************************* */ double testFunc(const Point3& P, const Point3& Q) { - return distance(P,Q); + return distance3(P, Q); } TEST (Point3, distance) { Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3); Matrix H1, H2; - double d = distance(P, Q, H1, H2); + double d = distance3(P, Q, H1, H2); double expectedDistance = 55.542686; Matrix numH1 = numericalDerivative21(testFunc, P, Q); Matrix numH2 = numericalDerivative22(testFunc, P, Q); diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index ca716cb80..10fd431bc 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -743,7 +743,7 @@ namespace { /* ************************************************************************* */ struct Triangle { size_t i_,j_,k_;}; - boost::optional align(const vector& ps, const vector& qs, + boost::optional align2(const vector& ps, const vector& qs, const pair& trianglePair) { const Triangle& t1 = trianglePair.first, t2 = trianglePair.second; vector correspondences; @@ -762,7 +762,7 @@ TEST(Pose2, align_4) { Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2; Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0; - boost::optional actual = align(ps, qs, make_pair(t1,t2)); + boost::optional actual = align2(ps, qs, make_pair(t1,t2)); EXPECT(assert_equal(expected, *actual)); } diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 744fd9e8b..880c23fd1 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -112,7 +112,8 @@ std::pair triangulationGraph( Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; - static SharedNoiseModel unit(noiseModel::Unit::Create(CAMERA::Measurement::dimension)); + static SharedNoiseModel unit(noiseModel::Unit::Create( + traits::dimension)); for (size_t i = 0; i < measurements.size(); i++) { const CAMERA& camera_i = cameras[i]; graph.push_back(TriangulationFactor // @@ -457,7 +458,7 @@ TriangulationResult triangulateSafe(const std::vector& cameras, for(const CAMERA& camera: cameras) { const Pose3& pose = camera.pose(); if (params.landmarkDistanceThreshold > 0 - && distance(pose.translation(), point) + && distance3(pose.translation(), point) > params.landmarkDistanceThreshold) return TriangulationResult::Degenerate(); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 0d8d717bc..c6aa52ab2 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -260,7 +260,7 @@ public: std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key()) << ")," << "\n"; this->noiseModel_->print(); - value_.print("Value"); + traits::Print(value_, "Value"); } private: diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 6e4a1b58c..db98971ff 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -111,24 +111,43 @@ TEST(Expression, Unary3) { } /* ************************************************************************* */ +class Class : public Point3 { + public: + enum {dimension = 3}; + using Point3::Point3; + const Vector3& vector() const { return *this; } + inline static Class identity() { return Class(0,0,0); } + double norm(OptionalJacobian<1,3> H = boost::none) const { + return norm3(*this, H); + } + bool equals(const Class &q, double tol) const { + return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && fabs(z() - q.z()) < tol); + } + void print(const string& s) const { cout << s << *this << endl;} +}; + +namespace gtsam { +template<> struct traits : public internal::VectorSpace {}; +} + // Nullary Method TEST(Expression, NullaryMethod) { // Create expression - Expression p(67); - Expression norm(>sam::norm, p); + Expression p(67); + Expression norm_(p, &Class::norm); // Create Values Values values; - values.insert(67, Point3(3, 4, 5)); + values.insert(67, Class(3, 4, 5)); // Check dims as map std::map map; - norm.dims(map); + norm_.dims(map); LONGS_EQUAL(1, map.size()); // Get value and Jacobians std::vector H(1); - double actual = norm.value(values, H); + double actual = norm_.value(values, H); // Check all EXPECT(actual == sqrt(50)); @@ -370,7 +389,7 @@ TEST(Expression, TripleSum) { /* ************************************************************************* */ TEST(Expression, SumOfUnaries) { const Key key(67); - const Double_ norm_(>sam::norm, Point3_(key)); + const Double_ norm_(>sam::norm3, Point3_(key)); const Double_ sum_ = norm_ + norm_; Values values; @@ -389,7 +408,7 @@ TEST(Expression, SumOfUnaries) { TEST(Expression, UnaryOfSum) { const Key key1(42), key2(67); const Point3_ sum_ = Point3_(key1) + Point3_(key2); - const Double_ norm_(>sam::norm, sum_); + const Double_ norm_(>sam::norm3, sum_); map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); norm_.dims(actual_dims); diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index d49d2ec48..9356aceb2 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -107,7 +107,7 @@ public: */ void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /** @@ -115,7 +115,7 @@ public: */ bool equals(const NonlinearFactor &p, double tol = 1e-9) const { const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol); + return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** h(x)-z */ @@ -241,7 +241,7 @@ public: */ void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /** @@ -249,7 +249,7 @@ public: */ bool equals(const NonlinearFactor &p, double tol = 1e-9) const { const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol); + return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** h(x)-z */ diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 481d5e541..d13c28e11 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -110,7 +110,7 @@ namespace gtsam { */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "GenericProjectionFactor, z = "; - measured_.print(); + traits::Print(measured_); if(this->body_P_sensor_) this->body_P_sensor_->print(" sensor pose in body frame: "); Base::print("", keyFormatter); @@ -121,7 +121,7 @@ namespace gtsam { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); } diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 966ade343..68b42f210 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -84,7 +84,7 @@ public: * each degree of freedom. */ ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2) - : Base(noiseModel::Isotropic::Sigma(POINT::dimension, sigma), + : Base(noiseModel::Isotropic::Sigma(traits::dimension, sigma), globalKey, transKey, localKey) {} virtual ~ReferenceFrameFactor(){} @@ -100,7 +100,7 @@ public: boost::optional Dlocal = boost::none) const { Point newlocal = transform_point(trans, global, Dtrans, Dforeign); if (Dlocal) - *Dlocal = -1* Matrix::Identity(Point::dimension,Point::dimension); + *Dlocal = -1* Matrix::Identity(traits::dimension, traits::dimension); return traits::Local(local,newlocal); } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 3128390c7..93d2ff218 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -189,7 +189,7 @@ public: bool areMeasurementsEqual = true; for (size_t i = 0; i < measured_.size(); i++) { - if (this->measured_.at(i).equals(e->measured_.at(i), tol) == false) + if (traits::Equals(this->measured_.at(i), e->measured_.at(i), tol) == false) areMeasurementsEqual = false; break; } diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index bbb877b59..811d92fbc 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -78,10 +78,10 @@ public: bool verboseCheirality = false) : Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_( throwCheirality), verboseCheirality_(verboseCheirality) { - if (model && model->dim() != Measurement::dimension) + if (model && model->dim() != traits::dimension) throw std::invalid_argument( "TriangulationFactor must be created with " - + boost::lexical_cast((int) Measurement::dimension) + + boost::lexical_cast((int) traits::dimension) + "-dimensional noise model."); } @@ -104,7 +104,7 @@ public: DefaultKeyFormatter) const { std::cout << s << "TriangulationFactor,"; camera_.print("camera"); - measured_.print("z"); + traits::Print(measured_, "z"); Base::print("", keyFormatter); } @@ -112,7 +112,7 @@ public: virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol) - && this->measured_.equals(e->measured_, tol); + && traits::Equals(this->measured_, e->measured_, tol); } /// Evaluate error h(x)-z and optionally derivatives @@ -122,14 +122,14 @@ public: return traits::Local(measured_, camera_.project2(point, boost::none, H2)); } catch (CheiralityException& e) { if (H2) - *H2 = Matrix::Zero(Measurement::dimension, 3); + *H2 = Matrix::Zero(traits::dimension, 3); if (verboseCheirality_) std::cout << e.what() << ": Landmark " << DefaultKeyFormatter(this->key()) << " moved behind camera" << std::endl; if (throwCheirality_) throw e; - return Eigen::Matrix::Constant(2.0 * camera_.calibration().fx()); + return Eigen::Matrix::dimension,1>::Constant(2.0 * camera_.calibration().fx()); } } @@ -151,9 +151,9 @@ public: // Allocate memory for Jacobian factor, do only once if (Ab.rows() == 0) { std::vector dimensions(1, 3); - Ab = VerticalBlockMatrix(dimensions, Measurement::dimension, true); - A.resize(Measurement::dimension,3); - b.resize(Measurement::dimension); + Ab = VerticalBlockMatrix(dimensions, traits::dimension, true); + A.resize(traits::dimension,3); + b.resize(traits::dimension); } // Would be even better if we could pass blocks to project diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 8ac6c48b6..2e3d613d6 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -222,8 +222,7 @@ TEST (EssentialMatrixFactor2, factor) { // Check evaluation Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1); const Point2 pi = PinholeBase::Project(P2); - Point2 reprojectionError(pi - pB(i)); - Vector expected = reprojectionError.vector(); + Point2 expected(pi - pB(i)); Matrix Hactual1, Hactual2; double d(baseline / P1.z()); @@ -296,8 +295,7 @@ TEST (EssentialMatrixFactor3, factor) { // Check evaluation Point3 P1 = data.tracks[i].p; const Point2 pi = camera2.project(P1); - Point2 reprojectionError(pi - pB(i)); - Vector expected = reprojectionError.vector(); + Point2 expected(pi - pB(i)); Matrix Hactual1, Hactual2; double d(baseline / P1.z()); @@ -438,8 +436,7 @@ TEST (EssentialMatrixFactor2, extraTest) { // Check evaluation Point3 P1 = data.tracks[i].p; const Point2 pi = camera2.project(P1); - Point2 reprojectionError(pi - pB(i)); - Vector expected = reprojectionError.vector(); + Point2 expected(pi - pB(i)); Matrix Hactual1, Hactual2; double d(baseline / P1.z()); @@ -507,8 +504,7 @@ TEST (EssentialMatrixFactor3, extraTest) { // Check evaluation Point3 P1 = data.tracks[i].p; const Point2 pi = camera2.project(P1); - Point2 reprojectionError(pi - pB(i)); - Vector expected = reprojectionError.vector(); + Point2 expected(pi - pB(i)); Matrix Hactual1, Hactual2; double d(baseline / P1.z()); diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index c1afe3882..c8c46ee7b 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -172,7 +172,7 @@ double PoseRTV::range(const PoseRTV& other, const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0); const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0); Matrix13 D_d_t1, D_d_t2; - double d = distance(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); + double d = distance3(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0; if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0; return d; diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 615b1d467..40c70696b 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -85,7 +85,7 @@ public: OptionalJacobian<1, 3> H2 = boost::none) const { static const double Speed = 330; Matrix13 D1, D2; - double distance = gtsam::distance(location_, microphone, D1, D2); + double distance = gtsam::distance3(location_, microphone, D1, D2); if (H1) // derivative of toa with respect to event *H1 << 1.0, D1 / Speed; diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index 610fddfbf..ba1445b20 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -46,7 +46,7 @@ SimPolygon2D SimPolygon2D::createRectangle(const Point2& p, double height, doubl bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const { if (p.size() != size()) return false; for (size_t i=0; i::Equals(landmarks_[i], p.landmarks_[i], tol)) return false; return true; } @@ -55,7 +55,7 @@ bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const { void SimPolygon2D::print(const string& s) const { cout << "SimPolygon " << s << ": " << endl; for(const Point2& p: landmarks_) - p.print(" "); + traits::Print(p, " "); } /* ************************************************************************* */ @@ -151,7 +151,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( Pose2 xC = xB.retract(Vector::Unit(3,0)*dBC); // use triangle equality to verify non-degenerate triangle - double dAC = xA.t().distance(xC.t()); + double dAC = distance2(xA.t(), xC.t()); // form a triangle and test if it meets requirements SimPolygon2D test_tri = SimPolygon2D::createTriangle(xA.t(), xB.t(), xC.t()); @@ -164,7 +164,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( insideBox(side_len, test_tri.landmark(0)) && insideBox(side_len, test_tri.landmark(1)) && insideBox(side_len, test_tri.landmark(2)) && - test_tri.landmark(1).distance(test_tri.landmark(2)) > min_side_len && + distance2(test_tri.landmark(1), test_tri.landmark(2)) > min_side_len && !nearExisting(lms, test_tri.landmark(0), min_vertex_dist) && !nearExisting(lms, test_tri.landmark(1), min_vertex_dist) && !nearExisting(lms, test_tri.landmark(2), min_vertex_dist) && @@ -320,7 +320,7 @@ bool SimPolygon2D::insideBox(double s, const Point2& p) { bool SimPolygon2D::nearExisting(const std::vector& S, const Point2& p, double threshold) { for(const Point2& Sp: S) - if (Sp.distance(p) < threshold) + if (distance2(Sp, p) < threshold) return true; return false; } diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp index fb8270dd4..111d23b91 100644 --- a/gtsam_unstable/geometry/SimWall2D.cpp +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -14,13 +14,14 @@ using namespace std; /* ************************************************************************* */ void SimWall2D::print(const std::string& s) const { std::cout << "SimWall2D " << s << ":" << std::endl; - a_.print(" a"); - b_.print(" b"); + traits::Print(a_, " a"); + traits::Print(b_, " b"); } /* ************************************************************************* */ bool SimWall2D::equals(const SimWall2D& other, double tol) const { - return a_.equals(other.a_, tol) && b_.equals(other.b_, tol); + return traits::Equals(a_, other.a_, tol) && + traits::Equals(b_, other.b_, tol); } /* ************************************************************************* */ @@ -37,8 +38,8 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons if (debug) cout << "len: " << len << endl; Point2 Ba = transform.transform_to(B.a()), Bb = transform.transform_to(B.b()); - if (debug) Ba.print("Ba"); - if (debug) Bb.print("Bb"); + if (debug) traits::Print(Ba, "Ba"); + if (debug) traits::Print(Bb, "Bb"); // check sides of line if (Ba.y() * Bb.y() > 0.0 || @@ -81,8 +82,8 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons high = Bb; low = Ba; } - if (debug) high.print("high"); - if (debug) low.print("low"); + if (debug) traits::Print(high, "high"); + if (debug) traits::Print(low, "low"); // find x-intercept double slope = (high.y() - low.y())/(high.x() - low.x()); @@ -138,7 +139,7 @@ std::pair moveWithBounce(const Pose2& cur_pose, double step_size, Point2 cur_intersection; if (wall.intersects(traj,cur_intersection)) { collision = true; - if (cur_pose.t().distance(cur_intersection) < cur_pose.t().distance(intersection)) { + if (distance2(cur_pose.t(), cur_intersection) < distance2(cur_pose.t(), intersection)) { intersection = cur_intersection; closest_wall = wall; } @@ -154,7 +155,7 @@ std::pair moveWithBounce(const Pose2& cur_pose, double step_size, norm = norm / norm.norm(); // Simple check to make sure norm is on side closest robot - if (cur_pose.t().distance(intersection + norm) > cur_pose.t().distance(intersection - norm)) + if (distance2(cur_pose.t(), intersection + norm) > distance2(cur_pose.t(), intersection - norm)) norm = - norm; // using the reflection diff --git a/gtsam_unstable/geometry/SimWall2D.h b/gtsam_unstable/geometry/SimWall2D.h index 38bba2ee3..c143bc36d 100644 --- a/gtsam_unstable/geometry/SimWall2D.h +++ b/gtsam_unstable/geometry/SimWall2D.h @@ -43,7 +43,7 @@ namespace gtsam { SimWall2D scale(double s) const { return SimWall2D(s*a_, s*b_); } /** geometry */ - double length() const { return a_.distance(b_); } + double length() const { return distance2(a_, b_); } Point2 midpoint() const; /** diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index f9874e065..0d10b0123 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -72,13 +72,13 @@ public: void print(const std::string& s = "InvDepthFactor3", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /// equals virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol); + return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } /// Evaluate error h(x)-z and optionally derivatives diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 30323a545..ad66eee5b 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -68,7 +68,7 @@ public: void print(const std::string& s = "InvDepthFactorVariant1", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /// equals @@ -76,7 +76,7 @@ public: const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index d3bc7a740..c6b5d5af8 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -71,7 +71,7 @@ public: void print(const std::string& s = "InvDepthFactorVariant2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /// equals @@ -79,7 +79,7 @@ public: const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol) && traits::Equals(this->referencePoint_, e->referencePoint_, tol); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index e15e77a5f..3041f5f23 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -70,7 +70,7 @@ public: void print(const std::string& s = "InvDepthFactorVariant3a", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /// equals @@ -78,7 +78,7 @@ public: const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } @@ -190,7 +190,7 @@ public: void print(const std::string& s = "InvDepthFactorVariant3", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /// equals @@ -198,7 +198,7 @@ public: const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 160a4fbf8..19b8d56e6 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -107,7 +107,7 @@ namespace gtsam { */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "ProjectionFactorPPP, z = "; - measured_.print(); + traits::Print(measured_); Base::print("", keyFormatter); } @@ -116,7 +116,7 @@ namespace gtsam { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index e20c2ba9b..369075abf 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -102,7 +102,7 @@ namespace gtsam { */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "ProjectionFactorPPPC, z = "; - measured_.print(); + traits::Print(measured_); Base::print("", keyFormatter); } @@ -111,7 +111,7 @@ namespace gtsam { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol); + && traits::Equals(this->measured_, e->measured_, tol); } /// Evaluate error h(x)-z and optionally derivatives diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 7989e2775..5e107ea58 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -101,11 +101,11 @@ public: // loop over all circles for(const Circle2& it: circles) { // distance between circle centers. - double d = circle1.center.distance(it.center); + double d = distance2(circle1.center, it.center); if (d < 1e-9) continue; // skip circles that are in the same location // Find f and h, the intersection points in normalized circles - boost::optional fh = Point2::CircleCircleIntersection( + boost::optional fh = circleCircleIntersection( circle1.radius / d, it.radius / d); // Check if this pair is better by checking h = fh->y() // if h is large, the intersections are well defined. @@ -116,15 +116,15 @@ public: } // use best fh to find actual intersection points - std::list intersections = Point2::CircleCircleIntersection( + std::list intersections = circleCircleIntersection( circle1.center, best_circle->center, best_fh); // pick winner based on other measurements double error1 = 0, error2 = 0; Point2 p1 = intersections.front(), p2 = intersections.back(); for(const Circle2& it: circles) { - error1 += it.center.distance(p1); - error2 += it.center.distance(p2); + error1 += distance2(it.center, p1); + error2 += distance2(it.center, p2); } return (error1 < error2) ? p1 : p2; //gttoc_(triangulate); diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index b698cfeaa..7d399dc02 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -111,7 +111,7 @@ namespace simulated2D { * @return a scalar distance between values */ template - double range_trait(const T1& a, const T2& b) { return a.distance(b); } + double range_trait(const T1& a, const T2& b) { return distance2(a, b); } /** * Binary inequality constraint forcing the range between points diff --git a/tests/testLie.cpp b/tests/testLie.cpp index c153adf5f..a134a899c 100644 --- a/tests/testLie.cpp +++ b/tests/testLie.cpp @@ -40,7 +40,7 @@ template<> struct traits : internal::LieGroupTraits { << m.second.theta() << ")" << endl; } static bool Equals(const Product& m1, const Product& m2, double tol = 1e-8) { - return m1.first.equals(m2.first, tol) && m1.second.equals(m2.second, tol); + return traits::Equals(m1.first, m2.first, tol) && m1.second.equals(m2.second, tol); } }; }