Fixed keys in planarSLAM matlab example

examples/matlab/PlanarSLAMExample_easy.m

@@ -20,24 +20,24 @@
 %  - Landmarks are 2 meters away from the robot trajectory
 
 %% Create keys for variables
-x1 = 1; x2 = 2; x3 = 3;
+x1 = symbol('x',1); x2 = symbol('x',2); x3 = symbol('x',3);
-l1 = 1; l2 = 2;
+l1 = symbol('l',1); l2 = symbol('l',2);
 
 %% Create graph container and add factors to it
 graph = planarSLAMGraph;
 
 %% Add prior
 % gaussian for prior
-prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
+priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
-prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
+priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
-graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
+graph.addPrior(x1, priorMean, priorNoise); % add directly to graph
 
 %% Add odometry
 % general noisemodel for odometry
-odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
-odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
+odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
-graph.addOdometry(x1, x2, odom_measurement, odom_model);
+graph.addOdometry(x1, x2, odometry, odometryNoise);
-graph.addOdometry(x2, x3, odom_measurement, odom_model);
+graph.addOdometry(x2, x3, odometry, odometryNoise);
 
 %% Add measurements
 % general noisemodel for measurements

examples/matlab/symbol.m

@@ -0,0 +1,4 @@
+function key = symbol(c,i)
+% generate a key corresponding to a symbol
+s = gtsamSymbol(c,i);
+key = s.key();

gtsam.h

@@ -9,7 +9,7 @@
  *   Only one Method/Constructor per line
  *   Methods can return
  *     - Eigen types:       Matrix, Vector
- *     - C/C++ basic types: string, bool, size_t, int, double, char
+ *     - C/C++ basic types: string, bool, size_t, size_t, double, char
  *     - void
  *     - Any class with which be copied with boost::make_shared()
  *     - boost::shared_ptr of any object type
@@ -19,7 +19,7 @@
  *   Arguments to functions any of
  *   	 - Eigen types:       Matrix, Vector
  *   	 - Eigen types and classes as an optionally const reference
- *     - C/C++ basic types: string, bool, size_t, int, double, char
+ *     - C/C++ basic types: string, bool, size_t, size_t, double, char
  *     - Any class with which be copied with boost::make_shared() (except Eigen)
  *     - boost::shared_ptr of any object type (except Eigen)
  *   Comments can use either C++ or C style, with multiple lines
@@ -150,7 +150,7 @@ class Rot3 {
 	static Vector Logmap(const gtsam::Rot3& p);
 	Matrix matrix() const;
 	Matrix transpose() const;
-	gtsam::Point3 column(int index) const;
+	gtsam::Point3 column(size_t index) const;
 	Vector xyz() const;
 	Vector ypr() const;
 	Vector rpy() const;
@@ -173,7 +173,7 @@ class Pose2 {
 	double x() const;
 	double y() const;
 	double theta() const;
-	int dim() const;
+	size_t dim() const;
 	Vector localCoordinates(const gtsam::Pose2& p);
 	gtsam::Pose2 retract(Vector v);
 	gtsam::Pose2 compose(const gtsam::Pose2& p2);
@@ -228,10 +228,13 @@ class CalibratedCamera {
 	static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
 };
 
+
 //*************************************************************************
 // inference
 //*************************************************************************
 
+
+
 //*************************************************************************
 // linear
 //*************************************************************************
@@ -260,25 +263,25 @@ class SharedNoiseModel {
 
 class VectorValues {
 	VectorValues();
-	VectorValues(int nVars, int varDim);
+	VectorValues(size_t nVars, size_t varDim);
 	void print(string s) const;
 	bool equals(const gtsam::VectorValues& expected, double tol) const;
-	int size() const;
+	size_t size() const;
-	void insert(int j, Vector value);
+	void insert(size_t j, Vector value);
 };
 
 class GaussianConditional {
-	GaussianConditional(int key, Vector d, Matrix R, Vector sigmas);
+	GaussianConditional(size_t key, Vector d, Matrix R, Vector sigmas);
-	GaussianConditional(int key, Vector d, Matrix R, int name1, Matrix S,
+	GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
 			Vector sigmas);
-	GaussianConditional(int key, Vector d, Matrix R, int name1, Matrix S,
+	GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
-			int name2, Matrix T, Vector sigmas);
+			size_t name2, Matrix T, Vector sigmas);
 	void print(string s) const;
 	bool equals(const gtsam::GaussianConditional &cg, double tol) const;
 };
 
 class GaussianDensity {
-	GaussianDensity(int key, Vector d, Matrix R, Vector sigmas);
+	GaussianDensity(size_t key, Vector d, Matrix R, Vector sigmas);
 	void print(string s) const;
 	Vector mean() const;
 	Matrix information() const;
@@ -302,11 +305,11 @@ class GaussianFactor {
 class JacobianFactor {
 	JacobianFactor();
 	JacobianFactor(Vector b_in);
-	JacobianFactor(int i1, Matrix A1, Vector b,
+	JacobianFactor(size_t i1, Matrix A1, Vector b,
 			const gtsam::SharedDiagonal& model);
-	JacobianFactor(int i1, Matrix A1, int i2, Matrix A2, Vector b,
+	JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b,
 			const gtsam::SharedDiagonal& model);
-	JacobianFactor(int i1, Matrix A1, int i2, Matrix A2, int i3, Matrix A3,
+	JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3,
 			Vector b, const gtsam::SharedDiagonal& model);
 	void print(string s) const;
 	bool equals(const gtsam::GaussianFactor& lf, double tol) const;
@@ -319,11 +322,11 @@ class JacobianFactor {
 class HessianFactor {
 	HessianFactor(const gtsam::HessianFactor& gf);
 	HessianFactor();
-	HessianFactor(int j, Matrix G, Vector g, double f);
+	HessianFactor(size_t j, Matrix G, Vector g, double f);
-	HessianFactor(int j, Vector mu, Matrix Sigma);
+	HessianFactor(size_t j, Vector mu, Matrix Sigma);
-	HessianFactor(int j1, int j2, Matrix G11, Matrix G12, Vector g1, Matrix G22,
+	HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22,
 			Vector g2, double f);
-	HessianFactor(int j1, int j2, int j3, Matrix G11, Matrix G12, Matrix G13,
+	HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13,
 			Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3,
 			double f);
 	HessianFactor(const gtsam::GaussianConditional& cg);
@@ -341,16 +344,16 @@ class GaussianFactorGraph {
 	void push_back(gtsam::GaussianFactor* factor);
 	void print(string s) const;
 	bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const;
-	int size() const;
+	size_t size() const;
 
 	// Building the graph
 	void add(gtsam::JacobianFactor* 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(size_t key1, Matrix A1, Vector b, const gtsam::SharedDiagonal& model);
-//	void add(int key1, Matrix A1, int key2, Matrix A2, Vector b,
+//	void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b,
 //			const gtsam::SharedDiagonal& model);
-//	void add(int key1, Matrix A1, int key2, Matrix A2, int key3, Matrix A3,
+//	void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3,
 //			Vector b, const gtsam::SharedDiagonal& model);
 //	void add(gtsam::HessianFactor* factor);
 
@@ -375,8 +378,8 @@ class GaussianSequentialSolver {
 			bool useQR);
 	gtsam::GaussianBayesNet* eliminate() const;
 	gtsam::VectorValues* optimize() const;
-	gtsam::GaussianFactor* marginalFactor(int j) const;
+	gtsam::GaussianFactor* marginalFactor(size_t j) const;
-	Matrix marginalCovariance(int j) const;
+	Matrix marginalCovariance(size_t j) const;
 };
 
 class KalmanFilter {
@@ -384,7 +387,7 @@ class KalmanFilter {
 	// 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);
+	static size_t step(gtsam::GaussianDensity* p);
 	gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F,
 			Matrix B, Vector u, const gtsam::SharedDiagonal& modelQ);
 	gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F,
@@ -420,6 +423,7 @@ class NonlinearFactorGraph {
 
 class Values {
 	Values();
+	size_t size() const;
 	void print(string s) const;
 	bool exists(size_t j) const;
 };
@@ -432,57 +436,7 @@ class Marginals {
 	Matrix marginalInformation(size_t variable) const;
 };
 
-} ///\namespace gtsam
+}///\namespace gtsam
-
-//*************************************************************************
-// planarSLAM
-//*************************************************************************
-
-#include <gtsam/slam/planarSLAM.h>
-namespace planarSLAM {
-
-class Values {
-	Values();
-	void print(string s) const;
-	gtsam::Pose2 pose(int key) const;
-	gtsam::Point2 point(int key) const;
-	void insertPose(int key, const gtsam::Pose2& pose);
-	void insertPoint(int key, const gtsam::Point2& point);
-};
-
-class Graph {
-	Graph();
-
-	void print(string s) const;
-
-	double error(const planarSLAM::Values& values) const;
-	gtsam::Ordering* orderingCOLAMD(const planarSLAM::Values& values) const;
-	gtsam::GaussianFactorGraph* linearize(const planarSLAM::Values& values,
-			const gtsam::Ordering& ordering) const;
-
-	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);
-	void addBearing(int poseKey, int pointKey, const gtsam::Rot2& bearing,
-			const gtsam::SharedNoiseModel& noiseModel);
-	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);
-	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 planarSLAM::Values& center,
-			const gtsam::Ordering& ordering) const;
-};
-
-} ///\namespace planarSLAM
 
 //*************************************************************************
 // Pose2SLAM
@@ -493,9 +447,10 @@ namespace pose2SLAM {
 
 class Values {
 	Values();
+	void insertPose(size_t key, const gtsam::Pose2& pose);
+    size_t size() const;
 	void print(string s) const;
-	void insertPose(int key, const gtsam::Pose2& pose);
+	gtsam::Pose2 pose(size_t i);
-	gtsam::Pose2 pose(int i);
 };
 
 class Graph {
@@ -508,16 +463,68 @@ class Graph {
 	gtsam::GaussianFactorGraph* linearize(const pose2SLAM::Values& values,
 			const gtsam::Ordering& ordering) const;
 
-	void addPrior(int key, const gtsam::Pose2& pose,
+	void addPrior(size_t key, const gtsam::Pose2& pose,
 			const gtsam::SharedNoiseModel& noiseModel);
-	void addPoseConstraint(int key, const gtsam::Pose2& pose);
+	void addPoseConstraint(size_t key, const gtsam::Pose2& pose);
-	void addOdometry(int key1, int key2, const gtsam::Pose2& odometry,
+	void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry,
+			const gtsam::SharedNoiseModel& noiseModel);
+	void addConstraint(size_t key1, size_t key2, const gtsam::Pose2& odometry,
 			const gtsam::SharedNoiseModel& noiseModel);
 	pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate) const;
 	gtsam::Marginals marginals(const pose2SLAM::Values& solution) const;
 };
 
-} ///\namespace pose2SLAM
+}///\namespace pose2SLAM
+
+//*************************************************************************
+// planarSLAM
+//*************************************************************************
+
+#include <gtsam/slam/planarSLAM.h>
+namespace planarSLAM {
+
+class Values {
+	Values();
+	void insertPose(size_t key, const gtsam::Pose2& pose);
+	void insertPoint(size_t key, const gtsam::Point2& point);
+	void print(string s) const;
+	gtsam::Pose2 pose(size_t key) const;
+	gtsam::Point2 point(size_t key) const;
+};
+
+class Graph {
+	Graph();
+
+	void print(string s) const;
+
+	double error(const planarSLAM::Values& values) const;
+	gtsam::Ordering* orderingCOLAMD(const planarSLAM::Values& values) const;
+	gtsam::GaussianFactorGraph* linearize(const planarSLAM::Values& values,
+			const gtsam::Ordering& ordering) const;
+
+	void addPrior(size_t key, const gtsam::Pose2& pose,
+			const gtsam::SharedNoiseModel& noiseModel);
+	void addPoseConstraint(size_t key, const gtsam::Pose2& pose);
+	void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry,
+			const gtsam::SharedNoiseModel& noiseModel);
+	void addBearing(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing,
+			const gtsam::SharedNoiseModel& noiseModel);
+	void addRange(size_t poseKey, size_t pointKey, double range,
+			const gtsam::SharedNoiseModel& noiseModel);
+	void addBearingRange(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing,
+			double range, const gtsam::SharedNoiseModel& noiseModel);
+	planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
+};
+
+class Odometry {
+	Odometry(size_t key1, size_t key2, const gtsam::Pose2& measured,
+			const gtsam::SharedNoiseModel& model);
+	void print(string s) const;
+	gtsam::GaussianFactor* linearize(const planarSLAM::Values& center,
+			const gtsam::Ordering& ordering) const;
+};
+
+}///\namespace planarSLAM
 
 //*************************************************************************
 // Simulated2D
@@ -528,12 +535,12 @@ namespace simulated2D {
 
 class Values {
 	Values();
-	void insertPose(int i, const gtsam::Point2& p);
+	void insertPose(size_t i, const gtsam::Point2& p);
-	void insertPoint(int j, const gtsam::Point2& p);
+	void insertPoint(size_t j, const gtsam::Point2& p);
-	int nrPoses() const;
+	size_t nrPoses() const;
-	int nrPoints() const;
+	size_t nrPoints() const;
-	gtsam::Point2 pose(int i);
+	gtsam::Point2 pose(size_t i);
-	gtsam::Point2 point(int j);
+	gtsam::Point2 point(size_t j);
 };
 
 class Graph {
@@ -550,12 +557,12 @@ namespace simulated2DOriented {
 
 class Values {
 	Values();
-	void insertPose(int i, const gtsam::Pose2& p);
+	void insertPose(size_t i, const gtsam::Pose2& p);
-	void insertPoint(int j, const gtsam::Point2& p);
+	void insertPoint(size_t j, const gtsam::Point2& p);
-	int nrPoses() const;
+	size_t nrPoses() const;
-	int nrPoints() const;
+	size_t nrPoints() const;
-	gtsam::Pose2 pose(int i);
+	gtsam::Pose2 pose(size_t i);
-	gtsam::Point2 point(int j);
+	gtsam::Point2 point(size_t j);
 };
 
 class Graph {