Merge branch 'master' into new_wrap_local

Conflicts:
	.cproject
	gtsam.h

.cproject

@@ -1484,6 +1484,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testSimpleCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testSimpleCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>

gtsam.h

@@ -671,7 +671,7 @@ class VariableIndex {
   size_t size() const;
   size_t nFactors() const;
   size_t nEntries() const;
-  void permute(const gtsam::Permutation& permutation);
+  void permuteInPlace(const gtsam::Permutation& permutation);
 };*/
 
 //*************************************************************************

gtsam/discrete/DecisionTreeFactor.cpp

@@ -49,10 +49,11 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	void DecisionTreeFactor::print(const string& s) const {
+	void DecisionTreeFactor::print(const string& s,
+			const IndexFormatter& formatter) const {
 		cout << s;
-		IndexFactor::print("IndexFactor:");
+		IndexFactor::print("IndexFactor:",formatter);
-		Potentials::print("Potentials:");
+		Potentials::print("Potentials:",formatter);
 	}
 
 	/* ************************************************************************* */

gtsam/discrete/DecisionTreeFactor.h

@@ -72,7 +72,8 @@ namespace gtsam {
 		bool equals(const DecisionTreeFactor& other, double tol = 1e-9) const;
 
 		// print
-		void print(const std::string& s = "DecisionTreeFactor:\n") const;
+		virtual void print(const std::string& s = "DecisionTreeFactor:\n",
+				const IndexFormatter& formatter = DefaultIndexFormatter) const;
 
 		/// @}
 		/// @name Standard Interface

gtsam/discrete/Potentials.cpp

@@ -51,10 +51,11 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	void Potentials::print(const string&s) const {
+	void Potentials::print(const string& s,
+			const IndexFormatter& formatter) const {
 		cout << s << "\n  Cardinalities: ";
 		BOOST_FOREACH(const DiscreteKey& key, cardinalities_)
-			cout << key.first << "=" << key.second << " ";
+			cout << formatter(key.first) << "=" << formatter(key.second) << " ";
 		cout << endl;
 		ADT::print(" ");
 	}

gtsam/discrete/Potentials.h

@@ -65,7 +65,8 @@ namespace gtsam {
 
 		// Testable
 		bool equals(const Potentials& other, double tol = 1e-9) const;
-		void print(const std::string& s = "Potentials: ") const;
+		void print(const std::string& s = "Potentials: ",
+				const IndexFormatter& formatter = DefaultIndexFormatter) const;
 
 		size_t cardinality(Index j) const { return cardinalities_.at(j);}
 

gtsam/geometry/SimpleCamera.cpp

@@ -0,0 +1,49 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 SimpleCamera.cpp
+ * @brief A simple camera class with a Cal3_S2 calibration
+ * @date June 30, 2012
+ * @author Frank Dellaert
+ */
+
+#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
+
+namespace gtsam {
+
+	SimpleCamera simpleCamera(const Matrix& P) {
+
+		// P = [A|a] = s K cRw [I|-T], with s the unknown scale
+		Matrix A = P.topLeftCorner(3, 3);
+		Vector a = P.col(3);
+
+		// do RQ decomposition to get s*K and cRw angles
+		Matrix sK;
+		Vector xyz;
+		boost::tie(sK, xyz) = RQ(A);
+
+		// Recover scale factor s and K
+		double s = sK(2, 2);
+		Matrix K = sK / s;
+
+		// Recover cRw itself, and its inverse
+		Rot3 cRw = Rot3::RzRyRx(xyz);
+		Rot3 wRc = cRw.inverse();
+
+		// Now, recover T from a = - s K cRw T = - A T
+		Vector T = -(A.inverse() * a);
+		return SimpleCamera(Pose3(wRc, T),
+				Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)));
+	}
+
+}

gtsam/geometry/SimpleCamera.h

@@ -22,5 +22,10 @@
 #include <gtsam/geometry/Cal3_S2.h>
 
 namespace gtsam {
+
+	/// A simple camera class with a Cal3_S2 calibration
   typedef PinholeCamera<Cal3_S2> SimpleCamera;
+
+  /// Recover camera from 3*4 camera matrix
+	SimpleCamera simpleCamera(const Matrix& P);
 }

gtsam/geometry/tests/testCal3_S2.cpp

@@ -24,8 +24,8 @@ using namespace gtsam;
 GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2)
 GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2)
 
-Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2);
+static Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2);
-Point2 p(1, -2);
+static Point2 p(1, -2);
 
 /* ************************************************************************* */
 TEST( Cal3_S2, easy_constructor)

gtsam/geometry/tests/testSerializationGeometry.cpp

@@ -36,37 +36,20 @@ using namespace gtsam;
 using namespace gtsam::serializationTestHelpers;
 
 /* ************************************************************************* */
-// Export all classes derived from Value
+static Point3 pt3(1.0, 2.0, 3.0);
-BOOST_CLASS_EXPORT(gtsam::Cal3_S2)
+static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
-BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo)
+static Pose3 pose3(rt3, pt3);
-BOOST_CLASS_EXPORT(gtsam::Cal3Bundler)
-BOOST_CLASS_EXPORT(gtsam::CalibratedCamera)
-BOOST_CLASS_EXPORT(gtsam::Point2)
-BOOST_CLASS_EXPORT(gtsam::Point3)
-BOOST_CLASS_EXPORT(gtsam::Pose2)
-BOOST_CLASS_EXPORT(gtsam::Pose3)
-BOOST_CLASS_EXPORT(gtsam::Rot2)
-BOOST_CLASS_EXPORT(gtsam::Rot3)
-BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
-BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
-BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
-BOOST_CLASS_EXPORT(gtsam::StereoPoint2)
 
-/* ************************************************************************* */
+static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
-Point3 pt3(1.0, 2.0, 3.0);
+static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
-Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
+static Cal3Bundler cal3(1.0, 2.0, 3.0);
-Pose3 pose3(rt3, pt3);
+static Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
+static Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
+static CalibratedCamera cal5(Pose3(rt3, pt3));
 
-Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
+static PinholeCamera<Cal3_S2> cam1(pose3, cal1);
-Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
+static StereoCamera cam2(pose3, cal4ptr);
-Cal3Bundler cal3(1.0, 2.0, 3.0);
+static StereoPoint2 spt(1.0, 2.0, 3.0);
-Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
-Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
-CalibratedCamera cal5(Pose3(rt3, pt3));
-
-PinholeCamera<Cal3_S2> cam1(pose3, cal1);
-StereoCamera cam2(pose3, cal4ptr);
-StereoPoint2 spt(1.0, 2.0, 3.0);
 
 /* ************************************************************************* */
 TEST (Serialization, text_geometry) {

gtsam/geometry/tests/testSimpleCamera.cpp

@@ -133,6 +133,26 @@ TEST( SimpleCamera, Dproject_point_pose)
 	CHECK(assert_equal(Dpoint, numerical_point,1e-7));
 }
 
+/* ************************************************************************* */
+TEST( SimpleCamera, simpleCamera)
+{
+	Cal3_S2 K(468.2,427.2,91.2,300,200);
+	Rot3 R(
+			0.41380,0.90915,0.04708,
+			-0.57338,0.22011,0.78917,
+			0.70711,-0.35355,0.61237);
+	Point3 T(1000,2000,1500);
+	SimpleCamera expected(Pose3(R.inverse(),T),K);
+	// H&Z example, 2nd edition, page 163
+	Matrix P = Matrix_(3,4,
+			3.53553e2, 3.39645e2, 2.77744e2, -1.44946e6,
+			-1.03528e2, 2.33212e1, 4.59607e2, -6.32525e5,
+			7.07107e-1, -3.53553e-1,6.12372e-1, -9.18559e2);
+	SimpleCamera actual = simpleCamera(P);
+	// Note precision of numbers given in book
+	CHECK(assert_equal(expected, actual,1e-1));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/geometry/tests/testStereoCamera.cpp

@@ -74,21 +74,21 @@ TEST( StereoCamera, project)
 
 /* ************************************************************************* */
 
-Pose3 camera1(Matrix_(3,3,
+static Pose3 camera1(Matrix_(3,3,
 		       1., 0., 0.,
 		       0.,-1., 0.,
 		       0., 0.,-1.
 		       ),
 	      Point3(0,0,6.25));
 
-Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
+static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
-StereoCamera stereoCam(Pose3(), K);
+static StereoCamera stereoCam(Pose3(), K);
 
 // point X Y Z in meters
-Point3 p(0, 0, 5);
+static Point3 p(0, 0, 5);
 
 /* ************************************************************************* */
-StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); }
+static StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); }
 TEST( StereoCamera, Dproject_stereo_pose)
 {
 	Matrix expected = numericalDerivative21<StereoPoint2,StereoCamera,Point3>(project_,stereoCam, p);

gtsam/inference/Permutation.h

@@ -26,7 +26,7 @@
 
 namespace gtsam {
 
-/**
+	/**
  * A permutation reorders variables, for example to reduce fill-in during
  * elimination.  To save computation, the permutation can be applied to
  * the necessary data structures only once, then multiple computations
@@ -151,8 +151,8 @@ public:
    */
   Permutation::shared_ptr inverse() const;
 
-  const_iterator begin() const { return rangeIndices_.begin(); }	///<TODO: comment
+  const_iterator begin() const { return rangeIndices_.begin(); }	///< Iterate through the indices
-  const_iterator end() const { return rangeIndices_.end(); }			///<TODO: comment
+  const_iterator end() const { return rangeIndices_.end(); }			///< Iterate through the indices
 
 
 	/// @}
@@ -167,8 +167,8 @@ public:
    */
   Permutation::shared_ptr partialPermutation(const Permutation& selector, const Permutation& partialPermutation) const;
 
-  iterator begin() { return rangeIndices_.begin(); }	///<TODO: comment
+  iterator begin() { return rangeIndices_.begin(); }	///< Iterate through the indices
-  iterator end() { return rangeIndices_.end(); }			///<TODO: comment
+  iterator end() { return rangeIndices_.end(); }			///< Iterate through the indices
 
 protected:
   void check(Index variable) const { assert(variable < rangeIndices_.size()); }
@@ -176,77 +176,4 @@ protected:
 	/// @}
 };
 
-
-/**
- * Syntactic sugar for accessing another container through a permutation.
- * Allows the syntax:
- *   Permuted<Container> permuted(permutation, container);
- *   permuted[index1];
- *   permuted[index2];
- * which is equivalent to:
- *   container[permutation[index1]];
- *   container[permutation[index2]];
- * but more concise.
- */
-template<typename CONTAINER>
-class Permuted {
-  Permutation permutation_;
-  CONTAINER& container_;
-public:
-  typedef typename CONTAINER::iterator::value_type value_type;
-
-  /** Construct as a permuted view on the Container.  The permutation is copied
-   * but only a reference to the container is stored.
-   */
-  Permuted(const Permutation& permutation, CONTAINER& container) : permutation_(permutation), container_(container) {}
-
-  /** Construct as a view on the Container with an identity permutation.  Only
-   * a reference to the container is stored.
-   */
-  Permuted(CONTAINER& container) : permutation_(Permutation::Identity(container.size())), container_(container) {}
-
-  /** Print */
-  void print(const std::string& str = "") const {
-    std::cout << str;
-    permutation_.print("  permutation: ");
-    container_.print("  container: ");
-  }
-
-  /** Access the container through the permutation */
-  value_type& operator[](size_t index) { return container_[permutation_[index]]; }
-
-  /** Access the container through the permutation (const version) */
-  const value_type& operator[](size_t index) const { return container_[permutation_[index]]; }
-
-	/** Assignment operator for cloning in ISAM2 */
-  Permuted<CONTAINER> operator=(const Permuted<CONTAINER>& other) {
-    permutation_ = other.permutation_;
-    container_ = other.container_;
-    return *this;
-  }
-
-  /** Permute this view by applying a permutation to the underlying permutation */
-  void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); }
-
-  /** Access the underlying container */
-  CONTAINER* operator->() { return &container_; }
-
-  /** Access the underlying container (const version) */
-  const CONTAINER* operator->() const { return &container_; }
-
-  /** Size of the underlying container */
-  size_t size() const { return container_.size(); }
-
-  /** Access to the underlying container */
-  CONTAINER& container() { return container_; }
-
-  /** Access to the underlying container (const version) */
-  const CONTAINER& container() const { return container_; }
-
-  /** Access the underlying permutation */
-  Permutation& permutation() { return permutation_; }
-  const Permutation& permutation() const { return permutation_; }
-};
-
-
 }

gtsam/inference/SymbolicFactorGraph.cpp

@@ -85,8 +85,8 @@ namespace gtsam {
 				"IndexFactor::CombineAndEliminate called on factors with no variables.");
 
 		vector<Index> newKeys(keys.begin(), keys.end());
-		return make_pair(new IndexConditional(newKeys, nrFrontals),
+		return make_pair(boost::make_shared<IndexConditional>(newKeys, nrFrontals),
-				new IndexFactor(newKeys.begin() + nrFrontals, newKeys.end()));
+				boost::make_shared<IndexFactor>(newKeys.begin() + nrFrontals, newKeys.end()));
 	}
 
 	/* ************************************************************************* */

gtsam/inference/VariableIndex.cpp

@@ -18,37 +18,12 @@
 #include <iostream>
 
 #include <gtsam/inference/VariableIndex.h>
+#include <gtsam/inference/Permutation.h>
 
 namespace gtsam {
 
 using namespace std;
 
-/* ************************************************************************* */
-VariableIndex::VariableIndex(const VariableIndex& other) :
-    index_(indexUnpermuted_) {
-  *this = other;
-}
-
-/* ************************************************************************* */
-VariableIndex& VariableIndex::operator=(const VariableIndex& rhs) {
-  index_ = rhs.index_;
-  nFactors_ = rhs.nFactors_;
-  nEntries_ = rhs.nEntries_;
-  return *this;
-}
-
-/* ************************************************************************* */
-void VariableIndex::permute(const Permutation& permutation) {
-#ifndef NDEBUG
-  // Assert that the permutation does not leave behind any non-empty variables,
-  // otherwise the nFactors and nEntries counts would be incorrect.
-  for(Index j=0; j<this->index_.size(); ++j)
-    if(find(permutation.begin(), permutation.end(), j) == permutation.end())
-      assert(this->operator[](j).empty());
-#endif
-  index_.permute(permutation);
-}
-
 /* ************************************************************************* */
 bool VariableIndex::equals(const VariableIndex& other, double tol) const {
   if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) {
@@ -66,17 +41,13 @@ bool VariableIndex::equals(const VariableIndex& other, double tol) const {
 
 /* ************************************************************************* */
 void VariableIndex::print(const string& str) const {
-  cout << str << "\n";
+  cout << str;
   cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
-  Index var = 0;
+  for(Index var = 0; var < size(); ++var) {
-  BOOST_FOREACH(const Factors& variable, index_.container()) {
+    cout << "var " << var << ":";
-    Permutation::const_iterator rvar = find(index_.permutation().begin(), index_.permutation().end(), var);
+    BOOST_FOREACH(const size_t factor, index_[var])
-    assert(rvar != index_.permutation().end());
-    cout << "var " << (rvar-index_.permutation().begin()) << ":";
-    BOOST_FOREACH(const size_t factor, variable)
       cout << " " << factor;
     cout << "\n";
-    ++ var;
   }
   cout << flush;
 }
@@ -85,7 +56,7 @@ void VariableIndex::print(const string& str) const {
 void VariableIndex::outputMetisFormat(ostream& os) const {
   os << size() << " " << nFactors() << "\n";
   // run over variables, which will be hyper-edges.
-  BOOST_FOREACH(const Factors& variable, index_.container()) {
+  BOOST_FOREACH(const Factors& variable, index_) {
   	// every variable is a hyper-edge covering its factors
     BOOST_FOREACH(const size_t factor, variable)
       os << (factor+1) << " "; // base 1
@@ -94,4 +65,15 @@ void VariableIndex::outputMetisFormat(ostream& os) const {
   os << flush;
 }
 
+/* ************************************************************************* */
+void VariableIndex::permuteInPlace(const Permutation& permutation) {
+	// Create new index and move references to data into it in permuted order
+	vector<VariableIndex::Factors> newIndex(this->size());
+	for(Index i = 0; i < newIndex.size(); ++i)
+		newIndex[i].swap(this->index_[permutation[i]]);
+
+	// Move reference to entire index into the VariableIndex
+	index_.swap(newIndex);
+}
+
 }

gtsam/inference/VariableIndex.h

@@ -22,10 +22,12 @@
 #include <boost/foreach.hpp>
 
 #include <gtsam/base/FastList.h>
-#include <gtsam/inference/Permutation.h>
+#include <gtsam/base/types.h>
 
 namespace gtsam {
 
+	class Permutation;
+
 /**
  * The VariableIndex class computes and stores the block column structure of a
  * factor graph.  The factor graph stores a collection of factors, each of 
@@ -44,8 +46,7 @@ public:
   typedef Factors::const_iterator Factor_const_iterator;
 
 protected:
-  std::vector<Factors> indexUnpermuted_;
+  std::vector<Factors> index_;
-  Permuted<std::vector<Factors> > index_; // Permuted view of indexUnpermuted.
   size_t nFactors_; // Number of factors in the original factor graph.
   size_t nEntries_; // Sum of involved variable counts of each factor.
 
@@ -55,7 +56,7 @@ public:
 	/// @{
 
   /** Default constructor, creates an empty VariableIndex */
-  VariableIndex() : index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {}
+  VariableIndex() : nFactors_(0), nEntries_(0) {}
 
   /**
    * Create a VariableIndex that computes and stores the block column structure
@@ -70,16 +71,6 @@ public:
    */
   template<class FactorGraph> VariableIndex(const FactorGraph& factorGraph);
 
-  /**
-   * Copy constructor
-   */
-  VariableIndex(const VariableIndex& other);
-
-  /**
-   * Assignment operator
-   */
-  VariableIndex& operator=(const VariableIndex& rhs);
-
 	/// @}
 	/// @name Standard Interface
 	/// @{
@@ -120,9 +111,6 @@ public:
 	/// @name Advanced Interface
 	/// @{
 
-  /** Access a list of factors by variable */
-  Factors& operator[](Index variable) { checkVar(variable); return index_[variable]; }
-
   /**
    * Augment the variable index with new factors.  This can be used when
    * solving problems incrementally.
@@ -137,11 +125,8 @@ public:
    */
   template<typename CONTAINER, class FactorGraph> void remove(const CONTAINER& indices, const FactorGraph& factors);
 
-  /**
+	/// Permute the variables in the VariableIndex according to the given permutation
-   * Apply a variable permutation.  Does not rearrange data, just permutes
+	void permuteInPlace(const Permutation& permutation);
-   * future lookups by variable.
-   */
-  void permute(const Permutation& permutation);
 
 protected:
   Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); }	  ///<TODO: comment
@@ -150,13 +135,13 @@ protected:
   Factor_const_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); }	///<TODO: comment
   Factor_const_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); }			///<TODO: comment
 
-  ///TODO: comment
+  /// Internal constructor to allocate a VariableIndex of the requested size
-  VariableIndex(size_t nVars) : indexUnpermuted_(nVars), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {}
+  VariableIndex(size_t nVars) : index_(nVars), nFactors_(0), nEntries_(0) {}
 
-  ///TODO: comment
+  /// Internal check of the validity of a variable
   void checkVar(Index variable) const { assert(variable < index_.size()); }
 
-  ///TODO: comment
+  /// Internal function to populate the variable index from a factor graph
   template<class FactorGraph> void fill(const FactorGraph& factorGraph);
 
 	/// @}
@@ -183,7 +168,7 @@ void VariableIndex::fill(const FactorGraph& factorGraph) {
 /* ************************************************************************* */
 template<class FactorGraph>
 VariableIndex::VariableIndex(const FactorGraph& factorGraph) :
-    index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {
+    nFactors_(0), nEntries_(0) {
 
   // If the factor graph is empty, return an empty index because inside this
   // if block we assume at least one factor.
@@ -200,8 +185,7 @@ VariableIndex::VariableIndex(const FactorGraph& factorGraph) :
     }
 
     // Allocate array
-    index_.container().resize(maxVar+1);
+    index_.resize(maxVar+1);
-    index_.permutation() = Permutation::Identity(maxVar+1);
 
     fill(factorGraph);
   }
@@ -210,7 +194,7 @@ VariableIndex::VariableIndex(const FactorGraph& factorGraph) :
 /* ************************************************************************* */
 template<class FactorGraph>
 VariableIndex::VariableIndex(const FactorGraph& factorGraph, Index nVariables) :
-    indexUnpermuted_(nVariables), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {
+    index_(nVariables), nFactors_(0), nEntries_(0) {
   fill(factorGraph);
 }
 
@@ -232,11 +216,7 @@ void VariableIndex::augment(const FactorGraph& factors) {
     }
 
     // Allocate index
-    Index originalSize = index_.size();
+    index_.resize(std::max(index_.size(), maxVar+1));
-    index_.container().resize(std::max(index_.size(), maxVar+1));
-    index_.permutation().resize(index_.container().size());
-    for(Index var=originalSize; var<index_.permutation().size(); ++var)
-      index_.permutation()[var] = var;
 
     // Augment index mapping from variable id to factor index
     size_t orignFactors = nFactors_;

gtsam/inference/tests/testISAM.cpp

@@ -34,7 +34,7 @@ typedef ISAM<IndexConditional> SymbolicISAM;
 /* ************************************************************************* */
 // Some numbers that should be consistent among all smoother tests
 
-double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 =
+static double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 =
 		0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
 
 /* ************************************************************************* */

gtsam/linear/GaussianConditional.cpp

@@ -182,34 +182,18 @@ JacobianFactor::shared_ptr GaussianConditional::toFactor() const {
   return JacobianFactor::shared_ptr(new JacobianFactor(*this));
 }
 
-/* ************************************************************************* */
-template<class VALUES>
-inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES& x) {
-
-  // Helper function to solve-in-place on a VectorValues or Permuted<VectorValues>,
-  // called by GaussianConditional::solveInPlace(VectorValues&) and by
-  // GaussianConditional::solveInPlace(Permuted<VectorValues>&).
-
-  static const bool debug = false;
-  if(debug) conditional.print("Solving conditional in place");
-  Vector xS = internal::extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents());
-  xS = conditional.get_d() - conditional.get_S() * xS;
-  Vector soln = conditional.get_R().triangularView<Eigen::Upper>().solve(xS);
-  if(debug) {
-    gtsam::print(Matrix(conditional.get_R()), "Calling backSubstituteUpper on ");
-    gtsam::print(soln, "full back-substitution solution: ");
-  }
-  internal::writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals());
-}
-
 /* ************************************************************************* */
 void GaussianConditional::solveInPlace(VectorValues& x) const {
-  doSolveInPlace(*this, x); // Call helper version above
+	static const bool debug = false;
-}
+	if(debug) this->print("Solving conditional in place");
-
+	Vector xS = internal::extractVectorValuesSlices(x, this->beginParents(), this->endParents());
-/* ************************************************************************* */
+	xS = this->get_d() - this->get_S() * xS;
-void GaussianConditional::solveInPlace(Permuted<VectorValues>& x) const {
+	Vector soln = this->get_R().triangularView<Eigen::Upper>().solve(xS);
-  doSolveInPlace(*this, x); // Call helper version above
+	if(debug) {
+		gtsam::print(Matrix(this->get_R()), "Calling backSubstituteUpper on ");
+		gtsam::print(soln, "full back-substitution solution: ");
+	}
+	internal::writeVectorValuesSlices(soln, x, this->beginFrontals(), this->endFrontals());
 }
 
 /* ************************************************************************* */

gtsam/linear/GaussianConditional.h

@@ -196,23 +196,6 @@ public:
    */
   void solveInPlace(VectorValues& x) const;
 
-  /**
-   * Solves a conditional Gaussian and writes the solution into the entries of
-   * \c x for each frontal variable of the conditional (version for permuted
-   * VectorValues).  The parents are assumed to have already been solved in
-   * and their values are read from \c x.  This function works for multiple
-   * frontal variables.
-   *
-   * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2,
-   * where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator
-   * variables of this conditional, this solve function computes
-   * \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution.
-   *
-   * @param x VectorValues structure with solved parents \f$ x_s \f$, and into which the
-   * solution \f$ x_f \f$ will be written.
-   */
-  void solveInPlace(Permuted<VectorValues>& x) const;
-
   // functions for transpose backsubstitution
 
   /**

gtsam/linear/JacobianFactor.h

@@ -84,7 +84,7 @@ namespace gtsam {
 
   protected:
 
-    SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
+    noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
     std::vector<size_t> firstNonzeroBlocks_;
     AbMatrix matrix_; // the full matrix corresponding to the factor
     BlockAb Ab_;      // the block view of the full matrix

gtsam/linear/NoiseModel.h

@@ -19,6 +19,8 @@
 #pragma once
 
 #include <boost/serialization/nvp.hpp>
+#include <boost/serialization/shared_ptr.hpp>
+#include <boost/serialization/optional.hpp>
 #include <gtsam/base/Matrix.h>
 #include <cmath>
 

gtsam/linear/VectorValues.cpp

@@ -17,10 +17,12 @@
  */
 
 #include <gtsam/base/FastVector.h>
+#include <gtsam/inference/Permutation.h>
 #include <gtsam/linear/VectorValues.h>
 
 using namespace std;
-using namespace gtsam;
+
+namespace gtsam {
 
 /* ************************************************************************* */
 VectorValues::VectorValues(const VectorValues& other) {
@@ -166,20 +168,24 @@ void VectorValues::operator+=(const VectorValues& c) {
 }
 
 /* ************************************************************************* */
-VectorValues& VectorValues::operator=(const Permuted<VectorValues>& rhs) {
+VectorValues VectorValues::permute(const Permutation& permutation) const {
-  if(this->size() != rhs.size())
+	// Create result and allocate space
-    throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
+	VectorValues lhs;
-  for(size_t j=0; j<this->size(); ++j) {
+	lhs.values_.resize(this->dim());
-    if(exists(j)) {
+	lhs.maps_.reserve(this->size());
-      SubVector& l(this->at(j));
+
-      const SubVector& r(rhs[j]);
+	// Copy values from this VectorValues to the permuted VectorValues
-      if(l.rows() != r.rows())
+	size_t lhsPos = 0;
-        throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
+	for(size_t i = 0; i < this->size(); ++i) {
-      l = r;
+		// Map the next LHS subvector to the next slice of the LHS vector
-    } else {
+		lhs.maps_.push_back(SubVector(lhs.values_, lhsPos, this->at(permutation[i]).size()));
-      if(rhs.container().exists(rhs.permutation()[j]))
+		// Copy the data from the RHS subvector to the LHS subvector
-        throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
+		lhs.maps_[i] = this->at(permutation[i]);
-    }
+		// Increment lhs position
-  }
+		lhsPos += lhs.maps_[i].size();
-  return *this;
+	}
+
+	return lhs;
 }
+
+}

gtsam/linear/VectorValues.h

@@ -19,7 +19,6 @@
 
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/types.h>
-#include <gtsam/inference/Permutation.h>
 
 #include <boost/lexical_cast.hpp>
 #include <boost/foreach.hpp>
@@ -29,6 +28,9 @@
 
 namespace gtsam {
 
+	// Forward declarations
+	class Permutation;
+
   /**
    * This class represents a collection of vector-valued variables associated
    * each with a unique integer index.  It is typically used to store the variables
@@ -288,10 +290,11 @@ namespace gtsam {
      */
     void operator+=(const VectorValues& c);
 
-    /** Assignment operator from Permuted<VectorValues>, requires the dimensions
+		/**
-     * of the assignee to already be properly pre-allocated.
+		 * Permute the entries of this VectorValues, returns a new VectorValues as
-     */
+		 * the result.
-    VectorValues& operator=(const Permuted<VectorValues>& rhs);
+		 */
+		VectorValues permute(const Permutation& permutation) const;
 
     /// @}
 

gtsam/linear/tests/testGaussianConditional.cpp

@@ -267,55 +267,6 @@ TEST( GaussianConditional, solve_multifrontal )
 
 }
 
-/* ************************************************************************* */
-TEST( GaussianConditional, solve_multifrontal_permuted )
-{
-  // create full system, 3 variables, 2 frontals, all 2 dim
-  Matrix full_matrix = Matrix_(4, 7,
-      1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
-      0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
-      0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
-      0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4);
-
-  // 3 variables, all dim=2
-  vector<size_t> dims; dims += 2, 2, 2, 1;
-  GaussianConditional::rsd_type matrices(full_matrix, dims.begin(), dims.end());
-  Vector sigmas = ones(4);
-  vector<size_t> cgdims; cgdims += _x_, _x1_, _l1_;
-  GaussianConditional cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas);
-
-  EXPECT(assert_equal(Vector_(4, 0.1, 0.2, 0.3, 0.4), cg.get_d()));
-
-  // partial solution
-  Vector sl1 = Vector_(2, 9.0, 10.0);
-
-  // elimination order; _x_, _x1_, _l1_
-  VectorValues actualUnpermuted(vector<size_t>(3, 2));
-  Permutation permutation(3);
-  permutation[0] = 2;
-  permutation[1] = 0;
-  permutation[2] = 1;
-  Permuted<VectorValues> actual(permutation, actualUnpermuted);
-  actual[_x_]  = Vector_(2, 0.1, 0.2); // rhs
-  actual[_x1_] = Vector_(2, 0.3, 0.4); // rhs
-  actual[_l1_] = sl1; // parent
-
-  VectorValues expectedUnpermuted(vector<size_t>(3, 2));
-  Permuted<VectorValues> expected(permutation, expectedUnpermuted);
-  expected[_x_] = Vector_(2, -3.1,-3.4);
-  expected[_x1_] = Vector_(2, -11.9,-13.2);
-  expected[_l1_] = sl1;
-
-  // verify indices/size
-  EXPECT_LONGS_EQUAL(3, cg.size());
-  EXPECT_LONGS_EQUAL(4, cg.dim());
-
-  // solve and verify
-  cg.solveInPlace(actual);
-  EXPECT(assert_equal(expected.container(), actual.container(), tol));
-
-}
-
 /* ************************************************************************* */
 TEST( GaussianConditional, solveTranspose ) {
 	static const Index _y_=1;

gtsam/linear/tests/testGaussianJunctionTree.cpp

@@ -35,7 +35,7 @@ using namespace gtsam;
 
 static const Index x2=0, x1=1, x3=2, x4=3;
 
-GaussianFactorGraph createChain() {
+static GaussianFactorGraph createChain() {
 
   typedef GaussianFactorGraph::sharedFactor Factor;
   SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 0.5);

gtsam/linear/tests/testSerializationLinear.cpp

@@ -47,11 +47,11 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
 
 /* ************************************************************************* */
 // example noise models
-noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
+static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
-noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
+static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
-noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
+static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
-noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1));
+static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1));
-noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
+static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
 
 /* ************************************************************************* */
 TEST (Serialization, noiseModels) {

gtsam/linear/tests/testVectorValues.cpp

@@ -421,52 +421,31 @@ TEST(VectorValues, hasSameStructure) {
   EXPECT(!v1.hasSameStructure(VectorValues()));
 }
 
+
 /* ************************************************************************* */
-TEST(VectorValues, permuted_combined) {
+TEST(VectorValues, permute) {
-  Vector v1 = Vector_(3, 1.0,2.0,3.0);
-  Vector v2 = Vector_(2, 4.0,5.0);
-  Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0);
 
-  vector<size_t> dims(3); dims[0]=3; dims[1]=2; dims[2]=4;
+	VectorValues original;
-  VectorValues combined(dims);
+	original.insert(0, Vector_(1, 1.0));
-  combined[0] = v1;
+	original.insert(1, Vector_(2, 2.0, 3.0));
-  combined[1] = v2;
+	original.insert(2, Vector_(2, 4.0, 5.0));
-  combined[2] = v3;
+	original.insert(3, Vector_(2, 6.0, 7.0));
 
-  Permutation perm1(3);
+	VectorValues expected;
-  perm1[0] = 1;
+	expected.insert(0, Vector_(2, 4.0, 5.0)); // from 2
-  perm1[1] = 2;
+	expected.insert(1, Vector_(1, 1.0)); // from 0
-  perm1[2] = 0;
+	expected.insert(2, Vector_(2, 6.0, 7.0)); // from 3
+	expected.insert(3, Vector_(2, 2.0, 3.0)); // from 1
 
-  Permutation perm2(3);
+	Permutation permutation(4);
-  perm2[0] = 1;
+	permutation[0] = 2;
-  perm2[1] = 2;
+	permutation[1] = 0;
-  perm2[2] = 0;
+	permutation[2] = 3;
+	permutation[3] = 1;
 
-  Permuted<VectorValues> permuted1(combined);
+	VectorValues actual = original.permute(permutation);
-  CHECK(assert_equal(v1, permuted1[0]))
-  CHECK(assert_equal(v2, permuted1[1]))
-  CHECK(assert_equal(v3, permuted1[2]))
 
-  permuted1.permute(perm1);
+	EXPECT(assert_equal(expected, actual));
-  CHECK(assert_equal(v1, permuted1[2]))
-  CHECK(assert_equal(v2, permuted1[0]))
-  CHECK(assert_equal(v3, permuted1[1]))
-
-  permuted1.permute(perm2);
-  CHECK(assert_equal(v1, permuted1[1]))
-  CHECK(assert_equal(v2, permuted1[2]))
-  CHECK(assert_equal(v3, permuted1[0]))
-
-  Permuted<VectorValues> permuted2(perm1, combined);
-  CHECK(assert_equal(v1, permuted2[2]))
-  CHECK(assert_equal(v2, permuted2[0]))
-  CHECK(assert_equal(v3, permuted2[1]))
-
-  permuted2.permute(perm2);
-  CHECK(assert_equal(v1, permuted2[1]))
-  CHECK(assert_equal(v2, permuted2[2]))
-  CHECK(assert_equal(v3, permuted2[0]))
 }
 
 /* ************************************************************************* */

gtsam/nonlinear/ISAM2-impl.cpp

@@ -29,8 +29,8 @@ namespace gtsam {
 
 /* ************************************************************************* */
 void ISAM2::Impl::AddVariables(
-    const Values& newTheta, Values& theta, Permuted<VectorValues>& delta,
+    const Values& newTheta, Values& theta, VectorValues& delta,
-    Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& deltaGradSearch, vector<bool>& replacedKeys,
+    VectorValues& deltaNewton, VectorValues& deltaGradSearch, vector<bool>& replacedKeys,
     Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
   const bool debug = ISDEBUG("ISAM2 AddVariables");
 
@@ -40,28 +40,21 @@ void ISAM2::Impl::AddVariables(
   std::vector<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
   if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl;
   const size_t newDim = accumulate(dims.begin(), dims.end(), 0);
-  const size_t originalDim = delta->dim();
+  const size_t originalDim = delta.dim();
-  const size_t originalnVars = delta->size();
+  const size_t originalnVars = delta.size();
-  delta.container().append(dims);
+  delta.append(dims);
-  delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
+  delta.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
-  delta.permutation().resize(originalnVars + newTheta.size());
+  deltaNewton.append(dims);
-  deltaNewton.container().append(dims);
+  deltaNewton.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
-  deltaNewton.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
+  deltaGradSearch.append(dims);
-  deltaNewton.permutation().resize(originalnVars + newTheta.size());
+  deltaGradSearch.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
-  deltaGradSearch.container().append(dims);
-  deltaGradSearch.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
-  deltaGradSearch.permutation().resize(originalnVars + newTheta.size());
   {
     Index nextVar = originalnVars;
     BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
-      delta.permutation()[nextVar] = nextVar;
-      deltaNewton.permutation()[nextVar] = nextVar;
-      deltaGradSearch.permutation()[nextVar] = nextVar;
       ordering.insert(key_value.key, nextVar);
       if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl;
       ++ nextVar;
     }
-    assert(delta.permutation().size() == delta.container().size());
     assert(ordering.nVars() == delta.size());
     assert(ordering.size() == delta.size());
   }
@@ -82,7 +75,7 @@ FastSet<Index> ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const N
 }
 
 /* ************************************************************************* */
-FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const Permuted<VectorValues>& delta, const Ordering& ordering,
+FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering,
     const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
   FastSet<Index> relinKeys;
 
@@ -110,7 +103,7 @@ FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const Permuted<VectorValues
 }
 
 /* ************************************************************************* */
-void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double threshold, const Permuted<VectorValues>& delta, const ISAM2Clique::shared_ptr& clique) {
+void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double threshold, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) {
 
   // Check the current clique for relinearization
   bool relinearize = false;
@@ -131,7 +124,7 @@ void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double thres
 }
 
 /* ************************************************************************* */
-void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const Permuted<VectorValues>& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) {
+void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const VectorValues& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) {
 
   // Check the current clique for relinearization
   bool relinearize = false;
@@ -163,7 +156,7 @@ void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<c
 }
 
 /* ************************************************************************* */
-FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted<VectorValues>& delta, const Ordering& ordering,
+FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering,
     const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
 
   FastSet<Index> relinKeys;
@@ -201,13 +194,13 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Index>& keys,
 }
 
 /* ************************************************************************* */
-void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta, const Ordering& ordering,
+void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const Ordering& ordering,
-    const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug, const KeyFormatter& keyFormatter) {
+    const vector<bool>& mask, boost::optional<VectorValues&> invalidateIfDebug, const KeyFormatter& keyFormatter) {
   // If debugging, invalidate if requested, otherwise do not invalidate.
   // Invalidating means setting expmapped entries to Inf, to trigger assertions
   // if we try to re-use them.
 #ifdef NDEBUG
-  invalidateIfDebug = boost::optional<Permuted<VectorValues>&>();
+  invalidateIfDebug = boost::none;
 #endif
 
   assert(values.size() == ordering.nVars());
@@ -304,7 +297,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
   toc(4,"ccolamd permutations");
 
   tic(5,"permute affected variable index");
-  affectedFactorsIndex.permute(*affectedColamd);
+  affectedFactorsIndex.permuteInPlace(*affectedColamd);
   toc(5,"permute affected variable index");
 
   tic(6,"permute affected factors");
@@ -354,25 +347,13 @@ inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique,
 }
 
 /* ************************************************************************* */
-size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold) {
+size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, VectorValues& delta, double wildfireThreshold) {
 
   size_t lastBacksubVariableCount;
 
   if (wildfireThreshold <= 0.0) {
     // Threshold is zero or less, so do a full recalculation
-    // Collect dimensions and allocate new VectorValues
+    internal::optimizeInPlace(root, delta);
-    vector<size_t> dims(delta.size());
-    for(size_t j=0; j<delta.size(); ++j)
-      dims[j] = delta->dim(j);
-    VectorValues newDelta(dims);
-
-    // Optimize full solution delta
-    internal::optimizeInPlace(root, newDelta);
-
-    // Copy solution into delta
-    delta.permutation() = Permutation::Identity(delta.size());
-    delta.container() = newDelta;
-
     lastBacksubVariableCount = delta.size();
 
   } else {
@@ -380,8 +361,8 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std:
     lastBacksubVariableCount = optimizeWildfire(root, wildfireThreshold, replacedKeys, delta); // modifies delta_
 
 #ifndef NDEBUG
-    for(size_t j=0; j<delta.container().size(); ++j)
+    for(size_t j=0; j<delta.size(); ++j)
-      assert(delta.container()[j].unaryExpr(ptr_fun(isfinite<double>)).all());
+      assert(delta[j].unaryExpr(ptr_fun(isfinite<double>)).all());
 #endif
   }
 
@@ -394,7 +375,7 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std:
 /* ************************************************************************* */
 namespace internal {
 void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, std::vector<bool>& replacedKeys,
-    const VectorValues& grad, Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd, size_t& varsUpdated) {
+    const VectorValues& grad, VectorValues& deltaNewton, VectorValues& RgProd, size_t& varsUpdated) {
 
   // Check if any frontal or separator keys were recalculated, if so, we need
   // update deltas and recurse to children, but if not, we do not need to
@@ -433,7 +414,7 @@ void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, std::vecto
 
 /* ************************************************************************* */
 size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector<bool>& replacedKeys,
-    Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd) {
+    VectorValues& deltaNewton, VectorValues& RgProd) {
 
   // Get gradient
   VectorValues grad = *allocateVectorValues(isam);

gtsam/nonlinear/ISAM2-impl.h

@@ -46,10 +46,10 @@ struct ISAM2::Impl {
    * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
    * @param keyFormatter Formatter for printing nonlinear keys during debugging
    */
-  static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta,
+  static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta,
-      Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& deltaGradSearch, std::vector<bool>& replacedKeys,
+      VectorValues& deltaNewton, VectorValues& deltaGradSearch, std::vector<bool>& replacedKeys,
       Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
-
+	  
   /**
    * Extract the set of variable indices from a NonlinearFactorGraph.  For each Symbol
    * in each NonlinearFactor, obtains the index by calling ordering[symbol].
@@ -68,7 +68,7 @@ struct ISAM2::Impl {
    * @return The set of variable indices in delta whose magnitude is greater than or
    * equal to relinearizeThreshold
    */
-  static FastSet<Index> CheckRelinearizationFull(const Permuted<VectorValues>& delta, const Ordering& ordering,
+  static FastSet<Index> CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering,
       const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
 
   /**
@@ -82,7 +82,7 @@ struct ISAM2::Impl {
    * @return The set of variable indices in delta whose magnitude is greater than or
    * equal to relinearizeThreshold
    */
-  static FastSet<Index> CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted<VectorValues>& delta, const Ordering& ordering,
+  static FastSet<Index> CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering,
       const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
 
   /**
@@ -115,9 +115,9 @@ struct ISAM2::Impl {
    * recalculate its delta.
    * @param keyFormatter Formatter for printing nonlinear keys during debugging
    */
-  static void ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
+  static void ExpmapMasked(Values& values, const VectorValues& delta,
       const Ordering& ordering, const std::vector<bool>& mask,
-      boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>(),
+      boost::optional<VectorValues&> invalidateIfDebug = boost::none,
       const KeyFormatter& keyFormatter = DefaultKeyFormatter);
 
   /**
@@ -137,10 +137,10 @@ struct ISAM2::Impl {
   static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet<Index>& keys,
       const ReorderingMode& reorderingMode, bool useQR);
 
-  static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold);
+  static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, VectorValues& delta, double wildfireThreshold);
 
   static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector<bool>& replacedKeys,
-      Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd);
+      VectorValues& deltaNewton, VectorValues& RgProd);
 
 };
 

gtsam/nonlinear/ISAM2-inl.h

@@ -37,7 +37,7 @@ VALUE ISAM2::calculateEstimate(Key key) const {
 namespace internal {
 template<class CLIQUE>
 void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
-    std::vector<bool>& changed, const std::vector<bool>& replaced, Permuted<VectorValues>& delta, int& count) {
+    std::vector<bool>& changed, const std::vector<bool>& replaced, VectorValues& delta, int& count) {
   // if none of the variables in this clique (frontal and separator!) changed
   // significantly, then by the running intersection property, none of the
   // cliques in the children need to be processed
@@ -114,7 +114,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
 
 /* ************************************************************************* */
 template<class CLIQUE>
-int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const std::vector<bool>& keys, Permuted<VectorValues>& delta) {
+int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const std::vector<bool>& keys, VectorValues& delta) {
   std::vector<bool> changed(keys.size(), false);
   int count = 0;
   // starting from the root, call optimize on each conditional

gtsam/nonlinear/ISAM2.cpp

@@ -41,7 +41,6 @@ static const double batchThreshold = 0.65;
 
 /* ************************************************************************* */
 ISAM2::ISAM2(const ISAM2Params& params):
-    delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
     deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) {
   if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
     doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
@@ -49,15 +48,13 @@ ISAM2::ISAM2(const ISAM2Params& params):
 
 /* ************************************************************************* */
 ISAM2::ISAM2():
-    delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
     deltaDoglegUptodate_(true), deltaUptodate_(true) {
   if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
     doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
 }
 
 /* ************************************************************************* */
-ISAM2::ISAM2(const ISAM2& other):
+ISAM2::ISAM2(const ISAM2& other) {
-    delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_) {
   *this = other;
 }
 
@@ -308,12 +305,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
 
     // Reorder
     tic(2,"permute global variable index");
-    variableIndex_.permute(*colamd);
+    variableIndex_.permuteInPlace(*colamd);
     toc(2,"permute global variable index");
     tic(3,"permute delta");
-    delta_.permute(*colamd);
+    delta_ = delta_.permute(*colamd);
-    deltaNewton_.permute(*colamd);
+    deltaNewton_ = deltaNewton_.permute(*colamd);
-    RgProd_.permute(*colamd);
+    RgProd_ = RgProd_.permute(*colamd);
     toc(3,"permute delta");
     tic(4,"permute ordering");
     ordering_.permuteWithInverse(*colamdInverse);
@@ -429,12 +426,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
     // re-eliminate.  The reordered variables are also mentioned in the
     // orphans and the leftover cached factors.
     tic(3,"permute global variable index");
-    variableIndex_.permute(partialSolveResult.fullReordering);
+    variableIndex_.permuteInPlace(partialSolveResult.fullReordering);
     toc(3,"permute global variable index");
     tic(4,"permute delta");
-    delta_.permute(partialSolveResult.fullReordering);
+    delta_ = delta_.permute(partialSolveResult.fullReordering);
-    deltaNewton_.permute(partialSolveResult.fullReordering);
+    deltaNewton_ = deltaNewton_.permute(partialSolveResult.fullReordering);
-    RgProd_.permute(partialSolveResult.fullReordering);
+    RgProd_ = RgProd_.permute(partialSolveResult.fullReordering);
     toc(4,"permute delta");
     tic(5,"permute ordering");
     ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
@@ -723,8 +720,7 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
     tic(2, "Copy dx_d");
     // Update Delta and linear step
     doglegDelta_ = doglegResult.Delta;
-    delta_.permutation() = Permutation::Identity(delta_.size());  // Dogleg solves for the full delta so there is no permutation
+    delta_ = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution
-    delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution
     toc(2, "Copy dx_d");
   }
 
@@ -739,7 +735,7 @@ Values ISAM2::calculateEstimate() const {
   Values ret(theta_);
   toc(1, "Copy Values");
   tic(2, "getDelta");
-  const Permuted<VectorValues>& delta(getDelta());
+  const VectorValues& delta(getDelta());
   toc(2, "getDelta");
   tic(3, "Expmap");
   vector<bool> mask(ordering_.nVars(), true);
@@ -756,7 +752,7 @@ Values ISAM2::calculateBestEstimate() const {
 }
 
 /* ************************************************************************* */
-const Permuted<VectorValues>& ISAM2::getDelta() const {
+const VectorValues& ISAM2::getDelta() const {
   if(!deltaUptodate_)
     updateDelta();
   return delta_;
@@ -829,7 +825,7 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) {
 
   tic(3, "Compute minimizing step size");
   // Compute minimizing step size
-  double RgNormSq = isam.RgProd_.container().vector().squaredNorm();
+  double RgNormSq = isam.RgProd_.vector().squaredNorm();
   double step = -gradientSqNorm / RgNormSq;
   toc(3, "Compute minimizing step size");
 

gtsam/nonlinear/ISAM2.h

@@ -206,7 +206,7 @@ struct ISAM2Result {
    * factors passed as \c newFactors to ISAM2::update().  These indices may be
    * used later to refer to the factors in order to remove them.
    */
-  FastVector<size_t> newFactorsIndices;
+  std::vector<size_t> newFactorsIndices;
 
   /** A struct holding detailed results, which must be enabled with
    * ISAM2Params::enableDetailedResults.
@@ -347,26 +347,16 @@ protected:
   /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */
   VariableIndex variableIndex_;
 
-  /** The linear delta from the last linear solution, an update to the estimate in theta */
+  /** The linear delta from the last linear solution, an update to the estimate in theta
-  VectorValues deltaUnpermuted_;
+	 *
+	 * This is \c mutable because it is a "cached" variable - it is not updated
+	 * until either requested with getDelta() or calculateEstimate(), or needed
+	 * during update() to evaluate whether to relinearize variables.
+	 */
+  mutable VectorValues delta_;
 
-  /** The permutation through which the deltaUnpermuted_ is
+  mutable VectorValues deltaNewton_;
-   * referenced.
+  mutable VectorValues RgProd_;
-   *
-   * Permuting Vector entries would be slow, so for performance we
-   * instead maintain this permutation through which we access the linear delta
-   * indirectly
-   *
-   * This is \c mutable because it is a "cached" variable - it is not updated
-   * until either requested with getDelta() or calculateEstimate(), or needed
-   * during update() to evaluate whether to relinearize variables.
-   */
-  mutable Permuted<VectorValues> delta_;
-
-  VectorValues deltaNewtonUnpermuted_;
-  mutable Permuted<VectorValues> deltaNewton_;
-  VectorValues RgProdUnpermuted_;
-  mutable Permuted<VectorValues> RgProd_;
   mutable bool deltaDoglegUptodate_;
 
   /** Indicates whether the current delta is up-to-date, only used
@@ -497,7 +487,7 @@ public:
   Values calculateBestEstimate() const;
 
   /** Access the current delta, computed during the last call to update */
-  const Permuted<VectorValues>& getDelta() const;
+  const VectorValues& getDelta() const;
 
   /** Access the set of nonlinear factors */
   const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; }
@@ -555,7 +545,7 @@ void optimizeInPlace(const ISAM2& isam, VectorValues& delta);
 /// @return The number of variables that were solved for
 template<class CLIQUE>
 int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
-    double threshold, const std::vector<bool>& replaced, Permuted<VectorValues>& delta);
+    double threshold, const std::vector<bool>& replaced, VectorValues& delta);
 
 /**
  * Optimize along the gradient direction, with a closed-form computation to

gtsam/nonlinear/Values.cpp

@@ -120,7 +120,7 @@ namespace gtsam {
   /* ************************************************************************* */
   void Values::insert(Key j, const Value& val) {
   	Key key = j; // Non-const duplicate to deal with non-const insert argument
-  	std::pair<iterator,bool> insertResult = values_.insert(key, val.clone_());
+  	std::pair<KeyValueMap::iterator,bool> insertResult = values_.insert(key, val.clone_());
   	if(!insertResult.second)
   		throw ValuesKeyAlreadyExists(j);
   }

gtsam/nonlinear/tests/testSerializationNonlinear.cpp

@@ -47,13 +47,13 @@ typedef PinholeCamera<Cal3DS2>        PinholeCal3DS2;
 typedef PinholeCamera<Cal3Bundler>    PinholeCal3Bundler;
 
 /* ************************************************************************* */
-Point3 pt3(1.0, 2.0, 3.0);
+static Point3 pt3(1.0, 2.0, 3.0);
-Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
+static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
-Pose3 pose3(rt3, pt3);
+static Pose3 pose3(rt3, pt3);
 
-Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
+static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
-Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
+static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
-Cal3Bundler cal3(1.0, 2.0, 3.0);
+static Cal3Bundler cal3(1.0, 2.0, 3.0);
 
 TEST (Serialization, TemplatedValues) {
   Values values;

gtsam/slam/tests/testStereoFactor.cpp

@@ -28,18 +28,18 @@
 using namespace std;
 using namespace gtsam;
 
-Pose3 camera1(Matrix_(3,3,
+static Pose3 camera1(Matrix_(3,3,
 		       1., 0., 0.,
 		       0.,-1., 0.,
 		       0., 0.,-1.
 		       ),
 	      Point3(0,0,6.25));
 
-Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
+static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
-StereoCamera stereoCam(Pose3(), K);
+static StereoCamera stereoCam(Pose3(), K);
 
 // point X Y Z in meters
-Point3 p(0, 0, 5);
+static Point3 p(0, 0, 5);
 static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
 
 // Convenience for named keys

gtsam_unstable/discrete/AllDiff.cpp

@@ -20,10 +20,11 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	void AllDiff::print(const std::string& s) const {
+	void AllDiff::print(const std::string& s,
+			const IndexFormatter& formatter) const {
 		std::cout << s << "AllDiff on ";
 		BOOST_FOREACH (Index dkey, keys_)
-			std::cout << dkey << " ";
+			std::cout << formatter(dkey) << " ";
 		std::cout << std::endl;
 	}
 

gtsam_unstable/discrete/AllDiff.h

@@ -34,7 +34,8 @@ namespace gtsam {
 		AllDiff(const DiscreteKeys& dkeys);
 
 		// print
-		virtual void print(const std::string& s = "") const;
+		virtual void print(const std::string& s = "",
+				const IndexFormatter& formatter = DefaultIndexFormatter) const;
 
 		/// Calculate value = expensive !
 		virtual double operator()(const Values& values) const;

gtsam_unstable/discrete/BinaryAllDiff.h

@@ -33,9 +33,10 @@ namespace gtsam {
 		}
 
 		// print
-		virtual void print(const std::string& s = "") const {
+		virtual void print(const std::string& s = "",
-			std::cout << s << "BinaryAllDiff on " << keys_[0] << " and " << keys_[1]
+				const IndexFormatter& formatter = DefaultIndexFormatter) const {
-					<< std::endl;
+			std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and "
+					<< formatter(keys_[1]) << std::endl;
 		}
 
 		/// Calculate value

gtsam_unstable/discrete/Domain.cpp

@@ -15,9 +15,10 @@ namespace gtsam {
 	using namespace std;
 
 	/* ************************************************************************* */
-	void Domain::print(const string& s) const {
+	void Domain::print(const string& s,
-//		cout << s << ": Domain on " << keys_[0] << " (j=" << keys_[0]
+			const IndexFormatter& formatter) const {
-//				<< ") with values";
+//		cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" <<
+//		formatter(keys_[0]) << ") with values";
 //		BOOST_FOREACH (size_t v,values_) cout << " " << v;
 //		cout << endl;
 		BOOST_FOREACH (size_t v,values_) cout << v;

gtsam_unstable/discrete/Domain.h

@@ -66,7 +66,8 @@ namespace gtsam {
 		}
 
 		// print
-		virtual void print(const std::string& s = "") const;
+		virtual void print(const std::string& s = "",
+				const IndexFormatter& formatter = DefaultIndexFormatter) const;
 
 		bool contains(size_t value) const {
 			return values_.count(value)>0;

gtsam_unstable/discrete/SingleValue.cpp

@@ -16,8 +16,9 @@ namespace gtsam {
 	using namespace std;
 
 	/* ************************************************************************* */
-	void SingleValue::print(const string& s) const {
+	void SingleValue::print(const string& s,
-		cout << s << "SingleValue on " << "j=" << keys_[0]
+			const IndexFormatter& formatter) const {
+		cout << s << "SingleValue on " << "j=" << formatter(keys_[0])
 				<< " with value " << value_ << endl;
 	}
 

gtsam_unstable/discrete/SingleValue.h

@@ -42,7 +42,8 @@ namespace gtsam {
 		}
 
 		// print
-		virtual void print(const std::string& s = "") const;
+		virtual void print(const std::string& s = "",
+				const IndexFormatter& formatter = DefaultIndexFormatter) const;
 
 		/// Calculate value
 		virtual double operator()(const Values& values) const;

gtsam_unstable/discrete/tests/testScheduler.cpp

@@ -149,7 +149,7 @@ TEST( schedulingExample, test)
 /* ************************************************************************* */
 TEST( schedulingExample, smallFromFile)
 {
-	string path("../../../gtsam_unstable/discrete/examples/");
+	string path(TOPSRCDIR "/gtsam_unstable/discrete/examples/");
 	Scheduler s(2, path + "small.csv");
 
 	// add areas

tests/testGaussianISAM.cpp

@@ -41,10 +41,10 @@ using symbol_shorthand::L;
 /* ************************************************************************* */
 // Some numbers that should be consistent among all smoother tests
 
-double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 =
+static double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 =
 		0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
 
-const double tol = 1e-4;
+static const double tol = 1e-4;
 
 /* ************************************************************************* */
 TEST_UNSAFE( ISAM, iSAM_smoother )

tests/testGaussianISAM2.cpp

@@ -139,112 +139,69 @@ TEST_UNSAFE(ISAM2, AddVariables) {
 
   // Create initial state
   Values theta;
-  theta.insert((0), Pose2(.1, .2, .3));
+  theta.insert(0, Pose2(.1, .2, .3));
   theta.insert(100, Point2(.4, .5));
   Values newTheta;
-  newTheta.insert((1), Pose2(.6, .7, .8));
+  newTheta.insert(1, Pose2(.6, .7, .8));
 
-  VectorValues deltaUnpermuted;
+  VectorValues delta;
-  deltaUnpermuted.insert(0, Vector_(3, .1, .2, .3));
+  delta.insert(0, Vector_(3, .1, .2, .3));
-  deltaUnpermuted.insert(1, Vector_(2, .4, .5));
+  delta.insert(1, Vector_(2, .4, .5));
 
-  Permutation permutation(2);
+  VectorValues deltaNewton;
-  permutation[0] = 1;
+  deltaNewton.insert(0, Vector_(3, .1, .2, .3));
-  permutation[1] = 0;
+  deltaNewton.insert(1, Vector_(2, .4, .5));
 
-  Permuted<VectorValues> delta(permutation, deltaUnpermuted);
+  VectorValues deltaRg;
-
+  deltaRg.insert(0, Vector_(3, .1, .2, .3));
-  VectorValues deltaNewtonUnpermuted;
+  deltaRg.insert(1, Vector_(2, .4, .5));
-  deltaNewtonUnpermuted.insert(0, Vector_(3, .1, .2, .3));
-  deltaNewtonUnpermuted.insert(1, Vector_(2, .4, .5));
-
-  Permutation permutationNewton(2);
-  permutationNewton[0] = 1;
-  permutationNewton[1] = 0;
-
-  Permuted<VectorValues> deltaNewton(permutationNewton, deltaNewtonUnpermuted);
-
-  VectorValues deltaRgUnpermuted;
-  deltaRgUnpermuted.insert(0, Vector_(3, .1, .2, .3));
-  deltaRgUnpermuted.insert(1, Vector_(2, .4, .5));
-
-  Permutation permutationRg(2);
-  permutationRg[0] = 1;
-  permutationRg[1] = 0;
-
-  Permuted<VectorValues> deltaRg(permutationRg, deltaRgUnpermuted);
 
   vector<bool> replacedKeys(2, false);
 
-  Ordering ordering; ordering += 100, (0);
+  Ordering ordering; ordering += 100, 0;
 
   ISAM2::Nodes nodes(2);
 
   // Verify initial state
   LONGS_EQUAL(0, ordering[100]);
-  LONGS_EQUAL(1, ordering[(0)]);
+  LONGS_EQUAL(1, ordering[0]);
-  EXPECT(assert_equal(deltaUnpermuted[1], delta[ordering[100]]));
+  EXPECT(assert_equal(delta[0], delta[ordering[100]]));
-  EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[(0)]]));
+  EXPECT(assert_equal(delta[1], delta[ordering[0]]));
 
   // Create expected state
   Values thetaExpected;
-  thetaExpected.insert((0), Pose2(.1, .2, .3));
+  thetaExpected.insert(0, Pose2(.1, .2, .3));
   thetaExpected.insert(100, Point2(.4, .5));
-  thetaExpected.insert((1), Pose2(.6, .7, .8));
+  thetaExpected.insert(1, Pose2(.6, .7, .8));
 
-  VectorValues deltaUnpermutedExpected;
+  VectorValues deltaExpected;
-  deltaUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
+  deltaExpected.insert(0, Vector_(3, .1, .2, .3));
-  deltaUnpermutedExpected.insert(1, Vector_(2, .4, .5));
+  deltaExpected.insert(1, Vector_(2, .4, .5));
-  deltaUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
+  deltaExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
 
-  Permutation permutationExpected(3);
+  VectorValues deltaNewtonExpected;
-  permutationExpected[0] = 1;
+  deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3));
-  permutationExpected[1] = 0;
+  deltaNewtonExpected.insert(1, Vector_(2, .4, .5));
-  permutationExpected[2] = 2;
+  deltaNewtonExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
 
-  Permuted<VectorValues> deltaExpected(permutationExpected, deltaUnpermutedExpected);
+  VectorValues deltaRgExpected;
-
+  deltaRgExpected.insert(0, Vector_(3, .1, .2, .3));
-  VectorValues deltaNewtonUnpermutedExpected;
+  deltaRgExpected.insert(1, Vector_(2, .4, .5));
-  deltaNewtonUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
+  deltaRgExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
-  deltaNewtonUnpermutedExpected.insert(1, Vector_(2, .4, .5));
-  deltaNewtonUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
-
-  Permutation permutationNewtonExpected(3);
-  permutationNewtonExpected[0] = 1;
-  permutationNewtonExpected[1] = 0;
-  permutationNewtonExpected[2] = 2;
-
-  Permuted<VectorValues> deltaNewtonExpected(permutationNewtonExpected, deltaNewtonUnpermutedExpected);
-
-  VectorValues deltaRgUnpermutedExpected;
-  deltaRgUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
-  deltaRgUnpermutedExpected.insert(1, Vector_(2, .4, .5));
-  deltaRgUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
-
-  Permutation permutationRgExpected(3);
-  permutationRgExpected[0] = 1;
-  permutationRgExpected[1] = 0;
-  permutationRgExpected[2] = 2;
-
-  Permuted<VectorValues> deltaRgExpected(permutationRgExpected, deltaRgUnpermutedExpected);
 
   vector<bool> replacedKeysExpected(3, false);
 
-  Ordering orderingExpected; orderingExpected += 100, (0), (1);
+  Ordering orderingExpected; orderingExpected += 100, 0, 1;
 
-  ISAM2::Nodes nodesExpected(
+  ISAM2::Nodes nodesExpected(3, ISAM2::sharedClique());
-          3, ISAM2::sharedClique());
 
   // Expand initial state
   ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes);
 
   EXPECT(assert_equal(thetaExpected, theta));
-  EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted));
+  EXPECT(assert_equal(deltaExpected, delta));
-  EXPECT(assert_equal(deltaExpected.permutation(), delta.permutation()));
+  EXPECT(assert_equal(deltaNewtonExpected, deltaNewton));
-  EXPECT(assert_equal(deltaNewtonUnpermutedExpected, deltaNewtonUnpermuted));
+  EXPECT(assert_equal(deltaRgExpected, deltaRg));
-  EXPECT(assert_equal(deltaNewtonExpected.permutation(), deltaNewton.permutation()));
-  EXPECT(assert_equal(deltaRgUnpermutedExpected, deltaRgUnpermuted));
-  EXPECT(assert_equal(deltaRgExpected.permutation(), deltaRg.permutation()));
   EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys));
   EXPECT(assert_equal(orderingExpected, ordering));
 }

tests/testGaussianJunctionTreeB.cpp

@@ -52,7 +52,7 @@ using symbol_shorthand::L;
 	 C3		    x1 : x2
 	 C4		  x7 : x6
 */
-TEST( GaussianJunctionTree, constructor2 )
+TEST( GaussianJunctionTreeB, constructor2 )
 {
 	// create a graph
   Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
@@ -88,7 +88,7 @@ TEST( GaussianJunctionTree, constructor2 )
 }
 
 /* ************************************************************************* */
-TEST( GaussianJunctionTree, optimizeMultiFrontal )
+TEST( GaussianJunctionTreeB, optimizeMultiFrontal )
 {
 	// create a graph
   GaussianFactorGraph fg;
@@ -108,7 +108,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal )
 }
 
 /* ************************************************************************* */
-TEST( GaussianJunctionTree, optimizeMultiFrontal2)
+TEST( GaussianJunctionTreeB, optimizeMultiFrontal2)
 {
 	// create a graph
 	example::Graph nlfg = createNonlinearFactorGraph();
@@ -126,7 +126,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2)
 }
 
 /* ************************************************************************* */
-TEST(GaussianJunctionTree, slamlike) {
+TEST(GaussianJunctionTreeB, slamlike) {
   Values init;
   planarSLAM::Graph newfactors;
   planarSLAM::Graph fullgraph;
@@ -188,7 +188,7 @@ TEST(GaussianJunctionTree, slamlike) {
 }
 
 /* ************************************************************************* */
-TEST(GaussianJunctionTree, simpleMarginal) {
+TEST(GaussianJunctionTreeB, simpleMarginal) {
 
   typedef BayesTree<GaussianConditional> GaussianBayesTree;
 

tests/testNonlinearEquality.cpp

@@ -37,7 +37,7 @@ typedef PriorFactor<Pose2> PosePrior;
 typedef NonlinearEquality<Pose2> PoseNLE;
 typedef boost::shared_ptr<PoseNLE> shared_poseNLE;
 
-Symbol key('x',1);
+static Symbol key('x',1);
 
 /* ************************************************************************* */
 TEST ( NonlinearEquality, linearization ) {
@@ -241,8 +241,8 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
 }
 
 /* ************************************************************************* */
-SharedDiagonal hard_model = noiseModel::Constrained::All(2);
+static SharedDiagonal hard_model = noiseModel::Constrained::All(2);
-SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
+static SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
 
 /* ************************************************************************* */
 TEST( testNonlinearEqualityConstraint, unary_basics ) {
@@ -504,10 +504,10 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
 }
 
 // make a realistic calibration matrix
-double fov = 60; // degrees
+static double fov = 60; // degrees
-size_t w=640,h=480;
+static size_t w=640,h=480;
-Cal3_S2 K(fov,w,h);
+static Cal3_S2 K(fov,w,h);
-boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
+static boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
 
 // typedefs for visual SLAM example
 typedef visualSLAM::Graph VGraph;

tests/testSerializationSLAM.cpp

@@ -174,10 +174,10 @@ BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor,    "gtsam::visualSLAM::StereoF
 BOOST_CLASS_EXPORT(gtsam::Pose3)
 BOOST_CLASS_EXPORT(gtsam::Point3)
 
-Point3 pt3(1.0, 2.0, 3.0);
+static Point3 pt3(1.0, 2.0, 3.0);
-Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
+static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
-Pose3 pose3(rt3, pt3);
+static Pose3 pose3(rt3, pt3);
-Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
+static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
 
 /* ************************************************************************* */
 TEST (Serialization, visual_system) {

wrap/CMakeLists.txt

@@ -4,7 +4,7 @@ find_package(Boost 1.42 COMPONENTS system filesystem thread REQUIRED)
 
 # Build the executable itself
 file(GLOB wrap_srcs "*.cpp")
-list(REMOVE_ITEM wrap_srcs wrap.cpp)
+list(REMOVE_ITEM wrap_srcs ${CMAKE_CURRENT_SOURCE_DIR}/wrap.cpp)
 add_library(wrap_lib STATIC ${wrap_srcs})
 add_executable(wrap wrap.cpp)
 target_link_libraries(wrap wrap_lib ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY})
@@ -19,7 +19,6 @@ install(FILES matlab.h DESTINATION include/wrap)
 
 # Build tests
 if (GTSAM_BUILD_TESTS) 
-    add_definitions(-DTOPSRCDIR="${CMAKE_SOURCE_DIR}")
     set(wrap_local_libs wrap_lib ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}) 
     gtsam_add_subdir_tests("wrap" "${wrap_local_libs}" "${wrap_local_libs}" "") 
 endif(GTSAM_BUILD_TESTS)