Merge remote-tracking branch 'svn/trunk' into windows

Conflicts:
	gtsam/nonlinear/Marginals.h

.cproject

@@ -689,18 +689,26 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="nonlinear.testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
-				<buildTarget>nonlinear.testValues.run</buildTarget>
+				<buildTarget>testValues.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="nonlinear.testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
-				<buildTarget>nonlinear.testOrdering.run</buildTarget>
+				<buildTarget>testOrdering.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testKey.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>

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,26 @@ 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
+%% Query the marginals
-% marginals = gtsamMarginals(graph, result);
+marginals = graph.marginals(result);
-% marginals.marginalCovariance(pose2SLAMPoseKey(1))
+x{1}=gtsamSymbol('x',1); P{1}=marginals.marginalCovariance(x{1}.key)
-% marginals.marginalCovariance(pose2SLAMPoseKey(2))
+x{2}=gtsamSymbol('x',2); P{2}=marginals.marginalCovariance(x{2}.key)
-% marginals.marginalCovariance(pose2SLAMPoseKey(3))
+x{3}=gtsamSymbol('x',3); P{3}=marginals.marginalCovariance(x{3}.key)
+
+%% Plot Trajectory
+figure(1)
+clf
+X=[];Y=[];
+for i=1:3
+   pose_i = result.pose(i);
+   X=[X;pose_i.x];
+   Y=[Y;pose_i.y];
+end
+plot(X,Y,'b*-');
+
+%% Plot Covariance Ellipses
+hold on
+for i=1:3
+   pose_i = result.pose(i);
+   covarianceEllipse([pose_i.x;pose_i.y],P{i},'g')
+end

examples/matlab/covarianceEllipse.m

@@ -0,0 +1,34 @@
+function covarianceEllipse(x,P,color)
+% covarianceEllipse: plot a Gaussian as an uncertainty ellipse
+% Based on Maybeck Vol 1, page 366
+% k=2.296 corresponds to 1 std, 68.26% of all probability
+% k=11.82 corresponds to 3 std, 99.74% of all probability  
+%
+% covarianceEllipse(x,P,color)
+% it is assumed x and y are the first two components of state x
+
+[e,s] = eig(P(1:2,1:2));
+s1 = s(1,1);
+s2 = s(2,2);
+k = 2.296;
+[ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) );
+line(ex,ey,'color',color);
+
+function [x,y] = ellipse(a,b,c);
+% ellipse: return the x and y coordinates for an ellipse
+% [x,y] = ellipse(a,b,c);
+% a, and b are the axes. c is the center
+
+global ellipse_x ellipse_y
+if ~exist('elipse_x')
+  q =0:2*pi/25:2*pi;
+  ellipse_x = cos(q);
+  ellipse_y = sin(q);
+end
+
+points = a*ellipse_x + b*ellipse_y;
+x = c(1) + points(1,:);
+y = c(2) + points(2,:);
+end
+
+end

gtsam.h

@@ -347,13 +347,14 @@ class GaussianFactorGraph {
 
 	// Building the graph
 	void add(gtsam::JacobianFactor* factor);
-	void add(Vector b);
+	// all these won't work as MATLAB can't handle overloading
-	void add(int key1, Matrix A1, Vector b, const gtsam::SharedDiagonal& model);
+//	void add(Vector b);
-	void add(int key1, Matrix A1, int key2, Matrix A2, Vector b,
+//	void add(int key1, Matrix A1, Vector b, const gtsam::SharedDiagonal& model);
-			const gtsam::SharedDiagonal& model);
+//	void add(int key1, Matrix A1, int key2, Matrix A2, Vector b,
-	void add(int key1, Matrix A1, int key2, Matrix A2, int key3, Matrix A3,
+//			const gtsam::SharedDiagonal& model);
-			Vector b, const gtsam::SharedDiagonal& model);
+//	void add(int key1, Matrix A1, int key2, Matrix A2, int key3, Matrix A3,
-	void add(gtsam::HessianFactor* factor);
+//			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

gtsam/inference/BayesNet-inl.h

@@ -37,7 +37,7 @@ namespace gtsam {
 	/* ************************************************************************* */
 	template<class CONDITIONAL>
 	void BayesNet<CONDITIONAL>::print(const string& s) const {
-		cout << s << ":\n";
+		cout << s;
 		BOOST_REVERSE_FOREACH(sharedConditional conditional,conditionals_)
 			conditional->print();
 	}

gtsam/nonlinear/Marginals.cpp

@@ -43,6 +43,14 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution,
     bayesTree_ = *GaussianMultifrontalSolver(graph_, true).eliminate();
 }
 
+/* ************************************************************************* */
+void Marginals::print(const std::string& str, const KeyFormatter& keyFormatter) const {
+	ordering_.print(str+"Ordering: ", keyFormatter);
+	graph_.print(str+"Graph: ");
+	values_.print(str+"Solution: ", keyFormatter);
+	bayesTree_.print(str+"Bayes Tree: ");
+}
+
 /* ************************************************************************* */
 Matrix Marginals::marginalCovariance(Key variable) const {
   return marginalInformation(variable).inverse();

gtsam/nonlinear/Marginals.h

@@ -40,6 +40,16 @@ public:
     QR
   };
 
+protected:
+
+  GaussianFactorGraph graph_;
+  Ordering ordering_;
+  Values values_;
+  Factorization factorization_;
+  GaussianBayesTree bayesTree_;
+
+public:
+
   /** Construct a marginals class.
    * @param graph The factor graph defining the full joint density on all variables.
    * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer).
@@ -47,6 +57,9 @@ public:
    */
   Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
 
+  /** print */
+  void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+
   /** Compute the marginal covariance of a single variable */
   Matrix marginalCovariance(Key variable) const;
 
@@ -60,14 +73,6 @@ public:
 
   /** Compute the joint marginal information of several variables */
   JointMarginal jointMarginalInformation(const std::vector<Key>& variables) const;
-
-protected:
-
-  GaussianFactorGraph graph_;
-  Values values_;
-  Ordering ordering_;
-  Factorization factorization_;
-  GaussianBayesTree bayesTree_;
 };
 
 /**
@@ -76,7 +81,17 @@ protected:
 class JointMarginal {
 
 protected:
-  typedef SymmetricBlockView<Matrix> BlockView;
+
+	typedef SymmetricBlockView<Matrix> BlockView;
+
+  Matrix fullMatrix_;
+  BlockView blockView_;
+  Ordering indices_;
+
+  JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const Ordering& indices) :
+    fullMatrix_(fullMatrix), blockView_(fullMatrix_, dims.begin(), dims.end()), indices_(indices) {}
+
+  friend class Marginals;
 
 public:
   /** A block view of the joint marginal - this stores a reference to the
@@ -107,16 +122,6 @@ public:
 
   /** Assignment operator */
   JointMarginal& operator=(const JointMarginal& rhs);
-
-protected:
-  Matrix fullMatrix_;
-  BlockView blockView_;
-  Ordering indices_;
-
-  JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const Ordering& indices) :
-    fullMatrix_(fullMatrix), blockView_(fullMatrix_, dims.begin(), dims.end()), indices_(indices) {}
-
-  friend class Marginals;
 };
 
 } /* namespace gtsam */

gtsam/nonlinear/Symbol.h

@@ -18,8 +18,8 @@
 
 #pragma once
 
-#include <list>
+#include <gtsam/nonlinear/Key.h>
-#include <iostream>
+
 #include <boost/mpl/char.hpp>
 #include <boost/format.hpp>
 #include <boost/serialization/nvp.hpp>
@@ -28,7 +28,8 @@
 #include <boost/lambda/construct.hpp>
 #include <boost/lambda/lambda.hpp>
 
-#include <gtsam/nonlinear/Key.h>
+#include <list>
+#include <iostream>
 
 namespace gtsam {
 
@@ -70,8 +71,8 @@ public:
     j_ = key & indexMask;
   }
 
-  /** Cast to integer */
+  /** return Key (integer) representation */
-  operator Key() const {
+  Key key() const {
     const size_t keyBits = sizeof(Key) * 8;
     const size_t chrBits = sizeof(unsigned char) * 8;
     const size_t indexBits = keyBits - chrBits;
@@ -83,9 +84,12 @@ public:
     return key;
   }
 
+  /** Cast to integer */
+  operator Key() const { return key(); }
+
   // Testable Requirements
   void print(const std::string& s = "") const {
-    std::cout << s << ": " << (std::string) (*this) << std::endl;
+    std::cout << s << (std::string) (*this) << std::endl;
   }
   bool equals(const Symbol& expected, double tol = 0.0) const {
     return (*this) == expected;

gtsam/nonlinear/Values.h

@@ -24,8 +24,10 @@
 
 #pragma once
 
-#include <string>
+#include <gtsam/base/Value.h>
-#include <utility>
+#include <gtsam/base/FastMap.h>
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/nonlinear/Ordering.h>
 
 #include <boost/pool/pool_alloc.hpp>
 #include <boost/ptr_container/ptr_map.hpp>
@@ -37,10 +39,8 @@
 #include <boost/iterator_adaptors.hpp>
 #include <boost/lambda/lambda.hpp>
 
-#include <gtsam/base/Value.h>
+#include <string>
-#include <gtsam/base/FastMap.h>
+#include <utility>
-#include <gtsam/linear/VectorValues.h>
-#include <gtsam/nonlinear/Ordering.h>
 
 namespace gtsam {
 

gtsam/nonlinear/tests/testKey.cpp

@@ -27,11 +27,11 @@ using namespace gtsam;
 
 /* ************************************************************************* */
 TEST(Key, KeySymbolConversion) {
-  Symbol expected('j', 4);
+  Symbol original('j', 4);
-  Key key(expected);
+  Key key(original);
+  EXPECT(assert_equal(key, original.key()))
   Symbol actual(key);
-
+  EXPECT(assert_equal(original, actual))
-  EXPECT(assert_equal(expected, actual))
 }
 
 /* ************************************************************************* */

gtsam/slam/tests/testPose2SLAM.cpp

@@ -36,12 +36,12 @@ using pose2SLAM::PoseKey;
 
 // common measurement covariance
 static double sx=0.5, sy=0.5,st=0.1;
-static noiseModel::Gaussian::shared_ptr covariance(
+static Matrix cov(Matrix_(3, 3,
-		noiseModel::Gaussian::Covariance(Matrix_(3, 3,
 	sx*sx, 0.0, 0.0,
 	0.0, sy*sy, 0.0,
 	0.0, 0.0, st*st
-	)));
+	));
+static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Gaussian::Covariance(cov));
 //static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3));
 
 const Key kx0 = Symbol('x',0), kx1 = Symbol('x',1), kx2 = Symbol('x',2), kx3 = Symbol('x',3), kx4 = Symbol('x',4), kx5 = Symbol('x',5), kl1 = Symbol('l',1);
@@ -175,6 +175,12 @@ TEST(Pose2SLAM, optimize) {
 
   // Check marginals
   Marginals marginals = fg.marginals(actual);
+	// Matrix expectedP0 = Infinity, as we have a pose constraint !?
+	// Matrix actualP0 = marginals.marginalCovariance(pose2SLAM::PoseKey(0));
+	// EQUALITY(expectedP0, actualP0);
+	Matrix expectedP1 = cov; // the second pose really should have just the noise covariance
+	Matrix actualP1 = marginals.marginalCovariance(pose2SLAM::PoseKey(1));
+	EQUALITY(expectedP1, actualP1);
 }
 
 /* ************************************************************************* */

tests/CMakeLists.txt

@@ -12,7 +12,6 @@ set(tests_local_libs
 # exclude certain files
 # note the source dir on each 
 set (tests_exclude
-    "${CMAKE_CURRENT_SOURCE_DIR}/testPose2SLAMwSPCG.cpp"
     #"${CMAKE_CURRENT_SOURCE_DIR}/testOccupancyGrid.cpp"
 )
 

tests/testOccupancyGrid.cpp

@@ -286,84 +286,38 @@ TEST_UNSAFE( OccupancyGrid, Test1) {
 	//Build a small grid and test optimization
 
 	//Build small grid
-	double width 		=	20; 		//meters
+	double width 		=	3; 		//meters
-	double height 		= 	20; 		//meters
+	double height 		= 	2; 		//meters
-	double resolution 	= 	0.2; 	//meters
+	double resolution 	= 	0.5; 	//meters
 	OccupancyGrid occupancyGrid(width, height, resolution); //default center to middle
 
 	//Add measurements
-//	Pose2 pose(0,0,0);
+	Pose2 pose(0,0,0);
-//	double range = 4.499765;
+	double range = 1;
-//
-//	occupancyGrid.addPrior(0, 0.7);
-//	EXPECT_LONGS_EQUAL(1, occupancyGrid.size());
-//
-//	occupancyGrid.addLaser(pose, range);
-//	EXPECT_LONGS_EQUAL(2, occupancyGrid.size());
 
-	//add lasers
+	occupancyGrid.addPrior(0, 0.7);
-	int n_frames = 1;
+	EXPECT_LONGS_EQUAL(1, occupancyGrid.size());
-	int n_lasers_per_frame = 640;
+
-	char laser_list_file[1000];
+	occupancyGrid.addLaser(pose, range);
+	EXPECT_LONGS_EQUAL(2, occupancyGrid.size());
+
+	OccupancyGrid::Occupancy occupancy = occupancyGrid.emptyOccupancy();
+	EXPECT_LONGS_EQUAL(900, occupancyGrid.laserFactorValue(0,occupancy));
 
 
-	for(int i = 0; i < n_frames; i++){
+	occupancy[16] = 1;
-		sprintf(laser_list_file, "/home/brian/Desktop/research/user/bpeasle/code/KinectInterface/Data/ScanLinesAsLasers/KinectRecording9/laser_list%.4d", i);
+	EXPECT_LONGS_EQUAL(1, occupancyGrid.laserFactorValue(0,occupancy));
-		FILE *fptr = fopen(laser_list_file,"r");
-		double x,y, theta;
-		double range, angle;
-		fscanf(fptr, "%lf %lf %lf", &x, &y, &theta);
 
-		for(int j = 0; j < n_lasers_per_frame; j++){
+	occupancy[15] = 1;
-			fscanf(fptr, "%lf %lf", &range, &angle);
+	EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
-			//if(j == 159){
-				Pose2 pose(x,y, theta+angle);
 
-				occupancyGrid.addLaser(pose, range);
+	occupancy[16] = 0;
-			//}
+	EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
-		}
-		fclose(fptr);
-
-	}
-
-
-//	OccupancyGrid::Occupancy occupancy = occupancyGrid.emptyOccupancy();
-//	EXPECT_LONGS_EQUAL(900, occupancyGrid.laserFactorValue(0,occupancy));
-//
-//
-//	occupancy[16] = 1;
-//	EXPECT_LONGS_EQUAL(1, occupancyGrid.laserFactorValue(0,occupancy));
-//
-//	occupancy[15] = 1;
-//	EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
-//
-//	occupancy[16] = 0;
-//	EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
 
 
 	//run MCMC
 	OccupancyGrid::Marginals occupancyMarginals = occupancyGrid.runMetropolis(50000);
-	//EXPECT_LONGS_EQUAL( (width*height)/pow(resolution,2), occupancyMarginals.size());
+	EXPECT_LONGS_EQUAL( (width*height)/pow(resolution,2), occupancyMarginals.size());
-	//select a cell at a random to flip
-
-
-	printf("\n");
-	for(size_t i = 0, it = 0; i < occupancyGrid.height(); i++){
-		for(size_t j = 0; j < occupancyGrid.width(); j++, it++){
-			printf("%.2lf ", occupancyMarginals[it]);
-		}
-		printf("\n");
-	}
-
-	char marginalsOutput[1000];
-	sprintf(marginalsOutput, "/home/brian/Desktop/research/user/bpeasle/code/KinectInterface/marginals.txt");
-	FILE *fptr = fopen(marginalsOutput, "w");
-	fprintf(fptr, "%d %d\n", occupancyGrid.width(), occupancyGrid.height());
-
-	for(int i = 0; i < occupancyMarginals.size(); i++){
-		fprintf(fptr, "%lf ", occupancyMarginals[i]);
-	}
-	fclose(fptr);
 
 
 

tests/testPose2SLAMwSPCG.cpp

@@ -1,72 +0,0 @@
-/**
- * @file testPose2SLAMwSPCG
- * @author Alex Cunningham
- */
-
-#include <CppUnitLite/TestHarness.h>
-
-#include <gtsam/slam/pose2SLAM.h>
-#include <gtsam/nonlinear/NonlinearOptimization.h>
-
-using namespace std;
-using namespace gtsam;
-using namespace pose2SLAM;
-
-const double tol = 1e-5;
-
-/* ************************************************************************* */
-TEST(testPose2SLAMwSPCG, example1) {
-
-	/* generate synthetic data */
-	const SharedNoiseModel sigma(noiseModel::Unit::Create(0.1));
-	Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
-
-	// create a 3 by 3 grid
-	// x3 x6 x9
-	// x2 x5 x8
-	// x1 x4 x7
-	Graph graph;
-	graph.addConstraint(x1,x2,Pose2(0,2,0),sigma) ;
-	graph.addConstraint(x2,x3,Pose2(0,2,0),sigma) ;
-	graph.addConstraint(x4,x5,Pose2(0,2,0),sigma) ;
-	graph.addConstraint(x5,x6,Pose2(0,2,0),sigma) ;
-	graph.addConstraint(x7,x8,Pose2(0,2,0),sigma) ;
-	graph.addConstraint(x8,x9,Pose2(0,2,0),sigma) ;
-	graph.addConstraint(x1,x4,Pose2(2,0,0),sigma) ;
-	graph.addConstraint(x4,x7,Pose2(2,0,0),sigma) ;
-	graph.addConstraint(x2,x5,Pose2(2,0,0),sigma) ;
-	graph.addConstraint(x5,x8,Pose2(2,0,0),sigma) ;
-	graph.addConstraint(x3,x6,Pose2(2,0,0),sigma) ;
-	graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ;
-	graph.addPrior(x1, Pose2(0,0,0), sigma) ;
-
-	Values initial;
-	initial.insert(x1, Pose2(  0,  0,   0));
-	initial.insert(x2, Pose2(  0, 2.1, 0.01));
-	initial.insert(x3, Pose2(  0, 3.9,-0.01));
-	initial.insert(x4, Pose2(2.1,-0.1,    0));
-	initial.insert(x5, Pose2(1.9, 2.1, 0.02));
-	initial.insert(x6, Pose2(2.0, 3.9,-0.02));
-	initial.insert(x7, Pose2(4.0, 0.1, 0.03 ));
-	initial.insert(x8, Pose2(3.9, 2.1, 0.01));
-	initial.insert(x9, Pose2(4.1, 3.9,-0.01));
-
-	Values expected;
-	expected.insert(x1, Pose2(0.0, 0.0, 0.0));
-	expected.insert(x2, Pose2(0.0, 2.0, 0.0));
-	expected.insert(x3, Pose2(0.0, 4.0, 0.0));
-	expected.insert(x4, Pose2(2.0, 0.0, 0.0));
-	expected.insert(x5, Pose2(2.0, 2.0, 0.0));
-	expected.insert(x6, Pose2(2.0, 4.0, 0.0));
-	expected.insert(x7, Pose2(4.0, 0.0, 0.0 ));
-	expected.insert(x8, Pose2(4.0, 2.0, 0.0));
-	expected.insert(x9, Pose2(4.0, 4.0, 0.0));
-
-	Values actual = optimizeSPCG(graph, initial);
-
-	EXPECT(assert_equal(expected, actual, tol));
-}
-
-/* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
-/* ************************************************************************* */

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);
+  return myGetScalar<char>(array);
-}
-
-// specialization to size_t
-template<>
-size_t unwrap<size_t>(const mxArray* array) {
-	checkScalar(array,"unwrap<size_t>");
-  return (size_t)mxGetScalar(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