diff --git a/cpp/Point2.cpp b/cpp/Point2.cpp
index 63a256fc0..9af53dda1 100644
--- a/cpp/Point2.cpp
+++ b/cpp/Point2.cpp
@@ -25,8 +25,8 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  double Point2::dist(const Point2& p2) const {
-    return sqrt(pow(x() - p2.x(), 2.0) + pow(y() - p2.y(), 2.0));
+  double Point2::norm() const {
+    return sqrt(x_*x_ + y_*y_);
   }
 
   /* ************************************************************************* */
diff --git a/cpp/Point2.h b/cpp/Point2.h
index 07a6fe9e6..c9c35dfe2 100644
--- a/cpp/Point2.h
+++ b/cpp/Point2.h
@@ -48,8 +48,13 @@ namespace gtsam {
     inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}
     inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);}
 
+    /** norm of point */
+    double norm() const;
+
     /** distance between two points */
-    double dist(const Point2& p2) const;
+    inline double dist(const Point2& p2) const {
+			return (p2 - *this).norm();
+		}
 
   private:
     /** Serialization function */
diff --git a/cpp/testPoint2.cpp b/cpp/testPoint2.cpp
index 9c1410b08..cccd6819c 100644
--- a/cpp/testPoint2.cpp
+++ b/cpp/testPoint2.cpp
@@ -11,22 +11,40 @@ using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
-TEST( Point2, expmap) {
-  Vector d(2);d(0)=1;d(1)=-1;
-  Point2 a(4,5), b=expmap(a,d),c(5,4);
-  CHECK(assert_equal(b,c));
+TEST( Point2, expmap)
+{
+	Vector d(2);
+	d(0) = 1;
+	d(1) = -1;
+	Point2 a(4, 5), b = expmap(a, d), c(5, 4);
+	CHECK(assert_equal(b,c));
 }
 
 /* ************************************************************************* */
-TEST( Point2, add) {
-  CHECK(assert_equal( Point2(4,5)+Point2(1,1), Point2(5,6) ));
+TEST( Point2, add)
+{
+	CHECK(assert_equal( Point2(4,5)+Point2(1,1), Point2(5,6) ));
 }
 
 /* ************************************************************************* */
-TEST( Point2, subtract) {
-  CHECK(assert_equal( Point2(4,5)-Point2(1,1), Point2(3,4) ));
+TEST( Point2, subtract)
+{
+	CHECK(assert_equal( Point2(4,5)-Point2(1,1), Point2(3,4) ));
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
+TEST( Point2, norm)
+{
+	Point2 p0(cos(5), sin(5));
+	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,(p2-p1).norm(),1e-6);
+}
+
+/* ************************************************************************* */
+int main() {
+	TestResult tr;
+	return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */