diff --git a/cpp/planarSLAM.h b/cpp/planarSLAM.h index 91a68d6d9..104664591 100644 --- a/cpp/planarSLAM.h +++ b/cpp/planarSLAM.h @@ -46,6 +46,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 /** @@ -75,6 +80,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 // Use planarSLAM namespace for specific SLAM instance