From 67e2d832feafea9413c5f6d783b6f3d04cf87203 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 22 Jul 2012 04:52:01 +0000 Subject: [PATCH] Updated the VisualSLAM examples, removing the SLAM namespaces --- examples/VisualISAMExample.cpp | 184 ++++++++++++++++++++------------- examples/VisualSLAMData.h | 89 ---------------- examples/VisualSLAMExample.cpp | 134 ++++++++++++++++++------ 3 files changed, 216 insertions(+), 191 deletions(-) delete mode 100644 examples/VisualSLAMData.h diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 25e993bca..eab860db0 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -11,99 +11,139 @@ /** * @file VisualISAMExample.cpp - * @brief An ISAM example for synthesis sequence, single camera + * @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 */ +/** + * A structure-from-motion example with landmarks + * - The landmarks form a 10 meter cube + * - The robot rotates around the landmarks, always facing towards the cube + */ + +// As this is a full 3D problem, we will use Pose3 variables to represent the camera +// positions and Point3 variables (x, y, z) to represent the landmark coordinates. +// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +// We will also need a camera object to hold calibration information and perform projections. +#include +#include +#include +#include + +// Each variable in the system (poses and landmarks) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use Symbols #include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use Projection factors to model the camera's landmark observations. +// Also, we will initialize the robot at some location using a Prior factor. +#include +#include + +// We want to use iSAM to solve the structure-from-motion problem incrementally, so +// include iSAM here #include -#include -#include -#include "VisualSLAMData.h" + +// iSAM requires as input a set set of new factors to be added stored in a factor graph, +// and initial guesses for any new variables used in the added factors +#include +#include + +#include using namespace std; using namespace gtsam; -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::L; - /* ************************************************************************* */ int main(int argc, char* argv[]) { - VisualSLAMExampleData data = VisualSLAMExampleData::generate(); + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - /* 1. Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates */ - int relinearizeInterval = 3; - NonlinearISAM isam(relinearizeInterval); + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v - /* 2. At each frame (poseId) with new camera pose and set of associated measurements, - * create a graph of new factors and update ISAM */ + // Create the set of ground-truth landmarks + std::vector points; + points.push_back(gtsam::Point3(10.0,10.0,10.0)); + points.push_back(gtsam::Point3(-10.0,10.0,10.0)); + points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); + points.push_back(gtsam::Point3(10.0,-10.0,10.0)); + points.push_back(gtsam::Point3(10.0,10.0,-10.0)); + points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); + points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); + points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); - // Store the current best estimate from ISAM - Values currentEstimate; - - // First two frames: - // Add factors and initial values for the first two poses and landmarks then update ISAM. - // Note: measurements from the first pose only are not enough to update ISAM: - // the system is underconstrained. - { - visualSLAM::Graph newFactors; - - // First pose with prior factor - newFactors.addPosePrior(X(0), data.poses[0], data.noiseX); - - // Second pose with odometry measurement - newFactors.addRelativePose(X(0), X(1), data.odometry, data.noiseX); - - // Visual measurements at both poses - for (size_t i=0; i<2; ++i) { - for (size_t j=0; j poses; + double radius = 30.0; + int i = 0; + double theta = 0.0; + gtsam::Point3 up(0,0,1); + gtsam::Point3 target(0,0,0); + for(; i < 8; ++i, theta += 2*M_PI/8) { + gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0); + gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up); + poses.push_back(camera.pose()); } - // Subsequent frames: Add new odometry and measurement factors and initial values, - // then update ISAM at each frame - for (size_t i=2; i(X(i-1))*data.odometry); + // Create a Factor Graph and Values to hold the new data + NonlinearFactorGraph graph; + Values initialEstimate; - // update ISAM - isam.update(newFactors, initials); - currentEstimate = isam.estimate(); - cout << "****************************************************" << endl; - cout << "Frame " << i << ": " << endl; - currentEstimate.print("Current estimate: "); + // Loop over the different poses, adding the observations to iSAM incrementally + for (size_t i = 0; i < poses.size(); ++i) { + + // Add factors for each landmark observation + 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)); + } + + // 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)))); + + // 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, 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 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 + + // 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))); + + } else { + // Update iSAM with the new factors + isam.update(graph, initialEstimate); + Values currentEstimate = isam.estimate(); + cout << "****************************************************" << endl; + cout << "Frame " << i << ": " << endl; + currentEstimate.print("Current estimate: "); + + // Clear the factor graph and values for the next iteration + graph.resize(0); + initialEstimate.clear(); + } } return 0; } /* ************************************************************************* */ - diff --git a/examples/VisualSLAMData.h b/examples/VisualSLAMData.h deleted file mode 100644 index 28fa3040c..000000000 --- a/examples/VisualSLAMData.h +++ /dev/null @@ -1,89 +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 VisualSLAMData.cpp - * @brief Generate ground-truth simulated data for VisualSLAM examples - * @author Duy-Nguyen Ta - */ - -#pragma once - -#include -#include -#include - -/* ************************************************************************* */ -/** - * Simulated data for the visual SLAM examples: - * - 8 Landmarks: (10,10,10) (-10,10,10) (-10,-10,10) (10,-10,10) - * (10,10,-10) (-10,10,-10) (-10,-10,-10) (10,-10,-10) - * - n 90-deg-FoV cameras with the same calibration parameters: - * f = 50.0, Image: 100x100, center: 50.0, 50.0 - * and ground-truth poses on a circle around the landmarks looking at the world's origin: - * Rot3(-sin(theta), 0, -cos(theta), - * cos(theta), 0, -sin(theta), - * 0, -1, 0 ), - * Point3(r*cos(theta), r*sin(theta), 0.0) - * (theta += 2*pi/N) - * - Measurement noise: 1 pix sigma - */ -struct VisualSLAMExampleData { - gtsam::shared_ptrK sK; // camera calibration parameters - std::vector poses; // ground-truth camera poses - gtsam::Pose3 odometry; // ground-truth odometry between 2 consecutive poses (simulated data for iSAM) - std::vector points; // ground-truth landmarks - std::map > z; // 2D measurements of landmarks in each camera frame - gtsam::SharedDiagonal noiseZ; // measurement noise (noiseModel::Isotropic::Sigma(2, 5.0f)); - gtsam::SharedDiagonal noiseX; // noise for camera poses - gtsam::SharedDiagonal noiseL; // noise for landmarks - - static const VisualSLAMExampleData generate() { - VisualSLAMExampleData data; - // Landmarks (ground truth) - data.points.push_back(gtsam::Point3(10.0,10.0,10.0)); - data.points.push_back(gtsam::Point3(-10.0,10.0,10.0)); - data.points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); - data.points.push_back(gtsam::Point3(10.0,-10.0,10.0)); - data.points.push_back(gtsam::Point3(10.0,10.0,-10.0)); - data.points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); - data.points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); - data.points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); - - // Camera calibration parameters - data.sK = gtsam::shared_ptrK(new gtsam::Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - - // n camera poses - int n = 8; - double theta = 0.0; - double r = 30.0; - for (int i=0; isample()))*/); // you can add noise as desired - } - } - data.noiseX = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector_(6, 0.001, 0.001, 0.001, 0.1, 0.1, 0.1)); - data.noiseL = gtsam::noiseModel::Isotropic::Sigma(3, 0.1); - - return data; - } -}; diff --git a/examples/VisualSLAMExample.cpp b/examples/VisualSLAMExample.cpp index 1fc343203..8aed051eb 100644 --- a/examples/VisualSLAMExample.cpp +++ b/examples/VisualSLAMExample.cpp @@ -15,49 +15,123 @@ * @author Duy-Nguyen Ta */ -#include +/** + * A structure-from-motion example with landmarks + * - The landmarks form a 10 meter cube + * - The robot rotates around the landmarks, always facing towards the cube + */ + +// As this is a full 3D problem, we will use Pose3 variables to represent the camera +// positions and Point3 variables (x, y, z) to represent the landmark coordinates. +// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +// We will also need a camera object to hold calibration information and perform projections. +#include +#include +#include +#include + +// Each variable in the system (poses and landmarks) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use Symbols #include -#include -#include -#include "VisualSLAMData.h" + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use Projection factors to model the camera's landmark observations. +// Also, we will initialize the robot at some location using a Prior factor. +#include +#include + +// 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. +#include + +// Finally, once all of the factors have been added to our factor graph, we will want to +// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. +// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a +// trust-region method known as Powell's Degleg +#include + +// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +// nonlinear functions around an initial linearization point, then solve the linear system +// to update the linearization point. This happens repeatedly until the solver converges +// to a consistent set of variable values. This requires us to specify an initial guess +// for each variable, held in a Values container. +#include + +#include using namespace std; using namespace gtsam; -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::L; - /* ************************************************************************* */ int main(int argc, char* argv[]) { - VisualSLAMExampleData data = VisualSLAMExampleData::generate(); + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - /* 1. Create graph *///using the 2D measurements (features) and the calibration data - visualSLAM::Graph graph; + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v - /* 2. Add factors to the graph */ - // 2a. Measurement factors - for (size_t i=0; i points; + points.push_back(gtsam::Point3(10.0,10.0,10.0)); + points.push_back(gtsam::Point3(-10.0,10.0,10.0)); + points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); + points.push_back(gtsam::Point3(10.0,-10.0,10.0)); + points.push_back(gtsam::Point3(10.0,10.0,-10.0)); + points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); + points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); + points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); + + // Create the set of ground-truth poses + std::vector poses; + double radius = 30.0; + int i = 0; + double theta = 0.0; + gtsam::Point3 up(0,0,1); + gtsam::Point3 target(0,0,0); + for(; i < 8; ++i, theta += 2*M_PI/8) { + gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0); + gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up); + poses.push_back(camera.pose()); } - // 2b. Prior factor for the first pose and point to constraint the system - graph.addPosePrior(X(0), data.poses[0], data.noiseX); - graph.addPointPrior(L(0), data.points[0], data.noiseL); - /* 3. Initial estimates for variable nodes, simulated by Gaussian noises */ - Values initial; - for (size_t i=0; isample())*/); // you can add noise if you want - for (size_t j=0; jsample())*/); // you can add noise if you want - initial.print("Intial Estimates: "); + // Create a factor graph + NonlinearFactorGraph graph; - /* 4. Optimize the graph and print results */ - visualSLAM::Values result = GaussNewtonOptimizer(graph, initial).optimize(); -// visualSLAM::Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); - result.print("Final results: "); + // 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 + + // 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)); + } + } + + // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained + // 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.print("Factor Graph:\n"); + + // Create the data structure to hold the initialEstimate estimate to the solution + // Intentionally initialize the variables off from the ground truth + Values initialEstimate; + for (size_t i = 0; i < poses.size(); ++i) + initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + for (size_t j = 0; j < points.size(); ++j) + initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + initialEstimate.print("Initial Estimates:\n"); + + /* Optimize the graph and print results */ + Values result = DoglegOptimizer(graph, initialEstimate).optimize(); + result.print("Final results:\n"); return 0; }