diff --git a/cpp/BearingFactor.h b/cpp/BearingFactor.h index 50f0df1df..b037fbbe2 100644 --- a/cpp/BearingFactor.h +++ b/cpp/BearingFactor.h @@ -13,37 +13,11 @@ namespace gtsam { /** - * Calculate bearing to a landmark - * @param pose 2D pose of robot - * @param point 2D location of landmark - * @return 2D rotation \in SO(2) - */ - Rot2 bearing(const Pose2& pose, const Point2& point) { - Point2 d = transform_to(pose, point); - return relativeBearing(d); - } - - /** - * Calculate bearing and optional derivative(s) - */ - Rot2 bearing(const Pose2& pose, const Point2& point, - boost::optional H1, boost::optional H2) { - if (!H1 && !H2) return bearing(pose, point); - Point2 d = transform_to(pose, point); - Matrix D_result_d; - Rot2 result = relativeBearing(d, D_result_d); - if (H1) *H1 = D_result_d * Dtransform_to1(pose, point); - if (H2) *H2 = D_result_d * Dtransform_to2(pose, point); - return result; - } - - /** - * Non-linear factor for a constraint derived from a 2D measurement, - * i.e. the main building block for visual SLAM. + * Binary factor for a bearing measurement */ template class BearingFactor: public NonlinearFactor2 { + PointKey, Point2> { private: Rot2 z_; /** measurement */ @@ -53,9 +27,9 @@ namespace gtsam { public: BearingFactor(); /* Default constructor */ - BearingFactor(const Rot2& z, double sigma, const PoseKey& i, - const PointKey& j) : - Base(sigma, i, j), z_(z) { + BearingFactor(const PoseKey& i, const PointKey& j, const Rot2& z, + const SharedGaussian& model) : + Base(model, i, j), z_(z) { } /** h(x)-z -> between(z,h(x)) for Rot2 manifold */ @@ -64,6 +38,11 @@ namespace gtsam { Rot2 hx = bearing(pose, point, H1, H2); return logmap(between(z_, hx)); } + + /** return the measured */ + inline const Rot2 measured() const { + return z_; + } }; // BearingFactor } // namespace gtsam diff --git a/cpp/Makefile.am b/cpp/Makefile.am index cb82a0a00..b48ff3fea 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 += +headers += BearingFactor.h RangeFactor.h sources += planarSLAM.cpp check_PROGRAMS += testPlanarSLAM testPlanarSLAM_SOURCES = testPlanarSLAM.cpp diff --git a/cpp/RangeFactor.h b/cpp/RangeFactor.h index 7c6f6503b..9595bd406 100644 --- a/cpp/RangeFactor.h +++ b/cpp/RangeFactor.h @@ -12,37 +12,11 @@ namespace gtsam { /** - * Calculate range to a landmark - * @param pose 2D pose of robot - * @param point 2D location of landmark - * @return range (double) - */ - double range(const Pose2& pose, const Point2& point) { - Point2 d = transform_to(pose, point); - return d.norm(); - } - - /** - * Calculate range and optional derivative(s) - */ - double range(const Pose2& pose, const Point2& point, - boost::optional H1, boost::optional H2) { - if (!H1 && !H2) return range(pose, point); - Point2 d = transform_to(pose, point); - double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); - Matrix D_result_d = Matrix_(1, 2, x / n, y / n); - if (H1) *H1 = D_result_d * Dtransform_to1(pose, point); - if (H2) *H2 = D_result_d * Dtransform_to2(pose, point); - return n; - } - - /** - * Non-linear factor for a constraint derived from a 2D measurement, - * i.e. the main building block for visual SLAM. + * Binary factor for a range measurement */ template class RangeFactor: public NonlinearFactor2 { + Point2> { private: double z_; /** measurement */ @@ -53,8 +27,9 @@ namespace gtsam { RangeFactor(); /* Default constructor */ - RangeFactor(double z, double sigma, const PoseKey& i, const PointKey& j) : - Base(sigma, i, j), z_(z) { + RangeFactor(const PoseKey& i, const PointKey& j, double z, + const SharedGaussian& model) : + Base(model, i, j), z_(z) { } /** h(x)-z */ @@ -63,6 +38,11 @@ namespace gtsam { double hx = gtsam::range(pose, point, H1, H2); return Vector_(1, hx - z_); } + + /** return the measured */ + inline const double measured() const { + return z_; + } }; // RangeFactor } // namespace gtsam diff --git a/cpp/planarSLAM.h b/cpp/planarSLAM.h index 104664591..c721cc76b 100644 --- a/cpp/planarSLAM.h +++ b/cpp/planarSLAM.h @@ -6,10 +6,8 @@ #pragma once -#include "Key.h" -#include "Pose2.h" -#include "Point2.h" -#include "NonlinearFactor.h" +#include "BearingFactor.h" +#include "RangeFactor.h" #include "TupleConfig.h" #include "NonlinearEquality.h" #include "PriorFactor.h" @@ -20,73 +18,6 @@ // We use gtsam namespace for generally useful factors namespace gtsam { - /** - * Binary factor for a bearing measurement - */ - template - class BearingFactor: public NonlinearFactor2 { - private: - - Rot2 z_; /** measurement */ - - typedef NonlinearFactor2 Base; - - public: - - BearingFactor(); /* Default constructor */ - BearingFactor(const PoseKey& i, const PointKey& j, const Rot2& z, - const SharedGaussian& model) : - Base(model, i, j), z_(z) { - } - - /** 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 { - Rot2 hx = bearing(pose, point, H1, H2); - return logmap(between(z_, hx)); - } - - /** return the measured */ - inline const Rot2 measured() const { - return z_; - } - }; // BearingFactor - - /** - * Binary factor for a range measurement - */ - template - class RangeFactor: public NonlinearFactor2 { - private: - - double z_; /** measurement */ - - typedef NonlinearFactor2 Base; - - public: - - RangeFactor(); /* Default constructor */ - - RangeFactor(const PoseKey& i, const PointKey& j, double z, - const SharedGaussian& model) : - Base(model, i, j), z_(z) { - } - - /** h(x)-z */ - Vector evaluateError(const Pose2& pose, const Point2& point, - boost::optional H1, boost::optional H2) const { - double hx = gtsam::range(pose, point, H1, H2); - return Vector_(1, hx - z_); - } - - /** return the measured */ - inline const double measured() const { - return z_; - } - }; // RangeFactor - // Use planarSLAM namespace for specific SLAM instance namespace planarSLAM {