diff --git a/.cproject b/.cproject
index 6ae853633..9848b8ba2 100644
--- a/.cproject
+++ b/.cproject
@@ -375,6 +375,14 @@
true
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -401,7 +409,6 @@
make
-
tests/testBayesTree.run
true
false
@@ -409,7 +416,6 @@
make
-
testBinaryBayesNet.run
true
false
@@ -457,7 +463,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -465,7 +470,6 @@
make
-
tests/testSymbolicFactor.run
true
false
@@ -473,7 +477,6 @@
make
-
testSymbolicFactorGraph.run
true
false
@@ -489,20 +492,11 @@
make
-
tests/testBayesTree
true
false
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j2
@@ -529,6 +523,7 @@
make
+
testGraph.run
true
false
@@ -600,6 +595,7 @@
make
+
testInference.run
true
false
@@ -607,6 +603,7 @@
make
+
testGaussianFactor.run
true
false
@@ -614,6 +611,7 @@
make
+
testJunctionTree.run
true
false
@@ -621,6 +619,7 @@
make
+
testSymbolicBayesNet.run
true
false
@@ -628,6 +627,7 @@
make
+
testSymbolicFactorGraph.run
true
false
@@ -721,6 +721,14 @@
true
true
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
make
-j2
@@ -761,14 +769,6 @@
true
true
-
- make
- -j2
- check
- true
- true
- true
-
make
-j2
@@ -825,14 +825,6 @@
true
true
-
- make
- -j2
- vSFMexample.run
- true
- true
- true
-
make
-j2
@@ -841,6 +833,14 @@
true
true
+
+ make
+ -j2
+ vSFMexample.run
+ true
+ true
+ true
+
make
-j2
@@ -1083,7 +1083,6 @@
make
-
testErrors.run
true
false
@@ -1385,6 +1384,14 @@
true
true
+
+ make
+ -j2
+ tests/testPose2.run
+ true
+ true
+ true
+
make
-j2
@@ -1467,6 +1474,7 @@
make
+
testSimulated2DOriented.run
true
false
@@ -1506,6 +1514,7 @@
make
+
testSimulated2D.run
true
false
@@ -1513,6 +1522,7 @@
make
+
testSimulated3D.run
true
false
@@ -1704,6 +1714,7 @@
make
+
tests/testGaussianISAM2
true
false
@@ -1725,46 +1736,6 @@
true
true
-
- make
- -j2
- install
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- dist
- true
- true
- true
-
make
-j2
@@ -1861,6 +1832,54 @@
true
true
+
+ make
+ -j2
+ install
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ dist
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
make
-j2
@@ -1901,14 +1920,6 @@
true
true
-
- make
- -j2
- check
- true
- true
- true
-
diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 9d09c7947..0fc02ea95 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -210,7 +210,7 @@ namespace gtsam {
/* ************************************************************************* */
double Pose2::range(const Point2& point,
boost::optional H1, boost::optional H2) const {
- if (!H1 && !H2) return transform_to(point).norm();
+ 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;
@@ -224,6 +224,21 @@ namespace gtsam {
return n;
}
+ /* ************************************************************************* */
+ double Pose2::range(const Pose2& point,
+ boost::optional H1,
+ boost::optional H2) const {
+ double r = range(point.t(), H1, H2);
+ if (H2) {
+ // NOTE: expmap changes the orientation of expmap direction,
+ // so we must rotate the jacobian
+ Matrix H2_ = *H2 * point.r().matrix();
+ *H2 = zeros(1, 3);
+ insertSub(*H2, H2_, 0, 0);
+ }
+ return r;
+ }
+
/* *************************************************************************
* New explanation, from scan.ml
* It finds the angle using a linear method:
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 6e53ac4f3..549963856 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -166,6 +166,15 @@ namespace gtsam {
boost::optional H1=boost::none,
boost::optional H2=boost::none) const;
+ /**
+ * Calculate range to another pose
+ * @param point 2D location of other pose
+ * @return range (double)
+ */
+ double range(const Pose2& point,
+ boost::optional H1=boost::none,
+ boost::optional H2=boost::none) const;
+
/**
* Calculate Adjoint map
* Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp
index 200a2e2ff..d498bef1f 100644
--- a/gtsam/geometry/tests/testPose2.cpp
+++ b/gtsam/geometry/tests/testPose2.cpp
@@ -539,6 +539,43 @@ TEST( Pose2, range )
EXPECT(assert_equal(expectedH2,actualH2));
}
+/* ************************************************************************* */
+LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) {
+ return LieVector(pose.range(point));
+}
+TEST( Pose2, range_pose )
+{
+ Pose2 xl1(1, 0, M_PI_2), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI_2), xl4(1, 3, 0);
+
+ Matrix expectedH1, actualH1, expectedH2, actualH2;
+
+ // establish range is indeed zero
+ EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);
+
+ // establish range is indeed 45 degrees
+ EXPECT_DOUBLES_EQUAL(sqrt(2),x1.range(xl2),1e-9);
+
+ // Another pair
+ double actual23 = x2.range(xl3, actualH1, actualH2);
+ EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9);
+
+ // Check numerical derivatives
+ expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3);
+ expectedH2 = numericalDerivative22(range_pose_proxy, x2, xl3);
+ EXPECT(assert_equal(expectedH1,actualH1));
+ EXPECT(assert_equal(expectedH2,actualH2));
+
+ // Another test
+ double actual34 = x3.range(xl4, actualH1, actualH2);
+ EXPECT_DOUBLES_EQUAL(2,actual34,1e-9);
+
+ // Check numerical derivatives
+ expectedH1 = numericalDerivative21(range_pose_proxy, x3, xl4);
+ expectedH2 = numericalDerivative22(range_pose_proxy, x3, xl4);
+ EXPECT(assert_equal(expectedH1,actualH1));
+ EXPECT(assert_equal(expectedH2,actualH2));
+}
+
/* ************************************************************************* */
TEST(Pose2, align_1) {