diff --git a/examples/InverseKinematicsExampleExpressions.cpp b/examples/InverseKinematicsExampleExpressions.cpp index 9da1c62c0..1df83d9a1 100644 --- a/examples/InverseKinematicsExampleExpressions.cpp +++ b/examples/InverseKinematicsExampleExpressions.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include diff --git a/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp index 3760d0dd0..c35b9bc45 100644 --- a/examples/Pose2SLAMExampleExpressions.cpp +++ b/examples/Pose2SLAMExampleExpressions.cpp @@ -21,7 +21,6 @@ #include // For an explanation of headers below, please see Pose2SLAMExample.cpp -#include #include #include #include diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp index b72acd30d..abadce975 100644 --- a/examples/Pose3SLAMExample_changeKeys.cpp +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -19,7 +19,6 @@ #include #include -#include #include using namespace std; diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 916238a28..c444c85aa 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -32,8 +32,8 @@ // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. -// We will apply a simple prior on the rotation -#include +// We will apply a simple prior on the rotation. We do so via the `addPrior` convenience +// method in NonlinearFactorGraph. // When the factors are created, we will add them to a Factor Graph. As the factors we are using // are nonlinear factors, we will need a Nonlinear Factor Graph. @@ -78,7 +78,6 @@ int main() { prior.print("goal angle"); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree); Symbol key('x',1); - PriorFactor factor(key, prior, model); /** * Step 2: Create a graph container and add the factor to it @@ -90,7 +89,7 @@ int main() { * many more factors would be added. */ NonlinearFactorGraph graph; - graph.push_back(factor); + graph.addPrior(key, prior, model); graph.print("full graph"); /** diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 087d7c86a..6c0f9c8f4 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -22,7 +22,6 @@ #include #include #include -#include #include #include diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index c903d5232..81f67f1ee 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -17,7 +17,6 @@ */ #include -#include #include #include #include @@ -49,10 +48,9 @@ TEST (OrientedPlane3Factor, lm_translation_error) { Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); Vector sigmas(6); sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; - PriorFactor pose_prior(init_sym, init_pose, - noiseModel::Diagonal::Sigmas(sigmas)); + new_graph.addPrior( + init_sym, init_pose, noiseModel::Diagonal::Sigmas(sigmas)); new_values.insert(init_sym, init_pose); - new_graph.add(pose_prior); // Add two landmark measurements, differing in range new_values.insert(lm_sym, test_lm0); @@ -94,11 +92,10 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose Symbol init_sym('x', 0); Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); - PriorFactor pose_prior(init_sym, init_pose, + new_graph.addPrior(init_sym, init_pose, noiseModel::Diagonal::Sigmas( (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); new_values.insert(init_sym, init_pose); - new_graph.add(pose_prior); // // Add two landmark measurements, differing in angle new_values.insert(lm_sym, test_lm0); diff --git a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp index 2ae1b1d5b..141a50465 100644 --- a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp @@ -16,7 +16,6 @@ * @date Nov 2009 */ -#include #include #include #include