Removed sample() functions and global random number generator

gtsam.h

@@ -690,7 +690,6 @@ class Diagonal {
 	static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas);
 	static gtsam::noiseModel::Diagonal* Variances(Vector variances);
 	static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
-	Vector sample() const;
 //	Matrix R() const;		// FIXME: cannot parse!!!
 	void print(string s) const;
 };
@@ -699,7 +698,6 @@ class Isotropic {
 	static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma);
 	static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
 	static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
-	Vector sample() const;
 	void print(string s) const;
 };
 
@@ -731,7 +729,19 @@ class SharedGaussian {
 class SharedDiagonal {
 	SharedDiagonal(Vector sigmas);
 	void print(string s) const;
-	Vector sample() const;
+};
+
+class Sampler {
+	Sampler(gtsam::noiseModel::Diagonal* model, int seed);
+	Sampler(Vector sigmas, int seed);
+	Sampler(int seed);
+
+	size_t dim() const;
+	Vector sigmas() const;
+	gtsam::noiseModel::Diagonal* model() const;
+
+	Vector sample();
+	Vector sampleNewModel(gtsam::noiseModel::Diagonal* model);
 };
 
 class VectorValues {
@@ -926,7 +936,7 @@ class NonlinearFactor {
 	void equals(const gtsam::NonlinearFactor& other, double tol) const;
 	gtsam::KeyVector keys() const;
 	size_t size() const;
-	size_t dim() const;
+//	size_t dim() const; // FIXME: Doesn't link
 };
 
 class Values {

gtsam/base/Vector.cpp

@@ -23,13 +23,11 @@
 #include <sstream>
 #include <iomanip>
 #include <cmath>
+#include <stdexcept>
 #include <boost/foreach.hpp>
 #include <boost/optional.hpp>
 #include <cstdio>
 
-#include <boost/random/normal_distribution.hpp>
-#include <boost/random/variate_generator.hpp>
-
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/types.h>
 
@@ -39,8 +37,6 @@
 
 using namespace std;
 
-boost::minstd_rand generator(42u);
-
 namespace gtsam {
 
 /* ************************************************************************* */
@@ -432,19 +428,6 @@ Vector concatVectors(size_t nrVectors, ...)
 	return concatVectors(vs);
 }
 
-/* ************************************************************************* */
-Vector rand_vector_norm(size_t dim, double mean, double sigma)
-{
-	boost::normal_distribution<double> norm_dist(mean, sigma);
-	boost::variate_generator<boost::minstd_rand&, boost::normal_distribution<double> > norm(generator, norm_dist);
-
-	Vector v(dim);
-	for(int i = 0; i<v.size(); ++i)
-		v(i) = norm();
-
-	return v;
-}
-
 /* ************************************************************************* */
 
 } // namespace gtsam

gtsam/base/Vector.h

@@ -22,19 +22,9 @@
 
 #include <list>
 #include <vector>
+#include <iostream>
 #include <gtsam/base/types.h>
 #include <gtsam/3rdparty/Eigen/Eigen/Dense>
-#include <boost/random/linear_congruential.hpp>
-
-/**
- * Static random number generator - needs to maintain a state
- * over time, hence the static generator.  Be careful in
- * cases where multiple processes (as is frequently the case with
- * multi-robot scenarios) are using the sample() facilities
- * in NoiseModel, as they will each have the same seed.
- */
-// FIXME: make this go away - use the Sampler class instead
-extern boost::minstd_rand generator;
 
 namespace gtsam {
 
@@ -358,20 +348,6 @@ Vector concatVectors(const std::list<Vector>& vs);
  */
 Vector concatVectors(size_t nrVectors, ...);
 
-/**
- * random vector
- */
-Vector rand_vector_norm(size_t dim, double mean = 0, double sigma = 1);
-
-/**
- * Sets the generator to use a different seed value.
- * Default argument resets the RNG
- * @param seed is the new seed
- */
-inline void seedRNG(unsigned int seed = 42u) {
-	generator.seed(seed);
-}
-
 } // namespace gtsam
 
 #include <boost/serialization/nvp.hpp>

gtsam/base/tests/testVector.cpp

@@ -297,27 +297,6 @@ TEST( TestVector, linear_dependent3 )
 	EXPECT(!linear_dependent(v1, v2));
 }
 
-/* ************************************************************************* */
-TEST( TestVector, random )
-{
-	// Assumes seed not previously reset during this test
-	seedRNG();
-	Vector v1_42 = rand_vector_norm(5);
-
-	// verify that resetting the RNG produces the same value
-	seedRNG();
-	Vector v2_42 = rand_vector_norm(5);
-
-	EXPECT(assert_equal(v1_42, v2_42, 1e-6));
-
-	// verify that different seed produces a different value
-	seedRNG(41u);
-
-	Vector v3_41 = rand_vector_norm(5);
-
-	EXPECT(assert_inequal(v1_42, v3_41, 1e-6));
-}
-
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/linear/NoiseModel.cpp

@@ -255,18 +255,6 @@ void Diagonal::WhitenInPlace(Matrix& H) const {
 	vector_scale_inplace(invsigmas(), H);
 }
 
-/* ************************************************************************* */
-Vector Diagonal::sample() const {
-	Vector result(dim_);
-	for (size_t i = 0; i < dim_; i++) {
-		typedef boost::normal_distribution<double> Normal;
-		Normal dist(0.0, this->sigmas_(i));
-		boost::variate_generator<boost::minstd_rand&, Normal> norm(generator, dist);
-		result(i) = norm();
-	}
-	return result;
-}
-
 /* ************************************************************************* */
 // Constrained
 /* ************************************************************************* */
@@ -464,18 +452,6 @@ void Isotropic::WhitenInPlace(Matrix& H) const {
 	H *= invsigma_;
 }
 
-/* ************************************************************************* */
-// faster version
-Vector Isotropic::sample() const {
-	typedef boost::normal_distribution<double> Normal;
-	Normal dist(0.0, this->sigma_);
-	boost::variate_generator<boost::minstd_rand&, Normal> norm(generator, dist);
-	Vector result(dim_);
-	for (size_t i = 0; i < dim_; i++)
-		result(i) = norm();
-	return result;
-}
-
 /* ************************************************************************* */
 // Unit
 /* ************************************************************************* */

gtsam/linear/NoiseModel.h

@@ -298,11 +298,6 @@ namespace gtsam {
 			Vector invsigmas() const;
 			double invsigma(size_t i) const;
 
-			/**
-			 * generate random variate
-			 */
-			virtual Vector sample() const;
-
 			/**
 			 * Return R itself, but note that Whiten(H) is cheaper than R*H
 			 */
@@ -525,11 +520,6 @@ namespace gtsam {
 			 */
 			inline double sigma() const { return sigma_; }
 
-			/**
-			 * generate random variate
-			 */
-			virtual Vector sample() const;
-
 		private:
 			/** Serialization function */
 			friend class boost::serialization::access;

gtsam/linear/Sampler.cpp

@@ -66,7 +66,7 @@ Vector Sampler::sample() {
 }
 
 /* ************************************************************************* */
-Vector Sampler::sample(const noiseModel::Diagonal::shared_ptr& model) {
+Vector Sampler::sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) {
 	assert(model.get());
 	const Vector& sigmas = model->sigmas();
 	return sampleDiagonal(sigmas);

gtsam/linear/Sampler.h

@@ -20,6 +20,8 @@
 
 #include <gtsam/linear/NoiseModel.h>
 
+#include <boost/random/linear_congruential.hpp>
+
 namespace gtsam {
 
 /**
@@ -79,7 +81,7 @@ public:
 	 *
 	 * NOTE: not const due to need to update the underlying generator
 	 */
-	Vector sample(const noiseModel::Diagonal::shared_ptr& model);
+	Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model);
 
 protected:
 

gtsam/linear/SharedDiagonal.h

@@ -58,9 +58,6 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
 		/// Print
 		inline void print(const std::string &s) const { (*this)->print(s); }
 
-		/// Generate a sample
-		inline Vector sample() const { return (*this)->sample(); }
-
 		/** Serialization function */
 		friend class boost::serialization::access;
 		template<class ARCHIVE>

gtsam/linear/VectorValues.h

@@ -24,6 +24,7 @@
 #include <boost/foreach.hpp>
 #include <boost/shared_ptr.hpp>
 #include <numeric>
+#include <stdexcept>
 
 namespace gtsam {
 

gtsam/linear/tests/testNoiseModel.cpp

@@ -131,15 +131,6 @@ TEST(NoiseModel, equals)
 	EXPECT(assert_inequal(*i1,*i2));
 }
 
-/* ************************************************************************* */
-TEST(NoiseModel, sample)
-{
-	Vector s = Vector_(3,1.0,2.0,3.0);
-	SharedDiagonal model = sharedSigmas(s);
-	Vector v = model->sample();
-	// no check as no way yet to set random seed
-}
-
 // TODO enable test once a mechanism for smart constraints exists
 ///* ************************************************************************* */
 //TEST(NoiseModel, ConstrainedSmart )
@@ -346,8 +337,5 @@ TEST(NoiseModel, robustNoise)
 }
 
 /* ************************************************************************* */
-int main() {
+int main() {	TestResult tr; return TestRegistry::runAllTests(tr); }
-	TestResult tr;
-	return TestRegistry::runAllTests(tr);
-}
 /* ************************************************************************* */

gtsam/slam/dataset.cpp

@@ -20,226 +20,231 @@
 #include <sstream>
 #include <cstdlib>
 
+#include <gtsam/linear/Sampler.h>
 #include <gtsam/slam/dataset.h>
 
 using namespace std;
-using namespace gtsam;
 
 #define LINESIZE 81920
 
 namespace gtsam {
 
-	/* ************************************************************************* */
+/* ************************************************************************* */
-	pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset,
+pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset,
-			const string& user_path) {
+		const string& user_path) {
-		string path = user_path, set = dataset;
+	string path = user_path, set = dataset;
-		boost::optional<SharedDiagonal> null_model;
+	boost::optional<SharedDiagonal> null_model;
-		boost::optional<SharedDiagonal> identity(noiseModel::Unit::Create(3));
+	boost::optional<SharedDiagonal> identity(noiseModel::Unit::Create(3));
-		boost::optional<SharedDiagonal> small(
+	boost::optional<SharedDiagonal> small(
-				noiseModel::Diagonal::Variances(
+			noiseModel::Diagonal::Variances(
-						gtsam::Vector_(3, 0.0001, 0.0001, 0.0003)));
+					gtsam::Vector_(3, 0.0001, 0.0001, 0.0003)));
 
-		if (path.empty())
+	if (path.empty())
-			path = string(getenv("HOME")) + "/data";
+		path = string(getenv("HOME")) + "/data";
-		if (set.empty())
+	if (set.empty())
-			set = string(getenv("DATASET"));
+		set = string(getenv("DATASET"));
 
-		if (set == "intel")
+	if (set == "intel")
-			return make_pair(path + "/Intel/intel.graph", null_model);
+		return make_pair(path + "/Intel/intel.graph", null_model);
-		if (set == "intel-gfs")
+	if (set == "intel-gfs")
-			return make_pair(path + "/Intel/intel.gfs.graph", null_model);
+		return make_pair(path + "/Intel/intel.gfs.graph", null_model);
-		if (set == "Killian-gfs")
+	if (set == "Killian-gfs")
-			return make_pair(path + "/Killian/Killian.gfs.graph", null_model);
+		return make_pair(path + "/Killian/Killian.gfs.graph", null_model);
-		if (set == "Killian")
+	if (set == "Killian")
-			return make_pair(path + "/Killian/Killian.graph", small);
+		return make_pair(path + "/Killian/Killian.graph", small);
-		if (set == "Killian-noised")
+	if (set == "Killian-noised")
-			return make_pair(path + "/Killian/Killian-noised.graph", null_model);
+		return make_pair(path + "/Killian/Killian-noised.graph", null_model);
-		if (set == "3")
+	if (set == "3")
-			return make_pair(path + "/TORO/w3-odom.graph", identity);
+		return make_pair(path + "/TORO/w3-odom.graph", identity);
-		if (set == "100")
+	if (set == "100")
-			return make_pair(path + "/TORO/w100-odom.graph", identity);
+		return make_pair(path + "/TORO/w100-odom.graph", identity);
-		if (set == "10K")
+	if (set == "10K")
-			return make_pair(path + "/TORO/w10000-odom.graph", identity);
+		return make_pair(path + "/TORO/w10000-odom.graph", identity);
-		if (set == "10K2")
+	if (set == "10K2")
-			return make_pair(path + "/hogman/data/2D/w10000.graph",
+		return make_pair(path + "/hogman/data/2D/w10000.graph",
-					noiseModel::Diagonal::Variances(gtsam::Vector_(3, 0.1, 0.1, 0.05)));
+				noiseModel::Diagonal::Variances(gtsam::Vector_(3, 0.1, 0.1, 0.05)));
-		if (set == "Eiffel100")
+	if (set == "Eiffel100")
-			return make_pair(path + "/TORO/w100-Eiffel.graph", identity);
+		return make_pair(path + "/TORO/w100-Eiffel.graph", identity);
-		if (set == "Eiffel10K")
+	if (set == "Eiffel10K")
-			return make_pair(path + "/TORO/w10000-Eiffel.graph", identity);
+		return make_pair(path + "/TORO/w10000-Eiffel.graph", identity);
-		if (set == "olson")
+	if (set == "olson")
-			return make_pair(path + "/Olson/olson06icra.graph", null_model);
+		return make_pair(path + "/Olson/olson06icra.graph", null_model);
-		if (set == "victoria")
+	if (set == "victoria")
-			return make_pair(path + "/VictoriaPark/victoria_park.graph", null_model);
+		return make_pair(path + "/VictoriaPark/victoria_park.graph", null_model);
-		if (set == "beijing")
+	if (set == "beijing")
-			return make_pair(path + "/Beijing/beijingData_trips.graph", null_model);
+		return make_pair(path + "/Beijing/beijingData_trips.graph", null_model);
 
-		return make_pair("unknown", null_model);
+	return make_pair("unknown", null_model);
-	}
-
-	/* ************************************************************************* */
-
-	pair<pose2SLAM::Graph::shared_ptr, pose2SLAM::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<pose2SLAM::Graph::shared_ptr, pose2SLAM::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());
-		if (!is) {
-			cout << "load2D: can not find the file!";
-			exit(-1);
-		}
-
-		pose2SLAM::Values::shared_ptr poses(new pose2SLAM::Values);
-		pose2SLAM::Graph::shared_ptr graph(new pose2SLAM::Graph);
-
-		string tag;
-
-		// load the poses
-		while (is) {
-			is >> tag;
-
-			if ((tag == "VERTEX2") || (tag == "VERTEX")) {
-				int id;
-				double x, y, yaw;
-				is >> id >> x >> y >> yaw;
-				// optional filter
-				if (maxID && id >= maxID)
-					continue;
-				poses->insert(id, Pose2(x, y, yaw));
-			}
-			is.ignore(LINESIZE, '\n');
-		}
-		is.clear(); /* clears the end-of-file and error flags */
-		is.seekg(0, ios::beg);
-
-		// load the factors
-		while (is) {
-			is >> tag;
-
-			if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) {
-				int id1, id2;
-				double x, y, yaw;
-
-				is >> id1 >> id2 >> x >> y >> yaw;
-				Matrix m = eye(3);
-				is >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >> m(1, 2);
-				m(2, 0) = m(0, 2);
-				m(2, 1) = m(1, 2);
-				m(1, 0) = m(0, 1);
-
-				// optional filter
-				if (maxID && (id1 >= maxID || id2 >= maxID))
-					continue;
-
-				Pose2 l1Xl2(x, y, yaw);
-
-				// SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart);
-				if (!model) {
-					Vector variances = Vector_(3, m(0, 0), m(1, 1), m(2, 2));
-					model = noiseModel::Diagonal::Variances(variances, smart);
-				}
-
-				if (addNoise)
-					l1Xl2 = l1Xl2.retract((*model)->sample());
-
-				// Insert vertices if pure odometry file
-				if (!poses->exists(id1))
-					poses->insert(id1, Pose2());
-				if (!poses->exists(id2))
-					poses->insert(id2, poses->at<Pose2>(id1) * l1Xl2);
-
-				pose2SLAM::Graph::sharedFactor factor(
-						new pose2SLAM::Odometry(id1, id2, l1Xl2, *model));
-				graph->push_back(factor);
-			}
-			is.ignore(LINESIZE, '\n');
-		}
-
-		cout << "load2D read a graph file with " << poses->size()
-				<< " vertices and " << graph->nrFactors() << " factors" << endl;
-
-		return make_pair(graph, poses);
-	}
-
-	/* ************************************************************************* */
-	void save2D(const pose2SLAM::Graph& graph, const Values& config,
-			const SharedDiagonal model, const string& filename) {
-
-		fstream stream(filename.c_str(), fstream::out);
-
-		// save poses
-		BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config)
-				{
-					const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
-					stream << "VERTEX2 " << key_value.key << " " << 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> factor_, graph)
-				{
-					boost::shared_ptr<pose2SLAM::Odometry> factor =
-							boost::dynamic_pointer_cast<pose2SLAM::Odometry>(factor_);
-					if (!factor)
-						continue;
-
-					Pose2 pose = factor->measured().inverse();
-					stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " "
-							<< pose.x() << " " << pose.y() << " " << pose.theta() << " "
-							<< RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " "
-							<< RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl;
-				}
-
-		stream.close();
-	}
-
-	/* ************************************************************************* */
-	bool load3D(const string& filename) {
-		ifstream is(filename.c_str());
-		if (!is)
-			return false;
-
-		while (is) {
-			char buf[LINESIZE];
-			is.getline(buf, LINESIZE);
-			istringstream ls(buf);
-			string tag;
-			ls >> tag;
-
-			if (tag == "VERTEX3") {
-				int id;
-				double x, y, z, roll, pitch, yaw;
-				ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
-			}
-		}
-		is.clear(); /* clears the end-of-file and error flags */
-		is.seekg(0, ios::beg);
-
-		while (is) {
-			char buf[LINESIZE];
-			is.getline(buf, LINESIZE);
-			istringstream ls(buf);
-			string tag;
-			ls >> tag;
-
-			if (tag == "EDGE3") {
-				int id1, id2;
-				double x, y, z, roll, pitch, yaw;
-				ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
-				Matrix m = eye(6);
-				for (int i = 0; i < 6; i++)
-					for (int j = i; j < 6; j++)
-						ls >> m(i, j);
-			}
-		}
-		return true;
-	}
 }
+
+/* ************************************************************************* */
+pair<pose2SLAM::Graph::shared_ptr, pose2SLAM::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<pose2SLAM::Graph::shared_ptr, pose2SLAM::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());
+	if (!is) {
+		cout << "load2D: can not find the file!";
+		exit(-1);
+	}
+
+	pose2SLAM::Values::shared_ptr poses(new pose2SLAM::Values);
+	pose2SLAM::Graph::shared_ptr graph(new pose2SLAM::Graph);
+
+	string tag;
+
+	// load the poses
+	while (is) {
+		is >> tag;
+
+		if ((tag == "VERTEX2") || (tag == "VERTEX")) {
+			int id;
+			double x, y, yaw;
+			is >> id >> x >> y >> yaw;
+			// optional filter
+			if (maxID && id >= maxID)
+				continue;
+			poses->insert(id, Pose2(x, y, yaw));
+		}
+		is.ignore(LINESIZE, '\n');
+	}
+	is.clear(); /* clears the end-of-file and error flags */
+	is.seekg(0, ios::beg);
+
+	// Create a sampler with random number generator
+	Sampler sampler(42u);
+
+	// load the factors
+	while (is) {
+		is >> tag;
+
+		if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) {
+			int id1, id2;
+			double x, y, yaw;
+
+			is >> id1 >> id2 >> x >> y >> yaw;
+			Matrix m = eye(3);
+			is >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >> m(1, 2);
+			m(2, 0) = m(0, 2);
+			m(2, 1) = m(1, 2);
+			m(1, 0) = m(0, 1);
+
+			// optional filter
+			if (maxID && (id1 >= maxID || id2 >= maxID))
+				continue;
+
+			Pose2 l1Xl2(x, y, yaw);
+
+			// SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart);
+			if (!model) {
+				Vector variances = Vector_(3, m(0, 0), m(1, 1), m(2, 2));
+				model = noiseModel::Diagonal::Variances(variances, smart);
+			}
+
+			if (addNoise)
+				l1Xl2 = l1Xl2.retract(sampler.sampleNewModel(*model));
+
+			// Insert vertices if pure odometry file
+			if (!poses->exists(id1))
+				poses->insert(id1, Pose2());
+			if (!poses->exists(id2))
+				poses->insert(id2, poses->at<Pose2>(id1) * l1Xl2);
+
+			pose2SLAM::Graph::sharedFactor factor(
+					new pose2SLAM::Odometry(id1, id2, l1Xl2, *model));
+			graph->push_back(factor);
+		}
+		is.ignore(LINESIZE, '\n');
+	}
+
+	cout << "load2D read a graph file with " << poses->size()
+						<< " vertices and " << graph->nrFactors() << " factors" << endl;
+
+	return make_pair(graph, poses);
+}
+
+/* ************************************************************************* */
+void save2D(const pose2SLAM::Graph& graph, const Values& config,
+		const SharedDiagonal model, const string& filename) {
+
+	fstream stream(filename.c_str(), fstream::out);
+
+	// save poses
+	BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config)
+	{
+		const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
+		stream << "VERTEX2 " << key_value.key << " " << 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> factor_, graph)
+	{
+		boost::shared_ptr<pose2SLAM::Odometry> factor =
+				boost::dynamic_pointer_cast<pose2SLAM::Odometry>(factor_);
+		if (!factor)
+			continue;
+
+		Pose2 pose = factor->measured().inverse();
+		stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " "
+				<< pose.x() << " " << pose.y() << " " << pose.theta() << " "
+				<< RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " "
+				<< RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl;
+	}
+
+	stream.close();
+}
+
+/* ************************************************************************* */
+bool load3D(const string& filename) {
+	ifstream is(filename.c_str());
+	if (!is)
+		return false;
+
+	while (is) {
+		char buf[LINESIZE];
+		is.getline(buf, LINESIZE);
+		istringstream ls(buf);
+		string tag;
+		ls >> tag;
+
+		if (tag == "VERTEX3") {
+			int id;
+			double x, y, z, roll, pitch, yaw;
+			ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
+		}
+	}
+	is.clear(); /* clears the end-of-file and error flags */
+	is.seekg(0, ios::beg);
+
+	while (is) {
+		char buf[LINESIZE];
+		is.getline(buf, LINESIZE);
+		istringstream ls(buf);
+		string tag;
+		ls >> tag;
+
+		if (tag == "EDGE3") {
+			int id1, id2;
+			double x, y, z, roll, pitch, yaw;
+			ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
+			Matrix m = eye(6);
+			for (int i = 0; i < 6; i++)
+				for (int j = i; j < 6; j++)
+					ls >> m(i, j);
+		}
+	}
+	return true;
+}
+/* ************************************************************************* */
+
+} // \namespace gtsam