diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index b54a4e255..4276fe9b1 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -42,32 +42,35 @@ struct BearingRange typename Range::result_type> { typedef typename Bearing::result_type B; typedef typename Range::result_type R; + typedef ProductManifold Base; BearingRange() {} - BearingRange(const ProductManifold& br) : ProductManifold(br) {} - BearingRange(const B& b, const R& r) : ProductManifold(b, r) {} + BearingRange(const ProductManifold& br) : Base(br) {} + BearingRange(const B& b, const R& r) : Base(b, r) {} /// Prediction function that stacks measurements - // BearingRange operator()( - // const A1& a1, const A2& a2, - // typename MakeOptionalJacobian::type H1, - // typename MakeOptionalJacobian::type H2) { - // typename MakeJacobian::type HB1; - // typename MakeJacobian::type HB2; - // typename MakeJacobian::type HR1; - // typename MakeJacobian::type HR2; - // - // B b = Bearing()(a1, a2, H1 ? &HB1 : 0, H2 ? &HB2 : 0); - // R r = Range()(a1, a2, H1 ? &HR1 : 0, H2 ? &HR2 : 0); - // - // if (H1) *H1 << HB1, HR1; - // if (H2) *H2 << HB2, HR2; - // return BearingRange(b, r); - // } + static BearingRange Measure( + const A1& a1, const A2& a2, + OptionalJacobian::dimension> H1 = boost::none, + OptionalJacobian::dimension> H2 = + boost::none) { + typename MakeJacobian::type HB1; + typename MakeJacobian::type HB2; + typename MakeJacobian::type HR1; + typename MakeJacobian::type HR2; + + B b = Bearing()(a1, a2, H1 ? &HB1 : 0, H2 ? &HB2 : 0); + R r = Range()(a1, a2, H1 ? &HR1 : 0, H2 ? &HR2 : 0); + + if (H1) *H1 << HB1, HR1; + if (H2) *H2 << HB2, HR2; + return BearingRange(b, r); + } void print(const std::string& str = "") const { - traits::Print(this->first, str); - traits::Print(this->second, str); + std::cout << str; + traits::Print(this->first, "bearing "); + traits::Print(this->second, "range "); } bool equals(const BearingRange& m2, double tol = 1e-8) const { return traits::Equals(this->first, m2.first, tol) && diff --git a/gtsam/geometry/tests/testBearingRange.cpp b/gtsam/geometry/tests/testBearingRange.cpp index 636f7458b..8c5d2208e 100644 --- a/gtsam/geometry/tests/testBearingRange.cpp +++ b/gtsam/geometry/tests/testBearingRange.cpp @@ -39,7 +39,11 @@ TEST(BearingRange2D, Concept) { } /* ************************************************************************* */ -TEST(BearingRange, 2D) {} +TEST(BearingRange, 2D) { + BearingRange2D expected(0, 1); + BearingRange2D actual = BearingRange2D::Measure(Pose2(), Point2(1, 0)); + EXPECT(assert_equal(expected, actual)); +} /* ************************************************************************* */ TEST(BearingRange, Serialization2D) { @@ -54,7 +58,11 @@ TEST(BearingRange3D, Concept) { } /* ************************************************************************* */ -TEST(BearingRange, 3D) {} +TEST(BearingRange, 3D) { + BearingRange3D expected(Unit3(), 1); + BearingRange3D actual = BearingRange3D::Measure(Pose3(), Point3(1, 0, 0)); + EXPECT(assert_equal(expected, actual)); +} /* ************************************************************************* */ TEST(BearingRange, Serialization3D) {