put back Value in slam sub-namespaces for the wrapper to interface with MATLAB. Cannot solve the return shared_ptr problem in NonlinearOptimizationParameters::newDecreaseThresholds

examples/PlanarSLAMExample_easy.cpp

@@ -76,7 +76,7 @@ int main(int argc, char** argv) {
 	graph.print("full graph");
 
 	// initialize to noisy points
-	Values initialEstimate;
+	planarSLAM::Values initialEstimate;
 	initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
 	initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
 	initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
@@ -86,7 +86,7 @@ int main(int argc, char** argv) {
 	initialEstimate.print("initial estimate");
 
 	// optimize using Levenberg-Marquardt optimization with an ordering from colamd
-	Values result = optimize<Graph>(graph, initialEstimate);
+	planarSLAM::Values result = optimize<Graph>(graph, initialEstimate);
 	result.print("final result");
 
 	return 0;

examples/Pose2SLAMExample_advanced.cpp

@@ -57,7 +57,7 @@ int main(int argc, char** argv) {
 
   /* 3. Create the data structure to hold the initial estimate to the solution
    * initialize to noisy points */
-	shared_ptr<Values> initial(new Values);
+	shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values);
 	initial->insert(x1, Pose2(0.5, 0.0, 0.2));
 	initial->insert(x2, Pose2(2.3, 0.1,-0.2));
 	initial->insert(x3, Pose2(4.1, 0.1, 0.1));
@@ -72,7 +72,7 @@ int main(int argc, char** argv) {
 	Optimizer optimizer(graph, initial, ordering, params);
 	Optimizer optimizer_result = optimizer.levenbergMarquardt();
 
-	Values result = *optimizer_result.values();
+	pose2SLAM::Values result = *optimizer_result.values();
 	result.print("final result");
 
 	/* Get covariances */

examples/Pose2SLAMExample_easy.cpp

@@ -55,7 +55,7 @@ int main(int argc, char** argv) {
 
     /* 3. Create the data structure to hold the initial estinmate to the solution
      * initialize to noisy points */
-	Values initial;
+	pose2SLAM::Values initial;
 	initial.insert(x1, Pose2(0.5, 0.0, 0.2));
 	initial.insert(x2, Pose2(2.3, 0.1,-0.2));
 	initial.insert(x3, Pose2(4.1, 0.1, 0.1));
@@ -63,7 +63,7 @@ int main(int argc, char** argv) {
 
 	/* 4 Single Step Optimization
 	* optimize using Levenberg-Marquardt optimization with an ordering from colamd */
-	Values result = optimize<Graph>(graph, initial);
+	pose2SLAM::Values result = optimize<Graph>(graph, initial);
 	result.print("final result");
 
 

gtsam.h

@@ -389,6 +389,8 @@ class NonlinearOptimizationParameters {
 	NonlinearOptimizationParameters(double absDecrease, double relDecrease,
 			double sumError, int iIters, double lambda, double lambdaFactor);
 	void print(string s) const;
+	static gtsam::NonlinearOptimizationParameters* newDrecreaseThresholds(double absDecrease,
+			double relDecrease);
 };
 
 
@@ -427,14 +429,14 @@ class Graph {
 	void addRange(int poseKey, int pointKey, double range, const gtsam::SharedNoiseModel& noiseModel);
 	void addBearingRange(int poseKey, int pointKey, const gtsam::Rot2& bearing, double range,
 			const gtsam::SharedNoiseModel& noiseModel);
-	Values optimize(const Values& initialEstimate);
+	planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
 };
 
 class Odometry {
 	Odometry(int key1, int key2, const gtsam::Pose2& measured,
 			const gtsam::SharedNoiseModel& model);
 	void print(string s) const;
-	gtsam::GaussianFactor* linearize(const Values& center, const Ordering& ordering) const;
+	gtsam::GaussianFactor* linearize(const planarSLAM::Values& center, const gtsam::Ordering& ordering) const;
 };
 
 class Optimizer {

gtsam/slam/planarSLAM.h

@@ -54,6 +54,33 @@ namespace planarSLAM {
 	/// A factor between a PoseKey and a PointKey to express difference in rotation and location
 	typedef BearingRangeFactor<PoseKey, PointKey> BearingRange;
 
+  /**  Values class, using specific PoseKeys and PointKeys
+   * Mainly as a convenience for MATLAB wrapper, which does not allow for identically named methods
+   */
+  struct Values: public gtsam::Values {
+
+    /// Default constructor
+    Values() {}
+
+    /// Copy constructor
+    Values(const gtsam::Values& values) :
+      gtsam::Values(values) {
+    }
+
+    /// get a pose
+    Pose2 pose(int key) const {	return (*this)[PoseKey(key)]; }
+
+    /// get a point
+    Point2 point(int key) const {	return (*this)[PointKey(key)]; }
+
+    /// insert a pose
+    void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); }
+
+    /// insert a point
+    void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
+  };
+
+
 	/// Creates a NonlinearFactorGraph with the Values type
 	struct Graph: public NonlinearFactorGraph {
 
@@ -93,8 +120,8 @@ namespace planarSLAM {
 		}
 	};
 
-		/// Optimizer
-		typedef NonlinearOptimizer<Graph> Optimizer;
+/// Optimizer
+typedef NonlinearOptimizer<Graph> Optimizer;
 
 } // planarSLAM
 

gtsam/slam/pose2SLAM.h

@@ -36,6 +36,25 @@ namespace pose2SLAM {
   /// Keys with Pose2 and symbol 'x'
   typedef TypedSymbol<Pose2, 'x'> PoseKey;
 
+  /// Values class, inherited from Values, using PoseKeys, mainly used as a convenience for MATLAB wrapper
+  struct Values: public gtsam::Values {
+
+    /// Default constructor
+    Values() {}
+
+    /// Copy constructor
+    Values(const gtsam::Values& values) :
+        gtsam::Values(values) {
+    }
+
+    // Convenience for MATLAB wrapper, which does not allow for identically named methods
+
+    /// get a pose
+    Pose2 pose(int key) const { return (*this)[PoseKey(key)]; }
+
+    /// insert a pose
+    void insertPose(int key, const Pose2& pose) { insert(PoseKey(key), pose); }
+  };
 
   /**
    * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)

gtsam/slam/simulated2D.h

@@ -32,6 +32,60 @@ namespace simulated2D {
   typedef TypedSymbol<Point2, 'x'> PoseKey;
   typedef TypedSymbol<Point2, 'l'> PointKey;
 
+  /**
+   *  Custom Values class that holds poses and points, mainly used as a convenience for MATLAB wrapper
+   */
+  class Values: public gtsam::Values {
+  private:
+  	int nrPoses_, nrPoints_;
+
+  public:
+    typedef gtsam::Values Base;  ///< base class
+    typedef boost::shared_ptr<Point2> sharedPoint;  ///< shortcut to shared Point type
+
+    /// Constructor
+    Values() : nrPoses_(0), nrPoints_(0) {
+    }
+
+    /// Copy constructor
+    Values(const Base& base) :
+        Base(base), nrPoses_(0), nrPoints_(0) {
+    }
+
+    /// Insert a pose
+    void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
+      insert(i, p);
+      nrPoses_++;
+    }
+
+    /// Insert a point
+    void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
+      insert(j, p);
+      nrPoints_++;
+    }
+
+    /// Number of poses
+    int nrPoses() const {
+      return nrPoses_;
+    }
+
+    /// Number of points
+    int nrPoints() const {
+      return nrPoints_;
+    }
+
+    /// Return pose i
+    Point2 pose(const simulated2D::PoseKey& i) const {
+      return (*this)[i];
+    }
+
+    /// Return point j
+    Point2 point(const simulated2D::PointKey& j) const {
+      return (*this)[j];
+    }
+  };
+
+
   /// Prior on a single pose
   inline Point2 prior(const Point2& x) {
     return x;

gtsam/slam/simulated2DOriented.h

@@ -31,6 +31,30 @@ namespace simulated2DOriented {
   typedef TypedSymbol<Point2, 'l'> PointKey;
   typedef TypedSymbol<Pose2, 'x'> PoseKey;
 
+  /// Specialized Values structure with syntactic sugar for
+  /// compatibility with matlab
+  class Values: public gtsam::Values {
+  	int nrPoses_, nrPoints_;
+  public:
+    Values() : nrPoses_(0), nrPoints_(0)  {}
+
+    void insertPose(const PoseKey& i, const Pose2& p) {
+      insert(i, p);
+      nrPoses_++;
+    }
+
+    void insertPoint(const PointKey& j, const Point2& p) {
+      insert(j, p);
+      nrPoints_++;
+    }
+
+    int nrPoses() const {	return nrPoses_;	}
+    int nrPoints() const { return nrPoints_;	}
+
+    Pose2 pose(const PoseKey& i) const { return (*this)[i];	}
+    Point2 point(const PointKey& j) const { return (*this)[j]; }
+  };
+
   //TODO:: point prior is not implemented right now
 
   /// Prior on a single pose

gtsam/slam/tests/testPlanarSLAM.cpp

@@ -51,7 +51,7 @@ TEST( planarSLAM, BearingFactor )
 	planarSLAM::Bearing factor(2, 3, z, sigma);
 
 	// create config
-	Values c;
+	planarSLAM::Values c;
 	c.insert(PoseKey(2), x2);
 	c.insert(PointKey(3), l3);
 
@@ -79,7 +79,7 @@ TEST( planarSLAM, RangeFactor )
 	planarSLAM::Range factor(2, 3, z, sigma);
 
 	// create config
-	Values c;
+	planarSLAM::Values c;
 	c.insert(PoseKey(2), x2);
 	c.insert(PointKey(3), l3);
 
@@ -106,7 +106,7 @@ TEST( planarSLAM, BearingRangeFactor )
 	planarSLAM::BearingRange factor(2, 3, r, b, sigma2);
 
 	// create config
-	Values c;
+	planarSLAM::Values c;
 	c.insert(PoseKey(2), x2);
 	c.insert(PointKey(3), l3);
 
@@ -139,7 +139,7 @@ TEST( planarSLAM, PoseConstraint_equals )
 TEST( planarSLAM, constructor )
 {
 	// create config
-	Values c;
+	planarSLAM::Values c;
 	c.insert(PoseKey(2), x2);
 	c.insert(PoseKey(3), x3);
 	c.insert(PointKey(3), l3);

gtsam/slam/tests/testPose2SLAM.cpp

@@ -345,13 +345,13 @@ using namespace pose2SLAM;
 TEST(Pose2Values, pose2Circle )
 {
 	// expected is 4 poses tangent to circle with radius 1m
-	Values expected;
+	pose2SLAM::Values expected;
 	expected.insert(pose2SLAM::PoseKey(0), Pose2( 1,  0,   M_PI_2));
 	expected.insert(pose2SLAM::PoseKey(1), Pose2( 0,  1, - M_PI  ));
 	expected.insert(pose2SLAM::PoseKey(2), Pose2(-1,  0, - M_PI_2));
 	expected.insert(pose2SLAM::PoseKey(3), Pose2( 0, -1,   0     ));
 
-	Values actual = pose2SLAM::circle(4,1.0);
+	pose2SLAM::Values actual = pose2SLAM::circle(4,1.0);
 	CHECK(assert_equal(expected,actual));
 }
 
@@ -359,21 +359,21 @@ TEST(Pose2Values, pose2Circle )
 TEST(Pose2SLAM, expmap )
 {
 	// expected is circle shifted to right
-	Values expected;
+	pose2SLAM::Values expected;
 	expected.insert(pose2SLAM::PoseKey(0), Pose2( 1.1,  0,   M_PI_2));
 	expected.insert(pose2SLAM::PoseKey(1), Pose2( 0.1,  1, - M_PI  ));
 	expected.insert(pose2SLAM::PoseKey(2), Pose2(-0.9,  0, - M_PI_2));
 	expected.insert(pose2SLAM::PoseKey(3), Pose2( 0.1, -1,   0     ));
 
 	// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
-  Values circle(pose2SLAM::circle(4,1.0));
+	pose2SLAM::Values circle(pose2SLAM::circle(4,1.0));
   Ordering ordering(*circle.orderingArbitrary());
 	VectorValues delta(circle.dims(ordering));
 	delta[ordering[pose2SLAM::PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0);
 	delta[ordering[pose2SLAM::PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0);
 	delta[ordering[pose2SLAM::PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0);
 	delta[ordering[pose2SLAM::PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0);
-	Values actual = circle.retract(delta, ordering);
+	pose2SLAM::Values actual = circle.retract(delta, ordering);
 	CHECK(assert_equal(expected,actual));
 }
 
@@ -386,7 +386,7 @@ TEST( Pose2Prior, error )
 {
 	// Choose a linearization point
 	Pose2 p1(1, 0, 0); // robot at (1,0)
-	Values x0;
+	pose2SLAM::Values x0;
 	x0.insert(pose2SLAM::PoseKey(1), p1);
 
 	// Create factor
@@ -408,7 +408,7 @@ TEST( Pose2Prior, error )
 	VectorValues addition(VectorValues::Zero(x0.dims(ordering)));
 	addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0);
 	VectorValues plus = delta + addition;
-	Values x1 = x0.retract(plus, ordering);
+	pose2SLAM::Values x1 = x0.retract(plus, ordering);
 	Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
 	CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
 	CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@@ -430,7 +430,7 @@ LieVector hprior(const Pose2& p1) {
 TEST( Pose2Prior, linearize )
 {
 	// Choose a linearization point at ground truth
-	Values x0;
+	pose2SLAM::Values x0;
 	x0.insert(pose2SLAM::PoseKey(1),priorVal);
 
 	// Actual linearization
@@ -450,7 +450,7 @@ TEST( Pose2Factor, error )
 	// Choose a linearization point
 	Pose2 p1; // robot at origin
 	Pose2 p2(1, 0, 0); // robot at (1,0)
-	Values x0;
+	pose2SLAM::Values x0;
 	x0.insert(pose2SLAM::PoseKey(1), p1);
 	x0.insert(pose2SLAM::PoseKey(2), p2);
 
@@ -474,7 +474,7 @@ TEST( Pose2Factor, error )
 	// Check error after increasing p2
 	VectorValues plus = delta;
 	plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0);
-	Values x1 = x0.retract(plus, ordering);
+	pose2SLAM::Values x1 = x0.retract(plus, ordering);
 	Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
 	CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
 	CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@@ -491,7 +491,7 @@ TEST( Pose2Factor, rhs )
 	// Choose a linearization point
 	Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
 	Pose2 p2(-1,4.1,M_PI);  // robot at (-1,4.1) looking at negative (ground truth is at -1,4)
-	Values x0;
+	pose2SLAM::Values x0;
 	x0.insert(pose2SLAM::PoseKey(1),p1);
 	x0.insert(pose2SLAM::PoseKey(2),p2);
 
@@ -521,7 +521,7 @@ TEST( Pose2Factor, linearize )
 	// Choose a linearization point at ground truth
 	Pose2 p1(1,2,M_PI_2);
 	Pose2 p2(-1,4,M_PI);
-	Values x0;
+	pose2SLAM::Values x0;
 	x0.insert(pose2SLAM::PoseKey(1),p1);
 	x0.insert(pose2SLAM::PoseKey(2),p2);
 

gtsam/slam/tests/testSimulated2D.cpp

@@ -30,7 +30,7 @@ using namespace simulated2D;
 /* ************************************************************************* */
 TEST( simulated2D, Simulated2DValues )
 {
-	Values actual;
+	simulated2D::Values actual;
 	actual.insert(simulated2D::PoseKey(1),Point2(1,1));
 	actual.insert(simulated2D::PointKey(2),Point2(2,2));
   EXPECT(assert_equal(actual,actual,1e-9));

gtsam/slam/tests/testSimulated2DOriented.cpp

@@ -57,7 +57,7 @@ TEST( simulated2DOriented, constructor )
 	SharedDiagonal model(Vector_(3, 1., 1., 1.));
 	simulated2DOriented::Odometry factor(measurement, model, simulated2DOriented::PoseKey(1), simulated2DOriented::PoseKey(2));
 
-	Values config;
+	simulated2DOriented::Values config;
 	config.insert(simulated2DOriented::PoseKey(1), Pose2(1., 0., 0.2));
 	config.insert(simulated2DOriented::PoseKey(2), Pose2(2., 0., 0.1));
 	boost::shared_ptr<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary());

tests/testNonlinearISAM.cpp

@@ -30,8 +30,8 @@ TEST(testNonlinearISAM, markov_chain ) {
 	Graph start_factors;
 	start_factors.addPoseConstraint(key, cur_pose);
 
-	Values init;
-	Values expected;
+	planarSLAM::Values init;
+	planarSLAM::Values expected;
 	init.insert(key, cur_pose);
 	expected.insert(key, cur_pose);
 	isam.update(start_factors, init);
@@ -43,7 +43,7 @@ TEST(testNonlinearISAM, markov_chain ) {
 		Graph new_factors;
 		PoseKey key1(i-1), key2(i);
 		new_factors.addOdometry(key1, key2, z, model);
-		Values new_init;
+		planarSLAM::Values new_init;
 
 		// perform a check on changing orderings
 		if (i == 5) {
@@ -67,7 +67,7 @@ TEST(testNonlinearISAM, markov_chain ) {
 	}
 
 	// verify values - all but the last one should be very close
-	Values actual = isam.estimate();
+	planarSLAM::Values actual = isam.estimate();
 	for (size_t i=0; i<nrPoses; ++i) {
 		PoseKey cur_key(i);
 		EXPECT(assert_equal(expected[cur_key], actual[cur_key], tol));

tests/testSerialization.cpp

@@ -501,7 +501,7 @@ BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint,  "gtsam::planarSLAM::Constraint"
 /* ************************************************************************* */
 TEST (Serialization, planar_system) {
 	using namespace planarSLAM;
-	Values values;
+	planarSLAM::Values values;
 	values.insert(PointKey(3), Point2(1.0, 2.0));
 	values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3));
 
@@ -527,7 +527,7 @@ TEST (Serialization, planar_system) {
 	// text
 	EXPECT(equalsObj<PoseKey>(PoseKey(2)));
 	EXPECT(equalsObj<PointKey>(PointKey(3)));
-	EXPECT(equalsObj<Values>(values));
+	EXPECT(equalsObj<planarSLAM::Values>(values));
 	EXPECT(equalsObj<Prior>(prior));
 	EXPECT(equalsObj<Bearing>(bearing));
 	EXPECT(equalsObj<BearingRange>(bearingRange));
@@ -539,7 +539,7 @@ TEST (Serialization, planar_system) {
 	// xml
 	EXPECT(equalsXML<PoseKey>(PoseKey(2)));
 	EXPECT(equalsXML<PointKey>(PointKey(3)));
-	EXPECT(equalsXML<Values>(values));
+	EXPECT(equalsXML<planarSLAM::Values>(values));
 	EXPECT(equalsXML<Prior>(prior));
 	EXPECT(equalsXML<Bearing>(bearing));
 	EXPECT(equalsXML<BearingRange>(bearingRange));