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 */