Added marginals in MATLAB, but had to fix a bug in converting from 64 bit ints to size_t. This begs the question what happens on 32-bit machines with Symbols.

examples/LocalizationExample.cpp

@@ -64,7 +64,7 @@ int main(int argc, char** argv) {
 	result.print("\nFinal result:\n  ");
 
 	// Query the marginals
-	Marginals marginals(graph, result);
+	Marginals marginals = graph.marginals(result);
 	cout.precision(2);
   cout << "\nP1:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(1)) << endl;
   cout << "\nP2:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(2)) << endl;

examples/matlab/LocalizationExample.m

@@ -43,8 +43,8 @@ initialEstimate.print(sprintf('\nInitial estimate:\n  '));
 result = graph.optimize(initialEstimate);
 result.print(sprintf('\nFinal result:\n  '));
 
-%% Use an explicit Optimizer object so we can query the marginals
-% marginals = gtsamMarginals(graph, result);
-% marginals.marginalCovariance(pose2SLAMPoseKey(1))
-% marginals.marginalCovariance(pose2SLAMPoseKey(2))
-% marginals.marginalCovariance(pose2SLAMPoseKey(3))
+%% Query the marginals
+marginals = graph.marginals(result);
+x1=gtsamSymbol('x',1); marginals.marginalCovariance(x1.key)
+x2=gtsamSymbol('x',2); marginals.marginalCovariance(x2.key)
+x3=gtsamSymbol('x',3); marginals.marginalCovariance(x3.key)

gtsam.h

@@ -347,13 +347,14 @@ class GaussianFactorGraph {
 
 	// Building the graph
 	void add(gtsam::JacobianFactor* factor);
-	void add(Vector b);
-	void add(int key1, Matrix A1, Vector b, const gtsam::SharedDiagonal& model);
-	void add(int key1, Matrix A1, int key2, Matrix A2, Vector b,
-			const gtsam::SharedDiagonal& model);
-	void add(int key1, Matrix A1, int key2, Matrix A2, int key3, Matrix A3,
-			Vector b, const gtsam::SharedDiagonal& model);
-	void add(gtsam::HessianFactor* factor);
+	// all these won't work as MATLAB can't handle overloading
+//	void add(Vector b);
+//	void add(int key1, Matrix A1, Vector b, const gtsam::SharedDiagonal& model);
+//	void add(int key1, Matrix A1, int key2, Matrix A2, Vector b,
+//			const gtsam::SharedDiagonal& model);
+//	void add(int key1, Matrix A1, int key2, Matrix A2, int key3, Matrix A3,
+//			Vector b, const gtsam::SharedDiagonal& model);
+//	void add(gtsam::HessianFactor* factor);
 
 	// error and probability
 	double error(const gtsam::VectorValues& c) const;
@@ -380,7 +381,7 @@ class GaussianSequentialSolver {
 
 class KalmanFilter {
 	KalmanFilter(size_t n);
-	gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0);
+	// gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0);
 	gtsam::GaussianDensity* init(Vector x0, Matrix P0);
 	void print(string s) const;
 	static int step(gtsam::GaussianDensity* p);
@@ -400,6 +401,12 @@ class KalmanFilter {
 // nonlinear
 //*************************************************************************
 
+class Symbol {
+  Symbol(char c, size_t j);
+	void print(string s) const;
+  size_t key() const;
+};
+
 class Ordering {
 	Ordering();
 	void print(string s) const;
@@ -407,20 +414,21 @@ class Ordering {
 	void push_back(size_t key);
 };
 
-//Andrew says: Required definitions for Marginal arguments
 class NonlinearFactorGraph {
     NonlinearFactorGraph();
-    //This need to be populated with whatever functions might be needed.
 };
 
 class Values {
     Values();
-    //Same here
+  	void print(string s) const;
+    bool exists(size_t j) const;
 };
 
-// Frank says: this does not work. Why not?
 class Marginals {
 	Marginals(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& solution);
+  void print(string s) const;
+  Matrix marginalCovariance(size_t variable) const;
+  Matrix marginalInformation(size_t variable) const;
 };
 
 }///\namespace gtsam
@@ -471,7 +479,7 @@ class Odometry {
 }///\namespace planarSLAM
 
 //*************************************************************************
-// gtsam::Pose2SLAM
+// Pose2SLAM
 //*************************************************************************
 
 #include <gtsam/slam/pose2SLAM.h>
@@ -481,6 +489,7 @@ class Values {
   Values();
   void print(string s) const;
   void insertPose(int key, const gtsam::Pose2& pose);
+  gtsam::Symbol poseKey(int i);
   gtsam::Pose2 pose(int i);
 };
 
@@ -497,7 +506,8 @@ class Graph {
   void addPrior(int key, const gtsam::Pose2& pose, const gtsam::SharedNoiseModel& noiseModel);
   void addPoseConstraint(int key, const gtsam::Pose2& pose);
   void addOdometry(int key1, int key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
-  pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate);
+  pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate) const;
+  gtsam::Marginals marginals(const pose2SLAM::Values& solution) const;
 };
 
 }///\namespace pose2SLAM

wrap/matlab.h

@@ -206,39 +206,53 @@ string unwrap<string>(const mxArray* array) {
   return str;
 }
 
+// Check for 64-bit, as Mathworks says mxGetScalar only good for 32 bit
+template <typename T>
+T myGetScalar(const mxArray* array) {
+	switch (mxGetClassID(array)) {
+		case mxINT64_CLASS:
+			return (T) *(int64_t*) mxGetData(array);
+		case mxUINT64_CLASS:
+			return (T) *(uint64_t*) mxGetData(array);
+		default:
+			// hope for the best!
+			return (T) mxGetScalar(array);
+	}
+}
+
 // specialization to bool
 template<>
 bool unwrap<bool>(const mxArray* array) {
 	checkScalar(array,"unwrap<bool>");
-  return mxGetScalar(array) != 0.0;
+  return myGetScalar<bool>(array);
 }
 
 // specialization to bool
 template<>
 char unwrap<char>(const mxArray* array) {
 	checkScalar(array,"unwrap<char>");
-  return (char)mxGetScalar(array);
-}
-
-// specialization to size_t
-template<>
-size_t unwrap<size_t>(const mxArray* array) {
-	checkScalar(array,"unwrap<size_t>");
-  return (size_t)mxGetScalar(array);
+  return myGetScalar<char>(array);
 }
 
 // specialization to int
 template<>
 int unwrap<int>(const mxArray* array) {
 	checkScalar(array,"unwrap<int>");
-  return (int)mxGetScalar(array);
+  return myGetScalar<int>(array);
+}
+
+// specialization to size_t
+template<>
+size_t unwrap<size_t>(const mxArray* array) {
+	checkScalar(array, "unwrap<size_t>");
+  return myGetScalar<size_t>(array);
 }
 
 // specialization to double
 template<>
 double unwrap<double>(const mxArray* array) {
 	checkScalar(array,"unwrap<double>");
-  return (double)mxGetScalar(array);
+  return myGetScalar<double>(array);
 }
 
 // specialization to Eigen vector