diff --git a/gtsam.h b/gtsam.h index d06bd0b4f..21ee927db 100644 --- a/gtsam.h +++ b/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 { diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 19546016f..a7e8f2769 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -23,13 +23,11 @@ #include #include #include +#include #include #include #include -#include -#include - #include #include @@ -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 norm_dist(mean, sigma); - boost::variate_generator > norm(generator, norm_dist); - - Vector v(dim); - for(int i = 0; i #include +#include #include #include -#include - -/** - * 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& 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 diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 6657d41f9..b8bb0333b 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/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); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 51f732db0..f212118c9 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/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 Normal; - Normal dist(0.0, this->sigmas_(i)); - boost::variate_generator 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 Normal; - Normal dist(0.0, this->sigma_); - boost::variate_generator norm(generator, dist); - Vector result(dim_); - for (size_t i = 0; i < dim_; i++) - result(i) = norm(); - return result; -} - /* ************************************************************************* */ // Unit /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index b1e65feff..50e59a85d 100644 --- a/gtsam/linear/NoiseModel.h +++ b/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; diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index ea2c04106..6031440bd 100644 --- a/gtsam/linear/Sampler.cpp +++ b/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); diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index 71abdfc07..3f6fee7ca 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -20,6 +20,8 @@ #include +#include + 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: diff --git a/gtsam/linear/SharedDiagonal.h b/gtsam/linear/SharedDiagonal.h index 193a30215..f57d6d726 100644 --- a/gtsam/linear/SharedDiagonal.h +++ b/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 diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 0a32e6812..f3d82511e 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -24,6 +24,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 6ca0ccd81..94a174543 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/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() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index d4be33f31..c2af9af36 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -20,226 +20,231 @@ #include #include +#include #include using namespace std; -using namespace gtsam; #define LINESIZE 81920 namespace gtsam { - /* ************************************************************************* */ - pair > dataset(const string& dataset, - const string& user_path) { - string path = user_path, set = dataset; - boost::optional null_model; - boost::optional identity(noiseModel::Unit::Create(3)); - boost::optional small( - noiseModel::Diagonal::Variances( - gtsam::Vector_(3, 0.0001, 0.0001, 0.0003))); +/* ************************************************************************* */ +pair > dataset(const string& dataset, + const string& user_path) { + string path = user_path, set = dataset; + boost::optional null_model; + boost::optional identity(noiseModel::Unit::Create(3)); + boost::optional small( + noiseModel::Diagonal::Variances( + gtsam::Vector_(3, 0.0001, 0.0001, 0.0003))); - if (path.empty()) - path = string(getenv("HOME")) + "/data"; - if (set.empty()) - set = string(getenv("DATASET")); + if (path.empty()) + path = string(getenv("HOME")) + "/data"; + if (set.empty()) + set = string(getenv("DATASET")); - if (set == "intel") - return make_pair(path + "/Intel/intel.graph", null_model); - if (set == "intel-gfs") - return make_pair(path + "/Intel/intel.gfs.graph", null_model); - if (set == "Killian-gfs") - return make_pair(path + "/Killian/Killian.gfs.graph", null_model); - if (set == "Killian") - return make_pair(path + "/Killian/Killian.graph", small); - if (set == "Killian-noised") - return make_pair(path + "/Killian/Killian-noised.graph", null_model); - if (set == "3") - return make_pair(path + "/TORO/w3-odom.graph", identity); - if (set == "100") - return make_pair(path + "/TORO/w100-odom.graph", identity); - if (set == "10K") - return make_pair(path + "/TORO/w10000-odom.graph", identity); - if (set == "10K2") - return make_pair(path + "/hogman/data/2D/w10000.graph", - noiseModel::Diagonal::Variances(gtsam::Vector_(3, 0.1, 0.1, 0.05))); - if (set == "Eiffel100") - return make_pair(path + "/TORO/w100-Eiffel.graph", identity); - if (set == "Eiffel10K") - return make_pair(path + "/TORO/w10000-Eiffel.graph", identity); - if (set == "olson") - return make_pair(path + "/Olson/olson06icra.graph", null_model); - if (set == "victoria") - return make_pair(path + "/VictoriaPark/victoria_park.graph", null_model); - if (set == "beijing") - return make_pair(path + "/Beijing/beijingData_trips.graph", null_model); + if (set == "intel") + return make_pair(path + "/Intel/intel.graph", null_model); + if (set == "intel-gfs") + return make_pair(path + "/Intel/intel.gfs.graph", null_model); + if (set == "Killian-gfs") + return make_pair(path + "/Killian/Killian.gfs.graph", null_model); + if (set == "Killian") + return make_pair(path + "/Killian/Killian.graph", small); + if (set == "Killian-noised") + return make_pair(path + "/Killian/Killian-noised.graph", null_model); + if (set == "3") + return make_pair(path + "/TORO/w3-odom.graph", identity); + if (set == "100") + return make_pair(path + "/TORO/w100-odom.graph", identity); + if (set == "10K") + return make_pair(path + "/TORO/w10000-odom.graph", identity); + if (set == "10K2") + return make_pair(path + "/hogman/data/2D/w10000.graph", + noiseModel::Diagonal::Variances(gtsam::Vector_(3, 0.1, 0.1, 0.05))); + if (set == "Eiffel100") + return make_pair(path + "/TORO/w100-Eiffel.graph", identity); + if (set == "Eiffel10K") + return make_pair(path + "/TORO/w10000-Eiffel.graph", identity); + if (set == "olson") + return make_pair(path + "/Olson/olson06icra.graph", null_model); + if (set == "victoria") + return make_pair(path + "/VictoriaPark/victoria_park.graph", null_model); + if (set == "beijing") + return make_pair(path + "/Beijing/beijingData_trips.graph", null_model); - return make_pair("unknown", null_model); - } - - /* ************************************************************************* */ - - pair load2D( - pair > dataset, - int maxID, bool addNoise, bool smart) { - return load2D(dataset.first, dataset.second, maxID, addNoise, smart); - } - - pair load2D( - const string& filename, boost::optional 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(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(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 factor_, graph) - { - boost::shared_ptr factor = - boost::dynamic_pointer_cast(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; - } + return make_pair("unknown", null_model); } + +/* ************************************************************************* */ +pair load2D( + pair > dataset, + int maxID, bool addNoise, bool smart) { + return load2D(dataset.first, dataset.second, maxID, addNoise, smart); +} + +/* ************************************************************************* */ +pair load2D( + const string& filename, boost::optional 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(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(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 factor_, graph) + { + boost::shared_ptr factor = + boost::dynamic_pointer_cast(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