diff --git a/cmake/example_cmake_find_gtsam/main.cpp b/cmake/example_cmake_find_gtsam/main.cpp index 4d93e1b19..2904e00dd 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..2befa9dc2 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..192ec11a4 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..7dbd8323b 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..c0a102d11 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 f83a539af..cb3650421 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 e038f5117..f9ac31bed 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/InverseKinematicsExampleExpressions.cpp b/examples/InverseKinematicsExampleExpressions.cpp index 9e86886e7..1df83d9a1 100644 --- a/examples/InverseKinematicsExampleExpressions.cpp +++ b/examples/InverseKinematicsExampleExpressions.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index 76b5098f6..bd25fdfdf 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 6dc0d9a93..2107b3244 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 52638fdca..15b4cd2d1 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 f977a08af..57cb82c69 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/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp index 1f6de6cb1..c35b9bc45 100644 --- a/examples/Pose2SLAMExampleExpressions.cpp +++ b/examples/Pose2SLAMExampleExpressions.cpp @@ -21,7 +21,6 @@ #include // For an explanation of headers below, please see Pose2SLAMExample.cpp -#include #include #include #include diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index a38dfafa6..738297230 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 c3d901507..c4301d28d 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 99711da2d..0a33f51aa 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 d6164450b..6bdc93e6f 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 0f306b7f4..d7ab55aaa 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 9b459d7b8..37559740b 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 05a04b353..512415221 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_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp index cceaac4ee..abadce975 100644 --- a/examples/Pose3SLAMExample_changeKeys.cpp +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -19,7 +19,6 @@ #include #include -#include #include using namespace std; diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp index 25297806e..06bf27850 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 9726f467c..f93be8659 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 000150846..3b60cca39 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 7b1667e12..c8e31e1c5 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 16cd25dba..784893ed9 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..4fcc1b297 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..d29910da6 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 9d5ad0b33..187a150de 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 b3dd3d25e..d6b25c23f 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 1d26ea0aa..4c88b7b4e 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/SimpleRotation.cpp b/examples/SimpleRotation.cpp index f70a44eec..c444c85aa 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -32,8 +32,8 @@ // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. -// We will apply a simple prior on the rotation -#include +// We will apply a simple prior on the rotation. We do so via the `addPrior` convenience +// method in NonlinearFactorGraph. // When the factors are created, we will add them to a Factor Graph. As the factors we are using // are nonlinear factors, we will need a Nonlinear Factor Graph. @@ -78,7 +78,6 @@ int main() { prior.print("goal angle"); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree); Symbol key('x',1); - PriorFactor factor(key, prior, model); /** * Step 2: Create a graph container and add the factor to it @@ -90,7 +89,7 @@ int main() { * many more factors would be added. */ NonlinearFactorGraph graph; - graph.push_back(factor); + graph.addPrior(key, prior, model); graph.print("full graph"); /** diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 80ac08e03..991be348c 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 db9090de6..b05fd9e6f 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 b4086910b..b09ad0f17 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/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index 49b99a5b6..7693fa4e4 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 1dde7efb5..a2ad8cf0b 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -20,7 +20,7 @@ * @author Stephen Williams */ -#include +#include #include //#include #include diff --git a/gtsam.h b/gtsam.h index 413a1bc7c..7a1a8864d 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2494,7 +2494,7 @@ class NonlinearISAM { #include #include -#include +#include template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index d2f0c91df..4f71a48da 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 e08ad97bb..60f491a1c 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/geometry/triangulation.h b/gtsam/geometry/triangulation.h index ed61c75b5..6c0f9c8f4 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -22,7 +22,6 @@ #include #include #include -#include #include #include diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 460695ec6..474b00add 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/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 232693a9e..0e17700d0 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -204,6 +205,33 @@ namespace gtsam { push_back(boost::make_shared >(R, z, h)); } + /** + * Convenience method which adds a PriorFactor to the factor graph. + * @param key Variable key + * @param prior The variable's prior value + * @param model Noise model for prior factor + */ + template + void addPrior(Key key, const T& prior, + const SharedNoiseModel& model = nullptr) { + emplace_shared>(key, prior, model); + } + + /** + * Convenience method which adds a PriorFactor to the factor graph. + * @param key Variable key + * @param prior The variable's prior value + * @param covariance Covariance matrix. + * + * Note that the smart noise model associated with the prior factor + * automatically picks the right noise model (e.g. a diagonal noise model + * if the provided covariance matrix is diagonal). + */ + template + void addPrior(Key key, const T& prior, const Matrix& covariance) { + emplace_shared>(key, prior, covariance); + } + private: /** diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h new file mode 100644 index 000000000..8d8c67d5c --- /dev/null +++ b/gtsam/nonlinear/PriorFactor.h @@ -0,0 +1,120 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PriorFactor.h + * @author Frank Dellaert + **/ +#pragma once + +#include +#include + +#include + +namespace gtsam { + + /** + * A class for a soft prior on any Value type + * @addtogroup SLAM + */ + template + class PriorFactor: public NoiseModelFactor1 { + + public: + typedef VALUE T; + + private: + + typedef NoiseModelFactor1 Base; + + VALUE prior_; /** The measurement */ + + /** concept check by type */ + GTSAM_CONCEPT_TESTABLE_TYPE(T) + + public: + + /// shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr > shared_ptr; + + /// Typedef to this class + typedef PriorFactor This; + + /** default constructor - only use for serialization */ + PriorFactor() {} + + virtual ~PriorFactor() {} + + /** Constructor */ + PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) : + Base(model, key), prior_(prior) { + } + + /** Convenience constructor that takes a full covariance argument */ + PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) : + Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; + traits::Print(prior_, " prior mean: "); + if (this->noiseModel_) + this->noiseModel_->print(" noise model: "); + else + std::cout << "no noise model" << std::endl; + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This* e = dynamic_cast (&expected); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(prior_, e->prior_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const T& x, boost::optional H = boost::none) const { + if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); + // manifold equivalent of z-x -> Local(x,z) + // TODO(ASL) Add Jacobians. + return -traits::Local(x, prior_); + } + + const VALUE & prior() const { return prior_; } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(prior_); + } + + // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html + enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + }; + +} /// namespace gtsam diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 1e398cd99..3d4a4d40d 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h index 076b0ff0c..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 8d8c67d5c..335eb2b5a 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -15,106 +15,6 @@ **/ #pragma once -#include -#include - -#include - -namespace gtsam { - - /** - * A class for a soft prior on any Value type - * @addtogroup SLAM - */ - template - class PriorFactor: public NoiseModelFactor1 { - - public: - typedef VALUE T; - - private: - - typedef NoiseModelFactor1 Base; - - VALUE prior_; /** The measurement */ - - /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(T) - - public: - - /// shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr > shared_ptr; - - /// Typedef to this class - typedef PriorFactor This; - - /** default constructor - only use for serialization */ - PriorFactor() {} - - virtual ~PriorFactor() {} - - /** Constructor */ - PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) : - Base(model, key), prior_(prior) { - } - - /** Convenience constructor that takes a full covariance argument */ - PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) : - Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) { - } - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** implement functions needed for Testable */ - - /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { - std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; - traits::Print(prior_, " prior mean: "); - if (this->noiseModel_) - this->noiseModel_->print(" noise model: "); - else - std::cout << "no noise model" << std::endl; - } - - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This* e = dynamic_cast (&expected); - return e != nullptr && Base::equals(*e, tol) && traits::Equals(prior_, e->prior_, tol); - } - - /** implement functions needed to derive from Factor */ - - /** vector of errors */ - Vector evaluateError(const T& x, boost::optional H = boost::none) const { - if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); - // manifold equivalent of z-x -> Local(x,z) - // TODO(ASL) Add Jacobians. - return -traits::Local(x, prior_); - } - - const VALUE & prior() const { return prior_; } - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor1", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(prior_); - } - - // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) - }; - -} /// namespace gtsam +// Note: PriorFactor has been moved to gtsam/nonlinear/PriorFactor.h. This file +// has been left here for backwards compatibility. +#include diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index 34c8385e8..76edc8b9d 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index e34e13279..4188f5639 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 8e0b5ad8f..e19a41b8d 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 6fe8b3d7c..ba41cdc9b 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 f8157c116..55449d86e 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/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index f1dbb4239..81f67f1ee 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -17,7 +17,6 @@ */ #include -#include #include #include #include @@ -49,10 +48,9 @@ TEST (OrientedPlane3Factor, lm_translation_error) { Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); Vector sigmas(6); sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; - PriorFactor pose_prior(init_sym, init_pose, - noiseModel::Diagonal::Sigmas(sigmas)); + new_graph.addPrior( + init_sym, init_pose, noiseModel::Diagonal::Sigmas(sigmas)); new_values.insert(init_sym, init_pose); - new_graph.add(pose_prior); // Add two landmark measurements, differing in range new_values.insert(lm_sym, test_lm0); @@ -94,11 +92,10 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose Symbol init_sym('x', 0); Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); - PriorFactor pose_prior(init_sym, init_pose, + new_graph.addPrior(init_sym, init_pose, noiseModel::Diagonal::Sigmas( (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); new_values.insert(init_sym, init_pose); - new_graph.add(pose_prior); // // Add two landmark measurements, differing in angle new_values.insert(lm_sym, test_lm0); diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp index b26d633f5..2dc083cb2 100644 --- a/gtsam/slam/tests/testPriorFactor.cpp +++ b/gtsam/slam/tests/testPriorFactor.cpp @@ -6,7 +6,7 @@ */ #include -#include +#include #include using namespace std; diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 7b3158e9a..1fd06cc9f 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..ab32258ee 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/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 494f2731f..c7e46e400 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -8,7 +8,7 @@ #include #include -#include +#include #include #include #include diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index 41e75432f..95629fb43 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 429a2c2b2..939d3a5c8 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 dc9b00580..16ede58e0 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 9dbeeac89..5fdc7a743 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 a63a0ba20..90b2a30ff 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 aaeb0854d..b07b5acd6 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/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index b46c5354b..ef2d16bf0 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -274,7 +274,7 @@ class SimPolygon2D { }; // Nonlinear factors from gtsam, for our Value types -#include +#include template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index a4811abd8..a708c57cc 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 ff5a096b0..1f19c0e8a 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -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 47002acb6..ae9a3f827 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -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 342e2e79f..c5dba1a69 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -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 98b4bf8cc..5de115013 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -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 d4ebc7c0a..ec78ee6e2 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -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 8d99ed482..3454c352a 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 2240034af..e0c234b7b 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/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 5b4bd8929..8a661f2ef 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -15,7 +15,7 @@ #include //#include #include -#include +#include #include #include #include diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index 5bb4dfd2e..70368cc0e 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/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp index ca91d4cb5..141a50465 100644 --- a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp @@ -16,7 +16,6 @@ * @date Nov 2009 */ -#include #include #include #include diff --git a/gtsam_unstable/slam/tests/testSerialization.cpp b/gtsam_unstable/slam/tests/testSerialization.cpp index dc05711e3..792fd1133 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 8a6aab6b7..f900b2ffa 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..107defb4c 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..a0bfc3649 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/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index d9a013a60..d33c7ba1d 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 212a8e107..d759e83e1 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -14,7 +14,7 @@ * @author Stephen Williams */ -#include +#include #include #include #include diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index bf968c8d7..1f5ec6350 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 68d10bb7b..e8e916f04 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/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index e877e5a9d..dfdb32b46 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index e45f234aa..a44a28273 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 7afb4e178..9d6e58ac7 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 3c35c6bc0..d7746e08d 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/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index d59666655..9d91a344b 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -16,7 +16,7 @@ #include -#include +#include #include #include #include diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index e403edaab..fdb080a63 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -25,8 +25,8 @@ #include #include #include +#include #include -#include #include #include @@ -216,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)); @@ -243,6 +243,48 @@ TEST(testNonlinearFactorGraph, eliminate) { } /* ************************************************************************* */ +TEST(testNonlinearFactorGraph, addPrior) { + Key k(0); + + // Factor graph. + auto graph = NonlinearFactorGraph(); + + // Add a prior factor for key k. + auto model_double = noiseModel::Isotropic::Sigma(1, 1); + graph.addPrior(k, 10, model_double); + + // Assert the graph has 0 error with the correct values. + Values values; + values.insert(k, 10.0); + EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16); + + // Assert the graph has some error with incorrect values. + values.clear(); + values.insert(k, 11.0); + EXPECT(0 != graph.error(values)); + + // Clear the factor graph and values. + values.clear(); + graph.erase(graph.begin(), graph.end()); + + // Add a Pose3 prior to the factor graph. Use a gaussian noise model by + // providing the covariance matrix. + Eigen::DiagonalMatrix covariance_pose3; + covariance_pose3.setIdentity(); + Pose3 pose{Rot3(), Point3(0, 0, 0)}; + graph.addPrior(k, pose, covariance_pose3); + + // Assert the graph has 0 error with the correct values. + values.insert(k, pose); + EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16); + + // Assert the graph has some error with incorrect values. + values.clear(); + Pose3 pose_incorrect{Rot3::RzRyRx(-M_PI, M_PI, -M_PI / 8), Point3(1, 2, 3)}; + values.insert(k, pose_incorrect); + EXPECT(0 != graph.error(values)); +} + TEST(NonlinearFactorGraph, printErrors) { const NonlinearFactorGraph fg = createNonlinearFactorGraph(); diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index c06d10beb..88ae508b6 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 7b5e7a0e0..dd708b37a 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 31cf68ebd..5746022a3 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/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 9222894a4..e3252a90b 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -27,7 +27,7 @@ //#include #include //#include -#include +#include #include #include #include diff --git a/tests/testVisualISAM2.cpp b/tests/testVisualISAM2.cpp index 182408004..9fedc853b 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 ec5fc3fa5..6e0f4ccdf 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 d0b6b263c..7aaf37e92 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 7a4413ac7..a056bde24 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;