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>
using namespace gtsam;
using namespace gtsam::noiseModel;
using symbol_shorthand::X;
/**
@ -69,7 +70,7 @@ int main(int argc, char* argv[]) {
/* 2. add factors to the graph */
// 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;
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,

5
examples/LocalizationExample.cpp
View File

@ -27,6 +27,7 @@
using namespace std;
using namespace gtsam;
using namespace gtsam::noiseModel;
/**
* UnaryFactor
@ -63,12 +64,12 @@ int main(int argc, char** argv) {
// add two odometry factors
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(2, 3, odometry, odometryNoise);
// 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>(2, 2, 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 gtsam;
using namespace gtsam::noiseModel;
/**
* 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
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
// add two odometry factors
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(2, 3, odometry, odometryNoise);

7
examples/PlanarSLAMExample.cpp
View File

@ -23,6 +23,7 @@
using namespace std;
using namespace gtsam;
using namespace gtsam::noiseModel;
/**
* 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
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
// add two odometry factors
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(i2, i3, odometry, odometryNoise);
// 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)
Rot2 bearing11 = Rot2::fromDegrees(45),

7
examples/Pose2SLAMExample.cpp
View File

@ -22,6 +22,7 @@
using namespace std;
using namespace gtsam;
using namespace gtsam::noiseModel;
int main(int argc, char** argv) {
@ -30,18 +31,18 @@ int main(int argc, char** argv) {
// 2a. Add Gaussian prior
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);
// 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(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(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 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);
// print

5
examples/Pose2SLAMExample_advanced.cpp
View File

@ -29,6 +29,7 @@
using namespace std;
using namespace gtsam;
using namespace gtsam::noiseModel;
int main(int argc, char** argv) {
/* 1. create graph container and add factors to it */
@ -36,11 +37,11 @@ int main(int argc, char** argv) {
/* 2.a add prior */
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
/* 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)
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);

5
examples/Pose2SLAMExample_graph.cpp
View File

@ -24,6 +24,7 @@
using namespace std;
using namespace gtsam;
using namespace gtsam::noiseModel;
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
pose2SLAM::Graph::shared_ptr graph ;
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);
initial->print("Initial estimate:\n");
// Add a Gaussian prior on first poses
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);
// Single Step Optimization using Levenberg-Marquardt

7
examples/Pose2SLAMwSPCG.cpp
View File

@ -26,6 +26,7 @@
using namespace std;
using namespace gtsam;
using namespace gtsam::noiseModel;
/* ************************************************************************* */
int main(void) {
@ -35,18 +36,18 @@ int main(void) {
// 2a. Add Gaussian prior
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);
// 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(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(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 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);
// print

6
examples/VisualSLAMData.h
View File

@ -73,7 +73,7 @@ struct VisualSLAMExampleData {
data.odometry = data.poses[0].between(data.poses[1]);
// 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 j=0; j<data.points.size(); ++j) {
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
}
}
data.noiseX = gtsam::sharedSigmas(gtsam::Vector_(6, 0.001, 0.001, 0.001, 0.1, 0.1, 0.1));
data.noiseL = gtsam::sharedSigma(3, 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::noiseModel::Isotropic::Sigma(3, 0.1);
return data;
}

72
gtsam.h
View File

@ -679,6 +679,9 @@ class VariableIndex {
#include <gtsam/linear/NoiseModel.h>
namespace noiseModel {
class Base {
};
class Gaussian {
static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
@ -707,29 +710,6 @@ class Unit {
};
}///\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 {
Sampler(gtsam::noiseModel::Diagonal* model, int seed);
@ -789,11 +769,11 @@ class JacobianFactor {
JacobianFactor();
JacobianFactor(Vector b_in);
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,
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,
Vector b, const gtsam::SharedDiagonal& model);
Vector b, const gtsam::noiseModel::Diagonal* model);
void print(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
bool empty() const;
@ -881,13 +861,13 @@ class KalmanFilter {
void print(string s) const;
static size_t step(gtsam::GaussianDensity* p);
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,
Matrix B, Vector u, Matrix Q);
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,
Vector z, const gtsam::SharedDiagonal& model);
Vector z, const gtsam::noiseModel::Diagonal* model);
gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H,
Vector z, Matrix Q);
};
@ -1024,10 +1004,10 @@ class Graph {
const gtsam::Ordering& ordering) const;
// 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 addOdometry(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::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::noiseModel::Base* noiseModel);
pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate) const;
pose2SLAM::Values optimizeSPCG(const pose2SLAM::Values& initialEstimate) const;
gtsam::Marginals marginals(const pose2SLAM::Values& solution) const;
@ -1075,8 +1055,8 @@ class Graph {
const gtsam::Ordering& ordering) const;
// pose3SLAM-specific
void addPrior(size_t key, const gtsam::Pose3& p, const gtsam::SharedNoiseModel& model);
void addConstraint(size_t key1, size_t key2, const gtsam::Pose3& z, 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::noiseModel::Base* model);
void addHardConstraint(size_t i, const gtsam::Pose3& p);
pose3SLAM::Values optimize(const pose3SLAM::Values& initialEstimate) const;
gtsam::Marginals marginals(const pose3SLAM::Values& solution) const;
@ -1123,19 +1103,19 @@ class Graph {
const gtsam::Ordering& ordering) const;
// 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 addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
void addBearing(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing, const gtsam::SharedNoiseModel& noiseModel);
void addRange(size_t poseKey, size_t pointKey, double range, const gtsam::SharedNoiseModel& noiseModel);
void addBearingRange(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing,double range, 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::noiseModel::Base* 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::noiseModel::Base* noiseModel);
planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
gtsam::Marginals marginals(const planarSLAM::Values& solution) const;
};
class Odometry {
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;
gtsam::GaussianFactor* linearize(const planarSLAM::Values& center,
const gtsam::Ordering& ordering) const;
@ -1190,9 +1170,9 @@ class Graph {
const gtsam::Ordering& ordering) const;
// 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);
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);
// Constraints
@ -1200,10 +1180,10 @@ class Graph {
void addPointConstraint(size_t pointKey, const gtsam::Point3& p);
// Priors
void addPosePrior(size_t poseKey, const gtsam::Pose3& p, const gtsam::SharedNoiseModel& model);
void addPointPrior(size_t pointKey, const gtsam::Point3& p, const gtsam::SharedNoiseModel& model);
void addRangeFactor(size_t poseKey, size_t pointKey, double range, const gtsam::SharedNoiseModel& model);
void addOdometry(size_t poseKey1, size_t poseKey2, const gtsam::Pose3& odometry, 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::noiseModel::Base* 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::noiseModel::Base* model);
visualSLAM::Values optimize(const visualSLAM::Values& initialEstimate, size_t verbosity) const;
gtsam::Marginals marginals(const visualSLAM::Values& solution) const;

2
gtsam/linear/GaussianFactorGraph.h
View File

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

1
gtsam/linear/HessianFactor.h
View File

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

2
gtsam/linear/JacobianFactor.h
View File

@ -19,7 +19,7 @@
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/Errors.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/base/blockMatrices.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/JacobianFactor.h>
#include <gtsam/linear/KalmanFilter.h>
#include <gtsam/linear/SharedGaussian.h>
#include <gtsam/linear/HessianFactor.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/NoiseModel.h>
#ifndef KALMANFILTER_DEFAULT_FACTORIZATION
#define KALMANFILTER_DEFAULT_FACTORIZATION QR
@ -25,9 +26,6 @@
namespace gtsam {
class SharedDiagonal;
class SharedGaussian;
/**
* Kalman Filter class
*

1
gtsam/linear/NoiseModel.cpp
View File

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

36
gtsam/linear/NoiseModel.h
View File

@ -24,11 +24,10 @@
namespace gtsam {
class SharedDiagonal; // forward declare
/// All noise models live in the noiseModel namespace
namespace noiseModel {
// Forward declaration
class Gaussian;
class Diagonal;
class Constrained;
@ -157,7 +156,7 @@ namespace gtsam {
* A Gaussian noise model created by specifying a covariance matrix.
* @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 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]
* @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
* 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
@ -263,20 +262,20 @@ namespace gtsam {
* A diagonal noise model created by specifying a Vector of sigmas, i.e.
* 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.
* i.e. the diagonal of the covariance matrix.
* @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.
* 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);
}
@ -365,13 +364,13 @@ namespace gtsam {
* standard devations, some of which might be zero
*/
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
* 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);
}
@ -380,7 +379,7 @@ namespace gtsam {
* standard devations, some of which might be zero
*/
static shared_ptr MixedSigmas(double m, const Vector& sigmas,
bool smart = false) {
bool smart = true) {
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
*/
virtual SharedDiagonal QR(Matrix& Ab) const;
virtual Diagonal::shared_ptr QR(Matrix& Ab) const;
/**
* Check constrained is always true
@ -493,18 +492,18 @@ namespace gtsam {
/**
* 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.
* @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
*/
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);
}
@ -718,6 +717,13 @@ namespace gtsam {
} // 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

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
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);
/* ************************************************************************* */
@ -234,26 +234,26 @@ TEST( NonlinearFactorGraph, combine2){
A11(1,0) = 0; A11(1,1) = 1;
Vector b(2);
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;
A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -1;
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;
A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -1;
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
double sigma4 = 0.1;
A11(0,0) = 6; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = 7;
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;
lfg.push_back(f1);
@ -286,7 +286,7 @@ TEST( NonlinearFactorGraph, combine2){
TEST(GaussianFactor, linearFactorN){
Matrix I = eye(2);
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,
10.0, 5.0), model)));
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(11, Matrix(Ab.block(0, 8, 4, 2)));
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
list<pair<Index, Matrix> > terms2;
@ -384,7 +384,7 @@ TEST(GaussianFactor, eliminateFrontals)
make_pair(9, Matrix(Ab.block(4, 6, 4, 2))),
make_pair(11,Matrix(Ab.block(4, 8, 4, 2)));
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
list<pair<Index, Matrix> > terms3;
@ -393,14 +393,14 @@ TEST(GaussianFactor, eliminateFrontals)
make_pair(9, Matrix(Ab.block(8, 6, 4, 2))),
make_pair(11,Matrix(Ab.block(8, 8, 4, 2)));
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
list<pair<Index, Matrix> > terms4;
terms4 +=
make_pair(11, Matrix(Ab.block(12, 8, 2, 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
GaussianFactorGraph factors;
@ -554,13 +554,13 @@ TEST(GaussianFactor, permuteWithInverse)
inversePermutation[4] = 1;
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)));
VariableIndex actualIndex(actualFG);
actual.permuteWithInverse(inversePermutation);
// 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)));
// GaussianVariableIndex expectedIndex(expectedFG);
@ -609,7 +609,7 @@ TEST(GaussianFactorGraph, sparseJacobian) {
4., 6.,32.).transpose();
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, 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
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, 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() {
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 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));

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

@ -93,7 +93,7 @@ TEST(HessianFactor, ConversionConstructor) {
vector<pair<Index, Matrix> > meas;
meas.push_back(make_pair(0, Ax2));
meas.push_back(make_pair(1, Al1x1));
JacobianFactor combined(meas, b2, sigmas);
JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
HessianFactor actual(combined);
@ -468,7 +468,7 @@ TEST(HessianFactor, eliminate2 )
vector<pair<Index, Matrix> > meas;
meas.push_back(make_pair(0, Ax2));
meas.push_back(make_pair(1, Al1x1));
JacobianFactor combined(meas, b2, sigmas);
JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
// eliminate the combined factor
HessianFactor::shared_ptr combinedLF_Chol(new HessianFactor(combined));
@ -502,7 +502,7 @@ TEST(HessianFactor, eliminate2 )
0.00, 1.00, +0.00, -1.00
)/sigma;
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));
}

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 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);
/* ************************************************************************* */
@ -76,7 +76,7 @@ TEST(JacobianFactor, constructor2)
JacobianFactor construct() {
Matrix A = Matrix_(2,2, 1.,2.,3.,4.);
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(
new JacobianFactor(0, A, b, s));
return *shared;
@ -86,7 +86,7 @@ TEST(JacobianFactor, return_value)
{
Matrix A = Matrix_(2,2, 1.,2.,3.,4.);
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();
EXPECT(assert_equal(b, copied.getb()));
EXPECT(assert_equal(*s, *copied.get_model()));
@ -107,7 +107,7 @@ TEST(JabobianFactor, Hessian_conversion) {
1.2530, 2.1508, -0.8779, -1.8755,
0, 2.5858, 0.4789, -2.3943).finished(),
(Vector(2) << -6.2929, -5.7941).finished(),
sharedUnit(2));
noiseModel::Unit::Create(2));
JacobianFactor actual(hessian);
@ -202,7 +202,7 @@ TEST(JacobianFactor, eliminate2 )
vector<pair<Index, Matrix> > meas;
meas.push_back(make_pair(_x2_, Ax2));
meas.push_back(make_pair(_l11_, Al1x1));
JacobianFactor combined(meas, b2, sigmas);
JacobianFactor combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
// eliminate the combined factor
GaussianConditional::shared_ptr actualCG_QR;
@ -236,7 +236,7 @@ TEST(JacobianFactor, eliminate2 )
0.00, 1.00, +0.00, -1.00
)/sigma;
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));
}
@ -304,7 +304,7 @@ TEST(JacobianFactor, CONSTRUCTOR_GaussianConditional )
// Call the constructor we are testing !
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));
}

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

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

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

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

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

@ -101,7 +101,7 @@ int main()
b2(7) = -1;
// 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();
int n = 1000000;
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);
for(size_t trial=0; trial<nTrials; ++trial) {
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(size_t d=0; d<blocksPerVar; ++d) {
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/IndexFactor.h>
#include <gtsam/linear/SharedNoiseModel.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/JacobianFactor.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]);
// 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 ;
@ -386,8 +386,8 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
// Tests range factor between a GeneralCamera and a Pose3
Graph graph;
graph.addCameraConstraint(0, GeneralCamera());
graph.add(RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.0, sharedSigma(1, 1.0)));
graph.add(PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 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)), noiseModel::Isotropic::Sigma(6, 1.0)));
Values init;
init.insert(X(0), GeneralCamera());
@ -410,9 +410,9 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
// Tests range factor between a CalibratedCamera and a Pose3
NonlinearFactorGraph graph;
graph.add(PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(), sharedSigma(6, 1.0)));
graph.add(RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.0, sharedSigma(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<CalibratedCamera>(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 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)), noiseModel::Isotropic::Sigma(6, 1.0)));
Values init;
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]);
// 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 ;

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

@ -393,7 +393,7 @@ TEST_UNSAFE(Pose2SLAM, expmap )
}
// 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)

10
matlab/VisualISAMInitialize.m
View File

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

1
matlab/VisualISAM_gui.m
View File

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

4
matlab/examples/LocalizationExample.m
View File

@ -20,13 +20,13 @@ 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 = 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(2, 3, odometry, odometryNoise);
%% Add three "GPS" measurements
% 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(2, gtsamPose2(2.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
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
%% Add two odometry factors
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(2, 3, odometry, odometryNoise);

7
matlab/examples/PlanarSLAMExample.m
View File

@ -28,18 +28,18 @@ graph = planarSLAMGraph;
%% Add prior
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
%% Add odometry
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(i2, i3, odometry, odometryNoise);
%% Add bearing/range measurement factors
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(i2, j1, gtsamRot2(90*degrees), 2, noiseModel);
graph.addBearingRange(i3, j2, gtsamRot2(90*degrees), 2, noiseModel);
@ -75,6 +75,7 @@ end
for i=1:3
plotPose2(pose{i},'g',P{i})
end
point = {};
for j=1:2
key = symbol('l',j);
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);
graph = planarSLAMGraph;
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
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(i2, i3, odometry, odometryNoise);
%% Except, for measurements we offer a choice
j1 = symbol('l',1); j2 = symbol('l',2);
degrees = pi/180;
noiseModel = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]);
noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.2]);
if 1
graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel);
graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel);
else
bearingModel = gtsamSharedNoiseModel_Sigmas(0.1);
bearingModel = gtsamnoiseModelDiagonal_Sigmas(0.1);
graph.addBearing(i1, j1, gtsamRot2(45*degrees), bearingModel);
graph.addBearing(i2, j1, gtsamRot2(90*degrees), bearingModel);
end

12
matlab/examples/Pose2SLAMExample.m
View File

@ -24,19 +24,19 @@ graph = pose2SLAMGraph;
%% Add prior
% gaussian for prior
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
%% Add 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(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 = 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);
% print
@ -60,11 +60,11 @@ cla;
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-');
marginals = graph.marginals(result);
P={};
for i=1:result.size()
pose_i = result.pose(i);
P{i}=marginals.marginalCovariance(i);
plotPose2(pose_i,'g',P{i})
P = marginals.marginalCovariance(i)
plotPose2(pose_i,'g',P);
end
axis([-0.6 4.8 -1 1])
axis equal

6
matlab/examples/Pose2SLAMExample_advanced.m
View File

@ -25,20 +25,20 @@ graph = pose2SLAMGraph;
%% Add 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
graph.addPrior(1, priorMean, priorNoise); % add directly to graph
%% Add 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)
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
%% Add measurements
% general noisemodel for measurements
measurementNoise = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]);
measurementNoise = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.2]);
% print
graph.print('full graph');

2
matlab/examples/Pose2SLAMExample_circle.m
View File

@ -19,7 +19,7 @@ p1 = hexagon.pose(1);
fg = pose2SLAMGraph;
fg.addPoseConstraint(0, p0);
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(1,2, 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
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);
initial.print(sprintf('Initial estimate:\n'));
%% Add a Gaussian prior on pose x_1
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
%% Plot Initial Estimate
@ -31,6 +31,7 @@ result.print(sprintf('\nFinal result:\n'));
%% Plot Covariance Ellipses
marginals = graph.marginals(result);
P={};
for i=1:result.size()-1
pose_i = result.pose(i);
P{i}=marginals.marginalCovariance(i);

6
matlab/examples/Pose2SLAMwSPCG.m
View File

@ -22,19 +22,19 @@ graph = pose2SLAMGraph;
%% Add prior
% gaussian for prior
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
%% Add 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(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 = 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);
% print

2
matlab/examples/Pose3SLAMExample.m
View File

@ -19,7 +19,7 @@ p1 = hexagon.pose(1);
fg = pose3SLAMGraph;
fg.addHardConstraint(0, p0);
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(1,2, 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';
%% 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);
%% 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;
%% Add factors for all measurements
measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma);
measurementNoise = gtsamnoiseModelIsotropic_Sigma(2,measurementNoiseSigma);
for i=1:length(data.Z)
for k=1:length(data.Z{i})
j = data.J{i}{k};
@ -41,9 +41,9 @@ for i=1:length(data.Z)
end
%% 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);
pointPriorNoise = gtsamSharedNoiseModel_Sigma(3,pointNoiseSigma);
pointPriorNoise = gtsamnoiseModelIsotropic_Sigma(3,pointNoiseSigma);
graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise);
%% 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
% format: fx fy skew cx cy baseline
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
% pose 1

4
matlab/examples/StereoVOExample_large.m
View File

@ -14,7 +14,7 @@
% format: fx fy skew cx cy baseline
calib = dlmread('../../examples/Data/VO_calibration.txt');
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
graph = visualSLAMGraph;
@ -57,7 +57,7 @@ graph.addPoseConstraint(key, first_pose);
%% optimize
fprintf(1,'Optimizing\n'); tic
result = graph.optimize(initial);
result = graph.optimize(initial,1);
toc
%% visualize initial trajectory, final trajectory, and final points

4
matlab/tests/testJacobianFactor.m
View File

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

4
matlab/tests/testKalmanFilter.m
View File

@ -22,13 +22,13 @@
F = eye(2,2);
B = eye(2,2);
u = [1.0; 0.0];
modelQ = gtsamSharedDiagonal([0.1;0.1]);
modelQ = gtsamnoiseModelDiagonal_Sigmas([0.1;0.1]);
Q = 0.01*eye(2,2);
H = eye(2,2);
z1 = [1.0, 0.0]';
z2 = [2.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);
%% 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'
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
display 'Tests complete!'

8
tests/smallExample.cpp
View File

@ -37,6 +37,8 @@ using namespace std;
namespace gtsam {
namespace example {
using namespace gtsam::noiseModel;
typedef boost::shared_ptr<NonlinearFactor> shared;
static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
@ -425,14 +427,14 @@ namespace example {
NonlinearFactorGraph nlfg;
// 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);
// Create horizontal constraints, 1...N*(N-1)
Point2 z1(1.0, 0.0); // move right
for (size_t x = 1; x < N; x++)
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);
}
@ -440,7 +442,7 @@ namespace example {
Point2 z2(0.0, 1.0); // move up
for (size_t x = 1; x <= N; x++)
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);
}

2
tests/testGaussianFactor.cpp
View File

@ -41,7 +41,7 @@ using symbol_shorthand::X;
using symbol_shorthand::L;
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);
//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,
7.071067811865475, 0.,
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((const GaussianFactor&)expectedFactor,*remaining.back(),tol));
@ -370,7 +370,7 @@ TEST( GaussianFactorGraph, eliminateAll )
// GaussianFactorGraph expected = createGaussianFactorGraph(ordering);
// Matrix A = eye(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[X(1)],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;
Matrix Ap = eye(1), An = eye(1) * -1;
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)], 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 )
//{
@ -922,7 +922,7 @@ SharedDiagonal model = sharedSigma(2,1);
TEST(GaussianFactorGraph, replace)
{
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(
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();
gfg.add(0, eye(2), zero(2), sharedSigma(2, 1.0));
gfg.add(0, -eye(2), 1, eye(2), ones(2), sharedSigma(2, 1.0));
gfg.add(1, -eye(2), 2, A12, ones(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), noiseModel::Isotropic::Sigma(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 actual(GaussianMultifrontalSolver(gfg).marginalCovariance(2));

10
tests/testGaussianISAM2.cpp
View File

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

8
tests/testGaussianJunctionTreeB.cpp
View File

@ -130,8 +130,8 @@ TEST(GaussianJunctionTree, slamlike) {
Values init;
planarSLAM::Graph newfactors;
planarSLAM::Graph fullgraph;
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas(Vector_(2, M_PI/100.0, 0.1));
size_t i = 0;
@ -194,8 +194,8 @@ TEST(GaussianJunctionTree, simpleMarginal) {
// Create a simple graph
pose2SLAM::Graph fg;
fg.addPrior(X(0), Pose2(), sharedSigma(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.addPrior(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.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;
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
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);
// 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(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(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 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);
// 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 )
{
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 p12=p1.between(p2), p23=p2.between(p3), p43=p4.between(p3);
graph.addOdometry(1,2, p12, cov);

4
tests/testInferenceB.cpp
View File

@ -55,8 +55,8 @@ TEST( inference, marginals )
TEST( inference, marginals2)
{
planarSLAM::Graph fg;
SharedDiagonal poseModel(sharedSigma(3, 0.1));
SharedDiagonal pointModel(sharedSigma(3, 0.1));
SharedDiagonal poseModel(noiseModel::Isotropic::Sigma(3, 0.1));
SharedDiagonal pointModel(noiseModel::Isotropic::Sigma(3, 0.1));
fg.addPrior(X(0), Pose2(), 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> {
public:
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
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> {
public:
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
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> {
public:
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
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) {
NonlinearFactorGraph fg;
fg.add(PriorFactor<Pose2>(0, Pose2(0,0,0), sharedSigma(3,1)));
fg.add(BetweenFactor<Pose2>(0, 1, Pose2(1,0,M_PI/2), sharedSigma(3,1)));
fg.add(BetweenFactor<Pose2>(1, 2, Pose2(1,0,M_PI/2), 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), noiseModel::Isotropic::Sigma(3,1)));
fg.add(BetweenFactor<Pose2>(1, 2, Pose2(1,0,M_PI/2), noiseModel::Isotropic::Sigma(3,1)));
Values init;
init.insert(0, Pose2(3,4,-M_PI));

4
tests/testRot3Optimization.cpp
View File

@ -41,11 +41,11 @@ TEST(Rot3, optimize) {
Values truth;
Values initial;
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) {
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)));
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();

2
tests/testSimulated2DOriented.cpp
View File

@ -58,7 +58,7 @@ TEST( simulated2DOriented, Dprior )
TEST( simulated2DOriented, constructor )
{
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::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
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
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
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
data.first->addPoseConstraint(0, Pose2());

27
wrap/matlab.h
View File

@ -25,6 +25,7 @@
using gtsam::Vector;
using gtsam::Matrix;
using gtsam::noiseModel::Base;
using gtsam::noiseModel::Gaussian;
using gtsam::noiseModel::Diagonal;
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 isUnit = mxIsClass(obj, "gtsamnoiseModelUnit");
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.");
}
#endif
@ -501,7 +502,7 @@ boost::shared_ptr<Diagonal> unwrap_shared_ptr(const mxArray* obj, const string&
bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic");
bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit");
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.");
}
#endif
@ -520,7 +521,27 @@ boost::shared_ptr<Gaussian> unwrap_shared_ptr(const mxArray* obj, const string&
bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic");
bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit");
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.");
}
#endif