diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index 6816346e8..1d5ad695a 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -83,7 +83,7 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const { const int maxIterations = 10; int iteration; for ( iteration = 0 ; iteration < maxIterations ; ++iteration ) { - if ( uncalibrate(pn).dist(pi) <= tol ) break; + if ( uncalibrate(pn).distance(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/Point3.h b/gtsam/geometry/Point3.h index 7e0c59ba9..4713dacab 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -153,8 +153,13 @@ namespace gtsam { Point3 operator / (double s) const; /** distance between two points */ - double dist(const Point3& p2) const { - return std::sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0)); + inline double distance(const Point3& p2) const { + return (p2 - *this).norm(); + } + + /** @deprecated The following function has been deprecated, use distance above */ + inline double dist(const Point3& p2) const { + return (p2 - *this).norm(); } /** Distance of the point from the origin */ diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 446255803..7c73b28a2 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -75,19 +75,28 @@ 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.dist(p2),1e-6); + DOUBLES_EQUAL( 5,p1.distance(p2),1e-6); DOUBLES_EQUAL( 5,(p2-p1).norm(),1e-6); } /* ************************************************************************* */ TEST( Point2, unit) { - Point2 p0(10.0, 0.0), p1(0.0,-10.0), p2(10.0, 10.0); - EXPECT(assert_equal(Point2(1.0, 0.0), p0.unit(), 1e-6)); - EXPECT(assert_equal(Point2(0.0,-1.0), p1.unit(), 1e-6)); + 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)); } +/* ************************************************************************* */ +TEST( Point2, stream) +{ + Point2 p(1,2); + std::ostringstream os; + os << p; + EXPECT(os.str() == "(1, 2)"); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 5a442560e..dd2671530 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -232,7 +232,7 @@ double PoseRTV::range(const PoseRTV& other, boost::optional H1, boost::optional H2) const { if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5); if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5); - return t().dist(other.t()); + return t().distance(other.t()); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index bc22cb024..85b592b98 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -152,7 +152,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( Pose2 xC = xB.retract(delta(3, 0, dBC)); // use triangle equality to verify non-degenerate triangle - double dAC = xA.t().dist(xC.t()); + double dAC = xA.t().distance(xC.t()); // form a triangle and test if it meets requirements SimPolygon2D test_tri = SimPolygon2D::createTriangle(xA.t(), xB.t(), xC.t()); @@ -165,7 +165,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).dist(test_tri.landmark(2)) > min_side_len && + test_tri.landmark(1).distance(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) && @@ -321,7 +321,7 @@ bool SimPolygon2D::insideBox(double s, const Point2& p) { bool SimPolygon2D::nearExisting(const std::vector& S, const Point2& p, double threshold) { BOOST_FOREACH(const Point2& Sp, S) - if (Sp.dist(p) < threshold) + if (Sp.distance(p) < threshold) return true; return false; } diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp index b03f52890..78347a077 100644 --- a/gtsam_unstable/geometry/SimWall2D.cpp +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -139,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().dist(cur_intersection) < cur_pose.t().dist(intersection)) { + if (cur_pose.t().distance(cur_intersection) < cur_pose.t().distance(intersection)) { intersection = cur_intersection; closest_wall = wall; } @@ -155,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().dist(intersection + norm) > cur_pose.t().dist(intersection - norm)) + if (cur_pose.t().distance(intersection + norm) > cur_pose.t().distance(intersection - norm)) norm = norm.inverse(); // using the reflection diff --git a/gtsam_unstable/geometry/SimWall2D.h b/gtsam_unstable/geometry/SimWall2D.h index d3e86296a..cf2142af8 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_.dist(b_); } + double length() const { return a_.distance(b_); } Point2 midpoint() const; /**