From f39cbf736d215ef43de259232affd4ce575576bc Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Wed, 10 Aug 2016 22:06:53 -0400 Subject: [PATCH 1/8] Test the newly-added function emplace_shared --- gtsam/inference/tests/testFactorGraph.cpp | 44 +++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 gtsam/inference/tests/testFactorGraph.cpp diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp new file mode 100644 index 000000000..a3a69e72d --- /dev/null +++ b/gtsam/inference/tests/testFactorGraph.cpp @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testFactorGraph.cpp + * @author Yao Chen + */ + +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + + +TEST(FactorGraph, testEmplaceShared) { + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); + Point2 measurement(10.0, 10.0); + + NonlinearFactorGraph graph; + graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', 0), Symbol('l', 0), K)); + graph.emplace_shared >(measurement, measurementNoise, Symbol('x', 0), Symbol('l', 0), K); + + EXPECT_LONGS_EQUAL(2, graph.size()); + EXPECT(graph[0]->equals(*(graph[1]))); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ From 3d33b7e2cd4f2aa5b224d6eb4723388b4cffeb8f Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Wed, 10 Aug 2016 22:07:05 -0400 Subject: [PATCH 2/8] Added one function emplace_shared --- gtsam/inference/FactorGraph.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 38f30f5a3..604de632c 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -33,6 +33,8 @@ #include #include +#include + namespace gtsam { // Forward declarations @@ -159,6 +161,12 @@ namespace gtsam { void push_back(const sharedFactor& factor) { factors_.push_back(factor); } + /** Emplace a factor */ + template + void emplace_shared(Args&&... args) { + factors_.push_back(boost::make_shared(std::forward(args)...)); + } + /** push back many factors with an iterator over shared_ptr (factors are not copied) */ template typename boost::enable_if >::type @@ -355,4 +363,4 @@ namespace gtsam { } // namespace gtsam -#include \ No newline at end of file +#include From 95c75b8bae1a31a11497604b2bdf5fefe6d3018f Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Fri, 9 Sep 2016 08:33:51 -0400 Subject: [PATCH 3/8] Updated functions --- examples/CameraResectioning.cpp | 20 ++++++++------------ examples/Pose2SLAMExample_graphviz.cpp | 12 ++++++------ examples/Pose2SLAMwSPCG.cpp | 12 ++++++------ examples/SFMExample.cpp | 6 +++--- examples/SFMExample_SmartFactor.cpp | 4 ++-- examples/SFMExample_SmartFactorPCG.cpp | 4 ++-- examples/SFMExample_bal.cpp | 6 +++--- examples/SFMExample_bal_COLAMD_METIS.cpp | 1 + 8 files changed, 31 insertions(+), 34 deletions(-) diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 676dd42ec..204af1fea 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -71,18 +71,14 @@ int main(int argc, char* argv[]) { // add measurement factors SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5)); boost::shared_ptr factor; - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(55, 45), Point3(10, 10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(45, 45), Point3(-10, 10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(45, 55), Point3(-10, -10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(55, 55), Point3(10, -10, 0))); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(55, 45), Point3(10, 10, 0)); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(45, 45), Point3(-10, 10, 0)); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(45, 55), Point3(-10, -10, 0)); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(55, 55), Point3(10, -10, 0)); /* 3. Create an initial estimate for the camera pose */ Values initial; diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 314ccbdb4..99711da2d 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -33,19 +33,19 @@ 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.push_back(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + graph.emplace_shared >(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)); // 2b. Add odometry factors - graph.push_back(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); - graph.push_back(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); - graph.push_back(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); - graph.push_back(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(1, 2, Pose2(2, 0, 0 ), model); + graph.emplace_shared >(2, 3, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(3, 4, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(4, 5, Pose2(2, 0, M_PI_2), model); // 2c. Add the loop closure constraint - graph.push_back(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(5, 2, Pose2(2, 0, M_PI_2), model); // 3. Create the data structure to hold the initial estimate to the solution // For illustrative purposes, these have been deliberately set to incorrect values diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 2c25d2f8e..9b459d7b8 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -36,18 +36,18 @@ 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.push_back(PriorFactor(1, prior, priorNoise)); + graph.emplace_shared >(1, prior, priorNoise); // 2b. Add odometry factors noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.push_back(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.push_back(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.emplace_shared >(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared >(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared >(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared >(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add the loop closure constraint noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph.push_back(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); + graph.emplace_shared >(5, 1, Pose2(0.0, 0.0, 0.0), model); graph.print("\nFactor Graph:\n"); // print diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 893454936..0e48bb892 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -75,14 +75,14 @@ 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.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + graph.emplace_shared >(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) { SimpleCamera camera(poses[i], *K); for (size_t j = 0; j < points.size(); ++j) { Point2 measurement = camera.project(points[j]); - graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); } } @@ -90,7 +90,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.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.emplace_shared >(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 4112afcad..e9d02b15c 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.3), Vector3::Constant(0.1)).finished()); - graph.push_back(PriorFactor(0, poses[0], noise)); + graph.emplace_shared >(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.push_back(PriorFactor(1, poses[1], noise)); // add directly to graph + graph.emplace_shared >(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 7f6c0ba71..da7c5ba9b 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.3), Vector3::Constant(0.1)).finished()); - graph.push_back(PriorFactor(0, poses[0], noise)); + graph.emplace_shared >(0, poses[0], noise); // Fix the scale ambiguity by adding a prior - graph.push_back(PriorFactor(1, poses[0], noise)); + graph.emplace_shared >(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 4c655fb7a..a3a416eb3 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -59,15 +59,15 @@ int main (int argc, char* argv[]) { for(const SfM_Measurement& m: track.measurements) { size_t i = m.first; Point2 uv = m.second; - graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P + graph.emplace_shared(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P } j += 1; } // 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.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); - graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); + graph.emplace_shared >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + graph.emplace_shared > (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); // Create initial estimate Values initial; diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 44a402b33..71eb1ac8f 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -82,6 +82,7 @@ int main (int argc, char* argv[]) { /** --------------- COMPARISON -----------------------**/ /** ----------------------------------------------------**/ +M\] LevenbergMarquardtParams params_using_COLAMD, params_using_METIS; try { From 249d6b0b1b180d43e59d1898904d8cc76a209320 Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Sat, 1 Oct 2016 11:17:41 -0400 Subject: [PATCH 4/8] Replaced graph.push_back with graph.emplace_shared if needed. --- examples/SFMExample_bal_COLAMD_METIS.cpp | 7 +- examples/SelfCalibrationExample.cpp | 8 +- examples/StereoVOExample.cpp | 14 +-- examples/StereoVOExample_large.cpp | 8 +- examples/VisualISAM2Example.cpp | 6 +- gtsam/geometry/tests/testTriangulation.cpp | 12 +-- gtsam/geometry/triangulation.h | 8 +- gtsam/slam/tests/testAntiFactor.cpp | 4 +- gtsam/slam/tests/testGeneralSFMFactor.cpp | 36 ++++---- .../testGeneralSFMFactor_Cal3Bundler.cpp | 6 +- gtsam/slam/tests/testReferenceFrameFactor.cpp | 24 ++--- .../tests/testSmartProjectionCameraFactor.cpp | 20 ++--- .../tests/testSmartProjectionPoseFactor.cpp | 87 ++++++++----------- gtsam/slam/tests/testStereoFactor.cpp | 4 +- .../examples/ConcurrentCalibration.cpp | 10 +-- .../examples/SmartProjectionFactorExample.cpp | 2 +- .../SmartStereoProjectionFactorExample.cpp | 2 +- gtsam_unstable/linear/LPSolver.h | 7 +- .../tests/testConcurrentBatchFilter.cpp | 58 ++++++------- .../tests/testConcurrentBatchSmoother.cpp | 48 +++++----- .../tests/testConcurrentIncrementalFilter.cpp | 52 +++++------ .../slam/tests/testSmartRangeFactor.cpp | 4 +- .../testSmartStereoProjectionPoseFactor.cpp | 16 ++-- tests/testGeneralSFMFactorB.cpp | 2 +- timing/timeSFMBAL.cpp | 2 +- 25 files changed, 215 insertions(+), 232 deletions(-) diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 71eb1ac8f..13a07dda5 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -64,15 +64,15 @@ int main (int argc, char* argv[]) { for(const SfM_Measurement& m: track.measurements) { size_t i = m.first; Point2 uv = m.second; - graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P + graph.emplace_shared(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P } j += 1; } // 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.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); - graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); + graph.emplace_shared >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + graph.emplace_shared >(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); // Create initial estimate Values initial; @@ -82,7 +82,6 @@ int main (int argc, char* argv[]) { /** --------------- COMPARISON -----------------------**/ /** ----------------------------------------------------**/ -M\] LevenbergMarquardtParams params_using_COLAMD, params_using_METIS; try { diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index 1c5d155dc..93e010543 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -54,7 +54,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.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + graph.emplace_shared >(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); @@ -65,17 +65,17 @@ int main(int argc, char* argv[]) { Point2 measurement = camera.project(points[j]); // The only real difference with the Visual SLAM example is that here we use a // different factor type, that also calculates the Jacobian with respect to calibration - graph.push_back(GeneralSFMFactor2(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0))); + graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0)); } } // Add a prior on the position of the first landmark. noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.emplace_shared >(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.push_back(PriorFactor(Symbol('K', 0), K, calNoise)); + graph.emplace_shared >(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/StereoVOExample.cpp b/examples/StereoVOExample.cpp index 35d6747bf..27c10deb3 100644 --- a/examples/StereoVOExample.cpp +++ b/examples/StereoVOExample.cpp @@ -39,7 +39,7 @@ int main(int argc, char** argv){ //create graph object, add first pose at origin with key '1' NonlinearFactorGraph graph; Pose3 first_pose; - graph.push_back(NonlinearEquality(1, Pose3())); + graph.emplace_shared >(1, Pose3()); //create factor noise model with 3 sigmas of value 1 const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); @@ -47,14 +47,14 @@ int main(int argc, char** argv){ const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)); //create and add stereo factors between first pose (key value 1) and the three landmarks - graph.push_back(GenericStereoFactor(StereoPoint2(520, 480, 440), model, 1, 3, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(120, 80, 440), model, 1, 4, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(320, 280, 140), model, 1, 5, K)); + graph.emplace_shared >(StereoPoint2(520, 480, 440), model, 1, 3, K); + graph.emplace_shared >(StereoPoint2(120, 80, 440), model, 1, 4, K); + graph.emplace_shared >(StereoPoint2(320, 280, 140), model, 1, 5, K); //create and add stereo factors between second pose and the three landmarks - graph.push_back(GenericStereoFactor(StereoPoint2(570, 520, 490), model, 2, 3, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(70, 20, 490), model, 2, 4, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(320, 270, 115), model, 2, 5, K)); + graph.emplace_shared >(StereoPoint2(570, 520, 490), model, 2, 3, K); + graph.emplace_shared >(StereoPoint2(70, 20, 490), model, 2, 4, K); + graph.emplace_shared >(StereoPoint2(320, 270, 115), model, 2, 5, K); //create Values object to contain initial estimates of camera poses and landmark locations Values initial_estimate; diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index 8b88c772d..80831bd21 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -83,9 +83,9 @@ int main(int argc, char** argv){ cout << "Reading stereo factors" << endl; //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { - graph.push_back( - GenericStereoFactor(StereoPoint2(uL, uR, v), model, - Symbol('x', x), Symbol('l', l), K)); + graph.emplace_shared< + GenericStereoFactor >(StereoPoint2(uL, uR, v), model, + Symbol('x', x), Symbol('l', l), K); //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it if (!initial_estimate.exists(Symbol('l', l))) { Pose3 camPose = initial_estimate.at(Symbol('x', x)); @@ -99,7 +99,7 @@ int main(int argc, char** argv){ //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + graph.emplace_shared >(Symbol('x',1),first_pose); cout << "Optimizing" << endl; //create Levenberg-Marquardt optimizer to optimize the factor graph diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 78bc463ec..157768be7 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -90,7 +90,7 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) { SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); - graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); } // Add an initial guess for the current pose @@ -104,11 +104,11 @@ int main(int argc, char* argv[]) { if( i == 0) { // Add a prior on pose x0 noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); // Add a prior on landmark l0 noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // add directly to graph // Add initial guesses to all observed landmarks // Intentionally initialize the variables off from the ground truth diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index f57ca60d3..9e599f3f5 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -378,12 +378,12 @@ TEST( triangulation, StereotriangulateNonlinear ) { NonlinearFactorGraph graph; static SharedNoiseModel unit(noiseModel::Unit::Create(3)); - graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK))); - graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK))); + graph.emplace_shared(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK); + graph.emplace_shared(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK); const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9); - graph.push_back(PriorFactor(Symbol('x',1), Pose3(m1), posePrior)); - graph.push_back(PriorFactor(Symbol('x',2), Pose3(m2), posePrior)); + graph.emplace_shared >(Symbol('x',1), Pose3(m1), posePrior); + graph.emplace_shared >(Symbol('x',2), Pose3(m2), posePrior); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); @@ -399,8 +399,8 @@ TEST( triangulation, StereotriangulateNonlinear ) { NonlinearFactorGraph graph; static SharedNoiseModel unit(noiseModel::Unit::Create(3)); - graph.push_back(TriangulationFactor(cameras[0], measurements[0], unit, Symbol('l',1))); - graph.push_back(TriangulationFactor(cameras[1], measurements[1], unit, Symbol('l',1))); + graph.emplace_shared >(cameras[0], measurements[0], unit, Symbol('l',1)); + graph.emplace_shared >(cameras[1], measurements[1], unit, Symbol('l',1)); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 81c8ed2f3..fdfe32e8b 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -89,8 +89,8 @@ std::pair triangulationGraph( const Pose3& pose_i = poses[i]; typedef PinholePose Camera; Camera camera_i(pose_i, sharedCal); - graph.push_back(TriangulationFactor // - (camera_i, measurements[i], unit2, landmarkKey)); + graph.emplace_shared > // + (camera_i, measurements[i], unit2, landmarkKey); } return std::make_pair(graph, values); } @@ -116,8 +116,8 @@ std::pair triangulationGraph( traits::dimension)); for (size_t i = 0; i < measurements.size(); i++) { const CAMERA& camera_i = cameras[i]; - graph.push_back(TriangulationFactor // - (camera_i, measurements[i], unit, landmarkKey)); + graph.emplace_shared > // + (camera_i, measurements[i], unit, landmarkKey); } return std::make_pair(graph, values); } diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 7a95e0fa9..e34e13279 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -90,8 +90,8 @@ TEST( AntiFactor, EquivalentBayesNet) SharedNoiseModel sigma(noiseModel::Unit::Create(6)); NonlinearFactorGraph graph; - graph.push_back(PriorFactor(1, pose1, sigma)); - graph.push_back(BetweenFactor(1, 2, pose1.between(pose2), sigma)); + graph.emplace_shared >(1, pose1, sigma); + graph.emplace_shared >(1, 2, pose1.between(pose2), sigma); // Create a configuration corresponding to the ground truth Values values; diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index b99cb5d9c..8e0b5ad8f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -368,9 +368,9 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 10.))); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.)); const double reproj_error = 1e-5; @@ -385,12 +385,12 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { // Tests range factor between a GeneralCamera and a Pose3 Graph graph; graph.addCameraConstraint(0, GeneralCamera()); - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 1.))); - graph.push_back( - PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), - noiseModel::Isotropic::Sigma(6, 1.))); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.)); + graph.emplace_shared< + PriorFactor >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + noiseModel::Isotropic::Sigma(6, 1.)); Values init; init.insert(X(0), GeneralCamera()); @@ -413,15 +413,15 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { // Tests range factor between a CalibratedCamera and a Pose3 NonlinearFactorGraph graph; - graph.push_back( - PriorFactor(X(0), CalibratedCamera(), - noiseModel::Isotropic::Sigma(6, 1.))); - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 1.))); - graph.push_back( - PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), - noiseModel::Isotropic::Sigma(6, 1.))); + graph.emplace_shared< + PriorFactor >(X(0), CalibratedCamera(), + noiseModel::Isotropic::Sigma(6, 1.)); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.)); + graph.emplace_shared< + PriorFactor >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + noiseModel::Isotropic::Sigma(6, 1.)); Values init; init.insert(X(0), CalibratedCamera()); diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 9fda7d745..d8ac0aa5b 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -369,9 +369,9 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) { graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 10.))); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.)); const double reproj_error = 1e-5; diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index f955aa191..24b818835 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -136,15 +136,15 @@ TEST( ReferenceFrameFactor, converge_trans ) { // Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 2.0); // beyond pi/2 - fails NonlinearFactorGraph graph; - graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1)); - graph.push_back(PointReferenceFrameFactor(lB2, tA1, lA2)); + graph.emplace_shared(lB1, tA1, lA1); + graph.emplace_shared(lB2, tA1, lA2); // hard constraints on points double error_gain = 1000.0; - graph.push_back(NonlinearEquality(lA1, local1, error_gain)); - graph.push_back(NonlinearEquality(lA2, local2, error_gain)); - graph.push_back(NonlinearEquality(lB1, global1, error_gain)); - graph.push_back(NonlinearEquality(lB2, global2, error_gain)); + graph.emplace_shared >(lA1, local1, error_gain); + graph.emplace_shared >(lA2, local2, error_gain); + graph.emplace_shared >(lB1, global1, error_gain); + graph.emplace_shared >(lB2, global2, error_gain); // create initial estimate Values init; @@ -186,9 +186,9 @@ TEST( ReferenceFrameFactor, converge_local ) { NonlinearFactorGraph graph; double error_gain = 1000.0; - graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1)); - graph.push_back(NonlinearEquality(lB1, global, error_gain)); - graph.push_back(NonlinearEquality(tA1, trans, error_gain)); + graph.emplace_shared(lB1, tA1, lA1); + graph.emplace_shared >(lB1, global, error_gain); + graph.emplace_shared >(tA1, trans, error_gain); // create initial estimate Values init; @@ -222,9 +222,9 @@ TEST( ReferenceFrameFactor, converge_global ) { NonlinearFactorGraph graph; double error_gain = 1000.0; - graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1)); - graph.push_back(NonlinearEquality(lA1, local, error_gain)); - graph.push_back(NonlinearEquality(tA1, trans, error_gain)); + graph.emplace_shared(lB1, tA1, lA1); + graph.emplace_shared >(lA1, local, error_gain); + graph.emplace_shared >(tA1, trans, error_gain); // create initial estimate Values init; diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 0db8149eb..4feeadc86 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -209,8 +209,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); // Create initial estimate Values initial; @@ -321,8 +321,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -398,8 +398,8 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { graph.push_back(smartFactor3); graph.push_back(smartFactor4); graph.push_back(smartFactor5); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -476,8 +476,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -552,8 +552,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index f0a864fdd..20b4e7cab 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -262,8 +262,8 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, bodyPose1, noisePrior)); - graph.push_back(PriorFactor(x2, bodyPose2, noisePrior)); + graph.emplace_shared >(x1, bodyPose1, noisePrior); + graph.emplace_shared >(x2, bodyPose2, noisePrior); // Check errors at ground truth poses Values gtValues; @@ -319,8 +319,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); Values groundTruth; groundTruth.insert(x1, cam1.pose()); @@ -547,8 +547,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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), @@ -614,8 +614,8 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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), @@ -675,8 +675,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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), @@ -747,8 +747,8 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); Values values; values.insert(x1, cam1.pose()); @@ -800,8 +800,8 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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 @@ -830,30 +830,21 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { NonlinearFactorGraph graph; // Project three landmarks into three cameras - graph.push_back( - ProjectionFactor(cam1.project(landmark1), model, x1, L(1), sharedK2)); - graph.push_back( - ProjectionFactor(cam2.project(landmark1), model, x2, L(1), sharedK2)); - graph.push_back( - ProjectionFactor(cam3.project(landmark1), model, x3, L(1), sharedK2)); + graph.emplace_shared(cam1.project(landmark1), model, x1, L(1), sharedK2); + graph.emplace_shared(cam2.project(landmark1), model, x2, L(1), sharedK2); + graph.emplace_shared(cam3.project(landmark1), model, x3, L(1), sharedK2); - graph.push_back( - ProjectionFactor(cam1.project(landmark2), model, x1, L(2), sharedK2)); - graph.push_back( - ProjectionFactor(cam2.project(landmark2), model, x2, L(2), sharedK2)); - graph.push_back( - ProjectionFactor(cam3.project(landmark2), model, x3, L(2), sharedK2)); + graph.emplace_shared(cam1.project(landmark2), model, x1, L(2), sharedK2); + graph.emplace_shared(cam2.project(landmark2), model, x2, L(2), sharedK2); + graph.emplace_shared(cam3.project(landmark2), model, x3, L(2), sharedK2); - graph.push_back( - ProjectionFactor(cam1.project(landmark3), model, x1, L(3), sharedK2)); - graph.push_back( - ProjectionFactor(cam2.project(landmark3), model, x2, L(3), sharedK2)); - graph.push_back( - ProjectionFactor(cam3.project(landmark3), model, x3, L(3), sharedK2)); + graph.emplace_shared(cam1.project(landmark3), model, x1, L(3), sharedK2); + graph.emplace_shared(cam2.project(landmark3), model, x2, L(3), sharedK2); + graph.emplace_shared(cam3.project(landmark3), model, x3, L(3), sharedK2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); - graph.push_back(PriorFactor(x2, pose_right, noisePrior)); + graph.emplace_shared >(x1, level_pose, noisePrior); + graph.emplace_shared >(x2, pose_right, noisePrior); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); @@ -1000,11 +991,9 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise @@ -1068,11 +1057,9 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); // 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), @@ -1286,8 +1273,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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), @@ -1362,11 +1349,9 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); // 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), diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 45b978c27..c802603db 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -185,11 +185,11 @@ TEST( StereoFactor, singlePoint) { NonlinearFactorGraph graph; - graph.push_back(NonlinearEquality(X(1), camera1)); + graph.emplace_shared >(X(1), camera1); StereoPoint2 measurement(320, 320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph.push_back(GenericStereoFactor(measurement, model, X(1), L(1), K)); + graph.emplace_shared >(measurement, model, X(1), L(1), K); // Create a configuration corresponding to the ground truth Values values; diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index 28f7e8444..fe1c83ce3 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -61,7 +61,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.push_back(PriorFactor(Symbol('K', 0), *noisy_K, calNoise)); + graph.emplace_shared >(Symbol('K', 0), *noisy_K, calNoise); ifstream pose_file(pose_loc.c_str()); @@ -77,7 +77,7 @@ int main(int argc, char** argv){ } noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01); - graph.push_back(PriorFactor(Symbol('x', pose_id), Pose3(m), poseNoise)); + graph.emplace_shared >(Symbol('x', pose_id), Pose3(m), poseNoise); // camera and landmark keys size_t x, l; @@ -89,9 +89,9 @@ int main(int argc, char** argv){ cout << "Reading stereo factors" << endl; //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { -// graph.push_back( GenericStereoFactor(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K)); +// graph.emplace_shared >(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K); - graph.push_back(GeneralSFMFactor2(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0))); + graph.emplace_shared >(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0)); //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it @@ -107,7 +107,7 @@ int main(int argc, char** argv){ //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + graph.emplace_shared >(Symbol('x',1),first_pose); cout << "Optimizing" << endl; LevenbergMarquardtParams params; diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index c5ba616ed..94a70470a 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -104,7 +104,7 @@ int main(int argc, char** argv){ //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(1,firstPose)); + graph.emplace_shared >(1,firstPose); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 40ad0cb52..c8023b23c 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -126,7 +126,7 @@ int main(int argc, char** argv){ //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + graph.emplace_shared >(Symbol('x',1),first_pose); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 91cee3941..153aa7fda 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -51,7 +51,7 @@ struct LPPolicy { ++it) { size_t dim = lp.cost.getDim(it); Vector b = xk.at(*it) - lp.cost.getA(it).transpose(); // b = xk-g - graph.push_back(JacobianFactor(*it, Matrix::Identity(dim, dim), b)); + graph.emplace_shared(*it, Matrix::Identity(dim, dim), b); } KeySet allKeys = lp.inequalities.keys(); @@ -67,8 +67,7 @@ struct LPPolicy { std::inserter(difference, difference.end())); for (Key k : difference) { size_t dim = lp.constrainedKeyDimMap().at(k); - graph.push_back( - JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k))); + graph.emplace_shared(k, Matrix::Identity(dim, dim), xk.at(k)); } } return graph; @@ -77,4 +76,4 @@ struct LPPolicy { using LPSolver = ActiveSetSolver; -} \ No newline at end of file +} diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index 74ee23fe5..4aff1b465 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -403,9 +403,9 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) NonlinearFactorGraph partialGraph; - partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); Values partialValues; partialValues.insert(1, optimalValues.at(1)); @@ -437,10 +437,10 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(NonlinearFactor::shared_ptr()); // ========================================================== - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); for(const GaussianFactor::shared_ptr& factor: result) { - expectedGraph.push_back(LinearContainerFactor(factor, partialValues)); + expectedGraph.emplace_shared(factor, partialValues); } @@ -1126,13 +1126,13 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(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()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1181,11 +1181,11 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // 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()); @@ -1238,10 +1238,10 @@ 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.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1258,11 +1258,11 @@ TEST( ConcurrentBatchFilter, removeFactors_values ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(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)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.emplace_shared >(1, poseInitial, noisePrior); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); Values newValues; newValues.insert(1, Pose3().compose(poseError)); @@ -1291,11 +1291,11 @@ TEST( ConcurrentBatchFilter, removeFactors_values ) // note: factors are removed before the optimization NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // 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()); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index dc316e2ac..a0b6e2c1b 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -617,13 +617,13 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = smoother.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(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()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -670,11 +670,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = smoother.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // 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()); @@ -724,10 +724,10 @@ 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.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -744,11 +744,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_values ) // Add some factors to the Smoother NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(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)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.emplace_shared >(1, poseInitial, noisePrior); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); Values newValues; newValues.insert(1, Pose3().compose(poseError)); @@ -774,11 +774,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_values ) // note: factors are removed before the optimization NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // 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()); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index 0c922fb9e..d6b7ab851 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -423,9 +423,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues); @@ -441,7 +441,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(NonlinearFactor::shared_ptr()); // ========================================================== - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); for(const GaussianFactor::shared_ptr& factor: marginal) { // the linearization point for the linear container is optional, but it is not used in the filter, @@ -507,9 +507,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); GaussianFactorGraph linearGraph = *partialGraph.linearize(optimalValues); @@ -525,7 +525,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(NonlinearFactor::shared_ptr()); // ========================================================== - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); for(const GaussianFactor::shared_ptr& factor: marginal) { // the linearization point for the linear container is optional, but it is not used in the filter, @@ -1231,13 +1231,13 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.emplace_shared >(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()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1289,11 +1289,11 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // 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()); @@ -1350,10 +1350,10 @@ 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.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1406,11 +1406,11 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values ) Values actualValues = filter.getLinearizationPoint(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // 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()); diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 9ae4aa928..e18505af6 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -116,8 +116,8 @@ TEST( SmartRangeFactor, optimization ) { graph.push_back(f); const noiseModel::Base::shared_ptr // priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI)); - graph.push_back(PriorFactor(1, pose1, priorNoise)); - graph.push_back(PriorFactor(2, pose2, priorNoise)); + graph.emplace_shared >(1, pose1, priorNoise); + graph.emplace_shared >(2, pose2, priorNoise); // Try optimizing LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 8051e238a..8bd874711 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -254,8 +254,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(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), @@ -394,8 +394,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(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), @@ -463,8 +463,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(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), @@ -545,8 +545,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(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/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index 410617cbf..95caaaf9c 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -51,7 +51,7 @@ TEST(PinholeCamera, BAL) { for (size_t j = 0; j < db.number_tracks(); j++) { for (const SfM_Measurement& m: db.tracks[j].measurements) - graph.push_back(sfmFactor(m.second, unit2, m.first, P(j))); + graph.emplace_shared(m.second, unit2, m.first, P(j)); } Values initial = initialCamerasAndPointsEstimate(db); diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index c1e695b3a..be1104b1a 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -40,7 +40,7 @@ int main(int argc, char* argv[]) { for (const SfM_Measurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; - graph.push_back(SfmFactor(z, gNoiseModel, C(i), P(j))); + graph.emplace_shared(z, gNoiseModel, C(i), P(j)); } } From 3c1a0a8801ed02d6f3073730db4332f03f78221e Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Sat, 1 Oct 2016 11:41:37 -0400 Subject: [PATCH 5/8] Replaced graph.add with graph.emplace_shared if needed. --- examples/LocalizationExample.cpp | 10 +-- examples/METISOrderingExample.cpp | 6 +- examples/OdometryExample.cpp | 6 +- examples/PlanarSLAMExample.cpp | 12 +-- examples/Pose2SLAMExample.cpp | 12 +-- examples/VisualISAMExample.cpp | 9 +- gtsam/geometry/tests/testUnit3.cpp | 2 +- gtsam/navigation/tests/testImuFactor.cpp | 4 +- gtsam/slam/InitializePose3.cpp | 7 +- gtsam/slam/tests/testDataset.cpp | 86 +++++++++---------- .../slam/tests/testEssentialMatrixFactor.cpp | 10 +-- gtsam/slam/tests/testRotateFactor.cpp | 12 +-- .../tests/testNonlinearClusterTree.cpp | 12 +-- 13 files changed, 93 insertions(+), 95 deletions(-) diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 84bca65f3..17f921fd1 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -120,15 +120,15 @@ int main(int argc, char** argv) { // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); - graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.emplace_shared >(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); + graph.emplace_shared >(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise); // 2b. Add "GPS-like" measurements // We will use our custom UnaryFactor for this. noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y - graph.add(boost::make_shared(1, 0.0, 0.0, unaryNoise)); - graph.add(boost::make_shared(2, 2.0, 0.0, unaryNoise)); - graph.add(boost::make_shared(3, 4.0, 0.0, unaryNoise)); + graph.emplace_shared(1, 0.0, 0.0, unaryNoise); + graph.emplace_shared(2, 2.0, 0.0, unaryNoise); + graph.emplace_shared(3, 4.0, 0.0, unaryNoise); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index eb6bdd49d..3416eb6b7 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -37,12 +37,12 @@ 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.add(PriorFactor(1, priorMean, priorNoise)); + graph.emplace_shared >(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)); - graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); - graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); + graph.emplace_shared >(1, 2, odometry, odometryNoise); + graph.emplace_shared >(2, 3, odometry, odometryNoise); graph.print("\nFactor Graph:\n"); // print Values initial; diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index b5cd681e5..3b547e70c 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -65,15 +65,15 @@ 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.add(PriorFactor(1, priorMean, priorNoise)); + graph.emplace_shared >(1, priorMean, priorNoise); // Add odometry factors Pose2 odometry(2.0, 0.0, 0.0); // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); - graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); + graph.emplace_shared >(1, 2, odometry, odometryNoise); + graph.emplace_shared >(2, 3, odometry, odometryNoise); graph.print("\nFactor Graph:\n"); // print // Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index a716f9cd8..7c43c22e2 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -81,13 +81,13 @@ 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.add(PriorFactor(x1, prior, priorNoise)); // add directly to graph + graph.emplace_shared >(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) noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); - graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); + graph.emplace_shared >(x1, x2, odometry, odometryNoise); + graph.emplace_shared >(x2, x3, odometry, odometryNoise); // Add Range-Bearing measurements to two different landmarks // create a noise model for the landmark measurements @@ -101,9 +101,9 @@ int main(int argc, char** argv) { range32 = 2.0; // Add Bearing-Range factors - graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); - graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); - graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); + graph.emplace_shared >(x1, l1, bearing11, range11, measurementNoise); + graph.emplace_shared >(x2, l1, bearing21, range21, measurementNoise); + graph.emplace_shared >(x3, l2, bearing32, range32, measurementNoise); // Print graph.print("Factor Graph:\n"); diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 7991f7fbf..f977a08af 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -72,23 +72,23 @@ 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.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + graph.emplace_shared >(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)); // 2b. Add odometry factors // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); - graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(1, 2, Pose2(2, 0, 0 ), model); + graph.emplace_shared >(2, 3, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(3, 4, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(4, 5, Pose2(2, 0, M_PI_2), model); // 2c. Add the loop closure constraint // This factor encodes the fact that we have returned to the same pose. In real systems, // these constraints may be identified in many ways, such as appearance-based techniques // with camera images. We will use another Between Factor to enforce this constraint: - graph.add(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(5, 2, Pose2(2, 0, M_PI_2), model); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index a839289c2..8ca1be596 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -89,9 +89,8 @@ int main(int argc, char* argv[]) { SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); // Add measurement - graph.add( - GenericProjectionFactor(measurement, noise, - Symbol('x', i), Symbol('l', j), K)); + graph.emplace_shared >(measurement, noise, + Symbol('x', i), Symbol('l', j), K); } // Intentionally initialize the variables off from the ground truth @@ -109,12 +108,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.3), Vector3::Constant(0.1)).finished()); - graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); // Add a prior on landmark l0 noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); + graph.emplace_shared >(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/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 912b26262..71093d668 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -426,7 +426,7 @@ TEST(Unit3, ErrorBetweenFactor) { // Add prior factors. SharedNoiseModel R_prior = noiseModel::Unit::Create(2); for (size_t i = 0; i < data.size(); i++) { - graph.add(PriorFactor(U(i), data[i], R_prior)); + graph.emplace_shared >(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 d1dc6316d..6f2d60f2f 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -781,8 +781,8 @@ TEST(ImuFactor, bodyPSensorWithBias) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factors - graph.add(ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim)); - graph.add(BetweenFactor(B(i - 1), B(i), zeroBias, biasNoiseModel)); + graph.emplace_shared(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim); + graph.emplace_shared >(B(i - 1), B(i), zeroBias, biasNoiseModel); values.insert(X(i), Pose3()); values.insert(V(i), zeroVel); diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index f049e9d62..58408e7e3 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -120,9 +120,8 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { boost::shared_ptr > pose3Prior = boost::dynamic_pointer_cast >(factor); if (pose3Prior) - pose3Graph.add( - BetweenFactor(keyAnchor, pose3Prior->keys()[0], - pose3Prior->prior(), pose3Prior->noiseModel())); + pose3Graph.emplace_shared >(keyAnchor, pose3Prior->keys()[0], + pose3Prior->prior(), pose3Prior->noiseModel()); } return pose3Graph; } @@ -330,7 +329,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { // add prior noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); initialPose.insert(keyAnchor, Pose3()); - pose3graph.add(PriorFactor(keyAnchor, Pose3(), priorModel)); + pose3graph.emplace_shared >(keyAnchor, Pose3(), priorModel); // Create optimizer GaussNewtonParams params; diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 1dd4e8abc..b3e208b91 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -112,18 +112,18 @@ TEST( dataSet, readG2o) noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); NonlinearFactorGraph expectedGraph; - expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); - expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); - expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); - expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); - expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); - expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); - expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); - expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); - expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); - expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); - expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); - expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); + expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); + expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); + expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); + expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); + expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); + expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); + expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); + expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); + expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); + expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); + expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } @@ -164,27 +164,27 @@ TEST( dataSet, readG2o3D) Point3 p01 = Point3(1.001367, 0.015390, 0.004948); Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); + expectedGraph.emplace_shared >(0, 1, Pose3(R01,p01), model); Point3 p12 = Point3(0.523923, 0.776654, 0.326659); Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); - expectedGraph.add(BetweenFactor(1, 2, Pose3(R12,p12), model)); + expectedGraph.emplace_shared >(1, 2, Pose3(R12,p12), model); Point3 p23 = Point3(0.910927, 0.055169, -0.411761); Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); - expectedGraph.add(BetweenFactor(2, 3, Pose3(R23,p23), model)); + expectedGraph.emplace_shared >(2, 3, Pose3(R23,p23), model); Point3 p34 = Point3(0.775288, 0.228798, -0.596923); Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); - expectedGraph.add(BetweenFactor(3, 4, Pose3(R34,p34), model)); + expectedGraph.emplace_shared >(3, 4, Pose3(R34,p34), model); Point3 p14 = Point3(-0.577841, 0.628016, -0.543592); Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); - expectedGraph.add(BetweenFactor(1, 4, Pose3(R14,p14), model)); + expectedGraph.emplace_shared >(1, 4, Pose3(R14,p14), model); Point3 p30 = Point3(-0.623267, 0.086928, 0.773222); Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); - expectedGraph.add(BetweenFactor(3, 0, Pose3(R30,p30), model)); + expectedGraph.emplace_shared >(3, 0, Pose3(R30,p30), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } @@ -224,7 +224,7 @@ TEST( dataSet, readG2o3DNonDiagonalNoise) NonlinearFactorGraph expectedGraph; Point3 p01 = Point3(1.001367, 0.015390, 0.004948); Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); + expectedGraph.emplace_shared >(0, 1, Pose3(R01,p01), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2)); } @@ -242,18 +242,18 @@ TEST( dataSet, readG2oHuber) SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel); NonlinearFactorGraph expectedGraph; - expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); - expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); - expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); - expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); - expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); - expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); - expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); - expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); - expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); - expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); - expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); - expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); + expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); + expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); + expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); + expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); + expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); + expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); + expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); + expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); + expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); + expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); + expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } @@ -270,18 +270,18 @@ TEST( dataSet, readG2oTukey) SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); NonlinearFactorGraph expectedGraph; - expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); - expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); - expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); - expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); - expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); - expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); - expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); - expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); - expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); - expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); - expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); - expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); + expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); + expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); + expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); + expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); + expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); + expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); + expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); + expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); + expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); + expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); + expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 2e3d613d6..d33551edf 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -178,7 +178,7 @@ TEST (EssentialMatrixFactor, minimization) { // Noise sigma is 1cm, assuming metric measurements NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1)); + graph.emplace_shared(1, pA(i), pB(i), model1); // Check error at ground truth Values truth; @@ -251,7 +251,7 @@ TEST (EssentialMatrixFactor2, minimization) { // Noise sigma is 1cm, assuming metric measurements NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2)); + graph.emplace_shared(100, i, pA(i), pB(i), model2); // Check error at ground truth Values truth; @@ -323,7 +323,7 @@ TEST (EssentialMatrixFactor3, minimization) { NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) // but now we specify the rotation bRc - graph.add(EssentialMatrixFactor3(100, i, pA(i), pB(i), cRb, model2)); + graph.emplace_shared(100, i, pA(i), pB(i), cRb, model2); // Check error at ground truth Values truth; @@ -391,7 +391,7 @@ TEST (EssentialMatrixFactor, extraMinimization) { NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1, K)); + graph.emplace_shared(1, pA(i), pB(i), model1, K); // Check error at ground truth Values truth; @@ -465,7 +465,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) { // Noise sigma is 1, assuming pixel measurements NonlinearFactorGraph graph; for (size_t i = 0; i < data.number_tracks(); i++) - graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2, K)); + graph.emplace_shared(100, i, pA(i), pB(i), model2, K); // Check error at ground truth Values truth; diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 9bb296444..caf3d31df 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -89,9 +89,9 @@ TEST (RotateFactor, minimization) { // Let's try to recover the correct iRc by minimizing NonlinearFactorGraph graph; Model model = noiseModel::Isotropic::Sigma(3, 0.01); - graph.add(RotateFactor(1, i1Ri2, c1Zc2, model)); - graph.add(RotateFactor(1, i2Ri3, c2Zc3, model)); - graph.add(RotateFactor(1, i3Ri4, c3Zc4, model)); + graph.emplace_shared(1, i1Ri2, c1Zc2, model); + graph.emplace_shared(1, i2Ri3, c2Zc3, model); + graph.emplace_shared(1, i3Ri4, c3Zc4, model); // Check error at ground truth Values truth; @@ -162,9 +162,9 @@ TEST (RotateDirectionsFactor, minimization) { // Let's try to recover the correct iRc by minimizing NonlinearFactorGraph graph; Model model = noiseModel::Isotropic::Sigma(2, 0.01); - graph.add(RotateDirectionsFactor(1, p1, z1, model)); - graph.add(RotateDirectionsFactor(1, p2, z2, model)); - graph.add(RotateDirectionsFactor(1, p3, z3, model)); + graph.emplace_shared(1, p1, z1, model); + graph.emplace_shared(1, p2, z2, model); + graph.emplace_shared(1, p3, z3, model); // Check error at ground truth Values truth; diff --git a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp index 9391814d4..2240034af 100644 --- a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp +++ b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp @@ -27,22 +27,22 @@ 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.add(PriorFactor(x1, prior, priorNoise)); + graph.emplace_shared >(x1, prior, priorNoise); // Two odometry factors Pose2 odometry(2.0, 0.0, 0.0); auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); - graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); + graph.emplace_shared >(x1, x2, odometry, odometryNoise); + graph.emplace_shared >(x2, x3, odometry, odometryNoise); // Add Range-Bearing measurements to two different landmarks auto measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90), bearing32 = Rot2::fromDegrees(90); double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0; - graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); - graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); - graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); + graph.emplace_shared >(x1, l1, bearing11, range11, measurementNoise); + graph.emplace_shared >(x2, l1, bearing21, range21, measurementNoise); + graph.emplace_shared >(x3, l2, bearing32, range32, measurementNoise); return graph; } From 60f556e513f4574bd3746b9003d0a0e1c5cca667 Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Sat, 1 Oct 2016 11:45:44 -0400 Subject: [PATCH 6/8] Replaced graph.push_back with graph.emplace_shared if needed. --- gtsam_unstable/linear/tests/testLPSolver.cpp | 12 ++++++------ .../tests/testConcurrentIncrementalSmootherGN.cpp | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index 8bf6be56b..a105a39f0 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -189,12 +189,12 @@ TEST(LPSolver, overConstrainedLinearSystem) { TEST(LPSolver, overConstrainedLinearSystem2) { GaussianFactorGraph graph; - graph.push_back(JacobianFactor(1, I_1x1, 2, I_1x1, kOne, - noiseModel::Constrained::All(1))); - graph.push_back(JacobianFactor(1, I_1x1, 2, -I_1x1, 5 * kOne, - noiseModel::Constrained::All(1))); - graph.push_back(JacobianFactor(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, - noiseModel::Constrained::All(1))); + graph.emplace_shared(1, I_1x1, 2, I_1x1, kOne, + noiseModel::Constrained::All(1)); + graph.emplace_shared(1, I_1x1, 2, -I_1x1, 5 * kOne, + noiseModel::Constrained::All(1)); + graph.emplace_shared(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, + noiseModel::Constrained::All(1)); VectorValues x = graph.optimize(); // This check confirms that gtsam linear constraint solver can't handle // over-constrained system diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 65f74dc3c..fdf9f08b5 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -639,13 +639,13 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) actualGraph.print("actual graph: \n"); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.emplace_shared >(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()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } From 9ba6b80f9ed49c212a22b589c47aeb95265bb1bb Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Sat, 8 Oct 2016 10:55:22 -0400 Subject: [PATCH 7/8] 1. Replace boost enable_if and is_base_of with those in stl 2. Removed boost header files 3. Removed testFactorGraph.cpp --- gtsam/inference/FactorGraph.h | 36 ++++++++++--------- gtsam/inference/tests/testFactorGraph.cpp | 44 ----------------------- 2 files changed, 19 insertions(+), 61 deletions(-) delete mode 100644 gtsam/inference/tests/testFactorGraph.cpp diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 604de632c..86e130cc7 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -22,17 +22,16 @@ #pragma once -#include -#include -#include -#include -#include -#include - #include #include #include +#include +#include +#include +#include + +#include #include namespace gtsam { @@ -153,7 +152,7 @@ namespace gtsam { /** Add a factor directly using a shared_ptr */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(boost::shared_ptr factor) { factors_.push_back(boost::shared_ptr(factor)); } @@ -163,19 +162,20 @@ namespace gtsam { /** Emplace a factor */ template - void emplace_shared(Args&&... args) { + typename std::enable_if::value>::type + emplace_shared(Args&&... args) { factors_.push_back(boost::make_shared(std::forward(args)...)); } /** push back many factors with an iterator over shared_ptr (factors are not copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(ITERATOR firstFactor, ITERATOR lastFactor) { factors_.insert(end(), firstFactor, lastFactor); } /** push back many factors as shared_ptr's in a container (factors are not copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const CONTAINER& container) { push_back(container.begin(), container.end()); } @@ -183,22 +183,24 @@ namespace gtsam { /** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived * classes in favor of a type-specialized version that calls this templated function. */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const BayesTree& bayesTree) { bayesTree.addFactorsToGraph(*this); } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid * the copy). */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const DERIVEDFACTOR& factor) { factors_.push_back(boost::make_shared(factor)); } +#endif /** push back many factors with an iterator over plain factors (factors are copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(ITERATOR firstFactor, ITERATOR lastFactor) { for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f); @@ -206,14 +208,14 @@ namespace gtsam { /** push back many factors as non-pointer objects in a container (factors are copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const CONTAINER& container) { push_back(container.begin(), container.end()); } /** Add a factor directly using a shared_ptr */ template - typename boost::enable_if, + typename std::enable_if::value, boost::assign::list_inserter > >::type operator+=(boost::shared_ptr factor) { return boost::assign::make_list_inserter(RefCallPushBack(*this))(factor); @@ -234,7 +236,7 @@ namespace gtsam { /** Add a factor directly using a shared_ptr */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type add(boost::shared_ptr factor) { push_back(factor); } diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp deleted file mode 100644 index a3a69e72d..000000000 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testFactorGraph.cpp - * @author Yao Chen - */ - -#include -#include -#include - -#include -#include - -using namespace std; -using namespace gtsam; - - -TEST(FactorGraph, testEmplaceShared) { - Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); - Point2 measurement(10.0, 10.0); - - NonlinearFactorGraph graph; - graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', 0), Symbol('l', 0), K)); - graph.emplace_shared >(measurement, measurementNoise, Symbol('x', 0), Symbol('l', 0), K); - - EXPECT_LONGS_EQUAL(2, graph.size()); - EXPECT(graph[0]->equals(*(graph[1]))); -} - - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ From 80562c8ae222acfdc5e79181f1c75b6ac3b987e8 Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Wed, 19 Oct 2016 09:16:47 -0400 Subject: [PATCH 8/8] Fixed the issue of compilation error. --- gtsam/inference/FactorGraph.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 86e130cc7..016488701 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -188,7 +188,7 @@ namespace gtsam { bayesTree.addFactorsToGraph(*this); } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +//#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid * the copy). */ template @@ -196,7 +196,7 @@ namespace gtsam { push_back(const DERIVEDFACTOR& factor) { factors_.push_back(boost::make_shared(factor)); } -#endif +//#endif /** push back many factors with an iterator over plain factors (factors are copied) */ template