(2.0_prep branch) Merged in virtual_values branch

examples/CameraResectioning.cpp

@@ -20,7 +20,6 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/SimpleCamera.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimization.h>
@@ -28,13 +27,12 @@
 using namespace gtsam;
 
 typedef TypedSymbol<Pose3, 'x'> PoseKey;
-typedef Values<PoseKey> PoseValues;
 
 /**
  * Unary factor for the pose.
  */
-class ResectioningFactor: public NonlinearFactor1<PoseValues, PoseKey> {
+class ResectioningFactor: public NonlinearFactor1<PoseKey> {
-  typedef NonlinearFactor1<PoseValues, PoseKey> Base;
+  typedef NonlinearFactor1<PoseKey> Base;
 
   shared_ptrK K_; // camera's intrinsic parameters
   Point3 P_; // 3D point on the calibration rig
@@ -46,6 +44,8 @@ public:
     Base(model, key), K_(calib), P_(P), p_(p) {
   }
 
+  virtual ~ResectioningFactor() {}
+
   virtual Vector evaluateError(const Pose3& X, boost::optional<Matrix&> H =
       boost::none) const {
     SimpleCamera camera(*K_, X);
@@ -72,7 +72,7 @@ int main(int argc, char* argv[]) {
   PoseKey X(1);
 
   /* 1. create graph */
-  NonlinearFactorGraph<PoseValues> graph;
+  NonlinearFactorGraph graph;
 
   /* 2. add factors to the graph */
   // add measurement factors
@@ -92,14 +92,13 @@ int main(int argc, char* argv[]) {
   graph.push_back(factor);
 
   /* 3. Create an initial estimate for the camera pose */
-  PoseValues initial;
+  Values initial;
   initial.insert(X, Pose3(Rot3(1.,0.,0.,
                                0.,-1.,0.,
                                0.,0.,-1.), Point3(0.,0.,2.0)));
 
   /* 4. Optimize the graph using Levenberg-Marquardt*/
-  PoseValues result = optimize<NonlinearFactorGraph<PoseValues> , PoseValues> (
+  Values result = optimize<NonlinearFactorGraph> (graph, initial);
-      graph, initial);
   result.print("Final result: ");
 
   return 0;

examples/Makefile.am

@@ -16,8 +16,8 @@ noinst_PROGRAMS += PlanarSLAMExample_easy			# Solves SLAM example from tutorial
 noinst_PROGRAMS += PlanarSLAMSelfContained_advanced	# Solves SLAM example from tutorial with all typedefs in the script
 noinst_PROGRAMS += Pose2SLAMExample_easy 			# Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface
 noinst_PROGRAMS += Pose2SLAMExample_advanced		# Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface
-noinst_PROGRAMS += Pose2SLAMwSPCG_easy      		# Solves a simple Pose2 SLAM example with advanced SPCG solver
+#noinst_PROGRAMS += Pose2SLAMwSPCG_easy      		# Solves a simple Pose2 SLAM example with advanced SPCG solver
-noinst_PROGRAMS += Pose2SLAMwSPCG_advanced  		# Solves a simple Pose2 SLAM example with easy SPCG solver
+#noinst_PROGRAMS += Pose2SLAMwSPCG_advanced  		# Solves a simple Pose2 SLAM example with easy SPCG solver
 noinst_PROGRAMS += elaboratePoint2KalmanFilter 		# simple linear Kalman filter on a moving 2D point, but done using factor graphs
 noinst_PROGRAMS += easyPoint2KalmanFilter 		    # uses the cool generic templated Kalman filter class to do the same
 noinst_PROGRAMS += CameraResectioning

examples/PlanarSLAMExample_easy.cpp

@@ -76,7 +76,7 @@ int main(int argc, char** argv) {
 	graph.print("full graph");
 
 	// initialize to noisy points
-	planarSLAM::Values initialEstimate;
+	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
-	planarSLAM::Values result = optimize<Graph, planarSLAM::Values>(graph, initialEstimate);
+	Values result = optimize<Graph>(graph, initialEstimate);
 	result.print("final result");
 
 	return 0;

examples/PlanarSLAMSelfContained_advanced.cpp

@@ -34,7 +34,6 @@
 #include <gtsam/slam/BearingRangeFactor.h>
 
 // implementations for structures - needed if self-contained, and these should be included last
-#include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
@@ -43,12 +42,9 @@
 // Main typedefs
 typedef gtsam::TypedSymbol<gtsam::Pose2, 'x'> PoseKey;       // Key for poses, with type included
 typedef gtsam::TypedSymbol<gtsam::Point2,'l'> PointKey;      // Key for points, with type included
-typedef gtsam::Values<PoseKey> PoseValues;                // config type for poses
+typedef gtsam::NonlinearFactorGraph Graph;			 // graph structure
-typedef gtsam::Values<PointKey> PointValues;              // config type for points
+typedef gtsam::NonlinearOptimizer<Graph,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential;   // optimization engine for this domain
-typedef gtsam::TupleValues2<PoseValues, PointValues> PlanarValues; // main config with two variable classes
+typedef gtsam::NonlinearOptimizer<Graph,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal;   // optimization engine for this domain
-typedef gtsam::NonlinearFactorGraph<PlanarValues> Graph;			 // graph structure
-typedef gtsam::NonlinearOptimizer<Graph,PlanarValues,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential;   // optimization engine for this domain
-typedef gtsam::NonlinearOptimizer<Graph,PlanarValues,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal;   // optimization engine for this domain
 
 using namespace std;
 using namespace gtsam;
@@ -74,7 +70,7 @@ int main(int argc, char** argv) {
 	// gaussian for prior
 	SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
 	Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
-	PriorFactor<PlanarValues, PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor
+	PriorFactor<PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor
 	graph->add(posePrior);  // add the factor to the graph
 
 	/* add odometry */
@@ -82,8 +78,8 @@ int main(int argc, char** argv) {
 	SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
 	Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
 	// create between factors to represent odometry
-	BetweenFactor<PlanarValues,PoseKey> odom12(x1, x2, odom_measurement, odom_model);
+	BetweenFactor<PoseKey> odom12(x1, x2, odom_measurement, odom_model);
-	BetweenFactor<PlanarValues,PoseKey> odom23(x2, x3, odom_measurement, odom_model);
+	BetweenFactor<PoseKey> odom23(x2, x3, odom_measurement, odom_model);
 	graph->add(odom12); // add both to graph
 	graph->add(odom23);
 
@@ -100,9 +96,9 @@ int main(int argc, char** argv) {
 		   range32 = 2.0;
 
 	// create bearing/range factors
-	BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model);
+	BearingRangeFactor<PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model);
-	BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
+	BearingRangeFactor<PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
-	BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model);
+	BearingRangeFactor<PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model);
 
 	// add the factors
 	graph->add(meas11);
@@ -112,7 +108,7 @@ int main(int argc, char** argv) {
 	graph->print("Full Graph");
 
 	// initialize to noisy points
-	boost::shared_ptr<PlanarValues> initial(new PlanarValues);
+	boost::shared_ptr<Values> initial(new 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));

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<pose2SLAM::Values> initial(new pose2SLAM::Values);
+	shared_ptr<Values> initial(new 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();
 
-	pose2SLAM::Values result = *optimizer_result.values();
+	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 */
-	pose2SLAM::Values initial;
+	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 */
-	pose2SLAM::Values result = optimize<Graph, pose2SLAM::Values>(graph, initial);
+	Values result = optimize<Graph>(graph, initial);
 	result.print("final result");
 
 

examples/Pose2SLAMwSPCG_advanced.cpp

@@ -31,17 +31,17 @@ using namespace gtsam;
 using namespace pose2SLAM;
 
 typedef boost::shared_ptr<Graph> sharedGraph ;
-typedef boost::shared_ptr<pose2SLAM::Values> sharedValue ;
+typedef boost::shared_ptr<Values> sharedValue ;
 //typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, SubgraphSolver<Graph,Values> > SPCGOptimizer;
 
 
-typedef SubgraphSolver<Graph, GaussianFactorGraph, pose2SLAM::Values> Solver;
+typedef SubgraphSolver<Graph, GaussianFactorGraph, Values> Solver;
 typedef boost::shared_ptr<Solver> sharedSolver ;
-typedef NonlinearOptimizer<Graph, pose2SLAM::Values, GaussianFactorGraph, Solver> SPCGOptimizer;
+typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, Solver> SPCGOptimizer;
 
 sharedGraph graph;
 sharedValue initial;
-pose2SLAM::Values result;
+Values result;
 
 /* ************************************************************************* */
 int main(void) {
@@ -51,7 +51,7 @@ int main(void) {
 	Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
 
 	graph = boost::make_shared<Graph>() ;
-	initial = boost::make_shared<pose2SLAM::Values>() ;
+	initial = boost::make_shared<Values>() ;
 
 	// create a 3 by 3 grid
 	// x3 x6 x9

examples/Pose2SLAMwSPCG_easy.cpp

@@ -31,7 +31,7 @@ using namespace gtsam;
 using namespace pose2SLAM;
 
 Graph graph;
-pose2SLAM::Values initial, result;
+Values initial, result;
 
 /* ************************************************************************* */
 int main(void) {

examples/SimpleRotation.cpp

@@ -23,7 +23,6 @@
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/nonlinear/Key.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimization.h>
 
@@ -53,8 +52,7 @@ using namespace gtsam;
  * and 2D poses.
  */
 typedef TypedSymbol<Rot2, 'x'> Key; 								/// Variable labels for a specific type
-typedef Values<Key> RotValues;											/// Class to store values - acts as a state for the system
+typedef NonlinearFactorGraph Graph;					/// Graph container for constraints - needs to know type of variables
-typedef NonlinearFactorGraph<RotValues> Graph;					/// Graph container for constraints - needs to know type of variables
 
 const double degree = M_PI / 180;
 
@@ -83,7 +81,7 @@ int main() {
 	prior.print("goal angle");
 	SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree);
 	Key key(1);
-	PriorFactor<RotValues, Key> factor(key, prior, model);
+	PriorFactor<Key> factor(key, prior, model);
 
 	/**
 	 *    Step 3: create a graph container and add the factor to it
@@ -114,7 +112,7 @@ int main() {
 	 * on the type of key used to find the appropriate value map if there
 	 * are multiple types of variables.
 	 */
-	RotValues initial;
+	Values initial;
 	initial.insert(key, Rot2::fromAngle(20 * degree));
 	initial.print("initial estimate");
 
@@ -126,7 +124,7 @@ int main() {
 	 * initial estimate.  This will yield a new RotValues structure
 	 * with the final state of the optimization.
 	 */
-	RotValues result = optimize<Graph, RotValues>(graph, initial);
+	Values result = optimize<Graph>(graph, initial);
 	result.print("final result");
 
 	return 0;

examples/easyPoint2KalmanFilter.cpp

@@ -24,7 +24,6 @@
 #include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/geometry/Point2.h>
 
 using namespace std;
@@ -32,7 +31,6 @@ using namespace gtsam;
 
 // Define Types for Linear System Test
 typedef TypedSymbol<Point2, 'x'> LinearKey;
-typedef Values<LinearKey> LinearValues;
 typedef Point2 LinearMeasurement;
 
 int main() {
@@ -42,7 +40,7 @@ int main() {
   SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
 
   // Create an ExtendedKalmanFilter object
-  ExtendedKalmanFilter<LinearValues,LinearKey> ekf(x_initial, P_initial);
+  ExtendedKalmanFilter<LinearKey> ekf(x_initial, P_initial);
 
 
 
@@ -67,7 +65,7 @@ int main() {
   // Predict delta based on controls
   Point2 difference(1,0);
   // Create Factor
-  BetweenFactor<LinearValues,LinearKey> factor1(x0, x1, difference, Q);
+  BetweenFactor<LinearKey> factor1(x0, x1, difference, Q);
 
   // Predict the new value with the EKF class
   Point2 x1_predict = ekf.predict(factor1);
@@ -88,7 +86,7 @@ int main() {
 
   // This simple measurement can be modeled with a PriorFactor
   Point2 z1(1.0, 0.0);
-  PriorFactor<LinearValues,LinearKey> factor2(x1, z1, R);
+  PriorFactor<LinearKey> factor2(x1, z1, R);
 
   // Update the Kalman Filter with the measurement
   Point2 x1_update = ekf.update(factor2);
@@ -100,13 +98,13 @@ int main() {
   // Predict
   LinearKey x2(2);
   difference = Point2(1,0);
-  BetweenFactor<LinearValues,LinearKey> factor3(x1, x2, difference, Q);
+  BetweenFactor<LinearKey> factor3(x1, x2, difference, Q);
   Point2 x2_predict = ekf.predict(factor1);
   x2_predict.print("X2 Predict");
 
   // Update
   Point2 z2(2.0, 0.0);
-  PriorFactor<LinearValues,LinearKey> factor4(x2, z2, R);
+  PriorFactor<LinearKey> factor4(x2, z2, R);
   Point2 x2_update = ekf.update(factor4);
   x2_update.print("X2 Update");
 
@@ -116,13 +114,13 @@ int main() {
   // Predict
   LinearKey x3(3);
   difference = Point2(1,0);
-  BetweenFactor<LinearValues,LinearKey> factor5(x2, x3, difference, Q);
+  BetweenFactor<LinearKey> factor5(x2, x3, difference, Q);
   Point2 x3_predict = ekf.predict(factor5);
   x3_predict.print("X3 Predict");
 
   // Update
   Point2 z3(3.0, 0.0);
-  PriorFactor<LinearValues,LinearKey> factor6(x3, z3, R);
+  PriorFactor<LinearKey> factor6(x3, z3, R);
   Point2 x3_update = ekf.update(factor6);
   x3_update.print("X3 Update");
 

examples/elaboratePoint2KalmanFilter.cpp

@@ -22,7 +22,6 @@
 
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/Ordering.h>
 #include <gtsam/nonlinear/Key.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
@@ -35,7 +34,6 @@ using namespace std;
 using namespace gtsam;
 
 typedef TypedSymbol<Point2, 'x'> Key;               /// Variable labels for a specific type
-typedef Values<Key> KalmanValues;                   /// Class to store values - acts as a state for the system
 
 int main() {
 
@@ -48,7 +46,7 @@ int main() {
   Ordering::shared_ptr ordering(new Ordering);
 
   // Create a structure to hold the linearization points
-  KalmanValues linearizationPoints;
+  Values linearizationPoints;
 
 	// Ground truth example
 	// Start at origin, move to the right (x-axis): 0,0  0,1  0,2
@@ -64,7 +62,7 @@ int main() {
   // This is equivalent to x_0 and P_0
   Point2 x_initial(0,0);
   SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
-  PriorFactor<KalmanValues, Key> factor1(x0, x_initial, P_initial);
+  PriorFactor<Key> factor1(x0, x_initial, P_initial);
   // Linearize the factor and add it to the linear factor graph
   linearizationPoints.insert(x0, x_initial);
   linearFactorGraph->push_back(factor1.linearize(linearizationPoints, *ordering));
@@ -95,7 +93,7 @@ int main() {
 
   Point2 difference(1,0);
   SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
-  BetweenFactor<KalmanValues, Key> factor2(x0, x1, difference, Q);
+  BetweenFactor<Key> factor2(x0, x1, difference, Q);
   // Linearize the factor and add it to the linear factor graph
   linearizationPoints.insert(x1, x_initial);
   linearFactorGraph->push_back(factor2.linearize(linearizationPoints, *ordering));
@@ -173,7 +171,7 @@ int main() {
   // This can be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R.
   Point2 z1(1.0, 0.0);
   SharedDiagonal R1 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
-  PriorFactor<KalmanValues, Key> factor4(x1, z1, R1);
+  PriorFactor<Key> factor4(x1, z1, R1);
   // Linearize the factor and add it to the linear factor graph
   linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering));
 
@@ -225,7 +223,7 @@ int main() {
   // Create a nonlinear factor describing the motion model
   difference = Point2(1,0);
   Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
-  BetweenFactor<KalmanValues, Key> factor6(x1, x2, difference, Q);
+  BetweenFactor<Key> factor6(x1, x2, difference, Q);
 
   // Linearize the factor and add it to the linear factor graph
   linearizationPoints.insert(x2, x1_update);
@@ -263,7 +261,7 @@ int main() {
   // And update using z2 ...
   Point2 z2(2.0, 0.0);
   SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
-  PriorFactor<KalmanValues, Key> factor8(x2, z2, R2);
+  PriorFactor<Key> factor8(x2, z2, R2);
 
   // Linearize the factor and add it to the linear factor graph
   linearFactorGraph->push_back(factor8.linearize(linearizationPoints, *ordering));
@@ -314,7 +312,7 @@ int main() {
   // Create a nonlinear factor describing the motion model
   difference = Point2(1,0);
   Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
-  BetweenFactor<KalmanValues, Key> factor10(x2, x3, difference, Q);
+  BetweenFactor<Key> factor10(x2, x3, difference, Q);
 
   // Linearize the factor and add it to the linear factor graph
   linearizationPoints.insert(x3, x2_update);
@@ -352,7 +350,7 @@ int main() {
   // And update using z3 ...
   Point2 z3(3.0, 0.0);
   SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
-  PriorFactor<KalmanValues, Key> factor12(x3, z3, R3);
+  PriorFactor<Key> factor12(x3, z3, R3);
 
   // Linearize the factor and add it to the linear factor graph
   linearFactorGraph->push_back(factor12.linearize(linearizationPoints, *ordering));

examples/vSLAMexample/vISAMexample.cpp

@@ -79,7 +79,7 @@ void readAllDataISAM() {
 /**
  * Setup newFactors and initialValues for each new pose and set of measurements at each frame.
  */
-void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<visualSLAM::Values>& initialValues,
+void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
     int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
 
   // Create a graph of newFactors with new measurements
@@ -100,10 +100,10 @@ void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<visualSLA
   }
 
   // Create initial values for all nodes in the newFactors
-  initialValues = shared_ptr<visualSLAM::Values> (new visualSLAM::Values());
+  initialValues = shared_ptr<Values> (new Values());
-  initialValues->insert(pose_id, pose);
+  initialValues->insert(PoseKey(pose_id), pose);
   for (size_t i = 0; i < measurements.size(); i++) {
-    initialValues->insert(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
+    initialValues->insert(PointKey(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
   }
 }
 
@@ -123,7 +123,7 @@ int main(int argc, char* argv[]) {
 
   // Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates
   int relinearizeInterval = 3;
-  NonlinearISAM<visualSLAM::Values> isam(relinearizeInterval);
+  NonlinearISAM<> isam(relinearizeInterval);
 
   // At each frame (poseId) with new camera pose and set of associated measurements,
   // create a graph of new factors and update ISAM
@@ -131,12 +131,12 @@ int main(int argc, char* argv[]) {
   BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) {
     const int poseId = features.first;
     shared_ptr<Graph> newFactors;
-    shared_ptr<visualSLAM::Values> initialValues;
+    shared_ptr<Values> initialValues;
     createNewFactors(newFactors, initialValues, poseId, g_poses[poseId],
             features.second, measurementSigma, g_calib);
 
     isam.update(*newFactors, *initialValues);
-    visualSLAM::Values currentEstimate = isam.estimate();
+    Values currentEstimate = isam.estimate();
     cout << "****************************************************" << endl;
     currentEstimate.print("Current estimate: ");
   }

examples/vSLAMexample/vSFMexample.cpp

@@ -102,17 +102,17 @@ Graph setupGraph(std::vector<Feature2D>& measurements, SharedNoiseModel measurem
  * Create a structure of initial estimates for all nodes (landmarks and poses) in the graph.
  * The returned Values structure contains all initial values for all nodes.
  */
-visualSLAM::Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
+Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
 
-	visualSLAM::Values initValues;
+	Values initValues;
 
   // Initialize landmarks 3D positions.
   for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
-    initValues.insert(lmit->first, lmit->second);
+    initValues.insert(PointKey(lmit->first), lmit->second);
 
   // Initialize camera poses.
   for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
-    initValues.insert(poseit->first, poseit->second);
+    initValues.insert(PoseKey(poseit->first), poseit->second);
 
   return initValues;
 }
@@ -135,7 +135,7 @@ int main(int argc, char* argv[]) {
   boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
 
   // Create an initial Values structure using groundtruth values as the initial estimates
-  boost::shared_ptr<visualSLAM::Values> initialEstimates(new visualSLAM::Values(initialize(g_landmarks, g_poses)));
+  boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses)));
   cout << "*******************************************************" << endl;
   initialEstimates->print("INITIAL ESTIMATES: ");
 

gtsam.h

@@ -427,14 +427,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);
-	planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
+	Values optimize(const 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;
+	gtsam::GaussianFactor* linearize(const Values& center, const Ordering& ordering) const;
 };
 
 class Optimizer {
@@ -637,8 +637,8 @@ class Graph {
 //	GaussianFactor* linearize(const gtsam::Pose2Values& config) const;
 //};
 //
-//class gtsam::Pose2Graph{
+//class gtsam::pose2SLAM::Graph{
-//	Pose2Graph();
+//	pose2SLAM::Graph();
 //	void print(string s) const;
 //	GaussianFactorGraph* linearize_(const gtsam::Pose2Values& config) const;
 //	void push_back(Pose2Factor* factor);

gtsam/base/DerivedValue.h

@@ -0,0 +1,95 @@
+/*
+ * DerivedValue.h
+ *
+ *  Created on: Jan 26, 2012
+ *      Author: thduynguyen
+ */
+
+#pragma once
+
+#include <boost/pool/singleton_pool.hpp>
+#include <gtsam/base/Value.h>
+namespace gtsam {
+
+template<class DERIVED>
+class DerivedValue : public Value {
+private:
+	/// Fake Tag struct for singleton pool allocator. In fact, it is never used!
+	struct PoolTag { };
+
+protected:
+	DerivedValue() {}
+
+public:
+
+	virtual ~DerivedValue() {}
+
+  /**
+   * Create a duplicate object returned as a pointer to the generic Value interface.
+   * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator
+   */
+  virtual Value* clone_() const {
+  	void *place = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
+  	DERIVED* ptr = new(place) DERIVED(static_cast<const DERIVED&>(*this));
+  	return ptr;
+  }
+
+  /**
+   * Destroy and deallocate this object, only if it was originally allocated using clone_().
+   */
+  virtual void deallocate_() const {
+    this->~Value();
+  	boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this);
+  }
+
+  /// equals implementing generic Value interface
+  virtual bool equals_(const Value& p, double tol = 1e-9) const {
+    // Cast the base class Value pointer to a derived class pointer
+    const DERIVED& derivedValue2 = dynamic_cast<const DERIVED&>(p);
+
+    // Return the result of calling equals on the derived class
+    return (static_cast<const DERIVED*>(this))->equals(derivedValue2, tol);
+  }
+
+  /// Generic Value interface version of retract
+	virtual Value* retract_(const Vector& delta) const {
+    // Call retract on the derived class
+    const DERIVED retractResult = (static_cast<const DERIVED*>(this))->retract(delta);
+
+    // Create a Value pointer copy of the result
+    void* resultAsValuePlace = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
+    Value* resultAsValue = new(resultAsValuePlace) DERIVED(retractResult);
+
+    // Return the pointer to the Value base class
+    return resultAsValue;
+	}
+
+	/// Generic Value interface version of localCoordinates
+	virtual Vector localCoordinates_(const Value& value2) const {
+    // Cast the base class Value pointer to a derived class pointer
+    const DERIVED& derivedValue2 = dynamic_cast<const DERIVED&>(value2);
+
+    // Return the result of calling localCoordinates on the derived class
+    return (static_cast<const DERIVED*>(this))->localCoordinates(derivedValue2);
+	}
+
+	/// Assignment operator
+	virtual Value& operator=(const Value& rhs) {
+    // Cast the base class Value pointer to a derived class pointer
+    const DERIVED& derivedRhs = dynamic_cast<const DERIVED&>(rhs);
+
+    // Do the assignment and return the result
+    return (static_cast<DERIVED*>(this))->operator=(derivedRhs);
+	}
+
+protected:
+	/// Assignment operator, protected because only the Value or DERIVED
+	/// assignment operators should be used.
+	DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
+	  // Nothing to do, do not call base class assignment operator
+	  return *this;
+	}
+
+};
+
+} /* namespace gtsam */

gtsam/base/LieVector.h

@@ -19,13 +19,14 @@
 
 #include <gtsam/base/Lie.h>
 #include <gtsam/base/Vector.h>
+#include <gtsam/base/DerivedValue.h>
 
 namespace gtsam {
 
 /**
  * LieVector is a wrapper around vector to allow it to be a Lie type
  */
-struct LieVector : public Vector {
+struct LieVector : public Vector, public DerivedValue<LieVector> {
 
 	/** default constructor - should be unnecessary */
 	LieVector() {}

gtsam/base/Makefile.am

@@ -26,7 +26,7 @@ sources += timing.cpp debug.cpp
 check_PROGRAMS += tests/testDebug tests/testTestableAssertions
 
 # Manifolds and Lie Groups
-headers += Manifold.h Group.h
+headers += DerivedValue.h Value.h Manifold.h Group.h
 headers += Lie.h Lie-inl.h lieProxies.h LieScalar.h
 sources += LieVector.cpp
 check_PROGRAMS += tests/testLieVector tests/testLieScalar

gtsam/base/Value.h

@@ -0,0 +1,147 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Value.h
+ * @brief The interface class for any variable that can be optimized or used in a factor.
+ * @author Richard Roberts
+ * @date Jan 14, 2012
+ */
+
+#pragma once
+
+#include <memory>
+
+#include <gtsam/base/Vector.h>
+
+namespace gtsam {
+
+  /**
+   * This is the interface class for any value that may be used as a variable
+   * assignment in a factor graph, and which you must derive to create new
+   * variable types to use with gtsam.  Examples of built-in classes
+   * implementing this are mainly in geometry, including Rot3, Pose2, etc.
+   *
+   * This interface specifies pure virtual retract_(), localCoordinates_() and
+   * equals_() functions that work with pointers and references to this interface
+   * class, i.e. the base class.  These functions allow containers, such as
+   * Values can operate generically on Value objects, retracting or computing
+   * local coordinates for many Value objects of different types.
+   *
+   * When you implement retract_(), localCoordinates_(), and equals_(), we
+   * suggest first implementing versions of these functions that work directly
+   * with derived objects, then using the provided helper functions to
+   * implement the generic Value versions.  This makes your implementation
+   * easier, and also improves performance in situations where the derived type
+   * is in fact known, such as in most implementations of \c evaluateError() in
+   * classes derived from NonlinearFactor.
+   *
+   * Using the above practice, here is an example of implementing a typical
+   * class derived from Value:
+   * \code
+     class Rot3 : public Value {
+     public:
+       // Constructor, there is never a need to call the Value base class constructor.
+       Rot3() { ... }
+
+       // Print for unit tests and debugging (virtual, implements Value::print())
+       virtual void print(const std::string& str = "") const;
+
+       // Equals working directly with Rot3 objects (non-virtual, non-overriding!)
+       bool equals(const Rot3& other, double tol = 1e-9) const;
+
+       // Tangent space dimensionality (virtual, implements Value::dim())
+       virtual size_t dim() const {
+         return 3;
+       }
+
+       // retract working directly with Rot3 objects (non-virtual, non-overriding!)
+       Rot3 retract(const Vector& delta) const {
+         // Math to implement a 3D rotation retraction e.g. exponential map
+         return Rot3(result);
+       }
+
+       // localCoordinates working directly with Rot3 objects (non-virtual, non-overriding!)
+       Vector localCoordinates(const Rot3& r2) const {
+         // Math to implement 3D rotation localCoordinates, e.g. logarithm map
+         return Vector(result);
+       }
+
+       // Equals implementing the generic Value interface (virtual, implements Value::equals_())
+       virtual bool equals_(const Value& other, double tol = 1e-9) const {
+         // Call our provided helper function to call your Rot3-specific
+         // equals with appropriate casting.
+         return CallDerivedEquals(this, other, tol);
+       }
+
+       // retract implementing the generic Value interface (virtual, implements Value::retract_())
+       virtual std::auto_ptr<Value> retract_(const Vector& delta) const {
+         // Call our provided helper function to call your Rot3-specific
+         // retract and do the appropriate casting and allocation.
+         return CallDerivedRetract(this, delta);
+       }
+
+       // localCoordinates implementing the generic Value interface (virtual, implements Value::localCoordinates_())
+       virtual Vector localCoordinates_(const Value& value) const {
+         // Call our provided helper function to call your Rot3-specific
+         // localCoordinates and do the appropriate casting.
+         return CallDerivedLocalCoordinates(this, value);
+       }
+     };
+     \endcode
+   */
+  class Value {
+  public:
+
+    /** Allocate and construct a clone of this value */
+    virtual Value* clone_() const = 0;
+
+    /** Deallocate a raw pointer of this value */
+    virtual void deallocate_() const = 0;
+
+    /** Compare this Value with another for equality. */
+    virtual bool equals_(const Value& other, double tol = 1e-9) const = 0;
+
+    /** Print this value, for debugging and unit tests */
+    virtual void print(const std::string& str = "") const = 0;
+
+    /** Return the dimensionality of the tangent space of this value.  This is
+     * the dimensionality of \c delta passed into retract() and of the vector
+     * returned by localCoordinates().
+     * @return The dimensionality of the tangent space
+     */
+    virtual size_t dim() const = 0;
+
+    /** Increment the value, by mapping from the vector delta in the tangent
+     * space of the current value back to the manifold to produce a new,
+     * incremented value.
+     * @param delta The delta vector in the tangent space of this value, by
+     * which to increment this value.
+     */
+    virtual Value* retract_(const Vector& delta) const = 0;
+
+    /** Compute the coordinates in the tangent space of this value that
+     * retract() would map to \c value.
+     * @param value The value whose coordinates should be determined in the
+     * tangent space of the value on which this function is called.
+     * @return The coordinates of \c value in the tangent space of \c this.
+     */
+    virtual Vector localCoordinates_(const Value& value) const = 0;
+
+    /** Assignment operator */
+    virtual Value& operator=(const Value& rhs) = 0;
+
+    /** Virutal destructor */
+    virtual ~Value() {}
+
+  };
+
+} /* namespace gtsam */

gtsam/geometry/Cal3Bundler.h

@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -27,7 +28,7 @@ namespace gtsam {
  * @ingroup geometry
  * \nosubgrouping
  */
-class Cal3Bundler {
+class Cal3Bundler : public DerivedValue<Cal3Bundler> {
 
 private:
 	double f_, k1_, k2_ ;
@@ -94,7 +95,7 @@ public:
 	Vector localCoordinates(const Cal3Bundler& T2) const ;
 
 	///TODO: comment
-	int dim() const { return 3 ; }	//TODO: make a final dimension variable (also, usually size_t in other classes  e.g. Pose2)
+	virtual size_t dim() const { return 3 ; }	//TODO: make a final dimension variable (also, usually size_t in other classes  e.g. Pose2)
 
 	///TODO: comment
 	static size_t Dim() { return 3; }	//TODO: make a final dimension variable

gtsam/geometry/Cal3DS2.h

@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -27,7 +28,7 @@ namespace gtsam {
  * @ingroup geometry
  * \nosubgrouping
  */
-class Cal3DS2 {
+class Cal3DS2 : public DerivedValue<Cal3DS2> {
 
 private:
 
@@ -97,7 +98,7 @@ public:
 	Vector localCoordinates(const Cal3DS2& T2) const ;
 
 	///TODO: comment
-	int dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
+	virtual size_t dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
 
 	///TODO: comment
 	static size_t Dim() { return 9; }	//TODO: make a final dimension variable

gtsam/geometry/Cal3_S2.h

@@ -21,6 +21,7 @@
 
 #pragma once
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -30,7 +31,7 @@ namespace gtsam {
 	 * @ingroup geometry
 	 * \nosubgrouping
 	 */
-	class Cal3_S2 {
+	class Cal3_S2 : public DerivedValue<Cal3_S2> {
 	private:
 		double fx_, fy_, s_, u0_, v0_;
 

gtsam/geometry/CalibratedCamera.h

@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
 
@@ -35,7 +36,7 @@ namespace gtsam {
 	 * @ingroup geometry
 	 * \nosubgrouping
 	 */
-	class CalibratedCamera {
+	class CalibratedCamera : public DerivedValue<CalibratedCamera> {
 	private:
 		Pose3 pose_; // 6DOF pose
 
@@ -61,9 +62,10 @@ namespace gtsam {
 		/// @name Testable
 		/// @{
 
-        void print(const std::string& s="") const {
+		virtual void print(const std::string& s = "") const {
-            pose_.print(); 
+			pose_.print(s);
-        }
+		}
+
 		/// check equality to another camera
 		bool equals (const CalibratedCamera &camera, double tol = 1e-9) const {
 			return pose_.equals(camera.pose(), tol) ;

gtsam/geometry/PinholeCamera.h

@@ -20,6 +20,7 @@
 
 #include <cmath>
 #include <boost/optional.hpp>
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/geometry/Point2.h>
@@ -35,7 +36,7 @@ namespace gtsam {
    * \nosubgrouping
    */
   template <typename Calibration>
-  class PinholeCamera {
+  class PinholeCamera : public DerivedValue<PinholeCamera<Calibration> > {
   private:
     Pose3 pose_;
     Calibration k_;

gtsam/geometry/Point2.h

@@ -18,6 +18,8 @@
 #pragma once
 
 #include <boost/serialization/nvp.hpp>
+
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Lie.h>
 
@@ -30,7 +32,7 @@ namespace gtsam {
  * @ingroup geometry
  * \nosubgrouping
  */
-class Point2 {
+class Point2 : public DerivedValue<Point2> {
 public:
 	/// dimension of the variable - used to autodetect sizes
 	static const size_t dimension = 2;

gtsam/geometry/Point3.h

@@ -24,6 +24,7 @@
 #include <boost/serialization/nvp.hpp>
 
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/base/Lie.h>
 
 namespace gtsam {
@@ -33,7 +34,7 @@ namespace gtsam {
    * @ingroup geometry
    * \nosubgrouping
    */
-  class Point3 {
+  class Point3 : public DerivedValue<Point3> {
   public:
 	  /// dimension of the variable - used to autodetect sizes
 	  static const size_t dimension = 3;

gtsam/geometry/Pose2.h

@@ -22,6 +22,7 @@
 
 #include <boost/optional.hpp>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Rot2.h>
 
@@ -32,7 +33,7 @@ namespace gtsam {
  * @ingroup geometry
  * \nosubgrouping
  */
-class Pose2 {
+class Pose2 : public DerivedValue<Pose2> {
 
 public:
 	static const size_t dimension = 3;

gtsam/geometry/Pose3.h

@@ -22,6 +22,7 @@
 #define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER
 #endif
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Rot3.h>
 
@@ -34,7 +35,7 @@ namespace gtsam {
    * @ingroup geometry
    * \nosubgrouping
    */
-  class Pose3 {
+  class Pose3 : public DerivedValue<Pose3> {
   public:
     static const size_t dimension = 6;
 

gtsam/geometry/Rot2.h

@@ -19,6 +19,8 @@
 #pragma once
 
 #include <boost/optional.hpp>
+
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -29,7 +31,7 @@ namespace gtsam {
 	 * @ingroup geometry
 	 * \nosubgrouping
 	 */
-	class Rot2 {
+	class Rot2 : public DerivedValue<Rot2> {
 
 	public:
 		/** get the dimension by the type */

gtsam/geometry/Rot3.h

@@ -32,6 +32,7 @@
 #endif
 #endif
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/3rdparty/Eigen/Eigen/Geometry>
 
@@ -49,7 +50,7 @@ namespace gtsam {
    * @ingroup geometry
    * \nosubgrouping
    */
-  class Rot3 {
+  class Rot3 : public DerivedValue<Rot3> {
   public:
     static const size_t dimension = 3;
 
@@ -92,6 +93,9 @@ namespace gtsam {
      */
     Rot3(const Quaternion& q);
 
+    /** Virtual destructor */
+    virtual ~Rot3() {}
+
     /* Static member function to generate some well known rotations */
 
     /// Rotation around X axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.

gtsam/geometry/StereoCamera.h

@@ -18,6 +18,8 @@
 #pragma once
 
 #include <boost/tuple/tuple.hpp>
+
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/StereoPoint2.h>

gtsam/geometry/StereoPoint2.h

@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
@@ -27,7 +28,7 @@ namespace gtsam {
 	 * @ingroup geometry
 	 * \nosubgrouping
 	 */
-	class StereoPoint2 {
+	class StereoPoint2 : public DerivedValue<StereoPoint2> {
 	public:
 		static const size_t dimension = 3;
 	private:

gtsam/inference/graph-inl.h

@@ -139,19 +139,19 @@ predecessorMap2Graph(const PredecessorMap<KEY>& p_map) {
 }
 
 /* ************************************************************************* */
-template <class V,class POSE, class VALUES>
+template <class V, class POSE, class KEY>
 class compose_key_visitor : public boost::default_bfs_visitor {
 
 private:
-	boost::shared_ptr<VALUES> config_;
+	boost::shared_ptr<Values> config_;
 
 public:
 
-	compose_key_visitor(boost::shared_ptr<VALUES> config_in) {config_ = config_in;}
+	compose_key_visitor(boost::shared_ptr<Values> config_in) {config_ = config_in;}
 
 	template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
-		typename VALUES::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
+		KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
-		typename VALUES::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
+		KEY key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
 		POSE relativePose = boost::get(boost::edge_weight, g, edge);
 		config_->insert(key_to, (*config_)[key_from].compose(relativePose));
 	}
@@ -159,23 +159,23 @@ public:
 };
 
 /* ************************************************************************* */
-template<class G, class Factor, class POSE, class VALUES>
+template<class G, class Factor, class POSE, class KEY>
-boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<typename VALUES::Key>& tree,
+boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>& tree,
 		const POSE& rootPose) {
 
 	//TODO: change edge_weight_t to edge_pose_t
 	typedef typename boost::adjacency_list<
 		boost::vecS, boost::vecS, boost::directedS,
-		boost::property<boost::vertex_name_t, typename VALUES::Key>,
+		boost::property<boost::vertex_name_t, KEY>,
 		boost::property<boost::edge_weight_t, POSE> > PoseGraph;
 	typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
 	typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge;
 
 	PoseGraph g;
 	PoseVertex root;
-	map<typename VALUES::Key, PoseVertex> key2vertex;
+	map<KEY, PoseVertex> key2vertex;
 	boost::tie(g, root, key2vertex) =
-			predecessorMap2Graph<PoseGraph, PoseVertex, typename VALUES::Key>(tree);
+			predecessorMap2Graph<PoseGraph, PoseVertex, KEY>(tree);
 
 	// attach the relative poses to the edges
 	PoseEdge edge12, edge21;
@@ -189,8 +189,8 @@ boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<type
 		boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
 		if (!factor) continue;
 
-		typename VALUES::Key key1 = factor->key1();
+		KEY key1 = factor->key1();
-		typename VALUES::Key key2 = factor->key2();
+		KEY key2 = factor->key2();
 
 		PoseVertex v1 = key2vertex.find(key1)->second;
 		PoseVertex v2 = key2vertex.find(key2)->second;
@@ -207,10 +207,10 @@ boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<type
 	}
 
 	// compose poses
-	boost::shared_ptr<VALUES> config(new VALUES);
+	boost::shared_ptr<Values> config(new Values);
-	typename VALUES::Key rootKey = boost::get(boost::vertex_name, g, root);
+	KEY rootKey = boost::get(boost::vertex_name, g, root);
 	config->insert(rootKey, rootPose);
-	compose_key_visitor<PoseVertex, POSE, VALUES> vis(config);
+	compose_key_visitor<PoseVertex, POSE, KEY> vis(config);
 	boost::breadth_first_search(g, root, boost::visitor(vis));
 
 	return config;

gtsam/inference/graph.h

@@ -25,6 +25,7 @@
 #include <boost/graph/graph_traits.hpp>
 #include <boost/graph/adjacency_list.hpp>
 #include <boost/shared_ptr.hpp>
+#include <gtsam/nonlinear/Values.h>
 
 namespace gtsam {
 
@@ -87,9 +88,9 @@ namespace gtsam {
 	/**
 	 * Compose the poses by following the chain specified by the spanning tree
 	 */
-	template<class G, class Factor, class POSE, class VALUES>
+	template<class G, class Factor, class POSE, class KEY>
-	boost::shared_ptr<VALUES>
+	boost::shared_ptr<Values>
-		composePoses(const G& graph, const PredecessorMap<typename VALUES::Key>& tree, const POSE& rootPose);
+		composePoses(const G& graph, const PredecessorMap<KEY>& tree, const POSE& rootPose);
 
 
 	/**

gtsam/linear/SubgraphSolver-inl.h

@@ -21,8 +21,8 @@ using namespace std;
 
 namespace gtsam {
 
-template<class GRAPH, class LINEAR, class VALUES>
+template<class GRAPH, class LINEAR, class KEY>
-void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
+void SubgraphSolver<GRAPH,LINEAR,KEY>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
 
 	GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>();
 	GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared<GaussianFactorGraph>();
@@ -47,8 +47,8 @@ void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::
 			Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(), Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),Rc1,xbar);
 }
 
-template<class GRAPH, class LINEAR, class VALUES>
+template<class GRAPH, class LINEAR, class KEY>
-VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() {
+VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,KEY>::optimize() const {
 
 	// preconditioned conjugate gradient
 	VectorValues zeros = pc_->zero();
@@ -60,18 +60,18 @@ VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() {
 	return xbar;
 }
 
-template<class GRAPH, class LINEAR, class VALUES>
+template<class GRAPH, class LINEAR, class KEY>
-void SubgraphSolver<GRAPH,LINEAR,VALUES>::initialize(const GRAPH& G, const VALUES& theta0) {
+void SubgraphSolver<GRAPH,LINEAR,KEY>::initialize(const GRAPH& G, const Values& theta0) {
 	// generate spanning tree
-	PredecessorMap<Key> tree_ = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
+	PredecessorMap<KEY> tree_ = gtsam::findMinimumSpanningTree<GRAPH, KEY, Constraint>(G);
 
 	// make the ordering
-	list<Key> keys = predecessorMap2Keys(tree_);
+	list<KEY> keys = predecessorMap2Keys(tree_);
 	ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end()));
 
 	// build factor pairs
 	pairs_.clear();
-	typedef pair<Key,Key> EG ;
+	typedef pair<KEY,KEY> EG ;
 	BOOST_FOREACH( const EG &eg, tree_ ) {
 		Symbol key1 = Symbol(eg.first),
 				key2 = Symbol(eg.second) ;

gtsam/linear/SubgraphSolver.h

@@ -16,6 +16,7 @@
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/IterativeSolver.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
+#include <gtsam/nonlinear/Values.h>
 
 namespace gtsam {
 
@@ -31,11 +32,11 @@ bool split(const std::map<Index, Index> &M,
  *   linearize: G * T -> L
  *   solve : L -> VectorValues
  */
-template<class GRAPH, class LINEAR, class VALUES>
+template<class GRAPH, class LINEAR, class KEY>
 class SubgraphSolver : public IterativeSolver {
 
 private:
-	typedef typename VALUES::Key Key;
+//	typedef typename VALUES::Key Key;
 	typedef typename GRAPH::Pose Pose;
 	typedef typename GRAPH::Constraint Constraint;
 
@@ -43,7 +44,7 @@ private:
 	typedef boost::shared_ptr<Ordering> shared_ordering ;
 	typedef boost::shared_ptr<GRAPH> shared_graph ;
 	typedef boost::shared_ptr<LINEAR> shared_linear ;
-	typedef boost::shared_ptr<VALUES> shared_values ;
+	typedef boost::shared_ptr<Values> shared_values ;
 	typedef boost::shared_ptr<SubgraphPreconditioner> shared_preconditioner ;
 	typedef std::map<Index,Index> mapPairIndex ;
 
@@ -61,7 +62,7 @@ private:
 
 public:
 
-	SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
+	SubgraphSolver(const GRAPH& G, const Values& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
 		IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); }
 
 	SubgraphSolver(const LINEAR& GFG) {
@@ -85,11 +86,11 @@ public:
 				IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc), useQR_(useQR) {}
 
 	void replaceFactors(const typename LINEAR::shared_ptr &graph);
-	VectorValues::shared_ptr optimize() ;
+	VectorValues::shared_ptr optimize() const;
 	shared_ordering ordering() const { return ordering_; }
 
 protected:
-	void initialize(const GRAPH& G, const VALUES& theta0);
+	void initialize(const GRAPH& G, const Values& theta0);
 
 private:
 	SubgraphSolver():IterativeSolver(){}

gtsam/linear/VectorValues.h

@@ -175,7 +175,7 @@ namespace gtsam {
     /// @name Advanced Constructors
     /// @{
 
-    /** Construct from a container of variable dimensions (in variable order). */
+    /** Construct from a container of variable dimensions (in variable order), without initializing any values. */
     template<class CONTAINER>
     VectorValues(const CONTAINER& dimensions) { append(dimensions); }
 

gtsam/nonlinear/DoglegOptimizerImpl.h

@@ -7,7 +7,7 @@
 
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianISAM.h> // To get optimize(BayesTree<GaussianConditional>)
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+//#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Ordering.h>
 
 namespace gtsam {

gtsam/nonlinear/ExtendedKalmanFilter-inl.h

@@ -27,10 +27,10 @@
 namespace gtsam {
 
 	/* ************************************************************************* */
-	template<class VALUES, class KEY>
+	template<class KEY>
-	typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::solve_(
+	typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::solve_(
 			const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering,
-			const VALUES& linearizationPoints, const KEY& lastKey,
+			const Values& linearizationPoints, const KEY& lastKey,
 			JacobianFactor::shared_ptr& newPrior) const {
 
 		// Extract the Index of the provided last key
@@ -66,8 +66,8 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES, class KEY>
+	template<class KEY>
-	ExtendedKalmanFilter<VALUES, KEY>::ExtendedKalmanFilter(T x_initial,
+	ExtendedKalmanFilter<KEY>::ExtendedKalmanFilter(T x_initial,
 			noiseModel::Gaussian::shared_ptr P_initial) {
 
 		// Set the initial linearization point to the provided mean
@@ -82,8 +82,8 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES, class KEY>
+	template<class KEY>
-	typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::predict(
+	typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::predict(
 			const MotionFactor& motionFactor) {
 
 		// TODO: This implementation largely ignores the actual factor symbols.
@@ -100,7 +100,7 @@ namespace gtsam {
 		ordering.insert(x1, 1);
 
 		// Create a set of linearization points
-		VALUES linearizationPoints;
+		Values linearizationPoints;
 		linearizationPoints.insert(x0, x_);
 		linearizationPoints.insert(x1, x_); // TODO should this really be x_ ?
 
@@ -122,8 +122,8 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES, class KEY>
+	template<class KEY>
-	typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::update(
+	typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::update(
 			const MeasurementFactor& measurementFactor) {
 
 		// TODO: This implementation largely ignores the actual factor symbols.
@@ -138,7 +138,7 @@ namespace gtsam {
 		ordering.insert(x0, 0);
 
 		// Create a set of linearization points
-		VALUES linearizationPoints;
+		Values linearizationPoints;
 		linearizationPoints.insert(x0, x_);
 
 		// Create a Gaussian Factor Graph

gtsam/nonlinear/ExtendedKalmanFilter.h

@@ -40,21 +40,21 @@ namespace gtsam {
 	 * \nosubgrouping
 	 */
 
-	template<class VALUES, class KEY>
+	template<class KEY>
 	class ExtendedKalmanFilter {
 	public:
 
-		typedef boost::shared_ptr<ExtendedKalmanFilter<VALUES, KEY> > shared_ptr;
+		typedef boost::shared_ptr<ExtendedKalmanFilter<KEY> > shared_ptr;
 		typedef typename KEY::Value T;
-		typedef NonlinearFactor2<VALUES, KEY, KEY> MotionFactor;
+		typedef NonlinearFactor2<KEY, KEY> MotionFactor;
-		typedef NonlinearFactor1<VALUES, KEY> MeasurementFactor;
+		typedef NonlinearFactor1<KEY> MeasurementFactor;
 
 	protected:
 		T x_; // linearization point
 		JacobianFactor::shared_ptr priorFactor_; // density
 
 		T solve_(const GaussianFactorGraph& linearFactorGraph,
-				const Ordering& ordering, const VALUES& linearizationPoints,
+				const Ordering& ordering, const Values& linearizationPoints,
 				const KEY& x, JacobianFactor::shared_ptr& newPrior) const;
 
 	public:

gtsam/nonlinear/GaussianISAM2.h

@@ -36,19 +36,19 @@ namespace gtsam {
  * variables.
  * @tparam GRAPH The NonlinearFactorGraph structure to store factors.  Defaults to standard NonlinearFactorGraph<VALUES>
  */
-template <class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
+template <class GRAPH = NonlinearFactorGraph>
-class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES, GRAPH> {
+class GaussianISAM2 : public ISAM2<GaussianConditional, GRAPH> {
-  typedef ISAM2<GaussianConditional, VALUES, GRAPH> Base;
+  typedef ISAM2<GaussianConditional, GRAPH> Base;
 public:
 
 	/// @name Standard Constructors
 	/// @{
 
   /** Create an empty ISAM2 instance */
-  GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES, GRAPH>(params) {}
+  GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, GRAPH>(params) {}
 
   /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
-  GaussianISAM2() : ISAM2<GaussianConditional, VALUES, GRAPH>() {}
+  GaussianISAM2() : ISAM2<GaussianConditional, GRAPH>() {}
 
 	/// @}
 	/// @name Advanced Interface

gtsam/nonlinear/ISAM2-impl-inl.h

@@ -19,10 +19,10 @@ namespace gtsam {
 
 using namespace std;
 
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
+struct ISAM2<CONDITIONAL, GRAPH>::Impl {
 
-  typedef ISAM2<CONDITIONAL, VALUES, GRAPH> ISAM2Type;
+  typedef ISAM2<CONDITIONAL, GRAPH> ISAM2Type;
 
   struct PartialSolveResult {
     typename ISAM2Type::sharedClique bayesTree;
@@ -45,7 +45,7 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
    * @param ordering Current ordering to be augmented with new variables
    * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
    */
-  static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
+  static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
 
   /**
    * Extract the set of variable indices from a NonlinearFactorGraph.  For each Symbol
@@ -95,7 +95,7 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
    * where we might expmap something twice, or expmap it but then not
    * recalculate its delta.
    */
-  static void ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta,
+  static void ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
       const Ordering& ordering, const std::vector<bool>& mask,
       boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>());
 
@@ -118,25 +118,9 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
 };
 
 /* ************************************************************************* */
-struct _VariableAdder {
+template<class CONDITIONAL, class GRAPH>
-  Ordering& ordering;
+void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
-  Permuted<VectorValues>& vconfig;
+    const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) {
-  Index nextVar;
-  _VariableAdder(Ordering& _ordering, Permuted<VectorValues>& _vconfig, Index _nextVar) : ordering(_ordering), vconfig(_vconfig), nextVar(_nextVar){}
-  template<typename I>
-  void operator()(I xIt) {
-    const bool debug = ISDEBUG("ISAM2 AddVariables");
-    vconfig.permutation()[nextVar] = nextVar;
-    ordering.insert(xIt->first, nextVar);
-    if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << nextVar << endl;
-    ++ nextVar;
-  }
-};
-
-/* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
-    const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) {
   const bool debug = ISDEBUG("ISAM2 AddVariables");
 
   theta.insert(newTheta);
@@ -151,8 +135,13 @@ void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
   delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
   delta.permutation().resize(originalnVars + newTheta.size());
   {
-    _VariableAdder vadder(ordering, delta, originalnVars);
+    Index nextVar = originalnVars;
-    newTheta.apply(vadder);
+    BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
+      delta.permutation()[nextVar] = nextVar;
+      ordering.insert(key_value.first, nextVar);
+      if(debug) cout << "Adding variable " << (string)key_value.first << " with order " << nextVar << endl;
+      ++ nextVar;
+    }
     assert(delta.permutation().size() == delta.container().size());
     assert(ordering.nVars() == delta.size());
     assert(ordering.size() == delta.size());
@@ -162,10 +151,10 @@ void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
+FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
   FastSet<Index> indices;
-  BOOST_FOREACH(const typename NonlinearFactor<VALUES>::shared_ptr& factor, factors) {
+  BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) {
     BOOST_FOREACH(const Symbol& key, factor->keys()) {
       indices.insert(ordering[key]);
     }
@@ -174,8 +163,8 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::IndicesFromFactors(const O
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
+FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
   FastSet<Index> relinKeys;
 
   if(relinearizeThreshold.type() == typeid(double)) {
@@ -202,8 +191,8 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(const
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
+void ISAM2<CONDITIONAL,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
   static const bool debug = false;
   // does the separator contain any of the variables?
   bool found = false;
@@ -223,41 +212,8 @@ void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITI
 }
 
 /* ************************************************************************* */
-// Internal class used in ExpmapMasked()
+template<class CONDITIONAL, class GRAPH>
-// This debug version sets delta entries that are applied to "Inf".  The
+void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
-// idea is that if a delta is applied, the variable is being relinearized,
-// so the same delta should not be re-applied because it will be recalc-
-// ulated.  This is a debug check to prevent against a mix-up of indices
-// or not keeping track of recalculated variables.
-struct _SelectiveExpmapAndClear {
-  const Permuted<VectorValues>& delta;
-  const Ordering& ordering;
-  const vector<bool>& mask;
-  const boost::optional<Permuted<VectorValues>&> invalidate;
-  _SelectiveExpmapAndClear(const Permuted<VectorValues>& _delta, const Ordering& _ordering, const vector<bool>& _mask, boost::optional<Permuted<VectorValues>&> _invalidate) :
-    delta(_delta), ordering(_ordering), mask(_mask), invalidate(_invalidate) {}
-  template<typename I>
-  void operator()(I it_x) {
-    Index var = ordering[it_x->first];
-    if(ISDEBUG("ISAM2 update verbose")) {
-      if(mask[var])
-        cout << "expmap " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
-      else
-        cout << "       " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
-    }
-    assert(delta[var].size() == (int)it_x->second.dim());
-    assert(delta[var].unaryExpr(&isfinite<double>).all());
-    if(mask[var]) {
-      it_x->second = it_x->second.retract(delta[var]);
-      if(invalidate)
-        (*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)
-    }
-  }
-};
-
-/* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
-void ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta,
     const Ordering& ordering, const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug) {
   // If debugging, invalidate if requested, otherwise do not invalidate.
   // Invalidating means setting expmapped entries to Inf, to trigger assertions
@@ -266,14 +222,35 @@ void ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::ExpmapMasked(VALUES& values, const
   invalidateIfDebug = boost::optional<Permuted<VectorValues>&>();
 #endif
 
-  _SelectiveExpmapAndClear selectiveExpmapper(delta, ordering, mask, invalidateIfDebug);
+  assert(values.size() == ordering.size());
-  values.apply(selectiveExpmapper);
+  Values::iterator key_value;
+  Ordering::const_iterator key_index;
+  for(key_value = values.begin(), key_index = ordering.begin();
+      key_value != values.end() && key_index != ordering.end(); ++key_value, ++key_index) {
+    assert(key_value->first == key_index->first);
+    const Index var = key_index->second;
+    if(ISDEBUG("ISAM2 update verbose")) {
+      if(mask[var])
+        cout << "expmap " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
+      else
+        cout << "       " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
+    }
+    assert(delta[var].size() == (int)key_value->second.dim());
+    assert(delta[var].unaryExpr(&isfinite<double>).all());
+    if(mask[var]) {
+      Value* retracted = key_value->second.retract_(delta[var]);
+      key_value->second = *retracted;
+      retracted->deallocate_();
+      if(invalidateIfDebug)
+        (*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)
+    }
+  }
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-typename ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolveResult
+typename ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolveResult
-ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
+ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
     const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
 
   static const bool debug = ISDEBUG("ISAM2 recalculate");

gtsam/nonlinear/ISAM2-inl.h

@@ -39,8 +39,8 @@ static const bool disableReordering = false;
 static const double batchThreshold = 0.65;
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2(const ISAM2Params& params):
+ISAM2<CONDITIONAL, GRAPH>::ISAM2(const ISAM2Params& params):
     delta_(Permutation(), deltaUnpermuted_), params_(params) {
   // See note in gtsam/base/boost_variant_with_workaround.h
   if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
@@ -48,8 +48,8 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2(const ISAM2Params& params):
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2():
+ISAM2<CONDITIONAL, GRAPH>::ISAM2():
     delta_(Permutation(), deltaUnpermuted_) {
   // See note in gtsam/base/boost_variant_with_workaround.h
   if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
@@ -57,14 +57,14 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2():
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-FastList<size_t> ISAM2<CONDITIONAL, VALUES, GRAPH>::getAffectedFactors(const FastList<Index>& keys) const {
+FastList<size_t> ISAM2<CONDITIONAL, GRAPH>::getAffectedFactors(const FastList<Index>& keys) const {
   static const bool debug = false;
   if(debug) cout << "Getting affected factors for ";
   if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } }
   if(debug) cout << endl;
 
-  FactorGraph<NonlinearFactor<VALUES> > allAffected;
+  FactorGraph<NonlinearFactor > allAffected;
   FastList<size_t> indices;
   BOOST_FOREACH(const Index key, keys) {
 //    const list<size_t> l = nonlinearFactors_.factors(key);
@@ -86,9 +86,9 @@ FastList<size_t> ISAM2<CONDITIONAL, VALUES, GRAPH>::getAffectedFactors(const Fas
 /* ************************************************************************* */
 // retrieve all factors that ONLY contain the affected variables
 // (note that the remaining stuff is summarized in the cached factors)
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
 FactorGraph<GaussianFactor>::shared_ptr
-ISAM2<CONDITIONAL, VALUES, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
+ISAM2<CONDITIONAL, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
 
   tic(1,"getAffectedFactors");
   FastList<size_t> candidates = getAffectedFactors(affectedKeys);
@@ -125,9 +125,9 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::relinearizeAffectedFactors(const FastList<Ind
 
 /* ************************************************************************* */
 // find intermediate (linearized) factors from cache that are passed into the affected area
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-FactorGraph<typename ISAM2<CONDITIONAL, VALUES, GRAPH>::CacheFactor>
+FactorGraph<typename ISAM2<CONDITIONAL, GRAPH>::CacheFactor>
-ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
+ISAM2<CONDITIONAL, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
 
   static const bool debug = false;
 
@@ -151,8 +151,8 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
   return cachedBoundary;
 }
 
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculate(
+boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
     const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
     const boost::optional<FastSet<Index> >& constrainKeys, ISAM2Result& result) {
 
@@ -395,8 +395,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculat
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
+ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
     const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
     const boost::optional<FastSet<Symbol> >& constrainedKeys, bool force_relinearize) {
 
@@ -579,36 +579,36 @@ ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate() const {
+Values ISAM2<CONDITIONAL, GRAPH>::calculateEstimate() const {
   // We use ExpmapMasked here instead of regular expmap because the former
   // handles Permuted<VectorValues>
-	VALUES ret(theta_);
+	Values ret(theta_);
   vector<bool> mask(ordering_.nVars(), true);
   Impl::ExpmapMasked(ret, delta_, ordering_, mask);
   return ret;
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
 template<class KEY>
-typename KEY::Value ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate(const KEY& key) const {
+typename KEY::Value ISAM2<CONDITIONAL, GRAPH>::calculateEstimate(const KEY& key) const {
   const Index index = getOrdering()[key];
   const SubVector delta = getDelta()[index];
   return getLinearizationPoint()[key].retract(delta);
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateBestEstimate() const {
+Values ISAM2<CONDITIONAL, GRAPH>::calculateBestEstimate() const {
   VectorValues delta(theta_.dims(ordering_));
   optimize2(this->root(), delta);
   return theta_.retract(delta, ordering_);
 }
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam) {
+VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam) {
   VectorValues delta = *allocateVectorValues(isam);
   optimize2(isam.root(), delta);
   return delta;

gtsam/nonlinear/ISAM2.h

@@ -266,13 +266,13 @@ private:
  * estimate of all variables.
  *
  */
-template<class CONDITIONAL, class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
+template<class CONDITIONAL, class GRAPH = NonlinearFactorGraph>
 class ISAM2: public BayesTree<CONDITIONAL, ISAM2Clique<CONDITIONAL> > {
 
 protected:
 
   /** The current linearization point */
-  VALUES theta_;
+  Values theta_;
 
   /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */
   VariableIndex variableIndex_;
@@ -314,8 +314,8 @@ private:
 public:
 
   typedef BayesTree<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class
-  typedef ISAM2<CONDITIONAL, VALUES> This; ///< This class
+  typedef ISAM2<CONDITIONAL> This; ///< This class
-  typedef VALUES Values;
+  typedef Values Values;
   typedef GRAPH Graph;
 
   /** Create an empty ISAM2 instance */
@@ -368,19 +368,19 @@ public:
    * (Params::relinearizeSkip).
    * @return An ISAM2Result struct containing information about the update
    */
-  ISAM2Result update(const GRAPH& newFactors = GRAPH(), const VALUES& newTheta = VALUES(),
+  ISAM2Result update(const GRAPH& newFactors = GRAPH(), const Values& newTheta = Values(),
       const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
       const boost::optional<FastSet<Symbol> >& constrainedKeys = boost::none,
       bool force_relinearize = false);
 
   /** Access the current linearization point */
-  const VALUES& getLinearizationPoint() const {return theta_;}
+  const Values& getLinearizationPoint() const {return theta_;}
 
   /** Compute an estimate from the incomplete linear delta computed during the last update.
    * This delta is incomplete because it was not updated below wildfire_threshold.  If only
    * a single variable is needed, it is faster to call calculateEstimate(const KEY&).
    */
-  VALUES calculateEstimate() const;
+  Values calculateEstimate() const;
 
   /** Compute an estimate for a single variable using its incomplete linear delta computed
    * during the last update.  This is faster than calling the no-argument version of
@@ -399,7 +399,7 @@ public:
 
   /** Compute an estimate using a complete delta computed by a full back-substitution.
    */
-  VALUES calculateBestEstimate() const;
+  Values calculateBestEstimate() const;
 
   /** Access the current delta, computed during the last call to update */
   const Permuted<VectorValues>& getDelta() const { return delta_; }
@@ -435,8 +435,8 @@ private:
 }; // ISAM2
 
 /** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam);
+VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam);
 
 } /// namespace gtsam
 

gtsam/nonlinear/Key.h

@@ -31,6 +31,9 @@
 
 namespace gtsam {
 
+  // Forward declarations
+  class Symbol;
+
 /**
  * TypedSymbol key class is templated on
  * 1) the type T it is supposed to retrieve, for extra type checking
@@ -76,6 +79,7 @@ public:
   std::string latex() const {
     return (boost::format("%c_{%d}") % C % j_).str();
   }
+  Symbol symbol() const;
 
   // logic:
 
@@ -298,6 +302,9 @@ public:
     std::string label_s = (boost::format("%1%") % label_).str();
     return (boost::format("%c%s_{%d}") % C % label_s % this->j_).str();
   }
+  Symbol symbol() const {
+    return Symbol(*this);
+  }
 
   // Needed for conversion to LabeledSymbol
   size_t convertLabel() const {
@@ -354,5 +361,11 @@ private:
   }
 };
 
+/* ************************************************************************* */
+template<class T, char C>
+Symbol TypedSymbol<T,C>::symbol() const {
+  return Symbol(*this);
+}
+
 } // namespace gtsam
 

gtsam/nonlinear/Makefile.am

@@ -16,15 +16,16 @@ check_PROGRAMS =
 #----------------------------------------------------------------------------------------------------
 
 # Lie Groups
-headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h 
+headers += Values-inl.h
+sources += Values.cpp
 check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering
 
 # Nonlinear nonlinear
 headers += Key.h 
-headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
+headers += NonlinearFactorGraph.h
 headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h 
 headers += NonlinearFactor.h
-sources += NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp NonlinearOptimizationParameters.cpp
+sources += NonlinearFactorGraph.cpp NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp NonlinearOptimizationParameters.cpp
 headers += DoglegOptimizer.h DoglegOptimizer-inl.h
 
 # Nonlinear iSAM(2)
@@ -62,7 +63,7 @@ AM_CXXFLAGS =
 #----------------------------------------------------------------------------------------------------
 TESTS = $(check_PROGRAMS)
 AM_LDFLAGS += $(boost_serialization)
-LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../base/libbase.la ../3rdparty/libccolamd.la
+LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../geometry/libgeometry.la ../base/libbase.la ../3rdparty/libccolamd.la
 LDADD += ../../CppUnitLite/libCppUnitLite.a
 AM_DEFAULT_SOURCE_EXT = .cpp
 

gtsam/nonlinear/NonlinearEquality.h

@@ -47,8 +47,8 @@ namespace gtsam {
 	 *
 	 * \nosubgrouping
 	 */
-	template<class VALUES, class KEY>
+	template<class KEY>
-	class NonlinearEquality: public NonlinearFactor1<VALUES, KEY> {
+	class NonlinearEquality: public NonlinearFactor1<KEY> {
 
 	public:
 		typedef typename KEY::Value T;
@@ -71,7 +71,7 @@ namespace gtsam {
 		 */
 		bool (*compare_)(const T& a, const T& b);
 
-		typedef NonlinearFactor1<VALUES, KEY> Base;
+		typedef NonlinearFactor1<KEY> Base;
 
 		/** default constructor - only for serialization */
 		NonlinearEquality() {}
@@ -110,7 +110,7 @@ namespace gtsam {
 		}
 
 		/** Check if two factors are equal */
-		bool equals(const NonlinearEquality<VALUES,KEY>& f, double tol = 1e-9) const {
+		bool equals(const NonlinearEquality<KEY>& f, double tol = 1e-9) const {
 			if (!Base::equals(f)) return false;
 			return feasible_.equals(f.feasible_, tol) &&
 					fabs(error_gain_ - f.error_gain_) < tol;
@@ -121,7 +121,7 @@ namespace gtsam {
 		/// @{
 
 		/** actual error function calculation */
-		virtual double error(const VALUES& c) const {
+		virtual double error(const Values& c) const {
 			const T& xj = c[this->key_];
 			Vector e = this->unwhitenedError(c);
 			if (allow_error_ || !compare_(xj, feasible_)) {
@@ -148,7 +148,7 @@ namespace gtsam {
 		}
 
 		// Linearize is over-written, because base linearization tries to whiten
-		virtual GaussianFactor::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const {
+		virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const {
 			const T& xj = x[this->key_];
 			Matrix A;
 			Vector b = evaluateError(xj, A);
@@ -177,14 +177,14 @@ namespace gtsam {
 	/**
 	 * Simple unary equality constraint - fixes a value for a variable
 	 */
-	template<class VALUES, class KEY>
+	template<class KEY>
-	class NonlinearEquality1 : public NonlinearFactor1<VALUES, KEY> {
+	class NonlinearEquality1 : public NonlinearFactor1<KEY> {
 
 	public:
 		typedef typename KEY::Value X;
 
 	protected:
-		typedef NonlinearFactor1<VALUES, KEY> Base;
+		typedef NonlinearFactor1<KEY> Base;
 
 		/** default constructor to allow for serialization */
 		NonlinearEquality1() {}
@@ -196,7 +196,7 @@ namespace gtsam {
 
 	public:
 
-		typedef boost::shared_ptr<NonlinearEquality1<VALUES, KEY> > shared_ptr;
+		typedef boost::shared_ptr<NonlinearEquality1<KEY> > shared_ptr;
 
 		///TODO: comment
 		NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0)
@@ -236,13 +236,13 @@ namespace gtsam {
 	 * Simple binary equality constraint - this constraint forces two factors to
 	 * be the same.
 	 */
-	template<class VALUES, class KEY>
+	template<class KEY>
-	class NonlinearEquality2 : public NonlinearFactor2<VALUES, KEY, KEY> {
+	class NonlinearEquality2 : public NonlinearFactor2<KEY, KEY> {
 	public:
 		typedef typename KEY::Value X;
 
 	protected:
-		typedef NonlinearFactor2<VALUES, KEY, KEY> Base;
+		typedef NonlinearFactor2<KEY, KEY> Base;
 
 		GTSAM_CONCEPT_MANIFOLD_TYPE(X);
 
@@ -251,7 +251,7 @@ namespace gtsam {
 
 	public:
 
-		typedef boost::shared_ptr<NonlinearEquality2<VALUES, KEY> > shared_ptr;
+		typedef boost::shared_ptr<NonlinearEquality2<KEY> > shared_ptr;
 
 		///TODO: comment
 		NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0)

gtsam/nonlinear/NonlinearFactor.h

@@ -31,6 +31,7 @@
 #include <gtsam/linear/SharedNoiseModel.h>
 #include <gtsam/linear/JacobianFactor.h>
 
+#include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/Ordering.h>
 
 namespace gtsam {
@@ -57,18 +58,17 @@ inline void __fill_from_tuple<boost::tuples::null_type>(std::vector<Symbol>& vec
  * which are objects in non-linear manifolds (Lie groups).
  * \nosubgrouping
  */
-template<class VALUES>
 class NonlinearFactor: public Factor<Symbol> {
 
 protected:
 
   // Some handy typedefs
   typedef Factor<Symbol> Base;
-  typedef NonlinearFactor<VALUES> This;
+  typedef NonlinearFactor This;
 
 public:
 
-  typedef boost::shared_ptr<NonlinearFactor<VALUES> > shared_ptr;
+  typedef boost::shared_ptr<NonlinearFactor > shared_ptr;
 
 	/// @name Standard Constructors
 	/// @{
@@ -120,7 +120,7 @@ public:
    * This is typically equal to log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian.
    * You can override this for systems with unusual noise models.
    */
-  virtual double error(const VALUES& c) const = 0;
+  virtual double error(const Values& c) const = 0;
 
   /** get the dimension of the factor (number of rows on linearization) */
   virtual size_t dim() const = 0;
@@ -135,11 +135,11 @@ public:
 	 * when the constraint is *NOT* fulfilled.
 	 * @return true if the constraint is active
 	 */
-	virtual bool active(const VALUES& c) const { return true; }
+	virtual bool active(const Values& c) const { return true; }
 
   /** linearize to a GaussianFactor */
   virtual boost::shared_ptr<GaussianFactor>
-  linearize(const VALUES& c, const Ordering& ordering) const = 0;
+  linearize(const Values& c, const Ordering& ordering) const = 0;
 
   /**
    * Create a symbolic factor using the given ordering to determine the
@@ -165,20 +165,19 @@ public:
 
  * The noise model is typically Gaussian, but robust and constrained error models are also supported.
  */
-template<class VALUES>
+class NoiseModelFactor: public NonlinearFactor {
-class NoiseModelFactor: public NonlinearFactor<VALUES> {
 
 protected:
 
   // handy typedefs
-  typedef NonlinearFactor<VALUES> Base;
+  typedef NonlinearFactor Base;
-  typedef NoiseModelFactor<VALUES> This;
+  typedef NoiseModelFactor This;
 
   SharedNoiseModel noiseModel_; /** Noise model */
 
 public:
 
-  typedef boost::shared_ptr<NoiseModelFactor<VALUES> > shared_ptr;
+  typedef boost::shared_ptr<NoiseModelFactor > shared_ptr;
 
   /** Default constructor for I/O only */
   NoiseModelFactor() {
@@ -225,7 +224,7 @@ public:
   }
 
   /** Check if two factors are equal */
-  virtual bool equals(const NoiseModelFactor<VALUES>& f, double tol = 1e-9) const {
+  virtual bool equals(const NoiseModelFactor& f, double tol = 1e-9) const {
     return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, tol);
   }
 
@@ -245,13 +244,13 @@ public:
    * If any of the optional Matrix reference arguments are specified, it should compute
    * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...).
    */
-  virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
 
   /**
    * Vector of errors, whitened
    * This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian
    */
-  Vector whitenedError(const VALUES& c) const {
+  Vector whitenedError(const Values& c) const {
     return noiseModel_->whiten(unwhitenedError(c));
   }
 
@@ -261,7 +260,7 @@ public:
    * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
    * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
    */
-  virtual double error(const VALUES& c) const {
+  virtual double error(const Values& c) const {
   	if (this->active(c))
   		return 0.5 * noiseModel_->distance(unwhitenedError(c));
   	else
@@ -273,7 +272,7 @@ public:
    * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
    * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
    */
-  boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
+  boost::shared_ptr<GaussianFactor> linearize(const Values& x, const Ordering& ordering) const {
   	// Only linearize if the factor is active
 		if (!this->active(x))
 			return boost::shared_ptr<JacobianFactor>();
@@ -321,8 +320,8 @@ private:
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 1
  * variable.  To derive from this class, implement evaluateError(). */
-template<class VALUES, class KEY>
+template<class KEY>
-class NonlinearFactor1: public NoiseModelFactor<VALUES> {
+class NonlinearFactor1: public NoiseModelFactor {
 
 public:
 
@@ -334,8 +333,8 @@ protected:
   // The value of the key. Not const to allow serialization
   KEY key_;
 
-  typedef NoiseModelFactor<VALUES> Base;
+  typedef NoiseModelFactor Base;
-  typedef NonlinearFactor1<VALUES, KEY> This;
+  typedef NonlinearFactor1<KEY> This;
 
 public:
 
@@ -357,7 +356,7 @@ public:
 
   /** Calls the 1-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       const X& x1 = x[key_];
       if(H) {
@@ -400,8 +399,8 @@ private:
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 2
  * variables.  To derive from this class, implement evaluateError(). */
-template<class VALUES, class KEY1, class KEY2>
+template<class KEY1, class KEY2>
-class NonlinearFactor2: public NoiseModelFactor<VALUES> {
+class NonlinearFactor2: public NoiseModelFactor {
 
 public:
 
@@ -415,8 +414,8 @@ protected:
   KEY1 key1_;
   KEY2 key2_;
 
-  typedef NoiseModelFactor<VALUES> Base;
+  typedef NoiseModelFactor Base;
-  typedef NonlinearFactor2<VALUES, KEY1, KEY2> This;
+  typedef NonlinearFactor2<KEY1, KEY2> This;
 
 public:
 
@@ -441,7 +440,7 @@ public:
 
   /** Calls the 2-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       const X1& x1 = x[key1_];
       const X2& x2 = x[key2_];
@@ -488,8 +487,8 @@ private:
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 3
  * variables.  To derive from this class, implement evaluateError(). */
-template<class VALUES, class KEY1, class KEY2, class KEY3>
+template<class KEY1, class KEY2, class KEY3>
-class NonlinearFactor3: public NoiseModelFactor<VALUES> {
+class NonlinearFactor3: public NoiseModelFactor {
 
 public:
 
@@ -505,8 +504,8 @@ protected:
   KEY2 key2_;
   KEY3 key3_;
 
-  typedef NoiseModelFactor<VALUES> Base;
+  typedef NoiseModelFactor Base;
-  typedef NonlinearFactor3<VALUES, KEY1, KEY2, KEY3> This;
+  typedef NonlinearFactor3<KEY1, KEY2, KEY3> This;
 
 public:
 
@@ -533,7 +532,7 @@ public:
 
   /** Calls the 3-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if(this->active(x)) {
       if(H)
         return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]);
@@ -582,8 +581,8 @@ private:
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 4
  * variables.  To derive from this class, implement evaluateError(). */
-template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4>
+template<class KEY1, class KEY2, class KEY3, class KEY4>
-class NonlinearFactor4: public NoiseModelFactor<VALUES> {
+class NonlinearFactor4: public NoiseModelFactor {
 
 public:
 
@@ -601,8 +600,8 @@ protected:
   KEY3 key3_;
   KEY4 key4_;
 
-  typedef NoiseModelFactor<VALUES> Base;
+  typedef NoiseModelFactor Base;
-  typedef NonlinearFactor4<VALUES, KEY1, KEY2, KEY3, KEY4> This;
+  typedef NonlinearFactor4<KEY1, KEY2, KEY3, KEY4> This;
 
 public:
 
@@ -631,7 +630,7 @@ public:
 
   /** Calls the 4-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
   		if(H)
   			return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
@@ -682,8 +681,8 @@ private:
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 5
  * variables.  To derive from this class, implement evaluateError(). */
-template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4, class KEY5>
+template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5>
-class NonlinearFactor5: public NoiseModelFactor<VALUES> {
+class NonlinearFactor5: public NoiseModelFactor {
 
 public:
 
@@ -703,8 +702,8 @@ protected:
   KEY4 key4_;
   KEY5 key5_;
 
-  typedef NoiseModelFactor<VALUES> Base;
+  typedef NoiseModelFactor Base;
-  typedef NonlinearFactor5<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5> This;
+  typedef NonlinearFactor5<KEY1, KEY2, KEY3, KEY4, KEY5> This;
 
 public:
 
@@ -735,7 +734,7 @@ public:
 
   /** Calls the 5-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
       if(H)
         return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
@@ -789,8 +788,8 @@ private:
 /* ************************************************************************* */
 /** A convenient base class for creating your own NoiseModelFactor with 6
  * variables.  To derive from this class, implement evaluateError(). */
-template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4, class KEY5, class KEY6>
+template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5, class KEY6>
-class NonlinearFactor6: public NoiseModelFactor<VALUES> {
+class NonlinearFactor6: public NoiseModelFactor {
 
 public:
 
@@ -812,8 +811,8 @@ protected:
   KEY5 key5_;
   KEY6 key6_;
 
-  typedef NoiseModelFactor<VALUES> Base;
+  typedef NoiseModelFactor Base;
-  typedef NonlinearFactor6<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> This;
+  typedef NonlinearFactor6<KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> This;
 
 public:
 
@@ -846,7 +845,7 @@ public:
 
   /** Calls the 6-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
   	if(this->active(x)) {
       if(H)
         return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -10,39 +10,35 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    NonlinearFactorGraph-inl.h
+ * @file    NonlinearFactorGraph.cpp
  * @brief   Factor Graph Consisting of non-linear factors
  * @author  Frank Dellaert
  * @author  Carlos Nieto
  * @author  Christian Potthast
  */
 
-#pragma once
+#include <cmath>
-
+#include <boost/foreach.hpp>
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/inference.h>
-#include <boost/foreach.hpp>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <cmath>
 
 using namespace std;
 
 namespace gtsam {
 
 	/* ************************************************************************* */
-	template<class VALUES>
+	double NonlinearFactorGraph::probPrime(const Values& c) const {
-	double NonlinearFactorGraph<VALUES>::probPrime(const VALUES& c) const {
 		return exp(-0.5 * error(c));
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES>
+	void NonlinearFactorGraph::print(const std::string& str) const {
-	void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
 		Base::print(str);
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES>
+	double NonlinearFactorGraph::error(const Values& c) const {
-	double NonlinearFactorGraph<VALUES>::error(const VALUES& c) const {
 		double total_error = 0.;
 		// iterate over all the factors_ to accumulate the log probabilities
 		BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
@@ -53,8 +49,7 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES>
+	std::set<Symbol> NonlinearFactorGraph::keys() const {
-	std::set<Symbol> NonlinearFactorGraph<VALUES>::keys() const {
 		std::set<Symbol> keys;
 		BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
 		  if(factor)
@@ -64,9 +59,8 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES>
+	Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
-	Ordering::shared_ptr NonlinearFactorGraph<VALUES>::orderingCOLAMD(
+			const Values& config) const {
-			const VALUES& config) const {
 
 		// Create symbolic graph and initial (iterator) ordering
 		SymbolicFactorGraph::shared_ptr symbolic;
@@ -91,8 +85,7 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES>
+	SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const {
-	SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::symbolic(const Ordering& ordering) const {
 		// Generate the symbolic factor graph
 		SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
 		symbolicfg->reserve(this->size());
@@ -108,21 +101,19 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES>
+	pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
-	pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph<
+		const Values& config) const {
-			VALUES>::symbolic(const VALUES& config) const {
 		// Generate an initial key ordering in iterator order
 		Ordering::shared_ptr ordering(config.orderingArbitrary());
 		return make_pair(symbolic(*ordering), ordering);
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES>
+	GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
-	typename GaussianFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::linearize(
+			const Values& config, const Ordering& ordering) const {
-			const VALUES& config, const Ordering& ordering) const {
 
 		// create an empty linear FG
-		typename GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
+		GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
 		linearFG->reserve(this->size());
 
 		// linearize all factors

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -28,42 +28,37 @@
 namespace gtsam {
 
 	/**
-	 * A non-linear factor graph is templated on a values structure, but the factor type
+	 * A non-linear factor graph is a graph of non-Gaussian, i.e. non-linear factors,
-	 * is fixed as a NonlinearFactor. The values structures are typically (in SAM) more general
+	 * which derive from NonlinearFactor. The values structures are typically (in SAM) more general
 	 * than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds.
 	 * Linearizing the non-linear factor graph creates a linear factor graph on the 
 	 * tangent vector space at the linearization point. Because the tangent space is a true
 	 * vector space, the config type will be an VectorValues in that linearized factor graph.
-	 * \nosubgrouping
 	 */
-	template<class VALUES>
+	class NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
-	class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<VALUES> > {
 
 	public:
 
-		typedef VALUES Values;
+	  typedef FactorGraph<NonlinearFactor> Base;
-	  typedef FactorGraph<NonlinearFactor<VALUES> > Base;
+	  typedef boost::shared_ptr<NonlinearFactorGraph> shared_ptr;
-	  typedef boost::shared_ptr<NonlinearFactorGraph<VALUES> > shared_ptr;
+		typedef boost::shared_ptr<NonlinearFactor> sharedFactor;
-		typedef boost::shared_ptr<NonlinearFactor<VALUES> > sharedFactor;
-
-		/// @name Testable
-		/// @{
 
     /** print just calls base class */
     void print(const std::string& str = "NonlinearFactorGraph: ") const;
 
-  	/// @}
-  	/// @name Standard Interface
-  	/// @{
-
     /** return keys in some random order */
     std::set<Symbol> keys() const;
 
 		/** unnormalized error */
-		double error(const VALUES& c) const;
+		double error(const Values& c) const;
 
 		/** Unnormalized probability. O(n) */
-		double probPrime(const VALUES& c) const;
+		double probPrime(const Values& c) const;
+
+		template<class F>
+		void add(const F& factor) {
+			this->push_back(boost::shared_ptr<F>(new F(factor)));
+		}
 
 		/**
 		 * Create a symbolic factor graph using an existing ordering
@@ -77,31 +72,20 @@ namespace gtsam {
 		 * ordering is found.
 		 */
 		std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
-		symbolic(const VALUES& config) const;
+		symbolic(const Values& config) const;
 
     /**
      * Compute a fill-reducing ordering using COLAMD.  This returns the
      * ordering and a VariableIndex, which can later be re-used to save
      * computation.
      */
-		Ordering::shared_ptr orderingCOLAMD(const VALUES& config) const;
+		Ordering::shared_ptr orderingCOLAMD(const Values& config) const;
 
 		/**
 		 * linearize a nonlinear factor graph
 		 */
 		boost::shared_ptr<GaussianFactorGraph >
-				linearize(const VALUES& config, const Ordering& ordering) const;
+				linearize(const Values& config, const Ordering& ordering) const;
-
-  	/// @}
-		/// @name Advanced Interface
-		/// @{
-
-		template<class F>
-		void add(const F& factor) {
-			this->push_back(boost::shared_ptr<F>(new F(factor)));
-		}
-
-		/// @}
 
 	private:
 
@@ -116,4 +100,3 @@ namespace gtsam {
 
 } // namespace
 
-#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>

gtsam/nonlinear/NonlinearISAM-inl.h

@@ -29,8 +29,8 @@ using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
-template<class VALUES, class GRAPH>
+template<class GRAPH>
-void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors,
+void NonlinearISAM<GRAPH>::update(const Factors& newFactors,
 		const Values& initialValues) {
 
   if(newFactors.size() > 0) {
@@ -62,8 +62,8 @@ void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors,
 }
 
 /* ************************************************************************* */
-template<class VALUES, class GRAPH>
+template<class GRAPH>
-void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() {
+void NonlinearISAM<GRAPH>::reorder_relinearize() {
 
 //  cout << "Reordering, relinearizing..." << endl;
 
@@ -89,8 +89,8 @@ void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() {
 }
 
 /* ************************************************************************* */
-template<class VALUES, class GRAPH>
+template<class GRAPH>
-VALUES NonlinearISAM<VALUES,GRAPH>::estimate() const {
+Values NonlinearISAM<GRAPH>::estimate() const {
   if(isam_.size() > 0)
     return linPoint_.retract(optimize(isam_), ordering_);
   else
@@ -98,14 +98,14 @@ VALUES NonlinearISAM<VALUES,GRAPH>::estimate() const {
 }
 
 /* ************************************************************************* */
-template<class VALUES, class GRAPH>
+template<class GRAPH>
-Matrix NonlinearISAM<VALUES,GRAPH>::marginalCovariance(const Symbol& key) const {
+Matrix NonlinearISAM<GRAPH>::marginalCovariance(const Symbol& key) const {
 	return isam_.marginalCovariance(ordering_[key]);
 }
 
 /* ************************************************************************* */
-template<class VALUES, class GRAPH>
+template<class GRAPH>
-void NonlinearISAM<VALUES,GRAPH>::print(const std::string& s) const {
+void NonlinearISAM<GRAPH>::print(const std::string& s) const {
 	cout << "ISAM - " << s << ":" << endl;
 	cout << "  ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl;
 	isam_.print("GaussianISAM");

gtsam/nonlinear/NonlinearISAM.h

@@ -24,11 +24,10 @@ namespace gtsam {
 /**
  * Wrapper class to manage ISAM in a nonlinear context
  */
-template<class VALUES, class GRAPH = gtsam::NonlinearFactorGraph<VALUES> >
+template<class GRAPH = gtsam::NonlinearFactorGraph >
 class NonlinearISAM {
 public:
 
-	typedef VALUES Values;
 	typedef GRAPH Factors;
 
 protected:

gtsam/nonlinear/NonlinearOptimization-inl.h

@@ -34,17 +34,17 @@ namespace gtsam {
 	/**
 	 * The Elimination solver
 	 */
-	template<class G, class T>
+	template<class G>
-	T	optimizeSequential(const G& graph, const T& initialEstimate,
+	Values	optimizeSequential(const G& graph, const Values& initialEstimate,
 			const NonlinearOptimizationParameters& parameters, bool useLM) {
 
 		// Use a variable ordering from COLAMD
 	  Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
 
 		// Create an nonlinear Optimizer that uses a Sequential Solver
-	  typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
+	  typedef NonlinearOptimizer<G, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
 	  Optimizer optimizer(boost::make_shared<const G>(graph),
-	  		boost::make_shared<const T>(initialEstimate), ordering,
+	  		boost::make_shared<const Values>(initialEstimate), ordering,
 	  		boost::make_shared<NonlinearOptimizationParameters>(parameters));
 
 	  // Now optimize using either LM or GN methods.
@@ -57,17 +57,17 @@ namespace gtsam {
 	/**
 	 * The multifrontal solver
 	 */
-	template<class G, class T>
+	template<class G>
-	T	optimizeMultiFrontal(const G& graph, const T& initialEstimate,
+	Values	optimizeMultiFrontal(const G& graph, const Values& initialEstimate,
 			const NonlinearOptimizationParameters& parameters, bool useLM) {
 
 		// Use a variable ordering from COLAMD
 	  Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
 
 		// Create an nonlinear Optimizer that uses a Multifrontal Solver
-	  typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
+	  typedef NonlinearOptimizer<G, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
 	  Optimizer optimizer(boost::make_shared<const G>(graph),
-	  		boost::make_shared<const T>(initialEstimate), ordering,
+	  		boost::make_shared<const Values>(initialEstimate), ordering,
 	  		boost::make_shared<NonlinearOptimizationParameters>(parameters));
 
 	  // now optimize using either LM or GN methods
@@ -81,20 +81,20 @@ namespace gtsam {
 	/**
 	 * The sparse preconditioned conjugate gradient solver
 	 */
-	template<class G, class T>
+	template<class G>
-	T	optimizeSPCG(const G& graph, const T& initialEstimate,
+	Values	optimizeSPCG(const G& graph, const Values& initialEstimate,
 			const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
 			bool useLM = true) {
 
 		// initial optimization state is the same in both cases tested
-		typedef SubgraphSolver<G,GaussianFactorGraph,T> Solver;
+		typedef SubgraphSolver<G,GaussianFactorGraph,Values> Solver;
 		typedef boost::shared_ptr<Solver> shared_Solver;
-		typedef NonlinearOptimizer<G, T, GaussianFactorGraph, Solver> SPCGOptimizer;
+		typedef NonlinearOptimizer<G, GaussianFactorGraph, Solver> SPCGOptimizer;
 		shared_Solver solver = boost::make_shared<Solver>(
 				graph, initialEstimate, IterativeOptimizationParameters());
 		SPCGOptimizer optimizer(
 				boost::make_shared<const G>(graph),
-				boost::make_shared<const T>(initialEstimate),
+				boost::make_shared<const Values>(initialEstimate),
 				solver->ordering(),
 				solver,
 				boost::make_shared<NonlinearOptimizationParameters>(parameters));
@@ -110,20 +110,20 @@ namespace gtsam {
 	/**
 	 * optimization that returns the values
 	 */
-	template<class G, class T>
+	template<class G>
-	T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters,
+	Values optimize(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters,
 			const LinearSolver& solver,
 			const NonlinearOptimizationMethod& nonlinear_method) {
 		switch (solver) {
 		case SEQUENTIAL:
-			return optimizeSequential<G,T>(graph, initialEstimate, parameters,
+			return optimizeSequential<G>(graph, initialEstimate, parameters,
 					nonlinear_method == LM);
 		case MULTIFRONTAL:
-			return optimizeMultiFrontal<G,T>(graph, initialEstimate, parameters,
+			return optimizeMultiFrontal<G>(graph, initialEstimate, parameters,
 					nonlinear_method == LM);
 #if ENABLE_SPCG
 		case SPCG:
-//			return optimizeSPCG<G,T>(graph, initialEstimate, parameters,
+//			return optimizeSPCG<G,Values>(graph, initialEstimate, parameters,
 //								nonlinear_method == LM);
 			throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint");
 #endif

gtsam/nonlinear/NonlinearOptimization.h

@@ -44,8 +44,8 @@ namespace gtsam {
 	/**
 	 * optimization that returns the values
 	 */
-	template<class G, class T>
+	template<class G>
-	T optimize(const G& graph, const T& initialEstimate,
+	Values optimize(const G& graph, const Values& initialEstimate,
 			const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
 			const LinearSolver& solver = MULTIFRONTAL,
 			const NonlinearOptimizationMethod& nonlinear_method = LM);

gtsam/nonlinear/NonlinearOptimizer-inl.h

@@ -30,8 +30,8 @@ using namespace std;
 namespace gtsam {
 
 	/* ************************************************************************* */
-	template<class G, class C, class L, class S, class W>
+	template<class G, class L, class S, class W>
-	NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
+	NonlinearOptimizer<G, L, S, W>::NonlinearOptimizer(shared_graph graph,
 			shared_values values, shared_ordering ordering, shared_parameters parameters) :
 			graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering),
 			parameters_(parameters), iterations_(0),
@@ -47,8 +47,8 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	// FIXME: remove this constructor
-	template<class G, class C, class L, class S, class W>
+	template<class G, class L, class S, class W>
-	NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
+	NonlinearOptimizer<G, L, S, W>::NonlinearOptimizer(shared_graph graph,
 			shared_values values, shared_ordering ordering,
 			shared_solver spcg_solver, shared_parameters parameters) :
 			graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering),
@@ -66,8 +66,8 @@ namespace gtsam {
 	/* ************************************************************************* */
 	// One iteration of Gauss Newton
 	/* ************************************************************************* */
-	template<class G, class C, class L, class S, class W>
+	template<class G, class L, class S, class W>
-	NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterate() const {
+	NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterate() const {
 
 		Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
 
@@ -86,7 +86,7 @@ namespace gtsam {
 		if (verbosity >= Parameters::DELTA) delta.print("delta");
 
 		// take old values and update it
-		shared_values newValues(new C(values_->retract(delta, *ordering_)));
+		shared_values newValues(new Values(values_->retract(delta, *ordering_)));
 
 		// maybe show output
 		if (verbosity >= Parameters::VALUES) newValues->print("newValues");
@@ -99,8 +99,8 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	template<class G, class C, class L, class S, class W>
+	template<class G, class L, class S, class W>
-	NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::gaussNewton() const {
+	NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::gaussNewton() const {
 		static W writer(error_);
 
 		if (error_ < parameters_->sumError_ ) {
@@ -130,8 +130,8 @@ namespace gtsam {
 	// TODO: in theory we can't infinitely recurse, but maybe we should put a max.
 	// Reminder: the parameters are Graph type $G$, Values class type $T$,
 	// linear system class $L$, the non linear solver type $S$, and the writer type $W$
-	template<class G, class T, class L, class S, class W>
+	template<class G, class L, class S, class W>
-	NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::try_lambda(const L& linearSystem) {
+	NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::try_lambda(const L& linearSystem) {
 
 		const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
 		const Parameters::LambdaMode lambdaMode = parameters_->lambdaMode_ ;
@@ -178,7 +178,7 @@ namespace gtsam {
 		    if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
 
 		    // update values
-		    shared_values newValues(new T(values_->retract(delta, *ordering_)));
+		    shared_values newValues(new Values(values_->retract(delta, *ordering_)));
 
 		    // create new optimization state with more adventurous lambda
 		    double error = graph_->error(*newValues);
@@ -228,8 +228,8 @@ namespace gtsam {
 	// One iteration of Levenberg Marquardt
 	// Reminder: the parameters are Graph type $G$, Values class type $T$,
 	// linear system class $L$, the non linear solver type $S$, and the writer type $W$
-	template<class G, class T, class L, class S, class W>
+	template<class G, class L, class S, class W>
-	NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateLM() {
+	NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterateLM() {
 
 		const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
 		const double lambda = parameters_->lambda_ ;
@@ -253,8 +253,8 @@ namespace gtsam {
 	/* ************************************************************************* */
 	// Reminder: the parameters are Graph type $G$, Values class type $T$,
 	// linear system class $L$, the non linear solver type $S$, and the writer type $W$
-	template<class G, class T, class L, class S, class W>
+	template<class G, class L, class S, class W>
-	NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::levenbergMarquardt() {
+	NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::levenbergMarquardt() {
 
 		// Initialize
 		bool converged = false;
@@ -299,20 +299,20 @@ namespace gtsam {
 	}
 
   /* ************************************************************************* */
-  template<class G, class T, class L, class S, class W>
+  template<class G, class L, class S, class W>
-  NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateDogLeg() {
+  NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterateDogLeg() {
 
     S solver(*graph_->linearize(*values_, *ordering_), parameters_->useQR_);
     DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(
         parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(),
         *graph_, *values_, *ordering_, error_, parameters_->verbosity_ > Parameters::ERROR);
-    shared_values newValues(new T(values_->retract(result.dx_d, *ordering_)));
+    shared_values newValues(new Values(values_->retract(result.dx_d, *ordering_)));
     return newValuesErrorLambda_(newValues, result.f_error, result.Delta);
   }
 
   /* ************************************************************************* */
-  template<class G, class T, class L, class S, class W>
+  template<class G, class L, class S, class W>
-  NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::dogLeg() {
+  NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::dogLeg() {
     static W writer(error_);
 
     // check if we're already close enough

gtsam/nonlinear/NonlinearOptimizer.h

@@ -37,20 +37,20 @@ public:
  * and then one of the optimization routines is called. These iterate
  * until convergence. All methods are functional and return a new state.
  *
- * The class is parameterized by the Graph type $G$, Values class type $T$,
+ * The class is parameterized by the Graph type $G$, Values class type $Values$,
  * linear system class $L$, the non linear solver type $S$, and the writer type $W$
  *
- * The values class type $T$ is in order to be able to optimize over non-vector values structures.
+ * The values class type $Values$ is in order to be able to optimize over non-vector values structures.
  *
  * A nonlinear system solver $S$
- * Concept NonLinearSolver<G,T,L> implements
+ * Concept NonLinearSolver<G,Values,L> implements
- *   linearize: G * T -> L
+ *   linearize: G * Values -> L
- *   solve : L -> T
+ *   solve : L -> Values
  *
  * The writer $W$ generates output to disk or the screen.
  *
- * For example, in a 2D case, $G$ can be Pose2Graph, $T$ can be Pose2Values,
+ * For example, in a 2D case, $G$ can be pose2SLAM::Graph, $Values$ can be Pose2Values,
- * $L$ can be GaussianFactorGraph and $S$ can be Factorization<Pose2Graph, Pose2Values>.
+ * $L$ can be GaussianFactorGraph and $S$ can be Factorization<pose2SLAM::Graph, Pose2Values>.
  * The solver class has two main functions: linearize and optimize. The first one
  * linearizes the nonlinear cost function around the current estimate, and the second
  * one optimizes the linearized system using various methods.
@@ -58,12 +58,12 @@ public:
  * To use the optimizer in code, include <gtsam/NonlinearOptimizer-inl.h> in your cpp file
  * \nosubgrouping
  */
-template<class G, class T, class L = GaussianFactorGraph, class GS = GaussianMultifrontalSolver, class W = NullOptimizerWriter>
+template<class G, class L = GaussianFactorGraph, class GS = GaussianMultifrontalSolver, class W = NullOptimizerWriter>
 class NonlinearOptimizer {
 public:
 
 	// For performance reasons in recursion, we store values in a shared_ptr
-	typedef boost::shared_ptr<const T> shared_values; ///Prevent memory leaks in Values
+	typedef boost::shared_ptr<const Values> shared_values; ///Prevent memory leaks in Values
 	typedef boost::shared_ptr<const G> shared_graph;  /// Prevent memory leaks in Graph
 	typedef boost::shared_ptr<L> shared_linear;    /// Not sure
 	typedef boost::shared_ptr<const Ordering> shared_ordering; ///ordering parameters
@@ -74,7 +74,7 @@ public:
 
 private:
 
-	typedef NonlinearOptimizer<G, T, L, GS> This;
+	typedef NonlinearOptimizer<G, L, GS> This;
 	typedef boost::shared_ptr<const std::vector<size_t> > shared_dimensions;
 
 	/// keep a reference to const version of the graph
@@ -182,7 +182,7 @@ public:
 	/**
 	 * Copy constructor
 	 */
-	NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) :
+	NonlinearOptimizer(const NonlinearOptimizer<G, L, GS> &optimizer) :
 		graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_),
 		ordering_(optimizer.ordering_), parameters_(optimizer.parameters_),
 		iterations_(optimizer.iterations_), dimensions_(optimizer.dimensions_), structure_(optimizer.structure_) {}
@@ -248,7 +248,7 @@ public:
 
 	/**
 	 *  linearize and optimize
-	 *  This returns an VectorValues, i.e., vectors in tangent space of T
+	 *  This returns an VectorValues, i.e., vectors in tangent space of Values
 	 */
 	VectorValues linearizeAndOptimizeForDelta() const {
 		return *createSolver()->optimize();
@@ -340,19 +340,19 @@ public:
 	 * Static interface to LM optimization (no shared_ptr arguments) - see above
 	 */
 	static shared_values optimizeLM(const G& graph,
-			const T& values,
+			const Values& values,
 			const Parameters parameters = Parameters()) {
 		return optimizeLM(boost::make_shared<const G>(graph),
-				boost::make_shared<const T>(values),
+				boost::make_shared<const Values>(values),
 				boost::make_shared<Parameters>(parameters));
 	}
 
 	///TODO: comment
 	static shared_values optimizeLM(const G& graph,
-			const T& values,
+			const Values& values,
 			Parameters::verbosityLevel verbosity) {
 		return optimizeLM(boost::make_shared<const G>(graph),
-				boost::make_shared<const T>(values),
+				boost::make_shared<const Values>(values),
 				verbosity);
 	}
 
@@ -392,19 +392,19 @@ public:
    * Static interface to Dogleg optimization (no shared_ptr arguments) - see above
    */
   static shared_values optimizeDogLeg(const G& graph,
-      const T& values,
+      const Values& values,
       const Parameters parameters = Parameters()) {
     return optimizeDogLeg(boost::make_shared<const G>(graph),
-        boost::make_shared<const T>(values),
+        boost::make_shared<const Values>(values),
         boost::make_shared<Parameters>(parameters));
   }
 
 	///TODO: comment
   static shared_values optimizeDogLeg(const G& graph,
-      const T& values,
+      const Values& values,
       Parameters::verbosityLevel verbosity) {
     return optimizeDogLeg(boost::make_shared<const G>(graph),
-        boost::make_shared<const T>(values),
+        boost::make_shared<const Values>(values),
         verbosity);
   }
 
@@ -431,9 +431,9 @@ public:
 	/**
 	 * Static interface to GN optimization (no shared_ptr arguments) - see above
 	 */
-	static shared_values optimizeGN(const G& graph, const T& values, const Parameters parameters = Parameters()) {
+	static shared_values optimizeGN(const G& graph, const Values& values, const Parameters parameters = Parameters()) {
 		return optimizeGN(boost::make_shared<const G>(graph),
-				boost::make_shared<const T>(values),
+				boost::make_shared<const Values>(values),
 				boost::make_shared<Parameters>(parameters));
 	}
 };

gtsam/nonlinear/TupleValues-inl.h

@@ -1,165 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file TupleValues-inl.h
- * @author Richard Roberts
- * @author Manohar Paluri
- * @author Alex Cunningham
- */
-
-#pragma once
-
-namespace gtsam {
-
-/* ************************************************************************* */
-/** TupleValuesN Implementations */
-/* ************************************************************************* */
-
-/* ************************************************************************* */
-/** TupleValues 1 */
-template<class VALUES1>
-TupleValues1<VALUES1>::TupleValues1(const TupleValues1<VALUES1>& values) :
-		  TupleValuesEnd<VALUES1> (values) {}
-
-template<class VALUES1>
-TupleValues1<VALUES1>::TupleValues1(const VALUES1& cfg1) :
-			  TupleValuesEnd<VALUES1> (cfg1) {}
-
-template<class VALUES1>
-TupleValues1<VALUES1>::TupleValues1(const TupleValuesEnd<VALUES1>& values) :
-	TupleValuesEnd<VALUES1>(values) {}
-
-/* ************************************************************************* */
-/** TupleValues 2 */
-template<class VALUES1, class VALUES2>
-TupleValues2<VALUES1, VALUES2>::TupleValues2(const TupleValues2<VALUES1, VALUES2>& values) :
-		  TupleValues<VALUES1, TupleValuesEnd<VALUES2> >(values) {}
-
-template<class VALUES1, class VALUES2>
-TupleValues2<VALUES1, VALUES2>::TupleValues2(const VALUES1& cfg1, const VALUES2& cfg2) :
-			  TupleValues<VALUES1, TupleValuesEnd<VALUES2> >(
-					  cfg1, TupleValuesEnd<VALUES2>(cfg2)) {}
-
-template<class VALUES1, class VALUES2>
-TupleValues2<VALUES1, VALUES2>::TupleValues2(const TupleValues<VALUES1, TupleValuesEnd<VALUES2> >& values) :
-	TupleValues<VALUES1, TupleValuesEnd<VALUES2> >(values) {}
-
-/* ************************************************************************* */
-/** TupleValues 3 */
-template<class VALUES1, class VALUES2, class VALUES3>
-TupleValues3<VALUES1, VALUES2, VALUES3>::TupleValues3(
-		const TupleValues3<VALUES1, VALUES2, VALUES3>& values) :
-		  TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >(values) {}
-
-template<class VALUES1, class VALUES2, class VALUES3>
-TupleValues3<VALUES1, VALUES2, VALUES3>::TupleValues3(
-		const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3) :
-			  TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >(
-					  cfg1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> >(
-							  cfg2, TupleValuesEnd<VALUES3>(cfg3))) {}
-
-template<class VALUES1, class VALUES2, class VALUES3>
-TupleValues3<VALUES1, VALUES2, VALUES3>::TupleValues3(
-		const TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >& values) :
-		  TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >(values) {}
-
-/* ************************************************************************* */
-/** TupleValues 4 */
-template<class VALUES1, class VALUES2, class VALUES3, class VALUES4>
-TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>::TupleValues4(
-		const TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>& values) :
-	TupleValues<VALUES1, TupleValues<VALUES2,
-		TupleValues<VALUES3, TupleValuesEnd<VALUES4> > > >(values) {}
-
-template<class VALUES1, class VALUES2, class VALUES3, class VALUES4>
-TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>::TupleValues4(
-		const VALUES1& cfg1, const VALUES2& cfg2,
-		const VALUES3& cfg3,const VALUES4& cfg4) :
-		  TupleValues<VALUES1, TupleValues<VALUES2,
-			  TupleValues<VALUES3, TupleValuesEnd<VALUES4> > > >(
-				  cfg1, TupleValues<VALUES2, TupleValues<VALUES3, TupleValuesEnd<VALUES4> > >(
-						  cfg2, TupleValues<VALUES3, TupleValuesEnd<VALUES4> >(
-								  cfg3, TupleValuesEnd<VALUES4>(cfg4)))) {}
-
-template<class VALUES1, class VALUES2, class VALUES3, class VALUES4>
-TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>::TupleValues4(
-		const TupleValues<VALUES1, TupleValues<VALUES2,
-				TupleValues<VALUES3, TupleValuesEnd<VALUES4> > > >& values) :
-	TupleValues<VALUES1, TupleValues<VALUES2,TupleValues<VALUES3,
-		TupleValuesEnd<VALUES4> > > >(values) {}
-
-/* ************************************************************************* */
-/** TupleValues 5 */
-template<class VALUES1, class VALUES2, class VALUES3, class VALUES4, class VALUES5>
-TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>::TupleValues5(
-		const TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>& values) :
-		  TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
-			  TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > > >(values) {}
-
-template<class VALUES1, class VALUES2, class VALUES3, class VALUES4, class VALUES5>
-TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>::TupleValues5(
-		const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3,
-				   const VALUES4& cfg4, const VALUES5& cfg5) :
-						   TupleValues<VALUES1, TupleValues<VALUES2,
-						   TupleValues<VALUES3, TupleValues<VALUES4,
-						   TupleValuesEnd<VALUES5> > > > >(
-								   cfg1, TupleValues<VALUES2, TupleValues<VALUES3,
-										 TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > >(
-										   cfg2, TupleValues<VALUES3, TupleValues<VALUES4, TupleValuesEnd<VALUES5> > >(
-												   cfg3, TupleValues<VALUES4, TupleValuesEnd<VALUES5> >(
-														   cfg4, TupleValuesEnd<VALUES5>(cfg5))))) {}
-
-template<class VALUES1, class VALUES2, class VALUES3, class VALUES4, class VALUES5>
-TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>::TupleValues5(
-		const TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
-			  TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > > >& values) :
-	TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
-	TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > > >(values) {}
-
-/* ************************************************************************* */
-/** TupleValues 6 */
-template<class VALUES1, class VALUES2, class VALUES3,
-		 class VALUES4, class VALUES5, class VALUES6>
-TupleValues6<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5, VALUES6>::TupleValues6(
-		const TupleValues6<VALUES1, VALUES2, VALUES3,
-						   VALUES4, VALUES5, VALUES6>& values) :
-			  TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
-			  TupleValues<VALUES4, TupleValues<VALUES5,
-			  TupleValuesEnd<VALUES6> > > > > >(values) {}
-
-template<class VALUES1, class VALUES2, class VALUES3,
-		 class VALUES4, class VALUES5, class VALUES6>
-TupleValues6<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5, VALUES6>::TupleValues6(
-		const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3,
-		const VALUES4& cfg4, const VALUES5& cfg5, const VALUES6& cfg6) :
-		TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
-		TupleValues<VALUES4, TupleValues<VALUES5, TupleValuesEnd<VALUES6> > > > > >(
-				cfg1, TupleValues<VALUES2, TupleValues<VALUES3, TupleValues<VALUES4,
-				      TupleValues<VALUES5, TupleValuesEnd<VALUES6> > > > >(
-						cfg2, TupleValues<VALUES3, TupleValues<VALUES4, TupleValues<VALUES5,
-							  TupleValuesEnd<VALUES6> > > >(
-								cfg3, TupleValues<VALUES4, TupleValues<VALUES5,
-								      TupleValuesEnd<VALUES6> > >(
-										cfg4, TupleValues<VALUES5, TupleValuesEnd<VALUES6> >(
-												cfg5, TupleValuesEnd<VALUES6>(cfg6)))))) {}
-
-template<class VALUES1, class VALUES2, class VALUES3,
-	     class VALUES4, class VALUES5, class VALUES6>
-TupleValues6<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5, VALUES6>::TupleValues6(
-		const TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
-		      TupleValues<VALUES4, TupleValues<VALUES5,
-		      TupleValuesEnd<VALUES6> > > > > >& values) :
-	TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
-	TupleValues<VALUES4, TupleValues<VALUES5,
-	TupleValuesEnd<VALUES6> > > > > >(values) {}
-
-}

gtsam/nonlinear/TupleValues.h

@@ -1,550 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file TupleValues.h
- * @author Richard Roberts
- * @author Manohar Paluri
- * @author Alex Cunningham
- */
-
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/linear/VectorValues.h>
-
-#pragma once
-
-namespace gtsam {
-
- /**
-   *  TupleValues are a structure to manage heterogenous Values, so as to
-   *  enable different types of variables/keys to be used simultaneously.  We
-   *  do this with recursive templates (instead of blind pointer casting) to
-   *  reduce run-time overhead and keep static type checking.  The interface
-   *  mimics that of a single Values.
-   *
-   *  This uses a recursive structure of values pairs to form a lisp-like
-   *  list, with a special case (TupleValuesEnd) that contains only one values
-   *  at the end.  Because this recursion is done at compile time, there is no
-   *  runtime performance hit to using this structure.
-   *
-   *  For an easy interface, there are TupleValuesN classes, which wrap
-   *  the recursive TupleValues together as a single class, so you can have
-   *  mixed-type classes from 2-6 types.  Note that a TupleValues2 is equivalent
-   *  to the previously used PairValues.
-   *
-   *  Design and extension note:
-   *  To implement a recursively templated data structure, note that most operations
-   *  have two versions: one with templates and one without.  The templated one allows
-   *  for the arguments to be passed to the next values, while the specialized one
-   *  operates on the "first" values. TupleValuesEnd contains only the specialized version.
-   *  \nosubgrouping
-   */
-  template<class VALUES1, class VALUES2>
-  class TupleValues {
-
-  protected:
-	  // Data for internal valuess
-	  VALUES1 first_;	/// Arbitrary values
-	  VALUES2 second_;	/// A TupleValues or TupleValuesEnd, which wraps an arbitrary values
-
-	  /** concept checks */
-	  GTSAM_CONCEPT_TESTABLE_TYPE(VALUES1)
-	  GTSAM_CONCEPT_TESTABLE_TYPE(VALUES2)
-
-  public:
-	  // typedefs for values subtypes
-	  typedef typename VALUES1::Key Key1;
-	  typedef typename VALUES1::Value Value1;
-
-		/// @name Standard Constructors
-		/// @{
-
-	  /** default constructor */
-	  TupleValues() {}
-
-	  /** Copy constructor */
-	  TupleValues(const TupleValues<VALUES1, VALUES2>& values) :
-		  first_(values.first_), second_(values.second_) {}
-
-	  /** Construct from valuess */
-	  TupleValues(const VALUES1& cfg1, const VALUES2& cfg2) :
-		  first_(cfg1), second_(cfg2) {}
-
-	  /// @}
-		/// @name Testable
-		/// @{
-
-	  /** Print */
-	  void print(const std::string& s = "") const {
-			first_.print(s);
-			second_.print();
-	  }
-
-	  /** Equality with tolerance for keys and values */
-	  bool equals(const TupleValues<VALUES1, VALUES2>& c, double tol=1e-9) const {
-		  return first_.equals(c.first_, tol) && second_.equals(c.second_, tol);
-	  }
-
-    /// @}
-    /// @name Advanced Interface
-    /// @{
-
-	  /**
-	   * Insert a key/value pair to the values.
-	   * Note: if the key is already in the values, the values will not be changed.
-	   * Use update() to allow for changing existing values.
-	   * @param key is the key - can be an int (second version) if the can can be initialized from an int
-	   * @param value is the value to insert
-	   */
-	  template<class KEY, class VALUE>
-	  void insert(const KEY& key, const VALUE& value) {second_.insert(key, value);}
-	  void insert(int key, const Value1& value) {first_.insert(Key1(key), value);}		///<TODO: comment
-	  void insert(const Key1& key, const Value1& value) {first_.insert(key, value);}	///<TODO: comment
-
-	  /**
-	   * Insert a complete values at a time.
-	   * Note: if the key is already in the values, the values will not be changed.
-	   * Use update() to allow for changing existing values.
-	   * @param values is a full values to add
-	   */
-	  template<class CFG1, class CFG2>
-	  void insert(const TupleValues<CFG1, CFG2>& values) { second_.insert(values); }
-	  void insert(const TupleValues<VALUES1, VALUES2>& values) {		///<TODO: comment
-		  first_.insert(values.first_);
-		  second_.insert(values.second_);
-	  }
-
-	  /**
-	   * Update function for whole valuess - this will change existing values
-	   * @param values is a values to add
-	   */
-	  template<class CFG1, class CFG2>
-	  void update(const TupleValues<CFG1, CFG2>& values) { second_.update(values); }
-	  void update(const TupleValues<VALUES1, VALUES2>& values) {		///<TODO: comment
-	  	first_.update(values.first_);
-	  	second_.update(values.second_);
-	  }
-
-	  /**
-	   * Update function for single key/value pairs - will change existing values
-	   * @param key is the variable identifier
-	   * @param value is the variable value to update
-	   */
-	  template<class KEY, class VALUE>
-	  void update(const KEY& key, const VALUE& value) { second_.update(key, value); }
-	  void update(const Key1& key, const Value1& value) { first_.update(key, value); }	///<TODO: comment
-
-	  /**
-	   * Insert a subvalues
-	   * @param values is the values to insert
-	   */
-	  template<class CFG>
-	  void insertSub(const CFG& values) { second_.insertSub(values); }
-	  void insertSub(const VALUES1& values) { first_.insert(values);  }	///<TODO: comment
-
-	  /** erase an element by key */
-	  template<class KEY>
-	  void erase(const KEY& j)  { second_.erase(j); }
-	  void erase(const Key1& j)  { first_.erase(j); }	///<TODO: comment
-
-	  /** clears the values */
-	  void clear() { first_.clear(); second_.clear(); }
-
-    /// @}
-    /// @name Standard Interface
-    /// @{
-
-	  /** determine whether an element exists */
-	  template<class KEY>
-	  bool exists(const KEY& j) const { return second_.exists(j); }
-	  bool exists(const Key1& j) const { return first_.exists(j); }	///<TODO: comment
-
-	  /** a variant of exists */
-	  template<class KEY>
-	  boost::optional<typename KEY::Value> exists_(const KEY& j)  const { return second_.exists_(j); }
-	  boost::optional<Value1>                exists_(const Key1& j) const { return first_.exists_(j); }	///<TODO: comment
-
-	  /** access operator */
-	  template<class KEY>
-	  const typename KEY::Value & operator[](const KEY& j) const { return second_[j]; }
-	  const Value1& operator[](const Key1& j) const { return first_[j]; }	///<TODO: comment
-
-	  /** at access function */
-	  template<class KEY>
-	  const typename KEY::Value & at(const KEY& j) const { return second_.at(j); }
-	  const Value1& at(const Key1& j) const { return first_.at(j); }	///<TODO: comment
-
-	  /** direct values access */
-	  const VALUES1& values() const { return first_; }
-	  const VALUES2& rest() const { return second_; }	///<TODO: comment
-
-	  /** zero: create VectorValues of appropriate structure */
-	  VectorValues zero(const Ordering& ordering) const {
-		  return VectorValues::Zero(this->dims(ordering));
-	  }
-
-	  /** @return number of key/value pairs stored */
-	  size_t size() const { return first_.size() + second_.size(); }
-
-	  /** @return true if values is empty */
-	  bool empty() const { return first_.empty() && second_.empty(); }
-
-    /**
-     * Generate a default ordering, simply in key sort order.  To instead
-     * create a fill-reducing ordering, use
-     * NonlinearFactorGraph::orderingCOLAMD().  Alternatively, you may permute
-     * this ordering yourself (as orderingCOLAMD() does internally).
-     */
-    Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const {
-      // Generate an initial key ordering in iterator order
-      _ValuesKeyOrderer keyOrderer(firstVar);
-      this->apply(keyOrderer);
-      return keyOrderer.ordering;
-    }
-
-	  /**
-	   * Apply a class with an application operator() to a const_iterator over
-	   * every <key,value> pair.  The operator must be able to handle such an
-	   * iterator for every type in the Values, (i.e. through templating).
-	   */
-    template<typename A>
-    void apply(A& operation) {
-    	first_.apply(operation);
-    	second_.apply(operation);
-    }
-
-    /**
-     * TODO: comment
-     */
-    template<typename A>
-    void apply(A& operation) const {
-    	first_.apply(operation);
-    	second_.apply(operation);
-    }
-
-    /// @}
-    /// @name Manifold
-    /// @{
-
-	  /** @return The dimensionality of the tangent space */
-	  size_t dim() const { return first_.dim() + second_.dim(); }
-
-	  /** Create an array of variable dimensions using the given ordering */
-	  std::vector<size_t> dims(const Ordering& ordering) const {
-	    _ValuesDimensionCollector dimCollector(ordering);
-	    this->apply(dimCollector);
-	    return dimCollector.dimensions;
-	  }
-
-	  /** Expmap */
-	  TupleValues<VALUES1, VALUES2> retract(const VectorValues& delta, const Ordering& ordering) const {
-	    return TupleValues(first_.retract(delta, ordering), second_.retract(delta, ordering));
-	  }
-
-	  /** logmap each element */
-	  VectorValues localCoordinates(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering) const {
-		  VectorValues delta(this->dims(ordering));
-		  localCoordinates(cp, ordering, delta);
-		  return delta;
-	  }
-
-    /** logmap each element */
-    void localCoordinates(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering, VectorValues& delta) const {
-      first_.localCoordinates(cp.first_, ordering, delta);
-      second_.localCoordinates(cp.second_, ordering, delta);
-    }
-
-    /// @}
-
-  private:
-	  /** Serialization function */
-	  friend class boost::serialization::access;
-	  template<class ARCHIVE>
-	  void serialize(ARCHIVE & ar, const unsigned int version) {
-		  ar & BOOST_SERIALIZATION_NVP(first_);
-		  ar & BOOST_SERIALIZATION_NVP(second_);
-	  }
-
-  };
-
-  /**
-   * End of a recursive TupleValues - contains only one values
-   *
-   * Do not use this class directly - it should only be used as a part
-   * of a recursive structure
-   */
-  template<class VALUES>
-  class TupleValuesEnd {
-
-  protected:
-	  // Data for internal valuess
-	  VALUES first_;
-
-  public:
-	  // typedefs
-	  typedef typename VALUES::Key Key1;
-	  typedef typename VALUES::Value Value1;
-
-	  TupleValuesEnd() {}
-
-	  TupleValuesEnd(const TupleValuesEnd<VALUES>& values) :
-		  first_(values.first_) {}
-
-	  TupleValuesEnd(const VALUES& cfg) :
-		  first_(cfg) {}
-
-	  void print(const std::string& s = "") const {
-			first_.print();
-	  }
-
-	  bool equals(const TupleValuesEnd<VALUES>& c, double tol=1e-9) const {
-		  return first_.equals(c.first_, tol);
-	  }
-
-	  void insert(const Key1& key, const Value1& value) {first_.insert(key, value); }
-	  void insert(int key, const Value1& value) {first_.insert(Key1(key), value);}
-
-	  void insert(const TupleValuesEnd<VALUES>& values) {first_.insert(values.first_); }
-
-	  void update(const TupleValuesEnd<VALUES>& values) {first_.update(values.first_); }
-
-	  void update(const Key1& key, const Value1& value) { first_.update(key, value); }
-
-	  void insertSub(const VALUES& values) {first_.insert(values); }
-
-	  const Value1& operator[](const Key1& j) const { return first_[j]; }
-
-	  const VALUES& values() const { return first_; }
-
-	  void erase(const Key1& j) { first_.erase(j); }
-
-	  void clear() { first_.clear(); }
-
-	  bool empty() const { return first_.empty(); }
-
-	  bool exists(const Key1& j) const { return first_.exists(j); }
-
-	  boost::optional<Value1> exists_(const Key1& j) const { return first_.exists_(j); }
-
-	  const Value1& at(const Key1& j) const { return first_.at(j); }
-
-	  VectorValues zero(const Ordering& ordering) const {
-		  return VectorValues::Zero(this->dims(ordering));
-	  }
-
-	  size_t size() const { return first_.size(); }
-
-	  size_t dim() const { return first_.dim(); }
-
-	  TupleValuesEnd<VALUES> retract(const VectorValues& delta, const Ordering& ordering) const {
-	        return TupleValuesEnd(first_.retract(delta, ordering));
-	  }
-
-    VectorValues localCoordinates(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering) const {
-      VectorValues delta(this->dims(ordering));
-      localCoordinates(cp, ordering, delta);
-      return delta;
-    }
-
-    void localCoordinates(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering, VectorValues& delta) const {
-      first_.localCoordinates(cp.first_, ordering, delta);
-    }
-
-    /**
-     * Apply a class with an application operator() to a const_iterator over
-     * every <key,value> pair.  The operator must be able to handle such an
-     * iterator for every type in the Values, (i.e. through templating).
-     */
-    template<typename A>
-    void apply(A& operation) {
-      first_.apply(operation);
-    }
-    template<typename A>
-    void apply(A& operation) const {
-      first_.apply(operation);
-    }
-
-    /** Create an array of variable dimensions using the given ordering */
-    std::vector<size_t> dims(const Ordering& ordering) const {
-    	_ValuesDimensionCollector dimCollector(ordering);
-    	this->apply(dimCollector);
-    	return dimCollector.dimensions;
-    }
-
-    /**
-     * Generate a default ordering, simply in key sort order.  To instead
-     * create a fill-reducing ordering, use
-     * NonlinearFactorGraph::orderingCOLAMD().  Alternatively, you may permute
-     * this ordering yourself (as orderingCOLAMD() does internally).
-     */
-    Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const {
-      // Generate an initial key ordering in iterator order
-      _ValuesKeyOrderer keyOrderer(firstVar);
-      this->apply(keyOrderer);
-      return keyOrderer.ordering;
-    }
-
-  private:
-	  friend class boost::serialization::access;
-	  template<class ARCHIVE>
-	  void serialize(ARCHIVE & ar, const unsigned int version) {
-		  ar & BOOST_SERIALIZATION_NVP(first_);
-	  }
-  };
-
-  /**
-   * Wrapper classes to act as containers for valuess.  Note that these can be cascaded
-   * recursively, as they are TupleValues, and are primarily a short form of the values
-   * structure to make use of the TupleValues easier.
-   *
-   * The interface is designed to mimic PairValues, but for 2-6 values types.
-   */
-
-  template<class C1>
-  class TupleValues1 : public TupleValuesEnd<C1> {
-  public:
- 	  // typedefs
- 	  typedef C1 Values1;
-
- 	  typedef TupleValuesEnd<C1> Base;
- 	  typedef TupleValues1<C1> This;
-
- 	  TupleValues1() {}
- 	  TupleValues1(const This& values);
- 	  TupleValues1(const Base& values);
- 	  TupleValues1(const Values1& cfg1);
-
- 	  // access functions
- 	  inline const Values1& first() const { return this->values(); }
-  };
-
-  template<class C1, class C2>
-  class TupleValues2 : public TupleValues<C1, TupleValuesEnd<C2> > {
-  public:
-	  // typedefs
-	  typedef C1 Values1;
-	  typedef C2 Values2;
-
-	  typedef TupleValues<C1, TupleValuesEnd<C2> > Base;
-	  typedef TupleValues2<C1, C2> This;
-
-	  TupleValues2() {}
-	  TupleValues2(const This& values);
-	  TupleValues2(const Base& values);
-	  TupleValues2(const Values1& cfg1, const Values2& cfg2);
-
-	  // access functions
-	  inline const Values1& first() const { return this->values(); }
-	  inline const Values2& second() const { return this->rest().values(); }
-  };
-
-  template<class C1, class C2, class C3>
-  class TupleValues3 : public TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > > {
-  public:
-	  // typedefs
-	  typedef C1 Values1;
-	  typedef C2 Values2;
-	  typedef C3 Values3;
-
-	  typedef TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > > Base;
-	  typedef TupleValues3<C1, C2, C3> This;
-
-	  TupleValues3() {}
-	  TupleValues3(const Base& values);
-	  TupleValues3(const This& values);
-	  TupleValues3(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3);
-
-	  // access functions
-	  inline const Values1& first() const { return this->values(); }
-	  inline const Values2& second() const { return this->rest().values(); }
-	  inline const Values3& third() const { return this->rest().rest().values(); }
-  };
-
-  template<class C1, class C2, class C3, class C4>
-  class TupleValues4 : public TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > {
-  public:
-	  // typedefs
-	  typedef C1 Values1;
-	  typedef C2 Values2;
-	  typedef C3 Values3;
-	  typedef C4 Values4;
-
-	  typedef TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > Base;
-	  typedef TupleValues4<C1, C2, C3, C4> This;
-
-	  TupleValues4() {}
-	  TupleValues4(const This& values);
-	  TupleValues4(const Base& values);
-	  TupleValues4(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,const Values4& cfg4);
-
-	  // access functions
-	  inline const Values1& first() const { return this->values(); }
-	  inline const Values2& second() const { return this->rest().values(); }
-	  inline const Values3& third() const { return this->rest().rest().values(); }
-	  inline const Values4& fourth() const { return this->rest().rest().rest().values(); }
-  };
-
-  template<class C1, class C2, class C3, class C4, class C5>
-  class TupleValues5 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > > {
-  public:
-	  typedef C1 Values1;
-	  typedef C2 Values2;
-	  typedef C3 Values3;
-	  typedef C4 Values4;
-	  typedef C5 Values5;
-
-	  typedef TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > > Base;
-	  typedef TupleValues5<C1, C2, C3, C4, C5> This;
-
-	  TupleValues5() {}
-	  TupleValues5(const This& values);
-	  TupleValues5(const Base& values);
-	  TupleValues5(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,
-				   const Values4& cfg4, const Values5& cfg5);
-
-	  // access functions
-	  inline const Values1& first() const { return this->values(); }
-	  inline const Values2& second() const { return this->rest().values(); }
-	  inline const Values3& third() const { return this->rest().rest().values(); }
-	  inline const Values4& fourth() const { return this->rest().rest().rest().values(); }
-	  inline const Values5& fifth() const { return this->rest().rest().rest().rest().values(); }
-  };
-
-  template<class C1, class C2, class C3, class C4, class C5, class C6>
-  class TupleValues6 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > > {
-  public:
-	  typedef C1 Values1;
-	  typedef C2 Values2;
-	  typedef C3 Values3;
-	  typedef C4 Values4;
-	  typedef C5 Values5;
-	  typedef C6 Values6;
-
-	  typedef TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > > Base;
-	  typedef TupleValues6<C1, C2, C3, C4, C5, C6> This;
-
-	  TupleValues6() {}
-	  TupleValues6(const This& values);
-	  TupleValues6(const Base& values);
-	  TupleValues6(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,
-				   const Values4& cfg4, const Values5& cfg5, const Values6& cfg6);
-	  // access functions
-	  inline const Values1& first() const { return this->values(); }
-	  inline const Values2& second() const { return this->rest().values(); }
-	  inline const Values3& third() const { return this->rest().rest().values(); }
-	  inline const Values4& fourth() const { return this->rest().rest().rest().values(); }
-	  inline const Values5& fifth() const { return this->rest().rest().rest().rest().values(); }
-	  inline const Values6& sixth() const { return this->rest().rest().rest().rest().rest().values(); }
-  };
-
-}
-
-#include <gtsam/nonlinear/TupleValues-inl.h>

gtsam/nonlinear/Values-inl.h

@@ -10,222 +10,87 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Values.cpp
+ * @file Values.h
  * @author Richard Roberts
+ *
+ * @brief A non-templated config holding any types of Manifold-group elements
+ *
+ *  Detailed story:
+ *  A values structure is a map from keys to values. It is used to specify the value of a bunch
+ *  of variables in a factor graph. A Values is a values structure which can hold variables that
+ *  are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
+ *  which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
  */
 
-#pragma once
-
 #include <utility>
-#include <iostream>
-#include <stdexcept>
 
-#include <boost/foreach.hpp>
+#include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition
-#include <boost/tuple/tuple.hpp>
-
-#include <gtsam/linear/VectorValues.h>
-#include <gtsam/base/LieVector.h>
-#include <gtsam/base/Lie-inl.h>
-#include <gtsam/nonlinear/Ordering.h>
-
-using namespace std;
 
 namespace gtsam {
 
-/* ************************************************************************* */
+  /* ************************************************************************* */
-  template<class J>
+  class ValueCloneAllocator {
-  void Values<J>::print(const string &s) const {
+  public:
-       cout << "Values " << s << ", size " << values_.size() << "\n";
+    static Value* allocate_clone(const Value& a) { return a.clone_(); }
-       BOOST_FOREACH(const KeyValuePair& v, values_) {
+    static void deallocate_clone(const Value* a) { a->deallocate_(); }
-         gtsam::print(v.second, (string)(v.first));
+  private:
-       }
+    ValueCloneAllocator() {}
-     }
+  };
 
   /* ************************************************************************* */
-  template<class J>
+  template<typename ValueType>
-  bool Values<J>::equals(const Values<J>& expected, double tol) const {
+  const ValueType& Values::at(const Symbol& j) const {
-    if (values_.size() != expected.values_.size()) return false;
+    // Find the item
-    BOOST_FOREACH(const KeyValuePair& v, values_) {
+    KeyValueMap::const_iterator item = values_.find(j);
-    	if (!expected.exists(v.first)) return false;
+
-      if(!gtsam::equal(v.second, expected[v.first], tol))
+    // Throw exception if it does not exist
-        return false;
+    if(item == values_.end())
-    }
+      throw ValuesKeyDoesNotExist("retrieve", j);
-    return true;
+
+    // Check the type and throw exception if incorrect
+    if(typeid(*item->second) != typeid(ValueType))
+      throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
+
+    // We have already checked the type, so do a "blind" static_cast, not dynamic_cast
+    return static_cast<const ValueType&>(*item->second);
   }
 
   /* ************************************************************************* */
-  template<class J>
+  template<typename TypedKey>
-  const typename J::Value& Values<J>::at(const J& j) const {
+  const typename TypedKey::Value& Values::at(const TypedKey& j) const {
-    const_iterator it = values_.find(j);
+    // Convert to Symbol
-    if (it == values_.end()) throw KeyDoesNotExist<J>("retrieve", j);
+    const Symbol symbol(j.symbol());
-    else return it->second;
+
+    // Call at with the Value type from the key
+    return at<typename TypedKey::Value>(symbol);
   }
 
   /* ************************************************************************* */
-  template<class J>
+  template<typename ValueType>
-  size_t Values<J>::dim() const {
+  boost::optional<const ValueType&> Values::exists(const Symbol& j) const {
-  	size_t n = 0;
+    // Find the item
-  	BOOST_FOREACH(const KeyValuePair& value, values_)
+    KeyValueMap::const_iterator item = values_.find(j);
-  		n += value.second.dim();
-  	return n;
-  }
 
-  /* ************************************************************************* */
+    if(item != values_.end()) {
-  template<class J>
+      // Check the type and throw exception if incorrect
-  VectorValues Values<J>::zero(const Ordering& ordering) const {
+      if(typeid(*item->second) != typeid(ValueType))
-  	return VectorValues::Zero(this->dims(ordering));
+        throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
-  }
 
-  /* ************************************************************************* */
+      // We have already checked the type, so do a "blind" static_cast, not dynamic_cast
-  template<class J>
+      return static_cast<const ValueType&>(*item->second);
-  void Values<J>::insert(const J& name, const typename J::Value& val) {
+    } else {
-    if(!values_.insert(make_pair(name, val)).second)
+      return boost::none;
-      throw KeyAlreadyExists<J>(name, val);
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  void Values<J>::insert(const Values<J>& cfg) {
-	  BOOST_FOREACH(const KeyValuePair& v, cfg.values_)
-		 insert(v.first, v.second);
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  void Values<J>::update(const Values<J>& cfg) {
-	  BOOST_FOREACH(const KeyValuePair& v, values_) {
-	  	boost::optional<typename J::Value> t = cfg.exists_(v.first);
-	  	if (t) values_[v.first] = *t;
-	  }
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  void Values<J>::update(const J& j, const typename J::Value& val) {
-	  	values_[j] = val;
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  std::list<J> Values<J>::keys() const {
-	  std::list<J> ret;
-	  BOOST_FOREACH(const KeyValuePair& v, values_)
-		  ret.push_back(v.first);
-	  return ret;
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  void Values<J>::erase(const J& j) {
-    size_t dim; // unused
-    erase(j, dim);
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  void Values<J>::erase(const J& j, size_t& dim) {
-    iterator it = values_.find(j);
-    if (it == values_.end())
-      throw KeyDoesNotExist<J>("erase", j);
-    dim = it->second.dim();
-    values_.erase(it);
-  }
-
-  /* ************************************************************************* */
-  // todo: insert for every element is inefficient
-  template<class J>
-  Values<J> Values<J>::retract(const VectorValues& delta, const Ordering& ordering) const {
-		Values<J> newValues;
-		typedef pair<J,typename J::Value> KeyValue;
-		BOOST_FOREACH(const KeyValue& value, this->values_) {
-			const J& j = value.first;
-			const typename J::Value& pj = value.second;
-			Index index;
-			if(ordering.tryAt(j,index)) {
-			  newValues.insert(j, pj.retract(delta[index]));
-			} else
-			  newValues.insert(j, pj);
-		}
-		return newValues;
-	}
-
-  /* ************************************************************************* */
-  template<class J>
-  std::vector<size_t> Values<J>::dims(const Ordering& ordering) const {
-    _ValuesDimensionCollector dimCollector(ordering);
-    this->apply(dimCollector);
-    return dimCollector.dimensions;
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  Ordering::shared_ptr Values<J>::orderingArbitrary(Index firstVar) const {
-    // Generate an initial key ordering in iterator order
-    _ValuesKeyOrderer keyOrderer(firstVar);
-    this->apply(keyOrderer);
-    return keyOrderer.ordering;
-  }
-
-//  /* ************************************************************************* */
-//  // todo: insert for every element is inefficient
-//  template<class J>
-//  Values<J> Values<J>::retract(const Vector& delta) const {
-//    if(delta.size() != dim()) {
-//    	cout << "Values::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl;
-//      throw invalid_argument("Delta vector length does not match config dimensionality.");
-//    }
-//    Values<J> newValues;
-//    int delta_offset = 0;
-//		typedef pair<J,typename J::Value> KeyValue;
-//		BOOST_FOREACH(const KeyValue& value, this->values_) {
-//			const J& j = value.first;
-//			const typename J::Value& pj = value.second;
-//      int cur_dim = pj.dim();
-//      newValues.insert(j,pj.retract(sub(delta, delta_offset, delta_offset+cur_dim)));
-//      delta_offset += cur_dim;
-//    }
-//    return newValues;
-//  }
-
-  /* ************************************************************************* */
-  // todo: insert for every element is inefficient
-  // todo: currently only logmaps elements in both configs
-  template<class J>
-  inline VectorValues Values<J>::localCoordinates(const Values<J>& cp, const Ordering& ordering) const {
-  	VectorValues delta(this->dims(ordering));
-  	localCoordinates(cp, ordering, delta);
-  	return delta;
-  }
-
-  /* ************************************************************************* */
-  template<class J>
-  void Values<J>::localCoordinates(const Values<J>& cp, const Ordering& ordering, VectorValues& delta) const {
-    typedef pair<J,typename J::Value> KeyValue;
-    BOOST_FOREACH(const KeyValue& value, cp) {
-      assert(this->exists(value.first));
-      delta[ordering[value.first]] = this->at(value.first).localCoordinates(value.second);
     }
   }
 
   /* ************************************************************************* */
-  template<class J>
+  template<class TypedKey>
-  const char* KeyDoesNotExist<J>::what() const throw() {
+  boost::optional<const typename TypedKey::Value&> Values::exists(const TypedKey& j) const {
-    if(message_.empty())
+    // Convert to Symbol
-      message_ = 
+    const Symbol symbol(j.symbol());
-          "Attempting to " + std::string(operation_) + " the key \"" +
-          (std::string)key_ + "\", which does not exist in the Values.";
-    return message_.c_str();
-  }
 
-  /* ************************************************************************* */
+    // Call exists with the Value type from the key
-  template<class J>
+    return exists<typename TypedKey::Value>(symbol);
-  const char* KeyAlreadyExists<J>::what() const throw() {
-    if(message_.empty())
-      message_ = 
-          "Attempting to add a key-value pair with key \"" + (std::string)key_ + "\", key already exists.";
-    return message_.c_str();
   }
 
 }
-
-

gtsam/nonlinear/Values.cpp

@@ -0,0 +1,213 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Values.h
+ * @author Richard Roberts
+ *
+ * @brief A non-templated config holding any types of Manifold-group elements
+ *
+ *  Detailed story:
+ *  A values structure is a map from keys to values. It is used to specify the value of a bunch
+ *  of variables in a factor graph. A Values is a values structure which can hold variables that
+ *  are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
+ *  which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
+ */
+
+#include <gtsam/nonlinear/Values.h>
+
+#include <list>
+
+#include <boost/foreach.hpp>
+#include <boost/bind.hpp>
+#include <boost/iterator/transform_iterator.hpp>
+
+using namespace std;
+
+namespace gtsam {
+
+  /* ************************************************************************* */
+  Values::Values(const Values& other) {
+    this->insert(other);
+  }
+
+  /* ************************************************************************* */
+  void Values::print(const string& str) const {
+    cout << str << "Values with " << size() << " values:\n" << endl;
+    for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
+      cout << "  " << (string)key_value->first << ": ";
+      key_value->second.print("");
+    }
+  }
+
+  /* ************************************************************************* */
+  bool Values::equals(const Values& other, double tol) const {
+    if(this->size() != other.size())
+      return false;
+    for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) {
+      if(typeid(it1->second) != typeid(it2->second))
+        return false;
+      if(it1->first != it2->first)
+        return false;
+      if(!it1->second.equals_(it2->second, tol))
+        return false;
+    }
+    return true; // We return false earlier if we find anything that does not match
+  }
+
+  /* ************************************************************************* */
+  bool Values::exists(const Symbol& j) const {
+    return values_.find(j) != values_.end();
+  }
+
+  /* ************************************************************************* */
+  VectorValues Values::zeroVectors(const Ordering& ordering) const {
+    return VectorValues::Zero(this->dims(ordering));
+  }
+
+  /* ************************************************************************* */
+  Values Values::retract(const VectorValues& delta, const Ordering& ordering) const {
+    Values result;
+
+    for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
+      const SubVector& singleDelta = delta[ordering[key_value->first]]; // Delta for this value
+      Symbol key = key_value->first;  // Non-const duplicate to deal with non-const insert argument
+      Value* retractedValue(key_value->second.retract_(singleDelta)); // Retract
+      result.values_.insert(key, retractedValue); // Add retracted result directly to result values
+    }
+
+    return result;
+  }
+
+  /* ************************************************************************* */
+  VectorValues Values::localCoordinates(const Values& cp, const Ordering& ordering) const {
+    VectorValues result(this->dims(ordering));
+    localCoordinates(cp, ordering, result);
+    return result;
+  }
+
+  /* ************************************************************************* */
+  void Values::localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& result) const {
+    if(this->size() != cp.size())
+      throw DynamicValuesMismatched();
+    for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) {
+      if(it1->first != it2->first)
+        throw DynamicValuesMismatched(); // If keys do not match
+      // Will throw a dynamic_cast exception if types do not match
+      result.insert(ordering[it1->first], it1->second.localCoordinates_(it2->second));
+    }
+  }
+
+  /* ************************************************************************* */
+  void Values::insert(const Symbol& j, const Value& val) {
+  	Symbol key = j; // Non-const duplicate to deal with non-const insert argument
+  	std::pair<iterator,bool> insertResult = values_.insert(key, val.clone_());
+  	if(!insertResult.second)
+  		throw ValuesKeyAlreadyExists(j);
+  }
+
+  /* ************************************************************************* */
+  void Values::insert(const Values& values) {
+    for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
+      Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
+      insert(key, key_value->second);
+    }
+  }
+
+  /* ************************************************************************* */
+  void Values::update(const Symbol& j, const Value& val) {
+  	// Find the value to update
+  	KeyValueMap::iterator item = values_.find(j);
+  	if(item == values_.end())
+  		throw ValuesKeyDoesNotExist("update", j);
+
+  	// Cast to the derived type
+  	if(typeid(*item->second) != typeid(val))
+  		throw ValuesIncorrectType(j, typeid(*item->second), typeid(val));
+
+  	values_.replace(item, val.clone_());
+  }
+
+  /* ************************************************************************* */
+  void Values::update(const Values& values) {
+    for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
+      this->update(key_value->first, key_value->second);
+    }
+  }
+
+  /* ************************************************************************* */
+  void Values::erase(const Symbol& j) {
+    KeyValueMap::iterator item = values_.find(j);
+    if(item == values_.end())
+      throw ValuesKeyDoesNotExist("erase", j);
+    values_.erase(item);
+  }
+
+  /* ************************************************************************* */
+  FastList<Symbol> Values::keys() const {
+    FastList<Symbol> result;
+    for(const_iterator key_value = begin(); key_value != end(); ++key_value)
+      result.push_back(key_value->first);
+    return result;
+  }
+
+  /* ************************************************************************* */
+  Values& Values::operator=(const Values& rhs) {
+    this->clear();
+    this->insert(rhs);
+    return *this;
+  }
+
+  /* ************************************************************************* */
+  vector<size_t> Values::dims(const Ordering& ordering) const {
+    vector<size_t> result(values_.size());
+    BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) {
+      result[ordering[key_value.first]] = key_value.second.dim();
+    }
+    return result;
+  }
+
+  /* ************************************************************************* */
+  Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const {
+    Ordering::shared_ptr ordering(new Ordering);
+    for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
+      ordering->insert(key_value->first, firstVar++);
+    }
+    return ordering;
+  }
+
+  /* ************************************************************************* */
+  const char* ValuesKeyAlreadyExists::what() const throw() {
+    if(message_.empty())
+      message_ =
+          "Attempting to add a key-value pair with key \"" + (std::string)key_ + "\", key already exists.";
+    return message_.c_str();
+  }
+
+  /* ************************************************************************* */
+  const char* ValuesKeyDoesNotExist::what() const throw() {
+    if(message_.empty())
+      message_ =
+          "Attempting to " + std::string(operation_) + " the key \"" +
+          (std::string)key_ + "\", which does not exist in the Values.";
+    return message_.c_str();
+  }
+
+  /* ************************************************************************* */
+  const char* ValuesIncorrectType::what() const throw() {
+    if(message_.empty())
+      message_ =
+          "Attempting to retrieve value with key \"" + (std::string)key_ + "\", type stored in Values is " +
+          std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name());
+    return message_.c_str();
+  }
+
+}

gtsam/nonlinear/Values.h

@@ -13,115 +13,159 @@
  * @file Values.h
  * @author Richard Roberts
  *
- * @brief A templated config for Manifold-group elements
+ * @brief A non-templated config holding any types of Manifold-group elements
  *
  *  Detailed story:
  *  A values structure is a map from keys to values. It is used to specify the value of a bunch
  *  of variables in a factor graph. A Values is a values structure which can hold variables that
  *  are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
  *  which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
- *  \nosubgrouping
  */
 
 #pragma once
 
-#include <set>
+#include <string>
+#include <utility>
 
-#include <gtsam/base/Testable.h>
+#include <boost/pool/pool_alloc.hpp>
+#include <boost/ptr_container/ptr_map.hpp>
+#include <boost/iterator/transform_iterator.hpp>
+#include <boost/function.hpp>
+#include <boost/bind.hpp>
+
+#include <gtsam/base/Value.h>
 #include <gtsam/base/FastMap.h>
-#include <gtsam/base/Vector.h>
+#include <gtsam/linear/VectorValues.h>
-#include <gtsam/base/Manifold.h>
+#include <gtsam/nonlinear/Key.h>
 #include <gtsam/nonlinear/Ordering.h>
 
-namespace boost { template<class T> class optional; }
-namespace gtsam { class VectorValues; class Ordering; }
-
 namespace gtsam {
 
   // Forward declarations
-  template<class J> class KeyDoesNotExist;
+  class ValueCloneAllocator;
-  template<class J> class KeyAlreadyExists;
 
-	/**
+/**
-	 * Manifold type values structure
+  * A non-templated config holding any types of Manifold-group elements.  A
-	 * Takes two template types
+  * values structure is a map from keys to values. It is used to specify the
-	 *  J: a key type to look up values in the values structure (need to be sortable)
+  * value of a bunch of variables in a factor graph. A Values is a values
-	 *
+  * structure which can hold variables that are elements on manifolds, not just
-	 * Key concept:
+  * vectors. It then, as a whole, implements a aggregate type which is also a
-	 *  The key will be assumed to be sortable, and must have a
+  * manifold element, and hence supports operations dim, retract, and
-	 *  typedef called "Value" with the type of the value the key
+  * localCoordinates.
-	 *  labels (example: Pose2, Point2, etc)
+  */
-	 */
-  template<class J>
   class Values {
 
-  public:
-
-    /**
-     * Typedefs
-     */
-  	typedef J Key;
-  	typedef typename J::Value Value;
-  	typedef std::map<Key, Value, std::less<Key>, boost::fast_pool_allocator<std::pair<const Key, Value> > > KeyValueMap;
-	//    typedef FastMap<J,Value> KeyValueMap;
-    typedef typename KeyValueMap::value_type KeyValuePair;
-    typedef typename KeyValueMap::iterator iterator;
-    typedef typename KeyValueMap::const_iterator const_iterator;
-
   private:
 
-    /** concept check */
+    // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined
-    GTSAM_CONCEPT_TESTABLE_TYPE(Value)
+    // below) to clone and deallocate the Value objects, and a boost
-    GTSAM_CONCEPT_MANIFOLD_TYPE(Value)
+    // fast_pool_allocator to allocate map nodes.  In this way, all memory is
+    // allocated in a boost memory pool.
+    typedef boost::ptr_map<
+        Symbol,
+        Value,
+        std::less<Symbol>,
+        ValueCloneAllocator,
+        boost::fast_pool_allocator<std::pair<const Symbol, void*> > > KeyValueMap;
 
+    // The member to store the values, see just above
     KeyValueMap values_;
 
+    // Types obtained by iterating
+    typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair;
+    typedef KeyValueMap::iterator::value_type KeyValuePtrPair;
+
   public:
 
-  	/// @name Standard Constructors
+    /// A shared_ptr to this class
-  	/// @{
+    typedef boost::shared_ptr<Values> shared_ptr;
 
-    ///TODO comment
+    /// A pair of const references to the key and value, the dereferenced type of the const_iterator and const_reverse_iterator
+    typedef std::pair<const Symbol&, const Value&> ConstKeyValuePair;
+
+    /// A pair of references to the key and value, the dereferenced type of the iterator and reverse_iterator
+    typedef std::pair<const Symbol&, Value&> KeyValuePair;
+
+    /// Mutable forward iterator, with value type KeyValuePair
+    typedef boost::transform_iterator<
+        boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::iterator> iterator;
+
+    /// Const forward iterator, with value type ConstKeyValuePair
+    typedef boost::transform_iterator<
+        boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_iterator> const_iterator;
+
+    /// Mutable reverse iterator, with value type KeyValuePair
+    typedef boost::transform_iterator<
+        boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::reverse_iterator> reverse_iterator;
+
+    /// Const reverse iterator, with value type ConstKeyValuePair
+    typedef boost::transform_iterator<
+        boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
+
+    /** Default constructor creates an empty Values class */
     Values() {}
 
-    ///TODO: comment
+    /** Copy constructor duplicates all keys and values */
-    Values(const Values& config) :
+    Values(const Values& other);
-      values_(config.values_) {}
 
-    ///TODO: comment
-    template<class J_ALT>
-    Values(const Values<J_ALT>& other) {} // do nothing when initializing with wrong type
-    virtual ~Values() {}
-
-  	/// @}
     /// @name Testable
     /// @{
 
-    /** print */
+    /** print method for testing and debugging */
-    void print(const std::string &s="") const;
+    void print(const std::string& str = "") const;
 
-    /** Test whether configs are identical in keys and values */
+    /** Test whether the sets of keys and values are identical */
-    bool equals(const Values& expected, double tol=1e-9) const;
+    bool equals(const Values& other, double tol=1e-9) const;
 
     /// @}
-  	/// @name Standard Interface
-  	/// @{
 
-    /** Retrieve a variable by j, throws KeyDoesNotExist<J> if not found */
+    /** Retrieve a variable by key \c j.  The type of the value associated with
-    const Value& at(const J& j) const;
+     * this key is supplied as a template argument to this function.
+     * @param j Retrieve the value associated with this key
+     * @tparam Value The type of the value stored with this key, this method
+     * throws DynamicValuesIncorrectType if this requested type is not correct.
+     * @return A const reference to the stored value
+     */
+    template<typename ValueType>
+    const ValueType& at(const Symbol& j) const;
 
-    /** operator[] syntax for get, throws KeyDoesNotExist<J> if not found */
+    /** Retrieve a variable using a special key (typically TypedSymbol), which
-    const Value& operator[](const J& j) const {
+     * contains the type of the value associated with the key, and which must
+     * be conversion constructible to a Symbol, e.g.
+     * <tt>Symbol(const TypedKey&)</tt>.  Throws DynamicValuesKeyDoesNotExist
+     * the key is not found, and DynamicValuesIncorrectType if the value type
+     * associated with the requested key does not match the stored value type.
+     */
+    template<class TypedKey>
+    const typename TypedKey::Value& at(const TypedKey& j) const;
+
+    /** operator[] syntax for at(const TypedKey& j) */
+    template<class TypedKey>
+    const typename TypedKey::Value& operator[](const TypedKey& j) const {
       return at(j); }
 
-    /** Check if a variable exists */
+    /** Check if a value exists with key \c j.  See exists<>(const Symbol& j)
-    bool exists(const J& i) const { return values_.find(i)!=values_.end(); }
+     * and exists(const TypedKey& j) for versions that return the value if it
+     * exists. */
+    bool exists(const Symbol& j) const;
 
-    /** Check if a variable exists and return it if so */
+    /** Check if a value with key \c j exists, returns the value with type
-    boost::optional<Value> exists_(const J& i) const {
+     * \c Value if the key does exist, or boost::none if it does not exist.
-    	const_iterator it = values_.find(i);
+     * Throws DynamicValuesIncorrectType if the value type associated with the
-    	if (it==values_.end()) return boost::none; else	return it->second;
+     * requested key does not match the stored value type. */
-    }
+    template<typename ValueType>
+    boost::optional<const ValueType&> exists(const Symbol& j) const;
+
+    /** Check if a value with key \c j exists, returns the value with type
+     * \c Value if the key does exist, or boost::none if it does not exist.
+     * Uses a special key (typically TypedSymbol), which contains the type of
+     * the value associated with the key, and which must be conversion
+     * constructible to a Symbol, e.g. <tt>Symbol(const TypedKey&)</tt>. Throws
+     * DynamicValuesIncorrectType if the value type associated with the
+     * requested key does not match the stored value type.
+     */
+    template<class TypedKey>
+    boost::optional<const typename TypedKey::Value&> exists(const TypedKey& j) const;
 
     /** The number of variables in this config */
     size_t size() const { return values_.size(); }
@@ -130,10 +174,64 @@ namespace gtsam {
     bool empty() const { return values_.empty(); }
 
     /** Get a zero VectorValues of the correct structure */
-    VectorValues zero(const Ordering& ordering) const;
+    VectorValues zeroVectors(const Ordering& ordering) const;
 
-    const_iterator begin() const { return values_.begin(); }	///<TODO: comment
+  private:
-    const_iterator end() const { return values_.end(); }			///<TODO: comment
+    static std::pair<const Symbol&, const Value&> make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) {
+      return std::make_pair<const Symbol&, const Value&>(key_value.first, *key_value.second); }
+    static std::pair<const Symbol&, Value&> make_deref_pair(const KeyValueMap::iterator::value_type& key_value) {
+      return std::make_pair<const Symbol&, Value&>(key_value.first, *key_value.second); }
+
+  public:
+    const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); }
+    const_iterator end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); }
+    iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); }
+    iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); }
+    const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); }
+    const_reverse_iterator rend() const { return boost::make_transform_iterator(values_.rend(), &make_const_deref_pair); }
+    reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); }
+    reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); }
+
+    /// @name Manifold Operations
+    /// @{
+
+    /** Add a delta config to current config and returns a new config */
+    Values retract(const VectorValues& delta, const Ordering& ordering) const;
+
+    /** Get a delta config about a linearization point c0 (*this) */
+    VectorValues localCoordinates(const Values& cp, const Ordering& ordering) const;
+
+    /** Get a delta config about a linearization point c0 (*this) */
+    void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const;
+
+    ///@}
+
+    /** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
+    void insert(const Symbol& j, const Value& val);
+
+    /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
+    void insert(const Values& values);
+
+    /** single element change of existing element */
+    void update(const Symbol& j, const Value& val);
+
+    /** update the current available values without adding new ones */
+    void update(const Values& values);
+
+    /** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
+    void erase(const Symbol& j);
+
+    /**
+     * Returns a set of keys in the config
+     * Note: by construction, the list is ordered
+     */
+    FastList<Symbol> keys() const;
+
+    /** Replace all keys and variables */
+    Values& operator=(const Values& rhs);
+
+    /** Remove all variables from the config */
+    void clear() { values_.clear(); }
 
     /** Create an array of variable dimensions using the given ordering */
     std::vector<size_t> dims(const Ordering& ordering) const;
@@ -146,174 +244,97 @@ namespace gtsam {
      */
     Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const;
 
-    /// @}
-    /// @name Manifold Operations
-    /// @{
-
-    /** The dimensionality of the tangent space */
-    size_t dim() const;
-
-    /** Add a delta config to current config and returns a new config */
-    Values retract(const VectorValues& delta, const Ordering& ordering) const;
-
-    /** Get a delta config about a linearization point c0 (*this) */
-    VectorValues localCoordinates(const Values& cp, const Ordering& ordering) const;
-
-    /** Get a delta config about a linearization point c0 (*this) */
-    void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const;
-
-    /// @}
-  	/// @name Advanced Interface
-  	/// @{
-
-    // imperative methods:
-
-    iterator begin() { return values_.begin(); }	///<TODO: comment
-    iterator end() { return values_.end(); }			///<TODO: comment
-
-    /** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
-    void insert(const J& j, const Value& val);
-
-    /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
-    void insert(const Values& cfg);
-
-    /** update the current available values without adding new ones */
-    void update(const Values& cfg);
-
-    /** single element change of existing element */
-    void update(const J& j, const Value& val);
-
-    /** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
-    void erase(const J& j);
-
-    /** Remove a variable from the config while returning the dimensionality of
-     * the removed element (normally not needed by user code), throws
-     * KeyDoesNotExist<J> if j is not present.
-     */
-    void erase(const J& j, size_t& dim);
-
-    /**
-     * Returns a set of keys in the config
-     * Note: by construction, the list is ordered
-     */
-    std::list<J> keys() const;
-
-    /** Replace all keys and variables */
-    Values& operator=(const Values& rhs) {
-      values_ = rhs.values_;
-      return (*this);
-    }
-
-    /** Remove all variables from the config */
-    void clear() {
-      values_.clear();
-    }
-
-    /**
-     * Apply a class with an application operator() to a const_iterator over
-     * every <key,value> pair.  The operator must be able to handle such an
-     * iterator for every type in the Values, (i.e. through templating).
-     */
-    template<typename A>
-    void apply(A& operation) {
-      for(iterator it = begin(); it != end(); ++it)
-        operation(it);
-    }
-    template<typename A>
-    void apply(A& operation) const {
-      for(const_iterator it = begin(); it != end(); ++it)
-        operation(it);
-    }
-
-    /// @}
-
-  private:
-  	/** Serialization function */
-  	friend class boost::serialization::access;
-  	template<class ARCHIVE>
-  	void serialize(ARCHIVE & ar, const unsigned int version) {
-  		ar & BOOST_SERIALIZATION_NVP(values_);
-  	}
-
-  };
-
-  struct _ValuesDimensionCollector {
-    const Ordering& ordering;
-    std::vector<size_t> dimensions;
-    _ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {}
-    template<typename I> void operator()(const I& key_value) {
-      Index var;
-      if(ordering.tryAt(key_value->first, var)) {
-        assert(var < dimensions.size());
-        dimensions[var] = key_value->second.dim();
-      }
-    }
   };
 
   /* ************************************************************************* */
-  struct _ValuesKeyOrderer {
+  class ValuesKeyAlreadyExists : public std::exception {
-    Index var;
+  protected:
-    Ordering::shared_ptr ordering;
+    const Symbol key_; ///< The key that already existed
-    _ValuesKeyOrderer(Index firstVar) : var(firstVar), ordering(new Ordering) {}
+
-    template<typename I> void operator()(const I& key_value) {
+  private:
-      ordering->insert(key_value->first, var);
+    mutable std::string message_;
-      ++ var;
+
+  public:
+    /// Construct with the key-value pair attemped to be added
+    ValuesKeyAlreadyExists(const Symbol& key) throw() :
+      key_(key) {}
+
+    virtual ~ValuesKeyAlreadyExists() throw() {}
+
+    /// The duplicate key that was attemped to be added
+    const Symbol& key() const throw() { return key_; }
+
+    /// The message to be displayed to the user
+    virtual const char* what() const throw();
+  };
+
+  /* ************************************************************************* */
+  class ValuesKeyDoesNotExist : public std::exception {
+  protected:
+    const char* operation_; ///< The operation that attempted to access the key
+    const Symbol key_; ///< The key that does not exist
+
+  private:
+    mutable std::string message_;
+
+  public:
+    /// Construct with the key that does not exist in the values
+    ValuesKeyDoesNotExist(const char* operation, const Symbol& key) throw() :
+      operation_(operation), key_(key) {}
+
+    virtual ~ValuesKeyDoesNotExist() throw() {}
+
+    /// The key that was attempted to be accessed that does not exist
+    const Symbol& key() const throw() { return key_; }
+
+    /// The message to be displayed to the user
+    virtual const char* what() const throw();
+  };
+
+  /* ************************************************************************* */
+  class ValuesIncorrectType : public std::exception {
+  protected:
+    const Symbol key_; ///< The key requested
+    const std::type_info& storedTypeId_;
+    const std::type_info& requestedTypeId_;
+
+  private:
+    mutable std::string message_;
+
+  public:
+    /// Construct with the key that does not exist in the values
+    ValuesIncorrectType(const Symbol& key,
+        const std::type_info& storedTypeId, const std::type_info& requestedTypeId) throw() :
+      key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
+
+    virtual ~ValuesIncorrectType() throw() {}
+
+    /// The key that was attempted to be accessed that does not exist
+    const Symbol& key() const throw() { return key_; }
+
+    /// The typeid of the value stores in the Values
+    const std::type_info& storedTypeId() const { return storedTypeId_; }
+
+    /// The requested typeid
+    const std::type_info& requestedTypeId() const { return requestedTypeId_; }
+
+    /// The message to be displayed to the user
+    virtual const char* what() const throw();
+  };
+
+  /* ************************************************************************* */
+  class DynamicValuesMismatched : public std::exception {
+
+  public:
+    DynamicValuesMismatched() throw() {}
+
+    virtual ~DynamicValuesMismatched() throw() {}
+
+    virtual const char* what() const throw() {
+      return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values";
     }
   };
 
-
+}
-/* ************************************************************************* */
-template<class J>
-class KeyAlreadyExists : public std::exception {
-protected:
-  const J key_; ///< The key that already existed
-  const typename J::Value value_; ///< The value attempted to be inserted
-
-private:
-  mutable std::string message_;
-
-public:
-  /// Construct with the key-value pair attemped to be added
-  KeyAlreadyExists(const J& key, const typename J::Value& value) throw() :
-    key_(key), value_(value) {}
-
-  virtual ~KeyAlreadyExists() throw() {}
-
-  /// The duplicate key that was attemped to be added
-  const J& key() const throw() { return key_; }
-
-  /// The value that was attempted to be added
-  const typename J::Value& value() const throw() { return value_; }
-
-  /// The message to be displayed to the user
-  virtual const char* what() const throw();
-};
-
-/* ************************************************************************* */
-template<class J>
-class KeyDoesNotExist : public std::exception {
-protected:
-  const char* operation_; ///< The operation that attempted to access the key
-  const J key_; ///< The key that does not exist
-
-private:
-  mutable std::string message_;
-
-public:
-  /// Construct with the key that does not exist in the values
-  KeyDoesNotExist(const char* operation, const J& key) throw() :
-    operation_(operation), key_(key) {}
-
-  virtual ~KeyDoesNotExist() throw() {}
-
-  /// The key that was attempted to be accessed that does not exist
-  const J& key() const throw() { return key_; }
-
-  /// The message to be displayed to the user
-  virtual const char* what() const throw();
-};
-
-} // \namespace gtsam
 
 #include <gtsam/nonlinear/Values-inl.h>
-

gtsam/nonlinear/tests/testValues.cpp

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file testValues.cpp
+ * @file testDynamicValues.cpp
  * @author Richard Roberts
  */
 
@@ -22,6 +22,7 @@ using namespace boost::assign;
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/LieVector.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/Values.h>
 
 using namespace gtsam;
@@ -29,27 +30,26 @@ using namespace std;
 static double inf = std::numeric_limits<double>::infinity();
 
 typedef TypedSymbol<LieVector, 'v'> VecKey;
-typedef Values<VecKey> TestValues;
 
 VecKey key1(1), key2(2), key3(3), key4(4);
 
 /* ************************************************************************* */
-TEST( TestValues, equals1 )
+TEST( Values, equals1 )
 {
-  TestValues expected;
+  Values expected;
-  Vector v = Vector_(3, 5.0, 6.0, 7.0);
+  LieVector v(3, 5.0, 6.0, 7.0);
   expected.insert(key1,v);
-  TestValues actual;
+  Values actual;
   actual.insert(key1,v);
   CHECK(assert_equal(expected,actual));
 }
 
 /* ************************************************************************* */
-TEST( TestValues, equals2 )
+TEST( Values, equals2 )
 {
-  TestValues cfg1, cfg2;
+  Values cfg1, cfg2;
-  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
+  LieVector v1(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 5.0, 6.0, 8.0);
+  LieVector v2(3, 5.0, 6.0, 8.0);
   cfg1.insert(key1, v1);
   cfg2.insert(key1, v2);
   CHECK(!cfg1.equals(cfg2));
@@ -57,11 +57,11 @@ TEST( TestValues, equals2 )
 }
 
 /* ************************************************************************* */
-TEST( TestValues, equals_nan )
+TEST( Values, equals_nan )
 {
-  TestValues cfg1, cfg2;
+  Values cfg1, cfg2;
-  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
+  LieVector v1(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, inf, inf, inf);
+  LieVector v2(3, inf, inf, inf);
   cfg1.insert(key1, v1);
   cfg2.insert(key1, v2);
   CHECK(!cfg1.equals(cfg2));
@@ -69,13 +69,13 @@ TEST( TestValues, equals_nan )
 }
 
 /* ************************************************************************* */
-TEST( TestValues, insert_good )
+TEST( Values, insert_good )
 {
-  TestValues cfg1, cfg2, expected;
+  Values cfg1, cfg2, expected;
-  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
+  LieVector v1(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
+  LieVector v2(3, 8.0, 9.0, 1.0);
-  Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
+  LieVector v3(3, 2.0, 4.0, 3.0);
-  Vector v4 = Vector_(3, 8.0, 3.0, 7.0);
+  LieVector v4(3, 8.0, 3.0, 7.0);
   cfg1.insert(key1, v1);
   cfg1.insert(key2, v2);
   cfg2.insert(key3, v4);
@@ -86,31 +86,31 @@ TEST( TestValues, insert_good )
   expected.insert(key2, v2);
   expected.insert(key3, v4);
 
-  CHECK(assert_equal(cfg1, expected));
+  CHECK(assert_equal(expected, cfg1));
 }
 
 /* ************************************************************************* */
-TEST( TestValues, insert_bad )
+TEST( Values, insert_bad )
 {
-  TestValues cfg1, cfg2;
+  Values cfg1, cfg2;
-  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
+  LieVector v1(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
+  LieVector v2(3, 8.0, 9.0, 1.0);
-  Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
+  LieVector v3(3, 2.0, 4.0, 3.0);
-  Vector v4 = Vector_(3, 8.0, 3.0, 7.0);
+  LieVector v4(3, 8.0, 3.0, 7.0);
   cfg1.insert(key1, v1);
   cfg1.insert(key2, v2);
   cfg2.insert(key2, v3);
   cfg2.insert(key3, v4);
 
-  CHECK_EXCEPTION(cfg1.insert(cfg2), const KeyAlreadyExists<VecKey>);
+  CHECK_EXCEPTION(cfg1.insert(cfg2), ValuesKeyAlreadyExists);
 }
 
 /* ************************************************************************* */
-TEST( TestValues, update_element )
+TEST( Values, update_element )
 {
-  TestValues cfg;
+  Values cfg;
-  Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
+  LieVector v1(3, 5.0, 6.0, 7.0);
-  Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
+  LieVector v2(3, 8.0, 9.0, 1.0);
 
   cfg.insert(key1, v1);
   CHECK(cfg.size() == 1);
@@ -122,12 +122,12 @@ TEST( TestValues, update_element )
 }
 
 ///* ************************************************************************* */
-//TEST(TestValues, dim_zero)
+//TEST(Values, dim_zero)
 //{
-//  TestValues config0;
+//  Values config0;
-//  config0.insert(key1, Vector_(2, 2.0, 3.0));
+//  config0.insert(key1, LieVector(2, 2.0, 3.0));
-//  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+//  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
-//  LONGS_EQUAL(5,config0.dim());
+//  LONGS_EQUAL(5, config0.dim());
 //
 //  VectorValues expected;
 //  expected.insert(key1, zero(2));
@@ -136,151 +136,130 @@ TEST( TestValues, update_element )
 //}
 
 /* ************************************************************************* */
-TEST(TestValues, expmap_a)
+TEST(Values, expmap_a)
 {
-  TestValues config0;
+  Values config0;
-  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
+  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
-  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
 
   Ordering ordering(*config0.orderingArbitrary());
   VectorValues increment(config0.dims(ordering));
   increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2);
   increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
 
-  TestValues expected;
+  Values expected;
-  expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
+  expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
-  expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
+  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
 
   CHECK(assert_equal(expected, config0.retract(increment, ordering)));
 }
 
-///* ************************************************************************* */
+/* ************************************************************************* */
-//TEST(TestValues, expmap_b)
+TEST(Values, expmap_b)
-//{
+{
-//  TestValues config0;
+  Values config0;
-//  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
+  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
-//  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
-//
-//  Ordering ordering(*config0.orderingArbitrary());
-//  VectorValues increment(config0.dims(ordering));
-//  increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
-//
-//  TestValues expected;
-//  expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
-//  expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
-//
-//  CHECK(assert_equal(expected, config0.retract(increment, ordering)));
-//}
 
-///* ************************************************************************* */
+  Ordering ordering(*config0.orderingArbitrary());
-//TEST(TestValues, expmap_c)
+  VectorValues increment(VectorValues::Zero(config0.dims(ordering)));
+  increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5);
+
+  Values expected;
+  expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
+  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
+
+  CHECK(assert_equal(expected, config0.retract(increment, ordering)));
+}
+
+/* ************************************************************************* */
+//TEST(Values, expmap_c)
 //{
-//  TestValues config0;
+//  Values config0;
-//  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
+//  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
-//  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+//  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
 //
-//  Vector increment = Vector_(6,
+//  Vector increment = LieVector(6,
 //      1.0, 1.1, 1.2,
 //      1.3, 1.4, 1.5);
 //
-//  TestValues expected;
+//  Values expected;
-//  expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
+//  expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
-//  expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
+//  expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
 //
 //  CHECK(assert_equal(expected, config0.retract(increment)));
 //}
 
 /* ************************************************************************* */
-/*TEST(TestValues, expmap_d)
+TEST(Values, expmap_d)
 {
-  TestValues config0;
+  Values config0;
-  config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
+  config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
-  config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
+  config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
   //config0.print("config0");
   CHECK(equal(config0, config0));
   CHECK(config0.equals(config0));
 
-  TestValues<string,Pose2> poseconfig;
+  typedef TypedSymbol<Pose2, 'p'> PoseKey;
-  poseconfig.insert("p1", Pose2(1,2,3));
+  Values poseconfig;
-  poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
+  poseconfig.insert(PoseKey(1), Pose2(1,2,3));
-  //poseconfig.print("poseconfig");
+  poseconfig.insert(PoseKey(2), Pose2(0.3, 0.4, 0.5));
+
   CHECK(equal(config0, config0));
   CHECK(config0.equals(config0));
-}*/
+}
 
 /* ************************************************************************* */
-/*TEST(TestValues, extract_keys)
+TEST(Values, extract_keys)
 {
 	typedef TypedSymbol<Pose2, 'x'> PoseKey;
-	TestValues<PoseKey, Pose2> config;
+	Values config;
 
 	config.insert(PoseKey(1), Pose2());
 	config.insert(PoseKey(2), Pose2());
 	config.insert(PoseKey(4), Pose2());
 	config.insert(PoseKey(5), Pose2());
 
-	list<PoseKey> expected, actual;
+	FastList<Symbol> expected, actual;
 	expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5);
 	actual = config.keys();
 
 	CHECK(actual.size() == expected.size());
-	list<PoseKey>::const_iterator itAct = actual.begin(), itExp = expected.begin();
+	FastList<Symbol>::const_iterator itAct = actual.begin(), itExp = expected.begin();
 	for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
 		CHECK(assert_equal(*itExp, *itAct));
 	}
-}*/
+}
 
 /* ************************************************************************* */
-TEST(TestValues, exists_)
+TEST(Values, exists_)
 {
-	TestValues config0;
+	Values config0;
-	config0.insert(key1, Vector_(1, 1.));
+	config0.insert(key1, LieVector(Vector_(1, 1.)));
-	config0.insert(key2, Vector_(1, 2.));
+	config0.insert(key2, LieVector(Vector_(1, 2.)));
 
-	boost::optional<LieVector> v = config0.exists_(key1);
+	boost::optional<const LieVector&> v = config0.exists(key1);
 	CHECK(assert_equal(Vector_(1, 1.),*v));
 }
 
 /* ************************************************************************* */
-TEST(TestValues, update)
+TEST(Values, update)
 {
-	TestValues config0;
+	Values config0;
-	config0.insert(key1, Vector_(1, 1.));
+	config0.insert(key1, LieVector(1, 1.));
-	config0.insert(key2, Vector_(1, 2.));
+	config0.insert(key2, LieVector(1, 2.));
 
-	TestValues superset;
+	Values superset;
-	superset.insert(key1, Vector_(1, -1.));
+	superset.insert(key1, LieVector(1, -1.));
-	superset.insert(key2, Vector_(1, -2.));
+	superset.insert(key2, LieVector(1, -2.));
-	superset.insert(key3, Vector_(1, -3.));
 	config0.update(superset);
 
-	TestValues expected;
+	Values expected;
-	expected.insert(key1, Vector_(1, -1.));
+	expected.insert(key1, LieVector(1, -1.));
-	expected.insert(key2, Vector_(1, -2.));
+	expected.insert(key2, LieVector(1, -2.));
 	CHECK(assert_equal(expected,config0));
 }
 
-/* ************************************************************************* */
-TEST(TestValues, dummy_initialization)
-{
-	typedef TypedSymbol<LieVector, 'z'> Key;
-	typedef Values<Key> Values1;
-
-	Values1 init1;
-	init1.insert(Key(1), Vector_(2, 1.0, 2.0));
-	init1.insert(Key(2), Vector_(2, 4.0, 3.0));
-
-	TestValues init2;
-	init2.insert(key1, Vector_(2, 1.0, 2.0));
-	init2.insert(key2, Vector_(2, 4.0, 3.0));
-
-	Values1 actual1(init2);
-	TestValues actual2(init1);
-
-	EXPECT(actual1.empty());
-	EXPECT(actual2.empty());
-}
-
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/slam/AntiFactor.h

@@ -27,27 +27,26 @@ namespace gtsam {
 	 * linearized Hessian matrices of the original factor, but with the opposite sign. This effectively
 	 * cancels out any affects of the original factor during optimization."
 	 */
-	template<class VALUES>
+	class AntiFactor: public NonlinearFactor {
-	class AntiFactor: public NonlinearFactor<VALUES> {
 
 	private:
 
-		typedef AntiFactor<VALUES> This;
+		typedef AntiFactor This;
-		typedef NonlinearFactor<VALUES> Base;
+		typedef NonlinearFactor Base;
-		typedef typename NonlinearFactor<VALUES>::shared_ptr sharedFactor;
+		typedef NonlinearFactor::shared_ptr sharedFactor;
 
 		sharedFactor factor_;
 
 	public:
 
 		// shorthand for a smart pointer to a factor
-		typedef typename boost::shared_ptr<AntiFactor> shared_ptr;
+		typedef boost::shared_ptr<AntiFactor> shared_ptr;
 
 		/** default constructor - only use for serialization */
 		AntiFactor() {}
 
 		/** constructor - Creates the equivalent AntiFactor from an existing factor */
-		AntiFactor(typename NonlinearFactor<VALUES>::shared_ptr factor) : Base(factor->begin(), factor->end()), factor_(factor) {}
+		AntiFactor(NonlinearFactor::shared_ptr factor) : Base(factor->begin(), factor->end()), factor_(factor) {}
 
 		virtual ~AntiFactor() {}
 
@@ -60,7 +59,7 @@ namespace gtsam {
 		}
 
 		/** equals */
-		virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
+		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
 			const This *e =	dynamic_cast<const This*> (&expected);
 			return e != NULL && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol);
 		}
@@ -72,7 +71,7 @@ namespace gtsam {
 	   * For the AntiFactor, this will have the same magnitude of the original factor,
 	   * but the opposite sign.
 	   */
-	  double error(const VALUES& c) const { return -factor_->error(c); }
+	  double error(const Values& c) const { return -factor_->error(c); }
 
 	  /** get the dimension of the factor (same as the original factor) */
 	  size_t dim() const { return factor_->dim(); }
@@ -81,7 +80,7 @@ namespace gtsam {
 	   * Checks whether this factor should be used based on a set of values.
 	   * The AntiFactor will have the same 'active' profile as the original factor.
 	   */
-	  bool active(const VALUES& c) const { return factor_->active(c); }
+	  bool active(const Values& c) const { return factor_->active(c); }
 
 	  /**
 	   * Linearize to a GaussianFactor. The AntiFactor always returns a Hessian Factor
@@ -90,7 +89,7 @@ namespace gtsam {
 	   * effectively cancels the effect of the original factor on the factor graph.
 	   */
 	  boost::shared_ptr<GaussianFactor>
-	  linearize(const VALUES& c, const Ordering& ordering) const {
+	  linearize(const Values& c, const Ordering& ordering) const {
 
 	    // Generate the linearized factor from the contained nonlinear factor
 	    GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c, ordering);

gtsam/slam/BearingFactor.h

@@ -25,16 +25,16 @@ namespace gtsam {
 	/**
 	 * Binary factor for a bearing measurement
 	 */
-	template<class VALUES, class POSEKEY, class POINTKEY>
+	template<class POSEKEY, class POINTKEY>
-	class BearingFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
+	class BearingFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
 	private:
 
 		typedef typename POSEKEY::Value Pose;
 		typedef typename POSEKEY::Value::Rotation Rot;
 		typedef typename POINTKEY::Value Point;
 
-		typedef BearingFactor<VALUES, POSEKEY, POINTKEY> This;
+		typedef BearingFactor<POSEKEY, POINTKEY> This;
-		typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
+		typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
 
 		Rot z_; /** measurement */
 
@@ -68,7 +68,7 @@ namespace gtsam {
 		}
 
 		/** equals */
-		virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
+		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
 			const This *e =	dynamic_cast<const This*> (&expected);
 			return e != NULL && Base::equals(*e, tol) && this->z_.equals(e->z_, tol);
 		}

gtsam/slam/BearingRangeFactor.h

@@ -27,16 +27,16 @@ namespace gtsam {
 	/**
 	 * Binary factor for a bearing measurement
 	 */
-	template<class VALUES, class POSEKEY, class POINTKEY>
+	template<class POSEKEY, class POINTKEY>
-	class BearingRangeFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
+	class BearingRangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
 	private:
 
 		typedef typename POSEKEY::Value Pose;
 		typedef typename POSEKEY::Value::Rotation Rot;
 		typedef typename POINTKEY::Value Point;
 
-		typedef BearingRangeFactor<VALUES, POSEKEY, POINTKEY> This;
+		typedef BearingRangeFactor<POSEKEY, POINTKEY> This;
-		typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
+		typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
 
 		// the measurement
 		Rot bearing_;
@@ -68,7 +68,7 @@ namespace gtsam {
 	  }
 
 		/** equals */
-		virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
+		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
 			const This *e =	dynamic_cast<const This*> (&expected);
 			return e != NULL && Base::equals(*e, tol) &&
 					fabs(this->range_ - e->range_) < tol &&

gtsam/slam/BetweenFactor.h

@@ -29,13 +29,13 @@ namespace gtsam {
 	 * KEY1::Value is the Lie Group type
 	 * T is the measurement type, by default the same
 	 */
-	template<class VALUES, class KEY1, class T = typename KEY1::Value>
+	template<class KEY1, class T = typename KEY1::Value>
-	class BetweenFactor: public NonlinearFactor2<VALUES, KEY1, KEY1> {
+	class BetweenFactor: public NonlinearFactor2<KEY1, KEY1> {
 
 	private:
 
-		typedef BetweenFactor<VALUES, KEY1, T> This;
+		typedef BetweenFactor<KEY1, T> This;
-		typedef NonlinearFactor2<VALUES, KEY1, KEY1> Base;
+		typedef NonlinearFactor2<KEY1, KEY1> Base;
 
 		T measured_; /** The measurement */
 
@@ -71,7 +71,7 @@ namespace gtsam {
 		}
 
 		/** equals */
-		virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
+		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
 			const This *e =	dynamic_cast<const This*> (&expected);
 			return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
 		}
@@ -114,15 +114,15 @@ namespace gtsam {
 	 * This constraint requires the underlying type to a Lie type
 	 *
 	 */
-	template<class VALUES, class KEY>
+	template<class KEY>
-	class BetweenConstraint : public BetweenFactor<VALUES, KEY> {
+	class BetweenConstraint : public BetweenFactor<KEY> {
 	public:
-		typedef boost::shared_ptr<BetweenConstraint<VALUES, KEY> > shared_ptr;
+		typedef boost::shared_ptr<BetweenConstraint<KEY> > shared_ptr;
 
 		/** Syntactic sugar for constrained version */
 		BetweenConstraint(const typename KEY::Value& measured,
 				const KEY& key1, const KEY& key2, double mu = 1000.0)
-			: BetweenFactor<VALUES, KEY>(key1, key2, measured,
+			: BetweenFactor<KEY>(key1, key2, measured,
 					noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {}
 
 	private:
@@ -132,7 +132,7 @@ namespace gtsam {
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
 			ar & boost::serialization::make_nvp("BetweenFactor",
-					boost::serialization::base_object<BetweenFactor<VALUES, KEY> >(*this));
+					boost::serialization::base_object<BetweenFactor<KEY> >(*this));
 		}
 	}; // \class BetweenConstraint
 

gtsam/slam/BoundingConstraint.h

@@ -28,11 +28,11 @@ namespace gtsam {
  * will need to have its value function implemented to return
  * a scalar for comparison.
  */
-template<class VALUES, class KEY>
+template<class KEY>
-struct BoundingConstraint1: public NonlinearFactor1<VALUES, KEY> {
+struct BoundingConstraint1: public NonlinearFactor1<KEY> {
 	typedef typename KEY::Value X;
-	typedef NonlinearFactor1<VALUES, KEY> Base;
+	typedef NonlinearFactor1<KEY> Base;
-	typedef boost::shared_ptr<BoundingConstraint1<VALUES, KEY> > shared_ptr;
+	typedef boost::shared_ptr<BoundingConstraint1<KEY> > shared_ptr;
 
 	double threshold_;
 	bool isGreaterThan_; /// flag for greater/less than
@@ -57,7 +57,7 @@ struct BoundingConstraint1: public NonlinearFactor1<VALUES, KEY> {
 			boost::none) const = 0;
 
 	/** active when constraint *NOT* met */
-	bool active(const VALUES& c) const {
+	bool active(const Values& c) const {
 		// note: still active at equality to avoid zigzagging
 		double x = value(c[this->key_]);
 		return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
@@ -95,13 +95,13 @@ private:
  * Binary scalar inequality constraint, with a similar value() function
  * to implement for specific systems
  */
-template<class VALUES, class KEY1, class KEY2>
+template<class KEY1, class KEY2>
-struct BoundingConstraint2: public NonlinearFactor2<VALUES, KEY1, KEY2> {
+struct BoundingConstraint2: public NonlinearFactor2<KEY1, KEY2> {
 	typedef typename KEY1::Value X1;
 	typedef typename KEY2::Value X2;
 
-	typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base;
+	typedef NonlinearFactor2<KEY1, KEY2> Base;
-	typedef boost::shared_ptr<BoundingConstraint2<VALUES, KEY1, KEY2> > shared_ptr;
+	typedef boost::shared_ptr<BoundingConstraint2<KEY1, KEY2> > shared_ptr;
 
 	double threshold_;
 	bool isGreaterThan_; /// flag for greater/less than
@@ -125,7 +125,7 @@ struct BoundingConstraint2: public NonlinearFactor2<VALUES, KEY1, KEY2> {
 			boost::optional<Matrix&> H2 = boost::none) const = 0;
 
 	/** active when constraint *NOT* met */
-	bool active(const VALUES& c) const {
+	bool active(const Values& c) const {
 		// note: still active at equality to avoid zigzagging
 		double x = value(c[this->key1_], c[this->key2_]);
 		return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;

gtsam/slam/GeneralSFMFactor.h

@@ -29,21 +29,21 @@ namespace gtsam {
 	/**
 	 * Non-linear factor for a constraint derived from a 2D measurement. The calibration is unknown here compared to GenericProjectionFactor
 	 */
-	template <class VALUES, class CamK, class LmK>
+	template <class CamK, class LmK>
 	class GeneralSFMFactor:
-	public NonlinearFactor2<VALUES, CamK, LmK> {
+	public NonlinearFactor2<CamK, LmK> {
 	protected:
 		Point2 z_;			///< the 2D measurement
 
 	public:
 
 		typedef typename CamK::Value Cam;										///< typedef for camera type
-		typedef GeneralSFMFactor<VALUES, CamK, LmK> Self ;	///< typedef for this object
+		typedef GeneralSFMFactor<CamK, LmK> Self ;	///< typedef for this object
-		typedef NonlinearFactor2<VALUES, CamK, LmK> Base;		///< typedef for the base class
+		typedef NonlinearFactor2<CamK, LmK> Base;		///< typedef for the base class
 		typedef Point2 Measurement;													///< typedef for the measurement
 
 		// shorthand for a smart pointer to a factor
-		typedef boost::shared_ptr<GeneralSFMFactor<VALUES, LmK, CamK> > shared_ptr;
+		typedef boost::shared_ptr<GeneralSFMFactor<LmK, CamK> > shared_ptr;
 
 		/**
 		 * Constructor
@@ -72,7 +72,7 @@ namespace gtsam {
 		/**
 		 * equals
 		 */
-		bool equals(const GeneralSFMFactor<VALUES, CamK, LmK> &p, double tol = 1e-9) const	{
+		bool equals(const GeneralSFMFactor<CamK, LmK> &p, double tol = 1e-9) const	{
 			return Base::equals(p, tol) && this->z_.equals(p.z_, tol) ;
 		}
 

gtsam/slam/PartialPriorFactor.h

@@ -38,16 +38,16 @@ namespace gtsam {
 	 * For practical use, it would be good to subclass this factor and have the class type
 	 * construct the mask.
 	 */
-	template<class VALUES, class KEY>
+	template<class KEY>
-	class PartialPriorFactor: public NonlinearFactor1<VALUES, KEY> {
+	class PartialPriorFactor: public NonlinearFactor1<KEY> {
 
 	public:
 		typedef typename KEY::Value T;
 
 	protected:
 
-		typedef NonlinearFactor1<VALUES, KEY> Base;
+		typedef NonlinearFactor1<KEY> Base;
-		typedef PartialPriorFactor<VALUES, KEY> This;
+		typedef PartialPriorFactor<KEY> This;
 
 		Vector prior_;             ///< measurement on logmap parameters, in compressed form
 		std::vector<size_t> mask_; ///< indices of values to constrain in compressed prior vector
@@ -95,7 +95,7 @@ namespace gtsam {
 		}
 
 		/** equals */
-		virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
+		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
 			const This *e = dynamic_cast<const This*> (&expected);
 			return e != NULL && Base::equals(*e, tol) &&
 					gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) &&

gtsam/slam/PriorFactor.h

@@ -28,15 +28,15 @@ namespace gtsam {
 	 * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
 	 * a simple type like int will not work
 	 */
-	template<class VALUES, class KEY>
+	template<class KEY>
-	class PriorFactor: public NonlinearFactor1<VALUES, KEY> {
+	class PriorFactor: public NonlinearFactor1<KEY> {
 
 	public:
 		typedef typename KEY::Value T;
 
 	private:
 
-		typedef NonlinearFactor1<VALUES, KEY> Base;
+		typedef NonlinearFactor1<KEY> Base;
 
 		T prior_; /** The measurement */
 
@@ -69,9 +69,8 @@ namespace gtsam {
 		}
 
 		/** equals */
-		virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
+		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
-			const PriorFactor<VALUES, KEY> *e = dynamic_cast<const PriorFactor<
+			const PriorFactor<KEY> *e = dynamic_cast<const PriorFactor<KEY>*> (&expected);
-					VALUES, KEY>*> (&expected);
 			return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol);
 		}
 

gtsam/slam/ProjectionFactor.h

@@ -29,8 +29,8 @@ namespace gtsam {
 	 * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
 	 * i.e. the main building block for visual SLAM.
 	 */
-	template<class VALUES, class LMK, class POSK>
+	template<class LMK, class POSK>
-	class GenericProjectionFactor: public NonlinearFactor2<VALUES, POSK, LMK> {
+	class GenericProjectionFactor: public NonlinearFactor2<POSK, LMK> {
 	protected:
 
 		// Keep a copy of measurement and calibration for I/O
@@ -40,10 +40,10 @@ namespace gtsam {
 	public:
 
 		/// shorthand for base class type
-		typedef NonlinearFactor2<VALUES, POSK, LMK> Base;
+		typedef NonlinearFactor2<POSK, LMK> Base;
 
 		/// shorthand for a smart pointer to a factor
-		typedef boost::shared_ptr<GenericProjectionFactor<VALUES, LMK, POSK> > shared_ptr;
+		typedef boost::shared_ptr<GenericProjectionFactor<LMK, POSK> > shared_ptr;
 
 		/// Default constructor
 		GenericProjectionFactor() :
@@ -73,7 +73,7 @@ namespace gtsam {
 		}
 
 		/// equals
-		bool equals(const GenericProjectionFactor<VALUES, LMK, POSK>& p
+		bool equals(const GenericProjectionFactor<LMK, POSK>& p
 				, double tol = 1e-9) const {
 			return Base::equals(p, tol) && this->z_.equals(p.z_, tol)
 					&& this->K_->equals(*p.K_, tol);

gtsam/slam/RangeFactor.h

@@ -25,14 +25,14 @@ namespace gtsam {
 	/**
 	 * Binary factor for a range measurement
 	 */
-	template<class VALUES, class POSEKEY, class POINTKEY>
+	template<class POSEKEY, class POINTKEY>
-	class RangeFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
+	class RangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
 	private:
 
 		double z_; /** measurement */
 
-		typedef RangeFactor<VALUES, POSEKEY, POINTKEY> This;
+		typedef RangeFactor<POSEKEY, POINTKEY> This;
-		typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
+		typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
 
 		typedef typename POSEKEY::Value Pose;
 		typedef typename POINTKEY::Value Point;
@@ -64,7 +64,7 @@ namespace gtsam {
 		}
 
 		/** equals specialized to this factor */
-		virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
+		virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
 			const This *e = dynamic_cast<const This*> (&expected);
 			return e != NULL && Base::equals(*e, tol) && fabs(this->z_ - e->z_) < tol;
 		}

gtsam/slam/StereoFactor.h

@@ -22,8 +22,8 @@
 
 namespace gtsam {
 
-template<class VALUES, class KEY1, class KEY2>
+template<class KEY1, class KEY2>
-class GenericStereoFactor: public NonlinearFactor2<VALUES, KEY1, KEY2> {
+class GenericStereoFactor: public NonlinearFactor2<KEY1, KEY2> {
 private:
 
 	// Keep a copy of measurement and calibration for I/O
@@ -33,7 +33,7 @@ private:
 public:
 
 	// shorthand for base class type
-	typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base;					///< typedef for base class
+	typedef NonlinearFactor2<KEY1, KEY2> Base;					///< typedef for base class
 	typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;  ///< typedef for shared pointer to this object
 	typedef typename KEY1::Value CamPose;												///< typedef for Pose Lie Value type
 

gtsam/slam/dataset.cpp

@@ -29,7 +29,6 @@ using namespace gtsam;
 #define LINESIZE 81920
 
 typedef boost::shared_ptr<pose2SLAM::Graph> sharedPose2Graph;
-typedef boost::shared_ptr<pose2SLAM::Values> sharedPose2Values;
 typedef pose2SLAM::Odometry Pose2Factor; ///< Typedef for Constraint class for backwards compatibility
 
 namespace gtsam {
@@ -66,13 +65,13 @@ pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset,  c
 
 /* ************************************************************************* */
 
-pair<sharedPose2Graph, sharedPose2Values> load2D(
+pair<sharedPose2Graph, Values::shared_ptr> load2D(
 		pair<string, boost::optional<SharedDiagonal> > dataset,
 		int maxID, bool addNoise, bool smart) {
 	return load2D(dataset.first, dataset.second, maxID, addNoise, smart);
 }
 
-pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
+pair<sharedPose2Graph, Values::shared_ptr> load2D(const string& filename,
 		boost::optional<SharedDiagonal> model, int maxID, bool addNoise, bool smart) {
 	cout << "Will try to read " << filename << endl;
 	ifstream is(filename.c_str());
@@ -81,7 +80,7 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
 		exit(-1);
 	}
 
-	sharedPose2Values poses(new pose2SLAM::Values);
+	Values::shared_ptr poses(new Values);
 	sharedPose2Graph graph(new pose2SLAM::Graph);
 
 	string tag;
@@ -96,7 +95,7 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
 			is >> id >> x >> y >> yaw;
 			// optional filter
 			if (maxID && id >= maxID) continue;
-			poses->insert(id, Pose2(x, y, yaw));
+			poses->insert(pose2SLAM::PoseKey(id), Pose2(x, y, yaw));
 		}
 		is.ignore(LINESIZE, '\n');
 	}
@@ -133,8 +132,8 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
 				l1Xl2 = l1Xl2.retract((*model)->sample());
 
 			// Insert vertices if pure odometry file
-			if (!poses->exists(id1)) poses->insert(id1, Pose2());
+			if (!poses->exists(pose2SLAM::PoseKey(id1))) poses->insert(pose2SLAM::PoseKey(id1), Pose2());
-			if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * l1Xl2);
+			if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at(pose2SLAM::PoseKey(id1)) * l1Xl2);
 
 			pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model));
 			graph->push_back(factor);
@@ -149,26 +148,25 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
 }
 
 /* ************************************************************************* */
-void save2D(const pose2SLAM::Graph& graph, const pose2SLAM::Values& config, const SharedDiagonal model,
+void save2D(const pose2SLAM::Graph& graph, const Values& config, const SharedDiagonal model,
 		const string& filename) {
-	typedef pose2SLAM::Values::Key Key;
 
 	fstream stream(filename.c_str(), fstream::out);
 
 	// save poses
-	pose2SLAM::Values::Key key;
+	BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) {
-	Pose2 pose;
+	  const Pose2& pose = dynamic_cast<const Pose2&>(key_value.second);
-	BOOST_FOREACH(boost::tie(key, pose), config)
+		stream << "VERTEX2 " << key_value.first.index() << " " <<  pose.x() << " " << pose.y() << " " << pose.theta() << endl;
-		stream << "VERTEX2 " << key.index() << " " <<  pose.x() << " " << pose.y() << " " << pose.theta() << endl;
+	}
 
 	// save edges
 	Matrix R = model->R();
 	Matrix RR = trans(R)*R;//prod(trans(R),R);
-	BOOST_FOREACH(boost::shared_ptr<NonlinearFactor<pose2SLAM::Values> > factor_, graph) {
+	BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
 		boost::shared_ptr<Pose2Factor> factor = boost::dynamic_pointer_cast<Pose2Factor>(factor_);
 		if (!factor) continue;
 
-		pose = factor->measured().inverse();
+		Pose2 pose = factor->measured().inverse();
 		stream << "EDGE2 " << factor->key2().index() << " " << factor->key1().index()
 				<< " " << pose.x() << " " << pose.y() << " " << pose.theta()
 				<< " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2)

gtsam/slam/dataset.h

@@ -40,16 +40,16 @@ std::pair<std::string, boost::optional<gtsam::SharedDiagonal> >
  * @param maxID, if non-zero cut out vertices >= maxID
  * @param smart: try to reduce complexity of covariance to cheapest model
  */
-std::pair<boost::shared_ptr<pose2SLAM::Graph>, boost::shared_ptr<pose2SLAM::Values> > load2D(
+std::pair<boost::shared_ptr<pose2SLAM::Graph>, boost::shared_ptr<Values> > load2D(
 		std::pair<std::string, boost::optional<SharedDiagonal> > dataset,
 		int maxID = 0, bool addNoise=false, bool smart=true);
-std::pair<boost::shared_ptr<pose2SLAM::Graph>, boost::shared_ptr<pose2SLAM::Values> > load2D(
+std::pair<boost::shared_ptr<pose2SLAM::Graph>, boost::shared_ptr<Values> > load2D(
 		const std::string& filename,
 		boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(),
 		int maxID = 0, bool addNoise=false, bool smart=true);
 
 /** save 2d graph */
-void save2D(const pose2SLAM::Graph& graph, const pose2SLAM::Values& config, const gtsam::SharedDiagonal model,
+void save2D(const pose2SLAM::Graph& graph, const Values& config, const gtsam::SharedDiagonal model,
 		const std::string& filename);
 
 /**

gtsam/slam/planarSLAM.cpp

@@ -23,8 +23,8 @@
 // Use planarSLAM namespace for specific SLAM instance
 namespace planarSLAM {
 
-  Graph::Graph(const NonlinearFactorGraph<Values>& graph) :
+  Graph::Graph(const NonlinearFactorGraph& graph) :
-      NonlinearFactorGraph<Values>(graph) {}
+      NonlinearFactorGraph(graph) {}
 
   void Graph::addPrior(const PoseKey& i, const Pose2& p,
       const SharedNoiseModel& model) {

gtsam/slam/planarSLAM.h

@@ -22,7 +22,6 @@
 #include <gtsam/slam/RangeFactor.h>
 #include <gtsam/slam/BearingFactor.h>
 #include <gtsam/slam/BearingRangeFactor.h>
-#include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimization.h>
@@ -39,107 +38,63 @@ namespace planarSLAM {
 
   /// Typedef for a PointKey with Point2 data and 'l' symbol
   typedef TypedSymbol<Point2, 'l'> PointKey;
-
-  /// Typedef for Values structure with PoseKey type
-  typedef Values<PoseKey> PoseValues;
-
-  /// Typedef for Values structure with PointKey type
-  typedef Values<PointKey> PointValues;
-
-  /// Values class, inherited from TupleValues2, using PoseKeys and PointKeys
-  struct Values: public TupleValues2<PoseValues, PointValues> {
-
-    /// Default constructor
-    Values() {}
-
-    /// Copy constructor
-    Values(const TupleValues2<PoseValues, PointValues>& values) :
-      TupleValues2<PoseValues, PointValues>(values) {
-    }
-
-    /// Copy constructor
-    Values(const TupleValues2<PoseValues, PointValues>::Base& values) :
-      TupleValues2<PoseValues, PointValues>(values) {
-    }
-
-    /// From sub-values
-    Values(const PoseValues& poses, const PointValues& points) :
-      TupleValues2<PoseValues, PointValues>(poses, points) {
-    }
-
-    // Convenience for MATLAB wrapper, which does not allow for identically named methods
-
-    /// 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); }
-  };
-
   /**
    * List of typedefs for factors
    */
+		/// A hard constraint for PoseKeys to enforce particular values
+		typedef NonlinearEquality<PoseKey> Constraint;
+		/// A prior factor to bias the value of a PoseKey
+		typedef PriorFactor<PoseKey> Prior;
+		/// A factor between two PoseKeys set with a Pose2
+		typedef BetweenFactor<PoseKey> Odometry;
+		/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
+		typedef BearingFactor<PoseKey, PointKey> Bearing;
+		/// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
+		typedef RangeFactor<PoseKey, PointKey> Range;
+		/// A factor between a PoseKey and a PointKey to express difference in rotation and location
+		typedef BearingRangeFactor<PoseKey, PointKey> BearingRange;
 
-  /// A hard constraint for PoseKeys to enforce particular values
+		/// Creates a NonlinearFactorGraph with the Values type
-  typedef NonlinearEquality<Values, PoseKey> Constraint;
+		struct Graph: public NonlinearFactorGraph {
-  /// A prior factor to bias the value of a PoseKey
-  typedef PriorFactor<Values, PoseKey> Prior;
-  /// A factor between two PoseKeys set with a Pose2
-  typedef BetweenFactor<Values, PoseKey> Odometry;
-  /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
-  typedef BearingFactor<Values, PoseKey, PointKey> Bearing;
-  /// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
-  typedef RangeFactor<Values, PoseKey, PointKey> Range;
-  /// A factor between a PoseKey and a PointKey to express difference in rotation and location
-  typedef BearingRangeFactor<Values, PoseKey, PointKey> BearingRange;
 
-  /// Creates a NonlinearFactorGraph with the Values type
+		  /// Default constructor for a NonlinearFactorGraph
-  struct Graph: public NonlinearFactorGraph<Values> {
+		  Graph(){}
 
-    /// Default constructor for a NonlinearFactorGraph
+		  /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
-    Graph(){}
+		  Graph(const NonlinearFactorGraph& graph);
 
-    /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
+		  /// Biases the value of PoseKey key with Pose2 p given a noise model
-    Graph(const NonlinearFactorGraph<Values>& graph);
+		  void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel);
 
-    /// Biases the value of PoseKey key with Pose2 p given a noise model
+		  /// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
-    void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel);
+		  void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose);
 
-    /// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
+		  /// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
-    void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose);
+		  void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry,
+		      const SharedNoiseModel& model);
 
-    /// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
+		  /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation
-    void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry,
+		  void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing,
-        const SharedNoiseModel& model);
+		      const SharedNoiseModel& model);
 
-    /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation
+		  /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location
-    void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing,
+		  void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range,
-        const SharedNoiseModel& model);
+		      const SharedNoiseModel& model);
 
-    /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location
+		  /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location
-    void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range,
+		  void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey,
-        const SharedNoiseModel& model);
+		      const Rot2& bearing, double range, const SharedNoiseModel& model);
 
-    /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location
+		  /// Optimize
-    void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey,
+		  Values optimize(const Values& initialEstimate) {
-        const Rot2& bearing, double range, const SharedNoiseModel& model);
+		    typedef NonlinearOptimizer<Graph> Optimizer;
+		    return *Optimizer::optimizeLM(*this, initialEstimate,
+		        NonlinearOptimizationParameters::LAMBDA);
+		  }
+		};
 
-    /// Optimize
+		/// Optimizer
-    Values optimize(const Values& initialEstimate) {
+		typedef NonlinearOptimizer<Graph> Optimizer;
-      typedef NonlinearOptimizer<Graph, Values> Optimizer;
-      return *Optimizer::optimizeLM(*this, initialEstimate,
-                  NonlinearOptimizationParameters::LAMBDA);
-    }
-  };
-
-  /// Optimizer
-  typedef NonlinearOptimizer<Graph, Values> Optimizer;
 
 } // planarSLAM
 

gtsam/slam/pose2SLAM.cpp

@@ -21,7 +21,7 @@
 
 // Use pose2SLAM namespace for specific SLAM instance
 
-template class gtsam::NonlinearOptimizer<pose2SLAM::Graph, pose2SLAM::Values, gtsam::GaussianFactorGraph, gtsam::GaussianSequentialSolver>;
+	template class gtsam::NonlinearOptimizer<pose2SLAM::Graph, gtsam::GaussianFactorGraph, gtsam::GaussianSequentialSolver>;
 
 namespace pose2SLAM {
 
@@ -30,7 +30,7 @@ namespace pose2SLAM {
     Values x;
     double theta = 0, dtheta = 2 * M_PI / n;
     for (size_t i = 0; i < n; i++, theta += dtheta)
-      x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
+      x.insert(PoseKey(i), Pose2(cos(theta), sin(theta), M_PI_2 + theta));
     return x;
   }
 

gtsam/slam/pose2SLAM.h

@@ -18,10 +18,10 @@
 #pragma once
 
 #include <gtsam/geometry/Pose2.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/Key.h>
+#include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
@@ -36,25 +36,6 @@ namespace pose2SLAM {
   /// Keys with Pose2 and symbol 'x'
   typedef TypedSymbol<Pose2, 'x'> PoseKey;
 
-  /// Values class, inherited from Values, using PoseKeys
-  struct Values: public gtsam::Values<PoseKey> {
-
-    /// Default constructor
-    Values() {}
-
-    /// Copy constructor
-    Values(const gtsam::Values<PoseKey>& values) :
-        gtsam::Values<PoseKey>(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)
@@ -70,20 +51,20 @@ namespace pose2SLAM {
    */
 
   /// A hard constraint to enforce a specific value for a pose
-  typedef NonlinearEquality<Values, PoseKey> HardConstraint;
+  typedef NonlinearEquality<PoseKey> HardConstraint;
   /// A prior factor on a pose with Pose2 data type.
-  typedef PriorFactor<Values, PoseKey> Prior;
+  typedef PriorFactor<PoseKey> Prior;
   /// A factor to add an odometry measurement between two poses.
-  typedef BetweenFactor<Values, PoseKey> Odometry;
+  typedef BetweenFactor<PoseKey> Odometry;
 
   /// Graph
-  struct Graph: public NonlinearFactorGraph<Values> {
+  struct Graph: public NonlinearFactorGraph {
 
     /// Default constructor for a NonlinearFactorGraph
     Graph(){}
 
     /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
-    Graph(const NonlinearFactorGraph<Values>& graph);
+    Graph(const NonlinearFactorGraph& graph);
 
     /// Adds a Pose2 prior with a noise model to one of the keys in the nonlinear factor graph
     void addPrior(const PoseKey& i, const Pose2& p, const SharedNoiseModel& model);
@@ -97,18 +78,18 @@ namespace pose2SLAM {
 
     /// Optimize
     Values optimize(const Values& initialEstimate) {
-      typedef NonlinearOptimizer<Graph, Values> Optimizer;
+      typedef NonlinearOptimizer<Graph> Optimizer;
       return *Optimizer::optimizeLM(*this, initialEstimate,
                   NonlinearOptimizationParameters::LAMBDA);
     }
   };
 
   /// The sequential optimizer
-  typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph,
+  typedef NonlinearOptimizer<Graph, GaussianFactorGraph,
       GaussianSequentialSolver> OptimizerSequential;
 
   /// The multifrontal optimizer
-  typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph,
+  typedef NonlinearOptimizer<Graph, GaussianFactorGraph,
       GaussianMultifrontalSolver> Optimizer;
 
 } // pose2SLAM

gtsam/slam/pose3SLAM.cpp

@@ -39,7 +39,7 @@ namespace gtsam {
 				Point3 gti(radius*cos(theta), radius*sin(theta), 0);
 				Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down !
 				Pose3 gTi(gR0 * _0Ri, gti);
-				x.insert(i, gTi);
+				x.insert(Key(i), gTi);
 			}
 			return x;
 		}

gtsam/slam/pose3SLAM.h

@@ -18,7 +18,6 @@
 #pragma once
 
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/Key.h>
@@ -33,8 +32,6 @@ namespace gtsam {
 
 		/// Creates a Key with data Pose3 and symbol 'x'
 		typedef TypedSymbol<Pose3, 'x'> Key;
-		/// Creates a Values structure with type 'Key'
-		typedef Values<Key> Values;
 
 		/**
 		 * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
@@ -45,14 +42,14 @@ namespace gtsam {
 		Values circle(size_t n, double R);
 
 		/// A prior factor on Key with Pose3 data type.
-		typedef PriorFactor<Values, Key> Prior;
+		typedef PriorFactor<Key> Prior;
 		/// A factor to put constraints between two factors.
-		typedef BetweenFactor<Values, Key> Constraint;
+		typedef BetweenFactor<Key> Constraint;
 		/// A hard constraint would enforce that the given key would have the input value in the results.
-		typedef NonlinearEquality<Values, Key> HardConstraint;
+		typedef NonlinearEquality<Key> HardConstraint;
 
 		/// Graph
-		struct Graph: public NonlinearFactorGraph<Values> {
+		struct Graph: public NonlinearFactorGraph {
 
 			/// Adds a factor between keys of the same type
 			void addPrior(const Key& i, const Pose3& p,
@@ -67,14 +64,13 @@ namespace gtsam {
 		};
 
 		/// Optimizer
-		typedef NonlinearOptimizer<Graph, Values> Optimizer;
+		typedef NonlinearOptimizer<Graph> Optimizer;
 
 	} // pose3SLAM
 
 	/**
 	 * Backwards compatibility
 	 */
-	typedef pose3SLAM::Values Pose3Values;			///< Typedef for Values class for backwards compatibility
 	typedef pose3SLAM::Prior Pose3Prior;			///< Typedef for Prior class for backwards compatibility
 	typedef pose3SLAM::Constraint Pose3Factor;		///< Typedef for Constraint class for backwards compatibility
 	typedef pose3SLAM::Graph Pose3Graph;			///< Typedef for Graph class for backwards compatibility

gtsam/slam/simulated2D.h

@@ -19,7 +19,6 @@
 #pragma once
 
 #include <gtsam/geometry/Point2.h>
-#include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
@@ -32,56 +31,6 @@ namespace simulated2D {
   // Simulated2D robots have no orientation, just a position
   typedef TypedSymbol<Point2, 'x'> PoseKey;
   typedef TypedSymbol<Point2, 'l'> PointKey;
-  typedef Values<PoseKey> PoseValues;
-  typedef Values<PointKey> PointValues;
-
-  /**
-   *  Custom Values class that holds poses and points
-   */
-  class Values: public TupleValues2<PoseValues, PointValues> {
-  public:
-    typedef TupleValues2<PoseValues, PointValues> Base;  ///< base class
-    typedef boost::shared_ptr<Point2> sharedPoint;  ///< shortcut to shared Point type
-
-    /// Constructor
-    Values() {
-    }
-
-    /// Copy constructor
-    Values(const Base& base) :
-        Base(base) {
-    }
-
-    /// Insert a pose
-    void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
-      insert(i, p);
-    }
-
-    /// Insert a point
-    void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
-      insert(j, p);
-    }
-
-    /// Number of poses
-    int nrPoses() const {
-      return this->first_.size();
-    }
-
-    /// Number of points
-    int nrPoints() const {
-      return this->second_.size();
-    }
-
-    /// 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) {
@@ -112,18 +61,18 @@ namespace simulated2D {
   /**
    *  Unary factor encoding a soft prior on a vector
    */
-  template<class VALUES = Values, class KEY = PoseKey>
+  template<class KEY = PoseKey>
-  class GenericPrior: public NonlinearFactor1<VALUES, KEY> {
+  class GenericPrior: public NonlinearFactor1<KEY> {
   public:
-    typedef NonlinearFactor1<VALUES, KEY> Base;  ///< base class
+    typedef NonlinearFactor1<KEY> Base;  ///< base class
-    typedef boost::shared_ptr<GenericPrior<VALUES, KEY> > shared_ptr;
+    typedef boost::shared_ptr<GenericPrior<KEY> > shared_ptr;
     typedef typename KEY::Value Pose; ///< shortcut to Pose type
 
     Pose z_; ///< prior mean
 
     /// Create generic prior
     GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) :
-        NonlinearFactor1<VALUES, KEY>(model, key), z_(z) {
+      NonlinearFactor1<KEY>(model, key), z_(z) {
     }
 
     /// Return error and optional derivative
@@ -150,11 +99,11 @@ namespace simulated2D {
   /**
    * Binary factor simulating "odometry" between two Vectors
    */
-  template<class VALUES = Values, class KEY = PoseKey>
+  template<class KEY = PoseKey>
-  class GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> {
+  class GenericOdometry: public NonlinearFactor2<KEY, KEY> {
   public:
-    typedef NonlinearFactor2<VALUES, KEY, KEY> Base; ///< base class
+    typedef NonlinearFactor2<KEY, KEY> Base; ///< base class
-    typedef boost::shared_ptr<GenericOdometry<VALUES, KEY> > shared_ptr;
+    typedef boost::shared_ptr<GenericOdometry<KEY> > shared_ptr;
     typedef typename KEY::Value Pose; ///< shortcut to Pose type
 
     Pose z_; ///< odometry measurement
@@ -162,7 +111,7 @@ namespace simulated2D {
     /// Create odometry
     GenericOdometry(const Pose& z, const SharedNoiseModel& model,
         const KEY& i1, const KEY& i2) :
-        NonlinearFactor2<VALUES, KEY, KEY>(model, i1, i2), z_(z) {
+          NonlinearFactor2<KEY, KEY>(model, i1, i2), z_(z) {
     }
 
     /// Evaluate error and optionally return derivatives
@@ -190,11 +139,11 @@ namespace simulated2D {
   /**
    * Binary factor simulating "measurement" between two Vectors
    */
-  template<class VALUES = Values, class XKEY = PoseKey, class LKEY = PointKey>
+  template<class XKEY = PoseKey, class LKEY = PointKey>
-  class GenericMeasurement: public NonlinearFactor2<VALUES, XKEY, LKEY> {
+  class GenericMeasurement: public NonlinearFactor2<XKEY, LKEY> {
   public:
-    typedef NonlinearFactor2<VALUES, XKEY, LKEY> Base;  ///< base class
+    typedef NonlinearFactor2<XKEY, LKEY> Base;  ///< base class
-    typedef boost::shared_ptr<GenericMeasurement<VALUES, XKEY, LKEY> > shared_ptr;
+    typedef boost::shared_ptr<GenericMeasurement<XKEY, LKEY> > shared_ptr;
     typedef typename XKEY::Value Pose; ///< shortcut to Pose type
     typedef typename LKEY::Value Point; ///< shortcut to Point type
 
@@ -203,7 +152,7 @@ namespace simulated2D {
     /// Create measurement factor
     GenericMeasurement(const Point& z, const SharedNoiseModel& model,
         const XKEY& i, const LKEY& j) :
-        NonlinearFactor2<VALUES, XKEY, LKEY>(model, i, j), z_(z) {
+          NonlinearFactor2<XKEY, LKEY>(model, i, j), z_(z) {
     }
 
     /// Evaluate error and optionally return derivatives
@@ -229,13 +178,13 @@ namespace simulated2D {
   };
 
   /** Typedefs for regular use */
-  typedef GenericPrior<Values, PoseKey> Prior;
+  typedef GenericPrior<PoseKey> Prior;
-  typedef GenericOdometry<Values, PoseKey> Odometry;
+  typedef GenericOdometry<PoseKey> Odometry;
-  typedef GenericMeasurement<Values, PoseKey, PointKey> Measurement;
+  typedef GenericMeasurement<PoseKey, PointKey> Measurement;
 
   // Specialization of a graph for this example domain
   // TODO: add functions to add factor types
-  class Graph : public NonlinearFactorGraph<Values> {
+  class Graph : public NonlinearFactorGraph {
   public:
     Graph() {}
   };

gtsam/slam/simulated2DConstraints.h

@@ -33,13 +33,13 @@ namespace simulated2D {
   namespace equality_constraints {
 
     /** Typedefs for regular use */
-    typedef NonlinearEquality1<Values, PoseKey> UnaryEqualityConstraint;
+    typedef NonlinearEquality1<PoseKey> UnaryEqualityConstraint;
-    typedef NonlinearEquality1<Values, PointKey> UnaryEqualityPointConstraint;
+    typedef NonlinearEquality1<PointKey> UnaryEqualityPointConstraint;
-    typedef BetweenConstraint<Values, PoseKey> OdoEqualityConstraint;
+    typedef BetweenConstraint<PoseKey> OdoEqualityConstraint;
 
     /** Equality between variables */
-    typedef NonlinearEquality2<Values, PoseKey> PoseEqualityConstraint;
+    typedef NonlinearEquality2<PoseKey> PoseEqualityConstraint;
-    typedef NonlinearEquality2<Values, PointKey> PointEqualityConstraint;
+    typedef NonlinearEquality2<PointKey> PointEqualityConstraint;
 
   } // \namespace equality_constraints
 
@@ -51,10 +51,10 @@ namespace simulated2D {
      * @tparam KEY is the key type for the variable constrained
      * @tparam IDX is an index in tangent space to constrain, must be less than KEY::VALUE::Dim()
      */
-    template<class VALUES, class KEY, unsigned int IDX>
+    template<class KEY, unsigned int IDX>
-    struct ScalarCoordConstraint1: public BoundingConstraint1<VALUES, KEY> {
+    struct ScalarCoordConstraint1: public BoundingConstraint1<KEY> {
-      typedef BoundingConstraint1<VALUES, KEY> Base;  ///< Base class convenience typedef
+      typedef BoundingConstraint1<KEY> Base;  ///< Base class convenience typedef
-      typedef boost::shared_ptr<ScalarCoordConstraint1<VALUES, KEY, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
+      typedef boost::shared_ptr<ScalarCoordConstraint1<KEY, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
       typedef typename KEY::Value Point; ///< Constrained variable type
 
       virtual ~ScalarCoordConstraint1() {}
@@ -68,7 +68,7 @@ namespace simulated2D {
        */
       ScalarCoordConstraint1(const KEY& key, double c,
           bool isGreaterThan, double mu = 1000.0) :
-        Base(key, c, isGreaterThan, mu) {
+            Base(key, c, isGreaterThan, mu) {
       }
 
       /**
@@ -94,8 +94,8 @@ namespace simulated2D {
     };
 
     /** typedefs for use with simulated2D systems */
-    typedef ScalarCoordConstraint1<Values, PoseKey, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
+    typedef ScalarCoordConstraint1<PoseKey, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
-    typedef ScalarCoordConstraint1<Values, PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
+    typedef ScalarCoordConstraint1<PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
 
     /**
      * Trait for distance constraints to provide distance
@@ -114,9 +114,9 @@ namespace simulated2D {
      * @tparam VALUES is the variable set for the graph
      * @tparam KEY is the type of the keys for the variables constrained
      */
-    template<class VALUES, class KEY>
+    template<class KEY>
-    struct MaxDistanceConstraint : public BoundingConstraint2<VALUES, KEY, KEY> {
+    struct MaxDistanceConstraint : public BoundingConstraint2<KEY, KEY> {
-      typedef BoundingConstraint2<VALUES, KEY, KEY> Base;  ///< Base class for factor
+      typedef BoundingConstraint2<KEY, KEY> Base;  ///< Base class for factor
       typedef typename KEY::Value Point; ///< Type of variable constrained
 
       virtual ~MaxDistanceConstraint() {}
@@ -129,7 +129,7 @@ namespace simulated2D {
        * @param mu is the gain for the penalty function
        */
       MaxDistanceConstraint(const KEY& key1, const KEY& key2, double range_bound, double mu = 1000.0)
-        : Base(key1, key2, range_bound, false, mu) {}
+      : Base(key1, key2, range_bound, false, mu) {}
 
       /**
        * computes the range with derivatives
@@ -148,7 +148,7 @@ namespace simulated2D {
       }
     };
 
-    typedef MaxDistanceConstraint<Values, PoseKey> PoseMaxDistConstraint; ///< Simulated2D domain example factor
+    typedef MaxDistanceConstraint<PoseKey> PoseMaxDistConstraint; ///< Simulated2D domain example factor
 
     /**
      * Binary inequality constraint forcing a minimum range
@@ -157,9 +157,9 @@ namespace simulated2D {
      * @tparam XKEY is the type of the pose key constrained
      * @tparam PKEY is the type of the point key constrained
      */
-    template<class VALUES, class XKEY, class PKEY>
+    template<class XKEY, class PKEY>
-    struct MinDistanceConstraint : public BoundingConstraint2<VALUES, XKEY, PKEY> {
+    struct MinDistanceConstraint : public BoundingConstraint2<XKEY, PKEY> {
-      typedef BoundingConstraint2<VALUES, XKEY, PKEY> Base; ///< Base class for factor
+      typedef BoundingConstraint2<XKEY, PKEY> Base; ///< Base class for factor
       typedef typename XKEY::Value Pose; ///< Type of pose variable constrained
       typedef typename PKEY::Value Point; ///< Type of point variable constrained
 
@@ -174,7 +174,7 @@ namespace simulated2D {
        */
       MinDistanceConstraint(const XKEY& key1, const PKEY& key2,
           double range_bound, double mu = 1000.0)
-        : Base(key1, key2, range_bound, true, mu) {}
+      : Base(key1, key2, range_bound, true, mu) {}
 
       /**
        * computes the range with derivatives
@@ -193,7 +193,7 @@ namespace simulated2D {
       }
     };
 
-    typedef MinDistanceConstraint<Values, PoseKey, PointKey> LandmarkAvoid; ///< Simulated2D domain example factor
+    typedef MinDistanceConstraint<PoseKey, PointKey> LandmarkAvoid; ///< Simulated2D domain example factor
 
 
   } // \namespace inequality_constraints

gtsam/slam/simulated2DOriented.h

@@ -19,7 +19,6 @@
 #pragma once
 
 #include <gtsam/geometry/Pose2.h>
-#include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
@@ -31,29 +30,6 @@ namespace simulated2DOriented {
   // The types that take an oriented pose2 rather than point2
   typedef TypedSymbol<Point2, 'l'> PointKey;
   typedef TypedSymbol<Pose2, 'x'> PoseKey;
-  typedef Values<PoseKey> PoseValues;
-  typedef Values<PointKey> PointValues;
-
-  /// Specialized Values structure with syntactic sugar for
-  /// compatibility with matlab
-  class Values: public TupleValues2<PoseValues, PointValues> {
-  public:
-    Values() {}
-
-    void insertPose(const PoseKey& i, const Pose2& p) {
-      insert(i, p);
-    }
-
-    void insertPoint(const PointKey& j, const Point2& p) {
-      insert(j, p);
-    }
-
-    int nrPoses() const {	return this->first_.size();	}
-    int nrPoints() const { return this->second_.size();	}
-
-    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
 
@@ -75,15 +51,15 @@ namespace simulated2DOriented {
       boost::none, boost::optional<Matrix&> H2 = boost::none);
 
   /// Unary factor encoding a soft prior on a vector
-  template<class VALUES = Values, class Key = PoseKey>
+		template<class Key = PoseKey>
-  struct GenericPosePrior: public NonlinearFactor1<VALUES, Key> {
+		struct GenericPosePrior: public NonlinearFactor1<Key> {
 
     Pose2 z_; ///< measurement
 
     /// Create generic pose prior
     GenericPosePrior(const Pose2& z, const SharedNoiseModel& model,
         const Key& key) :
-        NonlinearFactor1<VALUES, Key>(model, key), z_(z) {
+					NonlinearFactor1<Key>(model, key), z_(z) {
     }
 
     /// Evaluate error and optionally derivative
@@ -97,8 +73,8 @@ namespace simulated2DOriented {
   /**
    * Binary factor simulating "odometry" between two Vectors
    */
-  template<class VALUES = Values, class KEY = PoseKey>
+		template<class KEY = PoseKey>
-  struct GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> {
+		struct GenericOdometry: public NonlinearFactor2<KEY, KEY> {
     Pose2 z_;   ///< Between measurement for odometry factor
 
     /**
@@ -106,7 +82,7 @@ namespace simulated2DOriented {
      */
     GenericOdometry(const Pose2& z, const SharedNoiseModel& model,
         const KEY& i1, const KEY& i2) :
-        NonlinearFactor2<VALUES, KEY, KEY>(model, i1, i2), z_(z) {
+					NonlinearFactor2<KEY, KEY>(model, i1, i2), z_(z) {
     }
 
     /// Evaluate error and optionally derivative
@@ -118,10 +94,10 @@ namespace simulated2DOriented {
 
   };
 
-  typedef GenericOdometry<Values, PoseKey> Odometry;
+		typedef GenericOdometry<PoseKey> Odometry;
 
   /// Graph specialization for syntactic sugar use with matlab
-  class Graph : public NonlinearFactorGraph<Values> {
+		class Graph : public NonlinearFactorGraph {
   public:
     Graph() {}
     // TODO: add functions to add factors

gtsam/slam/simulated3D.h

@@ -24,7 +24,6 @@
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/Key.h>
-#include <gtsam/nonlinear/TupleValues.h>
 
 // \namespace
 
@@ -40,10 +39,6 @@ namespace simulated3D {
 typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
 typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
 
-typedef Values<PoseKey> PoseValues;
-typedef Values<PointKey> PointValues;
-typedef TupleValues2<PoseValues, PointValues> Values;
-
 /**
  * Prior on a single pose
  */
@@ -66,7 +61,7 @@ Point3 mea(const Point3& x, const Point3& l,
 /**
  * A prior factor on a single linear robot pose
  */
-struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
+struct PointPrior3D: public NonlinearFactor1<PoseKey> {
 
 	Point3 z_; ///< The prior pose value for the variable attached to this factor
 
@@ -78,7 +73,7 @@ struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
 	 */
 	PointPrior3D(const Point3& z,
 			const SharedNoiseModel& model, const PoseKey& j) :
-				NonlinearFactor1<Values, PoseKey> (model, j), z_(z) {
+				NonlinearFactor1<PoseKey> (model, j), z_(z) {
 	}
 
 	/**
@@ -97,8 +92,7 @@ struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
 /**
  * Models a linear 3D measurement between 3D points
  */
-struct Simulated3DMeasurement: public NonlinearFactor2<Values,
+struct Simulated3DMeasurement: public NonlinearFactor2<PoseKey, PointKey> {
-PoseKey, PointKey> {
 
 	Point3 z_; ///< Linear displacement between a pose and landmark
 
@@ -111,7 +105,7 @@ PoseKey, PointKey> {
 	 */
 	Simulated3DMeasurement(const Point3& z,
 			const SharedNoiseModel& model, PoseKey& j1, PointKey j2) :
-				NonlinearFactor2<Values, PoseKey, PointKey> (
+				NonlinearFactor2<PoseKey, PointKey> (
 						model, j1, j2), z_(z) {
 	}
 

gtsam/slam/smallExample.cpp

@@ -39,7 +39,7 @@ using namespace std;
 namespace gtsam {
 namespace example {
 
-	typedef boost::shared_ptr<NonlinearFactor<Values> > shared;
+	typedef boost::shared_ptr<NonlinearFactor > shared;
 
 	static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
 	static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
@@ -195,14 +195,13 @@ namespace example {
 					 0.0, cos(v.y()));
 		}
 
-		struct UnaryFactor: public gtsam::NonlinearFactor1<Values,
+		struct UnaryFactor: public gtsam::NonlinearFactor1<simulated2D::PoseKey> {
-		simulated2D::PoseKey> {
 
 			Point2 z_;
 
 			UnaryFactor(const Point2& z, const SharedNoiseModel& model,
 					const simulated2D::PoseKey& key) :
-				gtsam::NonlinearFactor1<Values, simulated2D::PoseKey>(model, key), z_(z) {
+				gtsam::NonlinearFactor1<simulated2D::PoseKey>(model, key), z_(z) {
 			}
 
 			Vector evaluateError(const Point2& x, boost::optional<Matrix&> A =
@@ -423,7 +422,7 @@ namespace example {
 	boost::tuple<FactorGraph<GaussianFactor>, Ordering, VectorValues> planarGraph(size_t N) {
 
 		// create empty graph
-		NonlinearFactorGraph<Values> nlfg;
+		NonlinearFactorGraph nlfg;
 
 		// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
 		shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sharedSigma(2,1e-3), key(1,1)));

gtsam/slam/smallExample.h

@@ -28,8 +28,7 @@
 namespace gtsam {
 	namespace example {
 
-		typedef simulated2D::Values Values;
+		typedef NonlinearFactorGraph Graph;
-		typedef NonlinearFactorGraph<Values> Graph;
 
 		/**
 		 * Create small example for non-linear factor graph

gtsam/slam/tests/testAntiFactor.cpp

@@ -39,9 +39,9 @@ TEST( AntiFactor, NegativeHessian)
   SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim()));
 
   // Create a configuration corresponding to the ground truth
-  boost::shared_ptr<pose3SLAM::Values> values(new pose3SLAM::Values());
+  boost::shared_ptr<Values> values(new Values());
-  values->insert(1, pose1);
+  values->insert(pose3SLAM::Key(1), pose1);
-  values->insert(2, pose2);
+  values->insert(pose3SLAM::Key(2), pose2);
 
   // Define an elimination ordering
   Ordering::shared_ptr ordering(new Ordering());
@@ -50,7 +50,7 @@ TEST( AntiFactor, NegativeHessian)
 
 
   // Create a "standard" factor
-  BetweenFactor<pose3SLAM::Values,pose3SLAM::Key>::shared_ptr originalFactor(new BetweenFactor<pose3SLAM::Values,pose3SLAM::Key>(1, 2, z, sigma));
+  BetweenFactor<pose3SLAM::Key>::shared_ptr originalFactor(new BetweenFactor<pose3SLAM::Key>(1, 2, z, sigma));
 
   // Linearize it into a Jacobian Factor
   GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering);
@@ -59,7 +59,7 @@ TEST( AntiFactor, NegativeHessian)
   HessianFactor::shared_ptr originalHessian = HessianFactor::shared_ptr(new HessianFactor(*originalJacobian));
 
   // Create the AntiFactor version of the original nonlinear factor
-  AntiFactor<pose3SLAM::Values>::shared_ptr antiFactor(new AntiFactor<pose3SLAM::Values>(originalFactor));
+  AntiFactor::shared_ptr antiFactor(new AntiFactor(originalFactor));
 
   // Linearize the AntiFactor into a Hessian Factor
   GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(*values, *ordering);
@@ -100,9 +100,9 @@ TEST( AntiFactor, EquivalentBayesNet)
 	graph->addConstraint(1, 2, pose1.between(pose2), sigma);
 
 	// Create a configuration corresponding to the ground truth
-	boost::shared_ptr<pose3SLAM::Values> values(new pose3SLAM::Values());
+	boost::shared_ptr<Values> values(new Values());
-	values->insert(1, pose1);
+	values->insert(pose3SLAM::Key(1), pose1);
-	values->insert(2, pose2);
+	values->insert(pose3SLAM::Key(2), pose2);
 
 	// Define an elimination ordering
 	Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
@@ -119,7 +119,7 @@ TEST( AntiFactor, EquivalentBayesNet)
   graph->push_back(f1);
 
   // Add the corresponding AntiFactor between Pose1 and Pose2
-  AntiFactor<pose3SLAM::Values>::shared_ptr f2(new AntiFactor<pose3SLAM::Values>(f1));
+  AntiFactor::shared_ptr f2(new AntiFactor(f1));
   graph->push_back(f2);
 
 	// Again, Eliminate into a BayesNet

gtsam/slam/tests/testGeneralSFMFactor.cpp

@@ -21,7 +21,6 @@ using namespace boost;
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/linear/VectorValues.h>
-#include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 
@@ -31,14 +30,11 @@ using namespace gtsam;
 typedef PinholeCamera<Cal3_S2> GeneralCamera;
 typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
 typedef TypedSymbol<Point3, 'l'> PointKey;
-typedef Values<CameraKey> CameraConfig;
+typedef GeneralSFMFactor<CameraKey, PointKey> Projection;
-typedef Values<PointKey> PointConfig;
+typedef NonlinearEquality<CameraKey> CameraConstraint;
-typedef TupleValues2<CameraConfig, PointConfig> VisualValues;
+typedef NonlinearEquality<PointKey> Point3Constraint;
-typedef GeneralSFMFactor<VisualValues, CameraKey, PointKey> Projection;
-typedef NonlinearEquality<VisualValues, CameraKey> CameraConstraint;
-typedef NonlinearEquality<VisualValues, PointKey> Point3Constraint;
 
-class Graph: public NonlinearFactorGraph<VisualValues> {
+class Graph: public NonlinearFactorGraph {
 public:
 	void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
 		push_back(boost::make_shared<Projection>(z, model, i, j));
@@ -71,7 +67,7 @@ double getGaussian()
     return sqrt(-2.0f * (double)log(S) / S) * V1;
 }
 
-typedef NonlinearOptimizer<Graph,VisualValues> Optimizer;
+typedef NonlinearOptimizer<Graph> Optimizer;
 
 const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
 
@@ -99,12 +95,12 @@ TEST( GeneralSFMFactor, error ) {
 	boost::shared_ptr<Projection>
 	factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
 	// For the following configuration, the factor predicts 320,240
-	VisualValues values;
+	Values values;
 	Rot3 R;
 	Point3 t1(0,0,-6);
 	Pose3 x1(R,t1);
-	values.insert(1, GeneralCamera(x1));
+	values.insert(CameraKey(1), GeneralCamera(x1));
-	Point3 l1;  values.insert(1, l1);
+	Point3 l1;  values.insert(PointKey(1), l1);
 	EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
 }
 
@@ -172,15 +168,15 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
 
 	// add initial
 	const double noise = baseline*0.1;
-	boost::shared_ptr<VisualValues> values(new VisualValues);
+	boost::shared_ptr<Values> values(new Values);
 	for ( size_t i = 0 ; i < X.size() ; ++i )
-	  values->insert((int)i, X[i]) ;
+	  values->insert(CameraKey((int)i), X[i]) ;
 
 	for ( size_t i = 0 ; i < L.size() ; ++i ) {
 		Point3 pt(L[i].x()+noise*getGaussian(),
 		          L[i].y()+noise*getGaussian(),
 		          L[i].z()+noise*getGaussian());
-		values->insert(i, pt) ;
+		values->insert(PointKey(i), pt) ;
 	}
 
 	graph->addCameraConstraint(0, X[0]);
@@ -211,9 +207,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
 
   // add initial
   const double noise = baseline*0.1;
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert((int)i, X[i]) ;
+    values->insert(CameraKey((int)i), X[i]) ;
 
   // add noise only to the first landmark
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
@@ -221,10 +217,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
       Point3 pt(L[i].x()+noise*getGaussian(),
                 L[i].y()+noise*getGaussian(),
                 L[i].z()+noise*getGaussian());
-      values->insert(i, pt) ;
+      values->insert(PointKey(i), pt) ;
     }
     else {
-      values->insert(i, L[i]) ;
+      values->insert(PointKey(i), L[i]) ;
     }
   }
 
@@ -257,16 +253,16 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
 
   const size_t nMeasurements = L.size()*X.size();
 
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert((int)i, X[i]) ;
+    values->insert(CameraKey((int)i), X[i]) ;
 
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
     Point3 pt(L[i].x()+noise*getGaussian(),
               L[i].y()+noise*getGaussian(),
               L[i].z()+noise*getGaussian());
     //Point3 pt(L[i].x(), L[i].y(), L[i].z());
-    values->insert(i, pt) ;
+    values->insert(PointKey(i), pt) ;
   }
 
   for ( size_t i = 0 ; i < X.size() ; ++i )
@@ -300,7 +296,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
 
   const size_t nMeasurements = L.size()*X.size();
 
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i ) {
     const double
       rot_noise = 1e-5,
@@ -308,7 +304,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
       focal_noise = 1,
       skew_noise = 1e-5;
     if ( i == 0 ) {
-      values->insert((int)i, X[i]) ;
+      values->insert(CameraKey((int)i), X[i]) ;
     }
     else {
 
@@ -319,12 +315,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
           skew_noise, // s
           trans_noise, trans_noise // ux, uy
           ) ;
-      values->insert((int)i, X[i].retract(delta)) ;
+      values->insert(CameraKey((int)i), X[i].retract(delta)) ;
     }
   }
 
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
-    values->insert(i, L[i]) ;
+    values->insert(PointKey(i), L[i]) ;
   }
 
   // fix X0 and all landmarks, allow only the X[1] to move
@@ -362,16 +358,16 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
 
   // add initial
   const double noise = baseline*0.1;
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert((int)i, X[i]) ;
+    values->insert(CameraKey((int)i), X[i]) ;
 
   // add noise only to the first landmark
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
     Point3 pt(L[i].x()+noise*getGaussian(),
               L[i].y()+noise*getGaussian(),
               L[i].z()+noise*getGaussian());
-    values->insert(i, pt) ;
+    values->insert(PointKey(i), pt) ;
   }
 
   graph->addCameraConstraint(0, X[0]);

gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp

@@ -21,7 +21,6 @@ using namespace boost;
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/linear/VectorValues.h>
-#include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 
@@ -31,15 +30,12 @@ using namespace gtsam;
 typedef PinholeCamera<Cal3Bundler> GeneralCamera;
 typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
 typedef TypedSymbol<Point3, 'l'> PointKey;
-typedef Values<CameraKey> CameraConfig;
+typedef GeneralSFMFactor<CameraKey, PointKey> Projection;
-typedef Values<PointKey> PointConfig;
+typedef NonlinearEquality<CameraKey> CameraConstraint;
-typedef TupleValues2<CameraConfig, PointConfig> VisualValues;
+typedef NonlinearEquality<PointKey> Point3Constraint;
-typedef GeneralSFMFactor<VisualValues, CameraKey, PointKey> Projection;
-typedef NonlinearEquality<VisualValues, CameraKey> CameraConstraint;
-typedef NonlinearEquality<VisualValues, PointKey> Point3Constraint;
 
 /* ************************************************************************* */
-class Graph: public NonlinearFactorGraph<VisualValues> {
+class Graph: public NonlinearFactorGraph {
 public:
   void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
     push_back(boost::make_shared<Projection>(z, model, i, j));
@@ -72,7 +68,7 @@ double getGaussian()
     return sqrt(-2.0f * (double)log(S) / S) * V1;
 }
 
-typedef NonlinearOptimizer<Graph,VisualValues> Optimizer;
+typedef NonlinearOptimizer<Graph> Optimizer;
 
 const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
 
@@ -100,12 +96,12 @@ TEST( GeneralSFMFactor, error ) {
   boost::shared_ptr<Projection>
   factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
   // For the following configuration, the factor predicts 320,240
-  VisualValues values;
+  Values values;
   Rot3 R;
   Point3 t1(0,0,-6);
   Pose3 x1(R,t1);
-  values.insert(1, GeneralCamera(x1));
+  values.insert(CameraKey(1), GeneralCamera(x1));
-  Point3 l1;  values.insert(1, l1);
+  Point3 l1;  values.insert(PointKey(1), l1);
   EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
 }
 
@@ -173,15 +169,15 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
 
   // add initial
   const double noise = baseline*0.1;
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert((int)i, X[i]) ;
+    values->insert(CameraKey((int)i), X[i]) ;
 
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
     Point3 pt(L[i].x()+noise*getGaussian(),
               L[i].y()+noise*getGaussian(),
               L[i].z()+noise*getGaussian());
-    values->insert(i, pt) ;
+    values->insert(PointKey(i), pt) ;
   }
 
   graph->addCameraConstraint(0, X[0]);
@@ -212,9 +208,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
 
   // add initial
   const double noise = baseline*0.1;
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert((int)i, X[i]) ;
+    values->insert(CameraKey((int)i), X[i]) ;
 
   // add noise only to the first landmark
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
@@ -222,10 +218,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
       Point3 pt(L[i].x()+noise*getGaussian(),
                 L[i].y()+noise*getGaussian(),
                 L[i].z()+noise*getGaussian());
-      values->insert(i, pt) ;
+      values->insert(PointKey(i), pt) ;
     }
     else {
-      values->insert(i, L[i]) ;
+      values->insert(PointKey(i), L[i]) ;
     }
   }
 
@@ -258,16 +254,16 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
 
   const size_t nMeasurements = L.size()*X.size();
 
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert((int)i, X[i]) ;
+    values->insert(CameraKey((int)i), X[i]) ;
 
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
     Point3 pt(L[i].x()+noise*getGaussian(),
               L[i].y()+noise*getGaussian(),
               L[i].z()+noise*getGaussian());
     //Point3 pt(L[i].x(), L[i].y(), L[i].z());
-    values->insert(i, pt) ;
+    values->insert(PointKey(i), pt) ;
   }
 
   for ( size_t i = 0 ; i < X.size() ; ++i )
@@ -301,13 +297,13 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
 
   const size_t nMeasurements = L.size()*X.size();
 
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i ) {
     const double
       rot_noise = 1e-5, trans_noise = 1e-3,
       focal_noise = 1, distort_noise = 1e-3;
     if ( i == 0 ) {
-      values->insert((int)i, X[i]) ;
+      values->insert(CameraKey((int)i), X[i]) ;
     }
     else {
 
@@ -316,12 +312,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
           trans_noise, trans_noise, trans_noise, // translation
           focal_noise, distort_noise, distort_noise // f, k1, k2
           ) ;
-      values->insert((int)i, X[i].retract(delta)) ;
+      values->insert(CameraKey((int)i), X[i].retract(delta)) ;
     }
   }
 
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
-    values->insert(i, L[i]) ;
+    values->insert(PointKey(i), L[i]) ;
   }
 
   // fix X0 and all landmarks, allow only the X[1] to move
@@ -358,16 +354,16 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
 
   // add initial
   const double noise = baseline*0.1;
-  boost::shared_ptr<VisualValues> values(new VisualValues);
+  boost::shared_ptr<Values> values(new Values);
   for ( size_t i = 0 ; i < X.size() ; ++i )
-    values->insert((int)i, X[i]) ;
+    values->insert(CameraKey((int)i), X[i]) ;
 
   // add noise only to the first landmark
   for ( size_t i = 0 ; i < L.size() ; ++i ) {
     Point3 pt(L[i].x()+noise*getGaussian(),
               L[i].y()+noise*getGaussian(),
               L[i].z()+noise*getGaussian());
-    values->insert(i, pt) ;
+    values->insert(PointKey(i), pt) ;
   }
 
   graph->addCameraConstraint(0, X[0]);

gtsam/slam/tests/testPlanarSLAM.cpp

@@ -23,6 +23,7 @@
 
 using namespace std;
 using namespace gtsam;
+using namespace planarSLAM;
 
 // some shared test values
 static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
@@ -50,9 +51,9 @@ TEST( planarSLAM, BearingFactor )
 	planarSLAM::Bearing factor(2, 3, z, sigma);
 
 	// create config
-	planarSLAM::Values c;
+	Values c;
-	c.insert(2, x2);
+	c.insert(PoseKey(2), x2);
-	c.insert(3, l3);
+	c.insert(PointKey(3), l3);
 
 	// Check error
 	Vector actual = factor.unwhitenedError(c);
@@ -78,9 +79,9 @@ TEST( planarSLAM, RangeFactor )
 	planarSLAM::Range factor(2, 3, z, sigma);
 
 	// create config
-	planarSLAM::Values c;
+	Values c;
-	c.insert(2, x2);
+	c.insert(PoseKey(2), x2);
-	c.insert(3, l3);
+	c.insert(PointKey(3), l3);
 
 	// Check error
 	Vector actual = factor.unwhitenedError(c);
@@ -105,9 +106,9 @@ TEST( planarSLAM, BearingRangeFactor )
 	planarSLAM::BearingRange factor(2, 3, r, b, sigma2);
 
 	// create config
-	planarSLAM::Values c;
+	Values c;
-	c.insert(2, x2);
+	c.insert(PoseKey(2), x2);
-	c.insert(3, l3);
+	c.insert(PointKey(3), l3);
 
 	// Check error
 	Vector actual = factor.unwhitenedError(c);
@@ -138,10 +139,10 @@ TEST( planarSLAM, PoseConstraint_equals )
 TEST( planarSLAM, constructor )
 {
 	// create config
-	planarSLAM::Values c;
+	Values c;
-	c.insert(2, x2);
+	c.insert(PoseKey(2), x2);
-	c.insert(3, x3);
+	c.insert(PoseKey(3), x3);
-	c.insert(3, l3);
+	c.insert(PointKey(3), l3);
 
 	// create graph
 	planarSLAM::Graph G;
@@ -165,8 +166,8 @@ TEST( planarSLAM, constructor )
 	Vector expected2 = Vector_(1, -0.1);
 	Vector expected3 = Vector_(1, 0.22);
 	// Get NoiseModelFactors
-	FactorGraph<NoiseModelFactor<planarSLAM::Values> > GNM =
+	FactorGraph<NoiseModelFactor > GNM =
-	    *G.dynamicCastFactors<FactorGraph<NoiseModelFactor<planarSLAM::Values> > >();
+	    *G.dynamicCastFactors<FactorGraph<NoiseModelFactor > >();
 	EXPECT(assert_equal(expected0, GNM[0]->unwhitenedError(c)));
   EXPECT(assert_equal(expected1, GNM[1]->unwhitenedError(c)));
   EXPECT(assert_equal(expected2, GNM[2]->unwhitenedError(c)));

gtsam/slam/tests/testPose2SLAM.cpp

@@ -118,8 +118,8 @@ TEST( Pose2SLAM, linearization )
 	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) looking at negative (ground truth is at 4.1,2)
 	pose2SLAM::Values config;
-	config.insert(1,p1);
+	config.insert(pose2SLAM::PoseKey(1),p1);
-	config.insert(2,p2);
+	config.insert(pose2SLAM::PoseKey(2),p2);
 	// Linearize
 	Ordering ordering(*config.orderingArbitrary());
 	boost::shared_ptr<FactorGraph<GaussianFactor> > lfg_linearized = graph.linearize(config, ordering);
@@ -153,23 +153,23 @@ TEST(Pose2Graph, optimize) {
   fg->addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance);
 
   // Create initial config
-  boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
+  boost::shared_ptr<Values> initial(new Values());
-  initial->insert(0, Pose2(0,0,0));
+  initial->insert(pose2SLAM::PoseKey(0), Pose2(0,0,0));
-  initial->insert(1, Pose2(0,0,0));
+  initial->insert(pose2SLAM::PoseKey(1), Pose2(0,0,0));
 
   // Choose an ordering and optimize
   shared_ptr<Ordering> ordering(new Ordering);
   *ordering += "x0","x1";
-  typedef NonlinearOptimizer<pose2SLAM::Graph, pose2SLAM::Values> Optimizer;
+  typedef NonlinearOptimizer<pose2SLAM::Graph> Optimizer;
 
   NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15);
   Optimizer optimizer0(fg, initial, ordering, params);
   Optimizer optimizer = optimizer0.levenbergMarquardt();
 
   // Check with expected config
-  pose2SLAM::Values expected;
+  Values expected;
-  expected.insert(0, Pose2(0,0,0));
+  expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0));
-  expected.insert(1, Pose2(1,2,M_PI_2));
+  expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI_2));
   CHECK(assert_equal(expected, *optimizer.values()));
 }
 
@@ -178,8 +178,8 @@ TEST(Pose2Graph, optimize) {
 TEST(Pose2Graph, optimizeThreePoses) {
 
 	// Create a hexagon of poses
-	pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0);
+	Values hexagon = pose2SLAM::circle(3,1.0);
-  Pose2 p0 = hexagon[0], p1 = hexagon[1];
+  Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)];
 
 	// create a Pose graph with one equality constraint and one measurement
   shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
@@ -190,10 +190,10 @@ TEST(Pose2Graph, optimizeThreePoses) {
   fg->addOdometry(2, 0, delta, covariance);
 
   // Create initial config
-  boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
+  boost::shared_ptr<Values> initial(new Values());
-  initial->insert(0, p0);
+  initial->insert(pose2SLAM::PoseKey(0), p0);
-  initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1)));
+  initial->insert(pose2SLAM::PoseKey(1), hexagon[pose2SLAM::PoseKey(1)].retract(Vector_(3,-0.1, 0.1,-0.1)));
-  initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1)));
+  initial->insert(pose2SLAM::PoseKey(2), hexagon[pose2SLAM::PoseKey(2)].retract(Vector_(3, 0.1,-0.1, 0.1)));
 
   // Choose an ordering
   shared_ptr<Ordering> ordering(new Ordering);
@@ -204,7 +204,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
   pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
   pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
 
-  pose2SLAM::Values actual = *optimizer.values();
+  Values actual = *optimizer.values();
 
   // Check with ground truth
   CHECK(assert_equal(hexagon, actual));
@@ -215,8 +215,8 @@ TEST(Pose2Graph, optimizeThreePoses) {
 TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
 
 	// Create a hexagon of poses
-	pose2SLAM::Values hexagon = pose2SLAM::circle(6,1.0);
+	Values hexagon = pose2SLAM::circle(6,1.0);
-  Pose2 p0 = hexagon[0], p1 = hexagon[1];
+  Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)];
 
 	// create a Pose graph with one equality constraint and one measurement
   shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
@@ -230,13 +230,13 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
   fg->addOdometry(5, 0, delta, covariance);
 
   // Create initial config
-  boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
+  boost::shared_ptr<Values> initial(new Values());
-  initial->insert(0, p0);
+  initial->insert(pose2SLAM::PoseKey(0), p0);
-  initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1)));
+  initial->insert(pose2SLAM::PoseKey(1), hexagon[pose2SLAM::PoseKey(1)].retract(Vector_(3,-0.1, 0.1,-0.1)));
-  initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1)));
+  initial->insert(pose2SLAM::PoseKey(2), hexagon[pose2SLAM::PoseKey(2)].retract(Vector_(3, 0.1,-0.1, 0.1)));
-  initial->insert(3, hexagon[3].retract(Vector_(3,-0.1, 0.1,-0.1)));
+  initial->insert(pose2SLAM::PoseKey(3), hexagon[pose2SLAM::PoseKey(3)].retract(Vector_(3,-0.1, 0.1,-0.1)));
-  initial->insert(4, hexagon[4].retract(Vector_(3, 0.1,-0.1, 0.1)));
+  initial->insert(pose2SLAM::PoseKey(4), hexagon[pose2SLAM::PoseKey(4)].retract(Vector_(3, 0.1,-0.1, 0.1)));
-  initial->insert(5, hexagon[5].retract(Vector_(3,-0.1, 0.1,-0.1)));
+  initial->insert(pose2SLAM::PoseKey(5), hexagon[pose2SLAM::PoseKey(5)].retract(Vector_(3,-0.1, 0.1,-0.1)));
 
   // Choose an ordering
   shared_ptr<Ordering> ordering(new Ordering);
@@ -247,13 +247,13 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
   pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
   pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
 
-  pose2SLAM::Values actual = *optimizer.values();
+  Values actual = *optimizer.values();
 
   // Check with ground truth
   CHECK(assert_equal(hexagon, actual));
 
   // Check loop closure
-  CHECK(assert_equal(delta,actual[5].between(actual[0])));
+  CHECK(assert_equal(delta,actual[pose2SLAM::PoseKey(5)].between(actual[pose2SLAM::PoseKey(0)])));
 
 //  Pose2SLAMOptimizer myOptimizer("3");
 
@@ -281,7 +281,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
 //
 //  myOptimizer.update(x);
 //
-//  pose2SLAM::Values expected;
+//  Values expected;
 //  expected.insert(0, Pose2(0.,0.,0.));
 //  expected.insert(1, Pose2(1.,0.,0.));
 //  expected.insert(2, Pose2(2.,0.,0.));
@@ -315,8 +315,8 @@ TEST(Pose2Graph, optimize2) {
 //	G.addConstraint(1, 3, Pose2(0.,0.,0.), I3);
 //	G.addConstraint(2, 3, Pose2(0.,0.,0.), I3);
 //
-//	PredecessorMap<pose2SLAM::Key> tree =
+//	PredecessorMap<pose2SLAM::pose2SLAM::PoseKey> tree =
-//			G.findMinimumSpanningTree<pose2SLAM::Key, Pose2Factor>();
+//			G.findMinimumSpanningTree<pose2SLAM::pose2SLAM::PoseKey, Pose2Factor>();
 //	CHECK(tree[1] == 1);
 //	CHECK(tree[2] == 1);
 //	CHECK(tree[3] == 1);
@@ -329,12 +329,12 @@ TEST(Pose2Graph, optimize2) {
 //	G.addConstraint(1, 3, Pose2(0.,0.,0.), I3);
 //	G.addConstraint(2, 3, Pose2(0.,0.,0.), I3);
 //
-//	PredecessorMap<pose2SLAM::Key> tree;
+//	PredecessorMap<pose2SLAM::pose2SLAM::PoseKey> tree;
 //	tree.insert(1,2);
 //	tree.insert(2,2);
 //	tree.insert(3,2);
 //
-//	G.split<pose2SLAM::Key, Pose2Factor>(tree, T, C);
+//	G.split<pose2SLAM::pose2SLAM::PoseKey, Pose2Factor>(tree, T, C);
 //	LONGS_EQUAL(2, T.size());
 //	LONGS_EQUAL(1, C.size());
 //}
@@ -345,13 +345,13 @@ using namespace pose2SLAM;
 TEST(Pose2Values, pose2Circle )
 {
 	// expected is 4 poses tangent to circle with radius 1m
-	pose2SLAM::Values expected;
+	Values expected;
-	expected.insert(0, Pose2( 1,  0,   M_PI_2));
+	expected.insert(pose2SLAM::PoseKey(0), Pose2( 1,  0,   M_PI_2));
-	expected.insert(1, Pose2( 0,  1, - M_PI  ));
+	expected.insert(pose2SLAM::PoseKey(1), Pose2( 0,  1, - M_PI  ));
-	expected.insert(2, Pose2(-1,  0, - M_PI_2));
+	expected.insert(pose2SLAM::PoseKey(2), Pose2(-1,  0, - M_PI_2));
-	expected.insert(3, Pose2( 0, -1,   0     ));
+	expected.insert(pose2SLAM::PoseKey(3), Pose2( 0, -1,   0     ));
 
-	pose2SLAM::Values actual = pose2SLAM::circle(4,1.0);
+	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
-	pose2SLAM::Values expected;
+	Values expected;
-	expected.insert(0, Pose2( 1.1,  0,   M_PI_2));
+	expected.insert(pose2SLAM::PoseKey(0), Pose2( 1.1,  0,   M_PI_2));
-	expected.insert(1, Pose2( 0.1,  1, - M_PI  ));
+	expected.insert(pose2SLAM::PoseKey(1), Pose2( 0.1,  1, - M_PI  ));
-	expected.insert(2, Pose2(-0.9,  0, - M_PI_2));
+	expected.insert(pose2SLAM::PoseKey(2), Pose2(-0.9,  0, - M_PI_2));
-	expected.insert(3, Pose2( 0.1, -1,   0     ));
+	expected.insert(pose2SLAM::PoseKey(3), Pose2( 0.1, -1,   0     ));
 
 	// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
-  pose2SLAM::Values circle(pose2SLAM::circle(4,1.0));
+  Values circle(pose2SLAM::circle(4,1.0));
   Ordering ordering(*circle.orderingArbitrary());
 	VectorValues delta(circle.dims(ordering));
-	delta[ordering[PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0);
+	delta[ordering[pose2SLAM::PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0);
-	delta[ordering[PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0);
+	delta[ordering[pose2SLAM::PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0);
-	delta[ordering[PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0);
+	delta[ordering[pose2SLAM::PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0);
-	delta[ordering[PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0);
+	delta[ordering[pose2SLAM::PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0);
-	pose2SLAM::Values actual = circle.retract(delta, ordering);
+	Values actual = circle.retract(delta, ordering);
 	CHECK(assert_equal(expected,actual));
 }
 
@@ -386,8 +386,8 @@ TEST( Pose2Prior, error )
 {
 	// Choose a linearization point
 	Pose2 p1(1, 0, 0); // robot at (1,0)
-	pose2SLAM::Values x0;
+	Values x0;
-	x0.insert(1, p1);
+	x0.insert(pose2SLAM::PoseKey(1), p1);
 
 	// Create factor
 	pose2SLAM::Prior factor(1, p1, sigmas);
@@ -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;
-	pose2SLAM::Values x1 = x0.retract(plus, ordering);
+	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,8 +430,8 @@ LieVector hprior(const Pose2& p1) {
 TEST( Pose2Prior, linearize )
 {
 	// Choose a linearization point at ground truth
-	pose2SLAM::Values x0;
+	Values x0;
-	x0.insert(1,priorVal);
+	x0.insert(pose2SLAM::PoseKey(1),priorVal);
 
 	// Actual linearization
 	Ordering ordering(*x0.orderingArbitrary());
@@ -450,9 +450,9 @@ TEST( Pose2Factor, error )
 	// Choose a linearization point
 	Pose2 p1; // robot at origin
 	Pose2 p2(1, 0, 0); // robot at (1,0)
-	pose2SLAM::Values x0;
+	Values x0;
-	x0.insert(1, p1);
+	x0.insert(pose2SLAM::PoseKey(1), p1);
-	x0.insert(2, p2);
+	x0.insert(pose2SLAM::PoseKey(2), p2);
 
 	// Create factor
 	Pose2 z = p1.between(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);
-	pose2SLAM::Values x1 = x0.retract(plus, ordering);
+	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,9 +491,9 @@ 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)
-	pose2SLAM::Values x0;
+	Values x0;
-	x0.insert(1,p1);
+	x0.insert(pose2SLAM::PoseKey(1),p1);
-	x0.insert(2,p2);
+	x0.insert(pose2SLAM::PoseKey(2),p2);
 
 	// Actual linearization
 	Ordering ordering(*x0.orderingArbitrary());
@@ -521,9 +521,9 @@ TEST( Pose2Factor, linearize )
 	// Choose a linearization point at ground truth
 	Pose2 p1(1,2,M_PI_2);
 	Pose2 p2(-1,4,M_PI);
-	pose2SLAM::Values x0;
+	Values x0;
-	x0.insert(1,p1);
+	x0.insert(pose2SLAM::PoseKey(1),p1);
-	x0.insert(2,p2);
+	x0.insert(pose2SLAM::PoseKey(2),p2);
 
 	// expected linearization
 	Matrix expectedH1 = covariance->Whiten(Matrix_(3,3,

gtsam/slam/tests/testPose3SLAM.cpp

@@ -36,6 +36,7 @@ using namespace boost::assign;
 
 using namespace std;
 using namespace gtsam;
+using namespace pose3SLAM;
 
 // common measurement covariance
 static Matrix covariance = eye(6);
@@ -48,8 +49,8 @@ TEST(Pose3Graph, optimizeCircle) {
 
 	// Create a hexagon of poses
 	double radius = 10;
-	Pose3Values hexagon = pose3SLAM::circle(6,radius);
+	Values hexagon = pose3SLAM::circle(6,radius);
-  Pose3 gT0 = hexagon[0], gT1 = hexagon[1];
+  Pose3 gT0 = hexagon[Key(0)], gT1 = hexagon[Key(1)];
 
 	// create a Pose graph with one equality constraint and one measurement
   shared_ptr<Pose3Graph> fg(new Pose3Graph);
@@ -65,13 +66,13 @@ TEST(Pose3Graph, optimizeCircle) {
   fg->addConstraint(5,0, _0T1, covariance);
 
   // Create initial config
-  boost::shared_ptr<Pose3Values> initial(new Pose3Values());
+  boost::shared_ptr<Values> initial(new Values());
-  initial->insert(0, gT0);
+  initial->insert(Key(0), gT0);
-  initial->insert(1, hexagon[1].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
+  initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
-  initial->insert(2, hexagon[2].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
+  initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
-  initial->insert(3, hexagon[3].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
+  initial->insert(Key(3), hexagon[Key(3)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
-  initial->insert(4, hexagon[4].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
+  initial->insert(Key(4), hexagon[Key(4)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
-  initial->insert(5, hexagon[5].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
+  initial->insert(Key(5), hexagon[Key(5)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
 
   // Choose an ordering and optimize
   shared_ptr<Ordering> ordering(new Ordering);
@@ -80,18 +81,18 @@ TEST(Pose3Graph, optimizeCircle) {
   pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params);
   pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
 
-  Pose3Values actual = *optimizer.values();
+  Values actual = *optimizer.values();
 
   // Check with ground truth
   CHECK(assert_equal(hexagon, actual,1e-4));
 
   // Check loop closure
-  CHECK(assert_equal(_0T1,actual[5].between(actual[0]),1e-5));
+  CHECK(assert_equal(_0T1,actual[Key(5)].between(actual[Key(0)]),1e-5));
 }
 
 /* ************************************************************************* */
 TEST(Pose3Graph, partial_prior_height) {
-	typedef PartialPriorFactor<pose3SLAM::Values, pose3SLAM::Key> Partial;
+	typedef PartialPriorFactor<pose3SLAM::Key> Partial;
 
 	// reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3)
 
@@ -109,13 +110,13 @@ TEST(Pose3Graph, partial_prior_height) {
 	pose3SLAM::Graph graph;
 	graph.add(height);
 
-	pose3SLAM::Values values;
+	Values values;
 	values.insert(key, init);
 
 	// linearization
 	EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
 
-	pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
+	Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
 	EXPECT(assert_equal(expected, actual[key], tol));
 	EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
 }
@@ -133,9 +134,9 @@ TEST( Pose3Factor, error )
 	Pose3Factor factor(1,2, z, I6);
 
 	// Create config
-	Pose3Values x;
+	Values x;
-	x.insert(1,t1);
+	x.insert(Key(1),t1);
-	x.insert(2,t2);
+	x.insert(Key(2),t2);
 
 	// Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2))
 	Vector actual = factor.unwhitenedError(x);
@@ -145,7 +146,7 @@ TEST( Pose3Factor, error )
 
 /* ************************************************************************* */
 TEST(Pose3Graph, partial_prior_xy) {
-	typedef PartialPriorFactor<pose3SLAM::Values, pose3SLAM::Key> Partial;
+	typedef PartialPriorFactor<pose3SLAM::Key> Partial;
 
 	// XY prior - full mask interface
 	pose3SLAM::Key key(1);
@@ -164,10 +165,10 @@ TEST(Pose3Graph, partial_prior_xy) {
 	pose3SLAM::Graph graph;
 	graph.add(priorXY);
 
-	pose3SLAM::Values values;
+	Values values;
 	values.insert(key, init);
 
-	pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
+	Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
 	EXPECT(assert_equal(expected, actual[key], tol));
 	EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
 }
@@ -180,27 +181,27 @@ Rot3 R3(Point3( 0,-1, 0), Point3(-1, 0, 0), Point3(0, 0, -1));
 Rot3 R4(Point3( 1, 0, 0), Point3( 0,-1, 0), Point3(0, 0, -1));
 
 /* ************************************************************************* */
-TEST( Pose3Values, pose3Circle )
+TEST( Values, pose3Circle )
 {
 	// expected is 4 poses tangent to circle with radius 1m
-	Pose3Values expected;
+	Values expected;
-	expected.insert(0, Pose3(R1, Point3( 1, 0, 0)));
+	expected.insert(Key(0), Pose3(R1, Point3( 1, 0, 0)));
-	expected.insert(1, Pose3(R2, Point3( 0, 1, 0)));
+	expected.insert(Key(1), Pose3(R2, Point3( 0, 1, 0)));
-	expected.insert(2, Pose3(R3, Point3(-1, 0, 0)));
+	expected.insert(Key(2), Pose3(R3, Point3(-1, 0, 0)));
-	expected.insert(3, Pose3(R4, Point3( 0,-1, 0)));
+	expected.insert(Key(3), Pose3(R4, Point3( 0,-1, 0)));
 
-	Pose3Values actual = pose3SLAM::circle(4,1.0);
+	Values actual = pose3SLAM::circle(4,1.0);
 	CHECK(assert_equal(expected,actual));
 }
 
 /* ************************************************************************* */
-TEST( Pose3Values, expmap )
+TEST( Values, expmap )
 {
-	Pose3Values expected;
+	Values expected;
-	expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0)));
+	expected.insert(Key(0), Pose3(R1, Point3( 1.0, 0.1, 0)));
-	expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0)));
+	expected.insert(Key(1), Pose3(R2, Point3(-0.1, 1.0, 0)));
-	expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0)));
+	expected.insert(Key(2), Pose3(R3, Point3(-1.0,-0.1, 0)));
-	expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0)));
+	expected.insert(Key(3), Pose3(R4, Point3( 0.1,-1.0, 0)));
 
 	Ordering ordering(*expected.orderingArbitrary());
 	VectorValues delta(expected.dims(ordering));
@@ -209,7 +210,7 @@ TEST( Pose3Values, expmap )
 			0.0,0.0,0.0,  0.1, 0.0, 0.0,
 			0.0,0.0,0.0,  0.1, 0.0, 0.0,
 			0.0,0.0,0.0,  0.1, 0.0, 0.0);
-	Pose3Values actual = pose3SLAM::circle(4,1.0).retract(delta, ordering);
+	Values actual = pose3SLAM::circle(4,1.0).retract(delta, ordering);
 	CHECK(assert_equal(expected,actual));
 }
 

gtsam/slam/tests/testProjectionFactor.cpp

@@ -25,6 +25,7 @@
 
 using namespace std;
 using namespace gtsam;
+using namespace visualSLAM;
 
 // make cube
 static Point3
@@ -51,9 +52,9 @@ TEST( ProjectionFactor, error )
 	factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
 
 	// For the following values structure, the factor predicts 320,240
-	visualSLAM::Values config;
+	Values config;
-	Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1);
+	Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(PoseKey(1), x1);
-	Point3 l1;  config.insert(1, l1);
+	Point3 l1;  config.insert(PointKey(1), l1);
 	// Point should project to Point2(320.,240.)
 	CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(config)));
 
@@ -80,13 +81,13 @@ TEST( ProjectionFactor, error )
 	CHECK(assert_equal(expected_lfg,*actual_lfg));
 
 	// expmap on a config
-	visualSLAM::Values expected_config;
+	Values expected_config;
-  Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2);
+  Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(PoseKey(1), x2);
-  Point3 l2(1,2,3); expected_config.insert(1, l2);
+  Point3 l2(1,2,3); expected_config.insert(PointKey(1), l2);
 	VectorValues delta(expected_config.dims(ordering));
 	delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
 	delta[ordering["l1"]] = Vector_(3, 1.,2.,3.);
-	visualSLAM::Values actual_config = config.retract(delta, ordering);
+	Values actual_config = config.retract(delta, ordering);
 	CHECK(assert_equal(expected_config,actual_config,1e-9));
 }
 

gtsam/slam/tests/testSimulated2D.cpp

@@ -30,9 +30,9 @@ using namespace simulated2D;
 /* ************************************************************************* */
 TEST( simulated2D, Simulated2DValues )
 {
-	simulated2D::Values actual;
+	Values actual;
-	actual.insertPose(1,Point2(1,1));
+	actual.insert(simulated2D::PoseKey(1),Point2(1,1));
-	actual.insertPoint(2,Point2(2,2));
+	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));
 
-	simulated2DOriented::Values config;
+	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());

gtsam/slam/tests/testSimulated3D.cpp

@@ -29,7 +29,7 @@ using namespace simulated3D;
 /* ************************************************************************* */
 TEST( simulated3D, Values )
 {
-	simulated3D::Values actual;
+	Values actual;
 	actual.insert(simulated3D::PointKey(1),Point3(1,1,1));
 	actual.insert(simulated3D::PoseKey(2),Point3(2,2,2));
 	EXPECT(assert_equal(actual,actual,1e-9));

gtsam/slam/tests/testStereoFactor.cpp

@@ -29,6 +29,7 @@
 using namespace std;
 using namespace gtsam;
 using namespace boost;
+using namespace visualSLAM;
 
 Pose3 camera1(Matrix_(3,3,
 		       1., 0., 0.,
@@ -58,15 +59,15 @@ TEST( StereoFactor, singlePoint)
 	graph->add(visualSLAM::StereoFactor(z14,sigma, 1, 1, K));
 
 	// Create a configuration corresponding to the ground truth
-	boost::shared_ptr<visualSLAM::Values> values(new visualSLAM::Values());
+	boost::shared_ptr<Values> values(new Values());
-	values->insert(1, camera1); // add camera at z=6.25m looking towards origin
+	values->insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin
 
 	Point3 l1(0, 0, 0);
-	values->insert(1, l1);   // add point at origin;
+	values->insert(PointKey(1), l1);   // add point at origin;
 
 	Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
 
-	typedef gtsam::NonlinearOptimizer<visualSLAM::Graph,visualSLAM::Values,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> Optimizer;   // optimization engine for this domain
+	typedef gtsam::NonlinearOptimizer<visualSLAM::Graph, gtsam::GaussianFactorGraph, gtsam::GaussianMultifrontalSolver> Optimizer;   // optimization engine for this domain
 
 	double absoluteThreshold = 1e-9;
 	double relativeThreshold = 1e-5;

gtsam/slam/tests/testVSLAM.cpp

@@ -32,6 +32,7 @@ using namespace boost;
 using namespace std;
 using namespace gtsam;
 using namespace boost;
+using namespace visualSLAM;
 
 static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
 
@@ -90,13 +91,13 @@ TEST( Graph, optimizeLM)
   graph->addPointConstraint(3, landmark3);
 
   // Create an initial values structure corresponding to the ground truth
-  boost::shared_ptr<visualSLAM::Values> initialEstimate(new visualSLAM::Values);
+  boost::shared_ptr<Values> initialEstimate(new Values);
-  initialEstimate->insert(1, camera1);
+  initialEstimate->insert(PoseKey(1), camera1);
-  initialEstimate->insert(2, camera2);
+  initialEstimate->insert(PoseKey(2), camera2);
-  initialEstimate->insert(1, landmark1);
+  initialEstimate->insert(PointKey(1), landmark1);
-  initialEstimate->insert(2, landmark2);
+  initialEstimate->insert(PointKey(2), landmark2);
-  initialEstimate->insert(3, landmark3);
+  initialEstimate->insert(PointKey(3), landmark3);
-  initialEstimate->insert(4, landmark4);
+  initialEstimate->insert(PointKey(4), landmark4);
 
   // Create an ordering of the variables
   shared_ptr<Ordering> ordering(new Ordering);
@@ -127,13 +128,13 @@ TEST( Graph, optimizeLM2)
   graph->addPoseConstraint(2, camera2);
 
   // Create an initial values structure corresponding to the ground truth
-  boost::shared_ptr<visualSLAM::Values> initialEstimate(new visualSLAM::Values);
+  boost::shared_ptr<Values> initialEstimate(new Values);
-  initialEstimate->insert(1, camera1);
+  initialEstimate->insert(PoseKey(1), camera1);
-  initialEstimate->insert(2, camera2);
+  initialEstimate->insert(PoseKey(2), camera2);
-  initialEstimate->insert(1, landmark1);
+  initialEstimate->insert(PointKey(1), landmark1);
-  initialEstimate->insert(2, landmark2);
+  initialEstimate->insert(PointKey(2), landmark2);
-  initialEstimate->insert(3, landmark3);
+  initialEstimate->insert(PointKey(3), landmark3);
-  initialEstimate->insert(4, landmark4);
+  initialEstimate->insert(PointKey(4), landmark4);
 
   // Create an ordering of the variables
   shared_ptr<Ordering> ordering(new Ordering);
@@ -164,13 +165,13 @@ TEST( Graph, CHECK_ORDERING)
   graph->addPoseConstraint(2, camera2);
 
   // Create an initial values structure corresponding to the ground truth
-  boost::shared_ptr<visualSLAM::Values> initialEstimate(new visualSLAM::Values);
+  boost::shared_ptr<Values> initialEstimate(new Values);
-  initialEstimate->insert(1, camera1);
+  initialEstimate->insert(PoseKey(1), camera1);
-  initialEstimate->insert(2, camera2);
+  initialEstimate->insert(PoseKey(2), camera2);
-  initialEstimate->insert(1, landmark1);
+  initialEstimate->insert(PointKey(1), landmark1);
-  initialEstimate->insert(2, landmark2);
+  initialEstimate->insert(PointKey(2), landmark2);
-  initialEstimate->insert(3, landmark3);
+  initialEstimate->insert(PointKey(3), landmark3);
-  initialEstimate->insert(4, landmark4);
+  initialEstimate->insert(PointKey(4), landmark4);
 
   Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initialEstimate);
 
@@ -192,23 +193,23 @@ TEST( Graph, CHECK_ORDERING)
 TEST( Values, update_with_large_delta) {
 	// this test ensures that if the update for delta is larger than
 	// the size of the config, it only updates existing variables
-	visualSLAM::Values init;
+	Values init;
-	init.insert(1, Pose3());
+	init.insert(PoseKey(1), Pose3());
-	init.insert(1, Point3(1.0, 2.0, 3.0));
+	init.insert(PointKey(1), Point3(1.0, 2.0, 3.0));
 
-	visualSLAM::Values expected;
+	Values expected;
-	expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
+	expected.insert(PoseKey(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
-	expected.insert(1, Point3(1.1, 2.1, 3.1));
+	expected.insert(PointKey(1), Point3(1.1, 2.1, 3.1));
 
 	Ordering largeOrdering;
-	visualSLAM::Values largeValues = init;
+	Values largeValues = init;
-	largeValues.insert(2, Pose3());
+	largeValues.insert(PoseKey(2), Pose3());
 	largeOrdering += "x1","l1","x2";
 	VectorValues delta(largeValues.dims(largeOrdering));
 	delta[largeOrdering["x1"]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1);
 	delta[largeOrdering["l1"]] = Vector_(3, 0.1, 0.1, 0.1);
 	delta[largeOrdering["x2"]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1);
-	visualSLAM::Values actual = init.retract(delta, largeOrdering);
+	Values actual = init.retract(delta, largeOrdering);
 
 	CHECK(assert_equal(expected,actual));
 }

gtsam/slam/visualSLAM.h

@@ -25,7 +25,6 @@
 #include <gtsam/slam/RangeFactor.h>
 #include <gtsam/nonlinear/Key.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/geometry/SimpleCamera.h>
@@ -40,25 +39,22 @@ namespace visualSLAM {
    */
   typedef TypedSymbol<Pose3,'x'> PoseKey;									///< The key type used for poses
   typedef TypedSymbol<Point3,'l'> PointKey;								///< The key type used for points
-  typedef Values<PoseKey> PoseValues;									///< Values used for poses
-  typedef Values<PointKey> PointValues;								///< Values used for points
-  typedef TupleValues2<PoseValues, PointValues> Values;		///< Values data structure
   typedef boost::shared_ptr<Values> shared_values;				///< shared pointer to values data structure
 
-  typedef NonlinearEquality<Values, PoseKey> PoseConstraint;	 ///< put a hard constraint on a pose
+  typedef NonlinearEquality<PoseKey> PoseConstraint;	 ///< put a hard constraint on a pose
-  typedef NonlinearEquality<Values, PointKey> PointConstraint; ///< put a hard constraint on a point
+  typedef NonlinearEquality<PointKey> PointConstraint; ///< put a hard constraint on a point
-  typedef PriorFactor<Values, PoseKey> PosePrior;							 ///< put a soft prior on a Pose
+  typedef PriorFactor<PoseKey> PosePrior;							 ///< put a soft prior on a Pose
-  typedef PriorFactor<Values, PointKey> PointPrior;						 ///< put a soft prior on a point
+  typedef PriorFactor<PointKey> PointPrior;						 ///< put a soft prior on a point
-  typedef RangeFactor<Values, PoseKey, PointKey> RangeFactor;  ///< put a factor on the range from a pose to a point
+  typedef RangeFactor<PoseKey, PointKey> RangeFactor;  ///< put a factor on the range from a pose to a point
   
   /// monocular and stereo camera typedefs for general use
-  typedef GenericProjectionFactor<Values, PointKey, PoseKey> ProjectionFactor;
+  typedef GenericProjectionFactor<PointKey, PoseKey> ProjectionFactor;
-  typedef GenericStereoFactor<Values, PoseKey, PointKey> StereoFactor;
+  typedef GenericStereoFactor<PoseKey, PointKey> StereoFactor;
 
   /**
    * Non-linear factor graph for vanilla visual SLAM
    */
-  class Graph: public NonlinearFactorGraph<Values> {
+  class Graph: public NonlinearFactorGraph {
 
   public:
   	/// shared pointer to this type of graph
@@ -70,12 +66,12 @@ namespace visualSLAM {
 
     /// print out graph
     void print(const std::string& s = "") const {
-      NonlinearFactorGraph<Values>::print(s);
+      NonlinearFactorGraph::print(s);
     }
 
     /// check if two graphs are equal
     bool equals(const Graph& p, double tol = 1e-9) const {
-      return NonlinearFactorGraph<Values>::equals(p, tol);
+      return NonlinearFactorGraph::equals(p, tol);
     }
 
     /**
@@ -148,6 +144,6 @@ namespace visualSLAM {
   }; // Graph
 
   /// typedef for Optimizer. The current default will use the multi-frontal solver
-  typedef NonlinearOptimizer<Graph, Values> Optimizer;
+  typedef NonlinearOptimizer<Graph> Optimizer;
 
 } // namespaces

tests/Makefile.am

@@ -16,10 +16,10 @@ check_PROGRAMS += testGaussianJunctionTree
 check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph
 check_PROGRAMS += testNonlinearOptimizer testDoglegOptimizer
 check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph
-check_PROGRAMS += testTupleValues
+#check_PROGRAMS += testTupleValues
 check_PROGRAMS += testNonlinearISAM
 check_PROGRAMS += testBoundingConstraint
-check_PROGRAMS += testPose2SLAMwSPCG
+#check_PROGRAMS += testPose2SLAMwSPCG
 check_PROGRAMS += testGaussianISAM2
 check_PROGRAMS += testExtendedKalmanFilter
 check_PROGRAMS += testRot3Optimization