diff --git a/cmake/example_cmake_find_gtsam/main.cpp b/cmake/example_cmake_find_gtsam/main.cpp index 488883883..12026178d 100644 --- a/cmake/example_cmake_find_gtsam/main.cpp +++ b/cmake/example_cmake_find_gtsam/main.cpp @@ -33,7 +33,6 @@ // Here we will use Between factors for the relative motion described by odometry measurements. // We will also use a Between Factor to encode the loop closure constraint // Also, we will initialize the robot at the origin using a Prior factor. -#include #include // When the factors are created, we will add them to a Factor Graph. As the factors we are using @@ -69,7 +68,7 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.emplace_shared >(1, Pose2(0, 0, 0), priorNoise); + graph.addPrior<>(1, Pose2(0, 0, 0), priorNoise); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); diff --git a/doc/Code/OdometryExample.cpp b/doc/Code/OdometryExample.cpp index 6e736e2d7..0e7d38b76 100644 --- a/doc/Code/OdometryExample.cpp +++ b/doc/Code/OdometryExample.cpp @@ -5,7 +5,7 @@ NonlinearFactorGraph graph; Pose2 priorMean(0.0, 0.0, 0.0); noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); -graph.add(PriorFactor(1, priorMean, priorNoise)); +graph.addPrior<>(1, priorMean, priorNoise); // Add two odometry factors Pose2 odometry(2.0, 0.0, 0.0); diff --git a/doc/Code/Pose2SLAMExample.cpp b/doc/Code/Pose2SLAMExample.cpp index 1738aabc5..be7c5d732 100644 --- a/doc/Code/Pose2SLAMExample.cpp +++ b/doc/Code/Pose2SLAMExample.cpp @@ -1,7 +1,7 @@ NonlinearFactorGraph graph; noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); -graph.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); +graph.addPrior<>(1, Pose2(0, 0, 0), priorNoise); // Add odometry factors noiseModel::Diagonal::shared_ptr model = diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp index 21fe6e661..d0a7b7278 100644 --- a/examples/ISAM2Example_SmartFactor.cpp +++ b/examples/ISAM2Example_SmartFactor.cpp @@ -57,7 +57,7 @@ int main(int argc, char* argv[]) { vector poses = {pose1, pose2, pose3, pose4, pose5}; // Add first pose - graph.emplace_shared>(X(0), poses[0], noise); + graph.addPrior<>(X(0), poses[0], noise); initialEstimate.insert(X(0), poses[0].compose(delta)); // Create smart factor with measurement from first pose only @@ -71,7 +71,7 @@ int main(int argc, char* argv[]) { cout << "i = " << i << endl; // Add prior on new pose - graph.emplace_shared>(X(i), poses[i], noise); + graph.addPrior<>(X(i), poses[i], noise); initialEstimate.insert(X(i), poses[i].compose(delta)); // "Simulate" measurement from this pose diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp b/examples/ISAM2_SmartFactorStereo_IMU.cpp index f39e9f4eb..f7615635c 100644 --- a/examples/ISAM2_SmartFactorStereo_IMU.cpp +++ b/examples/ISAM2_SmartFactorStereo_IMU.cpp @@ -129,18 +129,16 @@ int main(int argc, char* argv[]) { // Pose prior - at identity auto priorPoseNoise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished()); - graph.emplace_shared>(X(1), Pose3::identity(), - priorPoseNoise); + graph.addPrior<>(X(1), Pose3::identity(), priorPoseNoise); initialEstimate.insert(X(0), Pose3::identity()); // Bias prior - graph.add(PriorFactor(B(1), imu.priorImuBias, - imu.biasNoiseModel)); + graph.addPrior<>(B(1), imu.priorImuBias, + imu.biasNoiseModel); initialEstimate.insert(B(0), imu.priorImuBias); // Velocity prior - assume stationary - graph.add( - PriorFactor(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel)); + graph.addPrior<>(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel); initialEstimate.insert(V(0), Vector3(0, 0, 0)); int lastFrame = 1; diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index c2b819a9e..7e444e29e 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -7,7 +7,6 @@ #include #include #include -#include #include @@ -61,21 +60,18 @@ int main(int argc, char* argv[]) { // 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z. auto noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); - newgraph.push_back(PriorFactor(X(0), pose_0, noise)); + newgraph.addPrior<>(X(0), pose_0, noise); // Add imu priors Key biasKey = Symbol('b', 0); auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1)); - PriorFactor biasprior(biasKey, imuBias::ConstantBias(), - biasnoise); - newgraph.push_back(biasprior); + newgraph.addPrior<>(biasKey, imuBias::ConstantBias(), biasnoise); initialEstimate.insert(biasKey, imuBias::ConstantBias()); auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); Vector n_velocity(3); n_velocity << 0, angular_velocity * radius, 0; - PriorFactor velprior(V(0), n_velocity, velnoise); - newgraph.push_back(velprior); + newgraph.addPrior<>(V(0), n_velocity, velnoise); initialEstimate.insert(V(0), n_velocity); diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index e1a3f6bcf..25099fc39 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -41,7 +41,6 @@ #include #include #include -#include #include #include #include @@ -129,9 +128,9 @@ int main(int argc, char* argv[]) // Add all prior factors (pose, velocity, bias) to the graph. NonlinearFactorGraph *graph = new NonlinearFactorGraph(); - graph->add(PriorFactor(X(correction_count), prior_pose, pose_noise_model)); - graph->add(PriorFactor(V(correction_count), prior_velocity,velocity_noise_model)); - graph->add(PriorFactor(B(correction_count), prior_imu_bias,bias_noise_model)); + graph->addPrior<>(X(correction_count), prior_pose, pose_noise_model); + graph->addPrior<>(V(correction_count), prior_velocity,velocity_noise_model); + graph->addPrior<>(B(correction_count), prior_imu_bias,bias_noise_model); // We use the sensor specs to build the noise model for the IMU factor. double accel_noise_sigma = 0.0003924; diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index b0736a27b..fc39d7aee 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -22,7 +22,6 @@ */ #include -#include #include #include #include @@ -37,7 +36,7 @@ int main(int argc, char** argv) { Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.emplace_shared >(1, priorMean, priorNoise); + graph.addPrior<>(1, priorMean, priorNoise); Pose2 odometry(2.0, 0.0, 0.0); noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index 7dec5ffca..cce27231d 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -29,7 +29,6 @@ // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. // Here we will use Between factors for the relative motion described by odometry measurements. // Also, we will initialize the robot at the origin using a Prior factor. -#include #include // When the factors are created, we will add them to a Factor Graph. As the factors we are using @@ -65,7 +64,7 @@ int main(int argc, char** argv) { // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.emplace_shared >(1, priorMean, priorNoise); + graph.addPrior<>(1, priorMean, priorNoise); // Add odometry factors Pose2 odometry(2.0, 0.0, 0.0); diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index 2b24b02fb..e329c0da0 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -40,7 +40,6 @@ // Here we will use a RangeBearing factor for the range-bearing measurements to identified // landmarks, and Between factors for the relative motion described by odometry measurements. // Also, we will initialize the robot at the origin using a Prior factor. -#include #include #include @@ -81,7 +80,7 @@ int main(int argc, char** argv) { // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.emplace_shared >(x1, prior, priorNoise); // add directly to graph + graph.addPrior<>(x1, prior, 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) diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index da29aff0e..f22b50a13 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -36,7 +36,6 @@ // Here we will use Between factors for the relative motion described by odometry measurements. // We will also use a Between Factor to encode the loop closure constraint // Also, we will initialize the robot at the origin using a Prior factor. -#include #include // When the factors are created, we will add them to a Factor Graph. As the factors we are using @@ -72,7 +71,7 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.emplace_shared >(1, Pose2(0, 0, 0), priorNoise); + graph.addPrior<>(1, Pose2(0, 0, 0), priorNoise); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index f4c38f3a4..68bd4cbbf 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -19,7 +19,6 @@ */ #include -#include #include #include #include @@ -65,7 +64,7 @@ int main(const int argc, const char *argv[]) { // Add prior on the pose having index (key) = 0 noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); - graph->add(PriorFactor(0, Pose2(), priorModel)); + graph.addPrior<>(0, Pose2(), priorModel); std::cout << "Adding prior on pose 0 " << std::endl; GaussNewtonParams params; diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 2f0496f3a..97a16e72d 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -17,7 +17,6 @@ */ // For an explanation of headers below, please see Pose2SLAMExample.cpp -#include #include #include #include @@ -43,7 +42,7 @@ int main (int argc, char** argv) { // Add a Gaussian prior on first poses Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01)); - graph->push_back(PriorFactor(0, priorMean, priorNoise)); + graph -> addPrior<>(0, priorMean, priorNoise); // Single Step Optimization using Levenberg-Marquardt Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 9f6ce922f..c3301812a 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -17,7 +17,6 @@ */ // For an explanation of headers below, please see Pose2SLAMExample.cpp -#include #include #include #include @@ -33,7 +32,7 @@ int main (int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.emplace_shared >(1, Pose2(0, 0, 0), priorNoise); + graph.addPrior<>(1, Pose2(0, 0, 0), priorNoise); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index 59b507bd5..a379ddd6b 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -21,7 +21,6 @@ #include #include -#include #include #include @@ -44,7 +43,7 @@ int main(const int argc, const char *argv[]) { // Add prior on the pose having index (key) = 0 noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); - graph->add(PriorFactor(0, Pose2(), priorModel)); + graph->addPrior<>(0, Pose2(), priorModel); graph->print(); std::cout << "Computing LAGO estimate" << std::endl; diff --git a/examples/Pose2SLAMStressTest.cpp b/examples/Pose2SLAMStressTest.cpp index 8c70d5d07..5c667a1e9 100644 --- a/examples/Pose2SLAMStressTest.cpp +++ b/examples/Pose2SLAMStressTest.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include @@ -48,7 +47,7 @@ void testGtsam(int numberNodes) { Pose3 first = Pose3(first_M); NonlinearFactorGraph graph; - graph.add(PriorFactor(0, first, priorModel)); + graph.addPrior<>(0, first, priorModel); // vo noise model auto VOCovarianceModel = noiseModel::Isotropic::Variance(6, 1e-3); diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 8de258977..0be18e827 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -17,7 +17,6 @@ */ // For an explanation of headers below, please see Pose2SLAMExample.cpp -#include #include #include #include @@ -36,7 +35,7 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin Pose2 prior(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.emplace_shared >(1, prior, priorNoise); + graph.addPrior<>(1, prior, priorNoise); // 2b. Add odometry factors noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); diff --git a/examples/Pose3Localization.cpp b/examples/Pose3Localization.cpp index 4dd7af960..7b89ed86a 100644 --- a/examples/Pose3Localization.cpp +++ b/examples/Pose3Localization.cpp @@ -19,7 +19,6 @@ #include #include -#include #include #include #include @@ -48,7 +47,7 @@ int main(const int argc, const char *argv[]) { for(const Values::ConstKeyValuePair& key_value: *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; - graph->add(PriorFactor(firstKey, Pose3(), priorModel)); + graph->addPrior<>(firstKey, Pose3(), priorModel); break; } diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp index d25982ef4..8e37d6950 100644 --- a/examples/Pose3SLAMExample_g2o.cpp +++ b/examples/Pose3SLAMExample_g2o.cpp @@ -19,7 +19,6 @@ #include #include -#include #include #include @@ -47,7 +46,7 @@ int main(const int argc, const char *argv[]) { for(const Values::ConstKeyValuePair& key_value: *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; - graph->add(PriorFactor(firstKey, Pose3(), priorModel)); + graph->addPrior<>(firstKey, Pose3(), priorModel); break; } diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp index 63e81d63f..4b34db252 100644 --- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp +++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include using namespace std; @@ -47,7 +46,7 @@ int main(const int argc, const char *argv[]) { for(const Values::ConstKeyValuePair& key_value: *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; - graph->add(PriorFactor(firstKey, Pose3(), priorModel)); + graph->addPrior<>(firstKey, Pose3(), priorModel); break; } diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index a7f773bcd..f059e4ae2 100644 --- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include using namespace std; @@ -47,7 +46,7 @@ int main(const int argc, const char *argv[]) { for(const Values::ConstKeyValuePair& key_value: *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; - graph->add(PriorFactor(firstKey, Pose3(), priorModel)); + graph->addPrior<>(firstKey, Pose3(), priorModel); break; } diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 8c19b44d2..50ade34e3 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -37,7 +37,6 @@ // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics SLAM problems. -#include #include #include #include @@ -124,7 +123,7 @@ int main (int argc, char** argv) { Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, M_PI - 2.02108900000000); NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(0, pose0, priorNoise)); + newFactors.addPrior<>(0, pose0, priorNoise); Values initial; initial.insert(0, pose0); diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 18ca0040b..8912dfd9c 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -30,7 +30,6 @@ // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. // Here we will use Projection factors to model the camera's landmark observations. // Also, we will initialize the robot at some location using a Prior factor. -#include #include // When the factors are created, we will add them to a Factor Graph. As the factors we are using @@ -75,7 +74,7 @@ int main(int argc, char* argv[]) { // Add a prior on pose x1. This indirectly specifies where the origin is. noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); // add directly to graph + graph.addPrior<>(Symbol('x', 0), poses[0], poseNoise); // add directly to graph // Simulated measurements from each camera pose, adding them to the factor graph for (size_t i = 0; i < poses.size(); ++i) { @@ -90,7 +89,7 @@ int main(int argc, char* argv[]) { // Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance // between the first camera and the first landmark. All other landmark positions are interpreted using this scale. noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // add directly to graph + graph.addPrior<>(Symbol('l', 0), points[0], pointNoise); // add directly to graph graph.print("Factor Graph:\n"); // Create the data structure to hold the initial estimate to the solution diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index 6e28b87c9..23f0dc26e 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -82,13 +82,13 @@ int main(int argc, char* argv[]) { // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); - graph.emplace_shared >(0, poses[0], noise); + graph.addPrior<>(0, poses[0], noise); // Because the structure-from-motion problem has a scale ambiguity, the problem is // still under-constrained. Here we add a prior on the second pose x1, so this will // fix the scale by indicating the distance between x0 and x1. // Because these two are fixed, the rest of the poses will be also be fixed. - graph.emplace_shared >(1, poses[1], noise); // add directly to graph + graph.addPrior<>(1, poses[1], noise); // add directly to graph graph.print("Factor Graph:\n"); diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 47d69160f..079877a3a 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -74,10 +74,10 @@ int main(int argc, char* argv[]) { // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); - graph.emplace_shared >(0, poses[0], noise); + graph.addPrior<>(0, poses[0], noise); // Fix the scale ambiguity by adding a prior - graph.emplace_shared >(1, poses[0], noise); + graph.addPrior<>(1, poses[0], noise); // Create the initial estimate to the solution Values initialEstimate; diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index c0c1e869e..97f830a05 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include // for loading BAL datasets ! #include @@ -66,8 +65,8 @@ int main (int argc, char* argv[]) { // Add a prior on pose x1. This indirectly specifies where the origin is. // and a prior on the position of the first landmark to fix the scale - graph.emplace_shared >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); - graph.emplace_shared > (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); + graph.addPrior<>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + graph.addPrior<>(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); // Create initial estimate Values initial; diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index e5bc8cb90..7c7a9a9af 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include // for loading BAL datasets ! @@ -71,8 +70,8 @@ int main (int argc, char* argv[]) { // Add a prior on pose x1. This indirectly specifies where the origin is. // and a prior on the position of the first landmark to fix the scale - graph.emplace_shared >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); - graph.emplace_shared >(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); + graph.addPrior<>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + graph.addPrior<>(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); // Create initial estimate Values initial; diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index 9324989f6..a2c709f9c 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -33,7 +33,6 @@ #include // SFM-specific factors -#include #include // does calibration ! // Standard headers @@ -54,7 +53,7 @@ int main(int argc, char* argv[]) { // Add a prior on pose x1. noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); + graph.addPrior<>(Symbol('x', 0), poses[0], poseNoise); // Simulated measurements from each camera pose, adding them to the factor graph Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); @@ -71,11 +70,11 @@ int main(int argc, char* argv[]) { // Add a prior on the position of the first landmark. noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // add directly to graph + graph.addPrior<>(Symbol('l', 0), points[0], pointNoise); // add directly to graph // Add a prior on the calibration. noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished()); - graph.emplace_shared >(Symbol('K', 0), K, calNoise); + graph.addPrior<>(Symbol('K', 0), K, calNoise); // Create the initial estimate to the solution // now including an estimate on the camera calibration parameters diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 525a6ab1e..37399b963 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -34,7 +34,6 @@ #include #include #include -#include #include #include #include @@ -281,7 +280,7 @@ void runIncremental() NonlinearFactorGraph newFactors; Values newVariables; - newFactors.push_back(boost::make_shared >(firstPose, Pose(), noiseModel::Unit::Create(3))); + newFactors.addPrior<>(firstPose, Pose(), noiseModel::Unit::Create(3)); newVariables.insert(firstPose, Pose()); isam2.update(newFactors, newVariables); @@ -464,7 +463,7 @@ void runBatch() cout << "Creating batch optimizer..." << endl; NonlinearFactorGraph measurements = datasetMeasurements; - measurements.push_back(boost::make_shared >(0, Pose(), noiseModel::Unit::Create(3))); + measurements.addPrior<>(0, Pose(), noiseModel::Unit::Create(3)); gttic_(Create_optimizer); GaussNewtonParams params; diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 182394617..0c4bf04d0 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -47,7 +47,6 @@ // Adjustment problems. Here we will use Projection factors to model the // camera's landmark observations. Also, we will initialize the robot at some // location using a Prior factor. -#include #include #include @@ -110,13 +109,11 @@ int main(int argc, char* argv[]) { static auto kPosePrior = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)) .finished()); - graph.emplace_shared >(Symbol('x', 0), poses[0], - kPosePrior); + graph.addPrior<>(Symbol('x', 0), poses[0], kPosePrior); // Add a prior on landmark l0 static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1); - graph.emplace_shared >(Symbol('l', 0), points[0], - kPointPrior); + graph.addPrior<>(Symbol('l', 0), points[0], kPointPrior); // Add initial guesses to all observed landmarks // Intentionally initialize the variables off from the ground truth diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 17f08afbe..5169c5bd4 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -38,7 +38,6 @@ // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. // Here we will use Projection factors to model the camera's landmark observations. // Also, we will initialize the robot at some location using a Prior factor. -#include #include // We want to use iSAM to solve the structure-from-motion problem incrementally, so @@ -108,12 +107,12 @@ int main(int argc, char* argv[]) { // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); - graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); + graph.addPrior<>(Symbol('x', 0), poses[0], poseNoise); // Add a prior on landmark l0 noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); + graph.addPrior<>(Symbol('l', 0), points[0], pointNoise); // Add initial guesses to all observed landmarks Point3 noise(-0.25, 0.20, 0.15); diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index d2f0c91df..8f47b0d37 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -382,8 +382,8 @@ TEST( triangulation, StereotriangulateNonlinear ) { graph.emplace_shared(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK); const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9); - graph.emplace_shared >(Symbol('x',1), Pose3(m1), posePrior); - graph.emplace_shared >(Symbol('x',2), Pose3(m2), posePrior); + graph.addPrior<>(Symbol('x',1), Pose3(m1), posePrior); + graph.addPrior<>(Symbol('x',2), Pose3(m2), posePrior); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 6af4de731..f9494d7ab 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -27,7 +27,6 @@ #include #include #include -#include #include @@ -456,7 +455,7 @@ TEST(Unit3, ErrorBetweenFactor) { // Add prior factors. SharedNoiseModel R_prior = noiseModel::Unit::Create(2); for (size_t i = 0; i < data.size(); i++) { - graph.emplace_shared >(U(i), data[i], R_prior); + graph.addPrior<>(U(i), data[i], R_prior); } // Add process factors using the dot product error function. diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 5f3e12399..be5295487 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -717,7 +717,6 @@ TEST(ImuFactor, bodyPSensorNoBias) { /* ************************************************************************* */ #include #include -#include #include #include @@ -763,20 +762,17 @@ TEST(ImuFactor, bodyPSensorWithBias) { NonlinearFactorGraph graph; Values values; - PriorFactor priorPose(X(0), Pose3(), priorNoisePose); - graph.add(priorPose); + graph.addPrior<>(X(0), Pose3(), priorNoisePose); values.insert(X(0), Pose3()); - PriorFactor priorVel(V(0), zeroVel, priorNoiseVel); - graph.add(priorVel); + graph.addPrior<>(V(0), zeroVel, priorNoiseVel); values.insert(V(0), zeroVel); // The key to this test is that we specify the bias, in the sensor frame, as known a priori // We also create factors below that encode our assumption that this bias is constant over time // In theory, after optimization, we should recover that same bias estimate Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) - PriorFactor priorBiasFactor(B(0), priorBias, priorNoiseBias); - graph.add(priorBiasFactor); + graph.addPrior<>(B(0), priorBias, priorNoiseBias); values.insert(B(0), priorBias); // Now add IMU factors and bias noise models diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h index 6f15c1a68..f10cc7e42 100644 --- a/gtsam/slam/KarcherMeanFactor-inl.h +++ b/gtsam/slam/KarcherMeanFactor-inl.h @@ -20,7 +20,6 @@ #include #include #include -#include using namespace std; @@ -33,7 +32,7 @@ T FindKarcherMeanImpl(const vector& rotations) { NonlinearFactorGraph graph; static const Key kKey(0); for (const auto& R : rotations) { - graph.emplace_shared >(kKey, R); + graph.addPrior(kKey, R); } Values initial; initial.insert(kKey, T()); diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 45bf2e4ed..d314bfd84 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -17,4 +17,4 @@ // Note: PriorFactor has been moved to gtsam/nonlinear. This file has been // left here for backwards compatibility. -#include \ No newline at end of file +#include diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 569289633..bc5d4f240 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -18,7 +18,6 @@ #include #include -#include #include #include #include @@ -90,7 +89,7 @@ TEST( AntiFactor, EquivalentBayesNet) SharedNoiseModel sigma(noiseModel::Unit::Create(6)); NonlinearFactorGraph graph; - graph.emplace_shared >(1, pose1, sigma); + graph.addPrior<>(1, pose1, sigma); graph.emplace_shared >(1, 2, pose1.between(pose2), sigma); // Create a configuration corresponding to the ground truth diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 13a2c8090..7b2f73780 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -18,7 +18,6 @@ #include #include -#include #include #include #include @@ -388,8 +387,7 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { graph.emplace_shared< RangeFactor >(X(0), X(1), 2., noiseModel::Isotropic::Sigma(1, 1.)); - graph.emplace_shared< - PriorFactor >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + graph.addPrior<>(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), noiseModel::Isotropic::Sigma(6, 1.)); Values init; @@ -413,14 +411,12 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { // Tests range factor between a CalibratedCamera and a Pose3 NonlinearFactorGraph graph; - graph.emplace_shared< - PriorFactor >(X(0), CalibratedCamera(), + graph.addPrior<>(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.)); graph.emplace_shared< RangeFactor >(X(0), X(1), 2., noiseModel::Isotropic::Sigma(1, 1.)); - graph.emplace_shared< - PriorFactor >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + graph.addPrior<>(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), noiseModel::Isotropic::Sigma(6, 1.)); Values init; diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 60e930661..8559b06ce 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include @@ -67,7 +66,7 @@ NonlinearFactorGraph graph() { g.add(BetweenFactor(x2, x3, pose2.between(pose3), model)); g.add(BetweenFactor(x2, x0, pose2.between(pose0), model)); g.add(BetweenFactor(x0, x3, pose0.between(pose3), model)); - g.add(PriorFactor(x0, pose0, model)); + g.addPrior<>(x0, pose0, model); return g; } @@ -78,7 +77,7 @@ NonlinearFactorGraph graph2() { g.add(BetweenFactor(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0))); g.add(BetweenFactor(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information g.add(BetweenFactor(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin - g.add(PriorFactor(x0, pose0, model)); + g.addPrior<>(x0, pose0, model); return g; } } @@ -267,7 +266,7 @@ TEST( InitializePose3, initializePoses ) bool is3D = true; boost::tie(inputGraph, expectedValues) = readG2o(g2oFile, is3D); noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); - inputGraph->add(PriorFactor(0, Pose3(), priorModel)); + inputGraph->addPrior<>(0, Pose3(), priorModel); Values initial = InitializePose3::initialize(*inputGraph); EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !! diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index 1dbf58352..02f8b6454 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -61,7 +60,7 @@ NonlinearFactorGraph graph() { g.add(BetweenFactor(x2, x3, pose2.between(pose3), model)); g.add(BetweenFactor(x2, x0, pose2.between(pose0), model)); g.add(BetweenFactor(x0, x3, pose0.between(pose3), model)); - g.add(PriorFactor(x0, pose0, model)); + g.addPrior<>(x0, pose0, model); return g; } } @@ -167,7 +166,7 @@ TEST( Lago, smallGraphVectorValuesSP ) { TEST( Lago, multiplePosePriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simpleLago::graph(); - g.add(PriorFactor(x1, simpleLago::pose1, model)); + g.addPrior<>(x1, simpleLago::pose1, model); VectorValues initial = lago::initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI @@ -180,7 +179,7 @@ TEST( Lago, multiplePosePriors ) { /* *************************************************************************** */ TEST( Lago, multiplePosePriorsSP ) { NonlinearFactorGraph g = simpleLago::graph(); - g.add(PriorFactor(x1, simpleLago::pose1, model)); + g.addPrior<>(x1, simpleLago::pose1, model); VectorValues initial = lago::initializeOrientations(g); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI @@ -194,7 +193,7 @@ TEST( Lago, multiplePosePriorsSP ) { TEST( Lago, multiplePoseAndRotPriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simpleLago::graph(); - g.add(PriorFactor(x1, simpleLago::pose1.theta(), model)); + g.addPrior<>(x1, simpleLago::pose1.theta(), model); VectorValues initial = lago::initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI @@ -207,7 +206,7 @@ TEST( Lago, multiplePoseAndRotPriors ) { /* *************************************************************************** */ TEST( Lago, multiplePoseAndRotPriorsSP ) { NonlinearFactorGraph g = simpleLago::graph(); - g.add(PriorFactor(x1, simpleLago::pose1.theta(), model)); + g.addPrior<>(x1, simpleLago::pose1.theta(), model); VectorValues initial = lago::initializeOrientations(g); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI @@ -267,7 +266,7 @@ TEST( Lago, largeGraphNoisy_orientations ) { // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = *g; noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(Vector3(1e-2, 1e-2, 1e-4)); - graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + graphWithPrior.addPrior<>(0, Pose2(), priorModel); VectorValues actualVV = lago::initializeOrientations(graphWithPrior); Values actual; @@ -302,7 +301,7 @@ TEST( Lago, largeGraphNoisy ) { // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = *g; noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(Vector3(1e-2, 1e-2, 1e-4)); - graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + graphWithPrior.addPrior<>(0, Pose2(), priorModel); Values actual = lago::initialize(graphWithPrior); diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 7b3158e9a..0368fbd94 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -195,8 +195,8 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimize ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); - graph.emplace_shared >(c1, cam1, noisePrior); - graph.emplace_shared >(c2, cam2, noisePrior); + graph.addPrior<>(c1, cam1, noisePrior); + graph.addPrior<>(c2, cam2, noisePrior); // Create initial estimate Values initial; @@ -304,8 +304,8 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(c1, cam1, noisePrior); - graph.emplace_shared >(c2, cam2, noisePrior); + graph.addPrior<>(c1, cam1, noisePrior); + graph.addPrior<>(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -378,8 +378,8 @@ TEST(SmartProjectionFactor, perturbCamerasAndOptimize ) { graph.push_back(smartFactor3); graph.push_back(smartFactor4); graph.push_back(smartFactor5); - graph.emplace_shared >(c1, cam1, noisePrior); - graph.emplace_shared >(c2, cam2, noisePrior); + graph.addPrior<>(c1, cam1, noisePrior); + graph.addPrior<>(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -453,8 +453,8 @@ TEST(SmartProjectionFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(c1, cam1, noisePrior); - graph.emplace_shared >(c2, cam2, noisePrior); + graph.addPrior<>(c1, cam1, noisePrior); + graph.addPrior<>(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -526,8 +526,8 @@ TEST(SmartProjectionFactor, Cal3Bundler2 ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(c1, cam1, noisePrior); - graph.emplace_shared >(c2, cam2, noisePrior); + graph.addPrior<>(c1, cam1, noisePrior); + graph.addPrior<>(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index f833941bc..53692e57f 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -243,8 +243,8 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, wTb1, noisePrior); - graph.emplace_shared >(x2, wTb2, noisePrior); + graph.addPrior<>(x1, wTb1, noisePrior); + graph.addPrior<>(x2, wTb2, noisePrior); // Check errors at ground truth poses Values gtValues; @@ -302,8 +302,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, cam2.pose(), noisePrior); + graph.addPrior<>(x1, cam1.pose(), noisePrior); + graph.addPrior<>(x2, cam2.pose(), noisePrior); Values groundTruth; groundTruth.insert(x1, cam1.pose()); @@ -525,8 +525,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, cam2.pose(), noisePrior); + graph.addPrior<>(x1, cam1.pose(), noisePrior); + graph.addPrior<>(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -589,8 +589,8 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, cam2.pose(), noisePrior); + graph.addPrior<>(x1, cam1.pose(), noisePrior); + graph.addPrior<>(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -647,8 +647,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, cam2.pose(), noisePrior); + graph.addPrior<>(x1, cam1.pose(), noisePrior); + graph.addPrior<>(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -716,8 +716,8 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, cam2.pose(), noisePrior); + graph.addPrior<>(x1, cam1.pose(), noisePrior); + graph.addPrior<>(x2, cam2.pose(), noisePrior); Values values; values.insert(x1, cam1.pose()); @@ -766,8 +766,8 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, cam2.pose(), noisePrior); + graph.addPrior<>(x1, cam1.pose(), noisePrior); + graph.addPrior<>(x2, cam2.pose(), noisePrior); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise @@ -806,8 +806,8 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { graph.emplace_shared(cam3.project(landmark3), model, x3, L(3), sharedK2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.emplace_shared >(x1, level_pose, noisePrior); - graph.emplace_shared >(x2, pose_right, noisePrior); + graph.addPrior<>(x1, level_pose, noisePrior); + graph.addPrior<>(x2, pose_right, noisePrior); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); @@ -948,7 +948,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.addPrior<>(x1, cam1.pose(), noisePrior); graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); @@ -1012,7 +1012,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.addPrior<>(x1, cam1.pose(), noisePrior); graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); @@ -1223,8 +1223,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, cam2.pose(), noisePrior); + graph.addPrior<>(x1, cam1.pose(), noisePrior); + graph.addPrior<>(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1296,7 +1296,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.addPrior<>(x1, cam1.pose(), noisePrior); graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index 70e08356e..9ff3c8fcd 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -26,7 +26,6 @@ #include #include #include -#include #include #include @@ -61,7 +60,7 @@ int main(int argc, char** argv){ initial_estimate.insert(Symbol('K', 0), *noisy_K); noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished()); - graph.emplace_shared >(Symbol('K', 0), *noisy_K, calNoise); + graph.addPrior<>(Symbol('K', 0), *noisy_K, calNoise); ifstream pose_file(pose_loc.c_str()); @@ -77,7 +76,7 @@ int main(int argc, char** argv){ } noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01); - graph.emplace_shared >(Symbol('x', pose_id), Pose3(m), poseNoise); + graph.addPrior<>(Symbol('x', pose_id), Pose3(m), poseNoise); // camera and landmark keys size_t x, l; diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp index 81cd2443a..b19ee499f 100644 --- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp +++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp @@ -34,7 +34,6 @@ // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. // Here we will use Between factors for the relative motion described by odometry measurements. // Also, we will initialize the robot at the origin using a Prior factor. -#include #include // When the factors are created, we will add them to a Factor Graph. As the factors we are using @@ -86,7 +85,7 @@ int main(int argc, char** argv) { Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); Key priorKey = 0; - newFactors.push_back(PriorFactor(priorKey, priorMean, priorNoise)); + newFactors.addPrior<>(priorKey, priorMean, priorNoise); newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds; diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index b6c0b5c2f..a673d2b64 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -30,7 +30,6 @@ // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. // Here we will use Between factors for the relative motion described by odometry measurements. // Also, we will initialize the robot at the origin using a Prior factor. -#include #include // When the factors are created, we will add them to a Factor Graph. As the factors we are using @@ -80,7 +79,7 @@ int main(int argc, char** argv) { Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); Key priorKey = 0; - newFactors.push_back(PriorFactor(priorKey, priorMean, priorNoise)); + newFactors.addPrior<>(priorKey, priorMean, priorNoise); newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds; diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index 97cc3a516..30dfd2498 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -37,7 +37,6 @@ // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics SLAM problems. -#include #include #include #include @@ -127,7 +126,7 @@ int main(int argc, char** argv) { Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, M_PI - 2.02108900000000); NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(0, pose0, priorNoise)); + newFactors.addPrior<>(0, pose0, priorNoise); ofstream os2("rangeResultLM.txt"); ofstream os3("rangeResultSR.txt"); diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp index 03c7bea1f..dde187723 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -37,7 +37,6 @@ // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics SLAM problems. -#include #include #include @@ -124,7 +123,7 @@ int main(int argc, char** argv) { // Add prior on first pose Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, -2.02108900000000); NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(0, pose0, priorNoise)); + newFactors.addPrior<>(0, pose0, priorNoise); // initialize points (Gaussian) Values initial; diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 8d44676af..095da8fd5 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -17,7 +17,6 @@ */ #include -#include #include #include #include @@ -263,11 +262,10 @@ TEST(Similarity3, Optimization) { Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); Symbol key('x', 1); - PriorFactor factor(key, prior, model); // Create graph NonlinearFactorGraph graph; - graph.push_back(factor); + graph.addPrior<>(key, prior, model); // Create initial estimate with identity transform Values initial; @@ -304,7 +302,6 @@ TEST(Similarity3, Optimization2) { (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished()); SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas( (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished()); - PriorFactor factor(X(1), prior, model); // Prior ! BetweenFactor b1(X(1), X(2), m1, betweenNoise); BetweenFactor b2(X(2), X(3), m2, betweenNoise); BetweenFactor b3(X(3), X(4), m3, betweenNoise); @@ -313,7 +310,7 @@ TEST(Similarity3, Optimization2) { // Create graph NonlinearFactorGraph graph; - graph.push_back(factor); + graph.addPrior<>(X(1), prior, model); // Prior ! graph.push_back(b1); graph.push_back(b2); graph.push_back(b3); diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index b2aafe535..7d8611d19 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -26,7 +26,6 @@ #include #include #include -#include #include using namespace std; @@ -84,7 +83,7 @@ TEST( BatchFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.push_back(PriorFactor(key0, Point2(0.0, 0.0), odometerNoise)); + newFactors.addPrior<>(key0, Point2(0.0, 0.0), odometerNoise); newValues.insert(key0, Point2(0.01, 0.01)); newTimestamps[key0] = 0.0; diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index 09fbc0578..540e61072 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -134,7 +134,7 @@ TEST( ConcurrentBatchFilter, getFactors ) // Add some factors to the filter NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior<>(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -184,7 +184,7 @@ TEST( ConcurrentBatchFilter, getLinearizationPoint ) // Add some factors to the filter NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior<>(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -246,7 +246,7 @@ TEST( ConcurrentBatchFilter, calculateEstimate ) // Add some factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior<>(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -330,7 +330,7 @@ TEST( ConcurrentBatchFilter, update_multiple ) // Add some factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior<>(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -380,7 +380,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -403,7 +403,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) NonlinearFactorGraph partialGraph; - partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.addPrior<>(1, poseInitial, noisePrior); partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); @@ -504,7 +504,7 @@ TEST( ConcurrentBatchFilter, synchronize_1 ) // Insert factors into the filter, but do not marginalize out any variables. // The synchronization should still be empty NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues; newValues.insert(1, Pose3().compose(poseError)); @@ -1090,7 +1090,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -1121,7 +1121,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior<>(1, poseInitial, noisePrior); // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); @@ -1145,7 +1145,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_2 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -1176,7 +1176,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior<>(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -1200,8 +1200,8 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_3 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -1233,7 +1233,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_3 ) NonlinearFactorGraph expectedGraph; // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior<>(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -1253,7 +1253,7 @@ TEST( ConcurrentBatchFilter, removeFactors_values ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.emplace_shared >(1, poseInitial, noisePrior); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); newFactors.emplace_shared >(2, 3, poseOdometry, noiseOdometery); newFactors.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -1286,7 +1286,7 @@ TEST( ConcurrentBatchFilter, removeFactors_values ) // note: factors are removed before the optimization NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior<>(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index 748a119a4..abf6b750c 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -104,7 +104,7 @@ TEST( ConcurrentBatchSmoother, getFactors ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior<>(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -154,7 +154,7 @@ TEST( ConcurrentBatchSmoother, getLinearizationPoint ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior<>(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -216,7 +216,7 @@ TEST( ConcurrentBatchSmoother, calculateEstimate ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior<>(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -302,7 +302,7 @@ TEST( ConcurrentBatchSmoother, update_multiple ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior<>(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -527,7 +527,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 ) filterSumarization.push_back(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues)); smootherFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); smootherFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - smootherFactors.push_back(PriorFactor(4, poseInitial, noisePrior)); + smootherFactors.addPrior<>(4, poseInitial, noisePrior); smootherValues.insert(3, filterSeparatorValues.at(2).compose(poseOdometry).compose(poseError)); smootherValues.insert(4, smootherValues.at(3).compose(poseOdometry).compose(poseError)); @@ -588,7 +588,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 ) // Add some factors to the smoother NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -617,7 +617,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = smoother.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior<>(1, poseInitial, noisePrior); // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); @@ -642,7 +642,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_2 ) // Add some factors to the smoother NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -670,7 +670,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = smoother.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior<>(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -694,8 +694,8 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_3 ) // Add some factors to the Smoother NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -724,7 +724,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_3 ) NonlinearFactorGraph expectedGraph; // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior<>(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -744,7 +744,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_values ) // Add some factors to the Smoother NonlinearFactorGraph newFactors; - newFactors.emplace_shared >(1, poseInitial, noisePrior); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); newFactors.emplace_shared >(2, 3, poseOdometry, noiseOdometery); newFactors.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -774,7 +774,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_values ) // note: factors are removed before the optimization NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior<>(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index ef40d9c61..759e180f2 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -153,7 +153,7 @@ TEST( ConcurrentIncrementalFilter, getFactors ) // Add some factors to the filter NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior<>(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -203,7 +203,7 @@ TEST( ConcurrentIncrementalFilter, getLinearizationPoint ) // Add some factors to the filter NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior<>(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -259,7 +259,7 @@ TEST( ConcurrentIncrementalFilter, calculateEstimate ) // Add some factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior<>(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -343,7 +343,7 @@ TEST( ConcurrentIncrementalFilter, update_multiple ) // Add some factors to the filter NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior<>(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -393,7 +393,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -423,7 +423,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.addPrior<>(1, poseInitial, noisePrior); partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); @@ -476,7 +476,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -507,7 +507,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.addPrior<>(1, poseInitial, noisePrior); partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); @@ -605,7 +605,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_1 ) // Insert factors into the filter, but do not marginalize out any variables. // The synchronization should still be empty NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues; newValues.insert(1, Pose3().compose(poseError)); @@ -1192,7 +1192,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -1224,7 +1224,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior<>(1, poseInitial, noisePrior); // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)) // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); @@ -1251,7 +1251,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -1282,7 +1282,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior<>(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -1310,8 +1310,8 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -1343,7 +1343,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 ) NonlinearFactorGraph expectedGraph; // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior<>(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); @@ -1367,7 +1367,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -1399,7 +1399,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values ) Values actualValues = filter.getLinearizationPoint(); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior<>(1, poseInitial, noisePrior); expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index adb7c8e39..f32a654b2 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -115,7 +115,7 @@ TEST( ConcurrentIncrementalSmootherDL, getFactors ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior<>(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -166,7 +166,7 @@ TEST( ConcurrentIncrementalSmootherDL, getLinearizationPoint ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior<>(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -223,7 +223,7 @@ TEST( ConcurrentIncrementalSmootherDL, calculateEstimate ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior<>(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -310,7 +310,7 @@ TEST( ConcurrentIncrementalSmootherDL, update_multiple ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior<>(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -545,7 +545,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 ) filterSumarization.push_back(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues)); smootherFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); smootherFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - smootherFactors.push_back(PriorFactor(4, poseInitial, noisePrior)); + smootherFactors.addPrior(4, poseInitial, noisePrior); smootherValues.insert(3, filterSeparatorValues.at(2).compose(poseOdometry).compose(poseError)); smootherValues.insert(4, smootherValues.at(3).compose(poseOdometry).compose(poseError)); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 3cba22c78..459da8547 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -114,7 +114,7 @@ TEST( ConcurrentIncrementalSmootherGN, getFactors ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior<>(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -165,7 +165,7 @@ TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors1.addPrior<>(1, poseInitial, noisePrior); newFactors1.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues1; newValues1.insert(1, Pose3()); @@ -222,7 +222,7 @@ TEST( ConcurrentIncrementalSmootherGN, calculateEstimate ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior<>(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -309,7 +309,7 @@ TEST( ConcurrentIncrementalSmootherGN, update_multiple ) // Add some factors to the smoother NonlinearFactorGraph newFactors2; - newFactors2.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors2.addPrior<>(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); @@ -546,7 +546,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 ) filterSumarization.push_back(LinearContainerFactor(BetweenFactor(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues)); smootherFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); smootherFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - smootherFactors.push_back(PriorFactor(4, poseInitial, noisePrior)); + smootherFactors.addPrior<>(4, poseInitial, noisePrior); smootherValues.insert(3, filterSeparatorValues.at(2).compose(poseOdometry).compose(poseError)); smootherValues.insert(4, smootherValues.at(3).compose(poseOdometry).compose(poseError)); @@ -609,7 +609,7 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) // Add some factors to the smoother NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); + newFactors.addPrior<>(1, poseInitial, noisePrior); newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); @@ -639,7 +639,7 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) actualGraph.print("actual graph: \n"); NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.addPrior<>(1, poseInitial, noisePrior); // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 60a805e1e..c995a89a5 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -18,7 +18,6 @@ #include -#include #include #include #include @@ -82,7 +81,7 @@ TEST( IncrementalFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.push_back(PriorFactor(key0, Point2(0.0, 0.0), odometerNoise)); + newFactors.addPrior<>(key0, Point2(0.0, 0.0), odometerNoise); newValues.insert(key0, Point2(0.01, 0.01)); newTimestamps[key0] = 0.0; diff --git a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp index 10e983fd5..7e05c3283 100644 --- a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp +++ b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp @@ -6,7 +6,6 @@ #include #include -#include #include #include #include @@ -27,7 +26,7 @@ NonlinearFactorGraph planarSLAMGraph() { // Prior on pose x1 at the origin. Pose2 prior(0.0, 0.0, 0.0); auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.emplace_shared >(x1, prior, priorNoise); + graph.addPrior<>(x1, prior, priorNoise); // Two odometry factors Pose2 odometry(2.0, 0.0, 0.0); diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index 9ae20eb11..efd358207 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -13,7 +13,6 @@ #include #include -#include using namespace std; @@ -280,8 +279,8 @@ TEST (BetweenFactorEM, updateNoiseModel ) { SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2)); NonlinearFactorGraph graph; - graph.push_back(gtsam::PriorFactor(key1, p1, model)); - graph.push_back(gtsam::PriorFactor(key2, p2, model)); + graph.addPrior<>(key1, p1, model); + graph.addPrior<>(key2, p2, model); f.updateNoiseModels(values, graph); diff --git a/gtsam_unstable/slam/tests/testSerialization.cpp b/gtsam_unstable/slam/tests/testSerialization.cpp index e1bcfe062..63911c5c7 100644 --- a/gtsam_unstable/slam/tests/testSerialization.cpp +++ b/gtsam_unstable/slam/tests/testSerialization.cpp @@ -15,7 +15,6 @@ #include #include -#include #include #include @@ -48,7 +47,7 @@ Values exampleValues() { NonlinearFactorGraph exampleGraph() { NonlinearFactorGraph graph; - graph.add(PriorFactor(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)))); + graph.addPrior<>(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3))); graph.add(BetweenFactor(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)))); graph.add(BearingRangeFactor(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(Vector::Ones(2)))); return graph; diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 202365dc6..f578bbcd3 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include using namespace std; @@ -116,8 +115,8 @@ TEST( SmartRangeFactor, optimization ) { graph.push_back(f); const noiseModel::Base::shared_ptr // priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI)); - graph.emplace_shared >(1, pose1, priorNoise); - graph.emplace_shared >(2, pose2, priorNoise); + graph.addPrior<>(1, pose1, priorNoise); + graph.addPrior<>(2, pose2, priorNoise); // Try optimizing LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp b/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp index 432bcb7d4..c3bc911fc 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp @@ -198,9 +198,7 @@ TEST(testISAM2SmartFactor, Stereo_Batch) { // prior, for the first keyframe: if (kf_id == 0) { - const auto prior = boost::make_shared>( - X(kf_id), Pose3::identity(), priorPoseNoise); - batch_graph.push_back(prior); + batch_graph.addPrior<>(X(kf_id), Pose3::identity(), priorPoseNoise); } batch_values.insert(X(kf_id), Pose3::identity()); @@ -309,9 +307,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) { // prior, for the first keyframe: if (kf_id == 0) { - const auto prior = boost::make_shared>( - X(kf_id), Pose3::identity(), priorPoseNoise); - newFactors.push_back(prior); + newFactors.addPrior<>(X(kf_id), Pose3::identity(), priorPoseNoise); } // 2) Run iSAM2: diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 14ac52c45..8d8665649 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -335,8 +335,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, pose1, noisePrior); - graph.emplace_shared >(x2, pose2, noisePrior); + graph.addPrior<>(x1, pose1, noisePrior); + graph.addPrior<>(x2, pose2, noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -396,8 +396,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { // add factors NonlinearFactorGraph graph2; - graph2.push_back(PriorFactor(x1, pose1, noisePrior)); - graph2.push_back(PriorFactor(x2, pose2, noisePrior)); + graph2.addPrior<>(x1, pose1, noisePrior); + graph2.addPrior<>(x2, pose2, noisePrior); typedef GenericStereoFactor ProjectionFactor; @@ -477,8 +477,8 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.addPrior<>(x1, pose1, noisePrior); + graph.addPrior<>(x2, pose2, noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -586,8 +586,8 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){ graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, bodyPose1, noisePrior)); - graph.push_back(PriorFactor(x2, bodyPose2, noisePrior)); + graph.addPrior<>(x1, bodyPose1, noisePrior); + graph.addPrior<>(x2, bodyPose2, noisePrior); // Check errors at ground truth poses Values gtValues; @@ -660,8 +660,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, pose1, noisePrior); - graph.emplace_shared >(x2, pose2, noisePrior); + graph.addPrior<>(x1, pose1, noisePrior); + graph.addPrior<>(x2, pose2, noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -732,8 +732,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.addPrior<>(x1, pose1, noisePrior); + graph.addPrior<>(x2, pose2, noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -801,8 +801,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.emplace_shared >(x1, pose1, noisePrior); - graph.emplace_shared >(x2, pose2, noisePrior); + graph.addPrior<>(x1, pose1, noisePrior); + graph.addPrior<>(x2, pose2, noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -883,8 +883,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.emplace_shared >(x1, pose1, noisePrior); - graph.emplace_shared >(x2, pose2, noisePrior); + graph.addPrior<>(x1, pose1, noisePrior); + graph.addPrior<>(x2, pose2, noisePrior); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 2d0ab2970..4882cb80f 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -401,7 +401,6 @@ TEST(GaussianFactorGraph, hasConstraints) #include #include -#include #include /* ************************************************************************* */ @@ -438,7 +437,7 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) { initValues.insert(l41, Point3(1.61051523, 6.7373052, -0.231582751) ); NonlinearFactorGraph factors; - factors += PriorFactor(xC1, + factors.addPrior<>(xC1, Pose3(Rot3( -1., 0.0, 1.2246468e-16, 0.0, 1., 0.0, diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index bec266bcc..b7352dc3c 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -7,7 +7,6 @@ #include #include -#include #include #include #include @@ -63,7 +62,7 @@ ISAM2 createSlamlikeISAM2( // Add a prior at time 0 and update isam { NonlinearFactorGraph newfactors; - newfactors += PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise); + newfactors.addPrior<>(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -499,7 +498,7 @@ TEST(ISAM2, constrained_ordering) // Add a prior at time 0 and update isam { NonlinearFactorGraph newfactors; - newfactors += PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise); + newfactors.addPrior<>(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -669,7 +668,7 @@ namespace { TEST(ISAM2, marginalizeLeaves1) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, 0.0, model); + factors.addPrior<>(0, 0.0, model); factors += BetweenFactor(0, 1, 0.0, model); factors += BetweenFactor(1, 2, 0.0, model); @@ -696,7 +695,7 @@ TEST(ISAM2, marginalizeLeaves2) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, 0.0, model); + factors.addPrior<>(0, 0.0, model); factors += BetweenFactor(0, 1, 0.0, model); factors += BetweenFactor(1, 2, 0.0, model); @@ -726,7 +725,7 @@ TEST(ISAM2, marginalizeLeaves3) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, 0.0, model); + factors.addPrior<>(0, 0.0, model); factors += BetweenFactor(0, 1, 0.0, model); factors += BetweenFactor(1, 2, 0.0, model); @@ -765,7 +764,7 @@ TEST(ISAM2, marginalizeLeaves4) { ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, 0.0, model); + factors.addPrior<>(0, 0.0, model); factors += BetweenFactor(0, 2, 0.0, model); factors += BetweenFactor(1, 2, 0.0, model); diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 14767c5db..1ff21e9c5 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -12,7 +12,6 @@ * @date Jun 11, 2012 */ -#include #include #include #include @@ -38,7 +37,7 @@ boost::tuple generateProblem() { Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas( Vector3(0.3, 0.3, 0.1)); - graph += PriorFactor(1, priorMean, priorNoise); + graph.addPrior<>(1, priorMean, priorNoise); // 2b. Add odometry factors SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas( diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index e08880191..883e1c8b3 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -16,7 +16,6 @@ **/ #include -#include #include #include #include @@ -117,7 +116,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) config.insert(X(2), Pose2(1.5,0.,0.)); NonlinearFactorGraph graph; - graph += PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); + graph.addPrior<>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); graph += BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); boost::shared_ptr fg = graph.linearize(config); diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index 1b2c8052c..6ab7e088f 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -29,7 +29,6 @@ #include // add in headers for specific factors -#include #include #include @@ -53,7 +52,7 @@ TEST(Marginals, planarSLAMmarginals) { // gaussian for prior SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - graph += PriorFactor(x1, priorMean, priorNoise); // add the factor to the graph + graph.addPrior<>(x1, priorMean, priorNoise); // add the factor to the graph /* add odometry */ // general noisemodel for odometry @@ -195,7 +194,7 @@ TEST(Marginals, planarSLAMmarginals) { /* ************************************************************************* */ TEST(Marginals, order) { NonlinearFactorGraph fg; - fg += PriorFactor(0, Pose2(), noiseModel::Unit::Create(3)); + fg.addPrior<>(0, Pose2(), noiseModel::Unit::Create(3)); fg += BetweenFactor(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3)); fg += BetweenFactor(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3)); fg += BetweenFactor(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3)); diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 978438402..ddbdc9cca 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -27,7 +27,6 @@ #include #include #include -#include #include #include @@ -217,8 +216,8 @@ TEST(testNonlinearFactorGraph, eliminate) { // Priors auto prior = noiseModel::Isotropic::Sigma(3, 1); - graph.add(PriorFactor(11, T11, prior)); - graph.add(PriorFactor(21, T21, prior)); + graph.addPrior<>(11, T11, prior); + graph.addPrior<>(21, T21, prior); // Odometry auto model = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.3)); @@ -273,7 +272,7 @@ TEST(testNonlinearFactorGraph, addPrior) { Eigen::DiagonalMatrix covariance_pose3; covariance_pose3.setIdentity(); Pose3 pose{Rot3(), Point3(0, 0, 0)}; - graph.addPrior(k, pose, covariance_pose3); + graph.addPrior<>(k, pose, covariance_pose3); // Assert the graph has 0 error with the correct values. values.insert(k, pose); diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 9291511a0..0d1f4ce13 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -8,7 +8,6 @@ #include #include #include -#include #include #include #include @@ -116,9 +115,9 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) { // Add a floating landmark constellation if (i == 7) { - new_factors += PriorFactor(lm1, landmark1, model2); - new_factors += PriorFactor(lm2, landmark2, model2); - new_factors += PriorFactor(lm3, landmark3, model2); + new_factors.addPrior<>(lm1, landmark1, model2); + new_factors.addPrior<>(lm2, landmark2, model2); + new_factors.addPrior<>(lm3, landmark3, model2); // Initialize to origin new_init.insert(lm1, Point2(0,0)); @@ -193,9 +192,9 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { // Add a floating landmark constellation if (i == 7) { - new_factors += PriorFactor(lm1, landmark1, model2); - new_factors += PriorFactor(lm2, landmark2, model2); - new_factors += PriorFactor(lm3, landmark3, model2); + new_factors.addPrior<>(lm1, landmark1, model2); + new_factors.addPrior<>(lm2, landmark2, model2); + new_factors.addPrior<>(lm3, landmark3, model2); // Initialize to origin new_init.insert(lm1, Point2(0,0)); @@ -296,8 +295,7 @@ TEST(testNonlinearISAM, loop_closures ) { if (id == 0) { noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.001, 0.001, 0.001)); - graph.emplace_shared >(Symbol('x', id), - Pose2(0, 0, 0), priorNoise); + graph.addPrior<>(Symbol('x', id), Pose2(0, 0, 0), priorNoise); } else { isam.update(graph, initialEstimate); diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 6758cf102..0264d5a4b 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -16,7 +16,6 @@ */ #include -#include #include #include #include @@ -182,7 +181,7 @@ TEST( NonlinearOptimizer, Factorization ) config.insert(X(2), Pose2(1.5,0.,0.)); NonlinearFactorGraph graph; - graph += PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); + graph.addPrior<>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); graph += BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); Ordering ordering; @@ -241,8 +240,7 @@ TEST(NonlinearOptimizer, NullFactor) { TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { NonlinearFactorGraph fg; - fg += PriorFactor(0, Pose2(0, 0, 0), - noiseModel::Isotropic::Sigma(3, 1)); + fg.addPrior<>(0, Pose2(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1)); fg += BetweenFactor(0, 1, Pose2(1, 0, M_PI / 2), noiseModel::Isotropic::Sigma(3, 1)); fg += BetweenFactor(1, 2, Pose2(1, 0, M_PI / 2), @@ -343,7 +341,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { NonlinearFactorGraph fg; - fg += PriorFactor(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1)); + fg.addPrior<>(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1)); fg += BetweenFactor(0, 1, Pose2(1,1.1,M_PI/4), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(2.0), noiseModel::Isotropic::Sigma(3,1))); @@ -374,7 +372,7 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { NonlinearFactorGraph fg; - fg += PriorFactor(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01)); + fg.addPrior<>(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01)); fg += BetweenFactor(0, 1, Point2(1,1.8), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), noiseModel::Isotropic::Sigma(2,1))); @@ -408,7 +406,7 @@ TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) { NonlinearFactorGraph fg; - fg += PriorFactor(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1)); + fg.addPrior<>(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1)); fg += BetweenFactor(0, 1, Pose2(0,9, M_PI/2), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), noiseModel::Isotropic::Sigma(3,1))); @@ -455,7 +453,7 @@ TEST(NonlinearOptimizer, RobustMeanCalculation) { vector pts{-10,-3,-1,1,3,10,1000}; for(auto pt : pts) { - fg += PriorFactor(0, pt, huber); + fg.addPrior<>(0, pt, huber); } init.insert(0, 100.0); @@ -485,9 +483,9 @@ TEST(NonlinearOptimizer, disconnected_graph) { init.insert(X(3), Pose2(0.,0.,0.)); NonlinearFactorGraph graph; - graph += PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph.addPrior<>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); graph += BetweenFactor(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1)); - graph += PriorFactor(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph.addPrior<>(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(graph, init).optimize())); } @@ -530,9 +528,9 @@ TEST(NonlinearOptimizer, subclass_solver) { init.insert(X(3), Pose2(0.,0.,0.)); NonlinearFactorGraph graph; - graph += PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph.addPrior<>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); graph += BetweenFactor(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1)); - graph += PriorFactor(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph.addPrior<>(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); ConjugateGradientParameters p; Values actual = IterativeLM(graph, init, p).optimize(); @@ -587,7 +585,7 @@ struct traits { TEST(NonlinearOptimizer, Traits) { NonlinearFactorGraph fg; - fg += PriorFactor(0, MyType(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1)); + fg.addPrior<>(0, MyType(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1)); Values init; init.insert(0, MyType(0,0,0)); diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp index 794559763..c0f2804cc 100644 --- a/tests/testRot3Optimization.cpp +++ b/tests/testRot3Optimization.cpp @@ -26,11 +26,9 @@ #include #include #include -#include using namespace gtsam; -typedef PriorFactor Prior; typedef BetweenFactor Between; typedef NonlinearFactorGraph Graph; @@ -41,7 +39,7 @@ TEST(Rot3, optimize) { Values truth; Values initial; Graph fg; - fg += Prior(Symbol('r',0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01)); + fg.addPrior<>(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))); @@ -59,4 +57,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/tests/testVisualISAM2.cpp b/tests/testVisualISAM2.cpp index 2caed695a..955cd9708 100644 --- a/tests/testVisualISAM2.cpp +++ b/tests/testVisualISAM2.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include #include @@ -78,13 +77,11 @@ TEST(testVisualISAM2, all) static auto kPosePrior = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)) .finished()); - graph.emplace_shared>(Symbol('x', 0), poses[0], - kPosePrior); + graph.addPrior<>(Symbol('x', 0), poses[0], kPosePrior); // Add a prior on landmark l0 static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1); - graph.emplace_shared>(Symbol('l', 0), points[0], - kPointPrior); + graph.addPrior<>(Symbol('l', 0), points[0], kPointPrior); // Add initial guesses to all observed landmarks // Intentionally initialize the variables off from the ground truth diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index 8928c9b3f..e7f751fce 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -15,7 +15,6 @@ */ #include -#include #include #include #include @@ -98,7 +97,7 @@ int main(int argc, char *argv[]) { // cout << "Initializing " << 0 << endl; newVariables.insert(0, Pose()); // Add prior - newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(3))); + newFactors.addPrior<>(0, Pose(), noiseModel::Unit::Create(3)); } while(nextMeasurement < measurements.size()) { diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp index 929a67876..1e4cb49e1 100644 --- a/timing/timeLago.cpp +++ b/timing/timeLago.cpp @@ -17,7 +17,6 @@ */ #include -#include #include #include #include @@ -51,7 +50,7 @@ int main(int argc, char *argv[]) { // Add prior on the pose having index (key) = 0 noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8)); - g->add(PriorFactor(0, Pose2(), priorModel)); + g->addPrior<>(0, Pose2(), priorModel); // LAGO for (size_t i = 0; i < trials; i++) { diff --git a/timing/timeiSAM2Chain.cpp b/timing/timeiSAM2Chain.cpp index e61764e22..0a381e92a 100644 --- a/timing/timeiSAM2Chain.cpp +++ b/timing/timeiSAM2Chain.cpp @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include @@ -61,7 +60,7 @@ int main(int argc, char *argv[]) { gttic_(Create_measurements); if(step == 0) { // Add prior - newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(3))); + newFactors.addPrior<>(0, Pose(), noiseModel::Unit::Create(3)); newVariables.insert(0, Pose()); } else { Vector eta = Vector::Random(3) * 0.1;