gradient descent (with fixed nr. of iterations, choosing optimal step size)

cpp/GaussianFactor.cpp

@@ -363,7 +363,7 @@ void GaussianFactor::addGradientContribution(const VectorConfig& x, VectorConfig
 	// calculate the value of the factor
 	Vector e = -b_;
   string j; Matrix Aj;
-  FOREACH_PAIR(j, Aj, As_) e += Vector(Aj * x[j]);
+  FOREACH_PAIR(j, Aj, As_) e += Aj * x[j];
 
   // transpose
   Vector et = trans(e);
@@ -376,6 +376,24 @@ void GaussianFactor::addGradientContribution(const VectorConfig& x, VectorConfig
   }
 }
 
+/* ************************************************************************* */
+// Creates a factor on step-size, given initial estimate and direction d, e.g.
+// Factor |A1*x+A2*y-b|/sigma -> |A1*(x0+alpha*dx)+A2*(y0+alpha*dy)-b|/sigma
+//                            -> |(A1*dx+A2*dy)*alpha-(b-A1*x0-A2*y0)|/sigma
+/* ************************************************************************* */
+GaussianFactor::shared_ptr GaussianFactor::alphaFactor(const VectorConfig& x,
+		const VectorConfig& d) const {
+	size_t m = b_.size();
+	Vector A = zero(m); Vector b = b_;
+  string j; Matrix Aj;
+  FOREACH_PAIR(j, Aj, As_) {
+  	A += Aj * d[j];
+  	b -= Aj * x[j];
+  }
+	shared_ptr factor(new GaussianFactor("alpha",Matrix_(A),b,sigmas_));
+	return factor;
+}
+
 /* ************************************************************************* */
 namespace gtsam {
 

cpp/GaussianFactor.h

@@ -58,6 +58,13 @@ public:
 		As_.insert(make_pair(key1, A1));
 	}
 
+	/** Construct unary factor with vector of sigmas*/
+	GaussianFactor(const std::string& key1, const Matrix& A1,
+			const Vector& b, const Vector& sigmas) :
+		b_(b), sigmas_(sigmas) {
+		As_.insert(make_pair(key1, A1));
+	}
+
 	/** Construct binary factor */
 	GaussianFactor(const std::string& key1, const Matrix& A1,
 			const std::string& key2, const Matrix& A2,
@@ -207,6 +214,20 @@ public:
 	boost::tuple<std::list<int>, std::list<int>, std::list<double> >
 		sparse(const Ordering& ordering, const Dimensions& variables) const;
 
+	/**
+	 * Add gradient contribution to gradient config g
+	 * @param x: confif at which to evaluate gradient
+	 * @param g: I/O parameter, evolving gradient
+	 */
+	void addGradientContribution(const VectorConfig& x, VectorConfig& g) const;
+
+	/**
+	 * Create a GaussianFactor on one variable 'alpha' (step size), in direction d
+	 * @param x: starting point for search
+	 * @param d: search direction
+	 */
+	shared_ptr alphaFactor(const VectorConfig& x, const VectorConfig& d) const;
+
 	/* ************************************************************************* */
 	// MUTABLE functions. FD:on the path to being eradicated
 	/* ************************************************************************* */
@@ -243,13 +264,6 @@ public:
 	 */
 	void append_factor(GaussianFactor::shared_ptr f, size_t m, size_t pos);
 
-	/**
-	 * Add gradient contribution to gradient config g
-	 * @param x: confif at which to evaluate gradient
-	 * @param g: I/O parameter, evolving gradient
-	 */
-	void addGradientContribution(const VectorConfig& x, VectorConfig& g) const;
-
 }; // GaussianFactor
 
 /* ************************************************************************* */

cpp/GaussianFactorGraph.cpp

@@ -209,3 +209,33 @@ VectorConfig GaussianFactorGraph::gradient(const VectorConfig& x) const {
 }
 
 /* ************************************************************************* */
+VectorConfig GaussianFactorGraph::optimalUpdate(const VectorConfig& x,
+		const VectorConfig& d) const {
+
+	// create a new graph on step-size
+	GaussianFactorGraph alphaGraph;
+	BOOST_FOREACH(sharedFactor factor,factors_) {
+		sharedFactor alphaFactor = factor->alphaFactor(x,d);
+		alphaGraph.push_back(alphaFactor);
+	}
+
+	// solve it for optimal step-size alpha
+	GaussianConditional::shared_ptr gc = alphaGraph.eliminateOne("alpha");
+	double alpha = gc->get_d()(0);
+
+	// return updated estimate by stepping in direction d
+  return x.exmap(d.scale(alpha));
+}
+
+/* ************************************************************************* */
+VectorConfig GaussianFactorGraph::gradientDescent(const VectorConfig& x0) const {
+	VectorConfig x = x0;
+	int K = 10*x.size();
+	for (int k=0;k<K;k++) {
+		VectorConfig g = gradient(x);
+		x = optimalUpdate(x,g);
+	}
+	return x;
+}
+
+/* ************************************************************************* */

cpp/GaussianFactorGraph.h

@@ -162,8 +162,24 @@ namespace gtsam {
 
   	/**
   	 * Calculate Gradient of 0.5*|Ax-b| for a given config
+  	 * @param x: VectorConfig specifying where to calculate gradient
+  	 * @return gradient, as a VectorConfig as well
   	 */
   	VectorConfig gradient(const VectorConfig& x) const;
+
+  	/**
+  	 * Take an optimal step in direction d by calculating optimal step-size
+  	 * @param x: starting point for search
+  	 * @param d: search direction
+  	 */
+  	VectorConfig optimalUpdate(const VectorConfig& x0, const VectorConfig& d) const;
+
+  	/**
+  	 * Find solution using gradient descent
+  	 * @param x0: VectorConfig specifying initial estimate
+  	 * @return solution
+  	 */
+  	VectorConfig gradientDescent(const VectorConfig& x0) const;
   };
 
 }

cpp/testGaussianFactorGraph.cpp

@@ -565,6 +565,22 @@ TEST( GaussianFactorGraph, gradient )
 	CHECK(assert_equal(zero,actual2));
 }
 
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, gradientDescent )
+{
+	// Expected solution
+	Ordering ord;
+  ord += "x2","l1","x1";
+	GaussianFactorGraph fg = createGaussianFactorGraph();
+  VectorConfig expected = fg.optimize(ord); // destructive
+
+  // Do gradient descent
+  GaussianFactorGraph fg2 = createGaussianFactorGraph();
+  VectorConfig zero = createZeroDelta();
+	VectorConfig actual = fg2.gradientDescent(zero);
+	CHECK(assert_equal(expected,actual,1e-2));
+}
+
 /* ************************************************************************* */
 // Tests ported from ConstrainedGaussianFactorGraph
 /* ************************************************************************* */