diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index a34a96c9b..5454f4c11 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((Vector(3) << 0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses - 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)); + 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)); // 2b. Add "GPS-like" measurements // We will use our custom UnaryFactor for this. noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // 10cm std on x,y - graph.push_back(boost::make_shared(1, 0.0, 0.0, unaryNoise)); - graph.push_back(boost::make_shared(2, 2.0, 0.0, unaryNoise)); - graph.push_back(boost::make_shared(3, 4.0, 0.0, unaryNoise)); + 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.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index f2d4c6497..70c6e6fb0 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.push_back(PriorFactor(1, priorMean, priorNoise)); + graph.add(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.push_back(BetweenFactor(1, 2, odometry, odometryNoise)); - graph.push_back(BetweenFactor(2, 3, odometry, odometryNoise)); + graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); + graph.add(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 a1c75eab7..91ca423a2 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.push_back(PriorFactor(x1, prior, priorNoise)); // add directly to graph + graph.add(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.push_back(BetweenFactor(x1, x2, odometry, odometryNoise)); - graph.push_back(BetweenFactor(x2, x3, odometry, odometryNoise)); + graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); + graph.add(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.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)); + graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); + graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); + graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); // Print graph.print("Factor Graph:\n"); diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index e1a51a493..ae34278ec 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.push_back(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + graph.add(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.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.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)); // 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.push_back(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.add(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/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index b5645e214..8a32f5dcc 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -14,6 +14,7 @@ * @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset * This version uses iSAM to solve the problem incrementally * @author Duy-Nguyen Ta + * @author Frank Dellaert */ /** @@ -61,7 +62,8 @@ int main(int argc, char* argv[]) { Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); // Define the camera observation noise model - noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + noiseModel::Isotropic::shared_ptr noise = // + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks vector points = createPoints(); @@ -69,7 +71,8 @@ int main(int argc, char* argv[]) { // Create the set of ground-truth poses vector poses = createPoses(); - // Create a NonlinearISAM object which will relinearize and reorder the variables every "relinearizeInterval" updates + // Create a NonlinearISAM object which will relinearize and reorder the variables + // every "relinearizeInterval" updates int relinearizeInterval = 3; NonlinearISAM isam(relinearizeInterval); @@ -82,32 +85,44 @@ int main(int argc, char* argv[]) { // Add factors for each landmark observation for (size_t j = 0; j < points.size(); ++j) { + // Create ground truth measurement SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); - graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + // Add measurement + graph.add( + GenericProjectionFactor(measurement, noise, + Symbol('x', i), Symbol('l', j), K)); } - // Add an initial guess for the current pose // Intentionally initialize the variables off from the ground truth - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + Pose3 noise(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + Pose3 initial_xi = poses[i].compose(noise); + + // Add an initial guess for the current pose + initialEstimate.insert(Symbol('x', i), initial_xi); // If this is the first iteration, add a prior on the first pose to set the coordinate frame // and a prior on the first landmark to set the scale // Also, as iSAM solves incrementally, we must wait until each is observed at least twice before // adding it to iSAM. - 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))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + if (i == 0) { + // 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))); + graph.add(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.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + noiseModel::Isotropic::shared_ptr pointNoise = + noiseModel::Isotropic::Sigma(3, 0.1); + graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // Add initial guesses to all observed landmarks - // Intentionally initialize the variables off from the ground truth - for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + Point3 noise(-0.25, 0.20, 0.15); + for (size_t j = 0; j < points.size(); ++j) { + // Intentionally initialize the variables off from the ground truth + Point3 initial_lj = points[j].compose(noise); + initialEstimate.insert(Symbol('l', j), initial_lj); + } } else { // Update iSAM with the new factors