Added a generic prior factor for specific components of a variable

gtsam/slam/PartialPriorFactor.h

@@ -0,0 +1,139 @@
+/**
+ * @file PartialPriorFactor.h
+ * @brief A simple prior factor that allows for setting a prior only on a part of linear parameters
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+	/**
+	 * A class for a soft partial prior on any Lie type, with a mask over Expmap
+	 * parameters. Note that this will use Logmap() to find a tangent space parameterization
+	 * for the variable attached, so this may fail for highly nonlinear manifolds.
+	 *
+	 * The prior vector used in this factor is stored in compressed form, such that
+	 * it only contains values for measurements that are to be compared, and they are in
+	 * the same order as T::Logmap().  The mask will determine which components to extract
+	 * in the error function.
+	 *
+	 * It takes two template parameters:
+	 *   Key (typically TypedSymbol) is used to look up T's in a Values
+	 *   Values where the T's are stored, typically LieValues<Key> or a TupleValues<...>
+	 *
+	 * For practical use, it would be good to subclass this factor and have the class type
+	 * construct the mask.
+	 */
+	template<class VALUES, class KEY>
+	class PartialPriorFactor: public NonlinearFactor1<VALUES, KEY> {
+
+	public:
+		typedef typename KEY::Value T;
+
+	protected:
+
+		typedef NonlinearFactor1<VALUES, KEY> Base;
+		typedef PartialPriorFactor<VALUES, KEY> This;
+
+		Vector prior_;            /// measurement on logmap parameters, in compressed form
+		std::vector<bool> mask_;  /// flags to mask all parameters not measured
+
+	public:
+
+		/** default constructor - only use for serialization */
+		PartialPriorFactor() {}
+
+		// shorthand for a smart pointer to a factor
+		typedef typename boost::shared_ptr<PartialPriorFactor> shared_ptr;
+
+		virtual ~PartialPriorFactor() {}
+
+		/** Full Constructor: requires mask and vector - not for typical use */
+		PartialPriorFactor(const KEY& key, const std::vector<bool>& mask,
+				const Vector& prior, const SharedGaussian& model) :
+			Base(model, key), prior_(prior), mask_(mask) {
+			assert(mask_.size() == T::Dim()); // NOTE: assumes constant size variable
+			assert(nrTrue() == model->dim());
+			assert(nrTrue() == prior_.size());
+		}
+
+		/** Single Element Constructor: acts on a single parameter specified by idx */
+		PartialPriorFactor(const KEY& key, size_t idx, double prior, const SharedGaussian& model) :
+			Base(model, key), prior_(Vector_(1, prior)), mask_(T::Dim(), false) {
+			assert(model->dim() == 1);
+			mask_[idx] = true;
+			assert(nrTrue() == 1);
+		}
+
+		/** Indices Constructor: specify the mask with a set of indices */
+		PartialPriorFactor(const KEY& key, const std::vector<size_t>& mask, const Vector& prior,
+				const SharedGaussian& model) :
+			Base(model, key), prior_(prior), mask_(T::Dim(), false) {
+			assert((size_t)prior_.size() == mask.size());
+			assert(model->dim() == (size_t) prior.size());
+			for (size_t i=0; i<mask.size(); ++i) {
+				assert(mask[i] < mask_.size());
+				mask_[mask[i]] = true;
+			}
+			assert(nrTrue() == this->dim());
+		}
+
+		/** implement functions needed for Testable */
+
+		/** print */
+		virtual void print(const std::string& s) const {
+			Base::print(s);
+			gtsam::print(prior_, "prior");
+		}
+
+		/** equals */
+		virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
+			const This *e = dynamic_cast<const This*> (&expected);
+			return e != NULL && Base::equals(*e, tol) &&
+					gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) &&
+					this->mask_ == e->mask_;
+		}
+
+		/** implement functions needed to derive from Factor */
+
+		/** vector of errors */
+		Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
+			if (H) (*H) = zeros(this->dim(), p.dim());
+			Vector full_logmap = T::Logmap(p);
+			Vector masked_logmap = zero(this->dim());
+			size_t masked_idx=0;
+			for (size_t i=0;i<mask_.size();++i)
+				if (mask_[i]) {
+					masked_logmap(masked_idx) = full_logmap(i);
+					if (H) (*H)(masked_idx, i) = 1.0;
+					++masked_idx;
+				}
+			return masked_logmap - prior_;
+		}
+
+	protected:
+
+		/** counts true elements in the mask */
+		size_t nrTrue() const {
+			size_t result=0;
+			for (size_t i=0; i<mask_.size(); ++i)
+				if (mask_[i])	++result;
+			return result;
+		}
+
+	private:
+		/** Serialization function */
+		friend class boost::serialization::access;
+		template<class ARCHIVE>
+		void serialize(ARCHIVE & ar, const unsigned int version) {
+			ar & boost::serialization::make_nvp("NonlinearFactor1",
+					boost::serialization::base_object<Base>(*this));
+			ar & BOOST_SERIALIZATION_NVP(prior_);
+			ar & BOOST_SERIALIZATION_NVP(mask_);
+		}
+	}; // \class PartialPriorFactor
+
+} /// namespace gtsam

gtsam/slam/PriorFactor.h

@@ -15,19 +15,15 @@
  **/
 #pragma once
 
-#include <ostream>
-#include <gtsam/linear/NoiseModel.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/geometry/Pose2.h>
 
 namespace gtsam {
 
 	/**
 	 * A class for a soft prior on any Lie type
-	 * It takes three template parameters:
+	 * It takes two template parameters:
-	 *   T is the Lie group type for which the prior is define
 	 *   Key (typically TypedSymbol) is used to look up T's in a Values
-	 *   Values where the T's are stored, typically LieValues<Key,T> or a TupleValues<...>
+	 *   Values where the T's are stored, typically LieValues<Key> or a TupleValues<...>
 	 * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
 	 * a simple type like int will not work
 	 */

gtsam/slam/tests/testPose3SLAM.cpp

@@ -19,6 +19,7 @@
 #include <iostream>
 #include <boost/shared_ptr.hpp>
 #include <boost/assign/std/list.hpp>
+#include <boost/assign/std/vector.hpp>
 using namespace boost;
 using namespace boost::assign;
 
@@ -29,6 +30,7 @@ using namespace boost::assign;
 #define GTSAM_MAGIC_KEY
 
 #include <gtsam/slam/pose3SLAM.h>
+#include <gtsam/slam/PartialPriorFactor.h>
 
 using namespace std;
 using namespace gtsam;
@@ -36,6 +38,10 @@ using namespace gtsam;
 // common measurement covariance
 static Matrix covariance = eye(6);
 
+const double tol=1e-5;
+
+using namespace pose3SLAM;
+
 /* ************************************************************************* */
 // test optimization with 6 poses arranged in a hexagon and a loop closure
 TEST(Pose3Graph, optimizeCircle) {
@@ -70,7 +76,6 @@ TEST(Pose3Graph, optimizeCircle) {
   // Choose an ordering and optimize
   shared_ptr<Ordering> ordering(new Ordering);
   *ordering += "x0","x1","x2","x3","x4","x5";
-  typedef NonlinearOptimizer<Pose3Graph, Pose3Values> Optimizer;
   NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15);
   Optimizer optimizer0(fg, initial, ordering, params);
   Optimizer optimizer = optimizer0.levenbergMarquardt();
@@ -84,6 +89,68 @@ TEST(Pose3Graph, optimizeCircle) {
   CHECK(assert_equal(_0T1,actual[5].between(actual[0]),1e-5));
 }
 
+/* ************************************************************************* */
+TEST(Pose3Graph, partial_prior_height) {
+	typedef PartialPriorFactor<Values, Key> Partial;
+
+	// reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3)
+
+	// height prior - single element interface
+	Key key(1);
+	double exp_height = 5.0;
+	SharedDiagonal model = noiseModel::Unit::Create(1);
+	Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height));
+	Partial height(key, 5, exp_height, model);
+	Matrix actA;
+	EXPECT(assert_equal(Vector_(1,-2.0), height.evaluateError(init, actA), tol));
+	Matrix expA = Matrix_(1, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
+	EXPECT(assert_equal(expA, actA));
+
+	Graph graph;
+//	graph.add(height); // FAIL - on compile, can't initialize a reference?
+	graph.push_back(boost::shared_ptr<Partial>(new Partial(height)));
+
+	Values values;
+	values.insert(key, init);
+
+	// linearization
+	EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
+
+	Values actual = *Optimizer::optimizeLM(graph, values);
+	EXPECT(assert_equal(expected, actual[key], tol));
+	EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
+}
+
+/* ************************************************************************* */
+TEST(Pose3Graph, partial_prior_xy) {
+	typedef PartialPriorFactor<Values, Key> Partial;
+
+	// XY prior - full mask interface
+	Key key(1);
+	Vector exp_xy = Vector_(2, 3.0, 4.0);
+	SharedDiagonal model = noiseModel::Unit::Create(2);
+	Pose3 init(Rot3(), Point3(1.0,-2.0, 3.0)), expected(Rot3(), Point3(3.0, 4.0, 3.0));
+	vector<size_t> mask; mask += 3, 4;
+	Partial priorXY(key, mask, exp_xy, model);
+	Matrix actA;
+	EXPECT(assert_equal(Vector_(2,-2.0,-6.0), priorXY.evaluateError(init, actA), tol));
+	Matrix expA = Matrix_(2, 6,
+			0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
+			0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
+	EXPECT(assert_equal(expA, actA));
+
+	Graph graph;
+//	graph.add(priorXY); // FAIL - on compile, can't initialize a reference?
+	graph.push_back(Partial::shared_ptr(new Partial(priorXY)));
+
+	Values values;
+	values.insert(key, init);
+
+	Values actual = *Optimizer::optimizeLM(graph, values);
+	EXPECT(assert_equal(expected, actual[key], tol));
+	EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;