diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index d4d4f314c..7f19de2a5 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -11,6 +11,7 @@ #include #include #include +#include #include namespace gtsam { @@ -59,11 +60,12 @@ namespace gtsam { }; }; + // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior class OrientedPlane3DirectionPrior: public NoiseModelFactor1 { protected: OrientedPlane3 measured_p_; /// measured plane parameters - Symbol landmarkSymbol_; + Key landmarkSymbol_; typedef NoiseModelFactor1 Base; @@ -75,10 +77,10 @@ namespace gtsam { {} /// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol - OrientedPlane3DirectionPrior (const Symbol& landmark, const Vector&z, + OrientedPlane3DirectionPrior (Key key, const Vector&z, const SharedGaussian& noiseModel) - : Base (noiseModel, landmark), - landmarkSymbol_ (landmark) + : Base (noiseModel, key), + landmarkSymbol_ (key) { measured_p_ = OrientedPlane3 (Sphere2 (z (0), z (1), z (2)), z (3)); }