12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Removed sample() functions and global random number generator

release/4.3a0
Alex Cunningham 2012-06-22 16:38:01 +00:00
parent ae9088efdc
commit 18ba9bcb3d
12 changed files with 238 additions and 331 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam.h
View File

@ -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 {

19
gtsam/base/Vector.cpp
View File

@ -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

26
gtsam/base/Vector.h
View File

@ -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>

21
gtsam/base/tests/testVector.cpp
View File

@ -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); }
/* ************************************************************************* */

24
gtsam/linear/NoiseModel.cpp
View File

@ -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
/* ************************************************************************* */

10
gtsam/linear/NoiseModel.h
View File

@ -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;

2
gtsam/linear/Sampler.cpp
View File

@ -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);

4
gtsam/linear/Sampler.h
View File

@ -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:

3
gtsam/linear/SharedDiagonal.h
View File

@ -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>

1
gtsam/linear/VectorValues.h
View File

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

14
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -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); }
/* ************************************************************************* */

427
gtsam/slam/dataset.cpp
View File

@ -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,
const string& user_path) {
string path = user_path, set = dataset;
boost::optional<SharedDiagonal> null_model;
boost::optional<SharedDiagonal> identity(noiseModel::Unit::Create(3));
boost::optional<SharedDiagonal> small(
noiseModel::Diagonal::Variances(
gtsam::Vector_(3, 0.0001, 0.0001, 0.0003)));
/* ************************************************************************* */
pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset,
const string& user_path) {
string path = user_path, set = dataset;
boost::optional<SharedDiagonal> null_model;
boost::optional<SharedDiagonal> identity(noiseModel::Unit::Create(3));
boost::optional<SharedDiagonal> 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<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;
}
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);
// 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