diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index b403d7e57..6a87d3d6b 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -172,6 +172,9 @@ namespace gtsam {
 	  // Calculate delta rotation = between(R1,R2)
 	  double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
 	  Rot2 R(Rot2::atan2(s,c)); // normalizes
+	  
+	  // Assert that R1 and R2 are normalized
+	  assert(fabs(c1*c1 + s1*s1 - 1.0) < 1e-5 && fabs(c2*c2 + s2*s2 - 1.0) < 1e-5);
 
 	  // Calculate delta translation = unrotate(R1, dt);
 	  Point2 dt = p2.t() - t_;
@@ -212,7 +215,12 @@ namespace gtsam {
 	  if (!H1 && !H2) return transform_to(point).norm();
 	  Point2 d = transform_to(point, H1, H2);
 	  double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
-	  Matrix D_result_d = Matrix_(1, 2, x / n, y / n);
+	  Matrix D_result_d;
+	  if(fabs(n) > 1e-5)
+	    D_result_d = Matrix_(1, 2, x / n, y / n);
+	  else {
+	    D_result_d = Matrix_(1,2, 1.0, 1.0);
+	  }
 	  if (H1) *H1 = D_result_d * (*H1);
 	  if (H2) *H2 = D_result_d * (*H2);
 	  return n;
diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp
index c30440525..f7aebbed7 100644
--- a/gtsam/geometry/Rot2.cpp
+++ b/gtsam/geometry/Rot2.cpp
@@ -96,8 +96,13 @@ Point2 Rot2::unrotate(const Point2& p,
 /* ************************************************************************* */
 Rot2 Rot2::relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
 	double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
-	if (H) *H = Matrix_(1, 2, -y / d2, x / d2);
-	return Rot2::fromCosSin(x / n, y / n);
+	if(fabs(n) > 1e-5) {
+	  if (H) *H = Matrix_(1, 2, -y / d2, x / d2);
+	  return Rot2::fromCosSin(x / n, y / n);
+	} else {
+	  if (H) *H = Matrix_(1,2, 0.0, 0.0);
+	  return Rot2();
+	}
 }
 
 /* ************************************************************************* */