From af173c1a1ca5285c7f52d597184db2904ebfeaf8 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Fri, 16 Aug 2013 17:13:45 +0000 Subject: [PATCH] fixed the basic examples. Discrete & Kalman filter examples are disabled for now per CMakeLists.txt --- examples/CMakeLists.txt | 11 +++++++++++ examples/LocalizationExample.cpp | 4 ++-- examples/OdometryExample.cpp | 6 +++--- examples/PlanarSLAMExample.cpp | 12 ++++++------ examples/Pose2SLAMExample.cpp | 12 ++++++------ examples/Pose2SLAMExample_graph.cpp | 2 +- examples/Pose2SLAMwSPCG.cpp | 12 ++++++------ examples/RangeISAMExample_plaza2.cpp | 6 +++--- examples/SimpleRotation.cpp | 2 +- examples/VisualISAM2Example.cpp | 6 +++--- examples/VisualISAMExample.cpp | 6 +++--- examples/VisualSLAMExample.cpp | 6 +++--- examples/elaboratePoint2KalmanFilter.cpp | 2 +- 13 files changed, 49 insertions(+), 38 deletions(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 1c38616a4..21c49acb3 100644 --- a/examples/CMakeLists.txt +++ b/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} ) diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 035139a5d..4df997dcd 100644 --- a/examples/LocalizationExample.cpp +++ b/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(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); - graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); // 2b. Add "GPS-like" measurements // We will use our custom UnaryFactor for this. diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index d556798f0..2ef322a9b 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(Vector_(3, 0.3, 0.3, 0.1)); - graph.add(PriorFactor(1, priorMean, priorNoise)); + graph.push_back(PriorFactor(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(1, 2, odometry, odometryNoise)); - graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); + graph.push_back(BetweenFactor(1, 2, odometry, odometryNoise)); + graph.push_back(BetweenFactor(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 88b8da3e0..e671c0fd9 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(Vector_(3, 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.push_back(PriorFactor(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(x1, x2, odometry, odometryNoise)); - graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); + graph.push_back(BetweenFactor(x1, x2, odometry, odometryNoise)); + graph.push_back(BetweenFactor(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.push_back(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); + graph.push_back(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); + graph.push_back(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); // Print graph.print("Factor Graph:\n"); diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 1220481f5..8d7ec7bc3 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(Vector_(3, 0.3, 0.3, 0.1)); - graph.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + graph.push_back(PriorFactor(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(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.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)); // 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.push_back(BetweenFactor(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/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 000a071da..8598d3c85 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -42,7 +42,7 @@ int main(int argc, char** argv) { // Add a Gaussian prior on first poses Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.01, 0.01, 0.01)); - graph->add(PriorFactor(0, priorMean, priorNoise)); + graph->push_back(PriorFactor(0, priorMean, priorNoise)); // Single Step Optimization using Levenberg-Marquardt Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 09af6351d..6fe5ca68d 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/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(1, prior, priorNoise)); + graph.push_back(PriorFactor(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(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.add(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.add(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + 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)); // 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(5, 1, Pose2(0.0, 0.0, 0.0), model)); + graph.push_back(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); graph.print("\nFactor Graph:\n"); // print diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 7c3c43272..fc2edb7e1 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/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(0, pose0, priorNoise)); + newFactors.push_back(PriorFactor(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(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas))); + newFactors.push_back(BetweenFactor(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(i, symbol('L', j), range,rangeNoise)); + newFactors.push_back(RangeFactor(i, symbol('L', j), range,rangeNoise)); k = k + 1; countK = countK + 1; } diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 54c64e820..830b202cb 100644 --- a/examples/SimpleRotation.cpp +++ b/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"); /** diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index f27664cfe..0f403ec8a 100644 --- a/examples/VisualISAM2Example.cpp +++ b/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(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.push_back(GenericProjectionFactor(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(Symbol('x', 0), poses[0], poseNoise)); + graph.push_back(PriorFactor(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)); // add directly to graph + graph.push_back(PriorFactor(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/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 832a2ab83..e3f8bbacd 100644 --- a/examples/VisualISAMExample.cpp +++ b/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(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.push_back(GenericProjectionFactor(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(Symbol('x', 0), poses[0], poseNoise)); + graph.push_back(PriorFactor(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)); // add directly to graph + graph.push_back(PriorFactor(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/examples/VisualSLAMExample.cpp b/examples/VisualSLAMExample.cpp index 8aed051eb..dd2d27d0c 100644 --- a/examples/VisualSLAMExample.cpp +++ b/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(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + graph.push_back(PriorFactor(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(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.push_back(GenericProjectionFactor(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(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.push_back(PriorFactor(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 diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 52cf83c93..218b031fd 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -22,7 +22,7 @@ #include #include -#include +//#include #include #include #include