diff --git a/cpp/BearingRangeFactor.h b/cpp/BearingRangeFactor.h new file mode 100644 index 000000000..4e8902fbd --- /dev/null +++ b/cpp/BearingRangeFactor.h @@ -0,0 +1,58 @@ +/* + * BearingRangeFactor.h + * + * Created on: Apr 1, 2010 + * Author: Kai Ni + * Description: a single factor contains both the bearing and the range to prevent handle to pair bearing and range factors + */ + +#pragma once + +#include "BearingFactor.h" +#include "RangeFactor.h" + +namespace gtsam { + + /** + * Binary factor for a bearing measurement + */ + template + class BearingRangeFactor: public NonlinearFactor2 { + private: + + // the bearing factor + BearingFactor bearing_; + + // the range factor + RangeFactor range_; + + typedef NonlinearFactor2 Base; + + public: + + BearingRangeFactor(); /* Default constructor */ + BearingRangeFactor(const PoseKey& i, const PointKey& j, const std::pair& z, + const SharedGaussian& model) : + Base(model, i, j), bearing_(i, j, z.first, model), range_(i, j, z.second, model) { + } + + /** h(x)-z -> between(z,h(x)) for Rot2 manifold */ + Vector evaluateError(const Pose2& pose, const Point2& point, + boost::optional H1, boost::optional H2) const { + boost::optional H11 = H1, H21 = H1; + boost::optional H12 = H2, H22 = H2; + Vector e1 = bearing_.evaluateError(pose, point, H11, H12); + Vector e2 = range_.evaluateError(pose, point, H21, H21); + if (H1) *H1 = stack(2, &(*H11), &(*H21)); + if (H2) *H2 = stack(2, &(*H12), &(*H22)); + return concatVectors(2, &e1, &e2); + } + + /** return the measured */ + inline const std::pair measured() const { + return concatVectors(2, bearing_.measured(), range_.measured()); + } + }; // BearingRangeFactor + +} // namespace gtsam diff --git a/cpp/Makefile.am b/cpp/Makefile.am index b48ff3fea..552ee0589 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -212,7 +212,7 @@ testPose2SLAM_SOURCES = testPose2SLAM.cpp testPose2SLAM_LDADD = libgtsam.la # 2D SLAM using Bearing and Range -headers += BearingFactor.h RangeFactor.h +headers += BearingFactor.h RangeFactor.h BearingRangeFactor.h sources += planarSLAM.cpp check_PROGRAMS += testPlanarSLAM testPlanarSLAM_SOURCES = testPlanarSLAM.cpp diff --git a/cpp/planarSLAM.h b/cpp/planarSLAM.h index c721cc76b..1715f0be8 100644 --- a/cpp/planarSLAM.h +++ b/cpp/planarSLAM.h @@ -6,8 +6,7 @@ #pragma once -#include "BearingFactor.h" -#include "RangeFactor.h" +#include "BearingRangeFactor.h" #include "TupleConfig.h" #include "NonlinearEquality.h" #include "PriorFactor.h" @@ -34,6 +33,7 @@ namespace gtsam { typedef BetweenFactor Odometry; typedef BearingFactor Bearing; typedef RangeFactor Range; + typedef BearingRangeFactor BearingRange; // Graph struct Graph: public NonlinearFactorGraph { diff --git a/cpp/testPlanarSLAM.cpp b/cpp/testPlanarSLAM.cpp index af7c7f537..efb8918c5 100644 --- a/cpp/testPlanarSLAM.cpp +++ b/cpp/testPlanarSLAM.cpp @@ -7,6 +7,7 @@ #include #include "planarSLAM.h" +#include "BearingRangeFactor.h" using namespace std; using namespace gtsam; @@ -17,6 +18,7 @@ static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)), + sigma2(noiseModel::Isotropic::Sigma(2,0.1)), I3(noiseModel::Unit::Create(3)); /* ************************************************************************* */ @@ -53,6 +55,24 @@ TEST( planarSLAM, RangeFactor ) CHECK(assert_equal(Vector_(1,0.22),actual)); } +/* ************************************************************************* */ +TEST( planarSLAM, BearingRangeFactor ) +{ + // Create factor + Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 + double b(sqrt(2) - 0.22); // h(x) - z = 0.22 + planarSLAM::BearingRange factor(2, 3, make_pair(r,b), sigma2); + + // create config + planarSLAM::Config c; + c.insert(2, x2); + c.insert(3, l3); + + // Check error + Vector actual = factor.unwhitenedError(c); + CHECK(assert_equal(Vector_(2,-0.1, 0.22),actual)); +} + /* ************************************************************************* */ TEST( planarSLAM, constructor ) {