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