From 6f1ea87a00f8a8ace1a5aecec1b9794ee5c20438 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Fri, 22 Jun 2012 19:36:49 +0000 Subject: [PATCH] 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 --- examples/CameraResectioning.cpp | 3 +- examples/LocalizationExample.cpp | 5 +- examples/OdometryExample.cpp | 5 +- examples/PlanarSLAMExample.cpp | 7 +- examples/Pose2SLAMExample.cpp | 7 +- examples/Pose2SLAMExample_advanced.cpp | 5 +- examples/Pose2SLAMExample_graph.cpp | 5 +- examples/Pose2SLAMwSPCG.cpp | 7 +- examples/VisualSLAMData.h | 6 +- gtsam.h | 72 ++++------- gtsam/linear/GaussianFactorGraph.h | 2 - gtsam/linear/HessianFactor.h | 1 - gtsam/linear/JacobianFactor.h | 2 +- gtsam/linear/KalmanFilter.cpp | 1 - gtsam/linear/KalmanFilter.h | 4 +- gtsam/linear/NoiseModel.cpp | 1 - gtsam/linear/NoiseModel.h | 36 +++--- gtsam/linear/SharedDiagonal.h | 88 ------------- gtsam/linear/SharedGaussian.h | 70 ---------- gtsam/linear/SharedNoiseModel.h | 121 ------------------ .../linear/tests/testGaussianFactorGraph.cpp | 28 ++-- .../linear/tests/testGaussianJunctionTree.cpp | 2 +- gtsam/linear/tests/testHessianFactor.cpp | 6 +- gtsam/linear/tests/testJacobianFactor.cpp | 14 +- gtsam/linear/tests/testKalmanFilter.cpp | 3 +- gtsam/linear/tests/testNoiseModel.cpp | 8 +- .../linear/tests/testSerializationLinear.cpp | 3 - gtsam/linear/tests/timeFactorOverhead.cpp | 6 +- gtsam/linear/tests/timeGaussianFactor.cpp | 2 +- gtsam/linear/tests/timeSLAMlike.cpp | 2 +- gtsam/nonlinear/NonlinearFactor.h | 2 +- gtsam/slam/tests/testGeneralSFMFactor.cpp | 12 +- .../testGeneralSFMFactor_Cal3Bundler.cpp | 2 +- gtsam/slam/tests/testPose2SLAM.cpp | 2 +- matlab/VisualISAMInitialize.m | 10 +- matlab/VisualISAM_gui.m | 1 + matlab/examples/LocalizationExample.m | 4 +- matlab/examples/OdometryExample.m | 4 +- matlab/examples/PlanarSLAMExample.m | 7 +- matlab/examples/PlanarSLAMExample_sampling.m | 8 +- matlab/examples/Pose2SLAMExample.m | 12 +- matlab/examples/Pose2SLAMExample_advanced.m | 6 +- matlab/examples/Pose2SLAMExample_circle.m | 2 +- matlab/examples/Pose2SLAMExample_graph.m | 5 +- matlab/examples/Pose2SLAMwSPCG.m | 6 +- matlab/examples/Pose3SLAMExample.m | 2 +- matlab/examples/Pose3SLAMExample_graph.m | 2 +- matlab/examples/SFMExample.m | 6 +- matlab/examples/StereoVOExample.m | 2 +- matlab/examples/StereoVOExample_large.m | 4 +- matlab/tests/testJacobianFactor.m | 4 +- matlab/tests/testKalmanFilter.m | 4 +- matlab/tests/testLocalizationExample.m | 49 +++++++ matlab/tests/testOdometryExample.m | 40 ++++++ matlab/tests/testPlanarSLAMExample.m | 68 ++++++++++ matlab/tests/testPose2SLAMExample.m | 60 +++++++++ matlab/tests/testPose3SLAMExample.m | 46 +++++++ matlab/tests/testSFMExample.m | 72 +++++++++++ matlab/tests/testStereoVOExample.m | 66 ++++++++++ matlab/tests/testVisualISAMExample.m | 53 ++++++++ matlab/tests/test_gtsam.m | 24 ++++ tests/smallExample.cpp | 8 +- tests/testGaussianFactor.cpp | 2 +- tests/testGaussianFactorGraphB.cpp | 10 +- tests/testGaussianISAM.cpp | 6 +- tests/testGaussianISAM2.cpp | 10 +- tests/testGaussianJunctionTreeB.cpp | 8 +- tests/testGradientDescentOptimizer.cpp | 6 +- tests/testGraph.cpp | 2 +- tests/testInferenceB.cpp | 4 +- tests/testNonlinearFactor.cpp | 6 +- tests/testNonlinearOptimizer.cpp | 6 +- tests/testRot3Optimization.cpp | 4 +- tests/testSimulated2DOriented.cpp | 2 +- tests/timeMultifrontalOnDataset.cpp | 2 +- tests/timeSequentialOnDataset.cpp | 2 +- wrap/matlab.h | 27 +++- 77 files changed, 708 insertions(+), 504 deletions(-) delete mode 100644 gtsam/linear/SharedDiagonal.h delete mode 100644 gtsam/linear/SharedGaussian.h delete mode 100644 gtsam/linear/SharedNoiseModel.h create mode 100644 matlab/tests/testLocalizationExample.m create mode 100644 matlab/tests/testOdometryExample.m create mode 100644 matlab/tests/testPlanarSLAMExample.m create mode 100644 matlab/tests/testPose2SLAMExample.m create mode 100644 matlab/tests/testPose3SLAMExample.m create mode 100644 matlab/tests/testSFMExample.m create mode 100644 matlab/tests/testStereoVOExample.m create mode 100644 matlab/tests/testVisualISAMExample.m diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 277c78ee0..4133d1e1e 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -22,6 +22,7 @@ #include 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 factor; graph.push_back( boost::make_shared(measurementNoise, X(1), calib, diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index caa87e48f..39f8de8a6 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -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(1, 0, 0, noiseModel)); graph.push_back(boost::make_shared(2, 2, 0, noiseModel)); graph.push_back(boost::make_shared(3, 4, 0, noiseModel)); diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index 3c00aca81..87844dfbc 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -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); diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index d58c8eb3e..2d739416a 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -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), diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 8118004af..61bdb1bda 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -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 diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index 083585c73..f727e27e2 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -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); diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 6abeceb55..4dcdbea08 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -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 diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index ddf0a7ad0..53566e7e8 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -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 diff --git a/examples/VisualSLAMData.h b/examples/VisualSLAMData.h index b96c24295..5416b3d43 100644 --- a/examples/VisualSLAMData.h +++ b/examples/VisualSLAMData.h @@ -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; isample()))*/); // 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; } diff --git a/gtsam.h b/gtsam.h index 21ee927db..78607651b 100644 --- a/gtsam.h +++ b/gtsam.h @@ -679,6 +679,9 @@ class VariableIndex { #include 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; diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 07d0c8774..b4878565e 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -31,8 +31,6 @@ namespace gtsam { - class SharedDiagonal; - /** return A*x-b * \todo Make this a member function - affects SubgraphPreconditioner */ template diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 4be0b950c..46955d45a 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -33,7 +33,6 @@ namespace gtsam { // Forward declarations class JacobianFactor; - class SharedDiagonal; class GaussianConditional; template class BayesNet; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index f5c8bf29b..7e1381f38 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 0df9d99ce..987191254 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 6df029b61..aeec47a39 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -18,6 +18,7 @@ */ #include +#include #ifndef KALMANFILTER_DEFAULT_FACTORIZATION #define KALMANFILTER_DEFAULT_FACTORIZATION QR @@ -25,9 +26,6 @@ namespace gtsam { - class SharedDiagonal; - class SharedGaussian; - /** * Kalman Filter class * diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index f212118c9..6d231748e 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -29,7 +29,6 @@ #include #include #include -#include static double inf = std::numeric_limits::infinity(); using namespace std; diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 50e59a85d..c67828822 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -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 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 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 diff --git a/gtsam/linear/SharedDiagonal.h b/gtsam/linear/SharedDiagonal.h deleted file mode 100644 index f57d6d726..000000000 --- a/gtsam/linear/SharedDiagonal.h +++ /dev/null @@ -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 - -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 - * 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 - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("SharedDiagonal", - boost::serialization::base_object(*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); - } - -} - diff --git a/gtsam/linear/SharedGaussian.h b/gtsam/linear/SharedGaussian.h deleted file mode 100644 index a3154bac1..000000000 --- a/gtsam/linear/SharedGaussian.h +++ /dev/null @@ -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 -#include - -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 - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("SharedGaussian", - boost::serialization::base_object(*this)); - } - }; -} diff --git a/gtsam/linear/SharedNoiseModel.h b/gtsam/linear/SharedNoiseModel.h deleted file mode 100644 index c9b2de6d4..000000000 --- a/gtsam/linear/SharedNoiseModel.h +++ /dev/null @@ -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 - -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::Gaussian::Covariance(covariance))) {} - - SharedNoiseModel(const Vector& sigmas) : - Base(boost::static_pointer_cast( - 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::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 - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("SharedNoiseModel", - boost::serialization::base_object(*this)); - } - - /// @} - - }; -} diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 6bb4a6348..37aa440ff 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -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 lfg; lfg.push_back(f1); @@ -286,7 +286,7 @@ TEST( NonlinearFactorGraph, combine2){ TEST(GaussianFactor, linearFactorN){ Matrix I = eye(2); vector 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 > 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 > 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 > 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); diff --git a/gtsam/linear/tests/testGaussianJunctionTree.cpp b/gtsam/linear/tests/testGaussianJunctionTree.cpp index a10f80a4d..b49d7fff0 100644 --- a/gtsam/linear/tests/testGaussianJunctionTree.cpp +++ b/gtsam/linear/tests/testGaussianJunctionTree.cpp @@ -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)); diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index bd76fd1d0..9c2adcffa 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -93,7 +93,7 @@ TEST(HessianFactor, ConversionConstructor) { vector > 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 > 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)); } diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 0d46807b6..af1d02d1f 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -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 > 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)); } diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index ca50c12e9..b45ac6a7b 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -18,8 +18,7 @@ */ #include -#include -#include +#include #include #include diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 94a174543..9b53a3be1 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -25,8 +25,6 @@ using namespace boost::assign; #include #include #include -#include -#include 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; diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index 8082d3626..a646773f2 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -22,9 +22,6 @@ #include #include #include -#include -#include -#include #include #include diff --git a/gtsam/linear/tests/timeFactorOverhead.cpp b/gtsam/linear/tests/timeFactorOverhead.cpp index a843efd84..5ca4ceb59 100644 --- a/gtsam/linear/tests/timeFactorOverhead.cpp +++ b/gtsam/linear/tests/timeFactorOverhead.cpp @@ -60,7 +60,7 @@ int main(int argc, char *argv[]) { blockGfgs.reserve(nTrials); for(size_t trial=0; trial combGfgs; for(size_t trial=0; trial > terms; terms.reserve(varsPerBlock); diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index e1cfb2bc3..21e3d17d7 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index d6fcb5c5a..58f06c8d4 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -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(X(0), X(1), 2.0, sharedSigma(1, 10.0))); + graph.add(RangeFactor(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(X(0), X(1), 2.0, sharedSigma(1, 1.0))); - graph.add(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0))); + graph.add(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); + graph.add(PriorFactor(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(X(0), CalibratedCamera(), sharedSigma(6, 1.0))); - graph.add(RangeFactor(X(0), X(1), 2.0, sharedSigma(1, 1.0))); - graph.add(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0))); + graph.add(PriorFactor(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.0))); + graph.add(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); + graph.add(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0))); Values init; init.insert(X(0), CalibratedCamera()); diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 9afd8eff1..1b1ae5390 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -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(X(0), X(1), 2.0, sharedSigma(1, 10.0))); + graph.add(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0))); const double reproj_error = 1e-5 ; diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index 55558b106..96f5dad03 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -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) diff --git a/matlab/VisualISAMInitialize.m b/matlab/VisualISAMInitialize.m index 43e8162dd..01fdfe79c 100644 --- a/matlab/VisualISAMInitialize.m +++ b/matlab/VisualISAMInitialize.m @@ -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; \ No newline at end of file diff --git a/matlab/VisualISAM_gui.m b/matlab/VisualISAM_gui.m index 5fd0cd1d9..cdb2ddbaf 100644 --- a/matlab/VisualISAM_gui.m +++ b/matlab/VisualISAM_gui.m @@ -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) diff --git a/matlab/examples/LocalizationExample.m b/matlab/examples/LocalizationExample.m index d9c374e39..a523c312d 100644 --- a/matlab/examples/LocalizationExample.m +++ b/matlab/examples/LocalizationExample.m @@ -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); diff --git a/matlab/examples/OdometryExample.m b/matlab/examples/OdometryExample.m index d6dae314d..e712e4e21 100644 --- a/matlab/examples/OdometryExample.m +++ b/matlab/examples/OdometryExample.m @@ -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); diff --git a/matlab/examples/PlanarSLAMExample.m b/matlab/examples/PlanarSLAMExample.m index 2bf27d11c..27dc97a08 100644 --- a/matlab/examples/PlanarSLAMExample.m +++ b/matlab/examples/PlanarSLAMExample.m @@ -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); diff --git a/matlab/examples/PlanarSLAMExample_sampling.m b/matlab/examples/PlanarSLAMExample_sampling.m index f4124e003..99429900f 100644 --- a/matlab/examples/PlanarSLAMExample_sampling.m +++ b/matlab/examples/PlanarSLAMExample_sampling.m @@ -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 diff --git a/matlab/examples/Pose2SLAMExample.m b/matlab/examples/Pose2SLAMExample.m index f60154fa3..29133031e 100644 --- a/matlab/examples/Pose2SLAMExample.m +++ b/matlab/examples/Pose2SLAMExample.m @@ -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 diff --git a/matlab/examples/Pose2SLAMExample_advanced.m b/matlab/examples/Pose2SLAMExample_advanced.m index 827cf5661..fbffe9911 100644 --- a/matlab/examples/Pose2SLAMExample_advanced.m +++ b/matlab/examples/Pose2SLAMExample_advanced.m @@ -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'); diff --git a/matlab/examples/Pose2SLAMExample_circle.m b/matlab/examples/Pose2SLAMExample_circle.m index 7e7ab6694..828b6f5f6 100644 --- a/matlab/examples/Pose2SLAMExample_circle.m +++ b/matlab/examples/Pose2SLAMExample_circle.m @@ -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); diff --git a/matlab/examples/Pose2SLAMExample_graph.m b/matlab/examples/Pose2SLAMExample_graph.m index 7a8644432..06a16ac55 100644 --- a/matlab/examples/Pose2SLAMExample_graph.m +++ b/matlab/examples/Pose2SLAMExample_graph.m @@ -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); diff --git a/matlab/examples/Pose2SLAMwSPCG.m b/matlab/examples/Pose2SLAMwSPCG.m index f9c0537be..3f8f8adf1 100644 --- a/matlab/examples/Pose2SLAMwSPCG.m +++ b/matlab/examples/Pose2SLAMwSPCG.m @@ -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 diff --git a/matlab/examples/Pose3SLAMExample.m b/matlab/examples/Pose3SLAMExample.m index e8f5c7c32..4cc41a4de 100644 --- a/matlab/examples/Pose3SLAMExample.m +++ b/matlab/examples/Pose3SLAMExample.m @@ -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); diff --git a/matlab/examples/Pose3SLAMExample_graph.m b/matlab/examples/Pose3SLAMExample_graph.m index acaf2c920..f2c411d08 100644 --- a/matlab/examples/Pose3SLAMExample_graph.m +++ b/matlab/examples/Pose3SLAMExample_graph.m @@ -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 diff --git a/matlab/examples/SFMExample.m b/matlab/examples/SFMExample.m index 8e56c00b7..8aa0105b2 100644 --- a/matlab/examples/SFMExample.m +++ b/matlab/examples/SFMExample.m @@ -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 diff --git a/matlab/examples/StereoVOExample.m b/matlab/examples/StereoVOExample.m index 99717fda5..980a3c5a1 100644 --- a/matlab/examples/StereoVOExample.m +++ b/matlab/examples/StereoVOExample.m @@ -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 diff --git a/matlab/examples/StereoVOExample_large.m b/matlab/examples/StereoVOExample_large.m index 171836cf8..ef1211e2a 100644 --- a/matlab/examples/StereoVOExample_large.m +++ b/matlab/examples/StereoVOExample_large.m @@ -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 diff --git a/matlab/tests/testJacobianFactor.m b/matlab/tests/testJacobianFactor.m index 2209b36a6..321f7c4ab 100644 --- a/matlab/tests/testJacobianFactor.m +++ b/matlab/tests/testJacobianFactor.m @@ -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 diff --git a/matlab/tests/testKalmanFilter.m b/matlab/tests/testKalmanFilter.m index e9c1b7149..a181c68a1 100644 --- a/matlab/tests/testKalmanFilter.m +++ b/matlab/tests/testKalmanFilter.m @@ -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 diff --git a/matlab/tests/testLocalizationExample.m b/matlab/tests/testLocalizationExample.m new file mode 100644 index 000000000..07b47257b --- /dev/null +++ b/matlab/tests/testLocalizationExample.m @@ -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 diff --git a/matlab/tests/testOdometryExample.m b/matlab/tests/testOdometryExample.m new file mode 100644 index 000000000..e83d0ec0a --- /dev/null +++ b/matlab/tests/testOdometryExample.m @@ -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)); \ No newline at end of file diff --git a/matlab/tests/testPlanarSLAMExample.m b/matlab/tests/testPlanarSLAMExample.m new file mode 100644 index 000000000..6446da790 --- /dev/null +++ b/matlab/tests/testPlanarSLAMExample.m @@ -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)); + + diff --git a/matlab/tests/testPose2SLAMExample.m b/matlab/tests/testPose2SLAMExample.m new file mode 100644 index 000000000..bf4f261e7 --- /dev/null +++ b/matlab/tests/testPose2SLAMExample.m @@ -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)); + + diff --git a/matlab/tests/testPose3SLAMExample.m b/matlab/tests/testPose3SLAMExample.m new file mode 100644 index 000000000..9d2cf4271 --- /dev/null +++ b/matlab/tests/testPose3SLAMExample.m @@ -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)); + + diff --git a/matlab/tests/testSFMExample.m b/matlab/tests/testSFMExample.m new file mode 100644 index 000000000..ebda9535e --- /dev/null +++ b/matlab/tests/testSFMExample.m @@ -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 diff --git a/matlab/tests/testStereoVOExample.m b/matlab/tests/testStereoVOExample.m new file mode 100644 index 000000000..8f751d313 --- /dev/null +++ b/matlab/tests/testStereoVOExample.m @@ -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)); \ No newline at end of file diff --git a/matlab/tests/testVisualISAMExample.m b/matlab/tests/testVisualISAMExample.m new file mode 100644 index 000000000..c6fd46f4a --- /dev/null +++ b/matlab/tests/testVisualISAMExample.m @@ -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 diff --git a/matlab/tests/test_gtsam.m b/matlab/tests/test_gtsam.m index aab3dac0f..f35c0dc73 100644 --- a/matlab/tests/test_gtsam.m +++ b/matlab/tests/test_gtsam.m @@ -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!' diff --git a/tests/smallExample.cpp b/tests/smallExample.cpp index b0af6c723..100a4db4c 100644 --- a/tests/smallExample.cpp +++ b/tests/smallExample.cpp @@ -37,6 +37,8 @@ using namespace std; namespace gtsam { namespace example { + using namespace gtsam::noiseModel; + typedef boost::shared_ptr 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); } diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp index bc0040f37..0097ec49a 100644 --- a/tests/testGaussianFactor.cpp +++ b/tests/testGaussianFactor.cpp @@ -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 diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 9b3f9eb0d..20972add9 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -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)); diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index a6eccd70f..6ae4a7b20 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -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)); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index c79e4e8fc..b5c1d4b89 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -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 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 expected; // expected.insert(0); @@ -844,7 +844,7 @@ TEST(ISAM2, clone) { // Modify original isam NonlinearFactorGraph factors; factors.add(BetweenFactor(0, 10, - isam.calculateEstimate(0).between(isam.calculateEstimate(10)), sharedUnit(3))); + isam.calculateEstimate(0).between(isam.calculateEstimate(10)), noiseModel::Unit::Create(3))); isam.update(factors); CHECK(assert_equal(createSlamlikeISAM2(), clone2)); diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 5b7555d37..769e95d26 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -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()); diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 54130b664..34a39fdca 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -26,18 +26,18 @@ boost::tuple 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 diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 63cbd43c8..0a3f3a562 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -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); diff --git a/tests/testInferenceB.cpp b/tests/testInferenceB.cpp index 81b24a1e7..5da4ca955 100644 --- a/tests/testInferenceB.cpp +++ b/tests/testInferenceB.cpp @@ -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); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 0a9c59f8b..b1717a79b 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -240,7 +240,7 @@ TEST( NonlinearFactor, linearize_constraint2 ) class TestFactor4 : public NoiseModelFactor4 { public: typedef NoiseModelFactor4 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 { public: typedef NoiseModelFactor5 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 { public: typedef NoiseModelFactor6 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, diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index ee681db3e..f7112bb03 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -232,9 +232,9 @@ TEST(NonlinearOptimizer, NullFactor) { TEST(NonlinearOptimizer, MoreOptimization) { NonlinearFactorGraph fg; - fg.add(PriorFactor(0, Pose2(0,0,0), sharedSigma(3,1))); - fg.add(BetweenFactor(0, 1, Pose2(1,0,M_PI/2), sharedSigma(3,1))); - fg.add(BetweenFactor(1, 2, Pose2(1,0,M_PI/2), sharedSigma(3,1))); + fg.add(PriorFactor(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1))); + fg.add(BetweenFactor(0, 1, Pose2(1,0,M_PI/2), noiseModel::Isotropic::Sigma(3,1))); + fg.add(BetweenFactor(1, 2, Pose2(1,0,M_PI/2), noiseModel::Isotropic::Sigma(3,1))); Values init; init.insert(0, Pose2(3,4,-M_PI)); diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp index 3e78414e9..89b621ed2 100644 --- a/tests/testRot3Optimization.cpp +++ b/tests/testRot3Optimization.cpp @@ -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(); diff --git a/tests/testSimulated2DOriented.cpp b/tests/testSimulated2DOriented.cpp index 24fe6ec4e..bce932235 100644 --- a/tests/testSimulated2DOriented.cpp +++ b/tests/testSimulated2DOriented.cpp @@ -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; diff --git a/tests/timeMultifrontalOnDataset.cpp b/tests/timeMultifrontalOnDataset.cpp index 3d8ccf77e..05d2c9398 100644 --- a/tests/timeMultifrontalOnDataset.cpp +++ b/tests/timeMultifrontalOnDataset.cpp @@ -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()); diff --git a/tests/timeSequentialOnDataset.cpp b/tests/timeSequentialOnDataset.cpp index a37b9910c..4a53737e0 100644 --- a/tests/timeSequentialOnDataset.cpp +++ b/tests/timeSequentialOnDataset.cpp @@ -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()); diff --git a/wrap/matlab.h b/wrap/matlab.h index 4d98ede30..313cfaa9f 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -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 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 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 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* handle = ObjectHandle::from_mex_handle(mxh); + return handle->get_object(); +} + +// Base +template <> +boost::shared_ptr 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