diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 3c07fd4e3..a28ead5fe 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -66,12 +66,11 @@ using namespace gtsam; #include class UnaryFactor: public NoiseModelFactor1 { - // The factor will hold a measurement consisting of an (X,Y) location // We could this with a Point2 but here we just use two doubles double mx_, my_; -public: + public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -85,15 +84,15 @@ public: // The first is the 'evaluateError' function. This function implements the desired measurement // function, returning a vector of errors when evaluated at the provided variable value. It // must also calculate the Jacobians for this measurement function, if requested. - Vector evaluateError(const Pose2& q, boost::optional H = boost::none) const - { + Vector evaluateError(const Pose2& q, + boost::optional H = boost::none) const { // The measurement function for a GPS-like measurement is simple: // error_x = pose.x - measurement.x // error_y = pose.y - measurement.y // 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).finished(); + if (H) (*H) = (Matrix(2, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0).finished(); return (Vector(2) << q.x() - mx_, q.y() - my_).finished(); } @@ -107,29 +106,28 @@ public: // Additionally, we encourage you the use of unit testing your custom factors, // (as all GTSAM factors are), in which you would need an equals and print, to satisfy the // GTSAM_CONCEPT_TESTABLE_INST(T) defined in Testable.h, but these are not needed below. - -}; // UnaryFactor +}; // UnaryFactor int main(int argc, char** argv) { - // 1. Create a factor graph container and add factors to it NonlinearFactorGraph graph; // 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(Vector3(0.2, 0.2, 0.1)); + auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses graph.emplace_shared >(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); graph.emplace_shared >(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(Vector2(0.1, 0.1)); // 10cm std on x,y + auto unaryNoise = + noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y graph.emplace_shared(1, 0.0, 0.0, unaryNoise); graph.emplace_shared(2, 2.0, 0.0, unaryNoise); graph.emplace_shared(3, 4.0, 0.0, unaryNoise); - graph.print("\nFactor Graph:\n"); // print + graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution // For illustrative purposes, these have been deliberately set to incorrect values @@ -137,7 +135,7 @@ int main(int argc, char** argv) { initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)); initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); - initialEstimate.print("\nInitial Estimate:\n"); // print + initialEstimate.print("\nInitial Estimate:\n"); // print // 4. Optimize using Levenberg-Marquardt optimization. The optimizer // accepts an optional set of configuration parameters, controlling diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index bd25fdfdf..b91e113a4 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -11,7 +11,8 @@ /** * @file METISOrdering.cpp - * @brief Simple robot motion example, with prior and two odometry measurements, using a METIS ordering + * @brief Simple robot motion example, with prior and two odometry measurements, + * using a METIS ordering * @author Frank Dellaert * @author Andrew Melim */ @@ -22,11 +23,11 @@ */ #include -#include -#include #include #include +#include #include +#include using namespace std; using namespace gtsam; @@ -34,26 +35,26 @@ using namespace gtsam; int main(int argc, char** argv) { NonlinearFactorGraph graph; - Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin + auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.addPrior(1, priorMean, priorNoise); Pose2 odometry(2.0, 0.0, 0.0); - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.emplace_shared >(1, 2, odometry, odometryNoise); graph.emplace_shared >(2, 3, odometry, odometryNoise); - graph.print("\nFactor Graph:\n"); // print + graph.print("\nFactor Graph:\n"); // print 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, 0.1)); - initial.print("\nInitial Estimate:\n"); // print + initial.print("\nInitial Estimate:\n"); // print // optimize using Levenberg-Marquardt optimization LevenbergMarquardtParams params; - // In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType" - // By default this parameter is set to OrderingType::COLAMD + // In order to specify the ordering type, we need to se the "orderingType". By + // default this parameter is set to OrderingType::COLAMD params.orderingType = Ordering::METIS; LevenbergMarquardtOptimizer optimizer(graph, initial, params); Values result = optimizer.optimize(); diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index 2107b3244..1944d9e46 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -56,24 +56,23 @@ using namespace std; using namespace gtsam; int main(int argc, char** argv) { - // Create an empty nonlinear factor graph NonlinearFactorGraph graph; // 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(Vector3(0.3, 0.3, 0.1)); + Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin + auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.addPrior(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(Vector3(0.2, 0.2, 0.1)); + auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses graph.emplace_shared >(1, 2, odometry, odometryNoise); graph.emplace_shared >(2, 3, odometry, odometryNoise); - graph.print("\nFactor Graph:\n"); // print + graph.print("\nFactor Graph:\n"); // print // Create the data structure to hold the initialEstimate estimate to the solution // For illustrative purposes, these have been deliberately set to incorrect values @@ -81,7 +80,7 @@ int main(int argc, char** argv) { 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, 0.1)); - initial.print("\nInitial Estimate:\n"); // print + initial.print("\nInitial Estimate:\n"); // print // optimize using Levenberg-Marquardt optimization Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index 15b4cd2d1..ede27131d 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -69,35 +69,36 @@ using namespace std; using namespace gtsam; int main(int argc, char** argv) { - // Create a factor graph NonlinearFactorGraph graph; // Create the keys we need for this simple example - static Symbol x1('x',1), x2('x',2), x3('x',3); - static Symbol l1('l',1), l2('l',2); + static Symbol x1('x', 1), x2('x', 2), x3('x', 3); + static Symbol l1('l', 1), l2('l', 2); - // 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(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.addPrior(x1, prior, priorNoise); // add directly to graph + // 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 + auto priorNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta + graph.addPrior(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(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta + Pose2 odometry(2.0, 0.0, 0.0); + // create a measurement for both factors (the same in this case) + auto odometryNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta graph.emplace_shared >(x1, x2, odometry, odometryNoise); graph.emplace_shared >(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(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range + auto measurementNoise = noiseModel::Diagonal::Sigmas( + Vector2(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), + Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90), bearing32 = Rot2::fromDegrees(90); - double range11 = std::sqrt(4.0+4.0), - range21 = 2.0, - range32 = 2.0; + double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0; // Add Bearing-Range factors graph.emplace_shared >(x1, l1, bearing11, range11, measurementNoise); @@ -110,7 +111,7 @@ int main(int argc, char** argv) { // Create (deliberately inaccurate) initial estimate Values initialEstimate; initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); - initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); + initialEstimate.insert(x2, Pose2(2.3, 0.1, -0.2)); initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); initialEstimate.insert(l1, Point2(1.8, 2.1)); initialEstimate.insert(l2, Point2(4.1, 1.8)); @@ -138,4 +139,3 @@ int main(int argc, char** argv) { return 0; } - diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 57cb82c69..f3d555930 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -64,21 +64,20 @@ 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 // A prior factor consists of a mean and a noise model (covariance matrix) - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.addPrior(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(Vector3(0.2, 0.2, 0.1)); + auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors // Create odometry (Between) factors between consecutive poses - graph.emplace_shared >(1, 2, Pose2(2, 0, 0 ), model); + graph.emplace_shared >(1, 2, Pose2(2, 0, 0), model); graph.emplace_shared >(2, 3, Pose2(2, 0, M_PI_2), model); graph.emplace_shared >(3, 4, Pose2(2, 0, M_PI_2), model); graph.emplace_shared >(4, 5, Pose2(2, 0, M_PI_2), model); @@ -88,17 +87,17 @@ 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: graph.emplace_shared >(5, 2, Pose2(2, 0, M_PI_2), model); - graph.print("\nFactor Graph:\n"); // print + graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution // For illustrative purposes, these have been deliberately set to incorrect values Values initialEstimate; - initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); - initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 )); - initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2)); - initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI )); + initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)); + initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2)); + initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI)); initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2)); - initialEstimate.print("\nInitial Estimate:\n"); // print + initialEstimate.print("\nInitial Estimate:\n"); // print // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer // The optimizer accepts an optional set of configuration parameters, diff --git a/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp index c35b9bc45..41037d012 100644 --- a/examples/Pose2SLAMExampleExpressions.cpp +++ b/examples/Pose2SLAMExampleExpressions.cpp @@ -30,7 +30,6 @@ using namespace std; using namespace gtsam; int main(int argc, char** argv) { - // 1. Create a factor graph container and add factors to it ExpressionFactorGraph graph; @@ -38,31 +37,32 @@ int main(int argc, char** argv) { Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5); // 2a. Add a prior on the first pose, setting it to the origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.addExpressionFactor(x1, 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(Vector3(0.2, 0.2, 0.1)); + // For simplicity, we use the same noise model for odometry and loop closures + auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors - graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0 ), model); - graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model); - graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model); - graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model); + graph.addExpressionFactor(between(x1, x2), Pose2(2, 0, 0), model); + graph.addExpressionFactor(between(x2, x3), Pose2(2, 0, M_PI_2), model); + graph.addExpressionFactor(between(x3, x4), Pose2(2, 0, M_PI_2), model); + graph.addExpressionFactor(between(x4, x5), Pose2(2, 0, M_PI_2), model); // 2c. Add the loop closure constraint - graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model); - graph.print("\nFactor Graph:\n"); // print + graph.addExpressionFactor(between(x5, x2), 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 - // For illustrative purposes, these have been deliberately set to incorrect values + // 3. Create the data structure to hold the initialEstimate estimate to the + // solution For illustrative purposes, these have been deliberately set to + // incorrect values Values initialEstimate; - initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); - initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 )); - initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2)); - initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI )); + initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)); + initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2)); + initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI)); initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2)); - initialEstimate.print("\nInitial Estimate:\n"); // print + initialEstimate.print("\nInitial Estimate:\n"); // print // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer GaussNewtonParams parameters; diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 738297230..fb3869a50 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -28,41 +28,41 @@ using namespace gtsam; // HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber) int main(const int argc, const char *argv[]) { - string kernelType = "none"; - int maxIterations = 100; // default - string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default + int maxIterations = 100; // default + string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default // Parse user's inputs - if (argc > 1){ - g2oFile = argv[1]; // input dataset filename - // outputFile = g2oFile = argv[2]; // done later + if (argc > 1) { + g2oFile = argv[1]; // input dataset filename } - if (argc > 3){ - maxIterations = atoi(argv[3]); // user can specify either tukey or huber + if (argc > 3) { + maxIterations = atoi(argv[3]); // user can specify either tukey or huber } - if (argc > 4){ - kernelType = argv[4]; // user can specify either tukey or huber + if (argc > 4) { + kernelType = argv[4]; // user can specify either tukey or huber } // reading file and creating factor graph NonlinearFactorGraph::shared_ptr graph; Values::shared_ptr initial; bool is3D = false; - if(kernelType.compare("none") == 0){ - boost::tie(graph, initial) = readG2o(g2oFile,is3D); + if (kernelType.compare("none") == 0) { + boost::tie(graph, initial) = readG2o(g2oFile, is3D); } - if(kernelType.compare("huber") == 0){ + if (kernelType.compare("huber") == 0) { std::cout << "Using robust kernel: huber " << std::endl; - boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER); + boost::tie(graph, initial) = + readG2o(g2oFile, is3D, KernelFunctionTypeHUBER); } - if(kernelType.compare("tukey") == 0){ + if (kernelType.compare("tukey") == 0) { std::cout << "Using robust kernel: tukey " << std::endl; - boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY); + boost::tie(graph, initial) = + readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY); } // Add prior on the pose having index (key) = 0 - noiseModel::Diagonal::shared_ptr priorModel = // + auto priorModel = // noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); graph->addPrior(0, Pose2(), priorModel); std::cout << "Adding prior on pose 0 " << std::endl; @@ -71,7 +71,8 @@ int main(const int argc, const char *argv[]) { params.setVerbosity("TERMINATION"); if (argc > 3) { params.maxIterations = maxIterations; - std::cout << "User required to perform maximum " << params.maxIterations << " iterations "<< std::endl; + std::cout << "User required to perform maximum " << params.maxIterations + << " iterations " << std::endl; } std::cout << "Optimizing the factor graph" << std::endl; @@ -79,8 +80,8 @@ int main(const int argc, const char *argv[]) { Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; - std::cout << "initial error=" <error(*initial)<< std::endl; - std::cout << "final error=" <error(result)<< std::endl; + std::cout << "initial error=" << graph->error(*initial) << std::endl; + std::cout << "final error=" << graph->error(result) << std::endl; if (argc < 3) { result.print("result"); diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 0a33f51aa..27d556725 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -25,20 +25,20 @@ using namespace std; using namespace gtsam; -int main (int argc, char** argv) { - +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(Vector3(0.3, 0.3, 0.1)); + auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.addPrior(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(Vector3(0.2, 0.2, 0.1)); + // For simplicity, we will use the same noise model for odometry and loop + // closures + auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors - graph.emplace_shared >(1, 2, Pose2(2, 0, 0 ), model); + graph.emplace_shared >(1, 2, Pose2(2, 0, 0), model); graph.emplace_shared >(2, 3, Pose2(2, 0, M_PI_2), model); graph.emplace_shared >(3, 4, Pose2(2, 0, M_PI_2), model); graph.emplace_shared >(4, 5, Pose2(2, 0, M_PI_2), model); @@ -49,10 +49,10 @@ int main (int argc, char** argv) { // 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(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 diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index 6bdc93e6f..1d02c64fa 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -28,7 +28,6 @@ using namespace std; using namespace gtsam; int main(const int argc, const char *argv[]) { - // Read graph from file string g2oFile; if (argc < 2) @@ -41,8 +40,7 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile); // Add prior on the pose having index (key) = 0 - noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); + auto priorModel = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); graph->addPrior(0, Pose2(), priorModel); graph->print(); diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 37559740b..cd74b2c07 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -28,45 +28,45 @@ 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 - Pose2 prior(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + Pose2 prior(0.0, 0.0, 0.0); // prior at origin + auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.addPrior(1, prior, priorNoise); // 2b. Add odometry factors - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.emplace_shared >(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.emplace_shared >(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.emplace_shared >(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph.emplace_shared >(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add the loop closure constraint - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph.emplace_shared >(5, 1, Pose2(0.0, 0.0, 0.0), model); - graph.print("\nFactor Graph:\n"); // print + auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + graph.emplace_shared >(5, 1, Pose2(0.0, 0.0, 0.0), + model); + graph.print("\nFactor Graph:\n"); // print - - // 3. Create the data structure to hold the initialEstimate estimate to the solution + // 3. Create the data structure to hold the initialEstimate estimate to the + // solution Values initialEstimate; initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1)); initialEstimate.insert(3, Pose2(2.1, 1.9, 2.8)); initialEstimate.insert(4, Pose2(-.3, 2.5, 4.2)); - initialEstimate.insert(5, Pose2(0.1,-0.7, 5.8)); - initialEstimate.print("\nInitial Estimate:\n"); // print + initialEstimate.insert(5, Pose2(0.1, -0.7, 5.8)); + initialEstimate.print("\nInitial Estimate:\n"); // print // 4. Single Step Optimization using Levenberg-Marquardt LevenbergMarquardtParams parameters; parameters.verbosity = NonlinearOptimizerParams::ERROR; parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; - // LM is still the outer optimization loop, but by specifying "Iterative" below - // We indicate that an iterative linear solver should be used. - // In addition, the *type* of the iterativeParams decides on the type of + // LM is still the outer optimization loop, but by specifying "Iterative" + // below We indicate that an iterative linear solver should be used. In + // addition, the *type* of the iterativeParams decides on the type of // iterative solver, in this case the SPCG (subgraph PCG) parameters.linearSolverType = NonlinearOptimizerParams::Iterative; parameters.iterativeParams = boost::make_shared(); diff --git a/examples/Pose3Localization.cpp b/examples/Pose3Localization.cpp index 512415221..1667b43d9 100644 --- a/examples/Pose3Localization.cpp +++ b/examples/Pose3Localization.cpp @@ -10,9 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @file Pose3SLAMExample_initializePose3.cpp + * @file Pose3Localization.cpp * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 - * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o + * Syntax for the script is ./Pose3Localization input.g2o output.g2o * @date Aug 25, 2014 * @author Luca Carlone */ @@ -26,8 +26,7 @@ using namespace std; using namespace gtsam; -int main(const int argc, const char *argv[]) { - +int main(const int argc, const char* argv[]) { // Read graph from file string g2oFile; if (argc < 2) @@ -41,10 +40,10 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile, is3D); // Add prior on the first key - noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); + auto priorModel = noiseModel::Diagonal::Variances( + (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - for(const Values::ConstKeyValuePair& key_value: *initial) { + for (const Values::ConstKeyValuePair& key_value : *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; graph->addPrior(firstKey, Pose3(), priorModel); @@ -53,13 +52,13 @@ int main(const int argc, const char *argv[]) { std::cout << "Optimizing the factor graph" << std::endl; GaussNewtonParams params; - params.setVerbosity("TERMINATION"); // this will show info about stopping conditions + params.setVerbosity("TERMINATION"); // show info about stopping conditions GaussNewtonOptimizer optimizer(*graph, *initial, params); Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; - std::cout << "initial error=" <error(*initial)<< std::endl; - std::cout << "final error=" <error(result)<< std::endl; + std::cout << "initial error=" << graph->error(*initial) << std::endl; + std::cout << "final error=" << graph->error(result) << std::endl; if (argc < 3) { result.print("result"); @@ -75,7 +74,7 @@ int main(const int argc, const char *argv[]) { // Calculate and print marginal covariances for all variables Marginals marginals(*graph, result); - for(const auto& key_value: result) { + for (const auto& key_value : result) { auto p = dynamic_cast*>(&key_value.value); if (!p) continue; std::cout << marginals.marginalCovariance(key_value.key) << endl; diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp index 06bf27850..1cca92f59 100644 --- a/examples/Pose3SLAMExample_g2o.cpp +++ b/examples/Pose3SLAMExample_g2o.cpp @@ -10,9 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @file Pose3SLAMExample_initializePose3.cpp + * @file Pose3SLAMExample_g2o.cpp * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 - * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o + * Syntax for the script is ./Pose3SLAMExample_g2o input.g2o output.g2o * @date Aug 25, 2014 * @author Luca Carlone */ @@ -25,8 +25,7 @@ using namespace std; using namespace gtsam; -int main(const int argc, const char *argv[]) { - +int main(const int argc, const char* argv[]) { // Read graph from file string g2oFile; if (argc < 2) @@ -40,10 +39,10 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile, is3D); // Add prior on the first key - noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); + auto priorModel = noiseModel::Diagonal::Variances( + (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - for(const Values::ConstKeyValuePair& key_value: *initial) { + for (const Values::ConstKeyValuePair& key_value : *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; graph->addPrior(firstKey, Pose3(), priorModel); @@ -52,13 +51,13 @@ int main(const int argc, const char *argv[]) { std::cout << "Optimizing the factor graph" << std::endl; GaussNewtonParams params; - params.setVerbosity("TERMINATION"); // this will show info about stopping conditions + params.setVerbosity("TERMINATION"); // show info about stopping conditions GaussNewtonOptimizer optimizer(*graph, *initial, params); Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; - std::cout << "initial error=" <error(*initial)<< std::endl; - std::cout << "final error=" <error(result)<< std::endl; + std::cout << "initial error=" << graph->error(*initial) << std::endl; + std::cout << "final error=" << graph->error(result) << std::endl; if (argc < 3) { result.print("result"); diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp index f93be8659..00a546bb2 100644 --- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp +++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp @@ -10,9 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @file Pose3SLAMExample_initializePose3.cpp + * @file Pose3SLAMExample_initializePose3Chordal.cpp * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 - * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o + * Syntax for the script is ./Pose3SLAMExample_initializePose3Chordal input.g2o output.g2o * @date Aug 25, 2014 * @author Luca Carlone */ @@ -25,8 +25,7 @@ using namespace std; using namespace gtsam; -int main(const int argc, const char *argv[]) { - +int main(const int argc, const char* argv[]) { // Read graph from file string g2oFile; if (argc < 2) @@ -40,10 +39,10 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile, is3D); // Add prior on the first key - noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); + auto priorModel = noiseModel::Diagonal::Variances( + (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - for(const Values::ConstKeyValuePair& key_value: *initial) { + for (const Values::ConstKeyValuePair& key_value : *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; graph->addPrior(firstKey, Pose3(), priorModel); @@ -58,7 +57,7 @@ int main(const int argc, const char *argv[]) { initialization.print("initialization"); } else { const string outputFile = argv[2]; - std::cout << "Writing results to file: " << outputFile << std::endl; + std::cout << "Writing results to file: " << outputFile << std::endl; NonlinearFactorGraph::shared_ptr graphNoKernel; Values::shared_ptr initial2; boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index 3b60cca39..2f92afb9e 100644 --- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -10,9 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @file Pose3SLAMExample_initializePose3.cpp + * @file Pose3SLAMExample_initializePose3Gradient.cpp * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 - * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o + * Syntax for the script is ./Pose3SLAMExample_initializePose3Gradient input.g2o output.g2o * @date Aug 25, 2014 * @author Luca Carlone */ @@ -25,8 +25,7 @@ using namespace std; using namespace gtsam; -int main(const int argc, const char *argv[]) { - +int main(const int argc, const char* argv[]) { // Read graph from file string g2oFile; if (argc < 2) @@ -40,10 +39,10 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile, is3D); // Add prior on the first key - noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); + auto priorModel = noiseModel::Diagonal::Variances( + (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - for(const Values::ConstKeyValuePair& key_value: *initial) { + for (const Values::ConstKeyValuePair& key_value : *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; graph->addPrior(firstKey, Pose3(), priorModel); @@ -52,17 +51,19 @@ int main(const int argc, const char *argv[]) { std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl; bool useGradient = true; - Values initialization = InitializePose3::initialize(*graph, *initial, useGradient); + Values initialization = + InitializePose3::initialize(*graph, *initial, useGradient); std::cout << "done!" << std::endl; - std::cout << "initial error=" <error(*initial)<< std::endl; - std::cout << "initialization error=" <error(initialization)<< std::endl; + std::cout << "initial error=" << graph->error(*initial) << std::endl; + std::cout << "initialization error=" << graph->error(initialization) + << std::endl; if (argc < 3) { initialization.print("initialization"); } else { const string outputFile = argv[2]; - std::cout << "Writing results to file: " << outputFile << std::endl; + std::cout << "Writing results to file: " << outputFile << std::endl; NonlinearFactorGraph::shared_ptr graphNoKernel; Values::shared_ptr initial2; boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 784893ed9..7f0c79e0e 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -56,12 +56,12 @@ 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 + auto measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks vector points = createPoints(); @@ -73,32 +73,45 @@ int main(int argc, char* argv[]) { 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.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.addPrior(Symbol('x', 0), poses[0], poseNoise); // add directly to graph + auto poseNoise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)) + .finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.addPrior(Symbol('x', 0), poses[0], poseNoise); // add directly to graph - // Simulated measurements from each camera pose, adding them to the factor graph + // Simulated measurements from each camera pose, adding them to the factor + // graph for (size_t i = 0; i < poses.size(); ++i) { PinholeCamera camera(poses[i], *K); for (size_t j = 0; j < points.size(); ++j) { Point2 measurement = camera.project(points[j]); - graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); + graph.emplace_shared >( + 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.addPrior(Symbol('l', 0), points[0], pointNoise); // add directly to graph + // 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. + auto pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); + graph.addPrior(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::Rodrigues(-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] + Point3(-0.25, 0.20, 0.15)); + for (size_t i = 0; i < poses.size(); ++i) { + auto corrupted_pose = poses[i].compose( + Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))); + initialEstimate.insert( + Symbol('x', i), corrupted_pose); + } + for (size_t j = 0; j < points.size(); ++j) { + auto corrupted_point = points[j] + Point3(-0.25, 0.20, 0.15); + initialEstimate.insert(Symbol('l', j), corrupted_point); + } initialEstimate.print("Initial Estimates:\n"); /* Optimize the graph and print results */ diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index 22af9492f..3768ee2a3 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -28,7 +28,7 @@ // Header order is close to far #include #include -#include // for loading BAL datasets ! +#include // for loading BAL datasets ! #include using namespace std; @@ -40,20 +40,16 @@ using symbol_shorthand::P; // An SfmCamera is defined in datase.h as a camera with unknown Cal3Bundler calibration // and has a total of 9 free parameters -/* ************************************************************************* */ 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]); + if (argc > 1) filename = string(argv[1]); // Load the SfM data from file SfmData mydata; readBAL(filename, mydata); - cout - << boost::format("read %1% tracks on %2% cameras\n") - % mydata.number_tracks() % mydata.number_cameras(); + cout << boost::format("read %1% tracks on %2% cameras\n") % + mydata.number_tracks() % mydata.number_cameras(); // Create a factor graph ExpressionFactorGraph graph; @@ -65,23 +61,23 @@ int main(int argc, char* argv[]) { Pose3_ pose0_(&SfmCamera::getPose, camera0_); // Finally, we say it should be equal to first guess graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(), - noiseModel::Isotropic::Sigma(6, 0.1)); + noiseModel::Isotropic::Sigma(6, 0.1)); // similarly, we create a prior on the first point Point3_ point0_(P(0)); graph.addExpressionFactor(point0_, mydata.tracks[0].p, - noiseModel::Isotropic::Sigma(3, 0.1)); + noiseModel::Isotropic::Sigma(3, 0.1)); // 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 + auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v - // Simulated measurements from each camera pose, adding them to the factor graph + // Simulated measurements from each camera pose, adding them to the factor + // graph size_t j = 0; - for(const SfmTrack& track: mydata.tracks) { + for (const SfmTrack& track : mydata.tracks) { // Leaf expression for j^th point Point3_ point_('p', j); - for(const SfmMeasurement& m: track.measurements) { + for (const SfmMeasurement& m : track.measurements) { size_t i = m.first; Point2 uv = m.second; // Leaf expression for i^th camera @@ -98,10 +94,8 @@ int main(int argc, char* argv[]) { Values initial; size_t i = 0; j = 0; - for(const SfmCamera& camera: mydata.cameras) - initial.insert(C(i++), camera); - for(const SfmTrack& track: mydata.tracks) - initial.insert(P(j++), track.p); + for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera); + for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p); /* Optimize the graph and print results */ Values result; @@ -117,5 +111,3 @@ int main(int argc, char* argv[]) { return 0; } -/* ************************************************************************* */ - diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index 4fcc1b297..86e6b8ae9 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -44,7 +44,7 @@ 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 = + auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks and poses @@ -80,7 +80,7 @@ int main(int argc, char* argv[]) { // Add a prior on pose x0. This indirectly specifies where the origin is. // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( + auto noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); graph.addPrior(0, poses[0], noise); diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index d29910da6..3f553efc6 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -35,13 +35,12 @@ typedef PinholePose Camera; /* ************************************************************************* */ 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 + auto measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks and poses vector points = createPoints(); @@ -52,17 +51,16 @@ int main(int argc, char* argv[]) { // Simulated measurements from each camera pose, adding them to the factor graph for (size_t j = 0; j < points.size(); ++j) { - - // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. + // every landmark represent a single landmark, we use shared pointer to init + // the factor, and then insert measurements. SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K)); for (size_t i = 0; i < poses.size(); ++i) { - // generate the 2D measurement Camera camera(poses[i], K); Point2 measurement = camera.project(points[j]); - // call add() function to add measurement into a single factor, here we need to add: + // call add() function to add measurement into a single factor smartfactor->add(measurement, i); } @@ -72,7 +70,7 @@ int main(int argc, char* argv[]) { // Add a prior on pose x0. This indirectly specifies where the origin is. // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( + auto noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); graph.addPrior(0, poses[0], noise); @@ -85,9 +83,9 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < poses.size(); ++i) initialEstimate.insert(i, poses[i].compose(delta)); - // We will use LM in the outer optimization loop, but by specifying "Iterative" below - // We indicate that an iterative linear solver should be used. - // In addition, the *type* of the iterativeParams decides on the type of + // We will use LM in the outer optimization loop, but by specifying + // "Iterative" below We indicate that an iterative linear solver should be + // used. In addition, the *type* of the iterativeParams decides on the type of // iterative solver, in this case the SPCG (subgraph PCG) LevenbergMarquardtParams parameters; parameters.linearSolverType = NonlinearOptimizerParams::Iterative; @@ -110,11 +108,10 @@ int main(int argc, char* argv[]) { result.print("Final results:\n"); Values landmark_result; for (size_t j = 0; j < points.size(); ++j) { - SmartFactor::shared_ptr smart = // - boost::dynamic_pointer_cast(graph[j]); + auto smart = boost::dynamic_pointer_cast(graph[j]); if (smart) { boost::optional point = smart->point(result); - if (point) // ignore if boost::optional return nullptr + if (point) // ignore if boost::optional return nullptr landmark_result.insert(j, *point); } } diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 187a150de..ffb5b195b 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -49,7 +49,7 @@ int main (int argc, char* argv[]) { NonlinearFactorGraph graph; // We share *one* noiseModel between all projection factors - noiseModel::Isotropic::shared_ptr noise = + auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Add measurements to the factor graph diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index d6b25c23f..b79a9fa28 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SFMExample.cpp + * @file SFMExample_bal_COLAMD_METIS.cpp * @brief This file is to compare the ordering performance for COLAMD vs METIS. * Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file. * @author Frank Dellaert, Zhaoyang Lv @@ -22,7 +22,7 @@ #include #include #include -#include // for loading BAL datasets ! +#include // for loading BAL datasets ! #include @@ -34,50 +34,52 @@ 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; +// 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[]) { +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]); + if (argc > 1) filename = string(argv[1]); // Load the SfM data from file SfmData mydata; readBAL(filename, mydata); - cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); + 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 + auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Add measurements to the factor graph size_t j = 0; - for(const SfmTrack& track: mydata.tracks) { - for(const SfmMeasurement& m: track.measurements) { + for (const SfmTrack& track : mydata.tracks) { + for (const SfmMeasurement& m : track.measurements) { size_t i = m.first; Point2 uv = m.second; - graph.emplace_shared(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P + graph.emplace_shared( + 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.addPrior(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); - graph.addPrior(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); + graph.addPrior(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + graph.addPrior(P(0), mydata.tracks[0].p, + noiseModel::Isotropic::Sigma(3, 0.1)); // Create initial estimate Values initial; - size_t i = 0; j = 0; - for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera); - for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p); + size_t i = 0; + j = 0; + for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera); + for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p); /** --------------- COMPARISON -----------------------**/ /** ----------------------------------------------------**/ @@ -98,7 +100,7 @@ int main (int argc, char* argv[]) { } // expect they have different ordering results - if(params_using_COLAMD.ordering == params_using_METIS.ordering) { + if (params_using_COLAMD.ordering == params_using_METIS.ordering) { cout << "COLAMD and METIS produce the same ordering. " << "Problem here!!!" << endl; } @@ -120,8 +122,7 @@ int main (int argc, char* argv[]) { cout << e.what(); } - - { // printing the result + { // printing the result cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl; cout << "METIS final error: " << graph.error(result_METIS) << endl; @@ -129,15 +130,13 @@ int main (int argc, char* argv[]) { cout << endl << endl; cout << "Time comparison by solving " << filename << " results:" << endl; - cout << boost::format("%1% point tracks and %2% cameras\n") \ - % mydata.number_tracks() % mydata.number_cameras() \ + cout << boost::format("%1% point tracks and %2% cameras\n") % + mydata.number_tracks() % mydata.number_cameras() << endl; tictoc_print_(); } - return 0; } -/* ************************************************************************* */ diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index 4c88b7b4e..201860012 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -33,7 +33,7 @@ #include // SFM-specific factors -#include // does calibration ! +#include // does calibration ! // Standard headers #include @@ -41,9 +41,7 @@ using namespace std; using namespace gtsam; -/* ************************************************************************* */ int main(int argc, char* argv[]) { - // Create the set of ground-truth vector points = createPoints(); vector poses = createPoses(); @@ -52,28 +50,38 @@ int main(int argc, char* argv[]) { NonlinearFactorGraph graph; // Add a prior on pose x1. - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + auto poseNoise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)) + .finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw graph.addPrior(Symbol('x', 0), poses[0], poseNoise); - // Simulated measurements from each camera pose, adding them to the factor graph + // 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); + auto 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) { PinholeCamera 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.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0)); + // 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.emplace_shared >( + 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.addPrior(Symbol('l', 0), points[0], pointNoise); // add directly to graph + auto pointNoise = + noiseModel::Isotropic::Sigma(3, 0.1); + graph.addPrior(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).finished()); + auto calNoise = noiseModel::Diagonal::Sigmas( + (Vector(5) << 500, 500, 0.1, 100, 100).finished()); graph.addPrior(Symbol('K', 0), K, calNoise); // Create the initial estimate to the solution @@ -81,9 +89,12 @@ int main(int argc, char* argv[]) { 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::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + initialEstimate.insert( + Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-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] + Point3(-0.25, 0.20, 0.15)); + initialEstimate.insert(Symbol('l', j), + points[j] + Point3(-0.25, 0.20, 0.15)); /* Optimize the graph and print results */ Values result = DoglegOptimizer(graph, initialEstimate).optimize(); @@ -91,5 +102,4 @@ int main(int argc, char* argv[]) { return 0; } -/* ************************************************************************* */ diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index c444c85aa..19909e353 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -59,7 +59,6 @@ using namespace gtsam; const double degree = M_PI / 180; int main() { - /** * Step 1: Create a factor to express a unary constraint * The "prior" in this case is the measurement from a sensor, @@ -76,8 +75,8 @@ int main() { */ Rot2 prior = Rot2::fromAngle(30 * degree); prior.print("goal angle"); - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree); - Symbol key('x',1); + auto model = noiseModel::Isotropic::Sigma(1, 1 * degree); + Symbol key('x', 1); /** * Step 2: Create a graph container and add the factor to it diff --git a/examples/StereoVOExample.cpp b/examples/StereoVOExample.cpp index 65ab5422d..4e9415547 100644 --- a/examples/StereoVOExample.cpp +++ b/examples/StereoVOExample.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SteroVOExample.cpp + * @file StereoVOExample.cpp * @brief A stereo visual odometry example * @date May 25, 2014 * @author Stephen Camp @@ -34,17 +34,17 @@ using namespace std; using namespace gtsam; -int main(int argc, char** argv){ - - //create graph object, add first pose at origin with key '1' +int main(int argc, char** argv) { + // create graph object, add first pose at origin with key '1' NonlinearFactorGraph graph; Pose3 first_pose; graph.emplace_shared >(1, Pose3()); - //create factor noise model with 3 sigmas of value 1 - const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); - //create stereo camera calibration object with .2m between cameras - const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)); + // create factor noise model with 3 sigmas of value 1 + const auto model = noiseModel::Isotropic::Sigma(3, 1); + // create stereo camera calibration object with .2m between cameras + const Cal3_S2Stereo::shared_ptr K( + new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)); //create and add stereo factors between first pose (key value 1) and the three landmarks graph.emplace_shared >(StereoPoint2(520, 480, 440), model, 1, 3, K); @@ -56,17 +56,18 @@ int main(int argc, char** argv){ graph.emplace_shared >(StereoPoint2(70, 20, 490), model, 2, 4, K); graph.emplace_shared >(StereoPoint2(320, 270, 115), model, 2, 5, K); - //create Values object to contain initial estimates of camera poses and landmark locations + // create Values object to contain initial estimates of camera poses and + // landmark locations Values initial_estimate; - //create and add iniital estimates + // create and add iniital estimates initial_estimate.insert(1, first_pose); initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1))); initial_estimate.insert(3, Point3(1, 1, 5)); initial_estimate.insert(4, Point3(-1, 1, 5)); initial_estimate.insert(5, Point3(0, -0.5, 5)); - //create Levenberg-Marquardt optimizer for resulting factor graph, optimize + // create Levenberg-Marquardt optimizer for resulting factor graph, optimize LevenbergMarquardtOptimizer optimizer(graph, initial_estimate); Values result = optimizer.optimize(); diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index 5eeb6ac3c..af81db703 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** -* @file SteroVOExample.cpp +* @file StereoVOExample_large.cpp * @brief A stereo visual odometry example * @date May 25, 2014 * @author Stephen Camp @@ -41,31 +41,31 @@ using namespace std; using namespace gtsam; -int main(int argc, char** argv){ - +int main(int argc, char** argv) { Values initial_estimate; NonlinearFactorGraph graph; - const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); + const auto model = noiseModel::Isotropic::Sigma(3, 1); string calibration_loc = findExampleDataFile("VO_calibration.txt"); string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); - //read camera calibration info from file + // read camera calibration info from file // focal lengths fx, fy, skew s, principal point u0, v0, baseline b double fx, fy, s, u0, v0, b; ifstream calibration_file(calibration_loc.c_str()); cout << "Reading calibration info" << endl; calibration_file >> fx >> fy >> s >> u0 >> v0 >> b; - //create stereo camera calibration object - const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b)); + // create stereo camera calibration object + const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, s, u0, v0, b)); ifstream pose_file(pose_loc.c_str()); cout << "Reading camera poses" << endl; int pose_id; - MatrixRowMajor m(4,4); - //read camera pose parameters and use to make initial estimates of camera poses + MatrixRowMajor m(4, 4); + // read camera pose parameters and use to make initial estimates of camera + // poses while (pose_file >> pose_id) { for (int i = 0; i < 16; i++) { pose_file >> m.data()[i]; @@ -76,33 +76,37 @@ int main(int argc, char** argv){ // camera and landmark keys size_t x, l; - // pixel coordinates uL, uR, v (same for left/right images due to rectification) - // landmark coordinates X, Y, Z in camera frame, resulting from triangulation + // pixel coordinates uL, uR, v (same for left/right images due to + // rectification) landmark coordinates X, Y, Z in camera frame, resulting from + // triangulation double uL, uR, v, X, Y, Z; ifstream factor_file(factor_loc.c_str()); cout << "Reading stereo factors" << endl; - //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation + // read stereo measurement details from file and use to create and add + // GenericStereoFactor objects to the graph representation while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { - graph.emplace_shared< - GenericStereoFactor >(StereoPoint2(uL, uR, v), model, - Symbol('x', x), Symbol('l', l), K); - //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it + graph.emplace_shared >( + StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K); + // if the landmark variable included in this factor has not yet been added + // to the initial variable value estimate, add it if (!initial_estimate.exists(Symbol('l', l))) { Pose3 camPose = initial_estimate.at(Symbol('x', x)); - //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space + // transformFrom() transforms the input Point3 from the camera pose space, + // camPose, to the global space Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z)); initial_estimate.insert(Symbol('l', l), worldPoint); } } - Pose3 first_pose = initial_estimate.at(Symbol('x',1)); - //constrain the first pose such that it cannot change from its original value during optimization + Pose3 first_pose = initial_estimate.at(Symbol('x', 1)); + // constrain the first pose such that it cannot change from its original value + // during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.emplace_shared >(Symbol('x',1),first_pose); + graph.emplace_shared >(Symbol('x', 1), first_pose); cout << "Optimizing" << endl; - //create Levenberg-Marquardt optimizer to optimize the factor graph + // create Levenberg-Marquardt optimizer to optimize the factor graph LevenbergMarquardtParams params; params.orderingType = Ordering::METIS; LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params); diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index b09ad0f17..c18781169 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -56,13 +56,11 @@ 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 noise = // - noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks vector points = createPoints(); @@ -81,7 +79,6 @@ int main(int argc, char* argv[]) { // 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) { // Create ground truth measurement @@ -105,12 +102,12 @@ int main(int argc, char* argv[]) { // adding it to iSAM. 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( + auto poseNoise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); graph.addPrior(Symbol('x', 0), poses[0], poseNoise); // Add a prior on landmark l0 - noiseModel::Isotropic::shared_ptr pointNoise = + auto pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); graph.addPrior(Symbol('l', 0), points[0], pointNoise);