diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index e457da887..da7696d0d 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -93,20 +93,20 @@ TEST( Point3, dot) { // Use numerical derivatives to calculate the expected Jacobians Matrix H1, H2; - boost::function f = boost::bind(&Point3::dot, _1, _2, // - boost::none, boost::none); + boost::function f = + [](const Point3& p, const Point3& q) { return gtsam::dot(p, q); }; { - p.dot(q, H1, H2); + gtsam::dot(p, q, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); EXPECT(assert_equal(numericalDerivative22(f, p, q), H2, 1e-9)); } { - p.dot(r, H1, H2); + gtsam::dot(p, r, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, r), H1, 1e-9)); EXPECT(assert_equal(numericalDerivative22(f, p, r), H2, 1e-9)); } { - p.dot(t, H1, H2); + gtsam::dot(p, t, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, t), H1, 1e-9)); EXPECT(assert_equal(numericalDerivative22(f, p, t), H2, 1e-9)); } @@ -134,15 +134,15 @@ TEST( Point3, cross2) { // Use numerical derivatives to calculate the expected Jacobians Matrix H1, H2; - boost::function f = boost::bind(&Point3::cross, _1, _2, // + boost::function f = boost::bind(>sam::cross, _1, _2, // boost::none, boost::none); { - p.cross(q, H1, H2); + gtsam::cross(p, q, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); EXPECT(assert_equal(numericalDerivative22(f, p, q), H2, 1e-9)); } { - p.cross(r, H1, H2); + gtsam::cross(p, r, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, r), H1, 1e-9)); EXPECT(assert_equal(numericalDerivative22(f, p, r), H2, 1e-9)); } diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 782c98148..05b7259bf 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -66,20 +66,21 @@ pair GPSFactor::EstimateState(double t1, const Point3& NED1, //*************************************************************************** void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n"; - nT_.print(" GPS measurement: "); + cout << " GPS measurement: " << nT_.transpose() << endl; noiseModel_->print(" noise model: "); } //*************************************************************************** bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && nT_.equals(e->nT_, tol); + return e != NULL && Base::equals(*e, tol) && + traits::Equals(nT_, e->nT_, tol); } //*************************************************************************** Vector GPSFactor2::evaluateError(const NavState& p, boost::optional H) const { - return p.position(H).vector() -nT_.vector(); + return p.position(H) -nT_; } //*************************************************************************** diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index bbf19869d..43f1cd017 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -372,7 +372,7 @@ TEST(Expression, TripleSum) { /* ************************************************************************* */ TEST(Expression, SumOfUnaries) { const Key key(67); - const double_ norm_(Point3_(key), &Point3::norm); + const double_ norm_(>sam::norm, Point3_(key)); const double_ sum_ = norm_ + norm_; Values values; @@ -391,7 +391,7 @@ TEST(Expression, SumOfUnaries) { TEST(Expression, UnaryOfSum) { const Key key1(42), key2(67); const Point3_ sum_ = Point3_(key1) + Point3_(key2); - const double_ norm_(sum_, &Point3::norm); + const double_ norm_(>sam::norm, sum_); map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); norm_.dims(actual_dims); @@ -455,7 +455,7 @@ TEST(Expression, Subtract) { /* ************************************************************************* */ TEST(Expression, LinearExpression) { const Key key(67); - const boost::function f = boost::bind(&Point3::vector, _1); + const boost::function f = [](const Point3& p) { return (Vector3)p; }; const Matrix3 kIdentity = I_3x3; const Expression linear_ = linearExpression(f, Point3_(key), kIdentity);