Wrapped Pose[Translation|Rotation]Factor

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 <gtsam/geometry/Pose2.h>
 
-}///\namespace gtsam
+#include <gtsam_unstable/slam/PoseTranslationPrior.h>
+template<POSE = {gtsam::Pose2,gtsam::Pose3}>
+virtual class PoseTranslationPrior : gtsam::NonlinearFactor {
+	PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
+};
+
+#include <gtsam_unstable/slam/PoseRotationPrior.h>
+template<POSE = {gtsam::Pose2,gtsam::Pose3}>
+virtual class PoseRotationPrior : gtsam::NonlinearFactor {
+	PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
+};
+
+} //\namespace gtsam

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)
+	: Base(key, model) {
-	{
+		initialize(rot_z);
-		assert(rot_z.dim() == model->dim());
+	}
+
+	/** 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

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)
+	PoseTranslationPrior(Key key, const Translation& trans_z, const SharedNoiseModel& model)
-	: Base(key, model)
+	: Base(key, model) {
-	{
+		initialize(trans_z);
-		assert(rot_z.dim() == model->dim());
+	}
+
+	/** 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<size_t, size_t> 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