diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 1c38616a4..eae90e298 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -4,6 +4,16 @@ 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}/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/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 9ba415b7d..37ea57022 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -70,7 +70,7 @@ int main(int argc, char* argv[]) { /* 2. add factors to the graph */ // add measurement factors - SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector_(2, 0.5, 0.5)); + SharedDiagonal measurementNoise = Diagonal::Sigmas((Vector(2) << 0.5, 0.5)); boost::shared_ptr factor; graph.push_back( boost::make_shared(measurementNoise, X(1), calib, diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp new file mode 100644 index 000000000..b62b1c6fa --- /dev/null +++ b/examples/CreateSFMExampleData.cpp @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * 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 CreateSFMExampleData.cpp + * @brief Create some example data that for inclusion in the data folder + * @author Frank Dellaert + */ + +#include +#include +#include + +using namespace boost::assign; +using namespace std; +using namespace gtsam; + +void create5PointExample1() { + + // Class that will gather all data + SfM_data data; + + // Create two cameras and corresponding essential matrix E + Rot3 aRb = Rot3::yaw(M_PI_2); + Point3 aTb(0.1, 0, 0); + Pose3 identity, aPb(aRb, aTb); + data.cameras.push_back(SfM_Camera(identity)); + data.cameras.push_back(SfM_Camera(aPb)); + + // Create test data, we need at least 5 points + vector P; + P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // + Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5); + + BOOST_FOREACH(const Point3& p, P) { + + // Create the track + SfM_Track track; + track.p = p; + track.r = 1; + track.g = 1; + track.b = 1; + + // Project points in both cameras + for (size_t i = 0; i < 2; i++) + track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); + + // Add track to data + data.tracks.push_back(track); + } + + // Assumes example is run in ${GTSAM_TOP}/build/examples + const string filename = "../../examples/data/5pointExample1.txt"; + writeBAL(filename, data); +} + +int main(int argc, char* argv[]) { + create5PointExample1(); + return 0; +} + +/* ************************************************************************* */ + diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index dfe3d1e99..3ce3e0760 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -93,8 +93,8 @@ public: // Consequently, the Jacobians are: // [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0] // [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0] - if (H) (*H) = Matrix_(2,3, 1.0,0.0,0.0, 0.0,1.0,0.0); - return Vector_(2, q.x() - mx_, q.y() - my_); + if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0); + return (Vector(2) << q.x() - mx_, q.y() - my_); } // The second is a 'clone' function that allows the factor to be copied. Under most @@ -118,17 +118,17 @@ int main(int argc, char** argv) { // 2a. 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)); + 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)); // 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)); + noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // 10cm std on x,y + 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 d556798f0..70c6e6fb0 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -64,13 +64,13 @@ int main(int argc, char** argv) { // 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) 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)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); 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)); + 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)); diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index 88b8da3e0..ac8157be6 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -80,18 +80,18 @@ 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 + 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 // 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 + 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)); // Add Range-Bearing measurements to two different landmarks // create a noise model for the landmark measurements - noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range + noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range // create the measurement values - indices are (pose id, landmark id) Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90), diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index cbe97ae12..df924ff3e 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -71,11 +71,11 @@ 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)); + 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)); // 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)); + 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 diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 000a071da..d5e93a31b 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file Pose2SLAMExample_graph->cpp + * @file Pose2SLAMExample_graph.cpp * @brief Read graph from file and perform GraphSLAM * @date June 3, 2012 * @author Frank Dellaert @@ -20,28 +20,24 @@ #include #include #include -#include -#include #include -#include -#include using namespace std; using namespace gtsam; -int main(int argc, char** argv) { +int main (int argc, char** argv) { - // Read File and create graph and initial estimate + // Read File, create graph and initial estimate // we are in build/examples, data is in examples/Data - NonlinearFactorGraph::shared_ptr graph ; + NonlinearFactorGraph::shared_ptr graph; Values::shared_ptr initial; - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 5.0*M_PI/180.0)); - boost::tie(graph,initial) = load2D("../../examples/Data/w100-odom.graph",model); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0)); + boost::tie(graph, initial) = load2D("../../examples/Data/w100.graph", model); initial->print("Initial estimate:\n"); // 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)); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.01, 0.01, 0.01)); graph->add(PriorFactor(0, priorMean, priorNoise)); // Single Step Optimization using Levenberg-Marquardt @@ -53,5 +49,5 @@ int main(int argc, char** argv) { cout.precision(2); cout << "\nP3:\n" << marginals.marginalCovariance(99) << endl; -return 0; + return 0; } diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp new file mode 100644 index 000000000..0b318e7b3 --- /dev/null +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * 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 Pose2SLAMExample_graphviz.cpp + * @brief Save factor graph as graphviz dot file + * @date Sept 6, 2013 + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main (int argc, char** argv) { + + // 1. Create a factor graph container and add factors to it + NonlinearFactorGraph graph; + + // 2a. Add a prior on the first pose, setting it to the origin + 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)); + + // 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 + 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 + graph.add(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + + // 3. Create the data structure to hold the initial estimate to the solution + // For illustrative purposes, these have been deliberately set to incorrect values + Values initial; + initial.insert(1, Pose2(0.5, 0.0, 0.2 )); + initial.insert(2, Pose2(2.3, 0.1, -0.2 )); + initial.insert(3, Pose2(4.1, 0.1, M_PI_2)); + initial.insert(4, Pose2(4.0, 2.0, M_PI )); + initial.insert(5, Pose2(2.1, 2.1, -M_PI_2)); + + // Single Step Optimization using Levenberg-Marquardt + Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + + // save factor graph as graphviz dot file + // Render to PDF using "fdp Pose2SLAMExample.dot -Tpdf > graph.pdf" + ofstream os("Pose2SLAMExample.dot"); + graph.saveGraph(os, result); + + // Also print out to console + graph.saveGraph(cout, result); + + return 0; +} diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 36f2552a1..1c2952365 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -68,12 +68,12 @@ 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) 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)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); graph.add(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)); + 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)); @@ -85,7 +85,7 @@ int main(int argc, char** argv) { // 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, with the distance set to zero, - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + 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.print("\nFactor Graph:\n"); // print diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp new file mode 100644 index 000000000..c810cfe49 --- /dev/null +++ b/examples/SFMExample.cpp @@ -0,0 +1,118 @@ +/* ---------------------------------------------------------------------------- + + * 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 SFMExample.cpp + * @brief A structure-from-motion problem on a simulated dataset + * @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 + */ + +// For loading the data +#include "SFMdata.h" + +// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +#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 + +// 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; + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Define the camera calibration parameters + 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 + + // Create the set of ground-truth landmarks + vector points = createPoints(); + + // Create the set of ground-truth poses + vector poses = createPoses(); + + // Create a factor graph + NonlinearFactorGraph graph; + + // Add a prior on pose x1. This indirectly specifies where the origin is. + 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.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 initial 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; +} +/* ************************************************************************* */ + diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp new file mode 100644 index 000000000..672dbc86b --- /dev/null +++ b/examples/SFMExample_bal.cpp @@ -0,0 +1,94 @@ +/* ---------------------------------------------------------------------------- + + * 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 SFMExample.cpp + * @brief Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file + * @author Frank Dellaert + */ + +// For an explanation of headers, see SFMExample.cpp +#include +#include +#include +#include +#include +#include // for loading BAL datasets ! +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::C; +using symbol_shorthand::P; + +// We will be using a projection factor that ties a SFM_Camera to a 3D point. +// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration +// and has a total of 9 free parameters +typedef GeneralSFMFactor MyFactor; + +/* ************************************************************************* */ +int main (int argc, char* argv[]) { + + // Find default file, but if an argument is given, try loading a file + string filename = findExampleDataFile("dubrovnik-3-7-pre"); + if (argc>1) filename = string(argv[1]); + + // Load the SfM data from file + SfM_data mydata; + const bool success = readBAL(filename, mydata); + assert(success); + cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); + + // Create a factor graph + NonlinearFactorGraph graph; + + // We share *one* noiseModel between all projection factors + noiseModel::Isotropic::shared_ptr noise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Add measurements to the factor graph + size_t j = 0; + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) { + BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { + size_t i = m.first; + Point2 uv = m.second; + graph.add(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P + } + j += 1; + } + + // Add a prior on pose x1. This indirectly specifies where the origin is. + // and a prior on the position of the first landmark to fix the scale + graph.add(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); + graph.add(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); + + // Create initial estimate + Values initial; + size_t i = 0; j = 0; + BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera); + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p); + + /* Optimize the graph and print results */ + Values result; + try { + LevenbergMarquardtParams params; + params.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer lm(graph, initial, params); + result = lm.optimize(); + } catch (exception& e) { + cout << e.what(); + } + cout << "final error: " << graph.error(result) << endl; + + return 0; +} +/* ************************************************************************* */ + diff --git a/examples/SFMdata.h b/examples/SFMdata.h new file mode 100644 index 000000000..25442d527 --- /dev/null +++ b/examples/SFMdata.h @@ -0,0 +1,68 @@ +/* ---------------------------------------------------------------------------- + + * 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 SFMMdata.h + * @brief Simple example for the structure-from-motion problems + * @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 + +// We will also need a camera object to hold calibration information and perform projections. +#include + +/* ************************************************************************* */ +std::vector createPoints() { + + // 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)); + + return points; +} + +/* ************************************************************************* */ +std::vector createPoses() { + + // 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 = gtsam::Point3(radius*cos(theta), radius*sin(theta), 0.0); + gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(position, target, up); + poses.push_back(camera.pose()); + } + return poses; +} +/* ************************************************************************* */ diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp new file mode 100644 index 000000000..01a2300e3 --- /dev/null +++ b/examples/SelfCalibrationExample.cpp @@ -0,0 +1,96 @@ +/* ---------------------------------------------------------------------------- + + * 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 SelfCalibrationExample.cpp + * @brief Based on VisualSLAMExample, but with unknown (yet fixed) calibration. + * @author Frank Dellaert + */ + +/* + * See the detailed documentation in Visual SLAM. + * The only documentation below with deal with the self-calibration. + */ + +// For loading the data +#include "SFMdata.h" + +// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +#include + +// Inference and optimization +#include +#include +#include +#include + +// SFM-specific factors +#include +#include // does calibration ! + +// Standard headers +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Create the set of ground-truth + vector points = createPoints(); + vector poses = createPoses(); + + // Create the factor graph + NonlinearFactorGraph graph; + + // Add a prior on pose x1. + 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.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + + // Simulated measurements from each camera pose, adding them to the factor graph + Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); + noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); + 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]); + // The only real difference with the Visual SLAM example is that here we use a + // different factor type, that also calculates the Jacobian with respect to calibration + graph.add(GeneralSFMFactor2(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0))); + } + } + + // Add a prior on the position of the first landmark. + 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 a prior on the calibration. + noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100)); + graph.add(PriorFactor(Symbol('K', 0), K, calNoise)); + + // Create the initial estimate to the solution + // now including an estimate on the camera calibration parameters + Values initialEstimate; + initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0)); + 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))); + + /* Optimize the graph and print results */ + Values result = DoglegOptimizer(graph, initialEstimate).optimize(); + result.print("Final results:\n"); + + return 0; +} +/* ************************************************************************* */ + diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index f27664cfe..cb8dab2fb 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -22,14 +22,11 @@ * - 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. +// For loading the data +#include "SFMdata.h" + // 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). @@ -67,28 +64,10 @@ int main(int argc, char* argv[]) { noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // 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)); + vector points = createPoints(); // 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()); - } + vector poses = createPoses(); // Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization // and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter @@ -124,7 +103,7 @@ int main(int argc, char* argv[]) { // 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 + 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.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // Add a prior on landmark l0 diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 832a2ab83..6c4fe1d7e 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -22,14 +22,11 @@ * - 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. +// For loading the data +#include "SFMdata.h" + // 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). @@ -67,28 +64,10 @@ int main(int argc, char* argv[]) { noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // 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)); + vector points = createPoints(); // 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()); - } + vector poses = createPoses(); // Create a NonlinearISAM object which will relinearize and reorder the variables every "relinearizeInterval" updates int relinearizeInterval = 3; @@ -118,7 +97,7 @@ int main(int argc, char* argv[]) { // 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 + 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.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // Add a prior on landmark l0