diff --git a/.cproject b/.cproject
index 4dc49b994..b9fdfae51 100644
--- a/.cproject
+++ b/.cproject
@@ -2999,6 +2999,14 @@
true
true
+
+ make
+ -j4
+ testBetweenFactor.run
+ true
+ true
+ true
+
make
-j2
diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index 4c0333f69..0d8432d51 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -38,6 +38,13 @@ static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
static Point3 P(0.2, 0.7, -2.0);
static double error = 1e-9, epsilon = 0.001;
+//******************************************************************************
+TEST(Rot3 , Concept) {
+ BOOST_CONCEPT_ASSERT((IsGroup));
+ BOOST_CONCEPT_ASSERT((IsManifold));
+ BOOST_CONCEPT_ASSERT((IsLieGroup));
+}
+
/* ************************************************************************* */
TEST( Rot3, chart)
{
@@ -601,6 +608,45 @@ TEST( Rot3, slerp)
EXPECT(assert_equal(R1, R1.slerp(0.5,R1)));
}
+//******************************************************************************
+TEST(Rot3 , Traits) {
+ Rot3 R1(Rot3::rodriguez(Vector3(0, 0, 1), 1));
+ Rot3 R2(Rot3::rodriguez(Vector3(0, 1, 0), 2));
+ check_group_invariants(R1, R2);
+ check_manifold_invariants(R1, R2);
+
+ Rot3 expected, actual;
+ Matrix actualH1, actualH2;
+ Matrix numericalH1, numericalH2;
+
+ expected = R1 * R2;
+ actual = traits_x::Compose(R1, R2, actualH1, actualH2);
+ EXPECT(assert_equal(expected,actual));
+
+ numericalH1 = numericalDerivative21(traits_x::Compose, R1, R2);
+ EXPECT(assert_equal(numericalH1,actualH1));
+
+ numericalH2 = numericalDerivative22(traits_x::Compose, R1, R2);
+ EXPECT(assert_equal(numericalH2,actualH2));
+
+ expected = R1.inverse() * R2;
+ actual = traits_x::Between(R1, R2, actualH1, actualH2);
+ EXPECT(assert_equal(expected,actual));
+
+ numericalH1 = numericalDerivative21(traits_x::Between, R1, R2);
+ EXPECT(assert_equal(numericalH1,actualH1));
+
+ numericalH2 = numericalDerivative22(traits_x::Between, R1, R2);
+ EXPECT(assert_equal(numericalH2,actualH2));
+
+ expected = R1.inverse();
+ actual = traits_x::Inverse(R1, actualH1);
+ EXPECT(assert_equal(expected,actual));
+
+ numericalH1 = numericalDerivative11(traits_x::Inverse, R1);
+ EXPECT(assert_equal(numericalH1,actualH1));
+}
+
/* ************************************************************************* */
int main() {
TestResult tr;
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index 5889f6d03..ccb981de7 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -93,12 +93,18 @@ namespace gtsam {
boost::none) const {
T hx = traits_x::Between(p1, p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
- typename traits_x::ChartJacobian Hlocal;
- //OptionalJacobian::dimension, traits_x::dimension> Hlocal;
- Vector rval = traits_x::Local(measured_, hx, boost::none, Hlocal);
- (*H1) = ((*Hlocal) * (*H1)).eval();
- (*H2) = ((*Hlocal) * (*H2)).eval();
+#ifdef BETWEENFACTOR_ASSUME_SMALL
+ return traits_x::Local(measured_, hx);
+#else
+ return traits_x::Local(measured_, hx);
+ static const int N = traits_x::dimension;
+ Eigen::Matrix Hlocal;
+ Vector rval = traits_x::Local(measured_, hx, boost::none,
+ (H1 || H2) ? &Hlocal : 0);
+ if (H1) *H1 = Hlocal * (*H1);
+ if (H1) *H2 = Hlocal * (*H2);
return rval;
+#endif
}
/** return the measured */