diff --git a/cmake/example_cmake_find_gtsam/main.cpp b/cmake/example_cmake_find_gtsam/main.cpp index 12026178d..2904e00dd 100644 --- a/cmake/example_cmake_find_gtsam/main.cpp +++ b/cmake/example_cmake_find_gtsam/main.cpp @@ -68,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.addPrior<>(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 0e7d38b76..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.addPrior<>(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 be7c5d732..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.addPrior<>(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 d0a7b7278..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.addPrior<>(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.addPrior<>(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 f7615635c..c0a102d11 100644 --- a/examples/ISAM2_SmartFactorStereo_IMU.cpp +++ b/examples/ISAM2_SmartFactorStereo_IMU.cpp @@ -129,16 +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.addPrior<>(X(1), Pose3::identity(), priorPoseNoise); + graph.addPrior(X(1), Pose3::identity(), priorPoseNoise); initialEstimate.insert(X(0), Pose3::identity()); // Bias prior - graph.addPrior<>(B(1), imu.priorImuBias, + graph.addPrior(B(1), imu.priorImuBias, imu.biasNoiseModel); initialEstimate.insert(B(0), imu.priorImuBias); // Velocity prior - assume stationary - graph.addPrior<>(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 7e444e29e..cb3650421 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -60,18 +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.addPrior<>(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)); - newgraph.addPrior<>(biasKey, imuBias::ConstantBias(), biasnoise); + 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; - newgraph.addPrior<>(V(0), n_velocity, velnoise); + newgraph.addPrior(V(0), n_velocity, velnoise); initialEstimate.insert(V(0), n_velocity); diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index 25099fc39..f9ac31bed 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -128,9 +128,9 @@ int main(int argc, char* argv[]) // Add all prior factors (pose, velocity, bias) to the graph. NonlinearFactorGraph *graph = new NonlinearFactorGraph(); - 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); + graph->addPrior(X(correction_count), prior_pose, pose_noise_model); + graph->addPrior(V(correction_count), prior_velocity,velocity_noise_model); + graph->addPrior(B(correction_count), prior_imu_bias,bias_noise_model); // We use the sensor specs to build the noise model for the IMU factor. double accel_noise_sigma = 0.0003924; diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index fc39d7aee..bd25fdfdf 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -36,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.addPrior<>(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 cce27231d..2107b3244 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -64,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.addPrior<>(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 e329c0da0..15b4cd2d1 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -80,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.addPrior<>(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 f22b50a13..57cb82c69 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -71,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.addPrior<>(1, Pose2(0, 0, 0), priorNoise); + graph.addPrior(1, Pose2(0, 0, 0), priorNoise); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 68bd4cbbf..b36a22f25 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -64,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.addPrior<>(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 97a16e72d..c4301d28d 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -42,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 -> addPrior<>(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 c3301812a..0a33f51aa 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -32,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.addPrior<>(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 a379ddd6b..6bdc93e6f 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -43,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->addPrior<>(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 5c667a1e9..d7ab55aaa 100644 --- a/examples/Pose2SLAMStressTest.cpp +++ b/examples/Pose2SLAMStressTest.cpp @@ -47,7 +47,7 @@ void testGtsam(int numberNodes) { Pose3 first = Pose3(first_M); NonlinearFactorGraph graph; - graph.addPrior<>(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 0be18e827..37559740b 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -35,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.addPrior<>(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 7b89ed86a..512415221 100644 --- a/examples/Pose3Localization.cpp +++ b/examples/Pose3Localization.cpp @@ -47,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->addPrior<>(firstKey, Pose3(), priorModel); + graph->addPrior(firstKey, Pose3(), priorModel); break; } diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp index 8e37d6950..06bf27850 100644 --- a/examples/Pose3SLAMExample_g2o.cpp +++ b/examples/Pose3SLAMExample_g2o.cpp @@ -46,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->addPrior<>(firstKey, Pose3(), priorModel); + graph->addPrior(firstKey, Pose3(), priorModel); break; } diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp index 4b34db252..f93be8659 100644 --- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp +++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp @@ -46,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->addPrior<>(firstKey, Pose3(), priorModel); + graph->addPrior(firstKey, Pose3(), priorModel); break; } diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index f059e4ae2..3b60cca39 100644 --- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -46,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->addPrior<>(firstKey, Pose3(), priorModel); + graph->addPrior(firstKey, Pose3(), priorModel); break; } diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 50ade34e3..c8e31e1c5 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -123,7 +123,7 @@ int main (int argc, char** argv) { Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, M_PI - 2.02108900000000); NonlinearFactorGraph newFactors; - newFactors.addPrior<>(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 8912dfd9c..784893ed9 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -74,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.addPrior<>(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) { @@ -89,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.addPrior<>(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 23f0dc26e..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.addPrior<>(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.addPrior<>(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 079877a3a..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.addPrior<>(0, poses[0], noise); + graph.addPrior(0, poses[0], noise); // Fix the scale ambiguity by adding a prior - graph.addPrior<>(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 97f830a05..187a150de 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -65,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.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)); + 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 7c7a9a9af..d6b25c23f 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -70,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.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)); + 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 a2c709f9c..4c88b7b4e 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -53,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.addPrior<>(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); @@ -70,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.addPrior<>(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.addPrior<>(Symbol('K', 0), K, calNoise); + graph.addPrior(Symbol('K', 0), K, calNoise); // Create the initial estimate to the solution // now including an estimate on the camera calibration parameters diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 37399b963..991be348c 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -280,7 +280,7 @@ void runIncremental() NonlinearFactorGraph newFactors; Values newVariables; - newFactors.addPrior<>(firstPose, Pose(), noiseModel::Unit::Create(3)); + newFactors.addPrior(firstPose, Pose(), noiseModel::Unit::Create(3)); newVariables.insert(firstPose, Pose()); isam2.update(newFactors, newVariables); @@ -463,7 +463,7 @@ void runBatch() cout << "Creating batch optimizer..." << endl; NonlinearFactorGraph measurements = datasetMeasurements; - measurements.addPrior<>(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 0c4bf04d0..b05fd9e6f 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -109,11 +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.addPrior<>(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.addPrior<>(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 5169c5bd4..b09ad0f17 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -107,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.addPrior<>(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.addPrior<>(Symbol('l', 0), points[0], pointNoise); + graph.addPrior(Symbol('l', 0), points[0], pointNoise); // Add initial guesses to all observed landmarks Point3 noise(-0.25, 0.20, 0.15); diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 8f47b0d37..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.addPrior<>(Symbol('x',1), Pose3(m1), posePrior); - graph.addPrior<>(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 f9494d7ab..60f491a1c 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -455,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.addPrior<>(U(i), data[i], R_prior); + graph.addPrior(U(i), data[i], R_prior); } // Add process factors using the dot product error function. diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index be5295487..474b00add 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -762,17 +762,17 @@ TEST(ImuFactor, bodyPSensorWithBias) { NonlinearFactorGraph graph; Values values; - graph.addPrior<>(X(0), Pose3(), priorNoisePose); + graph.addPrior(X(0), Pose3(), priorNoisePose); values.insert(X(0), Pose3()); - graph.addPrior<>(V(0), zeroVel, priorNoiseVel); + 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) - graph.addPrior<>(B(0), priorBias, priorNoiseBias); + graph.addPrior(B(0), priorBias, priorNoiseBias); values.insert(B(0), priorBias); // Now add IMU factors and bias noise models diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index bc5d4f240..4188f5639 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -89,7 +89,7 @@ TEST( AntiFactor, EquivalentBayesNet) SharedNoiseModel sigma(noiseModel::Unit::Create(6)); NonlinearFactorGraph graph; - graph.addPrior<>(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 7b2f73780..e19a41b8d 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -387,7 +387,7 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { graph.emplace_shared< RangeFactor >(X(0), X(1), 2., noiseModel::Isotropic::Sigma(1, 1.)); - graph.addPrior<>(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; @@ -411,12 +411,12 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { // Tests range factor between a CalibratedCamera and a Pose3 NonlinearFactorGraph graph; - graph.addPrior<>(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.addPrior<>(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 8559b06ce..ba41cdc9b 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -66,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.addPrior<>(x0, pose0, model); + g.addPrior(x0, pose0, model); return g; } @@ -77,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.addPrior<>(x0, pose0, model); + g.addPrior(x0, pose0, model); return g; } } @@ -266,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->addPrior<>(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 02f8b6454..55449d86e 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -60,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.addPrior<>(x0, pose0, model); + g.addPrior(x0, pose0, model); return g; } } @@ -166,7 +166,7 @@ TEST( Lago, smallGraphVectorValuesSP ) { TEST( Lago, multiplePosePriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simpleLago::graph(); - g.addPrior<>(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 @@ -179,7 +179,7 @@ TEST( Lago, multiplePosePriors ) { /* *************************************************************************** */ TEST( Lago, multiplePosePriorsSP ) { NonlinearFactorGraph g = simpleLago::graph(); - g.addPrior<>(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 @@ -193,7 +193,7 @@ TEST( Lago, multiplePosePriorsSP ) { TEST( Lago, multiplePoseAndRotPriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simpleLago::graph(); - g.addPrior<>(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 @@ -206,7 +206,7 @@ TEST( Lago, multiplePoseAndRotPriors ) { /* *************************************************************************** */ TEST( Lago, multiplePoseAndRotPriorsSP ) { NonlinearFactorGraph g = simpleLago::graph(); - g.addPrior<>(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 @@ -266,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.addPrior<>(0, Pose2(), priorModel); + graphWithPrior.addPrior(0, Pose2(), priorModel); VectorValues actualVV = lago::initializeOrientations(graphWithPrior); Values actual; @@ -301,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.addPrior<>(0, Pose2(), priorModel); + graphWithPrior.addPrior(0, Pose2(), priorModel); Values actual = lago::initialize(graphWithPrior); diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 0368fbd94..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.addPrior<>(c1, cam1, noisePrior); - graph.addPrior<>(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.addPrior<>(c1, cam1, noisePrior); - graph.addPrior<>(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.addPrior<>(c1, cam1, noisePrior); - graph.addPrior<>(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.addPrior<>(c1, cam1, noisePrior); - graph.addPrior<>(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.addPrior<>(c1, cam1, noisePrior); - graph.addPrior<>(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 53692e57f..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.addPrior<>(x1, wTb1, noisePrior); - graph.addPrior<>(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.addPrior<>(x1, cam1.pose(), noisePrior); - graph.addPrior<>(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.addPrior<>(x1, cam1.pose(), noisePrior); - graph.addPrior<>(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.addPrior<>(x1, cam1.pose(), noisePrior); - graph.addPrior<>(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.addPrior<>(x1, cam1.pose(), noisePrior); - graph.addPrior<>(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.addPrior<>(x1, cam1.pose(), noisePrior); - graph.addPrior<>(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.addPrior<>(x1, cam1.pose(), noisePrior); - graph.addPrior<>(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.addPrior<>(x1, level_pose, noisePrior); - graph.addPrior<>(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.addPrior<>(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.addPrior<>(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.addPrior<>(x1, cam1.pose(), noisePrior); - graph.addPrior<>(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.addPrior<>(x1, cam1.pose(), noisePrior); + graph.addPrior(x1, cam1.pose(), noisePrior); graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index 9ff3c8fcd..95629fb43 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -60,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.addPrior<>(Symbol('K', 0), *noisy_K, calNoise); + graph.addPrior(Symbol('K', 0), *noisy_K, calNoise); ifstream pose_file(pose_loc.c_str()); @@ -76,7 +76,7 @@ int main(int argc, char** argv){ } noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01); - graph.addPrior<>(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 b19ee499f..939d3a5c8 100644 --- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp +++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp @@ -85,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.addPrior<>(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 a673d2b64..16ede58e0 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -79,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.addPrior<>(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 30dfd2498..5fdc7a743 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -126,7 +126,7 @@ int main(int argc, char** argv) { Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, M_PI - 2.02108900000000); NonlinearFactorGraph newFactors; - newFactors.addPrior<>(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 dde187723..90b2a30ff 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -123,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.addPrior<>(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 095da8fd5..b07b5acd6 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -265,7 +265,7 @@ TEST(Similarity3, Optimization) { // Create graph NonlinearFactorGraph graph; - graph.addPrior<>(key, prior, model); + graph.addPrior(key, prior, model); // Create initial estimate with identity transform Values initial; @@ -310,7 +310,7 @@ TEST(Similarity3, Optimization2) { // Create graph NonlinearFactorGraph graph; - graph.addPrior<>(X(1), prior, model); // Prior ! + graph.addPrior(X(1), prior, model); // Prior ! graph.push_back(b1); graph.push_back(b2); graph.push_back(b3); diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index 7d8611d19..a708c57cc 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -83,7 +83,7 @@ TEST( BatchFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.addPrior<>(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 540e61072..1f19c0e8a 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -134,7 +134,7 @@ TEST( ConcurrentBatchFilter, getFactors ) // Add some factors to the filter NonlinearFactorGraph newFactors1; - newFactors1.addPrior<>(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.addPrior<>(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.addPrior<>(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.addPrior<>(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.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)); @@ -403,7 +403,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) NonlinearFactorGraph partialGraph; - partialGraph.addPrior<>(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.addPrior<>(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.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)); @@ -1121,7 +1121,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.addPrior<>(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.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)); @@ -1176,7 +1176,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.addPrior<>(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.addPrior<>(1, poseInitial, noisePrior); - newFactors.addPrior<>(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.addPrior<>(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.addPrior<>(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.addPrior<>(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 abf6b750c..ae9a3f827 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -104,7 +104,7 @@ TEST( ConcurrentBatchSmoother, getFactors ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.addPrior<>(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.addPrior<>(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.addPrior<>(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.addPrior<>(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.addPrior<>(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.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)); @@ -617,7 +617,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = smoother.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.addPrior<>(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.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)); @@ -670,7 +670,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = smoother.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.addPrior<>(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.addPrior<>(1, poseInitial, noisePrior); - newFactors.addPrior<>(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.addPrior<>(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.addPrior<>(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.addPrior<>(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 759e180f2..c5dba1a69 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -153,7 +153,7 @@ TEST( ConcurrentIncrementalFilter, getFactors ) // Add some factors to the filter NonlinearFactorGraph newFactors1; - newFactors1.addPrior<>(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.addPrior<>(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.addPrior<>(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.addPrior<>(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.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)); @@ -423,7 +423,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.addPrior<>(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.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)); @@ -507,7 +507,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.addPrior<>(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.addPrior<>(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.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)); @@ -1224,7 +1224,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.addPrior<>(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.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)); @@ -1282,7 +1282,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.addPrior<>(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.addPrior<>(1, poseInitial, noisePrior); - newFactors.addPrior<>(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.addPrior<>(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.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)); @@ -1399,7 +1399,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values ) Values actualValues = filter.getLinearizationPoint(); NonlinearFactorGraph expectedGraph; - expectedGraph.addPrior<>(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 f32a654b2..5de115013 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -115,7 +115,7 @@ TEST( ConcurrentIncrementalSmootherDL, getFactors ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.addPrior<>(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.addPrior<>(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.addPrior<>(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.addPrior<>(1, poseInitial, noisePrior); + newFactors2.addPrior(1, poseInitial, noisePrior); newFactors2.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); Values newValues2; newValues2.insert(1, Pose3().compose(poseError)); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 459da8547..ec78ee6e2 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -114,7 +114,7 @@ TEST( ConcurrentIncrementalSmootherGN, getFactors ) // Add some factors to the smoother NonlinearFactorGraph newFactors1; - newFactors1.addPrior<>(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.addPrior<>(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.addPrior<>(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.addPrior<>(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.addPrior<>(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.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)); @@ -639,7 +639,7 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) actualGraph.print("actual graph: \n"); NonlinearFactorGraph expectedGraph; - expectedGraph.addPrior<>(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 c995a89a5..3454c352a 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -81,7 +81,7 @@ TEST( IncrementalFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.addPrior<>(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 7e05c3283..e0c234b7b 100644 --- a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp +++ b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp @@ -26,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.addPrior<>(x1, prior, priorNoise); + graph.addPrior(x1, prior, priorNoise); // Two odometry factors Pose2 odometry(2.0, 0.0, 0.0); diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index efd358207..70368cc0e 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -279,8 +279,8 @@ TEST (BetweenFactorEM, updateNoiseModel ) { SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2)); NonlinearFactorGraph graph; - graph.addPrior<>(key1, p1, model); - graph.addPrior<>(key2, p2, model); + graph.addPrior(key1, p1, model); + graph.addPrior(key2, p2, model); f.updateNoiseModels(values, graph); diff --git a/gtsam_unstable/slam/tests/testSerialization.cpp b/gtsam_unstable/slam/tests/testSerialization.cpp index 63911c5c7..792fd1133 100644 --- a/gtsam_unstable/slam/tests/testSerialization.cpp +++ b/gtsam_unstable/slam/tests/testSerialization.cpp @@ -47,7 +47,7 @@ Values exampleValues() { NonlinearFactorGraph exampleGraph() { NonlinearFactorGraph graph; - graph.addPrior<>(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 f578bbcd3..f900b2ffa 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -115,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.addPrior<>(1, pose1, priorNoise); - graph.addPrior<>(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 c3bc911fc..107defb4c 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp @@ -198,7 +198,7 @@ TEST(testISAM2SmartFactor, Stereo_Batch) { // prior, for the first keyframe: if (kf_id == 0) { - batch_graph.addPrior<>(X(kf_id), Pose3::identity(), priorPoseNoise); + batch_graph.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise); } batch_values.insert(X(kf_id), Pose3::identity()); @@ -307,7 +307,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) { // prior, for the first keyframe: if (kf_id == 0) { - newFactors.addPrior<>(X(kf_id), Pose3::identity(), priorPoseNoise); + 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 8d8665649..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.addPrior<>(x1, pose1, noisePrior); - graph.addPrior<>(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.addPrior<>(x1, pose1, noisePrior); - graph2.addPrior<>(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.addPrior<>(x1, pose1, noisePrior); - graph.addPrior<>(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.addPrior<>(x1, bodyPose1, noisePrior); - graph.addPrior<>(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.addPrior<>(x1, pose1, noisePrior); - graph.addPrior<>(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.addPrior<>(x1, pose1, noisePrior); - graph.addPrior<>(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.addPrior<>(x1, pose1, noisePrior); - graph.addPrior<>(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.addPrior<>(x1, pose1, noisePrior); - graph.addPrior<>(x2, pose2, noisePrior); + graph.addPrior(x1, pose1, noisePrior); + graph.addPrior(x2, pose2, noisePrior); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 4882cb80f..1f5ec6350 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -437,7 +437,7 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) { initValues.insert(l41, Point3(1.61051523, 6.7373052, -0.231582751) ); NonlinearFactorGraph factors; - factors.addPrior<>(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 b7352dc3c..e8e916f04 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -62,7 +62,7 @@ ISAM2 createSlamlikeISAM2( // Add a prior at time 0 and update isam { NonlinearFactorGraph newfactors; - newfactors.addPrior<>(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; @@ -498,7 +498,7 @@ TEST(ISAM2, constrained_ordering) // Add a prior at time 0 and update isam { NonlinearFactorGraph newfactors; - newfactors.addPrior<>(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; @@ -668,7 +668,7 @@ namespace { TEST(ISAM2, marginalizeLeaves1) { ISAM2 isam; NonlinearFactorGraph factors; - factors.addPrior<>(0, 0.0, model); + factors.addPrior(0, 0.0, model); factors += BetweenFactor(0, 1, 0.0, model); factors += BetweenFactor(1, 2, 0.0, model); @@ -695,7 +695,7 @@ TEST(ISAM2, marginalizeLeaves2) { ISAM2 isam; NonlinearFactorGraph factors; - factors.addPrior<>(0, 0.0, model); + factors.addPrior(0, 0.0, model); factors += BetweenFactor(0, 1, 0.0, model); factors += BetweenFactor(1, 2, 0.0, model); @@ -725,7 +725,7 @@ TEST(ISAM2, marginalizeLeaves3) { ISAM2 isam; NonlinearFactorGraph factors; - factors.addPrior<>(0, 0.0, model); + factors.addPrior(0, 0.0, model); factors += BetweenFactor(0, 1, 0.0, model); factors += BetweenFactor(1, 2, 0.0, model); @@ -764,7 +764,7 @@ TEST(ISAM2, marginalizeLeaves4) { ISAM2 isam; NonlinearFactorGraph factors; - factors.addPrior<>(0, 0.0, model); + factors.addPrior(0, 0.0, model); factors += BetweenFactor(0, 2, 0.0, model); factors += BetweenFactor(1, 2, 0.0, model); diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 1ff21e9c5..a44a28273 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -37,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.addPrior<>(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 883e1c8b3..9d6e58ac7 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -116,7 +116,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) config.insert(X(2), Pose2(1.5,0.,0.)); NonlinearFactorGraph graph; - graph.addPrior<>(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 6ab7e088f..d7746e08d 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -52,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.addPrior<>(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 @@ -194,7 +194,7 @@ TEST(Marginals, planarSLAMmarginals) { /* ************************************************************************* */ TEST(Marginals, order) { NonlinearFactorGraph fg; - fg.addPrior<>(0, Pose2(), noiseModel::Unit::Create(3)); + fg.addPrior(0, Pose2(), noiseModel::Unit::Create(3)); fg += BetweenFactor(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3)); fg += BetweenFactor(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3)); fg += BetweenFactor(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3)); diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index ddbdc9cca..a4ddf50f2 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -216,8 +216,8 @@ TEST(testNonlinearFactorGraph, eliminate) { // Priors auto prior = noiseModel::Isotropic::Sigma(3, 1); - graph.addPrior<>(11, T11, prior); - graph.addPrior<>(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)); @@ -272,7 +272,7 @@ TEST(testNonlinearFactorGraph, addPrior) { Eigen::DiagonalMatrix covariance_pose3; covariance_pose3.setIdentity(); Pose3 pose{Rot3(), Point3(0, 0, 0)}; - graph.addPrior<>(k, pose, covariance_pose3); + graph.addPrior(k, pose, covariance_pose3); // Assert the graph has 0 error with the correct values. values.insert(k, pose); diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 0d1f4ce13..88ae508b6 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -115,9 +115,9 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) { // Add a floating landmark constellation if (i == 7) { - new_factors.addPrior<>(lm1, landmark1, model2); - new_factors.addPrior<>(lm2, landmark2, model2); - new_factors.addPrior<>(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)); @@ -192,9 +192,9 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { // Add a floating landmark constellation if (i == 7) { - new_factors.addPrior<>(lm1, landmark1, model2); - new_factors.addPrior<>(lm2, landmark2, model2); - new_factors.addPrior<>(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)); @@ -295,7 +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.addPrior<>(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 0264d5a4b..dd708b37a 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -181,7 +181,7 @@ TEST( NonlinearOptimizer, Factorization ) config.insert(X(2), Pose2(1.5,0.,0.)); NonlinearFactorGraph graph; - graph.addPrior<>(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; @@ -240,7 +240,7 @@ TEST(NonlinearOptimizer, NullFactor) { TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { NonlinearFactorGraph fg; - fg.addPrior<>(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), @@ -341,7 +341,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { NonlinearFactorGraph fg; - fg.addPrior<>(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))); @@ -372,7 +372,7 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { NonlinearFactorGraph fg; - fg.addPrior<>(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))); @@ -406,7 +406,7 @@ TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) { NonlinearFactorGraph fg; - fg.addPrior<>(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))); @@ -453,7 +453,7 @@ TEST(NonlinearOptimizer, RobustMeanCalculation) { vector pts{-10,-3,-1,1,3,10,1000}; for(auto pt : pts) { - fg.addPrior<>(0, pt, huber); + fg.addPrior(0, pt, huber); } init.insert(0, 100.0); @@ -483,9 +483,9 @@ TEST(NonlinearOptimizer, disconnected_graph) { init.insert(X(3), Pose2(0.,0.,0.)); NonlinearFactorGraph graph; - graph.addPrior<>(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.addPrior<>(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())); } @@ -528,9 +528,9 @@ TEST(NonlinearOptimizer, subclass_solver) { init.insert(X(3), Pose2(0.,0.,0.)); NonlinearFactorGraph graph; - graph.addPrior<>(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.addPrior<>(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(); @@ -585,7 +585,7 @@ struct traits { TEST(NonlinearOptimizer, Traits) { NonlinearFactorGraph fg; - fg.addPrior<>(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 c0f2804cc..5746022a3 100644 --- a/tests/testRot3Optimization.cpp +++ b/tests/testRot3Optimization.cpp @@ -39,7 +39,7 @@ TEST(Rot3, optimize) { Values truth; Values initial; Graph fg; - fg.addPrior<>(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))); diff --git a/tests/testVisualISAM2.cpp b/tests/testVisualISAM2.cpp index 955cd9708..9fedc853b 100644 --- a/tests/testVisualISAM2.cpp +++ b/tests/testVisualISAM2.cpp @@ -77,11 +77,11 @@ TEST(testVisualISAM2, all) static auto kPosePrior = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)) .finished()); - graph.addPrior<>(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.addPrior<>(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 e7f751fce..6e0f4ccdf 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -97,7 +97,7 @@ int main(int argc, char *argv[]) { // cout << "Initializing " << 0 << endl; newVariables.insert(0, Pose()); // Add prior - newFactors.addPrior<>(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 1e4cb49e1..7aaf37e92 100644 --- a/timing/timeLago.cpp +++ b/timing/timeLago.cpp @@ -50,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->addPrior<>(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 0a381e92a..a056bde24 100644 --- a/timing/timeiSAM2Chain.cpp +++ b/timing/timeiSAM2Chain.cpp @@ -60,7 +60,7 @@ int main(int argc, char *argv[]) { gttic_(Create_measurements); if(step == 0) { // Add prior - newFactors.addPrior<>(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;