fixed the basic examples. Discrete & Kalman filter examples are disabled for now per CMakeLists.txt

examples/CMakeLists.txt

@@ -4,6 +4,17 @@ endif()
 
 # Build example executables
 FILE(GLOB example_srcs "*.cpp")
+
+set (excluded_examples #"")
+    "${CMAKE_CURRENT_SOURCE_DIR}/DiscreteBayesNet_FG.cpp"
+    "${CMAKE_CURRENT_SOURCE_DIR}/UGM_chain.cpp"
+    "${CMAKE_CURRENT_SOURCE_DIR}/UGM_small.cpp"
+    "${CMAKE_CURRENT_SOURCE_DIR}/easyPoint2KalmanFilter.cpp"
+    "${CMAKE_CURRENT_SOURCE_DIR}/elaboratePoint2KalmanFilter.cpp"
+)
+
+list(REMOVE_ITEM example_srcs ${excluded_examples})
+
 foreach(example_src ${example_srcs} )
     get_filename_component(example_base ${example_src} NAME_WE)
     set( example_bin ${example_base} )

examples/LocalizationExample.cpp

@@ -120,8 +120,8 @@ 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(Vector_(3, 0.2, 0.2, 0.1));
   // Create odometry (Between) factors between consecutive poses
-  graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
-  graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
+  graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
+  graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
 
   // 2b. Add "GPS-like" measurements
   // We will use our custom UnaryFactor for this.

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(Vector_(3, 0.3, 0.3, 0.1));
-  graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
+  graph.push_back(PriorFactor<Pose2>(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(Vector_(3, 0.2, 0.2, 0.1));
   // Create odometry (Between) factors between consecutive poses
-  graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
-  graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
+  graph.push_back(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
+  graph.push_back(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
   graph.print("\nFactor Graph:\n"); // print
 
   // Create the data structure to hold the initialEstimate estimate to the solution

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(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
-  graph.add(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
+  graph.push_back(PriorFactor<Pose2>(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(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
-  graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
-  graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
+  graph.push_back(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
+  graph.push_back(BetweenFactor<Pose2>(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<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
-  graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
-  graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
+  graph.push_back(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
+  graph.push_back(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
+  graph.push_back(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
 
   // Print
   graph.print("Factor Graph:\n");

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(Vector_(3, 0.3, 0.3, 0.1));
-  graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
+  graph.push_back(PriorFactor<Pose2>(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(Vector_(3, 0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
   // Create odometry (Between) factors between consecutive poses
-  graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
-  graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
-  graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
-  graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
+  graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
+  graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
+  graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
+  graph.push_back(BetweenFactor<Pose2>(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<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
+  graph.push_back(BetweenFactor<Pose2>(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

examples/Pose2SLAMExample_graph.cpp

@@ -42,7 +42,7 @@ int main(int argc, char** argv) {
   // Add a Gaussian prior on first poses
   Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
   SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.01, 0.01, 0.01));
-  graph->add(PriorFactor<Pose2>(0, priorMean, priorNoise));
+  graph->push_back(PriorFactor<Pose2>(0, priorMean, priorNoise));
 
   // Single Step Optimization using Levenberg-Marquardt
   Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize();

examples/Pose2SLAMwSPCG.cpp

@@ -69,16 +69,16 @@ int main(int argc, char** argv) {
   // A prior factor consists of a mean and a noise model (covariance matrix)
   Pose2 prior(0.0, 0.0, 0.0); // prior at origin
   noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
-  graph.add(PriorFactor<Pose2>(1, prior, priorNoise));
+  graph.push_back(PriorFactor<Pose2>(1, prior, priorNoise));
 
   // 2b. Add odometry factors
   // For simplicity, we will use the same noise model for each odometry factor
   noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
   // Create odometry (Between) factors between consecutive poses
-  graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2),    odometryNoise));
-  graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
-  graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
-  graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
+  graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2),    odometryNoise));
+  graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
+  graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
+  graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
 
   // 2c. Add the loop closure constraint
   // This factor encodes the fact that we have returned to the same pose. In real systems,
@@ -86,7 +86,7 @@ int main(int argc, char** argv) {
   // with camera images.
   // We will use another Between Factor to enforce this constraint, with the distance set to zero,
   noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
-  graph.add(BetweenFactor<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
+  graph.push_back(BetweenFactor<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
   graph.print("\nFactor Graph:\n"); // print
 
 

examples/RangeISAMExample_plaza2.cpp

@@ -125,7 +125,7 @@ int main (int argc, char** argv) {
   Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
       M_PI - 2.02108900000000);
   NonlinearFactorGraph newFactors;
-  newFactors.add(PriorFactor<Pose2>(0, pose0, priorNoise));
+  newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
   Values initial;
   initial.insert(0, pose0);
 
@@ -158,7 +158,7 @@ int main (int argc, char** argv) {
     boost::tie(t, odometry) = timedOdometry;
 
     // add odometry factor
-    newFactors.add(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));
+    newFactors.push_back(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));
 
     // predict pose and add as initial estimate
     Pose2 predictedPose = lastPose.compose(odometry);
@@ -169,7 +169,7 @@ int main (int argc, char** argv) {
     while (k < K && t >= boost::get<0>(triples[k])) {
       size_t j = boost::get<1>(triples[k]);
       double range = boost::get<2>(triples[k]);
-      newFactors.add(RangeFactor<Pose2, Point2>(i, symbol('L', j), range,rangeNoise));
+      newFactors.push_back(RangeFactor<Pose2, Point2>(i, symbol('L', j), range,rangeNoise));
       k = k + 1;
       countK = countK + 1;
     }

examples/SimpleRotation.cpp

@@ -90,7 +90,7 @@ int main() {
    * many more factors would be added.
    */
   NonlinearFactorGraph graph;
-  graph.add(factor);
+  graph.push_back(factor);
   graph.print("full graph");
 
   /**

examples/VisualISAM2Example.cpp

@@ -111,7 +111,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.add(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
+      graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
     }
 
     // Add an initial guess for the current pose
@@ -125,11 +125,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, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-      graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
+      graph.push_back(PriorFactor<Pose3>(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<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
+      graph.push_back(PriorFactor<Point3>(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

examples/VisualISAMExample.cpp

@@ -105,7 +105,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.add(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
+      graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
     }
 
     // Add an initial guess for the current pose
@@ -119,11 +119,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, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-      graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
+      graph.push_back(PriorFactor<Pose3>(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<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
+      graph.push_back(PriorFactor<Point3>(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

examples/VisualSLAMExample.cpp

@@ -102,14 +102,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, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
+  graph.push_back(PriorFactor<Pose3>(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) {
     for (size_t j = 0; j < points.size(); ++j) {
       SimpleCamera camera(poses[i], *K);
       Point2 measurement = camera.project(points[j]);
-      graph.add(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
+      graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
     }
   }
 
@@ -117,7 +117,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.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
+  graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
   graph.print("Factor Graph:\n");
 
   // Create the data structure to hold the initialEstimate estimate to the solution

examples/elaboratePoint2KalmanFilter.cpp

@@ -22,7 +22,7 @@
 
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/Ordering.h>
+//#include <gtsam/nonlinear/Ordering.h>
 #include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>