diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index a14ad34f9..60bd833ff 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -6,6 +6,7 @@ virtual class gtsam::Value; virtual class gtsam::Point3; virtual class gtsam::Rot3; +virtual class gtsam::Pose2; virtual class gtsam::Pose3; virtual class gtsam::noiseModel::Base; virtual class gtsam::NonlinearFactor; @@ -165,5 +166,21 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor { DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model); }; +//************************************************************************* +// General nonlinear factor types +//************************************************************************* +#include -}///\namespace gtsam +#include +template +virtual class PoseTranslationPrior : gtsam::NonlinearFactor { + PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); +}; + +#include +template +virtual class PoseRotationPrior : gtsam::NonlinearFactor { + PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); +}; + +} //\namespace gtsam diff --git a/gtsam_unstable/slam/PoseRotationPrior.h b/gtsam_unstable/slam/PoseRotationPrior.h index c5f039f7f..c4a7a7de7 100644 --- a/gtsam_unstable/slam/PoseRotationPrior.h +++ b/gtsam_unstable/slam/PoseRotationPrior.h @@ -29,9 +29,23 @@ public: /** standard constructor */ PoseRotationPrior(Key key, const Rotation& rot_z, const SharedNoiseModel& model) - : Base(key, model) - { - assert(rot_z.dim() == model->dim()); + : Base(key, model) { + initialize(rot_z); + } + + /** Constructor that pulls the translation from an incoming POSE */ + PoseRotationPrior(Key key, const POSE& pose_z, const SharedNoiseModel& model) + : Base(key, model) { + initialize(pose_z.rotation()); + } + + /** get the rotation used to create the measurement */ + Rotation priorRotation() const { return Rotation::Expmap(this->prior_); } + +protected: + /** loads the underlying partial prior factor */ + void initialize(const Rotation& rot_z) { + assert(rot_z.dim() == this->noiseModel_->dim()); // Calculate the prior applied this->prior_ = Rotation::Logmap(rot_z); @@ -52,9 +66,6 @@ public: this->fillH(); } - /** get the rotation used to create the measurement */ - Rotation priorRotation() const { return Rotation::Expmap(this->prior_); } - }; } // \namespace gtsam diff --git a/gtsam_unstable/slam/PoseTranslationPrior.h b/gtsam_unstable/slam/PoseTranslationPrior.h index 0e4d34b8b..4219e3252 100644 --- a/gtsam_unstable/slam/PoseTranslationPrior.h +++ b/gtsam_unstable/slam/PoseTranslationPrior.h @@ -28,17 +28,31 @@ public: GTSAM_CONCEPT_LIE_TYPE(Translation) /** standard constructor */ - PoseTranslationPrior(Key key, const Translation& rot_z, const SharedNoiseModel& model) - : Base(key, model) - { - assert(rot_z.dim() == model->dim()); + PoseTranslationPrior(Key key, const Translation& trans_z, const SharedNoiseModel& model) + : Base(key, model) { + initialize(trans_z); + } + + /** Constructor that pulls the translation from an incoming POSE */ + PoseTranslationPrior(Key key, const POSE& pose_z, const SharedNoiseModel& model) + : Base(key, model) { + initialize(pose_z.translation()); + } + + /** get the rotation used to create the measurement */ + Translation priorTranslation() const { return Translation::Expmap(this->prior_); } + +protected: + /** loads the underlying partial prior factor */ + void initialize(const Translation& trans_z) { + assert(trans_z.dim() == this->noiseModel_->dim()); // Calculate the prior applied - this->prior_ = Translation::Logmap(rot_z); + this->prior_ = Translation::Logmap(trans_z); // Create the mask for partial prior size_t pose_dim = Pose::identity().dim(); - size_t rot_dim = rot_z.dim(); + size_t rot_dim = trans_z.dim(); // get the interval of the lie coordinates corresponding to rotation std::pair interval = Pose::translationInterval(); @@ -52,9 +66,6 @@ public: this->fillH(); } - /** get the rotation used to create the measurement */ - Translation priorTranslation() const { return Translation::Expmap(this->prior_); } - }; } // \namespace gtsam