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

clean up noise model: Remove Shared[NoiseModel] classes and headers, typedef for backward compatibility in NoiseModel.h. Fix all tests and examples to create shared noise models through static functions in noise model classes. Fix MATLAB wrapper and examples as well. Add tests for MATLAB examples

release/4.3a0
Duy-Nguyen Ta 2012-06-22 19:36:49 +00:00
parent 7a48a03b25
commit 6f1ea87a00
77 changed files with 708 additions and 504 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
examples/CameraResectioning.cpp
View File

@ -22,6 +22,7 @@
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
using namespace gtsam; using namespace gtsam;
using namespace gtsam::noiseModel;
using symbol_shorthand::X; using symbol_shorthand::X;
/** /**
@ -69,7 +70,7 @@ int main(int argc, char* argv[]) {
/* 2. add factors to the graph */ /* 2. add factors to the graph */
// add measurement factors // add measurement factors
SharedDiagonal measurementNoise = sharedSigmas(Vector_(2, 0.5, 0.5)); SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector_(2, 0.5, 0.5));
boost::shared_ptr<ResectioningFactor> factor; boost::shared_ptr<ResectioningFactor> factor;
graph.push_back( graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib, boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,

5
examples/LocalizationExample.cpp
View File

@ -27,6 +27,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::noiseModel;
/** /**
* UnaryFactor * UnaryFactor
@ -63,12 +64,12 @@ int main(int argc, char** argv) {
// add two odometry factors // add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(1, 2, odometry, odometryNoise); graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise); graph.addOdometry(2, 3, odometry, odometryNoise);
// add unary measurement factors, like GPS, on all three poses // add unary measurement factors, like GPS, on all three poses
SharedDiagonal noiseModel(Vector_(2, 0.1, 0.1)); // 10cm std on x,y SharedDiagonal noiseModel = Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // 10cm std on x,y
graph.push_back(boost::make_shared<UnaryFactor>(1, 0, 0, noiseModel)); graph.push_back(boost::make_shared<UnaryFactor>(1, 0, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(2, 2, 0, noiseModel)); graph.push_back(boost::make_shared<UnaryFactor>(2, 2, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(3, 4, 0, noiseModel)); graph.push_back(boost::make_shared<UnaryFactor>(3, 4, 0, noiseModel));

5
examples/OdometryExample.cpp
View File

@ -26,6 +26,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::noiseModel;
/** /**
* Example of a simple 2D localization example * Example of a simple 2D localization example
@ -40,12 +41,12 @@ int main(int argc, char** argv) {
// add a Gaussian prior on pose x_1 // add a Gaussian prior on pose x_1
Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin
SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.addPrior(1, priorMean, priorNoise); // add directly to graph graph.addPrior(1, priorMean, priorNoise); // add directly to graph
// add two odometry factors // add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(1, 2, odometry, odometryNoise); graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise); graph.addOdometry(2, 3, odometry, odometryNoise);

7
examples/PlanarSLAMExample.cpp
View File

@ -23,6 +23,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::noiseModel;
/** /**
* Example of a simple 2D planar slam example with landmarls * Example of a simple 2D planar slam example with landmarls
@ -44,17 +45,17 @@ int main(int argc, char** argv) {
// add a Gaussian prior on pose x_1 // add a Gaussian prior on pose x_1
Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin
SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.addPrior(i1, priorMean, priorNoise); // add directly to graph graph.addPrior(i1, priorMean, priorNoise); // add directly to graph
// add two odometry factors // add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(i1, i2, odometry, odometryNoise); graph.addOdometry(i1, i2, odometry, odometryNoise);
graph.addOdometry(i2, i3, odometry, odometryNoise); graph.addOdometry(i2, i3, odometry, odometryNoise);
// create a noise model for the landmark measurements // create a noise model for the landmark measurements
SharedDiagonal measurementNoise(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
// create the measurement values - indices are (pose id, landmark id) // create the measurement values - indices are (pose id, landmark id)
Rot2 bearing11 = Rot2::fromDegrees(45), Rot2 bearing11 = Rot2::fromDegrees(45),

7
examples/Pose2SLAMExample.cpp
View File

@ -22,6 +22,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::noiseModel;
int main(int argc, char** argv) { int main(int argc, char** argv) {
@ -30,18 +31,18 @@ int main(int argc, char** argv) {
// 2a. Add Gaussian prior // 2a. Add Gaussian prior
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
graph.addPrior(1, priorMean, priorNoise); graph.addPrior(1, priorMean, priorNoise);
// 2b. Add odometry factors // 2b. Add odometry factors
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 2c. Add pose constraint // 2c. Add pose constraint
SharedDiagonal model(Vector_(3, 0.2, 0.2, 0.1)); SharedDiagonal model = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), model); graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), model);
// print // print

5
examples/Pose2SLAMExample_advanced.cpp
View File

@ -29,6 +29,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::noiseModel;
int main(int argc, char** argv) { int main(int argc, char** argv) {
/* 1. create graph container and add factors to it */ /* 1. create graph container and add factors to it */
@ -36,11 +37,11 @@ int main(int argc, char** argv) {
/* 2.a add prior */ /* 2.a add prior */
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.addPrior(1, priorMean, priorNoise); // add directly to graph graph.addPrior(1, priorMean, priorNoise); // add directly to graph
/* 2.b add odometry */ /* 2.b add odometry */
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph.addOdometry(1, 2, odometry, odometryNoise); graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise); graph.addOdometry(2, 3, odometry, odometryNoise);

5
examples/Pose2SLAMExample_graph.cpp
View File

@ -24,6 +24,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::noiseModel;
int main(int argc, char** argv) { int main(int argc, char** argv) {
@ -31,13 +32,13 @@ int main(int argc, char** argv) {
// we are in build/examples, data is in examples/Data // we are in build/examples, data is in examples/Data
pose2SLAM::Graph::shared_ptr graph ; pose2SLAM::Graph::shared_ptr graph ;
pose2SLAM::Values::shared_ptr initial; pose2SLAM::Values::shared_ptr initial;
SharedDiagonal model(Vector_(3, 0.05, 0.05, 5.0*M_PI/180.0)); SharedDiagonal model = Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 5.0*M_PI/180.0));
boost::tie(graph,initial) = load2D("../../examples/Data/w100-odom.graph",model); boost::tie(graph,initial) = load2D("../../examples/Data/w100-odom.graph",model);
initial->print("Initial estimate:\n"); initial->print("Initial estimate:\n");
// Add a Gaussian prior on first poses // Add a Gaussian prior on first poses
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise(Vector_(3, 0.01, 0.01, 0.01)); SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.01, 0.01, 0.01));
graph->addPrior(0, priorMean, priorNoise); graph->addPrior(0, priorMean, priorNoise);
// Single Step Optimization using Levenberg-Marquardt // Single Step Optimization using Levenberg-Marquardt

7
examples/Pose2SLAMwSPCG.cpp
View File

@ -26,6 +26,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::noiseModel;
/* ************************************************************************* */ /* ************************************************************************* */
int main(void) { int main(void) {
@ -35,18 +36,18 @@ int main(void) {
// 2a. Add Gaussian prior // 2a. Add Gaussian prior
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
graph.addPrior(1, priorMean, priorNoise); graph.addPrior(1, priorMean, priorNoise);
// 2b. Add odometry factors // 2b. Add odometry factors
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 2c. Add pose constraint // 2c. Add pose constraint
SharedDiagonal constraintUncertainty(Vector_(3, 0.2, 0.2, 0.1)); SharedDiagonal constraintUncertainty = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty);
// print // print

6
examples/VisualSLAMData.h
View File

@ -73,7 +73,7 @@ struct VisualSLAMExampleData {
data.odometry = data.poses[0].between(data.poses[1]); data.odometry = data.poses[0].between(data.poses[1]);
// Simulated measurements, possibly with Gaussian noise // Simulated measurements, possibly with Gaussian noise
data.noiseZ = gtsam::sharedSigma(2, 1.0); data.noiseZ = gtsam::noiseModel::Isotropic::Sigma(2, 1.0);
for (size_t i=0; i<data.poses.size(); ++i) { for (size_t i=0; i<data.poses.size(); ++i) {
for (size_t j=0; j<data.points.size(); ++j) { for (size_t j=0; j<data.points.size(); ++j) {
gtsam::SimpleCamera camera(data.poses[i], *data.sK); gtsam::SimpleCamera camera(data.poses[i], *data.sK);
@ -81,8 +81,8 @@ struct VisualSLAMExampleData {
/*+ gtsam::Point2(data.noiseZ->sample()))*/); // you can add noise as desired /*+ gtsam::Point2(data.noiseZ->sample()))*/); // you can add noise as desired
} }
} }
data.noiseX = gtsam::sharedSigmas(gtsam::Vector_(6, 0.001, 0.001, 0.001, 0.1, 0.1, 0.1)); data.noiseX = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector_(6, 0.001, 0.001, 0.001, 0.1, 0.1, 0.1));
data.noiseL = gtsam::sharedSigma(3, 0.1); data.noiseL = gtsam::noiseModel::Isotropic::Sigma(3, 0.1);
return data; return data;
} }

72
gtsam.h
View File

@ -679,6 +679,9 @@ class VariableIndex {
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
namespace noiseModel { namespace noiseModel {
class Base {
};
class Gaussian { class Gaussian {
static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
static gtsam::noiseModel::Gaussian* Covariance(Matrix R); static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
@ -707,29 +710,6 @@ class Unit {
}; };
}///\namespace noiseModel }///\namespace noiseModel
// TODO: smart flag not supported. Default argument is not wrapped properly yet
class SharedNoiseModel {
static gtsam::SharedNoiseModel SqrtInformation(Matrix R);
static gtsam::SharedNoiseModel Covariance(Matrix covariance);
static gtsam::SharedNoiseModel Sigmas(Vector sigmas);
static gtsam::SharedNoiseModel Variances(Vector variances);
static gtsam::SharedNoiseModel Precisions(Vector precisions);
static gtsam::SharedNoiseModel Sigma(size_t dim, double sigma);
static gtsam::SharedNoiseModel Variance(size_t dim, double variance);
static gtsam::SharedNoiseModel Precision(size_t dim, double precision);
static gtsam::SharedNoiseModel Unit(size_t dim);
void print(string s) const;
};
class SharedGaussian {
SharedGaussian(Matrix covariance);
void print(string s) const;
};
class SharedDiagonal {
SharedDiagonal(Vector sigmas);
void print(string s) const;
};
class Sampler { class Sampler {
Sampler(gtsam::noiseModel::Diagonal* model, int seed); Sampler(gtsam::noiseModel::Diagonal* model, int seed);
@ -789,11 +769,11 @@ class JacobianFactor {
JacobianFactor(); JacobianFactor();
JacobianFactor(Vector b_in); JacobianFactor(Vector b_in);
JacobianFactor(size_t i1, Matrix A1, Vector b, JacobianFactor(size_t i1, Matrix A1, Vector b,
const gtsam::SharedDiagonal& model); const gtsam::noiseModel::Diagonal* model);
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b,
const gtsam::SharedDiagonal& model); const gtsam::noiseModel::Diagonal* model);
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3,
Vector b, const gtsam::SharedDiagonal& model); Vector b, const gtsam::noiseModel::Diagonal* model);
void print(string s) const; void print(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const;
bool empty() const; bool empty() const;
@ -881,13 +861,13 @@ class KalmanFilter {
void print(string s) const; void print(string s) const;
static size_t step(gtsam::GaussianDensity* p); static size_t step(gtsam::GaussianDensity* p);
gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F,
Matrix B, Vector u, const gtsam::SharedDiagonal& modelQ); Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ);
gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F,
Matrix B, Vector u, Matrix Q); Matrix B, Vector u, Matrix Q);
gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0,
Matrix A1, Vector b, const gtsam::SharedDiagonal& model); Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model);
gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H,
Vector z, const gtsam::SharedDiagonal& model); Vector z, const gtsam::noiseModel::Diagonal* model);
gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H,
Vector z, Matrix Q); Vector z, Matrix Q);
}; };
@ -1024,10 +1004,10 @@ class Graph {
const gtsam::Ordering& ordering) const; const gtsam::Ordering& ordering) const;
// pose2SLAM-specific // pose2SLAM-specific
void addPrior(size_t key, const gtsam::Pose2& pose, const gtsam::SharedNoiseModel& noiseModel); void addPrior(size_t key, const gtsam::Pose2& pose, const gtsam::noiseModel::Base* noiseModel);
void addPoseConstraint(size_t key, const gtsam::Pose2& pose); void addPoseConstraint(size_t key, const gtsam::Pose2& pose);
void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel); void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::noiseModel::Base* noiseModel);
void addConstraint(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel); void addConstraint(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::noiseModel::Base* noiseModel);
pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate) const; pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate) const;
pose2SLAM::Values optimizeSPCG(const pose2SLAM::Values& initialEstimate) const; pose2SLAM::Values optimizeSPCG(const pose2SLAM::Values& initialEstimate) const;
gtsam::Marginals marginals(const pose2SLAM::Values& solution) const; gtsam::Marginals marginals(const pose2SLAM::Values& solution) const;
@ -1075,8 +1055,8 @@ class Graph {
const gtsam::Ordering& ordering) const; const gtsam::Ordering& ordering) const;
// pose3SLAM-specific // pose3SLAM-specific
void addPrior(size_t key, const gtsam::Pose3& p, const gtsam::SharedNoiseModel& model); void addPrior(size_t key, const gtsam::Pose3& p, const gtsam::noiseModel::Base* model);
void addConstraint(size_t key1, size_t key2, const gtsam::Pose3& z, const gtsam::SharedNoiseModel& model); void addConstraint(size_t key1, size_t key2, const gtsam::Pose3& z, const gtsam::noiseModel::Base* model);
void addHardConstraint(size_t i, const gtsam::Pose3& p); void addHardConstraint(size_t i, const gtsam::Pose3& p);
pose3SLAM::Values optimize(const pose3SLAM::Values& initialEstimate) const; pose3SLAM::Values optimize(const pose3SLAM::Values& initialEstimate) const;
gtsam::Marginals marginals(const pose3SLAM::Values& solution) const; gtsam::Marginals marginals(const pose3SLAM::Values& solution) const;
@ -1123,19 +1103,19 @@ class Graph {
const gtsam::Ordering& ordering) const; const gtsam::Ordering& ordering) const;
// planarSLAM-specific // planarSLAM-specific
void addPrior(size_t key, const gtsam::Pose2& pose, const gtsam::SharedNoiseModel& noiseModel); void addPrior(size_t key, const gtsam::Pose2& pose, const gtsam::noiseModel::Base* noiseModel);
void addPoseConstraint(size_t key, const gtsam::Pose2& pose); void addPoseConstraint(size_t key, const gtsam::Pose2& pose);
void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel); void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::noiseModel::Base* noiseModel);
void addBearing(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing, const gtsam::SharedNoiseModel& noiseModel); void addBearing(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing, const gtsam::noiseModel::Base* noiseModel);
void addRange(size_t poseKey, size_t pointKey, double range, const gtsam::SharedNoiseModel& noiseModel); void addRange(size_t poseKey, size_t pointKey, double range, const gtsam::noiseModel::Base* noiseModel);
void addBearingRange(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing,double range, const gtsam::SharedNoiseModel& noiseModel); void addBearingRange(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing,double range, const gtsam::noiseModel::Base* noiseModel);
planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate); planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
gtsam::Marginals marginals(const planarSLAM::Values& solution) const; gtsam::Marginals marginals(const planarSLAM::Values& solution) const;
}; };
class Odometry { class Odometry {
Odometry(size_t key1, size_t key2, const gtsam::Pose2& measured, Odometry(size_t key1, size_t key2, const gtsam::Pose2& measured,
const gtsam::SharedNoiseModel& model); const gtsam::noiseModel::Base* model);
void print(string s) const; void print(string s) const;
gtsam::GaussianFactor* linearize(const planarSLAM::Values& center, gtsam::GaussianFactor* linearize(const planarSLAM::Values& center,
const gtsam::Ordering& ordering) const; const gtsam::Ordering& ordering) const;
@ -1190,9 +1170,9 @@ class Graph {
const gtsam::Ordering& ordering) const; const gtsam::Ordering& ordering) const;
// Measurements // Measurements
void addMeasurement(const gtsam::Point2& measured, const gtsam::SharedNoiseModel& model, void addMeasurement(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model,
size_t poseKey, size_t pointKey, const gtsam::Cal3_S2* K); size_t poseKey, size_t pointKey, const gtsam::Cal3_S2* K);
void addStereoMeasurement(const gtsam::StereoPoint2& measured, const gtsam::SharedNoiseModel& model, void addStereoMeasurement(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* model,
size_t poseKey, size_t pointKey, const gtsam::Cal3_S2Stereo* K); size_t poseKey, size_t pointKey, const gtsam::Cal3_S2Stereo* K);
// Constraints // Constraints
@ -1200,10 +1180,10 @@ class Graph {
void addPointConstraint(size_t pointKey, const gtsam::Point3& p); void addPointConstraint(size_t pointKey, const gtsam::Point3& p);
// Priors // Priors
void addPosePrior(size_t poseKey, const gtsam::Pose3& p, const gtsam::SharedNoiseModel& model); void addPosePrior(size_t poseKey, const gtsam::Pose3& p, const gtsam::noiseModel::Base* model);
void addPointPrior(size_t pointKey, const gtsam::Point3& p, const gtsam::SharedNoiseModel& model); void addPointPrior(size_t pointKey, const gtsam::Point3& p, const gtsam::noiseModel::Base* model);
void addRangeFactor(size_t poseKey, size_t pointKey, double range, const gtsam::SharedNoiseModel& model); void addRangeFactor(size_t poseKey, size_t pointKey, double range, const gtsam::noiseModel::Base* model);
void addOdometry(size_t poseKey1, size_t poseKey2, const gtsam::Pose3& odometry, const gtsam::SharedNoiseModel& model); void addOdometry(size_t poseKey1, size_t poseKey2, const gtsam::Pose3& odometry, const gtsam::noiseModel::Base* model);
visualSLAM::Values optimize(const visualSLAM::Values& initialEstimate, size_t verbosity) const; visualSLAM::Values optimize(const visualSLAM::Values& initialEstimate, size_t verbosity) const;
gtsam::Marginals marginals(const visualSLAM::Values& solution) const; gtsam::Marginals marginals(const visualSLAM::Values& solution) const;

2
gtsam/linear/GaussianFactorGraph.h
View File

@ -31,8 +31,6 @@
namespace gtsam { namespace gtsam {
class SharedDiagonal;
/** return A*x-b /** return A*x-b
* \todo Make this a member function - affects SubgraphPreconditioner */ * \todo Make this a member function - affects SubgraphPreconditioner */
template<class FACTOR> template<class FACTOR>

1
gtsam/linear/HessianFactor.h
View File

@ -33,7 +33,6 @@ namespace gtsam {
// Forward declarations // Forward declarations
class JacobianFactor; class JacobianFactor;
class SharedDiagonal;
class GaussianConditional; class GaussianConditional;
template<class C> class BayesNet; template<class C> class BayesNet;

2
gtsam/linear/JacobianFactor.h
View File

@ -19,7 +19,7 @@
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/Errors.h> #include <gtsam/linear/Errors.h>
#include <gtsam/linear/SharedDiagonal.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/base/blockMatrices.h> #include <gtsam/base/blockMatrices.h>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>

1
gtsam/linear/KalmanFilter.cpp
View File

@ -23,7 +23,6 @@
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/KalmanFilter.h> #include <gtsam/linear/KalmanFilter.h>
#include <gtsam/linear/SharedGaussian.h>
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>

4
gtsam/linear/KalmanFilter.h
View File

@ -18,6 +18,7 @@
*/ */
#include <gtsam/linear/GaussianDensity.h> #include <gtsam/linear/GaussianDensity.h>
#include <gtsam/linear/NoiseModel.h>
#ifndef KALMANFILTER_DEFAULT_FACTORIZATION #ifndef KALMANFILTER_DEFAULT_FACTORIZATION
#define KALMANFILTER_DEFAULT_FACTORIZATION QR #define KALMANFILTER_DEFAULT_FACTORIZATION QR
@ -25,9 +26,6 @@
namespace gtsam { namespace gtsam {
class SharedDiagonal;
class SharedGaussian;
/** /**
* Kalman Filter class * Kalman Filter class
* *

1
gtsam/linear/NoiseModel.cpp
View File

@ -29,7 +29,6 @@
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/base/cholesky.h> #include <gtsam/base/cholesky.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedDiagonal.h>
static double inf = std::numeric_limits<double>::infinity(); static double inf = std::numeric_limits<double>::infinity();
using namespace std; using namespace std;

36
gtsam/linear/NoiseModel.h
View File

@ -24,11 +24,10 @@
namespace gtsam { namespace gtsam {
class SharedDiagonal; // forward declare
/// All noise models live in the noiseModel namespace /// All noise models live in the noiseModel namespace
namespace noiseModel { namespace noiseModel {
// Forward declaration
class Gaussian; class Gaussian;
class Diagonal; class Diagonal;
class Constrained; class Constrained;
@ -157,7 +156,7 @@ namespace gtsam {
* A Gaussian noise model created by specifying a covariance matrix. * A Gaussian noise model created by specifying a covariance matrix.
* @param smart check if can be simplified to derived class * @param smart check if can be simplified to derived class
*/ */
static shared_ptr Covariance(const Matrix& covariance, bool smart=false); static shared_ptr Covariance(const Matrix& covariance, bool smart = true);
virtual void print(const std::string& name) const; virtual void print(const std::string& name) const;
virtual bool equals(const Base& expected, double tol=1e-9) const; virtual bool equals(const Base& expected, double tol=1e-9) const;
@ -199,13 +198,13 @@ namespace gtsam {
* @param Ab is the m*(n+1) augmented system matrix [A b] * @param Ab is the m*(n+1) augmented system matrix [A b]
* @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!! * @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!!
*/ */
virtual SharedDiagonal QR(Matrix& Ab) const; virtual boost::shared_ptr<Diagonal> QR(Matrix& Ab) const;
/** /**
* Cholesky factorization * Cholesky factorization
* FIXME: this is never used anywhere * FIXME: this is never used anywhere
*/ */
virtual SharedDiagonal Cholesky(Matrix& Ab, size_t nFrontals) const; virtual boost::shared_ptr<Diagonal> Cholesky(Matrix& Ab, size_t nFrontals) const;
/** /**
* Return R itself, but note that Whiten(H) is cheaper than R*H * Return R itself, but note that Whiten(H) is cheaper than R*H
@ -263,20 +262,20 @@ namespace gtsam {
* A diagonal noise model created by specifying a Vector of sigmas, i.e. * A diagonal noise model created by specifying a Vector of sigmas, i.e.
* standard devations, the diagonal of the square root covariance matrix. * standard devations, the diagonal of the square root covariance matrix.
*/ */
static shared_ptr Sigmas(const Vector& sigmas, bool smart=false); static shared_ptr Sigmas(const Vector& sigmas, bool smart = true);
/** /**
* A diagonal noise model created by specifying a Vector of variances, i.e. * A diagonal noise model created by specifying a Vector of variances, i.e.
* i.e. the diagonal of the covariance matrix. * i.e. the diagonal of the covariance matrix.
* @param smart check if can be simplified to derived class * @param smart check if can be simplified to derived class
*/ */
static shared_ptr Variances(const Vector& variances, bool smart = false); static shared_ptr Variances(const Vector& variances, bool smart = true);
/** /**
* A diagonal noise model created by specifying a Vector of precisions, i.e. * A diagonal noise model created by specifying a Vector of precisions, i.e.
* i.e. the diagonal of the information matrix, i.e., weights * i.e. the diagonal of the information matrix, i.e., weights
*/ */
static shared_ptr Precisions(const Vector& precisions, bool smart = false) { static shared_ptr Precisions(const Vector& precisions, bool smart = true) {
return Variances(reciprocal(precisions), smart); return Variances(reciprocal(precisions), smart);
} }
@ -365,13 +364,13 @@ namespace gtsam {
* standard devations, some of which might be zero * standard devations, some of which might be zero
*/ */
static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas, static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas,
bool smart = false); bool smart = true);
/** /**
* A diagonal noise model created by specifying a Vector of * A diagonal noise model created by specifying a Vector of
* standard devations, some of which might be zero * standard devations, some of which might be zero
*/ */
static shared_ptr MixedSigmas(const Vector& sigmas, bool smart = false) { static shared_ptr MixedSigmas(const Vector& sigmas, bool smart = true) {
return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas, smart); return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas, smart);
} }
@ -380,7 +379,7 @@ namespace gtsam {
* standard devations, some of which might be zero * standard devations, some of which might be zero
*/ */
static shared_ptr MixedSigmas(double m, const Vector& sigmas, static shared_ptr MixedSigmas(double m, const Vector& sigmas,
bool smart = false) { bool smart = true) {
return MixedSigmas(repeat(sigmas.size(), m), sigmas, smart); return MixedSigmas(repeat(sigmas.size(), m), sigmas, smart);
} }
@ -442,7 +441,7 @@ namespace gtsam {
/** /**
* Apply QR factorization to the system [A b], taking into account constraints * Apply QR factorization to the system [A b], taking into account constraints
*/ */
virtual SharedDiagonal QR(Matrix& Ab) const; virtual Diagonal::shared_ptr QR(Matrix& Ab) const;
/** /**
* Check constrained is always true * Check constrained is always true
@ -493,18 +492,18 @@ namespace gtsam {
/** /**
* An isotropic noise model created by specifying a standard devation sigma * An isotropic noise model created by specifying a standard devation sigma
*/ */
static shared_ptr Sigma(size_t dim, double sigma, bool smart = false); static shared_ptr Sigma(size_t dim, double sigma, bool smart = true);
/** /**
* An isotropic noise model created by specifying a variance = sigma^2. * An isotropic noise model created by specifying a variance = sigma^2.
* @param smart check if can be simplified to derived class * @param smart check if can be simplified to derived class
*/ */
static shared_ptr Variance(size_t dim, double variance, bool smart = false); static shared_ptr Variance(size_t dim, double variance, bool smart = true);
/** /**
* An isotropic noise model created by specifying a precision * An isotropic noise model created by specifying a precision
*/ */
static shared_ptr Precision(size_t dim, double precision, bool smart = false) { static shared_ptr Precision(size_t dim, double precision, bool smart = true) {
return Variance(dim, 1.0/precision, smart); return Variance(dim, 1.0/precision, smart);
} }
@ -718,6 +717,13 @@ namespace gtsam {
} // namespace noiseModel } // namespace noiseModel
/** Note, deliberately not in noiseModel namespace.
* Deprecated. Only for compatibility with previous version.
*/
typedef noiseModel::Base::shared_ptr SharedNoiseModel;
typedef noiseModel::Gaussian::shared_ptr SharedGaussian;
typedef noiseModel::Diagonal::shared_ptr SharedDiagonal;
} // namespace gtsam } // namespace gtsam

88
gtsam/linear/SharedDiagonal.h
View File

@ -1,88 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SharedDiagonal.h
* @brief Class that wraps a shared noise model with diagonal covariance
* @author Frank Dellaert
* @date Jan 22, 2010
*/
#pragma once
#include <gtsam/linear/NoiseModel.h>
namespace gtsam { // note, deliberately not in noiseModel namespace
/**
* A useful convenience class to refer to a shared Diagonal model
* There are (somewhat dangerous) constructors from Vector and pair<size_t,double>
* that call Sigmas and Sigma, respectively.
*/
class SharedDiagonal: public noiseModel::Diagonal::shared_ptr {
public:
/// @name Standard Constructors
/// @{
SharedDiagonal() {
}
SharedDiagonal(const noiseModel::Diagonal::shared_ptr& p) :
noiseModel::Diagonal::shared_ptr(p) {
}
SharedDiagonal(const noiseModel::Constrained::shared_ptr& p) :
noiseModel::Diagonal::shared_ptr(p) {
}
SharedDiagonal(const noiseModel::Isotropic::shared_ptr& p) :
noiseModel::Diagonal::shared_ptr(p) {
}
SharedDiagonal(const noiseModel::Unit::shared_ptr& p) :
noiseModel::Diagonal::shared_ptr(p) {
}
SharedDiagonal(const Vector& sigmas) :
noiseModel::Diagonal::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) {
}
/// @}
/// @name Print
/// @{
/// Print
inline void print(const std::string &s) const { (*this)->print(s); }
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("SharedDiagonal",
boost::serialization::base_object<noiseModel::Diagonal::shared_ptr >(*this));
}
};
// TODO: make these the ones really used in unit tests
inline SharedDiagonal sharedSigmas(const Vector& sigmas) {
return noiseModel::Diagonal::Sigmas(sigmas);
}
inline SharedDiagonal sharedSigma(size_t dim, double sigma) {
return noiseModel::Isotropic::Sigma(dim, sigma);
}
inline SharedDiagonal sharedPrecisions(const Vector& precisions) {
return noiseModel::Diagonal::Precisions(precisions);
}
inline SharedDiagonal sharedPrecision(size_t dim, double precision) {
return noiseModel::Isotropic::Precision(dim, precision);
}
inline SharedDiagonal sharedUnit(size_t dim) {
return noiseModel::Unit::Create(dim);
}
}

70
gtsam/linear/SharedGaussian.h
View File

@ -1,70 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SharedGaussian.h
* @brief Class that wraps a shared Gaussian noise model
* @author Frank Dellaert
* @date Jan 22, 2010
*/
#pragma once
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedNoiseModel.h>
namespace gtsam { // note, deliberately not in noiseModel namespace
/**
* A useful convenience class to refer to a shared Gaussian model
* Also needed to make noise models in matlab
*/
class SharedGaussian: public SharedNoiseModel {
public:
typedef SharedNoiseModel Base;
SharedGaussian() {}
SharedGaussian(const noiseModel::Gaussian::shared_ptr& p) : Base(p) {}
SharedGaussian(const noiseModel::Diagonal::shared_ptr& p) : Base(p) {}
SharedGaussian(const noiseModel::Constrained::shared_ptr& p) : Base(p) {}
SharedGaussian(const noiseModel::Isotropic::shared_ptr& p) : Base(p) {}
SharedGaussian(const noiseModel::Unit::shared_ptr& p) : Base(p) {}
// Define GTSAM_MAGIC_GAUSSIAN to have access to slightly
// dangerous and non-shared (inefficient, wasteful) noise models.
// Intended to be used only in tests (if you must) and the MATLAB wrapper
#ifdef GTSAM_MAGIC_GAUSSIAN
SharedGaussian(const Matrix& covariance)
: Base(noiseModel::Gaussian::Covariance(covariance)) {}
SharedGaussian(const Vector& sigmas)
: Base(noiseModel::Diagonal::Sigmas(sigmas)) {}
#endif
// Define GTSAM_DANGEROUS_GAUSSIAN to have access to bug-prone fixed dimension Gaussians
// Not intended for human use, only for backwards compatibility of old unit tests
#ifdef GTSAM_DANGEROUS_GAUSSIAN
SharedGaussian(const double& s) :
Base(noiseModel::Isotropic::Sigma(GTSAM_DANGEROUS_GAUSSIAN, s)) {}
#endif
/// Print
inline void print(const std::string &s) const { (*this)->print(s); }
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("SharedGaussian",
boost::serialization::base_object<noiseModel::Gaussian::shared_ptr >(*this));
}
};
}

121
gtsam/linear/SharedNoiseModel.h
View File

@ -1,121 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/linear/NoiseModel.h>
namespace gtsam { // note, deliberately not in noiseModel namespace
/**
* Just a convenient class to generate shared pointers to a noise model
*/
struct SharedNoiseModel: public noiseModel::Base::shared_ptr {
typedef noiseModel::Base::shared_ptr Base;
/// @name Standard Constructors
/// @{
SharedNoiseModel() {}
SharedNoiseModel(const noiseModel::Base::shared_ptr& p): Base(p) {}
SharedNoiseModel(const noiseModel::Robust::shared_ptr& p): Base(p) {}
SharedNoiseModel(const noiseModel::Gaussian::shared_ptr& p): Base(p) {}
SharedNoiseModel(const noiseModel::Diagonal::shared_ptr& p): Base(p) {}
SharedNoiseModel(const noiseModel::Constrained::shared_ptr& p): Base(p) {}
SharedNoiseModel(const noiseModel::Isotropic::shared_ptr& p): Base(p) {}
SharedNoiseModel(const noiseModel::Unit::shared_ptr& p): Base(p) {}
/// @}
/// @name Dangerous constructors
/// @{
#ifdef GTSAM_MAGIC_GAUSSIAN
SharedNoiseModel(const Matrix& covariance) :
Base(boost::static_pointer_cast<noiseModel::Base>(
noiseModel::Gaussian::Covariance(covariance))) {}
SharedNoiseModel(const Vector& sigmas) :
Base(boost::static_pointer_cast<noiseModel::Base>(
noiseModel::Diagonal::Sigmas(sigmas))) {}
#endif
// Define GTSAM_DANGEROUS_GAUSSIAN to have access to bug-prone fixed dimension Gaussians
// Not intended for human use, only for backwards compatibility of old unit tests
#ifdef GTSAM_DANGEROUS_GAUSSIAN
SharedNoiseModel(const double& s) :
Base(boost::static_pointer_cast<noiseModel::Base>(
noiseModel::Isotropic::Sigma(GTSAM_DANGEROUS_GAUSSIAN, s))) {}
#endif
/// @}
/// @name Print
/// @{
/// Print
inline void print(const std::string &s) const { (*this)->print(s); }
/// @}
/// @name Static syntactic sugar (should only be used with the MATLAB interface)
/// @{
static inline SharedNoiseModel SqrtInformation(const Matrix& R) {
return noiseModel::Gaussian::SqrtInformation(R);
}
static inline SharedNoiseModel Covariance(const Matrix& covariance, bool smart=false) {
return noiseModel::Gaussian::Covariance(covariance, smart);
}
static inline SharedNoiseModel Sigmas(const Vector& sigmas, bool smart=false) {
return noiseModel::Diagonal::Sigmas(sigmas, smart);
}
static inline SharedNoiseModel Variances(const Vector& variances, bool smart=false) {
return noiseModel::Diagonal::Variances(variances, smart);
}
static inline SharedNoiseModel Precisions(const Vector& precisions, bool smart=false) {
return noiseModel::Diagonal::Precisions(precisions, smart);
}
static inline SharedNoiseModel Sigma(size_t dim, double sigma, bool smart=false) {
return noiseModel::Isotropic::Sigma(dim, sigma, smart);
}
static inline SharedNoiseModel Variance(size_t dim, double variance, bool smart=false) {
return noiseModel::Isotropic::Variance(dim, variance, smart);
}
static inline SharedNoiseModel Precision(size_t dim, double precision, bool smart=false) {
return noiseModel::Isotropic::Precision(dim, precision, smart);
}
static inline SharedNoiseModel Unit(size_t dim) {
return noiseModel::Unit::Create(dim);
}
/// @}
/// @name Serialization
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("SharedNoiseModel",
boost::serialization::base_object<Base>(*this));
}
/// @}
};
}

28
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -36,7 +36,7 @@ static const Index _x0_=0, _x1_=1, _x2_=2, _x3_=3, _x4_=4, _x_=5, _y_=6, _l1_=7,
#endif #endif
static SharedDiagonal static SharedDiagonal
sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2), sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2),
constraintModel = noiseModel::Constrained::All(2); constraintModel = noiseModel::Constrained::All(2);
/* ************************************************************************* */ /* ************************************************************************* */
@ -234,26 +234,26 @@ TEST( NonlinearFactorGraph, combine2){
A11(1,0) = 0; A11(1,1) = 1; A11(1,0) = 0; A11(1,1) = 1;
Vector b(2); Vector b(2);
b(0) = 2; b(1) = -1; b(0) = 2; b(1) = -1;
JacobianFactor::shared_ptr f1(new JacobianFactor(_x1_, A11, b*sigma1, sharedSigma(2,sigma1))); JacobianFactor::shared_ptr f1(new JacobianFactor(_x1_, A11, b*sigma1, noiseModel::Isotropic::Sigma(2,sigma1)));
double sigma2 = 0.5; double sigma2 = 0.5;
A11(0,0) = 1; A11(0,1) = 0; A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -1; A11(1,0) = 0; A11(1,1) = -1;
b(0) = 4 ; b(1) = -5; b(0) = 4 ; b(1) = -5;
JacobianFactor::shared_ptr f2(new JacobianFactor(_x1_, A11, b*sigma2, sharedSigma(2,sigma2))); JacobianFactor::shared_ptr f2(new JacobianFactor(_x1_, A11, b*sigma2, noiseModel::Isotropic::Sigma(2,sigma2)));
double sigma3 = 0.25; double sigma3 = 0.25;
A11(0,0) = 1; A11(0,1) = 0; A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -1; A11(1,0) = 0; A11(1,1) = -1;
b(0) = 3 ; b(1) = -88; b(0) = 3 ; b(1) = -88;
JacobianFactor::shared_ptr f3(new JacobianFactor(_x1_, A11, b*sigma3, sharedSigma(2,sigma3))); JacobianFactor::shared_ptr f3(new JacobianFactor(_x1_, A11, b*sigma3, noiseModel::Isotropic::Sigma(2,sigma3)));
// TODO: find a real sigma value for this example // TODO: find a real sigma value for this example
double sigma4 = 0.1; double sigma4 = 0.1;
A11(0,0) = 6; A11(0,1) = 0; A11(0,0) = 6; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = 7; A11(1,0) = 0; A11(1,1) = 7;
b(0) = 5 ; b(1) = -6; b(0) = 5 ; b(1) = -6;
JacobianFactor::shared_ptr f4(new JacobianFactor(_x1_, A11*sigma4, b*sigma4, sharedSigma(2,sigma4))); JacobianFactor::shared_ptr f4(new JacobianFactor(_x1_, A11*sigma4, b*sigma4, noiseModel::Isotropic::Sigma(2,sigma4)));
vector<JacobianFactor::shared_ptr> lfg; vector<JacobianFactor::shared_ptr> lfg;
lfg.push_back(f1); lfg.push_back(f1);
@ -286,7 +286,7 @@ TEST( NonlinearFactorGraph, combine2){
TEST(GaussianFactor, linearFactorN){ TEST(GaussianFactor, linearFactorN){
Matrix I = eye(2); Matrix I = eye(2);
vector<JacobianFactor::shared_ptr> f; vector<JacobianFactor::shared_ptr> f;
SharedDiagonal model = sharedSigma(2,1.0); SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1.0);
f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x1_, I, Vector_(2, f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x1_, I, Vector_(2,
10.0, 5.0), model))); 10.0, 5.0), model)));
f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x1_, -10 * I, f.push_back(JacobianFactor::shared_ptr(new JacobianFactor(_x1_, -10 * I,
@ -374,7 +374,7 @@ TEST(GaussianFactor, eliminateFrontals)
make_pair( 9, Matrix(Ab.block(0, 6, 4, 2))), make_pair( 9, Matrix(Ab.block(0, 6, 4, 2))),
make_pair(11, Matrix(Ab.block(0, 8, 4, 2))); make_pair(11, Matrix(Ab.block(0, 8, 4, 2)));
Vector b1 = Ab.col(10).segment(0, 4); Vector b1 = Ab.col(10).segment(0, 4);
JacobianFactor::shared_ptr factor1(new JacobianFactor(terms1, b1, sharedSigma(4, 0.5))); JacobianFactor::shared_ptr factor1(new JacobianFactor(terms1, b1, noiseModel::Isotropic::Sigma(4, 0.5)));
// Create second factor // Create second factor
list<pair<Index, Matrix> > terms2; list<pair<Index, Matrix> > terms2;
@ -384,7 +384,7 @@ TEST(GaussianFactor, eliminateFrontals)
make_pair(9, Matrix(Ab.block(4, 6, 4, 2))), make_pair(9, Matrix(Ab.block(4, 6, 4, 2))),
make_pair(11,Matrix(Ab.block(4, 8, 4, 2))); make_pair(11,Matrix(Ab.block(4, 8, 4, 2)));
Vector b2 = Ab.col(10).segment(4, 4); Vector b2 = Ab.col(10).segment(4, 4);
JacobianFactor::shared_ptr factor2(new JacobianFactor(terms2, b2, sharedSigma(4, 0.5))); JacobianFactor::shared_ptr factor2(new JacobianFactor(terms2, b2, noiseModel::Isotropic::Sigma(4, 0.5)));
// Create third factor // Create third factor
list<pair<Index, Matrix> > terms3; list<pair<Index, Matrix> > terms3;
@ -393,14 +393,14 @@ TEST(GaussianFactor, eliminateFrontals)
make_pair(9, Matrix(Ab.block(8, 6, 4, 2))), make_pair(9, Matrix(Ab.block(8, 6, 4, 2))),
make_pair(11,Matrix(Ab.block(8, 8, 4, 2))); make_pair(11,Matrix(Ab.block(8, 8, 4, 2)));
Vector b3 = Ab.col(10).segment(8, 4); Vector b3 = Ab.col(10).segment(8, 4);
JacobianFactor::shared_ptr factor3(new JacobianFactor(terms3, b3, sharedSigma(4, 0.5))); JacobianFactor::shared_ptr factor3(new JacobianFactor(terms3, b3, noiseModel::Isotropic::Sigma(4, 0.5)));
// Create fourth factor // Create fourth factor
list<pair<Index, Matrix> > terms4; list<pair<Index, Matrix> > terms4;
terms4 += terms4 +=
make_pair(11, Matrix(Ab.block(12, 8, 2, 2))); make_pair(11, Matrix(Ab.block(12, 8, 2, 2)));
Vector b4 = Ab.col(10).segment(12, 2); Vector b4 = Ab.col(10).segment(12, 2);
JacobianFactor::shared_ptr factor4(new JacobianFactor(terms4, b4, sharedSigma(2, 0.5))); JacobianFactor::shared_ptr factor4(new JacobianFactor(terms4, b4, noiseModel::Isotropic::Sigma(2, 0.5)));
// Create factor graph // Create factor graph
GaussianFactorGraph factors; GaussianFactorGraph factors;
@ -554,13 +554,13 @@ TEST(GaussianFactor, permuteWithInverse)
inversePermutation[4] = 1; inversePermutation[4] = 1;
inversePermutation[5] = 0; inversePermutation[5] = 0;
JacobianFactor actual(1, A1, 3, A2, 5, A3, b, sharedSigma(2, 1.0)); JacobianFactor actual(1, A1, 3, A2, 5, A3, b, noiseModel::Isotropic::Sigma(2, 1.0));
GaussianFactorGraph actualFG; actualFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(actual))); GaussianFactorGraph actualFG; actualFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(actual)));
VariableIndex actualIndex(actualFG); VariableIndex actualIndex(actualFG);
actual.permuteWithInverse(inversePermutation); actual.permuteWithInverse(inversePermutation);
// actualIndex.permute(*inversePermutation.inverse()); // actualIndex.permute(*inversePermutation.inverse());
JacobianFactor expected(4, A1, 2, A2, 0, A3, b, sharedSigma(2, 1.0)); JacobianFactor expected(4, A1, 2, A2, 0, A3, b, noiseModel::Isotropic::Sigma(2, 1.0));
GaussianFactorGraph expectedFG; expectedFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(expected))); GaussianFactorGraph expectedFG; expectedFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(expected)));
// GaussianVariableIndex expectedIndex(expectedFG); // GaussianVariableIndex expectedIndex(expectedFG);
@ -609,7 +609,7 @@ TEST(GaussianFactorGraph, sparseJacobian) {
4., 6.,32.).transpose(); 4., 6.,32.).transpose();
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
SharedDiagonal model = sharedSigma(2, 0.5); SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model); gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model);
gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model); gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model);
@ -628,7 +628,7 @@ TEST(GaussianFactorGraph, denseHessian) {
// 0 0 0 14 15 16 // 0 0 0 14 15 16
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
SharedDiagonal model = sharedUnit(2); SharedDiagonal model = noiseModel::Unit::Create(2);
gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model); gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model);
gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model); gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model);

2
gtsam/linear/tests/testGaussianJunctionTree.cpp
View File

@ -38,7 +38,7 @@ static const Index x2=0, x1=1, x3=2, x4=3;
GaussianFactorGraph createChain() { GaussianFactorGraph createChain() {
typedef GaussianFactorGraph::sharedFactor Factor; typedef GaussianFactorGraph::sharedFactor Factor;
SharedDiagonal model(Vector_(1, 0.5)); SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 0.5);
Factor factor1(new JacobianFactor(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), Vector_(1,1.), model)); Factor factor1(new JacobianFactor(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), Vector_(1,1.), model));
Factor factor2(new JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), Vector_(1,1.), model)); Factor factor2(new JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), Vector_(1,1.), model));
Factor factor3(new JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), Vector_(1,1.), model)); Factor factor3(new JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), Vector_(1,1.), model));

6
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -93,7 +93,7 @@ TEST(HessianFactor, ConversionConstructor) {
vector<pair<Index, Matrix> > meas; vector<pair<Index, Matrix> > meas;
meas.push_back(make_pair(0, Ax2)); meas.push_back(make_pair(0, Ax2));
meas.push_back(make_pair(1, Al1x1)); meas.push_back(make_pair(1, Al1x1));
JacobianFactor combined(meas, b2, sigmas); JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
HessianFactor actual(combined); HessianFactor actual(combined);
@ -468,7 +468,7 @@ TEST(HessianFactor, eliminate2 )
vector<pair<Index, Matrix> > meas; vector<pair<Index, Matrix> > meas;
meas.push_back(make_pair(0, Ax2)); meas.push_back(make_pair(0, Ax2));
meas.push_back(make_pair(1, Al1x1)); meas.push_back(make_pair(1, Al1x1));
JacobianFactor combined(meas, b2, sigmas); JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
// eliminate the combined factor // eliminate the combined factor
HessianFactor::shared_ptr combinedLF_Chol(new HessianFactor(combined)); HessianFactor::shared_ptr combinedLF_Chol(new HessianFactor(combined));
@ -502,7 +502,7 @@ TEST(HessianFactor, eliminate2 )
0.00, 1.00, +0.00, -1.00 0.00, 1.00, +0.00, -1.00
)/sigma; )/sigma;
Vector b1 = Vector_(2,0.0,0.894427); Vector b1 = Vector_(2,0.0,0.894427);
JacobianFactor expectedLF(1, Bl1x1, b1, repeat(2,1.0)); JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0));
EXPECT(assert_equal(HessianFactor(expectedLF), *actualFactor, 1.5e-3)); EXPECT(assert_equal(HessianFactor(expectedLF), *actualFactor, 1.5e-3));
} }

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

@ -29,7 +29,7 @@ using namespace gtsam;
static const Index _x0_=0, _x1_=1, _x2_=2, _x3_=3, _x4_=4, _x_=5, _y_=6, _l1_=7, _l11_=8; static const Index _x0_=0, _x1_=1, _x2_=2, _x3_=3, _x4_=4, _x_=5, _y_=6, _l1_=7, _l11_=8;
static SharedDiagonal static SharedDiagonal
sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2), sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2),
constraintModel = noiseModel::Constrained::All(2); constraintModel = noiseModel::Constrained::All(2);
/* ************************************************************************* */ /* ************************************************************************* */
@ -76,7 +76,7 @@ TEST(JacobianFactor, constructor2)
JacobianFactor construct() { JacobianFactor construct() {
Matrix A = Matrix_(2,2, 1.,2.,3.,4.); Matrix A = Matrix_(2,2, 1.,2.,3.,4.);
Vector b = Vector_(2, 1.0, 2.0); Vector b = Vector_(2, 1.0, 2.0);
SharedDiagonal s = sharedSigmas(Vector_(2, 3.0, 4.0)); SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector_(2, 3.0, 4.0));
JacobianFactor::shared_ptr shared( JacobianFactor::shared_ptr shared(
new JacobianFactor(0, A, b, s)); new JacobianFactor(0, A, b, s));
return *shared; return *shared;
@ -86,7 +86,7 @@ TEST(JacobianFactor, return_value)
{ {
Matrix A = Matrix_(2,2, 1.,2.,3.,4.); Matrix A = Matrix_(2,2, 1.,2.,3.,4.);
Vector b = Vector_(2, 1.0, 2.0); Vector b = Vector_(2, 1.0, 2.0);
SharedDiagonal s = sharedSigmas(Vector_(2, 3.0, 4.0)); SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector_(2, 3.0, 4.0));
JacobianFactor copied = construct(); JacobianFactor copied = construct();
EXPECT(assert_equal(b, copied.getb())); EXPECT(assert_equal(b, copied.getb()));
EXPECT(assert_equal(*s, *copied.get_model())); EXPECT(assert_equal(*s, *copied.get_model()));
@ -107,7 +107,7 @@ TEST(JabobianFactor, Hessian_conversion) {
1.2530, 2.1508, -0.8779, -1.8755, 1.2530, 2.1508, -0.8779, -1.8755,
0, 2.5858, 0.4789, -2.3943).finished(), 0, 2.5858, 0.4789, -2.3943).finished(),
(Vector(2) << -6.2929, -5.7941).finished(), (Vector(2) << -6.2929, -5.7941).finished(),
sharedUnit(2)); noiseModel::Unit::Create(2));
JacobianFactor actual(hessian); JacobianFactor actual(hessian);
@ -202,7 +202,7 @@ TEST(JacobianFactor, eliminate2 )
vector<pair<Index, Matrix> > meas; vector<pair<Index, Matrix> > meas;
meas.push_back(make_pair(_x2_, Ax2)); meas.push_back(make_pair(_x2_, Ax2));
meas.push_back(make_pair(_l11_, Al1x1)); meas.push_back(make_pair(_l11_, Al1x1));
JacobianFactor combined(meas, b2, sigmas); JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
// eliminate the combined factor // eliminate the combined factor
GaussianConditional::shared_ptr actualCG_QR; GaussianConditional::shared_ptr actualCG_QR;
@ -236,7 +236,7 @@ TEST(JacobianFactor, eliminate2 )
0.00, 1.00, +0.00, -1.00 0.00, 1.00, +0.00, -1.00
)/sigma; )/sigma;
Vector b1 = Vector_(2,0.0,0.894427); Vector b1 = Vector_(2,0.0,0.894427);
JacobianFactor expectedLF(_l11_, Bl1x1, b1, repeat(2,1.0)); JacobianFactor expectedLF(_l11_, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0));
EXPECT(assert_equal(expectedLF,*actualLF_QR,1e-3)); EXPECT(assert_equal(expectedLF,*actualLF_QR,1e-3));
} }
@ -304,7 +304,7 @@ TEST(JacobianFactor, CONSTRUCTOR_GaussianConditional )
// Call the constructor we are testing ! // Call the constructor we are testing !
JacobianFactor actualLF(*CG); JacobianFactor actualLF(*CG);
JacobianFactor expectedLF(_x2_,R11,_l11_,S12,d, sigmas); JacobianFactor expectedLF(_x2_,R11,_l11_,S12,d, noiseModel::Diagonal::Sigmas(sigmas));
EXPECT(assert_equal(expectedLF,actualLF,1e-5)); EXPECT(assert_equal(expectedLF,actualLF,1e-5));
} }

3
gtsam/linear/tests/testKalmanFilter.cpp
View File

@ -18,8 +18,7 @@
*/ */
#include <gtsam/linear/KalmanFilter.h> #include <gtsam/linear/KalmanFilter.h>
#include <gtsam/linear/SharedDiagonal.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedGaussian.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>

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

@ -25,8 +25,6 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedGaussian.h>
#include <gtsam/linear/SharedDiagonal.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -279,8 +277,8 @@ TEST(NoiseModel, QRNan )
TEST(NoiseModel, SmartCovariance ) TEST(NoiseModel, SmartCovariance )
{ {
bool smart = true; bool smart = true;
SharedGaussian expected = Unit::Create(3); gtsam::SharedGaussian expected = Unit::Create(3);
SharedGaussian actual = Gaussian::Covariance(eye(3), smart); gtsam::SharedGaussian actual = Gaussian::Covariance(eye(3), smart);
EXPECT(assert_equal(*expected,*actual)); EXPECT(assert_equal(*expected,*actual));
} }
@ -297,7 +295,7 @@ TEST(NoiseModel, ScalarOrVector )
TEST(NoiseModel, WhitenInPlace) TEST(NoiseModel, WhitenInPlace)
{ {
Vector sigmas = Vector_(3, 0.1, 0.1, 0.1); Vector sigmas = Vector_(3, 0.1, 0.1, 0.1);
SharedDiagonal model(sigmas); SharedDiagonal model = Diagonal::Sigmas(sigmas);
Matrix A = eye(3); Matrix A = eye(3);
model->WhitenInPlace(A); model->WhitenInPlace(A);
Matrix expected = eye(3) * 10; Matrix expected = eye(3) * 10;

3
gtsam/linear/tests/testSerializationLinear.cpp
View File

@ -22,9 +22,6 @@
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianISAM.h> #include <gtsam/linear/GaussianISAM.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedNoiseModel.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/linear/SharedGaussian.h>
#include <gtsam/base/serializationTestHelpers.h> #include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>

6
gtsam/linear/tests/timeFactorOverhead.cpp
View File

@ -60,7 +60,7 @@ int main(int argc, char *argv[]) {
blockGfgs.reserve(nTrials); blockGfgs.reserve(nTrials);
for(size_t trial=0; trial<nTrials; ++trial) { for(size_t trial=0; trial<nTrials; ++trial) {
blockGfgs.push_back(GaussianFactorGraph()); blockGfgs.push_back(GaussianFactorGraph());
SharedDiagonal noise = sharedSigma(blockdim, 1.0); SharedDiagonal noise = noiseModel::Isotropic::Sigma(blockdim, 1.0);
for(size_t i=0; i<nBlocks; ++i) { for(size_t i=0; i<nBlocks; ++i) {
// Generate a random Gaussian factor // Generate a random Gaussian factor
Matrix A(blockdim, vardim); Matrix A(blockdim, vardim);
@ -102,7 +102,7 @@ int main(int argc, char *argv[]) {
vector<GaussianFactorGraph> combGfgs; vector<GaussianFactorGraph> combGfgs;
for(size_t trial=0; trial<nTrials; ++trial) { for(size_t trial=0; trial<nTrials; ++trial) {
combGfgs.push_back(GaussianFactorGraph()); combGfgs.push_back(GaussianFactorGraph());
SharedDiagonal noise = sharedSigma(blockdim, 1.0); SharedDiagonal noise = noiseModel::Isotropic::Sigma(blockdim, 1.0);
Matrix Acomb(blockdim*nBlocks, vardim); Matrix Acomb(blockdim*nBlocks, vardim);
Vector bcomb(blockdim*nBlocks); Vector bcomb(blockdim*nBlocks);
@ -116,7 +116,7 @@ int main(int argc, char *argv[]) {
bcomb(blockdim*i+j) = rg(); bcomb(blockdim*i+j) = rg();
} }
combGfgs[trial].push_back(JacobianFactor::shared_ptr(new JacobianFactor(key, Acomb, bcomb, combGfgs[trial].push_back(JacobianFactor::shared_ptr(new JacobianFactor(key, Acomb, bcomb,
sharedSigma(blockdim*nBlocks, 1.0)))); noiseModel::Isotropic::Sigma(blockdim*nBlocks, 1.0))));
} }
combbuild = timer.elapsed(); combbuild = timer.elapsed();
cout << combbuild << " s" << endl; cout << combbuild << " s" << endl;

2
gtsam/linear/tests/timeGaussianFactor.cpp
View File

@ -101,7 +101,7 @@ int main()
b2(7) = -1; b2(7) = -1;
// time eliminate // time eliminate
JacobianFactor combined(_x2_, Ax2, _l1_, Al1, _x1_, Ax1, b2, sharedSigma(8,1)); JacobianFactor combined(_x2_, Ax2, _l1_, Al1, _x1_, Ax1, b2, noiseModel::Isotropic::Sigma(8,1));
long timeLog = clock(); long timeLog = clock();
int n = 1000000; int n = 1000000;
GaussianConditional::shared_ptr conditional; GaussianConditional::shared_ptr conditional;

2
gtsam/linear/tests/timeSLAMlike.cpp
View File

@ -69,7 +69,7 @@ int main(int argc, char *argv[]) {
blockGfgs.reserve(nTrials); blockGfgs.reserve(nTrials);
for(size_t trial=0; trial<nTrials; ++trial) { for(size_t trial=0; trial<nTrials; ++trial) {
blockGfgs.push_back(GaussianFactorGraph()); blockGfgs.push_back(GaussianFactorGraph());
SharedDiagonal noise = sharedSigma(blockdim, 1.0); SharedDiagonal noise = noiseModel::Isotropic::Sigma(blockdim, 1.0);
for(int c=0; c<nVars; ++c) { for(int c=0; c<nVars; ++c) {
for(size_t d=0; d<blocksPerVar; ++d) { for(size_t d=0; d<blocksPerVar; ++d) {
vector<pair<Index, Matrix> > terms; terms.reserve(varsPerBlock); vector<pair<Index, Matrix> > terms; terms.reserve(varsPerBlock);

2
gtsam/nonlinear/NonlinearFactor.h
View File

@ -28,7 +28,7 @@
#include <gtsam/inference/Factor-inl.h> #include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/IndexFactor.h> #include <gtsam/inference/IndexFactor.h>
#include <gtsam/linear/SharedNoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>

12
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -371,7 +371,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
graph.addCameraConstraint(0, cameras[0]); graph.addCameraConstraint(0, cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras // Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.add(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, sharedSigma(1, 10.0))); graph.add(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5 ;
@ -386,8 +386,8 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
// Tests range factor between a GeneralCamera and a Pose3 // Tests range factor between a GeneralCamera and a Pose3
Graph graph; Graph graph;
graph.addCameraConstraint(0, GeneralCamera()); graph.addCameraConstraint(0, GeneralCamera());
graph.add(RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.0, sharedSigma(1, 1.0))); graph.add(RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0)));
graph.add(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0))); graph.add(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0)));
Values init; Values init;
init.insert(X(0), GeneralCamera()); init.insert(X(0), GeneralCamera());
@ -410,9 +410,9 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
// Tests range factor between a CalibratedCamera and a Pose3 // Tests range factor between a CalibratedCamera and a Pose3
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.add(PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(), sharedSigma(6, 1.0))); graph.add(PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.0)));
graph.add(RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.0, sharedSigma(1, 1.0))); graph.add(RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0)));
graph.add(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0))); graph.add(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0)));
Values init; Values init;
init.insert(X(0), CalibratedCamera()); init.insert(X(0), CalibratedCamera());

2
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -369,7 +369,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) {
graph.addCameraConstraint(X(0), cameras[0]); graph.addCameraConstraint(X(0), cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras // Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.add(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, sharedSigma(1, 10.0))); graph.add(RangeFactor<GeneralCamera,GeneralCamera>(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0)));
const double reproj_error = 1e-5 ; const double reproj_error = 1e-5 ;

2
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -393,7 +393,7 @@ TEST_UNSAFE(Pose2SLAM, expmap )
} }
// Common measurement covariance // Common measurement covariance
static SharedNoiseModel sigmas = sharedSigmas(Vector_(3,sx,sy,st)); static SharedNoiseModel sigmas = noiseModel::Diagonal::Sigmas(Vector_(3,sx,sy,st));
/* ************************************************************************* */ /* ************************************************************************* */
// Very simple test establishing Ax-b \approx z-h(x) // Very simple test establishing Ax-b \approx z-h(x)

10
matlab/VisualISAMInitialize.m
View File

@ -6,10 +6,10 @@ function [noiseModels,isam,result] = VisualISAMInitialize(data,truth,options)
isam = visualSLAMISAM(options.reorderInterval); isam = visualSLAMISAM(options.reorderInterval);
%% Set Noise parameters %% Set Noise parameters
noiseModels.pose = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); noiseModels.pose = gtsamnoiseModelDiagonal_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
noiseModels.odometry = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); noiseModels.odometry = gtsamnoiseModelDiagonal_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
noiseModels.point = gtsamSharedNoiseModel_Sigma(3, 0.1); noiseModels.point = gtsamnoiseModelIsotropic_Sigma(3, 0.1);
noiseModels.measurement = gtsamSharedNoiseModel_Sigma(2, 1.0); noiseModels.measurement = gtsamnoiseModelIsotropic_Sigma(2, 1.0);
%% Add constraints/priors %% Add constraints/priors
% TODO: should not be from ground truth! % TODO: should not be from ground truth!
@ -62,5 +62,3 @@ result = isam.estimate();
if options.alwaysRelinearize % re-linearize if options.alwaysRelinearize % re-linearize
isam.reorder_relinearize(); isam.reorder_relinearize();
end end
cla;

1
matlab/VisualISAM_gui.m
View File

@ -234,6 +234,7 @@ initOptions(handles)
% Initialize and plot % Initialize and plot
[noiseModels,isam,result] = VisualISAMInitialize(data,truth,options); [noiseModels,isam,result] = VisualISAMInitialize(data,truth,options);
cla
VisualISAMPlot(truth, data, isam, result, options) VisualISAMPlot(truth, data, isam, result, options)
frame_i = 2; frame_i = 2;
showFramei(hObject, handles) showFramei(hObject, handles)

4
matlab/examples/LocalizationExample.m
View File

@ -20,13 +20,13 @@ graph = pose2SLAMGraph;
%% Add two odometry factors %% Add two odometry factors
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(1, 2, odometry, odometryNoise); graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise); graph.addOdometry(2, 3, odometry, odometryNoise);
%% Add three "GPS" measurements %% Add three "GPS" measurements
% We use Pose2 Priors here with high variance on theta % We use Pose2 Priors here with high variance on theta
noiseModel = gtsamSharedNoiseModel_Sigmas([0.1; 0.1; 10]); noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.1; 10]);
graph.addPrior(1, gtsamPose2(0.0, 0.0, 0.0), noiseModel); graph.addPrior(1, gtsamPose2(0.0, 0.0, 0.0), noiseModel);
graph.addPrior(2, gtsamPose2(2.0, 0.0, 0.0), noiseModel); graph.addPrior(2, gtsamPose2(2.0, 0.0, 0.0), noiseModel);
graph.addPrior(3, gtsamPose2(4.0, 0.0, 0.0), noiseModel); graph.addPrior(3, gtsamPose2(4.0, 0.0, 0.0), noiseModel);

4
matlab/examples/OdometryExample.m
View File

@ -20,12 +20,12 @@ graph = pose2SLAMGraph;
%% Add a Gaussian prior on pose x_1 %% Add a Gaussian prior on pose x_1
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin
priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
graph.addPrior(1, priorMean, priorNoise); % add directly to graph graph.addPrior(1, priorMean, priorNoise); % add directly to graph
%% Add two odometry factors %% Add two odometry factors
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(1, 2, odometry, odometryNoise); graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise); graph.addOdometry(2, 3, odometry, odometryNoise);

7
matlab/examples/PlanarSLAMExample.m
View File

@ -28,18 +28,18 @@ graph = planarSLAMGraph;
%% Add prior %% Add prior
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
graph.addPrior(i1, priorMean, priorNoise); % add directly to graph graph.addPrior(i1, priorMean, priorNoise); % add directly to graph
%% Add odometry %% Add odometry
odometry = gtsamPose2(2.0, 0.0, 0.0); odometry = gtsamPose2(2.0, 0.0, 0.0);
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addOdometry(i1, i2, odometry, odometryNoise); graph.addOdometry(i1, i2, odometry, odometryNoise);
graph.addOdometry(i2, i3, odometry, odometryNoise); graph.addOdometry(i2, i3, odometry, odometryNoise);
%% Add bearing/range measurement factors %% Add bearing/range measurement factors
degrees = pi/180; degrees = pi/180;
noiseModel = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]); noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.2]);
graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel); graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel);
graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel); graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel);
graph.addBearingRange(i3, j2, gtsamRot2(90*degrees), 2, noiseModel); graph.addBearingRange(i3, j2, gtsamRot2(90*degrees), 2, noiseModel);
@ -75,6 +75,7 @@ end
for i=1:3 for i=1:3
plotPose2(pose{i},'g',P{i}) plotPose2(pose{i},'g',P{i})
end end
point = {};
for j=1:2 for j=1:2
key = symbol('l',j); key = symbol('l',j);
point{j} = result.point(key); point{j} = result.point(key);

8
matlab/examples/PlanarSLAMExample_sampling.m
View File

@ -15,22 +15,22 @@
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
graph = planarSLAMGraph; graph = planarSLAMGraph;
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
graph.addPrior(i1, priorMean, priorNoise); % add directly to graph graph.addPrior(i1, priorMean, priorNoise); % add directly to graph
odometry = gtsamPose2(2.0, 0.0, 0.0); odometry = gtsamPose2(2.0, 0.0, 0.0);
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addOdometry(i1, i2, odometry, odometryNoise); graph.addOdometry(i1, i2, odometry, odometryNoise);
graph.addOdometry(i2, i3, odometry, odometryNoise); graph.addOdometry(i2, i3, odometry, odometryNoise);
%% Except, for measurements we offer a choice %% Except, for measurements we offer a choice
j1 = symbol('l',1); j2 = symbol('l',2); j1 = symbol('l',1); j2 = symbol('l',2);
degrees = pi/180; degrees = pi/180;
noiseModel = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]); noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.2]);
if 1 if 1
graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel); graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel);
graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel); graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel);
else else
bearingModel = gtsamSharedNoiseModel_Sigmas(0.1); bearingModel = gtsamnoiseModelDiagonal_Sigmas(0.1);
graph.addBearing(i1, j1, gtsamRot2(45*degrees), bearingModel); graph.addBearing(i1, j1, gtsamRot2(45*degrees), bearingModel);
graph.addBearing(i2, j1, gtsamRot2(90*degrees), bearingModel); graph.addBearing(i2, j1, gtsamRot2(90*degrees), bearingModel);
end end

12
matlab/examples/Pose2SLAMExample.m
View File

@ -24,19 +24,19 @@ graph = pose2SLAMGraph;
%% Add prior %% Add prior
% gaussian for prior % gaussian for prior
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
graph.addPrior(1, priorMean, priorNoise); % add directly to graph graph.addPrior(1, priorMean, priorNoise); % add directly to graph
%% Add odometry %% Add odometry
% general noisemodel for odometry % general noisemodel for odometry
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise); graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
%% Add pose constraint %% Add pose constraint
model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); model = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model); graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
% print % print
@ -60,11 +60,11 @@ cla;
plot(result.xs(),result.ys(),'k*-'); hold on plot(result.xs(),result.ys(),'k*-'); hold on
plot([result.pose(5).x;result.pose(2).x],[result.pose(5).y;result.pose(2).y],'r-'); plot([result.pose(5).x;result.pose(2).x],[result.pose(5).y;result.pose(2).y],'r-');
marginals = graph.marginals(result); marginals = graph.marginals(result);
P={};
for i=1:result.size() for i=1:result.size()
pose_i = result.pose(i); pose_i = result.pose(i);
P{i}=marginals.marginalCovariance(i); P = marginals.marginalCovariance(i)
plotPose2(pose_i,'g',P{i}) plotPose2(pose_i,'g',P);
end end
axis([-0.6 4.8 -1 1]) axis([-0.6 4.8 -1 1])
axis equal axis equal

6
matlab/examples/Pose2SLAMExample_advanced.m
View File

@ -25,20 +25,20 @@ graph = pose2SLAMGraph;
%% Add prior %% Add prior
% gaussian for prior % gaussian for prior
priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(1, priorMean, priorNoise); % add directly to graph graph.addPrior(1, priorMean, priorNoise); % add directly to graph
%% Add odometry %% Add odometry
% general noisemodel for odometry % general noisemodel for odometry
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addOdometry(1, 2, odometry, odometryNoise); graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise); graph.addOdometry(2, 3, odometry, odometryNoise);
%% Add measurements %% Add measurements
% general noisemodel for measurements % general noisemodel for measurements
measurementNoise = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]); measurementNoise = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.2]);
% print % print
graph.print('full graph'); graph.print('full graph');

2
matlab/examples/Pose2SLAMExample_circle.m
View File

@ -19,7 +19,7 @@ p1 = hexagon.pose(1);
fg = pose2SLAMGraph; fg = pose2SLAMGraph;
fg.addPoseConstraint(0, p0); fg.addPoseConstraint(0, p0);
delta = p0.between(p1); delta = p0.between(p1);
covariance = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 5*pi/180]); covariance = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 5*pi/180]);
fg.addOdometry(0,1, delta, covariance); fg.addOdometry(0,1, delta, covariance);
fg.addOdometry(1,2, delta, covariance); fg.addOdometry(1,2, delta, covariance);
fg.addOdometry(2,3, delta, covariance); fg.addOdometry(2,3, delta, covariance);

5
matlab/examples/Pose2SLAMExample_graph.m
View File

@ -11,13 +11,13 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Initialize graph, initial estimate, and odometry noise %% Initialize graph, initial estimate, and odometry noise
model = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 5*pi/180]); model = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 5*pi/180]);
[graph,initial]=load2D('../../examples/Data/w100-odom.graph',model); [graph,initial]=load2D('../../examples/Data/w100-odom.graph',model);
initial.print(sprintf('Initial estimate:\n')); initial.print(sprintf('Initial estimate:\n'));
%% Add a Gaussian prior on pose x_1 %% Add a Gaussian prior on pose x_1
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin
priorNoise = gtsamSharedNoiseModel_Sigmas([0.01; 0.01; 0.01]); priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.01; 0.01; 0.01]);
graph.addPrior(0, priorMean, priorNoise); % add directly to graph graph.addPrior(0, priorMean, priorNoise); % add directly to graph
%% Plot Initial Estimate %% Plot Initial Estimate
@ -31,6 +31,7 @@ result.print(sprintf('\nFinal result:\n'));
%% Plot Covariance Ellipses %% Plot Covariance Ellipses
marginals = graph.marginals(result); marginals = graph.marginals(result);
P={};
for i=1:result.size()-1 for i=1:result.size()-1
pose_i = result.pose(i); pose_i = result.pose(i);
P{i}=marginals.marginalCovariance(i); P{i}=marginals.marginalCovariance(i);

6
matlab/examples/Pose2SLAMwSPCG.m
View File

@ -22,19 +22,19 @@ graph = pose2SLAMGraph;
%% Add prior %% Add prior
% gaussian for prior % gaussian for prior
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
graph.addPrior(1, priorMean, priorNoise); % add directly to graph graph.addPrior(1, priorMean, priorNoise); % add directly to graph
%% Add odometry %% Add odometry
% general noisemodel for odometry % general noisemodel for odometry
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise); graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
%% Add pose constraint %% Add pose constraint
model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); model = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model); graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
% print % print

2
matlab/examples/Pose3SLAMExample.m
View File

@ -19,7 +19,7 @@ p1 = hexagon.pose(1);
fg = pose3SLAMGraph; fg = pose3SLAMGraph;
fg.addHardConstraint(0, p0); fg.addHardConstraint(0, p0);
delta = p0.between(p1); delta = p0.between(p1);
covariance = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); covariance = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
fg.addConstraint(0,1, delta, covariance); fg.addConstraint(0,1, delta, covariance);
fg.addConstraint(1,2, delta, covariance); fg.addConstraint(1,2, delta, covariance);
fg.addConstraint(2,3, delta, covariance); fg.addConstraint(2,3, delta, covariance);

2
matlab/examples/Pose3SLAMExample_graph.m
View File

@ -16,7 +16,7 @@ N = 2500;
filename = '../../examples/Data/sphere2500.txt'; filename = '../../examples/Data/sphere2500.txt';
%% Initialize graph, initial estimate, and odometry noise %% Initialize graph, initial estimate, and odometry noise
model = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); model = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
[graph,initial]=load3D(filename,model,true,N); [graph,initial]=load3D(filename,model,true,N);
%% Plot Initial Estimate %% Plot Initial Estimate

6
matlab/examples/SFMExample.m
View File

@ -32,7 +32,7 @@ poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
graph = visualSLAMGraph; graph = visualSLAMGraph;
%% Add factors for all measurements %% Add factors for all measurements
measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma); measurementNoise = gtsamnoiseModelIsotropic_Sigma(2,measurementNoiseSigma);
for i=1:length(data.Z) for i=1:length(data.Z)
for k=1:length(data.Z{i}) for k=1:length(data.Z{i})
j = data.J{i}{k}; j = data.J{i}{k};
@ -41,9 +41,9 @@ for i=1:length(data.Z)
end end
%% Add Gaussian priors for a pose and a landmark to constrain the system %% Add Gaussian priors for a pose and a landmark to constrain the system
posePriorNoise = gtsamSharedNoiseModel_Sigmas(poseNoiseSigmas); posePriorNoise = gtsamnoiseModelDiagonal_Sigmas(poseNoiseSigmas);
graph.addPosePrior(symbol('x',1), truth.cameras{1}.pose, posePriorNoise); graph.addPosePrior(symbol('x',1), truth.cameras{1}.pose, posePriorNoise);
pointPriorNoise = gtsamSharedNoiseModel_Sigma(3,pointNoiseSigma); pointPriorNoise = gtsamnoiseModelIsotropic_Sigma(3,pointNoiseSigma);
graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise); graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise);
%% Print the graph %% Print the graph

2
matlab/examples/StereoVOExample.m
View File

@ -31,7 +31,7 @@ graph.addPoseConstraint(x1, first_pose);
%% Create realistic calibration and measurement noise model %% Create realistic calibration and measurement noise model
% format: fx fy skew cx cy baseline % format: fx fy skew cx cy baseline
K = gtsamCal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); K = gtsamCal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
stereo_model = gtsamSharedNoiseModel_Sigmas([1.0; 1.0; 1.0]); stereo_model = gtsamnoiseModelDiagonal_Sigmas([1.0; 1.0; 1.0]);
%% Add measurements %% Add measurements
% pose 1 % pose 1

4
matlab/examples/StereoVOExample_large.m
View File

@ -14,7 +14,7 @@
% format: fx fy skew cx cy baseline % format: fx fy skew cx cy baseline
calib = dlmread('../../examples/Data/VO_calibration.txt'); calib = dlmread('../../examples/Data/VO_calibration.txt');
K = gtsamCal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); K = gtsamCal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6));
stereo_model = gtsamSharedNoiseModel_Sigmas([1.0; 1.0; 1.0]); stereo_model = gtsamnoiseModelDiagonal_Sigmas([1.0; 1.0; 1.0]);
%% create empty graph and values %% create empty graph and values
graph = visualSLAMGraph; graph = visualSLAMGraph;
@ -57,7 +57,7 @@ graph.addPoseConstraint(key, first_pose);
%% optimize %% optimize
fprintf(1,'Optimizing\n'); tic fprintf(1,'Optimizing\n'); tic
result = graph.optimize(initial); result = graph.optimize(initial,1);
toc toc
%% visualize initial trajectory, final trajectory, and final points %% visualize initial trajectory, final trajectory, and final points

4
matlab/tests/testJacobianFactor.m
View File

@ -30,7 +30,7 @@ x1 = 3;
% the RHS % the RHS
b2=[-1;1.5;2;-1]; b2=[-1;1.5;2;-1];
sigmas = [1;1;1;1]; sigmas = [1;1;1;1];
model4 = gtsamSharedDiagonal(sigmas); model4 = gtsamnoiseModelDiagonal_Sigmas(sigmas);
combined = gtsamJacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4); combined = gtsamJacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4);
% eliminate the first variable (x2) in the combined factor, destructive ! % eliminate the first variable (x2) in the combined factor, destructive !
@ -69,7 +69,7 @@ Bx1 = [
% the RHS % the RHS
b1= [0.0;0.894427]; b1= [0.0;0.894427];
model2 = gtsamSharedDiagonal([1;1]); model2 = gtsamnoiseModelDiagonal_Sigmas([1;1]);
expectedLF = gtsamJacobianFactor(l1, Bl1, x1, Bx1, b1, model2); expectedLF = gtsamJacobianFactor(l1, Bl1, x1, Bx1, b1, model2);
% check if the result matches the combined (reduced) factor % check if the result matches the combined (reduced) factor

4
matlab/tests/testKalmanFilter.m
View File

@ -22,13 +22,13 @@
F = eye(2,2); F = eye(2,2);
B = eye(2,2); B = eye(2,2);
u = [1.0; 0.0]; u = [1.0; 0.0];
modelQ = gtsamSharedDiagonal([0.1;0.1]); modelQ = gtsamnoiseModelDiagonal_Sigmas([0.1;0.1]);
Q = 0.01*eye(2,2); Q = 0.01*eye(2,2);
H = eye(2,2); H = eye(2,2);
z1 = [1.0, 0.0]'; z1 = [1.0, 0.0]';
z2 = [2.0, 0.0]'; z2 = [2.0, 0.0]';
z3 = [3.0, 0.0]'; z3 = [3.0, 0.0]';
modelR = gtsamSharedDiagonal([0.1;0.1]); modelR = gtsamnoiseModelDiagonal_Sigmas([0.1;0.1]);
R = 0.01*eye(2,2); R = 0.01*eye(2,2);
%% Create the set of expected output TestValues %% Create the set of expected output TestValues

49
matlab/tests/testLocalizationExample.m Normal file
View File

@ -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
%
% @brief Example of a simple 2D localization example
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
graph = pose2SLAMGraph;
%% Add two odometry factors
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
%% Add three "GPS" measurements
% We use Pose2 Priors here with high variance on theta
groundTruth = pose2SLAMValues;
groundTruth.insertPose(1, gtsamPose2(0.0, 0.0, 0.0));
groundTruth.insertPose(2, gtsamPose2(2.0, 0.0, 0.0));
groundTruth.insertPose(3, gtsamPose2(4.0, 0.0, 0.0));
noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.1; 10]);
for i=1:3
graph.addPrior(i, groundTruth.pose(i), noiseModel);
end
%% Initialize to noisy points
initialEstimate = pose2SLAMValues;
initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
%% Plot Covariance Ellipses
marginals = graph.marginals(result);
P={};
for i=1:result.size()
pose_i = result.pose(i);
CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.pose(i),1e-4));
P{i}=marginals.marginalCovariance(i);
end

40
matlab/tests/testOdometryExample.m Normal file
View File

@ -0,0 +1,40 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
%
% @brief Example of a simple 2D localization example
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
graph = pose2SLAMGraph;
%% Add a Gaussian prior on pose x_1
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin
priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
graph.addPrior(1, priorMean, priorNoise); % add directly to graph
%% Add two odometry factors
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
%% Initialize to noisy points
initialEstimate = pose2SLAMValues;
initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
marginals = graph.marginals(result);
marginals.marginalCovariance(i);
%% Check first pose equality
pose_i = result.pose(1);
CHECK('pose_1.equals(gtsamPose2,1e-4)',pose_1.equals(gtsamPose2,1e-4));

68
matlab/tests/testPlanarSLAMExample.m Normal file
View File

@ -0,0 +1,68 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
%
% @brief Simple robotics example using the pre-built planar SLAM domain
% @author Alex Cunningham
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Assumptions
% - All values are axis aligned
% - Robot poses are facing along the X axis (horizontal, to the right in images)
% - We have bearing and range information for measurements
% - We have full odometry for measurements
% - The robot and landmarks are on a grid, moving 2 meters each step
% - Landmarks are 2 meters away from the robot trajectory
%% Create keys for variables
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
j1 = symbol('l',1); j2 = symbol('l',2);
%% Create graph container and add factors to it
graph = planarSLAMGraph;
%% Add prior
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
graph.addPrior(i1, priorMean, priorNoise); % add directly to graph
%% Add odometry
odometry = gtsamPose2(2.0, 0.0, 0.0);
odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addOdometry(i1, i2, odometry, odometryNoise);
graph.addOdometry(i2, i3, odometry, odometryNoise);
%% Add bearing/range measurement factors
degrees = pi/180;
noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.2]);
graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel);
graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel);
graph.addBearingRange(i3, j2, gtsamRot2(90*degrees), 2, noiseModel);
%% Initialize to noisy points
initialEstimate = planarSLAMValues;
initialEstimate.insertPose(i1, gtsamPose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(i2, gtsamPose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(i3, gtsamPose2(4.1, 0.1, 0.1));
initialEstimate.insertPoint(j1, gtsamPoint2(1.8, 2.1));
initialEstimate.insertPoint(j2, gtsamPoint2(4.1, 1.8));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
marginals = graph.marginals(result);
%% Check first pose and point equality
pose_1 = result.pose(symbol('x',1));
marginals.marginalCovariance(symbol('x',1));
CHECK('pose_1.equals(gtsamPose2,1e-4)',pose_1.equals(gtsamPose2,1e-4));
point_1 = result.point(symbol('l',1));
marginals.marginalCovariance(symbol('l',1));
CHECK('point_1.equals(gtsamPoint2(2,2),1e-4)',point_1.equals(gtsamPoint2(2,2),1e-4));

60
matlab/tests/testPose2SLAMExample.m Normal file
View File

@ -0,0 +1,60 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
%
% @brief Simple robotics example using the pre-built planar SLAM domain
% @author Alex Cunningham
% @author Frank Dellaert
% @author Chris Beall
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Assumptions
% - All values are axis aligned
% - Robot poses are facing along the X axis (horizontal, to the right in images)
% - We have full odometry for measurements
% - The robot is on a grid, moving 2 meters each step
%% Create graph container and add factors to it
graph = pose2SLAMGraph;
%% Add prior
% gaussian for prior
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
graph.addPrior(1, priorMean, priorNoise); % add directly to graph
%% Add odometry
% general noisemodel for odometry
odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
%% Add pose constraint
model = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
%% Initialize to noisy points
initialEstimate = pose2SLAMValues;
initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2 ));
initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2 ));
initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, pi/2));
initialEstimate.insertPose(4, gtsamPose2(4.0, 2.0, pi ));
initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
%% Plot Covariance Ellipses
marginals = graph.marginals(result);
P = marginals.marginalCovariance(1);
pose_1 = result.pose(1);
CHECK('pose_1.equals(gtsamPose2,1e-4)',pose_1.equals(gtsamPose2,1e-4));

46
matlab/tests/testPose3SLAMExample.m Normal file
View File

@ -0,0 +1,46 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
%
% @brief Read graph from file and perform GraphSLAM
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create a hexagon of poses
hexagon = pose3SLAMValues_Circle(6,1.0);
p0 = hexagon.pose(0);
p1 = hexagon.pose(1);
%% create a Pose graph with one equality constraint and one measurement
fg = pose3SLAMGraph;
fg.addHardConstraint(0, p0);
delta = p0.between(p1);
covariance = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
fg.addConstraint(0,1, delta, covariance);
fg.addConstraint(1,2, delta, covariance);
fg.addConstraint(2,3, delta, covariance);
fg.addConstraint(3,4, delta, covariance);
fg.addConstraint(4,5, delta, covariance);
fg.addConstraint(5,0, delta, covariance);
%% Create initial config
initial = pose3SLAMValues;
s = 0.10;
initial.insertPose(0, p0);
initial.insertPose(1, hexagon.pose(1).retract(s*randn(6,1)));
initial.insertPose(2, hexagon.pose(2).retract(s*randn(6,1)));
initial.insertPose(3, hexagon.pose(3).retract(s*randn(6,1)));
initial.insertPose(4, hexagon.pose(4).retract(s*randn(6,1)));
initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1)));
%% optimize
result = fg.optimize(initial);
pose_1 = result.pose(1);
CHECK('pose_1.equals(gtsamPose3,1e-4)',pose_1.equals(p1,1e-4));

72
matlab/tests/testSFMExample.m Normal file
View File

@ -0,0 +1,72 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
%
% @brief A structure from motion example
% @author Duy-Nguyen Ta
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
options.triangle = false;
options.nrCameras = 10;
options.showImages = false;
[data,truth] = VisualISAMGenerateData(options);
measurementNoiseSigma = 1.0;
pointNoiseSigma = 0.1;
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
graph = visualSLAMGraph;
%% Add factors for all measurements
measurementNoise = gtsamnoiseModelIsotropic_Sigma(2,measurementNoiseSigma);
for i=1:length(data.Z)
for k=1:length(data.Z{i})
j = data.J{i}{k};
graph.addMeasurement(data.Z{i}{k}, measurementNoise, symbol('x',i), symbol('p',j), data.K);
end
end
posePriorNoise = gtsamnoiseModelDiagonal_Sigmas(poseNoiseSigmas);
graph.addPosePrior(symbol('x',1), truth.cameras{1}.pose, posePriorNoise);
pointPriorNoise = gtsamnoiseModelIsotropic_Sigma(3,pointNoiseSigma);
graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise);
%% Initial estimate
initialEstimate = visualSLAMValues;
for i=1:size(truth.cameras,2)
pose_i = truth.cameras{i}.pose;
initialEstimate.insertPose(symbol('x',i), pose_i);
end
for j=1:size(truth.points,2)
point_j = truth.points{j};
initialEstimate.insertPoint(symbol('p',j), point_j);
end
%% Optimization
parameters = gtsamLevenbergMarquardtParams(1e-5, 1e-5, 0, 0);
optimizer = graph.optimizer(initialEstimate, parameters);
for i=1:5
optimizer.iterate();
end
result = optimizer.values();
%% Marginalization
marginals = graph.marginals(result);
marginals.marginalCovariance(symbol('p',1));
marginals.marginalCovariance(symbol('x',1));
%% Check optimized results, should be equal to ground truth
for i=1:size(truth.cameras,2)
pose_i = result.pose(symbol('x',i));
CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))
end
for j=1:size(truth.points,2)
point_j = result.point(symbol('p',j));
CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
end

66
matlab/tests/testStereoVOExample.m Normal file
View File

@ -0,0 +1,66 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
%
% @brief Basic VO Example with 3 landmarks and two cameras
% @author Chris Beall
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Assumptions
% - For simplicity this example is in the camera's coordinate frame
% - X: right, Y: down, Z: forward
% - Pose x1 is at the origin, Pose 2 is 1 meter forward (along Z-axis)
% - x1 is fixed with a constraint, x2 is initialized with noisy values
% - No noise on measurements
%% Create keys for variables
x1 = symbol('x',1); x2 = symbol('x',2);
l1 = symbol('l',1); l2 = symbol('l',2); l3 = symbol('l',3);
%% Create graph container and add factors to it
graph = visualSLAMGraph;
%% add a constraint on the starting pose
first_pose = gtsamPose3();
graph.addPoseConstraint(x1, first_pose);
%% Create realistic calibration and measurement noise model
% format: fx fy skew cx cy baseline
K = gtsamCal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
stereo_model = gtsamnoiseModelDiagonal_Sigmas([1.0; 1.0; 1.0]);
%% Add measurements
% pose 1
graph.addStereoMeasurement(gtsamStereoPoint2(520, 480, 440), stereo_model, x1, l1, K);
graph.addStereoMeasurement(gtsamStereoPoint2(120, 80, 440), stereo_model, x1, l2, K);
graph.addStereoMeasurement(gtsamStereoPoint2(320, 280, 140), stereo_model, x1, l3, K);
%pose 2
graph.addStereoMeasurement(gtsamStereoPoint2(570, 520, 490), stereo_model, x2, l1, K);
graph.addStereoMeasurement(gtsamStereoPoint2( 70, 20, 490), stereo_model, x2, l2, K);
graph.addStereoMeasurement(gtsamStereoPoint2(320, 270, 115), stereo_model, x2, l3, K);
%% Create initial estimate for camera poses and landmarks
initialEstimate = visualSLAMValues;
initialEstimate.insertPose(x1, first_pose);
% noisy estimate for pose 2
initialEstimate.insertPose(x2, gtsamPose3(gtsamRot3(), gtsamPoint3(0.1,-.1,1.1)));
expected_l1 = gtsamPoint3( 1, 1, 5);
initialEstimate.insertPoint(l1, expected_l1);
initialEstimate.insertPoint(l2, gtsamPoint3(-1, 1, 5));
initialEstimate.insertPoint(l3, gtsamPoint3( 0,-.5, 5));
%% optimize
result = graph.optimize(initialEstimate,0);
%% check equality for the first pose and point
pose_x1 = result.pose(x1);
CHECK('pose_x1.equals(first_pose,1e-4)',pose_x1.equals(first_pose,1e-4));
point_l1 = result.point(l1);
CHECK('point_1.equals(expected_l1,1e-4)',point_l1.equals(expected_l1,1e-4));

53
matlab/tests/testVisualISAMExample.m Normal file
View File

@ -0,0 +1,53 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
%
% @brief A simple visual SLAM example for structure from motion
% @author Duy-Nguyen Ta
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Data Options
options.triangle = false;
options.nrCameras = 20;
options.showImages = false;
% iSAM Options
options.hardConstraint = false;
options.pointPriors = false;
options.batchInitialization = true;
options.reorderInterval = 10;
options.alwaysRelinearize = false;
% Display Options
options.saveDotFile = false;
options.printStats = false;
options.drawInterval = 5;
options.cameraInterval = 1;
options.drawTruePoses = false;
options.saveFigures = false;
options.saveDotFiles = false;
%% Generate data
[data,truth] = VisualISAMGenerateData(options);
%% Initialize iSAM with the first pose and points
[noiseModels,isam,result] = VisualISAMInitialize(data,truth,options);
%% Main loop for iSAM: stepping through all poses
for frame_i=3:options.nrCameras
[isam,result] = VisualISAMStep(data,noiseModels,isam,result,truth,options);
end
for i=1:size(truth.cameras,2)
pose_i = result.pose(symbol('x',i));
CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))
end
for j=1:size(truth.points,2)
point_j = result.point(symbol('l',j));
CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
end

24
matlab/tests/test_gtsam.m
View File

@ -6,5 +6,29 @@ testJacobianFactor
display 'Starting: testKalmanFilter' display 'Starting: testKalmanFilter'
testKalmanFilter testKalmanFilter
display 'Starting: testLocalizationExample'
testLocalizationExample
display 'Starting: testPose2SLAMExample'
testPose2SLAMExample
display 'Starting: testPose3SLAMExample'
testPose3SLAMExample
display 'Starting: testPlanarSLAMExample'
testPlanarSLAMExample
display 'Starting: testOdometryExample'
testOdometryExample
display 'Starting: testSFMExample'
testSFMExample
display 'Starting: testVisualISAMExample'
testVisualISAMExample
display 'Starting: testStereoVOExample'
testStereoVOExample
% end of tests % end of tests
display 'Tests complete!' display 'Tests complete!'

8
tests/smallExample.cpp
View File

@ -37,6 +37,8 @@ using namespace std;
namespace gtsam { namespace gtsam {
namespace example { namespace example {
using namespace gtsam::noiseModel;
typedef boost::shared_ptr<NonlinearFactor> shared; typedef boost::shared_ptr<NonlinearFactor> shared;
static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
@ -425,14 +427,14 @@ namespace example {
NonlinearFactorGraph nlfg; NonlinearFactorGraph nlfg;
// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal // Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sharedSigma(2,1e-3), key(1,1))); shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), Isotropic::Sigma(2,1e-3), key(1,1)));
nlfg.push_back(constraint); nlfg.push_back(constraint);
// Create horizontal constraints, 1...N*(N-1) // Create horizontal constraints, 1...N*(N-1)
Point2 z1(1.0, 0.0); // move right Point2 z1(1.0, 0.0); // move right
for (size_t x = 1; x < N; x++) for (size_t x = 1; x < N; x++)
for (size_t y = 1; y <= N; y++) { for (size_t y = 1; y <= N; y++) {
shared f(new simulated2D::Odometry(z1, sharedSigma(2,0.01), key(x, y), key(x + 1, y))); shared f(new simulated2D::Odometry(z1, Isotropic::Sigma(2,0.01), key(x, y), key(x + 1, y)));
nlfg.push_back(f); nlfg.push_back(f);
} }
@ -440,7 +442,7 @@ namespace example {
Point2 z2(0.0, 1.0); // move up Point2 z2(0.0, 1.0); // move up
for (size_t x = 1; x <= N; x++) for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y < N; y++) { for (size_t y = 1; y < N; y++) {
shared f(new simulated2D::Odometry(z2, sharedSigma(2,0.01), key(x, y), key(x, y + 1))); shared f(new simulated2D::Odometry(z2, Isotropic::Sigma(2,0.01), key(x, y), key(x, y + 1)));
nlfg.push_back(f); nlfg.push_back(f);
} }

2
tests/testGaussianFactor.cpp
View File

@ -41,7 +41,7 @@ using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
static SharedDiagonal static SharedDiagonal
sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2), sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2),
constraintModel = noiseModel::Constrained::All(2); constraintModel = noiseModel::Constrained::All(2);
//const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // FIXME: throws exception //const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // FIXME: throws exception

10
tests/testGaussianFactorGraphB.cpp
View File

@ -269,7 +269,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast )
0., -2.357022603955159, 0., -2.357022603955159,
7.071067811865475, 0., 7.071067811865475, 0.,
0., 7.071067811865475), 0., 7.071067811865475),
Vector_(4, -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094), sharedUnit(4)); Vector_(4, -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094), noiseModel::Unit::Create(4));
EXPECT(assert_equal(expected,*conditional,tol)); EXPECT(assert_equal(expected,*conditional,tol));
EXPECT(assert_equal((const GaussianFactor&)expectedFactor,*remaining.back(),tol)); EXPECT(assert_equal((const GaussianFactor&)expectedFactor,*remaining.back(),tol));
@ -370,7 +370,7 @@ TEST( GaussianFactorGraph, eliminateAll )
// GaussianFactorGraph expected = createGaussianFactorGraph(ordering); // GaussianFactorGraph expected = createGaussianFactorGraph(ordering);
// Matrix A = eye(2); // Matrix A = eye(2);
// Vector b = zero(2); // Vector b = zero(2);
// SharedDiagonal sigma = sharedSigma(2,3.0); // SharedDiagonal sigma = noiseModel::Isotropic::Sigma(2,3.0);
// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[L(1)],A,b,sigma))); // expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[L(1)],A,b,sigma)));
// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(1)],A,b,sigma))); // expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(1)],A,b,sigma)));
// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(2)],A,b,sigma))); // expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(2)],A,b,sigma)));
@ -768,7 +768,7 @@ TEST( GaussianFactorGraph, elimination )
GaussianFactorGraph fg; GaussianFactorGraph fg;
Matrix Ap = eye(1), An = eye(1) * -1; Matrix Ap = eye(1), An = eye(1) * -1;
Vector b = Vector_(1, 0.0); Vector b = Vector_(1, 0.0);
SharedDiagonal sigma = sharedSigma(1,2.0); SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,2.0);
fg.add(ord[X(1)], An, ord[X(2)], Ap, b, sigma); fg.add(ord[X(1)], An, ord[X(2)], Ap, b, sigma);
fg.add(ord[X(1)], Ap, b, sigma); fg.add(ord[X(1)], Ap, b, sigma);
fg.add(ord[X(2)], Ap, b, sigma); fg.add(ord[X(2)], Ap, b, sigma);
@ -873,7 +873,7 @@ TEST( GaussianFactorGraph, constrained_multi1 )
/* ************************************************************************* */ /* ************************************************************************* */
SharedDiagonal model = sharedSigma(2,1); SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1);
// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree ) // SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree )
//{ //{
@ -922,7 +922,7 @@ SharedDiagonal model = sharedSigma(2,1);
TEST(GaussianFactorGraph, replace) TEST(GaussianFactorGraph, replace)
{ {
Ordering ord; ord += X(1),X(2),X(3),X(4),X(5),X(6); Ordering ord; ord += X(1),X(2),X(3),X(4),X(5),X(6);
SharedDiagonal noise(sharedSigma(3, 1.0)); SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0));
GaussianFactorGraph::sharedFactor f1(new JacobianFactor( GaussianFactorGraph::sharedFactor f1(new JacobianFactor(
ord[X(1)], eye(3,3), ord[X(2)], eye(3,3), zero(3), noise)); ord[X(1)], eye(3,3), ord[X(2)], eye(3,3), zero(3), noise));

6
tests/testGaussianISAM.cpp
View File

@ -373,9 +373,9 @@ TEST_UNSAFE(BayesTree, simpleMarginal)
Matrix A12 = Rot2::fromDegrees(45.0).matrix(); Matrix A12 = Rot2::fromDegrees(45.0).matrix();
gfg.add(0, eye(2), zero(2), sharedSigma(2, 1.0)); gfg.add(0, eye(2), zero(2), noiseModel::Isotropic::Sigma(2, 1.0));
gfg.add(0, -eye(2), 1, eye(2), ones(2), sharedSigma(2, 1.0)); gfg.add(0, -eye(2), 1, eye(2), ones(2), noiseModel::Isotropic::Sigma(2, 1.0));
gfg.add(1, -eye(2), 2, A12, ones(2), sharedSigma(2, 1.0)); gfg.add(1, -eye(2), 2, A12, ones(2), noiseModel::Isotropic::Sigma(2, 1.0));
Matrix expected(GaussianSequentialSolver(gfg).marginalCovariance(2)); Matrix expected(GaussianSequentialSolver(gfg).marginalCovariance(2));
Matrix actual(GaussianMultifrontalSolver(gfg).marginalCovariance(2)); Matrix actual(GaussianMultifrontalSolver(gfg).marginalCovariance(2));

10
tests/testGaussianISAM2.cpp
View File

@ -33,8 +33,8 @@ const double tol = 1e-4;
// SETDEBUG("ISAM2 recalculate", true); // SETDEBUG("ISAM2 recalculate", true);
// Set up parameters // Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas(Vector_(2, M_PI/100.0, 0.1));
ISAM2 createSlamlikeISAM2( ISAM2 createSlamlikeISAM2(
boost::optional<Values&> init_values = boost::none, boost::optional<Values&> init_values = boost::none,
@ -257,8 +257,8 @@ TEST_UNSAFE(ISAM2, AddVariables) {
// //
// Ordering ordering; ordering += (0), (0), (1); // Ordering ordering; ordering += (0), (0), (1);
// planarSLAM::Graph graph; // planarSLAM::Graph graph;
// graph.addPrior((0), Pose2(), sharedUnit(Pose2::dimension)); // graph.addPrior((0), Pose2(), noiseModel::Unit::Create(Pose2::dimension));
// graph.addRange((0), (0), 1.0, sharedUnit(1)); // graph.addRange((0), (0), 1.0, noiseModel::Unit::Create(1));
// //
// FastSet<Index> expected; // FastSet<Index> expected;
// expected.insert(0); // expected.insert(0);
@ -844,7 +844,7 @@ TEST(ISAM2, clone) {
// Modify original isam // Modify original isam
NonlinearFactorGraph factors; NonlinearFactorGraph factors;
factors.add(BetweenFactor<Pose2>(0, 10, factors.add(BetweenFactor<Pose2>(0, 10,
isam.calculateEstimate<Pose2>(0).between(isam.calculateEstimate<Pose2>(10)), sharedUnit(3))); isam.calculateEstimate<Pose2>(0).between(isam.calculateEstimate<Pose2>(10)), noiseModel::Unit::Create(3)));
isam.update(factors); isam.update(factors);
CHECK(assert_equal(createSlamlikeISAM2(), clone2)); CHECK(assert_equal(createSlamlikeISAM2(), clone2));

8
tests/testGaussianJunctionTreeB.cpp
View File

@ -130,8 +130,8 @@ TEST(GaussianJunctionTree, slamlike) {
Values init; Values init;
planarSLAM::Graph newfactors; planarSLAM::Graph newfactors;
planarSLAM::Graph fullgraph; planarSLAM::Graph fullgraph;
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas(Vector_(2, M_PI/100.0, 0.1));
size_t i = 0; size_t i = 0;
@ -194,8 +194,8 @@ TEST(GaussianJunctionTree, simpleMarginal) {
// Create a simple graph // Create a simple graph
pose2SLAM::Graph fg; pose2SLAM::Graph fg;
fg.addPrior(X(0), Pose2(), sharedSigma(3, 10.0)); fg.addPrior(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0));
fg.addOdometry(X(0), X(1), Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0))); fg.addOdometry(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0)));
Values init; Values init;
init.insert(X(0), Pose2()); init.insert(X(0), Pose2());

6
tests/testGradientDescentOptimizer.cpp
View File

@ -26,18 +26,18 @@ boost::tuple<pose2SLAM::Graph, Values> generateProblem() {
// 2a. Add Gaussian prior // 2a. Add Gaussian prior
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
graph.addPrior(1, priorMean, priorNoise); graph.addPrior(1, priorMean, priorNoise);
// 2b. Add odometry factors // 2b. Add odometry factors
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 2c. Add pose constraint // 2c. Add pose constraint
SharedDiagonal constraintUncertainty(Vector_(3, 0.2, 0.2, 0.1)); SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty);
// 3. Create the data structure to hold the initialEstimate estinmate to the solution // 3. Create the data structure to hold the initialEstimate estinmate to the solution

2
tests/testGraph.cpp
View File

@ -76,7 +76,7 @@ TEST( Graph, predecessorMap2Graph )
TEST( Graph, composePoses ) TEST( Graph, composePoses )
{ {
pose2SLAM::Graph graph; pose2SLAM::Graph graph;
SharedNoiseModel cov = SharedNoiseModel::Unit(3); SharedNoiseModel cov = noiseModel::Unit::Create(3);
Pose2 p1(1.0, 2.0, 0.3), p2(4.0, 5.0, 0.6), p3(7.0, 8.0, 0.9), p4(2.0, 2.0, 2.9); Pose2 p1(1.0, 2.0, 0.3), p2(4.0, 5.0, 0.6), p3(7.0, 8.0, 0.9), p4(2.0, 2.0, 2.9);
Pose2 p12=p1.between(p2), p23=p2.between(p3), p43=p4.between(p3); Pose2 p12=p1.between(p2), p23=p2.between(p3), p43=p4.between(p3);
graph.addOdometry(1,2, p12, cov); graph.addOdometry(1,2, p12, cov);

4
tests/testInferenceB.cpp
View File

@ -55,8 +55,8 @@ TEST( inference, marginals )
TEST( inference, marginals2) TEST( inference, marginals2)
{ {
planarSLAM::Graph fg; planarSLAM::Graph fg;
SharedDiagonal poseModel(sharedSigma(3, 0.1)); SharedDiagonal poseModel(noiseModel::Isotropic::Sigma(3, 0.1));
SharedDiagonal pointModel(sharedSigma(3, 0.1)); SharedDiagonal pointModel(noiseModel::Isotropic::Sigma(3, 0.1));
fg.addPrior(X(0), Pose2(), poseModel); fg.addPrior(X(0), Pose2(), poseModel);
fg.addOdometry(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel); fg.addOdometry(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel);

6
tests/testNonlinearFactor.cpp
View File

@ -240,7 +240,7 @@ TEST( NonlinearFactor, linearize_constraint2 )
class TestFactor4 : public NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> { class TestFactor4 : public NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> {
public: public:
typedef NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> Base; typedef NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> Base;
TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), X(1), X(2), X(3), X(4)) {} TestFactor4() : Base(noiseModel::Diagonal::Sigmas(Vector_(1, 2.0)), X(1), X(2), X(3), X(4)) {}
virtual Vector virtual Vector
evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4, evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4,
@ -289,7 +289,7 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
class TestFactor5 : public NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> { class TestFactor5 : public NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> {
public: public:
typedef NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base; typedef NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base;
TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), X(1), X(2), X(3), X(4), X(5)) {} TestFactor5() : Base(noiseModel::Diagonal::Sigmas(Vector_(1, 2.0)), X(1), X(2), X(3), X(4), X(5)) {}
virtual Vector virtual Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
@ -339,7 +339,7 @@ TEST(NonlinearFactor, NoiseModelFactor5) {
class TestFactor6 : public NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> { class TestFactor6 : public NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> {
public: public:
typedef NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base; typedef NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base;
TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), X(1), X(2), X(3), X(4), X(5), X(6)) {} TestFactor6() : Base(noiseModel::Diagonal::Sigmas(Vector_(1, 2.0)), X(1), X(2), X(3), X(4), X(5), X(6)) {}
virtual Vector virtual Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,

6
tests/testNonlinearOptimizer.cpp
View File

@ -232,9 +232,9 @@ TEST(NonlinearOptimizer, NullFactor) {
TEST(NonlinearOptimizer, MoreOptimization) { TEST(NonlinearOptimizer, MoreOptimization) {
NonlinearFactorGraph fg; NonlinearFactorGraph fg;
fg.add(PriorFactor<Pose2>(0, Pose2(0,0,0), sharedSigma(3,1))); fg.add(PriorFactor<Pose2>(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1)));
fg.add(BetweenFactor<Pose2>(0, 1, Pose2(1,0,M_PI/2), sharedSigma(3,1))); fg.add(BetweenFactor<Pose2>(0, 1, Pose2(1,0,M_PI/2), noiseModel::Isotropic::Sigma(3,1)));
fg.add(BetweenFactor<Pose2>(1, 2, Pose2(1,0,M_PI/2), sharedSigma(3,1))); fg.add(BetweenFactor<Pose2>(1, 2, Pose2(1,0,M_PI/2), noiseModel::Isotropic::Sigma(3,1)));
Values init; Values init;
init.insert(0, Pose2(3,4,-M_PI)); init.insert(0, Pose2(3,4,-M_PI));

4
tests/testRot3Optimization.cpp
View File

@ -41,11 +41,11 @@ TEST(Rot3, optimize) {
Values truth; Values truth;
Values initial; Values initial;
Graph fg; Graph fg;
fg.add(Prior(Symbol('r',0), Rot3(), sharedSigma(3, 0.01))); fg.add(Prior(Symbol('r',0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01)));
for(int j=0; j<6; ++j) { for(int j=0; j<6; ++j) {
truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j))); truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j)));
initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
fg.add(Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01))); fg.add(Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), noiseModel::Isotropic::Sigma(3, 0.01)));
} }
Values final = GaussNewtonOptimizer(fg, initial).optimize(); Values final = GaussNewtonOptimizer(fg, initial).optimize();

2
tests/testSimulated2DOriented.cpp
View File

@ -58,7 +58,7 @@ TEST( simulated2DOriented, Dprior )
TEST( simulated2DOriented, constructor ) TEST( simulated2DOriented, constructor )
{ {
Pose2 measurement(0.2, 0.3, 0.1); Pose2 measurement(0.2, 0.3, 0.1);
SharedDiagonal model(Vector_(3, 1., 1., 1.)); SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1., 1., 1.));
simulated2DOriented::Odometry factor(measurement, model, X(1), X(2)); simulated2DOriented::Odometry factor(measurement, model, X(1), X(2));
simulated2DOriented::Values config; simulated2DOriented::Values config;

2
tests/timeMultifrontalOnDataset.cpp
View File

@ -48,7 +48,7 @@ int main(int argc, char *argv[]) {
// Add a prior on the first pose // Add a prior on the first pose
if (soft_prior) if (soft_prior)
data.first->addPrior(0, Pose2(), sharedSigma(Pose2::Dim(), 0.0005)); data.first->addPrior(0, Pose2(), noiseModel::Isotropic::Sigma(Pose2::Dim(), 0.0005));
else else
data.first->addPoseConstraint(0, Pose2()); data.first->addPoseConstraint(0, Pose2());

2
tests/timeSequentialOnDataset.cpp
View File

@ -48,7 +48,7 @@ int main(int argc, char *argv[]) {
// Add a prior on the first pose // Add a prior on the first pose
if (soft_prior) if (soft_prior)
data.first->addPrior(0, Pose2(), sharedSigma(Pose2::Dim(), 0.0005)); data.first->addPrior(0, Pose2(), noiseModel::Isotropic::Sigma(Pose2::Dim(), 0.0005));
else else
data.first->addPoseConstraint(0, Pose2()); data.first->addPoseConstraint(0, Pose2());

27
wrap/matlab.h
View File

@ -25,6 +25,7 @@
using gtsam::Vector; using gtsam::Vector;
using gtsam::Matrix; using gtsam::Matrix;
using gtsam::noiseModel::Base;
using gtsam::noiseModel::Gaussian; using gtsam::noiseModel::Gaussian;
using gtsam::noiseModel::Diagonal; using gtsam::noiseModel::Diagonal;
using gtsam::noiseModel::Isotropic; using gtsam::noiseModel::Isotropic;
@ -483,7 +484,7 @@ boost::shared_ptr<Isotropic> unwrap_shared_ptr(const mxArray* obj, const string&
bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic"); bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic");
bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit"); bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit");
if (!isIsotropic && !isUnit) { if (!isIsotropic && !isUnit) {
mexPrintf("Expected Isotropic or derived classes, got %s\n", mxGetClassName(obj)); mexPrintf("Expected gtsamnoiseModelIsotropic or derived classes, got %s\n", mxGetClassName(obj));
error("Argument has wrong type."); error("Argument has wrong type.");
} }
#endif #endif
@ -501,7 +502,7 @@ boost::shared_ptr<Diagonal> unwrap_shared_ptr(const mxArray* obj, const string&
bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic"); bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic");
bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit"); bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit");
if (!isDiagonal && !isIsotropic && !isUnit ) { if (!isDiagonal && !isIsotropic && !isUnit ) {
mexPrintf("Expected Diagonal or derived classes, got %s\n", mxGetClassName(obj)); mexPrintf("Expected gtsamnoiseModelDiagonal or derived classes, got %s\n", mxGetClassName(obj));
error("Argument has wrong type."); error("Argument has wrong type.");
} }
#endif #endif
@ -520,7 +521,27 @@ boost::shared_ptr<Gaussian> unwrap_shared_ptr(const mxArray* obj, const string&
bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic"); bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic");
bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit"); bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit");
if (!isGaussian && !isDiagonal && !isIsotropic && !isUnit) { if (!isGaussian && !isDiagonal && !isIsotropic && !isUnit) {
mexPrintf("Expected Gaussian or derived classes, got %s\n", mxGetClassName(obj)); mexPrintf("Expected gtsamnoiseModelGaussian or derived classes, got %s\n", mxGetClassName(obj));
error("Argument has wrong type.");
}
#endif
mxArray* mxh = mxGetProperty(obj,0,"self");
if (mxh==NULL) error("unwrap_reference: invalid wrap object");
ObjectHandle<Isotropic>* handle = ObjectHandle<Isotropic>::from_mex_handle(mxh);
return handle->get_object();
}
// Base
template <>
boost::shared_ptr<Base> unwrap_shared_ptr(const mxArray* obj, const string& className) {
#ifndef UNSAFE_WRAP
bool isBase = mxIsClass(obj, "gtsamnoiseModelBase");
bool isGaussian = mxIsClass(obj, "gtsamnoiseModelGaussian");
bool isDiagonal = mxIsClass(obj, "gtsamnoiseModelDiagonal");
bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic");
bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit");
if (!isBase && !isGaussian && !isDiagonal && !isIsotropic && !isUnit) {
mexPrintf("Expected gtsamnoiseModelBase or derived classes, got %s\n", mxGetClassName(obj));
error("Argument has wrong type."); error("Argument has wrong type.");
} }
#endif #endif