diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 21fb36860..c9d232df8 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -15,87 +15,166 @@ * @author Frank Dellaert */ -// pull in the 2D PoseSLAM domain with all typedefs and helper functions defined -#include +/** + * A simple 2D pose slam example with "GPS" measurements + * - The robot moves forward 2 meter each iteration + * - The robot initially faces along the X axis (horizontal, to the right in 2D) + * - We have full odometry between pose + * - We have "GPS-like" measurements implemented with a custom factor + */ -// include this for marginals +// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent +// the robot positions +#include + +// Each variable in the system (poses) 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 simple integer keys +#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 Between factors for the relative motion described by odometry measurements. +// Because we have global measurements in the form of "GPS-like" measurements, we don't +// actually need to provide an initial position prior in this example. We will create our +// custom factor shortly. +#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 + +// 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 + +// 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 the +// standard Levenberg-Marquardt solver #include + +// Once the optimized values have been calculated, we can also calculate the marginal covariance +// of desired variables #include -#include -#include using namespace std; using namespace gtsam; -using namespace gtsam::noiseModel; -/** - * UnaryFactor - * Example on how to create a GPS-like factor on position alone. - */ + +// Before we begin the example, we must create a custom unary factor to implement a +// "GPS-like" functionality. Because standard GPS measurements provide information +// only on the position, and not on the orientation, we cannot use a simple prior to +// properly model this measurement. +// +// The factor will be a unary factor, affect only a single system variable. It will +// also use a standard Gaussian noise model. Hence, we will derive our new factor from +// the NoiseModelFactor1. +#include class UnaryFactor: public NoiseModelFactor1 { - double mx_, my_; ///< X and Y measurements + + // The factor will hold a measurement consisting of an (X,Y) location + Point2 measurement_; public: + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + // The constructor requires the variable key, the (X, Y) measurement value, and the noise model UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model): - NoiseModelFactor1(model, j), mx_(x), my_(y) {} + NoiseModelFactor1(model, j), measurement_(x, y) {} virtual ~UnaryFactor() {} - Vector evaluateError(const Pose2& q, - boost::optional H = boost::none) const + // By using the NoiseModelFactor base classes, the only two function that must be overridden. + // 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& pose, boost::optional H = boost::none) const { - 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 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); + + return Vector_(2, pose.x() - measurement_.x(), pose.y() - measurement_.y()); } + + // The second is a 'clone' function that allows the factor to be copied. Under most + // circumstances, the following code that employs the default copy constructor should + // work fine. + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); } + + // Additionally, custom factors should really provide specific implementations of + // 'equals' to ensure proper operation will all GTSAM functionality, and a custom + // 'print' function, if desired. + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const UnaryFactor* e = dynamic_cast (&expected); + return e != NULL && NoiseModelFactor1::equals(*e, tol) && this->measurement_.equals(e->measurement_, tol); + } + + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "UnaryFactor(" << keyFormatter(this->key()) << ")\n"; + measurement_.print(" measurement: "); + this->noiseModel_->print(" noise model: "); + } + }; -/** - * Example of a more complex 2D localization example - * - Robot poses are facing along the X axis (horizontal, to the right in 2D) - * - The robot moves 2 meters each step - * - We have full odometry between poses - * - We have unary measurement factors at eacht time step - */ + + int main(int argc, char** argv) { - // create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) - pose2SLAM::Graph graph; + // 1. Create a factor graph container and add factors to it + NonlinearFactorGraph graph; - // add two odometry factors - Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.addRelativePose(1, 2, odometry, odometryNoise); - graph.addRelativePose(2, 3, odometry, odometryNoise); + // 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)); + // 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)); - // add unary measurement factors, like GPS, on all three poses - SharedDiagonal noiseModel = Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // 10cm std on x,y - graph.push_back(boost::make_shared(1, 0, 0, noiseModel)); - graph.push_back(boost::make_shared(2, 2, 0, noiseModel)); - graph.push_back(boost::make_shared(3, 4, 0, noiseModel)); + // 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.add(UnaryFactor(1, 0.0, 0.0, unaryNoise)); + graph.add(UnaryFactor(3, 4.0, 0.0, unaryNoise)); + graph.print("\nFactor Graph:\n"); // print - // print - graph.print("\nFactor graph:\n"); + // 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, 0.1)); + initialEstimate.print("\nInitial Estimate:\n"); // print - // create (deliberatly inaccurate) initial estimate - pose2SLAM::Values initialEstimate; - initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); - initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2)); - initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); - initialEstimate.print("\nInitial estimate:\n "); + // 4. Optimize using Levenberg-Marquardt optimization. The optimizer + // accepts an optional set of configuration parameters, controlling + // things like convergence criteria, the type of linear system solver + // to use, and the amount of information displayed during optimization. + // Here we will use the default set of parameters. See the + // documentation for the full set of parameters. + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); - // use an explicit Optimizer object - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); - pose2SLAM::Values result = optimizer.optimize(); - result.print("\nFinal result:\n "); + // 5. Calculate and print marginal covariances for all variables + Marginals marginals(graph, result); + cout << "Pose 1 covariance:\n" << marginals.marginalCovariance(1) << endl; + cout << "Pose 2 covariance:\n" << marginals.marginalCovariance(2) << endl; + cout << "Pose 3 covariance:\n" << marginals.marginalCovariance(3) << endl; - // Query the marginals - Marginals marginals(graph, result); - cout.precision(2); - cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl; - cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl; - cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl; - - return 0; + return 0; } - diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index 36ad5f228..f0874bedb 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -15,62 +15,91 @@ * @author Frank Dellaert */ -// pull in the 2D PoseSLAM domain with all typedefs and helper functions defined -#include - -// include this for marginals -#include - -#include -#include - -using namespace std; -using namespace gtsam; -using namespace gtsam::noiseModel; - /** * Example of a simple 2D localization example * - Robot poses are facing along the X axis (horizontal, to the right in 2D) * - The robot moves 2 meters each step * - We have full odometry between poses */ + +// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent +// the robot positions +#include +#include + +// Each variable in the system (poses) 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 Between factors for the relative motion described by odometry measurements. +// Also, we will initialize the robot at the origin 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 the +// Levenberg-Marquardt solver +#include + +// Once the optimized values have been calculated, we can also calculate the marginal covariance +// of desired variables +#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 + + +using namespace std; +using namespace gtsam; + int main(int argc, char** argv) { - // create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) - pose2SLAM::Graph graph; + // Create a factor graph container + NonlinearFactorGraph graph; - // add a Gaussian prior on pose x_1 - Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin - SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.addPosePrior(1, priorMean, priorNoise); // add directly to 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 prior(0.0, 0.0, 0.0); // prior at origin + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + graph.add(PriorFactor(Symbol('x', 1), prior, priorNoise)); - // add two odometry factors - Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.addRelativePose(1, 2, odometry, odometryNoise); - graph.addRelativePose(2, 3, odometry, odometryNoise); + // Add odometry factors + // For simplicity, we will use the same noise model for each odometry factor + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + // Create odometry (Between) factors between consecutive poses + graph.add(BetweenFactor(Symbol('x', 1), Symbol('x', 2), Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.add(BetweenFactor(Symbol('x', 2), Symbol('x', 3), Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.print("\nFactor Graph:\n"); // print - // print - graph.print("\nFactor graph:\n"); + // 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(Symbol('x', 1), Pose2(0.5, 0.0, 0.2)); + initialEstimate.insert(Symbol('x', 2), Pose2(2.3, 0.1, -0.2)); + initialEstimate.insert(Symbol('x', 3), Pose2(4.1, 0.1, 0.1)); + initialEstimate.print("\nInitial Estimate:\n"); // print - // create (deliberatly inaccurate) initial estimate - pose2SLAM::Values initialEstimate; - initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); - initialEstimate.insertPose(2, Pose2(2.3, 0.1, -0.2)); - initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); - initialEstimate.print("\nInitial estimate:\n "); + // optimize using Levenberg-Marquardt optimization + Values result = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); + result.print("Final Result:\n"); - // optimize using Levenberg-Marquardt optimization with an ordering from colamd - pose2SLAM::Values result = graph.optimize(initialEstimate); - result.print("\nFinal result:\n "); + // Calculate and print marginal covariances for all variables + Marginals marginals(graph, result); + cout << "Pose 1 covariance:\n" << marginals.marginalCovariance(Symbol('x', 1)) << endl; + cout << "Pose 2 covariance:\n" << marginals.marginalCovariance(Symbol('x', 2)) << endl; + cout << "Pose 3 covariance:\n" << marginals.marginalCovariance(Symbol('x', 3)) << endl; - // Query the marginals - cout.precision(2); - Marginals marginals = graph.marginals(result); - cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl; - cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl; - cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl; - - return 0; + return 0; } - diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index f05ffa9b2..c6f6b636f 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -11,22 +11,12 @@ /** * @file PlanarSLAMExample.cpp - * @brief Simple robotics example using the pre-built planar SLAM domain + * @brief Simple robotics example using odometry measurements and bearing-range (laser) measurements * @author Alex Cunningham */ -// pull in the planar SLAM domain with all typedefs and helper functions defined -#include - -// we will use Symbol keys -#include - -using namespace std; -using namespace gtsam; -using namespace gtsam::noiseModel; - /** - * Example of a simple 2D planar slam example with landmarls + * A simple 2D planar slam example with landmarks * - The robot and landmarks are on a 2 meter grid * - Robot poses are facing along the X axis (horizontal, to the right in 2D) * - The robot moves 2 meters each step @@ -34,29 +24,74 @@ using namespace gtsam::noiseModel; * - We have bearing and range information for measurements * - Landmarks are 2 meters away from the robot trajectory */ + +// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent +// the robot positions and Point2 variables (x, y) to represent the landmark coordinates. +#include +#include + +// Each variable in the system (poses and landmarks) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use Symbols +#include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use a RangeBearing factor for the range-bearing measurements to identified +// landmarks, and Between factors for the relative motion described by odometry measurements. +// Also, we will initialize the robot at the origin using a Prior factor. +#include +#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 the +// common Levenberg-Marquardt solver +#include + +// Once the optimized values have been calculated, we can also calculate the marginal covariance +// of desired variables +#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 + + +using namespace std; +using namespace gtsam; + int main(int argc, char** argv) { - // create the graph (defined in planarSlam.h, derived from NonlinearFactorGraph) - planarSLAM::Graph graph; + // Create a factor graph + NonlinearFactorGraph graph; - // Create some keys - static Symbol i1('x',1), i2('x',2), i3('x',3); - static Symbol j1('l',1), j2('l',2); + // 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); - // add a Gaussian prior on pose x_1 - Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin - SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.addPosePrior(i1, priorMean, 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 + 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 + // Add two odometry factors Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.addRelativePose(i1, i2, odometry, odometryNoise); - graph.addRelativePose(i2, i3, odometry, odometryNoise); + 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 - SharedDiagonal measurementNoise = 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), @@ -65,27 +100,42 @@ int main(int argc, char** argv) { range21 = 2.0, range32 = 2.0; - // add bearing/range factors (created by "addBearingRange") - graph.addBearingRange(i1, j1, bearing11, range11, measurementNoise); - graph.addBearingRange(i2, j1, bearing21, range21, measurementNoise); - graph.addBearingRange(i3, j2, bearing32, range32, measurementNoise); + // Add Bearing-Range factors + graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); + graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); + graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); - // print - graph.print("Factor graph"); + // Print + graph.print("Factor Graph:\n"); - // create (deliberatly inaccurate) initial estimate - planarSLAM::Values initialEstimate; - initialEstimate.insertPose(i1, Pose2(0.5, 0.0, 0.2)); - initialEstimate.insertPose(i2, Pose2(2.3, 0.1,-0.2)); - initialEstimate.insertPose(i3, Pose2(4.1, 0.1, 0.1)); - initialEstimate.insertPoint(j1, Point2(1.8, 2.1)); - initialEstimate.insertPoint(j2, Point2(4.1, 1.8)); + // 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(x3, Pose2(4.1, 0.1, 0.1)); + initialEstimate.insert(l1, Point2(1.8, 2.1)); + initialEstimate.insert(l2, Point2(4.1, 1.8)); - initialEstimate.print("Initial estimate:\n "); + // Print + initialEstimate.print("Initial Estimate:\n"); - // optimize using Levenberg-Marquardt optimization with an ordering from colamd - planarSLAM::Values result = graph.optimize(initialEstimate); - result.print("Final result:\n "); + // Optimize using Levenberg-Marquardt optimization. The optimizer + // accepts an optional set of configuration parameters, controlling + // things like convergence criteria, the type of linear system solver + // to use, and the amount of information displayed during optimization. + // Here we will use the default set of parameters. See the + // documentation for the full set of parameters. + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + + // Calculate and print marginal covariances for all variables + Marginals marginals(graph, result); + print(marginals.marginalCovariance(x1), "x1 covariance"); + print(marginals.marginalCovariance(x2), "x2 covariance"); + print(marginals.marginalCovariance(x3), "x3 covariance"); + print(marginals.marginalCovariance(l1), "l1 covariance"); + print(marginals.marginalCovariance(l2), "l2 covariance"); return 0; } diff --git a/examples/PlanarSLAMExample_selfcontained.cpp b/examples/PlanarSLAMExample_selfcontained.cpp deleted file mode 100644 index 7c8ccc134..000000000 --- a/examples/PlanarSLAMExample_selfcontained.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file PlanarSLAMExample_selfcontained.cpp - * @brief Simple robotics example with all typedefs internal to this script. - * @author Alex Cunningham - */ - -// add in headers for specific factors -#include -#include -#include - -// for all nonlinear keys -#include - -// implementations for structures - needed if self-contained, and these should be included last -#include -#include -#include - -// for modeling measurement uncertainty - all models included here -#include - -// for points and poses -#include -#include - -#include -#include - -using namespace std; -using namespace gtsam; - -/** - * In this version of the system we make the following assumptions: - * - All values are axis aligned - * - Robot poses are facing along the X axis (horizontal, to the right in images) - * - We have bearing and range information for measurements - * - We have full odometry for measurements - * - The robot and landmarks are on a grid, moving 2 meters each step - * - Landmarks are 2 meters away from the robot trajectory - */ -int main(int argc, char** argv) { - // create keys for variables - Symbol i1('x',1), i2('x',2), i3('x',3); - Symbol j1('l',1), j2('l',2); - - // create graph container and add factors to it - NonlinearFactorGraph graph; - - /* add prior */ - // gaussian for prior - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - PriorFactor posePrior(i1, priorMean, priorNoise); // create the factor - graph.add(posePrior); // add the factor to the graph - - /* add odometry */ - // general noisemodel for odometry - SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - // create between factors to represent odometry - BetweenFactor odom12(i1, i2, odometry, odometryNoise); - BetweenFactor odom23(i2, i3, odometry, odometryNoise); - graph.add(odom12); // add both to graph - graph.add(odom23); - - /* add measurements */ - // general noisemodel for measurements - SharedDiagonal measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); - - // create the measurement values - indices are (pose id, landmark id) - Rot2 bearing11 = Rot2::fromDegrees(45), - bearing21 = Rot2::fromDegrees(90), - bearing32 = Rot2::fromDegrees(90); - double range11 = sqrt(4.0+4.0), - range21 = 2.0, - range32 = 2.0; - - // create bearing/range factors - BearingRangeFactor meas11(i1, j1, bearing11, range11, measurementNoise); - BearingRangeFactor meas21(i2, j1, bearing21, range21, measurementNoise); - BearingRangeFactor meas32(i3, j2, bearing32, range32, measurementNoise); - - // add the factors - graph.add(meas11); - graph.add(meas21); - graph.add(meas32); - - graph.print("Full Graph"); - - // initialize to noisy points - Values initial; - initial.insert(i1, Pose2(0.5, 0.0, 0.2)); - initial.insert(i2, Pose2(2.3, 0.1,-0.2)); - initial.insert(i3, Pose2(4.1, 0.1, 0.1)); - initial.insert(j1, Point2(1.8, 2.1)); - initial.insert(j2, Point2(4.1, 1.8)); - - initial.print("initial estimate"); - - // optimize using Levenberg-Marquardt optimization with an ordering from colamd - - // first using sequential elimination - LevenbergMarquardtParams lmParams; - lmParams.linearSolverType = LevenbergMarquardtParams::SEQUENTIAL_CHOLESKY; - Values resultSequential = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize(); - resultSequential.print("final result (solved with a sequential solver)"); - - // then using multifrontal, advanced interface - // Note that we keep the original optimizer object so we can use the COLAMD - // ordering it computes. - LevenbergMarquardtOptimizer optimizer(graph, initial); - Values resultMultifrontal = optimizer.optimize(); - resultMultifrontal.print("final result (solved with a multifrontal solver)"); - - // Print marginals covariances for all variables - Marginals marginals(graph, resultMultifrontal, Marginals::CHOLESKY); - print(marginals.marginalCovariance(i1), "i1 covariance"); - print(marginals.marginalCovariance(i2), "i2 covariance"); - print(marginals.marginalCovariance(i3), "i3 covariance"); - print(marginals.marginalCovariance(j1), "j1 covariance"); - print(marginals.marginalCovariance(j2), "j2 covariance"); - - return 0; -} - diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 0777301ea..bf05cc729 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -11,55 +11,126 @@ /** * @file Pose2SLAMExample.cpp - * @brief A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h + * @brief A 2D Pose SLAM example * @date Oct 21, 2010 * @author Yong Dian Jian */ -// pull in the Pose2 SLAM domain with all typedefs and helper functions defined -#include -#include +/** + * A simple 2D pose slam example + * - The robot moves in a 2 meter square + * - The robot moves 2 meters each step, turning 90 degrees after each step + * - The robot initially faces along the X axis (horizontal, to the right in 2D) + * - We have full odometry between pose + * - We have a loop closure constraint when the robot returns to the first position + */ + +// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent +// the robot positions +#include +#include + +// Each variable in the system (poses) 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 simple integer keys +#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 Between factors for the relative motion described by odometry measurements. +// We will also use a Between Factor to encode the loop closure constraint +// Also, we will initialize the robot at the origin 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 the +// a Gauss-Newton solver +#include + +// Once the optimized values have been calculated, we can also calculate the marginal covariance +// of desired variables +#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 + using namespace std; using namespace gtsam; -using namespace gtsam::noiseModel; int main(int argc, char** argv) { - // 1. Create graph container and add factors to it - pose2SLAM::Graph graph; + // 1. Create a factor graph container and add factors to it + NonlinearFactorGraph graph; - // 2a. Add Gaussian prior - Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - graph.addPosePrior(1, priorMean, priorNoise); + // 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)); + graph.add(PriorFactor(1, prior, priorNoise)); // 2b. Add odometry factors - SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); - graph.addRelativePose(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addRelativePose(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addRelativePose(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + // For simplicity, we will use the same noise model for each odometry factor + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + // Create odometry (Between) factors between consecutive poses + graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - // 2c. Add pose constraint - SharedDiagonal model = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addRelativePose(5, 2, Pose2(2.0, 0.0, M_PI_2), model); + // 2c. Add the loop closure constraint + // This factor encodes the fact that we have returned to the same pose. In real systems, + // these constraints may be identified in many ways, such as appearance-based techniques + // with camera images. + // We will use another Between Factor to enforce this constraint, with the distance set to zero, + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + graph.add(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); + graph.print("\nFactor Graph:\n"); // print - // print - graph.print("\nFactor graph:\n"); // 3. Create the data structure to hold the initialEstimate estimate to the solution - pose2SLAM::Values initialEstimate; - initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); - initialEstimate.insertPose(2, Pose2(2.3, 0.1, -0.2)); - initialEstimate.insertPose(3, Pose2(4.1, 0.1, M_PI_2)); - initialEstimate.insertPose(4, Pose2(4.0, 2.0, M_PI)); - initialEstimate.insertPose(5, Pose2(2.1, 2.1, -M_PI_2)); - initialEstimate.print("\nInitial estimate:\n"); + // 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, 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 - // 4. Single Step Optimization using Levenberg-Marquardt - pose2SLAM::Values result = graph.optimize(initialEstimate); - result.print("\nFinal result:\n"); + // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer + // The optimizer accepts an optional set of configuration parameters, + // controlling things like convergence criteria, the type of linear + // system solver to use, and the amount of information displayed during + // optimization. We will set a few parameters as a demonstration. + GaussNewtonParams parameters; + // Stop iterating once the change in error between steps is less than this value + parameters.relativeErrorTol = 1e-5; + // Do not perform more than N iteration steps + parameters.maxIterations = 100; + // Create the optimizer ... + GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters); + // ... and optimize + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + + // 5. Calculate and print marginal covariances for all variables + Marginals marginals(graph, result); + cout << "Pose 1 covariance:\n" << marginals.marginalCovariance(1) << endl; + cout << "Pose 2 covariance:\n" << marginals.marginalCovariance(2) << endl; + cout << "Pose 3 covariance:\n" << marginals.marginalCovariance(3) << endl; + cout << "Pose 4 covariance:\n" << marginals.marginalCovariance(4) << endl; + cout << "Pose 5 covariance:\n" << marginals.marginalCovariance(5) << endl; return 0; } diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp deleted file mode 100644 index b5fcddfcf..000000000 --- a/examples/Pose2SLAMExample_advanced.cpp +++ /dev/null @@ -1,82 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Pose2SLAMExample_advanced.cpp - * @brief Simple Pose2SLAM Example using - * pre-built pose2SLAM domain - * @author Chris Beall - */ - -// pull in the Pose2 SLAM domain with all typedefs and helper functions defined -#include -#include -#include -#include -#include - -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace gtsam::noiseModel; - -int main(int argc, char** argv) { - /* 1. create graph container and add factors to it */ - pose2SLAM::Graph graph; - - /* 2.a add prior */ - Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.addPosePrior(1, priorMean, priorNoise); // add directly to graph - - /* 2.b add odometry */ - SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 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) - graph.addRelativePose(1, 2, odometry, odometryNoise); - graph.addRelativePose(2, 3, odometry, odometryNoise); - graph.print("full graph"); - - /* 3. Create the data structure to hold the initial estimate to the solution - * initialize to noisy points */ - pose2SLAM::Values initial; - initial.insertPose(1, Pose2(0.5, 0.0, 0.2)); - initial.insertPose(2, Pose2(2.3, 0.1, -0.2)); - initial.insertPose(3, Pose2(4.1, 0.1, 0.1)); - initial.print("initial estimate"); - - /* 4.2.1 Alternatively, you can go through the process step by step - * Choose an ordering */ - Ordering ordering = *graph.orderingCOLAMD(initial); - - /* 4.2.2 set up solver and optimize */ - LevenbergMarquardtParams params; - params.absoluteErrorTol = 1e-15; - params.relativeErrorTol = 1e-15; - params.ordering = ordering; - LevenbergMarquardtOptimizer optimizer(graph, initial, params); - - pose2SLAM::Values result = optimizer.optimize(); - result.print("final result"); - - /* Get covariances */ - Marginals marginals(graph, result, Marginals::CHOLESKY); - Matrix covariance1 = marginals.marginalCovariance(1); - Matrix covariance2 = marginals.marginalCovariance(2); - - print(covariance1, "Covariance1"); - print(covariance2, "Covariance2"); - - return 0; -} - diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index d27e184d8..cc873eb08 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -17,32 +17,35 @@ */ #include -#include +#include #include +#include +#include +#include +#include #include #include using namespace std; using namespace gtsam; -using namespace gtsam::noiseModel; int main(int argc, char** argv) { // Read File and create graph and initial estimate // we are in build/examples, data is in examples/Data - pose2SLAM::Graph::shared_ptr graph ; - pose2SLAM::Values::shared_ptr initial; - SharedDiagonal model = Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 5.0*M_PI/180.0)); + 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); 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 = Diagonal::Sigmas(Vector_(3, 0.01, 0.01, 0.01)); - graph->addPosePrior(0, priorMean, priorNoise); + 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 - pose2SLAM::Values result = graph->optimize(*initial); + Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); result.print("\nFinal result:\n"); // Plot the covariance of the last pose diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index e2f3801f7..240361db7 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -16,79 +16,115 @@ * @date June 2, 2012 */ +/** + * A simple 2D pose slam example solved using a Conjugate-Gradient method + * - The robot moves in a 2 meter square + * - The robot moves 2 meters each step, turning 90 degrees after each step + * - The robot initially faces along the X axis (horizontal, to the right in 2D) + * - We have full odometry between pose + * - We have a loop closure constraint when the robot returns to the first position + */ + +// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent +// the robot positions +#include +#include + +// Each variable in the system (poses) 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 simple integer keys +#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 Between factors for the relative motion described by odometry measurements. +// We will also use a Between Factor to encode the loop closure constraint +// Also, we will initialize the robot at the origin 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 + +// 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 #include #include -#include -#include -#include + using namespace std; using namespace gtsam; -using namespace gtsam::noiseModel; -/* ************************************************************************* */ -int main(void) { +int main(int argc, char** argv) { - // 1. Create graph container and add factors to it - pose2SLAM::Graph graph ; + // 1. Create a factor graph container and add factors to it + NonlinearFactorGraph graph; - // 2a. Add Gaussian prior - Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - graph.addPosePrior(1, priorMean, priorNoise); + // 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)); + graph.add(PriorFactor(1, prior, priorNoise)); // 2b. Add odometry factors - SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); - graph.addRelativePose(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addRelativePose(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addRelativePose(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + // For simplicity, we will use the same noise model for each odometry factor + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + // Create odometry (Between) factors between consecutive poses + graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - // 2c. Add pose constraint - SharedDiagonal constraintUncertainty = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addRelativePose(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); + // 2c. Add the loop closure constraint + // This factor encodes the fact that we have returned to the same pose. In real systems, + // these constraints may be identified in many ways, such as appearance-based techniques + // with camera images. + // We will use another Between Factor to enforce this constraint, with the distance set to zero, + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + graph.add(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); + graph.print("\nFactor Graph:\n"); // print - // print - graph.print("\nFactor graph:\n"); - // 3. Create the data structure to hold the initialEstimate estinmate to the solution - pose2SLAM::Values initialEstimate; - Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1); - Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2); - Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insertPose(3, x3); - Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insertPose(4, x4); - Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insertPose(5, x5); - initialEstimate.print("\nInitial estimate:\n "); - cout << "initial error = " << graph.error(initialEstimate) << endl ; + // 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, 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 // 4. Single Step Optimization using Levenberg-Marquardt - LevenbergMarquardtParams param; - param.verbosity = NonlinearOptimizerParams::ERROR; - param.verbosityLM = LevenbergMarquardtParams::LAMBDA; - param.linearSolverType = SuccessiveLinearizationParams::CG; + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::ERROR; + parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; + parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT; { - param.iterativeParams = boost::make_shared(); - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, param); + parameters.iterativeParams = boost::make_shared(); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); Values result = optimizer.optimize(); - result.print("\nFinal result:\n"); + result.print("Final Result:\n"); cout << "simple spcg solver final error = " << graph.error(result) << endl; } { - param.iterativeParams = boost::make_shared(); - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, param); + parameters.iterativeParams = boost::make_shared(); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); Values result = optimizer.optimize(); - result.print("\nFinal result:\n"); + result.print("Final Result:\n"); cout << "subgraph solver final error = " << graph.error(result) << endl; } - { - Values result = graph.optimizeSPCG(initialEstimate); - result.print("\nFinal result:\n"); - } - - return 0 ; + return 0; } diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 603d345bf..cff9d754e 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -17,20 +17,41 @@ * @author Alex Cunningham */ -#include -#include -#include + /** + * This example will perform a relatively trivial optimization on + * a single variable with a single factor. + */ + +// In this example, a 2D rotation will be used as the variable of interest #include -#include + +// Each variable in the system (poses) 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. +// We will apply a simple prior on the rotation +#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 + +// 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 + +// 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 the +// standard Levenberg-Marquardt solver #include -/* - * TODO: make factors independent of RotValues - * TODO: make toplevel documentation - * TODO: Clean up nonlinear optimization API - */ using namespace std; using namespace gtsam; @@ -40,12 +61,7 @@ const double degree = M_PI / 180; int main() { /** - * This example will perform a relatively trivial optimization on - * a single variable with a single factor. - */ - - /** - * Step 1: create a factor on to express a unary constraint + * Step 1: Create a factor to express a unary constraint * The "prior" in this case is the measurement from a sensor, * with a model of the noise on the measurement. * @@ -60,12 +76,12 @@ int main() { */ Rot2 prior = Rot2::fromAngle(30 * degree); prior.print("goal angle"); - SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree); Symbol key('x',1); PriorFactor factor(key, prior, model); /** - * Step 2: create a graph container and add the factor to it + * Step 2: Create a graph container and add the factor to it * Before optimizing, all factors need to be added to a Graph container, * which provides the necessary top-level functionality for defining a * system of constraints. @@ -78,7 +94,7 @@ int main() { graph.print("full graph"); /** - * Step 3: create an initial estimate + * Step 3: Create an initial estimate * An initial estimate of the solution for the system is necessary to * start optimization. This system state is the "RotValues" structure, * which is similar in structure to a STL map, in that it maps @@ -98,7 +114,7 @@ int main() { initial.print("initial estimate"); /** - * Step 4: optimize + * Step 4: Optimize * After formulating the problem with a graph of constraints * and an initial estimate, executing optimization is as simple * as calling a general optimization function with the graph and diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp new file mode 100644 index 000000000..1485da1e4 --- /dev/null +++ b/examples/VisualISAM2Example.cpp @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------------- + + * 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 VisualISAM2Example.cpp + * @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset + * This version uses iSAM2 to solve the problem incrementally + * @author Duy-Nguyen Ta + */ + +/** + * A structure-from-motion example with landmarks + * - The landmarks form a 10 meter cube + * - The robot rotates around the landmarks, always facing towards the cube + */ + +// As this is a full 3D problem, we will use Pose3 variables to represent the camera +// positions and Point3 variables (x, y, z) to represent the landmark coordinates. +// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +// We will also need a camera object to hold calibration information and perform projections. +#include +#include +#include +#include + +// Each variable in the system (poses and landmarks) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use Symbols +#include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use Projection factors to model the camera's landmark observations. +// Also, we will initialize the robot at some location using a Prior factor. +#include +#include + +// We want to use iSAM2 to solve the structure-from-motion problem incrementally, so +// include iSAM2 here +#include + +// iSAM2 requires as input a set set of new factors to be added stored in a factor graph, +// and initial guesses for any new variables used in the added factors +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +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 + 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)); + + // 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()); + } + + // 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 + // structure is available that allows the user to set various properties, such as the relinearization threshold + // and type of linear solver. For this example, we we set the relinearization threshold small so the iSAM2 result + // will approach the batch result. + ISAM2Params parameters; + parameters.relinearizeThreshold = 0.01; + parameters.relinearizeSkip = 1; + ISAM2 isam(parameters); + + // Create a Factor Graph and Values to hold the new data + NonlinearFactorGraph graph; + Values initialEstimate; + + // Loop over the different poses, adding the observations to iSAM incrementally + for (size_t i = 0; i < poses.size(); ++i) { + + // Add factors for each landmark observation + for (size_t j = 0; j < points.size(); ++j) { + SimpleCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + graph.add(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + } + + // Add an initial guess for the current pose + // Intentionally initialize the variables off from the ground truth + initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + + // If this is the first iteration, add a prior on the first pose to set the coordinate frame + // and a prior on the first landmark to set the scale + // Also, as iSAM solves incrementally, we must wait until each is observed at least twice before + // adding it to iSAM. + if( i == 0) { + // Add a prior on pose x0 + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + + // Add a prior on landmark l0 + noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); + graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + + // Add initial guesses to all observed landmarks + // Intentionally initialize the variables off from the ground truth + for (size_t j = 0; j < points.size(); ++j) + initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + + } else { + // Update iSAM with the new factors + isam.update(graph, initialEstimate); + // Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver. + // If accuracy is desired at the expense of time, update(*) can be called additional times + // to perform multiple optimizer iterations every step. + isam.update(); + Values currentEstimate = isam.calculateEstimate(); + cout << "****************************************************" << endl; + cout << "Frame " << i << ": " << endl; + currentEstimate.print("Current estimate: "); + + // Clear the factor graph and values for the next iteration + graph.resize(0); + initialEstimate.clear(); + } + } + + return 0; +} +/* ************************************************************************* */ diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 25e993bca..832a2ab83 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -11,99 +11,139 @@ /** * @file VisualISAMExample.cpp - * @brief An ISAM example for synthesis sequence, single camera + * @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset + * This version uses iSAM to solve the problem incrementally * @author Duy-Nguyen Ta */ +/** + * A structure-from-motion example with landmarks + * - The landmarks form a 10 meter cube + * - The robot rotates around the landmarks, always facing towards the cube + */ + +// As this is a full 3D problem, we will use Pose3 variables to represent the camera +// positions and Point3 variables (x, y, z) to represent the landmark coordinates. +// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +// We will also need a camera object to hold calibration information and perform projections. +#include +#include +#include +#include + +// Each variable in the system (poses and landmarks) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use Symbols #include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use Projection factors to model the camera's landmark observations. +// Also, we will initialize the robot at some location using a Prior factor. +#include +#include + +// We want to use iSAM to solve the structure-from-motion problem incrementally, so +// include iSAM here #include -#include -#include -#include "VisualSLAMData.h" + +// iSAM requires as input a set set of new factors to be added stored in a factor graph, +// and initial guesses for any new variables used in the added factors +#include +#include + +#include using namespace std; using namespace gtsam; -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::L; - /* ************************************************************************* */ int main(int argc, char* argv[]) { - VisualSLAMExampleData data = VisualSLAMExampleData::generate(); + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - /* 1. Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates */ + // 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 + 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)); + + // 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()); + } + + // Create a NonlinearISAM object which will relinearize and reorder the variables every "relinearizeInterval" updates int relinearizeInterval = 3; NonlinearISAM isam(relinearizeInterval); - /* 2. At each frame (poseId) with new camera pose and set of associated measurements, - * create a graph of new factors and update ISAM */ + // Create a Factor Graph and Values to hold the new data + NonlinearFactorGraph graph; + Values initialEstimate; - // Store the current best estimate from ISAM - Values currentEstimate; + // Loop over the different poses, adding the observations to iSAM incrementally + for (size_t i = 0; i < poses.size(); ++i) { - // First two frames: - // Add factors and initial values for the first two poses and landmarks then update ISAM. - // Note: measurements from the first pose only are not enough to update ISAM: - // the system is underconstrained. - { - visualSLAM::Graph newFactors; + // Add factors for each landmark observation + for (size_t j = 0; j < points.size(); ++j) { + SimpleCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + graph.add(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + } - // First pose with prior factor - newFactors.addPosePrior(X(0), data.poses[0], data.noiseX); + // Add an initial guess for the current pose + // Intentionally initialize the variables off from the ground truth + initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); - // Second pose with odometry measurement - newFactors.addRelativePose(X(0), X(1), data.odometry, data.noiseX); + // If this is the first iteration, add a prior on the first pose to set the coordinate frame + // and a prior on the first landmark to set the scale + // Also, as iSAM solves incrementally, we must wait until each is observed at least twice before + // adding it to iSAM. + if( i == 0) { + // Add a prior on pose x0 + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); - // Visual measurements at both poses - for (size_t i=0; i<2; ++i) { - for (size_t j=0; j(Symbol('l', 0), points[0], pointNoise)); // add directly to graph - // Initial values for the first two poses, simulated with Gaussian noise - Values initials; - initials.insert(X(0), data.poses[0]); - initials.insert(X(1), data.poses[0]*data.odometry); + // Add initial guesses to all observed landmarks + // Intentionally initialize the variables off from the ground truth + for (size_t j = 0; j < points.size(); ++j) + initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); - // Initial values for the landmarks - for (size_t j=0; j(X(i-1))*data.odometry); - - // update ISAM - isam.update(newFactors, initials); - currentEstimate = isam.estimate(); - cout << "****************************************************" << endl; - cout << "Frame " << i << ": " << endl; - currentEstimate.print("Current estimate: "); + // Clear the factor graph and values for the next iteration + graph.resize(0); + initialEstimate.clear(); + } } return 0; } /* ************************************************************************* */ - diff --git a/examples/VisualSLAMData.h b/examples/VisualSLAMData.h deleted file mode 100644 index 28fa3040c..000000000 --- a/examples/VisualSLAMData.h +++ /dev/null @@ -1,89 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file VisualSLAMData.cpp - * @brief Generate ground-truth simulated data for VisualSLAM examples - * @author Duy-Nguyen Ta - */ - -#pragma once - -#include -#include -#include - -/* ************************************************************************* */ -/** - * Simulated data for the visual SLAM examples: - * - 8 Landmarks: (10,10,10) (-10,10,10) (-10,-10,10) (10,-10,10) - * (10,10,-10) (-10,10,-10) (-10,-10,-10) (10,-10,-10) - * - n 90-deg-FoV cameras with the same calibration parameters: - * f = 50.0, Image: 100x100, center: 50.0, 50.0 - * and ground-truth poses on a circle around the landmarks looking at the world's origin: - * Rot3(-sin(theta), 0, -cos(theta), - * cos(theta), 0, -sin(theta), - * 0, -1, 0 ), - * Point3(r*cos(theta), r*sin(theta), 0.0) - * (theta += 2*pi/N) - * - Measurement noise: 1 pix sigma - */ -struct VisualSLAMExampleData { - gtsam::shared_ptrK sK; // camera calibration parameters - std::vector poses; // ground-truth camera poses - gtsam::Pose3 odometry; // ground-truth odometry between 2 consecutive poses (simulated data for iSAM) - std::vector points; // ground-truth landmarks - std::map > z; // 2D measurements of landmarks in each camera frame - gtsam::SharedDiagonal noiseZ; // measurement noise (noiseModel::Isotropic::Sigma(2, 5.0f)); - gtsam::SharedDiagonal noiseX; // noise for camera poses - gtsam::SharedDiagonal noiseL; // noise for landmarks - - static const VisualSLAMExampleData generate() { - VisualSLAMExampleData data; - // Landmarks (ground truth) - data.points.push_back(gtsam::Point3(10.0,10.0,10.0)); - data.points.push_back(gtsam::Point3(-10.0,10.0,10.0)); - data.points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); - data.points.push_back(gtsam::Point3(10.0,-10.0,10.0)); - data.points.push_back(gtsam::Point3(10.0,10.0,-10.0)); - data.points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); - data.points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); - data.points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); - - // Camera calibration parameters - data.sK = gtsam::shared_ptrK(new gtsam::Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - - // n camera poses - int n = 8; - double theta = 0.0; - double r = 30.0; - for (int i=0; isample()))*/); // you can add noise as desired - } - } - data.noiseX = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector_(6, 0.001, 0.001, 0.001, 0.1, 0.1, 0.1)); - data.noiseL = gtsam::noiseModel::Isotropic::Sigma(3, 0.1); - - return data; - } -}; diff --git a/examples/VisualSLAMExample.cpp b/examples/VisualSLAMExample.cpp index 1fc343203..8aed051eb 100644 --- a/examples/VisualSLAMExample.cpp +++ b/examples/VisualSLAMExample.cpp @@ -15,49 +15,123 @@ * @author Duy-Nguyen Ta */ -#include +/** + * A structure-from-motion example with landmarks + * - The landmarks form a 10 meter cube + * - The robot rotates around the landmarks, always facing towards the cube + */ + +// As this is a full 3D problem, we will use Pose3 variables to represent the camera +// positions and Point3 variables (x, y, z) to represent the landmark coordinates. +// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +// We will also need a camera object to hold calibration information and perform projections. +#include +#include +#include +#include + +// Each variable in the system (poses and landmarks) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use Symbols #include -#include -#include -#include "VisualSLAMData.h" + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use Projection factors to model the camera's landmark observations. +// Also, we will initialize the robot at some location using a Prior factor. +#include +#include + +// When the factors are created, we will add them to a Factor Graph. As the factors we are using +// are nonlinear factors, we will need a Nonlinear Factor Graph. +#include + +// Finally, once all of the factors have been added to our factor graph, we will want to +// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. +// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a +// trust-region method known as Powell's Degleg +#include + +// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +// nonlinear functions around an initial linearization point, then solve the linear system +// to update the linearization point. This happens repeatedly until the solver converges +// to a consistent set of variable values. This requires us to specify an initial guess +// for each variable, held in a Values container. +#include + +#include using namespace std; using namespace gtsam; -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::L; - /* ************************************************************************* */ int main(int argc, char* argv[]) { - VisualSLAMExampleData data = VisualSLAMExampleData::generate(); + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - /* 1. Create graph *///using the 2D measurements (features) and the calibration data - visualSLAM::Graph graph; + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v - /* 2. Add factors to the graph */ - // 2a. Measurement factors - for (size_t i=0; i points; + points.push_back(gtsam::Point3(10.0,10.0,10.0)); + points.push_back(gtsam::Point3(-10.0,10.0,10.0)); + points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); + points.push_back(gtsam::Point3(10.0,-10.0,10.0)); + points.push_back(gtsam::Point3(10.0,10.0,-10.0)); + points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); + points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); + points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); + + // Create the set of ground-truth poses + std::vector poses; + double radius = 30.0; + int i = 0; + double theta = 0.0; + gtsam::Point3 up(0,0,1); + gtsam::Point3 target(0,0,0); + for(; i < 8; ++i, theta += 2*M_PI/8) { + gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0); + gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up); + poses.push_back(camera.pose()); } - // 2b. Prior factor for the first pose and point to constraint the system - graph.addPosePrior(X(0), data.poses[0], data.noiseX); - graph.addPointPrior(L(0), data.points[0], data.noiseL); - /* 3. Initial estimates for variable nodes, simulated by Gaussian noises */ - Values initial; - for (size_t i=0; isample())*/); // you can add noise if you want - for (size_t j=0; jsample())*/); // you can add noise if you want - initial.print("Intial Estimates: "); + // Create a factor graph + NonlinearFactorGraph graph; - /* 4. Optimize the graph and print results */ - visualSLAM::Values result = GaussNewtonOptimizer(graph, initial).optimize(); -// visualSLAM::Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); - result.print("Final results: "); + // Add a prior on pose x1. This indirectly specifies where the origin is. + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + + // Simulated measurements from each camera pose, adding them to the factor graph + for (size_t i = 0; i < poses.size(); ++i) { + for (size_t j = 0; j < points.size(); ++j) { + SimpleCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + graph.add(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + } + } + + // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained + // Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance + // between the first camera and the first landmark. All other landmark positions are interpreted using this scale. + noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); + graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.print("Factor Graph:\n"); + + // Create the data structure to hold the initialEstimate estimate to the solution + // Intentionally initialize the variables off from the ground truth + Values initialEstimate; + for (size_t i = 0; i < poses.size(); ++i) + initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + for (size_t j = 0; j < points.size(); ++j) + initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + initialEstimate.print("Initial Estimates:\n"); + + /* Optimize the graph and print results */ + Values result = DoglegOptimizer(graph, initialEstimate).optimize(); + result.print("Final results:\n"); return 0; } diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index 2b4d2476a..6d75e9149 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -21,7 +21,7 @@ * @author Stephen Williams */ -#include +#include #include #include #include diff --git a/gtsam.h b/gtsam.h index 023b5c6f3..d15ef08d8 100644 --- a/gtsam.h +++ b/gtsam.h @@ -66,6 +66,9 @@ * or with typedefs, e.g. * template class Class2 { ... }; * typedef Class2 MyInstantiatedClass; + * - In the class definition, appearances of the template argument(s) will be replaced with their + * instantiated types, e.g. 'void setValue(const T& value);'. + * - To refer to the instantiation of the template class itself, use 'This', i.e. 'static This Create();' * - To create new instantiations in other modules, you must copy-and-paste the whole class definition * into the new module, but use only your new instantiation types. * - When forward-declaring template instantiations, use the generated/typedefed name, e.g. @@ -592,6 +595,43 @@ virtual class SimpleCamera : gtsam::Value { double range(const gtsam::Pose3& point); // FIXME, overload }; +// TODO: Add this back in when Cal3DS2 has a calibrate function +//template +//virtual class PinholeCamera : gtsam::Value { +// // Standard Constructors and Named Constructors +// PinholeCamera(); +// PinholeCamera(const gtsam::Pose3& pose); +// PinholeCamera(const gtsam::Pose3& pose, const gtsam::Cal3DS2& K); +// static This Level(const gtsam::Cal3DS2& K, +// const gtsam::Pose2& pose, double height); +// static This Level(const gtsam::Pose2& pose, double height); // FIXME overload +// static This Lookat(const gtsam::Point3& eye, +// const gtsam::Point3& target, const gtsam::Point3& upVector, +// const gtsam::Cal3DS2& K); +// +// // Testable +// void print(string s) const; +// bool equals(const This& camera, double tol) const; +// +// // Standard Interface +// gtsam::Pose3 pose() const; +// CALIBRATION calibration() const; +// +// // Manifold +// This retract(const Vector& d) const; +// Vector localCoordinates(const This& T2) const; +// size_t dim() const; +// static size_t Dim(); +// +// // Transformations and measurement functions +// static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); +// pair projectSafe(const gtsam::Point3& pw) const; +// gtsam::Point2 project(const gtsam::Point3& point); +// gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; +// double range(const gtsam::Point3& point); +// double range(const gtsam::Pose3& point); // FIXME, overload +//}; + //************************************************************************* // inference //************************************************************************* @@ -985,14 +1025,9 @@ class KalmanFilter { //************************************************************************* #include -class Symbol { - Symbol(char c, size_t j); - Symbol(size_t k); - void print(string s) const; - size_t key() const; - size_t index() const; - char chr() const; -}; +size_t symbol(char chr, size_t index); +char symbolChr(size_t key); +size_t symbolIndex(size_t key); #include class Ordering { @@ -1070,6 +1105,7 @@ class Values { void insert(size_t j, const gtsam::Value& value); bool exists(size_t j) const; gtsam::Value at(size_t j) const; + gtsam::KeyList keys() const; }; // Actually a FastList @@ -1137,6 +1173,7 @@ class KeyVector { void push_back(size_t key) const; }; +#include class Marginals { Marginals(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& solution); @@ -1179,12 +1216,22 @@ virtual class NonlinearOptimizerParams { virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { SuccessiveLinearizationParams(); + string getLinearSolverType() const; + + void setLinearSolverType(string solver); + void setOrdering(const gtsam::Ordering& ordering); + bool isMultifrontal() const; bool isSequential() const; bool isCholmod() const; bool isCG() const; }; +#include +virtual class GaussNewtonParams : gtsam::SuccessiveLinearizationParams { + GaussNewtonParams(); +}; + #include virtual class LevenbergMarquardtParams : gtsam::SuccessiveLinearizationParams { LevenbergMarquardtParams(); @@ -1212,6 +1259,7 @@ virtual class DoglegParams : gtsam::SuccessiveLinearizationParams { }; virtual class NonlinearOptimizer { + gtsam::Values optimize(); gtsam::Values optimizeSafely(); double error() const; int iterations() const; @@ -1219,6 +1267,11 @@ virtual class NonlinearOptimizer { void iterate() const; }; +virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); +}; + virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); @@ -1365,18 +1418,28 @@ class NonlinearISAM { //************************************************************************* // Nonlinear factor types //************************************************************************* +#include +#include +#include +#include +#include +#include + +#include template virtual class PriorFactor : gtsam::NonlinearFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); }; +#include template virtual class BetweenFactor : gtsam::NonlinearFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); }; +#include template virtual class NonlinearEquality : gtsam::NonlinearFactor { // Constructor - forces exact evaluation @@ -1386,6 +1449,7 @@ virtual class NonlinearEquality : gtsam::NonlinearFactor { }; +#include template virtual class RangeFactor : gtsam::NonlinearFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); @@ -1400,14 +1464,25 @@ typedef gtsam::RangeFactor RangeFactorSimple typedef gtsam::RangeFactor RangeFactorCalibratedCamera; typedef gtsam::RangeFactor RangeFactorSimpleCamera; -template + +#include +template virtual class BearingFactor : gtsam::NonlinearFactor { - BearingFactor(size_t key1, size_t key2, const ROT& measured, const gtsam::noiseModel::Base* noiseModel); + BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel); }; typedef gtsam::BearingFactor BearingFactor2D; +#include +template +virtual class BearingRangeFactor : gtsam::NonlinearFactor { + BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); +}; + +typedef gtsam::BearingRangeFactor BearingRangeFactor2D; + + #include template virtual class GenericProjectionFactor : gtsam::NonlinearFactor { @@ -1417,415 +1492,65 @@ virtual class GenericProjectionFactor : gtsam::NonlinearFactor { CALIBRATION* calibration() const; }; typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; +// FIXME: Add Cal3DS2 when it has a 'calibrate' function +//typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; + + +#include +template +virtual class GeneralSFMFactor : gtsam::NonlinearFactor { + GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); + gtsam::Point2 measured() const; +}; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +// FIXME: Add Cal3DS2 when it has a 'calibrate' function +//typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; + +// FIXME: Add Cal3DS2 when it has a 'calibrate' function +template +virtual class GeneralSFMFactor2 : gtsam::NonlinearFactor { + GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); + gtsam::Point2 measured() const; +}; + + +#include +template +virtual class GenericStereoFactor : gtsam::NonlinearFactor { + GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); + gtsam::StereoPoint2 measured() const; + gtsam::Cal3_S2Stereo* calibration() const; +}; +typedef gtsam::GenericStereoFactor GenericStereoFactor3D; #include -pair load2D(string filename, +pair load2D(string filename, gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model); -} //\namespace gtsam //************************************************************************* -// Pose2SLAM +// Utilities //************************************************************************* -namespace pose2SLAM { +namespace utilities { -#include -class Values { - Values(); - Values(const pose2SLAM::Values& values); - size_t size() const; - void print(string s) const; - bool exists(size_t key); - gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList + #include + Matrix extractPoint2(const gtsam::Values& values); + Matrix extractPoint3(const gtsam::Values& values); + Matrix extractPose2(const gtsam::Values& values); + Matrix extractPose3(const gtsam::Values& values); + void perturbPoint2(gtsam::Values& values, double sigma, int seed); + void perturbPoint3(gtsam::Values& values, double sigma, int seed); + void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); + Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); - static pose2SLAM::Values Circle(size_t n, double R); - void insertPose(size_t key, const gtsam::Pose2& pose); - void updatePose(size_t key, const gtsam::Pose2& pose); - gtsam::Pose2 pose(size_t i); - Matrix poses() const; -}; - -#include -class Graph { - Graph(); - Graph(const gtsam::NonlinearFactorGraph& graph); - Graph(const pose2SLAM::Graph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const pose2SLAM::Graph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - - // NonlinearFactorGraph - double error(const pose2SLAM::Values& values) const; - double probPrime(const pose2SLAM::Values& values) const; - gtsam::Ordering* orderingCOLAMD(const pose2SLAM::Values& values) const; - gtsam::GaussianFactorGraph* linearize(const pose2SLAM::Values& values, - const gtsam::Ordering& ordering) const; - - // pose2SLAM-specific - void addPoseConstraint(size_t key, const gtsam::Pose2& pose); - void addPosePrior(size_t key, const gtsam::Pose2& pose, const gtsam::noiseModel::Base* noiseModel); - void addRelativePose(size_t key1, size_t key2, const gtsam::Pose2& relativePoseMeasurement, const gtsam::noiseModel::Base* noiseModel); - pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate, size_t verbosity) const; - pose2SLAM::Values optimizeSPCG(const pose2SLAM::Values& initialEstimate, size_t verbosity) const; - gtsam::Marginals marginals(const pose2SLAM::Values& solution) const; -}; - -} //\namespace pose2SLAM - -//************************************************************************* -// Pose3SLAM -//************************************************************************* - -namespace pose3SLAM { - -#include -class Values { - Values(); - Values(const pose3SLAM::Values& values); - size_t size() const; - void print(string s) const; - bool exists(size_t key); - gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList - - static pose3SLAM::Values Circle(size_t n, double R); - void insertPose(size_t key, const gtsam::Pose3& pose); - void updatePose(size_t key, const gtsam::Pose3& pose); - gtsam::Pose3 pose(size_t i); - Matrix translations() const; -}; - -#include -class Graph { - Graph(); - Graph(const gtsam::NonlinearFactorGraph& graph); - Graph(const pose3SLAM::Graph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const pose3SLAM::Graph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - - // NonlinearFactorGraph - double error(const pose3SLAM::Values& values) const; - double probPrime(const pose3SLAM::Values& values) const; - gtsam::Ordering* orderingCOLAMD(const pose3SLAM::Values& values) const; - gtsam::GaussianFactorGraph* linearize(const pose3SLAM::Values& values, - const gtsam::Ordering& ordering) const; - - // pose3SLAM-specific - void addPoseConstraint(size_t i, const gtsam::Pose3& p); - void addPosePrior(size_t key, const gtsam::Pose3& p, const gtsam::noiseModel::Base* model); - void addRelativePose(size_t key1, size_t key2, const gtsam::Pose3& z, const gtsam::noiseModel::Base* model); - pose3SLAM::Values optimize(const pose3SLAM::Values& initialEstimate, size_t verbosity) const; - // FIXME gtsam::LevenbergMarquardtOptimizer optimizer(const pose3SLAM::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const; - gtsam::Marginals marginals(const pose3SLAM::Values& solution) const; -}; - -} //\namespace pose3SLAM - -//************************************************************************* -// planarSLAM -//************************************************************************* - -namespace planarSLAM { - -#include -class Values { - Values(); - Values(const planarSLAM::Values& values); - size_t size() const; - void print(string s) const; - bool exists(size_t key); - gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList - - // inherited from pose2SLAM - static planarSLAM::Values Circle(size_t n, double R); - void insertPose(size_t key, const gtsam::Pose2& pose); - void updatePose(size_t key, const gtsam::Pose2& pose); - gtsam::Pose2 pose(size_t i); - Matrix poses() const; - - // Access to poses - planarSLAM::Values allPoses() const; - size_t nrPoses() const; - gtsam::KeyVector poseKeys() const; // Note the switch to KeyVector, rather than KeyList - - // Access to points - planarSLAM::Values allPoints() const; - size_t nrPoints() const; - gtsam::KeyVector pointKeys() const; // Note the switch to KeyVector, rather than KeyList - - void insertPoint(size_t key, const gtsam::Point2& point); - void updatePoint(size_t key, const gtsam::Point2& point); - gtsam::Point2 point(size_t key) const; - Matrix points() const; -}; - -#include -class Graph { - Graph(); - Graph(const gtsam::NonlinearFactorGraph& graph); - Graph(const pose2SLAM::Graph& graph); - Graph(const planarSLAM::Graph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const planarSLAM::Graph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - - // NonlinearFactorGraph - double error(const planarSLAM::Values& values) const; - double probPrime(const planarSLAM::Values& values) const; - gtsam::Ordering* orderingCOLAMD(const planarSLAM::Values& values) const; - gtsam::GaussianFactorGraph* linearize(const planarSLAM::Values& values, - const gtsam::Ordering& ordering) const; - - // pose2SLAM-inherited - void addPoseConstraint(size_t key, const gtsam::Pose2& pose); - void addPosePrior(size_t key, const gtsam::Pose2& pose, const gtsam::noiseModel::Base* noiseModel); - void addRelativePose(size_t key1, size_t key2, const gtsam::Pose2& relativePoseMeasurement, const gtsam::noiseModel::Base* noiseModel); - planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate, size_t verbosity) const; - planarSLAM::Values optimizeSPCG(const planarSLAM::Values& initialEstimate, size_t verbosity) const; - gtsam::Marginals marginals(const planarSLAM::Values& solution) const; - - // planarSLAM-specific - void addPointConstraint(size_t pointKey, const gtsam::Point2& p); - void addPointPrior(size_t pointKey, const gtsam::Point2& p, const gtsam::noiseModel::Base* model); - void addBearing(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing, const gtsam::noiseModel::Base* noiseModel); - void addRange(size_t poseKey, size_t pointKey, double range, const gtsam::noiseModel::Base* noiseModel); - void addBearingRange(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing,double range, const gtsam::noiseModel::Base* noiseModel); -}; - -#include -class Odometry { - Odometry(size_t key1, size_t key2, const gtsam::Pose2& measured, - const gtsam::noiseModel::Base* model); - void print(string s) const; - gtsam::GaussianFactor* linearize(const planarSLAM::Values& center, - const gtsam::Ordering& ordering) const; -}; - -} //\namespace planarSLAM - -//************************************************************************* -// VisualSLAM -//************************************************************************* - -namespace visualSLAM { - -#include -class Values { - Values(); - Values(const visualSLAM::Values& values); - size_t size() const; - void print(string s) const; - bool exists(size_t key); - gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList - - // pose3SLAM inherited - static visualSLAM::Values Circle(size_t n, double R); - void insertPose(size_t key, const gtsam::Pose3& pose); - void updatePose(size_t key, const gtsam::Pose3& pose); - gtsam::Pose3 pose(size_t i); - Matrix translations() const; - - // Access to poses - visualSLAM::Values allPoses() const; - size_t nrPoses() const; - gtsam::KeyVector poseKeys() const; // Note the switch to KeyVector, rather than KeyList - - // Access to points - visualSLAM::Values allPoints() const; - size_t nrPoints() const; - gtsam::KeyVector pointKeys() const; // Note the switch to KeyVector, rather than KeyList - - void insertPoint(size_t key, const gtsam::Point3& pose); - void updatePoint(size_t key, const gtsam::Point3& pose); - gtsam::Point3 point(size_t j); - void insertBackprojections(const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); - void perturbPoints(double sigma, size_t seed); - Matrix points() const; -}; - -#include -class Graph { - Graph(); - Graph(const gtsam::NonlinearFactorGraph& graph); - Graph(const pose3SLAM::Graph& graph); - Graph(const visualSLAM::Graph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const visualSLAM::Graph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - - double error(const visualSLAM::Values& values) const; - gtsam::Ordering* orderingCOLAMD(const visualSLAM::Values& values) const; - gtsam::GaussianFactorGraph* linearize(const visualSLAM::Values& values, - const gtsam::Ordering& ordering) const; - - // pose3SLAM-inherited - void addPoseConstraint(size_t i, const gtsam::Pose3& p); - void addPosePrior(size_t key, const gtsam::Pose3& p, const gtsam::noiseModel::Base* model); - void addRelativePose(size_t key1, size_t key2, const gtsam::Pose3& z, const gtsam::noiseModel::Base* model); - visualSLAM::Values optimize(const visualSLAM::Values& initialEstimate, size_t verbosity) const; - visualSLAM::LevenbergMarquardtOptimizer optimizer(const visualSLAM::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const; - gtsam::Marginals marginals(const visualSLAM::Values& solution) const; - - // Priors and constraints - void addPointConstraint(size_t pointKey, const gtsam::Point3& p); - void addPointPrior(size_t pointKey, const gtsam::Point3& p, const gtsam::noiseModel::Base* model); - void addRangeFactor(size_t poseKey, size_t pointKey, double range, const gtsam::noiseModel::Base* model); - - // Measurements - void addMeasurement(const gtsam::Point2& measured, - const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey, - const gtsam::Cal3_S2* K); - void addMeasurements(size_t i, Vector J, Matrix Z, - const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); - void addStereoMeasurement(const gtsam::StereoPoint2& measured, - const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey, - const gtsam::Cal3_S2Stereo* K); - - // Information - Matrix reprojectionErrors(const visualSLAM::Values& values) const; - -}; - -#include -class ISAM { - ISAM(); - ISAM(int reorderInterval); - void print(string s) const; - void printStats() const; - void saveGraph(string s) const; - visualSLAM::Values estimate() const; - Matrix marginalCovariance(size_t key) const; - int reorderInterval() const; - int reorderCounter() const; - void update(const visualSLAM::Graph& newFactors, const visualSLAM::Values& initialValues); - void reorder_relinearize(); - void addKey(size_t key); - void setOrdering(const gtsam::Ordering& new_ordering); - - // These might be expensive as instead of a reference the wrapper will make a copy - gtsam::GaussianISAM bayesTree() const; - visualSLAM::Values getLinearizationPoint() const; - gtsam::Ordering getOrdering() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; -}; - -#include -class LevenbergMarquardtOptimizer { - double lambda() const; - void iterate(); - double error() const; - size_t iterations() const; - visualSLAM::Values optimize(); - visualSLAM::Values optimizeSafely(); - visualSLAM::Values values() const; -}; - -} //\namespace visualSLAM - -//************************************************************************ -// sparse BA -//************************************************************************ - -namespace sparseBA { - -#include -class Values { - Values(); - Values(const sparseBA::Values& values); - size_t size() const; - void print(string s) const; - bool exists(size_t key); - gtsam::KeyVector keys() const; - - // Access to cameras - sparseBA::Values allSimpleCameras() const ; - size_t nrSimpleCameras() const ; - gtsam::KeyVector simpleCameraKeys() const ; - void insertSimpleCamera(size_t j, const gtsam::SimpleCamera& camera); - void updateSimpleCamera(size_t j, const gtsam::SimpleCamera& camera); - gtsam::SimpleCamera simpleCamera(size_t j) const; - - // Access to points, inherited from visualSLAM - sparseBA::Values allPoints() const; - size_t nrPoints() const; - gtsam::KeyVector pointKeys() const; // Note the switch to KeyVector, rather than KeyList - void insertPoint(size_t key, const gtsam::Point3& pose); - void updatePoint(size_t key, const gtsam::Point3& pose); - gtsam::Point3 point(size_t j); - Matrix points() const; -}; - -#include -class Graph { - Graph(); - Graph(const gtsam::NonlinearFactorGraph& graph); - Graph(const sparseBA::Graph& graph); - - // Information - Matrix reprojectionErrors(const sparseBA::Values& values) const; - - // inherited from FactorGraph - void print(string s) const; - bool equals(const sparseBA::Graph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - - double error(const sparseBA::Values& values) const; - gtsam::Ordering* orderingCOLAMD(const sparseBA::Values& values) const; - gtsam::GaussianFactorGraph* linearize(const sparseBA::Values& values, const gtsam::Ordering& ordering) const; - - sparseBA::Values optimize(const sparseBA::Values& initialEstimate, size_t verbosity) const; - sparseBA::LevenbergMarquardtOptimizer optimizer(const sparseBA::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const; - gtsam::Marginals marginals(const sparseBA::Values& solution) const; - - // inherited from visualSLAM - void addPointConstraint(size_t pointKey, const gtsam::Point3& p); - void addPointPrior(size_t pointKey, const gtsam::Point3& p, const gtsam::noiseModel::Base* model); - - // add factors - void addSimpleCameraPrior(size_t cameraKey, const gtsam::SimpleCamera &camera, gtsam::noiseModel::Base* model); - void addSimpleCameraConstraint(size_t cameraKey, const gtsam::SimpleCamera &camera); - void addSimpleCameraMeasurement(const gtsam::Point2 &z, gtsam::noiseModel::Base* model, size_t cameraKey, size_t pointKey); -}; - -#include -class LevenbergMarquardtOptimizer { - double lambda() const; - void iterate(); - double error() const; - size_t iterations() const; - sparseBA::Values optimize(); - sparseBA::Values optimizeSafely(); - sparseBA::Values values() const; -}; -} //\namespace sparseBA +} //\namespace utilities +} diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 2ab15a054..5844e8201 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { @@ -135,5 +136,18 @@ struct LieMatrix : public Matrix, public DerivedValue { static inline Vector Logmap(const LieMatrix& p) { return Eigen::Map(&p(0,0), p.dim()); } +private: + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("LieMatrix", + boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Matrix", + boost::serialization::base_object(*this)); + + } + }; } // \namespace gtsam diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index ef31e93cc..916730fd3 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -119,5 +119,17 @@ struct LieVector : public Vector, public DerivedValue { /** Logmap around identity - just returns with default cast back */ static inline Vector Logmap(const LieVector& p) { return p; } +private: + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("LieVector", + boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Vector", + boost::serialization::base_object(*this)); + } + }; } // \namespace gtsam diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 867844fbb..140ab1bdf 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -141,7 +141,9 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(leftCamPose_); + ar & boost::serialization::make_nvp("StereoCamera", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(leftCamPose_); ar & BOOST_SERIALIZATION_NVP(K_); } diff --git a/gtsam/inference/graph.h b/gtsam/inference/graph.h index 98d41940b..88ab5d0e8 100644 --- a/gtsam/inference/graph.h +++ b/gtsam/inference/graph.h @@ -108,3 +108,5 @@ namespace gtsam { } // namespace gtsam + +#include diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index 8a21dd96e..70d55d192 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -57,6 +57,7 @@ public: void setDeltaInitial(double deltaInitial) { this->deltaInitial = deltaInitial; } void setVerbosityDL(const std::string& verbosityDL) { this->verbosityDL = verbosityDLTranslator(verbosityDL); } +private: VerbosityDL verbosityDLTranslator(const std::string& verbosityDL) const; std::string verbosityDLTranslator(VerbosityDL verbosityDL) const; }; diff --git a/gtsam/nonlinear/EasyFactorGraph.cpp b/gtsam/nonlinear/EasyFactorGraph.cpp index 28e344efe..5031975f7 100644 --- a/gtsam/nonlinear/EasyFactorGraph.cpp +++ b/gtsam/nonlinear/EasyFactorGraph.cpp @@ -44,7 +44,7 @@ namespace gtsam { size_t verbosity) const { LevenbergMarquardtParams p; p.verbosity = (NonlinearOptimizerParams::Verbosity) verbosity; - p.linearSolverType = SuccessiveLinearizationParams::CG; + p.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT; p.iterativeParams = boost::make_shared(); return optimizer(initialEstimate, p).optimizeSafely(); } diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index 353439d98..ebca4899c 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -90,3 +90,5 @@ namespace gtsam { }; } // namespace + +#include diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 5677f6c08..4b3f8d391 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -61,6 +61,7 @@ public: inline void setlambdaUpperBound(double value) { lambdaUpperBound = value; } inline void setVerbosityLM(const std::string &s) { verbosityLM = verbosityLMTranslator(s); } +private: VerbosityLM verbosityLMTranslator(const std::string &s) const; std::string verbosityLMTranslator(VerbosityLM value) const; }; diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 1c071faef..03f3a5e34 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -63,6 +63,7 @@ public: void setErrorTol(double value) { errorTol = value ; } void setVerbosity(const std::string &src) { verbosity = verbosityTranslator(src); } +private: Verbosity verbosityTranslator(const std::string &s) const; std::string verbosityTranslator(Verbosity value) const; }; diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp index 0fe0440ae..af3f70ddc 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp @@ -17,37 +17,37 @@ namespace gtsam { void SuccessiveLinearizationParams::print(const std::string& str) const { - NonlinearOptimizerParams::print(str); - switch ( linearSolverType ) { - case MULTIFRONTAL_CHOLESKY: - std::cout << " linear solver type: MULTIFRONTAL CHOLESKY\n"; - break; - case MULTIFRONTAL_QR: - std::cout << " linear solver type: MULTIFRONTAL QR\n"; - break; - case SEQUENTIAL_CHOLESKY: - std::cout << " linear solver type: SEQUENTIAL CHOLESKY\n"; - break; - case SEQUENTIAL_QR: - std::cout << " linear solver type: SEQUENTIAL QR\n"; - break; - case CHOLMOD: - std::cout << " linear solver type: CHOLMOD\n"; - break; - case CG: - std::cout << " linear solver type: CG\n"; - break; - default: - std::cout << " linear solver type: (invalid)\n"; - break; - } - - if(ordering) - std::cout << " ordering: custom\n"; - else - std::cout << " ordering: COLAMD\n"; - - std::cout.flush(); + NonlinearOptimizerParams::print(str); + switch ( linearSolverType ) { + case MULTIFRONTAL_CHOLESKY: + std::cout << " linear solver type: MULTIFRONTAL CHOLESKY\n"; + break; + case MULTIFRONTAL_QR: + std::cout << " linear solver type: MULTIFRONTAL QR\n"; + break; + case SEQUENTIAL_CHOLESKY: + std::cout << " linear solver type: SEQUENTIAL CHOLESKY\n"; + break; + case SEQUENTIAL_QR: + std::cout << " linear solver type: SEQUENTIAL QR\n"; + break; + case CHOLMOD: + std::cout << " linear solver type: CHOLMOD\n"; + break; + case CONJUGATE_GRADIENT: + std::cout << " linear solver type: CONJUGATE GRADIENT\n"; + break; + default: + std::cout << " linear solver type: (invalid)\n"; + break; + } + + if(ordering) + std::cout << " ordering: custom\n"; + else + std::cout << " ordering: COLAMD\n"; + + std::cout.flush(); } VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const SuccessiveLinearizationParams ¶ms) { diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index be66f37d4..e79a364c5 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -31,7 +31,7 @@ public: MULTIFRONTAL_QR, SEQUENTIAL_CHOLESKY, SEQUENTIAL_QR, - CG, /* Experimental Flag */ + CONJUGATE_GRADIENT, /* Experimental Flag */ CHOLMOD, /* Experimental Flag */ }; @@ -42,21 +42,15 @@ public: SuccessiveLinearizationParams() : linearSolverType(MULTIFRONTAL_CHOLESKY) {} virtual ~SuccessiveLinearizationParams() {} - inline bool isMultifrontal() const { - return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR); - } + inline bool isMultifrontal() const { + return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR); } inline bool isSequential() const { - return (linearSolverType == SEQUENTIAL_CHOLESKY) || (linearSolverType == SEQUENTIAL_QR); - } + return (linearSolverType == SEQUENTIAL_CHOLESKY) || (linearSolverType == SEQUENTIAL_QR); } - inline bool isCholmod() const { - return (linearSolverType == CHOLMOD); - } + inline bool isCholmod() const { return (linearSolverType == CHOLMOD); } - inline bool isCG() const { - return (linearSolverType == CG); - } + inline bool isCG() const { return (linearSolverType == CONJUGATE_GRADIENT); } virtual void print(const std::string& str) const; @@ -76,6 +70,33 @@ public: break; } } + + std::string getLinearSolverType() const { return linearSolverTranslator(linearSolverType); } + + void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); } + void setOrdering(const Ordering& ordering) { this->ordering = ordering; } + +private: + std::string linearSolverTranslator(LinearSolverType linearSolverType) const { + switch(linearSolverType) { + case MULTIFRONTAL_CHOLESKY: return "MULTIFRONTAL_CHOLESKY"; + case MULTIFRONTAL_QR: return "MULTIFRONTAL_QR"; + case SEQUENTIAL_CHOLESKY: return "SEQUENTIAL_CHOLESKY"; + case SEQUENTIAL_QR: return "SEQUENTIAL_QR"; + case CONJUGATE_GRADIENT: return "CONJUGATE_GRADIENT"; + case CHOLMOD: return "CHOLMOD"; + default: throw std::invalid_argument("Unknown linear solver type in SuccessiveLinearizationOptimizer"); + } + } + LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const { + if(linearSolverType == "MULTIFRONTAL_CHOLESKY") return MULTIFRONTAL_CHOLESKY; + if(linearSolverType == "MULTIFRONTAL_QR") return MULTIFRONTAL_QR; + if(linearSolverType == "SEQUENTIAL_CHOLESKY") return SEQUENTIAL_CHOLESKY; + if(linearSolverType == "SEQUENTIAL_QR") return SEQUENTIAL_QR; + if(linearSolverType == "CONJUGATE_GRADIENT") return CONJUGATE_GRADIENT; + if(linearSolverType == "CHOLMOD") return CHOLMOD; + throw std::invalid_argument("Unknown linear solver type in SuccessiveLinearizationOptimizer"); + } }; /* a wrapper for solving a GaussianFactorGraph according to the parameters */ diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h index b4bda4f1a..c128b1105 100644 --- a/gtsam/nonlinear/Symbol.h +++ b/gtsam/nonlinear/Symbol.h @@ -122,6 +122,15 @@ private: } }; +/** Create a symbol key from a character and index, i.e. x5. */ +inline Key symbol(unsigned char c, size_t j) { return (Key)Symbol(c,j); } + +/** Return the character portion of a symbol key. */ +inline unsigned char symbolChr(Key key) { return Symbol(key).chr(); } + +/** Return the index portion of a symbol key. */ +inline size_t symbolIndex(Key key) { return Symbol(key).index(); } + namespace symbol_shorthand { inline Key A(size_t j) { return Symbol('a', j); } inline Key B(size_t j) { return Symbol('b', j); } diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index 62e46d53e..326491e8c 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -25,12 +25,12 @@ namespace gtsam { /** * Binary factor for a bearing measurement */ - template + template class BearingFactor: public NoiseModelFactor2 { private: typedef POSE Pose; - typedef ROT Rot; + typedef ROTATION Rot; typedef POINT Point; typedef BearingFactor This; diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 933333fdf..bf5b6b3a5 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -27,12 +27,12 @@ namespace gtsam { /** * Binary factor for a bearing measurement */ - template + template class BearingRangeFactor: public NoiseModelFactor2 { private: typedef POSE Pose; - typedef typename POSE::Rotation Rot; + typedef ROTATION Rot; typedef POINT Point; typedef BearingRangeFactor This; diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index d94c162af..a9db7dcbe 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -111,6 +111,8 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } }; @@ -143,6 +145,7 @@ namespace gtsam { */ GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) : Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {} + GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor virtual ~GeneralSFMFactor2() {} ///< destructor @@ -197,6 +200,8 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor3", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } }; diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index d0c0bda6a..94af6e10e 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -18,6 +18,7 @@ #pragma once +#include #include namespace gtsam { @@ -74,7 +75,7 @@ public: /** * equals */ - virtual bool equals(const NonlinearFactor& f, double tol) const { + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const GenericStereoFactor* p = dynamic_cast (&f); return p && Base::equals(f) && measured_.equals(p->measured_, tol); } @@ -102,6 +103,8 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 0502bb69b..29154f686 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -20,8 +20,10 @@ #include #include +#include #include #include +#include using namespace std; @@ -30,71 +32,23 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -pair > dataset(const string& dataset, - const string& user_path) { - string path = user_path, set = dataset; - boost::optional null_model; - boost::optional identity(noiseModel::Unit::Create(3)); - boost::optional small( - noiseModel::Diagonal::Variances( - gtsam::Vector_(3, 0.0001, 0.0001, 0.0003))); - - if (path.empty()) - path = string(getenv("HOME")) + "/data"; - if (set.empty()) - set = string(getenv("DATASET")); - - if (set == "intel") - return make_pair(path + "/Intel/intel.graph", null_model); - if (set == "intel-gfs") - return make_pair(path + "/Intel/intel.gfs.graph", null_model); - if (set == "Killian-gfs") - return make_pair(path + "/Killian/Killian.gfs.graph", null_model); - if (set == "Killian") - return make_pair(path + "/Killian/Killian.graph", small); - if (set == "Killian-noised") - return make_pair(path + "/Killian/Killian-noised.graph", null_model); - if (set == "3") - return make_pair(path + "/TORO/w3-odom.graph", identity); - if (set == "100") - return make_pair(path + "/TORO/w100-odom.graph", identity); - if (set == "10K") - return make_pair(path + "/TORO/w10000-odom.graph", identity); - if (set == "10K2") - return make_pair(path + "/hogman/data/2D/w10000.graph", - noiseModel::Diagonal::Variances(gtsam::Vector_(3, 0.1, 0.1, 0.05))); - if (set == "Eiffel100") - return make_pair(path + "/TORO/w100-Eiffel.graph", identity); - if (set == "Eiffel10K") - return make_pair(path + "/TORO/w10000-Eiffel.graph", identity); - if (set == "olson") - return make_pair(path + "/Olson/olson06icra.graph", null_model); - if (set == "victoria") - return make_pair(path + "/VictoriaPark/victoria_park.graph", null_model); - if (set == "beijing") - return make_pair(path + "/Beijing/beijingData_trips.graph", null_model); - - return make_pair("unknown", null_model); -} - -/* ************************************************************************* */ -pair load2D( - pair > dataset, +pair load2D( + pair > dataset, int maxID, bool addNoise, bool smart) { return load2D(dataset.first, dataset.second, maxID, addNoise, smart); } /* ************************************************************************* */ -pair load2D( - const string& filename, boost::optional model, int maxID, +pair load2D( + const string& filename, boost::optional model, int maxID, bool addNoise, bool smart) { cout << "Will try to read " << filename << endl; ifstream is(filename.c_str()); if (!is) throw std::invalid_argument("load2D: can not find the file!"); - pose2SLAM::Values::shared_ptr poses(new pose2SLAM::Values); - pose2SLAM::Graph::shared_ptr graph(new pose2SLAM::Graph); + Values::shared_ptr poses(new Values); + NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); string tag; @@ -155,7 +109,7 @@ pair load2D( if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * l1Xl2); - pose2SLAM::Graph::sharedFactor factor( + NonlinearFactor::shared_ptr factor( new BetweenFactor(id1, id2, l1Xl2, *model)); graph->push_back(factor); } @@ -169,8 +123,8 @@ pair load2D( } /* ************************************************************************* */ -void save2D(const pose2SLAM::Graph& graph, const Values& config, - const SharedDiagonal model, const string& filename) { +void save2D(const NonlinearFactorGraph& graph, const Values& config, + const noiseModel::Diagonal::shared_ptr model, const string& filename) { fstream stream(filename.c_str(), fstream::out); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index a099fb3d5..c881ce2bd 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -18,19 +18,12 @@ #pragma once -#include +#include +#include + #include namespace gtsam { - /** - * Construct dataset filename from short name - * Currently has "Killian" "intel.gfs", "10K", etc... - * @param filename - * @param optional dataset, if empty will try to getenv $DATASET - * @param optional path, if empty will try to getenv $HOME - */ - std::pair > - dataset(const std::string& dataset = "", const std::string& path = ""); /** * Load TORO 2D Graph @@ -39,8 +32,8 @@ namespace gtsam { * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model */ - std::pair load2D( - std::pair > dataset, + std::pair load2D( + std::pair > dataset, int maxID = 0, bool addNoise = false, bool smart = true); /** @@ -51,15 +44,15 @@ namespace gtsam { * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model */ - std::pair load2D( + std::pair load2D( const std::string& filename, boost::optional model = boost::optional< - gtsam::SharedDiagonal>(), int maxID = 0, bool addNoise = false, + noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false, bool smart = true); /** save 2d graph */ - void save2D(const pose2SLAM::Graph& graph, const Values& config, - const gtsam::SharedDiagonal model, const std::string& filename); + void save2D(const NonlinearFactorGraph& graph, const Values& config, + const noiseModel::Diagonal::shared_ptr model, const std::string& filename); /** * Load TORO 3D Graph diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp deleted file mode 100644 index 20c149b66..000000000 --- a/gtsam/slam/planarSLAM.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file planarSLAM.cpp - * @brief: bearing/range measurements in 2D plane - * @author Frank Dellaert - **/ - -#include -#include - -// Use planarSLAM namespace for specific SLAM instance -namespace planarSLAM { - - /* ************************************************************************* */ - Matrix Values::points() const { - size_t j=0; - ConstFiltered points = filter(); - Matrix result(points.size(),2); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, points) - result.row(j++) = keyValue.value.vector(); - return result; - } - - /* ************************************************************************* */ - void Graph::addPointConstraint(Key pointKey, const Point2& p) { - push_back(boost::make_shared >(pointKey, p)); - } - - /* ************************************************************************* */ - void Graph::addPointPrior(Key pointKey, const Point2& p, const SharedNoiseModel& model) { - push_back(boost::make_shared >(pointKey, p, model)); - } - - /* ************************************************************************* */ - void Graph::addBearing(Key i, Key j, const Rot2& z, const SharedNoiseModel& model) { - push_back(boost::make_shared >(i, j, z, model)); - } - - /* ************************************************************************* */ - void Graph::addRange(Key i, Key j, double z, const SharedNoiseModel& model) { - push_back(boost::make_shared >(i, j, z, model)); - } - - /* ************************************************************************* */ - void Graph::addBearingRange(Key i, Key j, const Rot2& z1, - double z2, const SharedNoiseModel& model) { - push_back(boost::make_shared >(i, j, z1, z2, model)); - } - - /* ************************************************************************* */ - -} // planarSLAM - diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h deleted file mode 100644 index 967d91581..000000000 --- a/gtsam/slam/planarSLAM.h +++ /dev/null @@ -1,122 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file planarSLAM.h - * @brief bearing/range measurements in 2D plane - * @author Frank Dellaert - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -// Use planarSLAM namespace for specific SLAM instance -namespace planarSLAM { - - using namespace gtsam; - - /* - * Values class, inherited from pose2SLAM::Values, mainly used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Values: public pose2SLAM::Values { - - typedef boost::shared_ptr shared_ptr; - typedef gtsam::Values::ConstFiltered PoseFiltered; - typedef gtsam::Values::ConstFiltered PointFiltered; - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : - pose2SLAM::Values(values) { - } - - /// Constructor from filtered values view of poses - Values(const PoseFiltered& view) : pose2SLAM::Values(view) {} - - /// Constructor from filtered values view of points - Values(const PointFiltered& view) : pose2SLAM::Values(view) {} - - PoseFiltered allPoses() const { return this->filter(); } ///< pose view - size_t nrPoses() const { return allPoses().size(); } ///< get number of poses - KeyList poseKeys() const { return allPoses().keys(); } ///< get keys to poses only - - PointFiltered allPoints() const { return this->filter(); } ///< point view - size_t nrPoints() const { return allPoints().size(); } ///< get number of points - KeyList pointKeys() const { return allPoints().keys(); } ///< get keys to points only - - /// insert a point - void insertPoint(Key j, const Point2& point) { insert(j, point); } - - /// update a point - void updatePoint(Key j, const Point2& point) { update(j, point); } - - /// get a point - Point2 point(Key j) const { return at(j); } - - /// get all [x,y] coordinates in a 2*n matrix - Matrix points() const; -}; - - /** - * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Graph: public pose2SLAM::Graph { - - /// Default constructor - Graph(){} - - /// Copy constructor given any other NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph): - pose2SLAM::Graph(graph) {} - - /// Creates a hard constraint on a point - void addPointConstraint(Key j, const Point2& p); - - /// Adds a prior with mean p and given noise model on point j - void addPointPrior(Key j, const Point2& p, const SharedNoiseModel& model); - - /// Creates a bearing measurement from pose i to point j - void addBearing(Key i, Key j, const Rot2& bearing, const SharedNoiseModel& model); - - /// Creates a range measurement from pose i to point j - void addRange(Key i, Key k, double range, const SharedNoiseModel& model); - - /// Creates a range/bearing measurement from pose i to point j - void addBearingRange(Key i, Key j, const Rot2& bearing, double range, const SharedNoiseModel& model); - }; - -} // planarSLAM - - -/** - * Backwards compatibility and wrap use only, avoid using - */ -namespace planarSLAM { - typedef gtsam::NonlinearEquality Constraint; ///< \deprecated typedef for backwards compatibility - typedef gtsam::PriorFactor Prior; ///< \deprecated typedef for backwards compatibility - typedef gtsam::BetweenFactor Odometry; ///< \deprecated typedef for backwards compatibility - typedef gtsam::BearingFactor Bearing; ///< \deprecated typedef for backwards compatibility - typedef gtsam::RangeFactor Range; ///< \deprecated typedef for backwards compatibility - typedef gtsam::BearingRangeFactor BearingRange; ///< \deprecated typedef for backwards compatibility -} - - diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp deleted file mode 100644 index cc549d84a..000000000 --- a/gtsam/slam/pose2SLAM.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file pose2SLAM.cpp - * @brief: Odometry measurements in 2D plane - * @author Frank Dellaert - **/ - -#include - -// Use pose2SLAM namespace for specific SLAM instance - -namespace pose2SLAM { - - /* ************************************************************************* */ - Values Values::Circle(size_t n, double R) { - Values x; - double theta = 0, dtheta = 2 * M_PI / n; - for (size_t i = 0; i < n; i++, theta += dtheta) - x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta)); - return x; - } - - /* ************************************************************************* */ - Matrix Values::poses() const { - size_t j=0; - ConstFiltered poses = filter(); - Matrix result(poses.size(),3); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) { - const Pose2& r = keyValue.value; - result.row(j++) = Matrix_(1,3, r.x(), r.y(), r.theta()); - } - return result; - } - - /* ************************************************************************* */ - void Graph::addPoseConstraint(Key i, const Pose2& p) { - sharedFactor factor(new NonlinearEquality(i, p)); - push_back(factor); - } - - /* ************************************************************************* */ - void Graph::addPosePrior(Key i, const Pose2& p, const SharedNoiseModel& model) { - sharedFactor factor(new PriorFactor(i, p, model)); - push_back(factor); - } - - /* ************************************************************************* */ - void Graph::addRelativePose(Key i1, Key i2, const Pose2& z, - const SharedNoiseModel& model) { - sharedFactor factor(new BetweenFactor(i1, i2, z, model)); - push_back(factor); - } - - /* ************************************************************************* */ - -} // pose2SLAM diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h deleted file mode 100644 index 018090c6a..000000000 --- a/gtsam/slam/pose2SLAM.h +++ /dev/null @@ -1,107 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file pose2SLAM.h - * @brief: 2D Pose SLAM - * @author Frank Dellaert - **/ - -#pragma once - -#include -#include -#include -#include -#include - -// Use pose2SLAM namespace for specific SLAM instance -namespace pose2SLAM { - - using namespace gtsam; - - /* - * Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Values: public gtsam::Values { - - typedef boost::shared_ptr shared_ptr; - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : - gtsam::Values(values) { - } - - /** - * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) - * @param n number of poses - * @param R radius of circle - * @param c character to use for keys - * @return circle of n 2D poses - */ - static Values Circle(size_t n, double R); - - /// insert a pose - void insertPose(Key i, const Pose2& pose) { insert(i, pose); } - - /// update a pose - void updatePose(Key i, const Pose2& pose) { update(i, pose); } - - /// get a pose - Pose2 pose(Key i) const { return at(i); } - - /// get all [x,y,theta] coordinates in a 3*m matrix - Matrix poses() const; - }; - - /** - * List of typedefs for factors - */ - - /** - * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Graph: public EasyFactorGraph { - - typedef boost::shared_ptr shared_ptr; - - /// Default constructor - Graph(){} - - /// Copy constructor given any other NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph): - EasyFactorGraph(graph) {} - - /// Creates a hard constraint for key i with the given Pose2 p. - void addPoseConstraint(Key i, const Pose2& p); - - /// Adds a Pose2 prior with mean p and given noise model on pose i - void addPosePrior(Key i, const Pose2& p, const SharedNoiseModel& model); - - /// Creates an relative pose factor between poses with keys i1 and i2 - void addRelativePose(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model); - }; - -} // pose2SLAM - -/** - * Backwards compatibility and wrap use only, avoid using - */ -namespace pose2SLAM { - typedef gtsam::NonlinearEquality HardConstraint; ///< \deprecated typedef for backwards compatibility - typedef gtsam::PriorFactor Prior; ///< \deprecated typedef for backwards compatibility - typedef gtsam::BetweenFactor Odometry; ///< \deprecated typedef for backwards compatibility -} diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp deleted file mode 100644 index 07a89e419..000000000 --- a/gtsam/slam/pose3SLAM.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file pose3SLAM.cpp - * @brief: bearing/range measurements in 2D plane - * @author Frank Dellaert - **/ - -#include -#include - -// Use pose3SLAM namespace for specific SLAM instance -namespace pose3SLAM { - - /* ************************************************************************* */ - Values Values::Circle(size_t n, double radius) { - Values x; - double theta = 0, dtheta = 2 * M_PI / n; - // We use aerospace/navlab convention, X forward, Y right, Z down - // First pose will be at (R,0,0) - // ^y ^ X - // | | - // z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) - // Vehicle at p0 is looking towards y axis (X-axis points towards world y) - Rot3 gR0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); - for (size_t i = 0; i < n; i++, theta += dtheta) { - Point3 gti(radius*cos(theta), radius*sin(theta), 0); - Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down ! - Pose3 gTi(gR0 * _0Ri, gti); - x.insert(i, gTi); - } - return x; - } - - /* ************************************************************************* */ - Matrix Values::translations() const { - size_t j=0; - ConstFiltered poses = filter(); - Matrix result(poses.size(),3); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) - result.row(j++) = keyValue.value.translation().vector(); - return result; - } - - /* ************************************************************************* */ - void Graph::addPoseConstraint(Key i, const Pose3& p) { - sharedFactor factor(new NonlinearEquality(i, p)); - push_back(factor); - } - - /* ************************************************************************* */ - void Graph::addPosePrior(Key i, const Pose3& p, const SharedNoiseModel& model) { - sharedFactor factor(new PriorFactor(i, p, model)); - push_back(factor); - } - - /* ************************************************************************* */ - void Graph::addRelativePose(Key i1, Key i2, const Pose3& z, const SharedNoiseModel& model) { - push_back(boost::make_shared >(i1, i2, z, model)); - } - - /* ************************************************************************* */ - -} // pose3SLAM diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h deleted file mode 100644 index 20ea8b0d5..000000000 --- a/gtsam/slam/pose3SLAM.h +++ /dev/null @@ -1,120 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file pose3SLAM.h - * @brief: 3D Pose SLAM - * @author Frank Dellaert - **/ - -#pragma once - -#include -#include -#include -#include -#include - -/// Use pose3SLAM namespace for specific SLAM instance -namespace pose3SLAM { - - using namespace gtsam; - - /* - * Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Values: public gtsam::Values { - - typedef boost::shared_ptr shared_ptr; - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : - gtsam::Values(values) { - } - - /** - * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) - * @param n number of poses - * @param R radius of circle - * @return circle of n 3D poses - */ - static Values Circle(size_t n, double R); - - /// insert a pose - void insertPose(Key i, const Pose3& pose) { insert(i, pose); } - - /// update a pose - void updatePose(Key i, const Pose3& pose) { update(i, pose); } - - /// get a pose - Pose3 pose(Key i) const { return at(i); } - - Matrix translations() const; ///< get all pose translations coordinates in a matrix - }; - - /** - * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Graph: public EasyFactorGraph { - - /// Default constructor - Graph(){} - - /// Copy constructor given any other NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph): - EasyFactorGraph(graph) {} - - /** - * Add a prior on a pose - * @param key variable key of the camera pose - * @param p around which soft prior is defined - * @param model uncertainty model of this prior - */ - void addPosePrior(Key poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)); - - /** - * Add a constraint on a pose (for now, *must* be satisfied in any Values) - * @param key variable key of the camera pose - * @param p to which pose to constrain it to - */ - void addPoseConstraint(Key poseKey, const Pose3& p = Pose3()); - - /** - * Add a relative pose measurement between two poses - * @param x1 variable key of the first camera pose - * @param x2 variable key of the second camera pose - * @param relative pose measurement from x1 to x2 (x1.between(x2)) - * @param model uncertainty model of this measurement - */ - void addRelativePose(Key x1, Key x2, const Pose3& z, const SharedNoiseModel& model); - }; - -} // pose3SLAM - -/** - * Backwards compatibility and wrap use only, avoid using - */ -namespace pose3SLAM { - typedef gtsam::PriorFactor Prior; ///< \deprecated typedef for backwards compatibility - typedef gtsam::BetweenFactor Constraint; ///< \deprecated typedef for backwards compatibility - typedef gtsam::NonlinearEquality HardConstraint; ///< \deprecated typedef for backwards compatibility -} -namespace gtsam { - typedef pose3SLAM::Prior Pose3Prior; ///< \deprecated typedef for backwards compatibility - typedef pose3SLAM::Constraint Pose3Factor; ///< \deprecated typedef for backwards compatibility - typedef pose3SLAM::Graph Pose3Graph; ///< \deprecated typedef for backwards compatibility -} - diff --git a/gtsam/slam/sparseBA.cpp b/gtsam/slam/sparseBA.cpp deleted file mode 100644 index 0ff4e1c64..000000000 --- a/gtsam/slam/sparseBA.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/** - * @file sparseBA.cpp - * @brief - * @date Jul 6, 2012 - * @author Yong-Dian Jian - */ - -#include - -namespace sparseBA { - -/* ************************************************************************* */ -void Graph::addSimpleCameraConstraint(Key cameraKey, const SimpleCamera &camera) { - addCameraConstraint(cameraKey, camera); -} - -/* ************************************************************************* */ -void Graph::addSimpleCameraPrior(Key cameraKey, const SimpleCamera &camera, SharedNoiseModel &model) { - addCameraPrior(cameraKey, camera, model); -} - -/* ************************************************************************* */ -void Graph::addSimpleCameraMeasurement(const Point2 &z, SharedNoiseModel& model, Index cameraKey, Index pointKey) { - addMeasurement(z, model, cameraKey, pointKey); -} - -/* ************************************************************************* */ -Matrix Graph::reprojectionErrors(const Values& values) const { - - // TODO: support the other calibration objects. Now it only works for Cal3_S2. - - typedef GeneralSFMFactor SFMFactor; - typedef GeneralSFMFactor2 SFMFactor2; - - // first count - size_t K = 0, k=0; - BOOST_FOREACH(const sharedFactor& f, *this) - if (boost::dynamic_pointer_cast(f)) ++K; - else if (boost::dynamic_pointer_cast(f)) ++K; - - // now fill - Matrix errors(2,K); - BOOST_FOREACH(const sharedFactor& f, *this) { - boost::shared_ptr p = boost::dynamic_pointer_cast(f); - if (p) { - errors.col(k++) = p->unwhitenedError(values); - continue; - } - - boost::shared_ptr p2 = boost::dynamic_pointer_cast(f); - if (p2) { - errors.col(k++) = p2->unwhitenedError(values); - } - } - return errors; -} -/* ************************************************************************* */ -} - - diff --git a/gtsam/slam/sparseBA.h b/gtsam/slam/sparseBA.h deleted file mode 100644 index 07aaa5817..000000000 --- a/gtsam/slam/sparseBA.h +++ /dev/null @@ -1,143 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file sba.h - * @brief a suite for sparse bundle adjustment - * @date Jul 5, 2012 - * @author ydjian - */ - -#pragma once - -#include -#include -#include - -namespace sparseBA { - - using namespace gtsam; - - /// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper - struct Values: public visualSLAM::Values { - - typedef boost::shared_ptr shared_ptr; - typedef gtsam::Values::ConstFiltered SimpleCameraFiltered; - typedef gtsam::Values::ConstFiltered Cal3_S2Filtered; - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : visualSLAM::Values(values) {} - - /// Constructor from filtered values view of poses - Values(const SimpleCameraFiltered& view) : visualSLAM::Values(view) {} - - /// Constructor from filtered values view of points - Values(const PointFiltered& view) : visualSLAM::Values(view) {} - - SimpleCameraFiltered allSimpleCameras() const { return this->filter(); } ///< camera view - size_t nrSimpleCameras() const { return allSimpleCameras().size(); } ///< get number of poses - KeyList simpleCameraKeys() const { return allSimpleCameras().keys(); } ///< get keys to poses only - - /// insert a camera - void insertSimpleCamera(Key j, const SimpleCamera& camera) { insert(j, camera); } - - /// update a camera - void updateSimpleCamera(Key j, const SimpleCamera& camera) { update(j, camera); } - - /// get a camera - SimpleCamera simpleCamera(Key j) const { return at(j); } - }; - - /** - * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper - * @addtogroup SLAM - */ - struct Graph: public visualSLAM::Graph { - - /// Default constructor - Graph(){} - - /// Copy constructor given any other NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph): visualSLAM::Graph(graph) {} - - /// check if two graphs are equal - bool equals(const Graph& p, double tol = 1e-9) const { - return NonlinearFactorGraph::equals(p, tol); - } - - /** - * Add a prior on a pose - * @param key variable key of the camera - * @param p around which soft prior is defined - * @param model uncertainty model of this prior - */ - template - void addCameraPrior(Key cameraKey, const Camera &camera, SharedNoiseModel &model = noiseModel::Unit::Create(Camera::Dim())) { - sharedFactor factor(new PriorFactor(cameraKey, camera, model)); - push_back(factor); - } - - /** - * Add a constraint on a camera - * @param key variable key of the camera - * @param p to which camera to constrain it to - */ - template - void addCameraConstraint(Key cameraKey, const Camera &camera) { - sharedFactor factor(new NonlinearEquality(cameraKey, camera)); - push_back(factor); - } - - /** - * Add a 2d projection measurement - * @param z the measurement - * @param model the noise model for the measurement - * @param cameraKey variable key for the pose+calibration - * @param pointKey variable key for the point - */ - template - void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index cameraKey, const Index pointKey) { - typedef GeneralSFMFactor SFMFactor; - boost::shared_ptr factor(new SFMFactor(z, model, cameraKey, pointKey)); - push_back(factor); - } - - /** - * Add a 2d projection measurement, but supports separated (or shared) pose and calibration object - * @param z the measurement - * @param model the noise model for the measurement - * @param poseKey variable key for the pose - * @param pointKey variable key for the point - * @param calibKey variable key for the calibration - */ - template - void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index posekey, const Index pointkey, const Index calibkey) { - typedef GeneralSFMFactor2 SFMFactor; - boost::shared_ptr factor(new SFMFactor(z, model, posekey, pointkey, calibkey)); - push_back(factor); - } - - /// Return a 2*K Matrix of reprojection errors - Matrix reprojectionErrors(const Values& values) const; - - /** - * Matlab-specific wrappers - */ - - void addSimpleCameraPrior(Key cameraKey, const SimpleCamera &camera, SharedNoiseModel &model); - void addSimpleCameraConstraint(Key cameraKey, const SimpleCamera &camera) ; - void addSimpleCameraMeasurement(const Point2 &z, SharedNoiseModel& model, Index cameraKey, Index pointKey); - }; - -} diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index d99313767..0dd576aed 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -17,10 +17,12 @@ #include -#include #include -#include +#include +#include #include +#include +#include #include #include @@ -95,12 +97,12 @@ TEST( AntiFactor, EquivalentBayesNet) Pose3 z(Rot3(), Point3(1, 1, 1)); SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); - boost::shared_ptr graph(new pose3SLAM::Graph()); - graph->addPosePrior(1, pose1, sigma); - graph->addRelativePose(1, 2, pose1.between(pose2), sigma); + NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph()); + graph->add(PriorFactor(1, pose1, sigma)); + graph->add(BetweenFactor(1, 2, pose1.between(pose2), sigma)); // Create a configuration corresponding to the ground truth - boost::shared_ptr values(new Values()); + Values::shared_ptr values(new Values()); values->insert(1, pose1); values->insert(2, pose2); diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp deleted file mode 100644 index 5652b71b4..000000000 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ /dev/null @@ -1,218 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testPlanarSLAM.cpp - * @author Frank Dellaert - **/ - -#include -#include -#include -#include - -#include -#include - -using namespace std; -using namespace gtsam; - -// some shared test values -static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); -static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); -static Symbol i2('x',2), i3('x',3), j3('l',3); - -SharedNoiseModel - sigma(noiseModel::Isotropic::Sigma(1,0.1)), - sigma2(noiseModel::Isotropic::Sigma(2,0.1)), - I3(noiseModel::Unit::Create(3)); - -/* ************************************************************************* */ -TEST( planarSLAM, PriorFactor_equals ) -{ - PriorFactor factor1(i2, x1, I3), factor2(i2, x2, I3); - EXPECT(assert_equal(factor1, factor1, 1e-5)); - EXPECT(assert_equal(factor2, factor2, 1e-5)); - EXPECT(assert_inequal(factor1, factor2, 1e-5)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, BearingFactor ) -{ - // Create factor - Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 - BearingFactor factor(i2, j3, z, sigma); - - // create config - planarSLAM::Values c; - c.insert(i2, x2); - c.insert(j3, l3); - - // Check error - Vector actual = factor.unwhitenedError(c); - EXPECT(assert_equal(Vector_(1,-0.1),actual)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, BearingFactor_equals ) -{ - BearingFactor - factor1(i2, j3, Rot2::fromAngle(0.1), sigma), - factor2(i2, j3, Rot2::fromAngle(2.3), sigma); - EXPECT(assert_equal(factor1, factor1, 1e-5)); - EXPECT(assert_equal(factor2, factor2, 1e-5)); - EXPECT(assert_inequal(factor1, factor2, 1e-5)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, RangeFactor ) -{ - // Create factor - double z(sqrt(2.0) - 0.22); // h(x) - z = 0.22 - RangeFactor factor(i2, j3, z, sigma); - - // create config - planarSLAM::Values c; - c.insert(i2, x2); - c.insert(j3, l3); - - // Check error - Vector actual = factor.unwhitenedError(c); - EXPECT(assert_equal(Vector_(1,0.22),actual)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, RangeFactor_equals ) -{ - RangeFactor factor1(i2, j3, 1.2, sigma), factor2(i2, j3, 7.2, sigma); - EXPECT(assert_equal(factor1, factor1, 1e-5)); - EXPECT(assert_equal(factor2, factor2, 1e-5)); - EXPECT(assert_inequal(factor1, factor2, 1e-5)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, BearingRangeFactor ) -{ - // Create factor - Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 - double b(sqrt(2.0) - 0.22); // h(x) - z = 0.22 - BearingRangeFactor factor(i2, j3, r, b, sigma2); - - // create config - planarSLAM::Values c; - c.insert(i2, x2); - c.insert(j3, l3); - - // Check error - Vector actual = factor.unwhitenedError(c); - EXPECT(assert_equal(Vector_(2,-0.1, 0.22),actual)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, BearingRangeFactor_equals ) -{ - BearingRangeFactor - factor1(i2, j3, Rot2::fromAngle(0.1), 7.3, sigma2), - factor2(i2, j3, Rot2::fromAngle(3), 2.0, sigma2); - EXPECT(assert_equal(factor1, factor1, 1e-5)); - EXPECT(assert_equal(factor2, factor2, 1e-5)); - EXPECT(assert_inequal(factor1, factor2, 1e-5)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, BearingRangeFactor_poses ) -{ - typedef BearingRangeFactor PoseBearingRange; - PoseBearingRange actual(2, 3, Rot2::fromDegrees(60.0), 12.3, sigma2); -} - -/* ************************************************************************* */ -TEST( planarSLAM, PoseConstraint_equals ) -{ - NonlinearEquality factor1(i2, x2), factor2(i2, x3); - EXPECT(assert_equal(factor1, factor1, 1e-5)); - EXPECT(assert_equal(factor2, factor2, 1e-5)); - EXPECT(assert_inequal(factor1, factor2, 1e-5)); -} - -/* ************************************************************************* */ -TEST( planarSLAM, constructor ) -{ - // create config - planarSLAM::Values c; - c.insert(i2, x2); - c.insert(i3, x3); - c.insert(j3, l3); - - // create graph - planarSLAM::Graph G; - - // Add pose constraint - G.addPoseConstraint(i2, x2); // make it feasible :-) - - // Add odometry - G.addRelativePose(i2, i3, Pose2(0, 0, M_PI_4), I3); - - // Create bearing factor - Rot2 z1 = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 - G.addBearing(i2, j3, z1, sigma); - - // Create range factor - double z2(sqrt(2.0) - 0.22); // h(x) - z = 0.22 - G.addRange(i2, j3, z2, sigma); - - Vector expected0 = Vector_(3, 0.0, 0.0, 0.0); - Vector expected1 = Vector_(3, 0.0, 0.0, 0.0); - Vector expected2 = Vector_(1, -0.1); - Vector expected3 = Vector_(1, 0.22); - // Get NoiseModelFactors - EXPECT(assert_equal(expected0, boost::dynamic_pointer_cast(G[0])->unwhitenedError(c))); - EXPECT(assert_equal(expected1, boost::dynamic_pointer_cast(G[1])->unwhitenedError(c))); - EXPECT(assert_equal(expected2, boost::dynamic_pointer_cast(G[2])->unwhitenedError(c))); - EXPECT(assert_equal(expected3, boost::dynamic_pointer_cast(G[3])->unwhitenedError(c))); -} - -/* ************************************************************************* */ -TEST( planarSLAM, keys_and_view ) -{ - // create config - planarSLAM::Values c; - c.insert(i2, x2); - c.insert(i3, x3); - c.insert(j3, l3); - LONGS_EQUAL(2,c.nrPoses()); - LONGS_EQUAL(1,c.nrPoints()); - { - FastList expected, actual; - expected += j3, i2, i3; - actual = c.keys(); - CHECK(expected == actual); - } - { - FastList expected, actual; - expected += i2, i3; - actual = c.poseKeys(); - CHECK(expected == actual); - } - { - FastList expected, actual; - expected += j3; - actual = c.pointKeys(); - CHECK(expected == actual); - } -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp deleted file mode 100644 index 766d06ded..000000000 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ /dev/null @@ -1,608 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testPose2SLAM.cpp - * @author Frank Dellaert, Viorela Ila - **/ - -#include -#include -#include -#include -#include -#include -using namespace gtsam; - -#include - -#include -#include -using namespace boost; -using namespace boost::assign; - -#include -using namespace std; - -typedef BetweenFactor Pose2Factor; - -// common measurement covariance -static double sx=0.5, sy=0.5,st=0.1; -static Matrix cov(Matrix_(3, 3, - sx*sx, 0.0, 0.0, - 0.0, sy*sy, 0.0, - 0.0, 0.0, st*st - )); -static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Gaussian::Covariance(cov)); - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2SLAM, XYT ) -{ - pose2SLAM::Values values; - values.insertPose(1,Pose2(1,2,3)); - values.insertPose(2,Pose2(4,5,6)); - Matrix expected = Matrix_(2,3, 1.0,2.0,3.0, 4.0,5.0,6.0-2*M_PI); - EXPECT(assert_equal(expected,values.poses())); -} - -/* ************************************************************************* */ -// Test constraint, small displacement -Vector f1(const Pose2& pose1, const Pose2& pose2) { - Pose2 z(2.1130913087, 0.468461064817, 0.436332312999); - Pose2Factor constraint(1, 2, z, covariance); - return constraint.evaluateError(pose1, pose2); -} - -TEST_UNSAFE( Pose2SLAM, constraint1 ) -{ - // create a factor between unknown poses p1 and p2 - Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999); - Pose2Factor constraint(1, 2, pose2, covariance); - Matrix H1, H2; - Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); - - Matrix numericalH1 = numericalDerivative21(&f1 , pose1, pose2); - EXPECT(assert_equal(numericalH1,H1,5e-3)); - - Matrix numericalH2 = numericalDerivative22(&f1 , pose1, pose2); - EXPECT(assert_equal(numericalH2,H2)); -} - -/* ************************************************************************* */ -// Test constraint, large displacement -Vector f2(const Pose2& pose1, const Pose2& pose2) { - Pose2 z(2,2,M_PI_2); - Pose2Factor constraint(1, 2, z, covariance); - return constraint.evaluateError(pose1, pose2); -} - -TEST_UNSAFE( Pose2SLAM, constraint2 ) -{ - // create a factor between unknown poses p1 and p2 - Pose2 pose1, pose2(2,2,M_PI_2); - Pose2Factor constraint(1, 2, pose2, covariance); - Matrix H1, H2; - Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); - - Matrix numericalH1 = numericalDerivative21(&f2 , pose1, pose2); - EXPECT(assert_equal(numericalH1,H1,5e-3)); - - Matrix numericalH2 = numericalDerivative22(&f2 , pose1, pose2); - EXPECT(assert_equal(numericalH2,H2)); -} - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2SLAM, constructor ) -{ - // create a factor between unknown poses p1 and p2 - Pose2 measured(2,2,M_PI_2); - pose2SLAM::Graph graph; - graph.addRelativePose(1,2,measured, covariance); - // get the size of the graph - size_t actual = graph.size(); - // verify - size_t expected = 1; - CHECK(actual == expected); -} - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2SLAM, linearization ) -{ - // create a factor between unknown poses p1 and p2 - Pose2 measured(2,2,M_PI_2); - Pose2Factor constraint(1, 2, measured, covariance); - pose2SLAM::Graph graph; - graph.addRelativePose(1,2,measured, covariance); - - // Choose a linearization point - Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) - Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2) - pose2SLAM::Values config; - config.insert(1,p1); - config.insert(2,p2); - // Linearize - Ordering ordering(*config.orderingArbitrary()); - boost::shared_ptr > lfg_linearized = graph.linearize(config, ordering); - //lfg_linearized->print("lfg_actual"); - - // the expected linear factor - FactorGraph lfg_expected; - Matrix A1 = Matrix_(3,3, - 0.0,-2.0, -4.2, - 2.0, 0.0, -4.2, - 0.0, 0.0,-10.0); - - Matrix A2 = Matrix_(3,3, - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 10.0); - - Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0); - SharedDiagonal probModel1 = noiseModel::Unit::Create(3); - lfg_expected.push_back(JacobianFactor::shared_ptr(new JacobianFactor(ordering[1], A1, ordering[2], A2, b, probModel1))); - - CHECK(assert_equal(lfg_expected, *lfg_linearized)); -} - -/* ************************************************************************* */ -TEST_UNSAFE(Pose2SLAM, optimize) { - - // create a Pose graph with one equality constraint and one measurement - pose2SLAM::Graph fg; - fg.addPoseConstraint(0, Pose2(0,0,0)); - fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), covariance); - - // Create initial config - Values initial; - initial.insert(0, Pose2(0,0,0)); - initial.insert(1, Pose2(0,0,0)); - - // Choose an ordering and optimize - Ordering ordering; - ordering += 0, 1; - - LevenbergMarquardtParams params; - params.relativeErrorTol = 1e-15; - params.ordering = ordering; - Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize(); - - // Check with expected config - Values expected; - expected.insert(0, Pose2(0,0,0)); - expected.insert(1, Pose2(1,2,M_PI_2)); - CHECK(assert_equal(expected, actual)); - - // Check marginals - Marginals marginals = fg.marginals(actual); - // Matrix expectedP0 = Infinity, as we have a pose constraint !? - // Matrix actualP0 = marginals.marginalCovariance(0); - // EQUALITY(expectedP0, actualP0); - Matrix expectedP1 = cov; // the second pose really should have just the noise covariance - Matrix actualP1 = marginals.marginalCovariance(1); - EQUALITY(expectedP1, actualP1); -} - -/* ************************************************************************* */ -TEST_UNSAFE(Pose2SLAM, optimizeSPCG) { - - // create a Pose graph with one equality constraint and one measurement - pose2SLAM::Graph fg; - fg.addPosePrior(0, Pose2(0,0,0), noiseModel::Diagonal::Sigmas(Vector_(3,3.0,3.0,1.0))); - fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), covariance); - - // [Duy] For some unknown reason, SPCG needs this constraint to work. GaussNewton doesn't need this. - fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1))); - - // Create initial config - Values initial; - initial.insert(0, Pose2(0,0,0)); - initial.insert(1, Pose2(0,0,0)); - - // Optimize using SPCG - Values actual = fg.optimizeSPCG(initial,0); - - // Try GaussNewton without the above constraint to see if the problem is underconstrained. Still works! - Values actual2 = GaussNewtonOptimizer(fg, initial).optimize(); - - // Check with expected config - Values expected; - expected.insert(0, Pose2(0,0,0)); - expected.insert(1, Pose2(1,2,M_PI_2)); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(expected, actual2)); -} - -/* ************************************************************************* */ -// test optimization with 3 poses, note we use plain Keys here, not symbols -TEST_UNSAFE(Pose2SLAM, optimizeThreePoses) { - - // Create a hexagon of poses - pose2SLAM::Values hexagon = pose2SLAM::Values::Circle(3,1.0); - Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1); - - // create a Pose graph with one equality constraint and one measurement - pose2SLAM::Graph fg; - fg.addPoseConstraint(0, p0); - Pose2 delta = p0.between(p1); - fg.addRelativePose(0, 1, delta, covariance); - fg.addRelativePose(1, 2, delta, covariance); - fg.addRelativePose(2, 0, delta, covariance); - - // Create initial config - pose2SLAM::Values initial; - initial.insertPose(0, p0); - initial.insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1))); - initial.insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1))); - - // Choose an ordering - Ordering ordering; - ordering += 0,1,2; - - // optimize - LevenbergMarquardtParams params; - params.relativeErrorTol = 1e-15; - params.ordering = ordering; - Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize(); - - // Check with ground truth - CHECK(assert_equal((const Values&)hexagon, actual)); -} - -/* ************************************************************************* */ -// test optimization with 6 poses arranged in a hexagon and a loop closure -TEST_UNSAFE(Pose2SLAM, optimizeCircle) { - - // Create a hexagon of poses - pose2SLAM::Values hexagon = pose2SLAM::Values::Circle(6,1.0); - Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1); - - // create a Pose graph with one equality constraint and one measurement - pose2SLAM::Graph fg; - fg.addPoseConstraint(0, p0); - Pose2 delta = p0.between(p1); - fg.addRelativePose(0,1, delta, covariance); - fg.addRelativePose(1,2, delta, covariance); - fg.addRelativePose(2,3, delta, covariance); - fg.addRelativePose(3,4, delta, covariance); - fg.addRelativePose(4,5, delta, covariance); - fg.addRelativePose(5,0, delta, covariance); - - // Create initial config - pose2SLAM::Values initial; - initial.insertPose(0, p0); - initial.insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1))); - initial.insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1))); - initial.insertPose(3, hexagon.pose(3).retract(Vector_(3,-0.1, 0.1,-0.1))); - initial.insertPose(4, hexagon.pose(4).retract(Vector_(3, 0.1,-0.1, 0.1))); - initial.insertPose(5, hexagon.pose(5).retract(Vector_(3,-0.1, 0.1,-0.1))); - - // Choose an ordering - Ordering ordering; - ordering += 0,1,2,3,4,5; - - // optimize - LevenbergMarquardtParams params; - params.relativeErrorTol = 1e-15; - params.ordering = ordering; - Values actual = LevenbergMarquardtOptimizer(fg, initial, params).optimize(); - - // Check with ground truth - CHECK(assert_equal((const Values&)hexagon, actual)); - - // Check loop closure - CHECK(assert_equal(delta, actual.at(5).between(actual.at(0)))); - -// Pose2SLAMOptimizer myOptimizer("3"); - -// Matrix A1 = myOptimizer.a1(); -// LONGS_EQUAL(3, A1.rows()); -// LONGS_EQUAL(17, A1.cols()); // 7 + 7 + 3 -// -// Matrix A2 = myOptimizer.a2(); -// LONGS_EQUAL(3, A1.rows()); -// LONGS_EQUAL(7, A2.cols()); // 7 -// -// Vector b1 = myOptimizer.b1(); -// LONGS_EQUAL(9, b1.size()); // 3 + 3 + 3 -// -// Vector b2 = myOptimizer.b2(); -// LONGS_EQUAL(3, b2.size()); // 3 -// -// // Here, call matlab to -// // A=[A1;A2], b=[b1;b2] -// // R=qr(A1) -// // call pcg on A,b, with preconditioner R -> get x -// -// Vector x = myOptimizer.optimize(); -// LONGS_EQUAL(9, x.size()); // 3 + 3 + 3 -// -// myOptimizer.update(x); -// -// Values expected; -// expected.insert(0, Pose2(0.,0.,0.)); -// expected.insert(1, Pose2(1.,0.,0.)); -// expected.insert(2, Pose2(2.,0.,0.)); -// -// // Check with ground truth -// CHECK(assert_equal(expected, *myOptimizer.theta())); -} - -/* ************************************************************************* */ -TEST_UNSAFE(Pose2SLAM, optimize2) { -// Pose2SLAMOptimizer myOptimizer("100"); -// Matrix A1 = myOptimizer.a1(); -// Matrix A2 = myOptimizer.a2(); -// cout << "A1: " << A1.rows() << " " << A1.cols() << endl; -// cout << "A2: " << A2.rows() << " " << A2.cols() << endl; -// -// //cout << "error: " << myOptimizer.error() << endl; -// for(int i = 0; i<10; i++) { -// myOptimizer.linearize(); -// Vector x = myOptimizer.optimize(); -// myOptimizer.update(x); -// } -// //cout << "error: " << myOptimizer.error() << endl; -// CHECK(myOptimizer.error() < 1.); -} - -///* ************************************************************************* */ -// SL-NEEDED? TEST_UNSAFE(Pose2SLAM, findMinimumSpanningTree) { -// pose2SLAM::Graph G, T, C; -// G.addPoseConstraint(1, 2, Pose2(0.,0.,0.), I3); -// G.addPoseConstraint(1, 3, Pose2(0.,0.,0.), I3); -// G.addPoseConstraint(2, 3, Pose2(0.,0.,0.), I3); -// -// PredecessorMap tree = -// G.findMinimumSpanningTree(); -// CHECK(tree[1] == 1); -// CHECK(tree[2] == 1); -// CHECK(tree[3] == 1); -//} -// -///* ************************************************************************* */ -// SL-NEEDED? TEST_UNSAFE(Pose2SLAM, split) { -// pose2SLAM::Graph G, T, C; -// G.addPoseConstraint(1, 2, Pose2(0.,0.,0.), I3); -// G.addPoseConstraint(1, 3, Pose2(0.,0.,0.), I3); -// G.addPoseConstraint(2, 3, Pose2(0.,0.,0.), I3); -// -// PredecessorMap tree; -// tree.insert(1,2); -// tree.insert(2,2); -// tree.insert(3,2); -// -// G.split(tree, T, C); -// LONGS_EQUAL(2, T.size()); -// LONGS_EQUAL(1, C.size()); -//} - -using namespace pose2SLAM; - -/* ************************************************************************* */ -TEST_UNSAFE(Pose2Values, pose2Circle ) -{ - // expected is 4 poses tangent to circle with radius 1m - pose2SLAM::Values expected; - expected.insert(0, Pose2( 1, 0, M_PI_2)); - expected.insert(1, Pose2( 0, 1, - M_PI )); - expected.insert(2, Pose2(-1, 0, - M_PI_2)); - expected.insert(3, Pose2( 0, -1, 0 )); - - pose2SLAM::Values actual = pose2SLAM::Values::Circle(4,1.0); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST_UNSAFE(Pose2SLAM, expmap ) -{ - // expected is circle shifted to right - pose2SLAM::Values expected; - expected.insert(0, Pose2( 1.1, 0, M_PI_2)); - expected.insert(1, Pose2( 0.1, 1, - M_PI )); - expected.insert(2, Pose2(-0.9, 0, - M_PI_2)); - expected.insert(3, Pose2( 0.1, -1, 0 )); - - // Note expmap coordinates are in local coordinates, so shifting to right requires thought !!! - pose2SLAM::Values circle = pose2SLAM::Values::Circle(4,1.0); - Ordering ordering(*circle.orderingArbitrary()); - VectorValues delta(circle.dims(ordering)); - delta[ordering[0]] = Vector_(3, 0.0,-0.1,0.0); - delta[ordering[1]] = Vector_(3, -0.1,0.0,0.0); - delta[ordering[2]] = Vector_(3, 0.0,0.1,0.0); - delta[ordering[3]] = Vector_(3, 0.1,0.0,0.0); - pose2SLAM::Values actual = circle.retract(delta, ordering); - CHECK(assert_equal(expected,actual)); -} - -// Common measurement covariance -static SharedNoiseModel sigmas = noiseModel::Diagonal::Sigmas(Vector_(3,sx,sy,st)); - -/* ************************************************************************* */ -// Very simple test establishing Ax-b \approx z-h(x) -TEST_UNSAFE( Pose2Prior, error ) -{ - // Choose a linearization point - Pose2 p1(1, 0, 0); // robot at (1,0) - pose2SLAM::Values x0; - x0.insert(1, p1); - - // Create factor - PriorFactor factor(1, p1, sigmas); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr linear = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - - // Check error at x0, i.e. delta = zero ! - VectorValues delta(VectorValues::Zero(x0.dims(ordering))); - delta[ordering[1]] = zero(3); - Vector error_at_zero = Vector_(3,0.0,0.0,0.0); - CHECK(assert_equal(error_at_zero,factor.whitenedError(x0))); - CHECK(assert_equal(-error_at_zero,linear->error_vector(delta))); - - // Check error after increasing p2 - VectorValues addition(VectorValues::Zero(x0.dims(ordering))); - addition[ordering[1]] = Vector_(3, 0.1, 0.0, 0.0); - VectorValues plus = delta + addition; - pose2SLAM::Values x1 = x0.retract(plus, ordering); - Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! - CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); - CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); -} - -/* ************************************************************************* */ -// common Pose2Prior for tests below -static gtsam::Pose2 priorVal(2,2,M_PI_2); -static PriorFactor priorFactor(1, priorVal, sigmas); - -/* ************************************************************************* */ -// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector -// Hence i.e., b = approximates z-h(x0) = error_vector(x0) -LieVector hprior(const Pose2& p1) { - return LieVector(sigmas->whiten(priorFactor.evaluateError(p1))); -} - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2Prior, linearize ) -{ - // Choose a linearization point at ground truth - pose2SLAM::Values x0; - x0.insertPose(1, priorVal); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr actual = - boost::dynamic_pointer_cast(priorFactor.linearize(x0, ordering)); - - // Test with numerical derivative - Matrix numericalH = numericalDerivative11(hprior, priorVal); - CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering[1])))); -} - -/* ************************************************************************* */ -// Very simple test establishing Ax-b \approx z-h(x) -TEST_UNSAFE( Pose2Factor, error ) -{ - // Choose a linearization point - Pose2 p1; // robot at origin - Pose2 p2(1, 0, 0); // robot at (1,0) - pose2SLAM::Values x0; - x0.insertPose(1, p1); - x0.insertPose(2, p2); - - // Create factor - Pose2 z = p1.between(p2); - Pose2Factor factor(1, 2, z, covariance); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr linear = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - - // Check error at x0, i.e. delta = zero ! - VectorValues delta(x0.dims(ordering)); - delta[ordering[1]] = zero(3); - delta[ordering[2]] = zero(3); - Vector error_at_zero = Vector_(3,0.0,0.0,0.0); - CHECK(assert_equal(error_at_zero,factor.unwhitenedError(x0))); - CHECK(assert_equal(-error_at_zero, linear->error_vector(delta))); - - // Check error after increasing p2 - VectorValues plus = delta; - plus[ordering[2]] = Vector_(3, 0.1, 0.0, 0.0); - pose2SLAM::Values x1 = x0.retract(plus, ordering); - Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! - CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); - CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); -} - -/* ************************************************************************* */ -// common Pose2Factor for tests below -static Pose2 measured(2,2,M_PI_2); -static Pose2Factor factor(1,2,measured, covariance); - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2Factor, rhs ) -{ - // Choose a linearization point - Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) - Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4) - pose2SLAM::Values x0; - x0.insert(1,p1); - x0.insert(2,p2); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr linear = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - - // Check RHS - Pose2 hx0 = p1.between(p2); - CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0)); - Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0); - CHECK(assert_equal(expected_b,-factor.whitenedError(x0))); - CHECK(assert_equal(expected_b,linear->getb())); -} - -/* ************************************************************************* */ -// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector -// Hence i.e., b = approximates z-h(x0) = error_vector(x0) -LieVector h(const Pose2& p1,const Pose2& p2) { - return LieVector(covariance->whiten(factor.evaluateError(p1,p2))); -} - -/* ************************************************************************* */ -TEST_UNSAFE( Pose2Factor, linearize ) -{ - // Choose a linearization point at ground truth - Pose2 p1(1,2,M_PI_2); - Pose2 p2(-1,4,M_PI); - pose2SLAM::Values x0; - x0.insert(1,p1); - x0.insert(2,p2); - - // expected linearization - Matrix expectedH1 = covariance->Whiten(Matrix_(3,3, - 0.0,-1.0,-2.0, - 1.0, 0.0,-2.0, - 0.0, 0.0,-1.0 - )); - Matrix expectedH2 = covariance->Whiten(Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0 - )); - Vector expected_b = Vector_(3, 0.0, 0.0, 0.0); - - // expected linear factor - Ordering ordering(*x0.orderingArbitrary()); - SharedDiagonal probModel1 = noiseModel::Unit::Create(3); - JacobianFactor expected(ordering[1], expectedH1, ordering[2], expectedH2, expected_b, probModel1); - - // Actual linearization - boost::shared_ptr actual = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - CHECK(assert_equal(expected,*actual)); - - // Numerical do not work out because BetweenFactor is approximate ? - Matrix numericalH1 = numericalDerivative21(h, p1, p2); - CHECK(assert_equal(expectedH1,numericalH1)); - Matrix numericalH2 = numericalDerivative22(h, p1, p2); - CHECK(assert_equal(expectedH2,numericalH2)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp deleted file mode 100644 index 23367bd8a..000000000 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ /dev/null @@ -1,211 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testpose3SLAM.cpp - * @author Frank Dellaert - * @author Viorela Ila - **/ - -#include -#include -#include - -#include - -#include -#include -#include -using namespace boost; -using namespace boost::assign; - -#include - -using namespace std; -using namespace gtsam; - -// common measurement covariance -static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Unit::Create(6)); - -const double tol=1e-5; - -/* ************************************************************************* */ -// test optimization with 6 poses arranged in a hexagon and a loop closure -TEST(Pose3Graph, optimizeCircle) { - - // Create a hexagon of poses - double radius = 10; - Values hexagon = pose3SLAM::Values::Circle(6,radius); - Pose3 gT0 = hexagon.at(0), gT1 = hexagon.at(1); - - // create a Pose graph with one equality constraint and one measurement - pose3SLAM::Graph fg; - fg.addPoseConstraint(0, gT0); - Pose3 _0T1 = gT0.between(gT1); // inv(gT0)*gT1 - double theta = M_PI/3.0; - CHECK(assert_equal(Pose3(Rot3::yaw(-theta),Point3(radius*sin(theta),-radius*cos(theta),0)),_0T1)); - fg.addRelativePose(0,1, _0T1, covariance); - fg.addRelativePose(1,2, _0T1, covariance); - fg.addRelativePose(2,3, _0T1, covariance); - fg.addRelativePose(3,4, _0T1, covariance); - fg.addRelativePose(4,5, _0T1, covariance); - fg.addRelativePose(5,0, _0T1, covariance); - - // Create initial config - Values initial; - initial.insert(0, gT0); - initial.insert(1, hexagon.at(1).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial.insert(2, hexagon.at(2).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial.insert(3, hexagon.at(3).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial.insert(4, hexagon.at(4).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial.insert(5, hexagon.at(5).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - - // Choose an ordering and optimize - Ordering ordering; - ordering += 0,1,2,3,4,5; - - Values actual = LevenbergMarquardtOptimizer(fg, initial, ordering).optimize(); - - // Check with ground truth - CHECK(assert_equal(hexagon, actual,1e-4)); - - // Check loop closure - CHECK(assert_equal(_0T1, actual.at(5).between(actual.at(0)),1e-5)); -} - -/* ************************************************************************* */ -TEST(Pose3Graph, partial_prior_height) { - typedef PartialPriorFactor Partial; - - // reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3) - - // height prior - single element interface - double exp_height = 5.0; - SharedDiagonal model = noiseModel::Unit::Create(1); - Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height)); - Partial height(1, 5, exp_height, model); - Matrix actA; - EXPECT(assert_equal(Vector_(1,-2.0), height.evaluateError(init, actA), tol)); - Matrix expA = Matrix_(1, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); - EXPECT(assert_equal(expA, actA)); - - pose3SLAM::Graph graph; - graph.add(height); - - Values values; - values.insert(1, init); - - // linearization - EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); - - Values actual = LevenbergMarquardtOptimizer(graph, values).optimize(); - EXPECT(assert_equal(expected, actual.at(1), tol)); - EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); -} - -/* ************************************************************************* */ -TEST( Pose3Factor, error ) -{ - // Create example - Pose3 t1; // origin - Pose3 t2(Rot3::rodriguez(0.1,0.2,0.3),Point3(0,1,0)); - Pose3 z(Rot3::rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));; - - // Create factor - SharedNoiseModel I6(noiseModel::Unit::Create(6)); - BetweenFactor factor(1, 2, z, I6); - - // Create config - Values x; - x.insert(1,t1); - x.insert(2,t2); - - // Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2)) - Vector actual = factor.unwhitenedError(x); - Vector expected = z.localCoordinates(t1.between(t2)); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST(Pose3Graph, partial_prior_xy) { - typedef PartialPriorFactor Partial; - - // XY prior - full mask interface - Vector exp_xy = Vector_(2, 3.0, 4.0); - SharedDiagonal model = noiseModel::Unit::Create(2); - Pose3 init(Rot3(), Point3(1.0,-2.0, 3.0)), expected(Rot3(), Point3(3.0, 4.0, 3.0)); - vector mask; mask += 3, 4; - Partial priorXY(1, mask, exp_xy, model); - Matrix actA; - EXPECT(assert_equal(Vector_(2,-2.0,-6.0), priorXY.evaluateError(init, actA), tol)); - Matrix expA = Matrix_(2, 6, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0, 0.0); - EXPECT(assert_equal(expA, actA)); - - pose3SLAM::Graph graph; - graph.add(priorXY); - - Values values; - values.insert(1, init); - - Values actual = LevenbergMarquardtOptimizer(graph, values).optimize(); - EXPECT(assert_equal(expected, actual.at(1), tol)); - EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); -} - -// The world coordinate system has z pointing up, y north, x east -// The vehicle has X forward, Y right, Z down -Rot3 R1(Point3( 0, 1, 0), Point3( 1, 0, 0), Point3(0, 0, -1)); -Rot3 R2(Point3(-1, 0, 0), Point3( 0, 1, 0), Point3(0, 0, -1)); -Rot3 R3(Point3( 0,-1, 0), Point3(-1, 0, 0), Point3(0, 0, -1)); -Rot3 R4(Point3( 1, 0, 0), Point3( 0,-1, 0), Point3(0, 0, -1)); - -/* ************************************************************************* */ -TEST( Values, pose3Circle ) -{ - // expected is 4 poses tangent to circle with radius 1m - Values expected; - expected.insert(0, Pose3(R1, Point3( 1, 0, 0))); - expected.insert(1, Pose3(R2, Point3( 0, 1, 0))); - expected.insert(2, Pose3(R3, Point3(-1, 0, 0))); - expected.insert(3, Pose3(R4, Point3( 0,-1, 0))); - - Values actual = pose3SLAM::Values::Circle(4,1.0); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( Values, expmap ) -{ - Values expected; - expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0))); - expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0))); - expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0))); - expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0))); - - Ordering ordering(*expected.orderingArbitrary()); - VectorValues delta(expected.dims(ordering)); - delta.vector() = Vector_(24, - 0.0,0.0,0.0, 0.1, 0.0, 0.0, - 0.0,0.0,0.0, 0.1, 0.0, 0.0, - 0.0,0.0,0.0, 0.1, 0.0, 0.0, - 0.0,0.0,0.0, 0.1, 0.0, 0.0); - Values actual = pose3SLAM::Values::Circle(4,1.0).retract(delta, ordering); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index ac9a6891d..1bda97951 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -16,9 +16,13 @@ * @date Nov 2009 */ -#include +#include +#include +#include #include #include +#include +#include #include using namespace std; @@ -80,7 +84,7 @@ TEST( ProjectionFactor, error ) CHECK(assert_equal(expected,*actual,1e-3)); // linearize graph - visualSLAM::Graph graph; + NonlinearFactorGraph graph; graph.push_back(factor); FactorGraph expected_lfg; expected_lfg.push_back(actual); diff --git a/gtsam/slam/tests/testSparseBA.cpp b/gtsam/slam/tests/testSparseBA.cpp deleted file mode 100644 index cc6e11400..000000000 --- a/gtsam/slam/tests/testSparseBA.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testsparseBA.cpp - * @brief - * @date Jul 5, 2012 - * @author Yong-Dian Jian - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace boost; -using namespace gtsam; - -/* ************************************************************************* */ - -static SharedNoiseModel sigma(noiseModel::Unit::Create(2)); - -// Convenience for named keys -using symbol_shorthand::X; /* pose3 */ -using symbol_shorthand::K; /* calibration */ -using symbol_shorthand::C; /* camera = [pose calibration] */ -using symbol_shorthand::L; /* point3 */ - -static Point3 landmark1(-1.0,-1.0, 0.0); -static Point3 landmark2(-1.0, 1.0, 0.0); -static Point3 landmark3( 1.0, 1.0, 0.0); -static Point3 landmark4( 1.0,-1.0, 0.0); - -static Pose3 pose1(Matrix_(3,3, - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1.), - Point3(0,0,6.25)); - -static Pose3 pose2(Matrix_(3,3, - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1.), - Point3(0,0,5.00)); - -static Cal3_S2 calib1 (625, 625, 0, 0, 0); -static Cal3_S2 calib2 (625, 625, 0, 0, 0); - - -typedef PinholeCamera Camera; - -static Camera camera1(pose1, calib1); -static Camera camera2(pose2, calib2); - -/* ************************************************************************* */ -sparseBA::Graph testGraph1() { - Point2 z11(-100, 100); - Point2 z12(-100,-100); - Point2 z13( 100,-100); - Point2 z14( 100, 100); - Point2 z21(-125, 125); - Point2 z22(-125,-125); - Point2 z23( 125,-125); - Point2 z24( 125, 125); - - - sparseBA::Graph g; - g.addMeasurement(z11, sigma, C(1), L(1)); - g.addMeasurement(z12, sigma, C(1), L(2)); - g.addMeasurement(z13, sigma, C(1), L(3)); - g.addMeasurement(z14, sigma, C(1), L(4)); - g.addMeasurement(z21, sigma, C(2), L(1)); - g.addMeasurement(z22, sigma, C(2), L(2)); - g.addMeasurement(z23, sigma, C(2), L(3)); - g.addMeasurement(z24, sigma, C(2), L(4)); - return g; -} - -sparseBA::Graph testGraph2() { - Point2 z11(-100, 100); - Point2 z12(-100,-100); - Point2 z13( 100,-100); - Point2 z14( 100, 100); - Point2 z21(-125, 125); - Point2 z22(-125,-125); - Point2 z23( 125,-125); - Point2 z24( 125, 125); - - sparseBA::Graph g; - g.addMeasurement(z11, sigma, X(1), L(1), K(1)); - g.addMeasurement(z12, sigma, X(1), L(2), K(1)); - g.addMeasurement(z13, sigma, X(1), L(3), K(1)); - g.addMeasurement(z14, sigma, X(1), L(4), K(1)); - g.addMeasurement(z21, sigma, X(2), L(1), K(1)); - g.addMeasurement(z22, sigma, X(2), L(2), K(1)); - g.addMeasurement(z23, sigma, X(2), L(3), K(1)); - g.addMeasurement(z24, sigma, X(2), L(4), K(1)); - return g; -} - -/* ************************************************************************* */ -TEST( optimizeLM1, sparseBA ) -{ - // build a graph - sparseBA::Graph graph(testGraph1()); - - // add 3 landmark constraints - graph.addPointConstraint(L(1), landmark1); - graph.addPointConstraint(L(2), landmark2); - graph.addPointConstraint(L(3), landmark3); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(C(1), camera1); - initialEstimate.insert(C(2), camera2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - - // Create an ordering of the variables - Ordering ordering; - ordering += L(1),L(2),L(3),L(4),C(1),C(2); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); -} - -/* ************************************************************************* */ -TEST( optimizeLM2, sparseBA ) -{ - // build a graph - sparseBA::Graph graph(testGraph2()); - - // add 3 landmark constraints - graph.addPointConstraint(L(1), landmark1); - graph.addPointConstraint(L(2), landmark2); - graph.addPointConstraint(L(3), landmark3); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(X(1), pose1); - initialEstimate.insert(X(2), pose2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - initialEstimate.insert(K(1), calib2); - - // Create an ordering of the variables - Ordering ordering; - ordering += L(1),L(2),L(3),L(4),X(1),X(2),K(1); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index b38dae990..f6a861354 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -15,13 +15,16 @@ * @author Chris Beall */ -#include #include -#include +#include #include +#include +#include #include #include +#include #include +#include #include @@ -35,8 +38,7 @@ static Pose3 camera1(Matrix_(3,3, ), Point3(0,0,6.25)); -static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); -static StereoCamera stereoCam(Pose3(), K); +static boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); // point X Y Z in meters static Point3 p(0, 0, 5); @@ -46,20 +48,16 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); using symbol_shorthand::X; using symbol_shorthand::L; -typedef GenericStereoFactor MyStereoFactor; - /* ************************************************************************* */ TEST( StereoFactor, singlePoint) { - //Cal3_S2 K(625, 625, 0, 320, 240, 0.5); - boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); NonlinearFactorGraph graph; - graph.add(NonlinearEquality(X(1),camera1)); + graph.add(NonlinearEquality(X(1), camera1)); - StereoPoint2 z14(320,320.0-50, 240); + StereoPoint2 z14(320, 320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph.add(MyStereoFactor(z14,sigma, X(1), L(1), K)); + graph.add(GenericStereoFactor(z14, sigma, X(1), L(1), K)); // Create a configuration corresponding to the ground truth Values values; diff --git a/gtsam/slam/tests/testVisualSLAM.cpp b/gtsam/slam/tests/testVisualSLAM.cpp deleted file mode 100644 index 65c31adf9..000000000 --- a/gtsam/slam/tests/testVisualSLAM.cpp +++ /dev/null @@ -1,344 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testVisualSLAM.cpp - * @brief Unit test for two cameras and four landmarks, single camera - * @author Chris Beall - * @author Frank Dellaert - * @author Viorela Ila - */ - -#include -#include -#include -#include - -#include -#include -using namespace boost; - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ - -static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::L; - -static Point3 landmark1(-1.0,-1.0, 0.0); -static Point3 landmark2(-1.0, 1.0, 0.0); -static Point3 landmark3( 1.0, 1.0, 0.0); -static Point3 landmark4( 1.0,-1.0, 0.0); - -static Pose3 pose1(Matrix_(3,3, - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ), - Point3(0,0,6.25)); - -static Pose3 pose2(Matrix_(3,3, - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ), - Point3(0,0,5.00)); - -/* ************************************************************************* */ -visualSLAM::Graph testGraph() { - Point2 z11(-100, 100); - Point2 z12(-100,-100); - Point2 z13( 100,-100); - Point2 z14( 100, 100); - Point2 z21(-125, 125); - Point2 z22(-125,-125); - Point2 z23( 125,-125); - Point2 z24( 125, 125); - - shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0)); - visualSLAM::Graph g; - g.addMeasurement(z11, sigma, X(1), L(1), sK); - g.addMeasurement(z12, sigma, X(1), L(2), sK); - g.addMeasurement(z13, sigma, X(1), L(3), sK); - g.addMeasurement(z14, sigma, X(1), L(4), sK); - g.addMeasurement(z21, sigma, X(2), L(1), sK); - g.addMeasurement(z22, sigma, X(2), L(2), sK); - g.addMeasurement(z23, sigma, X(2), L(3), sK); - g.addMeasurement(z24, sigma, X(2), L(4), sK); - return g; -} - -/* ************************************************************************* */ -TEST( VisualSLAM, optimizeLM) -{ - // build a graph - visualSLAM::Graph graph(testGraph()); - // add 3 landmark constraints - graph.addPointConstraint(L(1), landmark1); - graph.addPointConstraint(L(2), landmark2); - graph.addPointConstraint(L(3), landmark3); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(X(1), pose1); - initialEstimate.insert(X(2), pose2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - - // Create an ordering of the variables - Ordering ordering; - ordering += L(1),L(2),L(3),L(4),X(1),X(2); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); -} - - -/* ************************************************************************* */ -TEST( VisualSLAM, optimizeLM2) -{ - // build a graph - visualSLAM::Graph graph(testGraph()); - // add 2 camera constraints - graph.addPoseConstraint(X(1), pose1); - graph.addPoseConstraint(X(2), pose2); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(X(1), pose1); - initialEstimate.insert(X(2), pose2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - - // Create an ordering of the variables - Ordering ordering; - ordering += L(1),L(2),L(3),L(4),X(1),X(2); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, LMoptimizer) -{ - // build a graph - visualSLAM::Graph graph(testGraph()); - // add 2 camera constraints - graph.addPoseConstraint(X(1), pose1); - graph.addPoseConstraint(X(2), pose2); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(X(1), pose1); - initialEstimate.insert(X(2), pose2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer = graph.optimizer(initialEstimate); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); - - // check errors - Matrix errors = graph.reprojectionErrors(optimizer.values()); - CHECK(assert_equal(zeros(2,8), errors)); -} - - -/* ************************************************************************* */ -TEST( VisualSLAM, CHECK_ORDERING) -{ - // build a graph - visualSLAM::Graph graph = testGraph(); - // add 2 camera constraints - graph.addPoseConstraint(X(1), pose1); - graph.addPoseConstraint(X(2), pose2); - - // Create an initial values structure corresponding to the ground truth - Values initialEstimate; - initialEstimate.insert(X(1), pose1); - initialEstimate.insert(X(2), pose2); - initialEstimate.insert(L(1), landmark1); - initialEstimate.insert(L(2), landmark2); - initialEstimate.insert(L(3), landmark3); - initialEstimate.insert(L(4), landmark4); - - // Create an optimizer and check its error - // We expect the initial to be zero because config is the ground truth - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // Iterate once, and the config should not have changed because we started - // with the ground truth - optimizer.iterate(); - EXPECT_DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); - - // check if correct - CHECK(assert_equal(initialEstimate, optimizer.values())); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, update_with_large_delta) { - // this test ensures that if the update for delta is larger than - // the size of the config, it only updates existing variables - Values init; - init.insert(X(1), Pose3()); - init.insert(L(1), Point3(1.0, 2.0, 3.0)); - - Values expected; - expected.insert(X(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); - expected.insert(L(1), Point3(1.1, 2.1, 3.1)); - - Ordering largeOrdering; - Values largeValues = init; - largeValues.insert(X(2), Pose3()); - largeOrdering += X(1),L(1),X(2); - VectorValues delta(largeValues.dims(largeOrdering)); - delta[largeOrdering[X(1)]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); - delta[largeOrdering[L(1)]] = Vector_(3, 0.1, 0.1, 0.1); - delta[largeOrdering[X(2)]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1); - Values actual = init.retract(delta, largeOrdering); - - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, dataAssociation) { - visualSLAM::ISAM isam; - // add two landmarks - // add two cameras and constraint and odometry -// std::pair actualJoint = isam.jointPrediction(); // 4*4 - // std::pair actualMarginals = isam.individualPredictions(); // 2 times 2*2 - // std::pair actualChowLiu = isam.chowLiuPredictions(); // 2 times 2*2 -} - -/* ************************************************************************* */ -TEST( VisualSLAM, filteredValues) { - visualSLAM::Values full; - full.insert(X(1), Pose3(Pose2(1.0, 2.0, 0.3))); - full.insert(L(1), Point3(1.0, 2.0, 3.0)); - - visualSLAM::Values actPoses(full.allPoses()); - visualSLAM::Values expPoses; expPoses.insert(X(1), Pose3(Pose2(1.0, 2.0, 0.3))); - EXPECT(assert_equal(expPoses, actPoses)); - - visualSLAM::Values actPoints(full.allPoints()); - visualSLAM::Values expPoints; expPoints.insert(L(1), Point3(1.0, 2.0, 3.0)); - EXPECT(assert_equal(expPoints, actPoints)); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, keys_and_view ) -{ - // create config - visualSLAM::Values c; - c.insert(X(1), pose1); - c.insert(X(2), pose2); - c.insert(L(2), landmark2); - LONGS_EQUAL(2,c.nrPoses()); - LONGS_EQUAL(1,c.nrPoints()); - { - FastList expected, actual; - expected += L(2), X(1), X(2); - actual = c.keys(); - CHECK(expected == actual); - } - { - FastList expected, actual; - expected += X(1), X(2); - actual = c.poseKeys(); - CHECK(expected == actual); - } - { - FastList expected, actual; - expected += L(2); - actual = c.pointKeys(); - CHECK(expected == actual); - } -} - -/* ************************************************************************* */ -TEST( VisualSLAM, addMeasurements ) -{ - // create config - visualSLAM::Graph g; - Vector J = Vector_(3,1.0,2.0,3.0); - Matrix Z = Matrix_(2,3, -1.0,0.0,1.0, -1.0,0.0,1.0); - shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0)); - g.addMeasurements(0,J,Z,sigma,sK); - EXPECT_LONGS_EQUAL(3,g.size()); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, insertBackProjections ) -{ - // create config - visualSLAM::Values c; - SimpleCamera camera(pose1); - Vector J = Vector_(3,1.0,2.0,3.0); - Matrix Z = Matrix_(2,3, -1.0,0.0,1.0, -1.0,0.0,1.0); - c.insertBackprojections(camera,J,Z,1.0); - EXPECT_LONGS_EQUAL(3,c.nrPoints()); -} - -/* ************************************************************************* */ -TEST( VisualSLAM, perturbPoints ) -{ - visualSLAM::Values c1,c2; - c1.insert(L(2), landmark2); - c1.perturbPoints(0.01,42u); - CHECK(assert_equal(Point3(-0.986984, 0.999534, -0.0147962),c1.point(L(2)),1e-6)); - c2.insert(L(2), landmark2); - c2.perturbPoints(0.01,42u); - CHECK(assert_equal(Point3(-0.986984, 0.999534, -0.0147962),c2.point(L(2)),1e-6)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp deleted file mode 100644 index 2133c0ee8..000000000 --- a/gtsam/slam/visualSLAM.cpp +++ /dev/null @@ -1,116 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file visualSLAM.cpp - * @date Jan 14, 2010 - * @author richard - */ - -#include -#include -#include -#include -#include - -namespace visualSLAM { - - using boost::make_shared; - - /* ************************************************************************* */ - void Values::insertBackprojections(const SimpleCamera& camera, - const Vector& J, const Matrix& Z, double depth) { - if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); - if (Z.cols() != J.size()) throw std::invalid_argument( - "insertBackProjections: J and Z must have same number of entries"); - for(int k=0;k points = allPoints(); - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); - Sampler sampler(model, seed); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, points) { - update(keyValue.key, keyValue.value.retract(sampler.sample())); - } - } - - /* ************************************************************************* */ - Matrix Values::points() const { - size_t j=0; - ConstFiltered points = allPoints(); - Matrix result(points.size(),3); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, points) - result.row(j++) = keyValue.value.vector(); - return result; - } - - /* ************************************************************************* */ - void Graph::addPointConstraint(Key pointKey, const Point3& p) { - push_back(make_shared >(pointKey, p)); - } - - /* ************************************************************************* */ - void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) { - push_back(make_shared >(pointKey, p, model)); - } - - /* ************************************************************************* */ - void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey, const shared_ptrK K) { - push_back( - make_shared > - (measured, model, poseKey, pointKey, K)); - } - - /* ************************************************************************* */ - void Graph::addMeasurements(Key i, const Vector& J, const Matrix& Z, - const SharedNoiseModel& model, const shared_ptrK K) { - if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K"); - if (Z.cols() != J.size()) throw std::invalid_argument( - "addMeasurements: J and Z must have same number of entries"); - for (int k = 0; k < Z.cols(); k++) - addMeasurement(Point2(Z(0, k), Z(1, k)), model, i, J(k), K); - } - - /* ************************************************************************* */ - void Graph::addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey, const shared_ptrKStereo K) { - push_back(make_shared >(measured, model, poseKey, pointKey, K)); - } - - /* ************************************************************************* */ - void Graph::addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model) { - push_back(make_shared >(poseKey, pointKey, range, model)); - } - - /* ************************************************************************* */ - Matrix Graph::reprojectionErrors(const Values& values) const { - // first count - size_t K = 0, k=0; - BOOST_FOREACH(const sharedFactor& f, *this) - if (boost::dynamic_pointer_cast(f)) ++K; - // now fill - Matrix errors(2,K); - BOOST_FOREACH(const sharedFactor& f, *this) { - boost::shared_ptr p = - boost::dynamic_pointer_cast(f); - if (p) errors.col(k++) = p->unwhitenedError(values); - } - return errors; - } - /* ************************************************************************* */ -} diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h deleted file mode 100644 index ae2956c5f..000000000 --- a/gtsam/slam/visualSLAM.h +++ /dev/null @@ -1,186 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file visualSLAM.h - * @brief Basic typedefs for general VisualSLAM problems. Useful for monocular and stereo systems. - * @date Jan 14, 2010 - * @author Richard Roberts - * @author Chris Beall - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace visualSLAM { - - using namespace gtsam; - - /// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper - struct Values: public pose3SLAM::Values { - - typedef boost::shared_ptr shared_ptr; - typedef gtsam::Values::ConstFiltered PoseFiltered; - typedef gtsam::Values::ConstFiltered PointFiltered; - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : - pose3SLAM::Values(values) { - } - - /// Constructor from filtered values view of poses - Values(const PoseFiltered& view) : pose3SLAM::Values(view) {} - - /// Constructor from filtered values view of points - Values(const PointFiltered& view) : pose3SLAM::Values(view) {} - - PoseFiltered allPoses() const { return this->filter(); } ///< pose view - size_t nrPoses() const { return allPoses().size(); } ///< get number of poses - KeyList poseKeys() const { return allPoses().keys(); } ///< get keys to poses only - - PointFiltered allPoints() const { return this->filter(); } ///< point view - size_t nrPoints() const { return allPoints().size(); } ///< get number of points - KeyList pointKeys() const { return allPoints().keys(); } ///< get keys to points only - - /// insert a point - void insertPoint(Key j, const Point3& point) { insert(j, point); } - - /// update a point - void updatePoint(Key j, const Point3& point) { update(j, point); } - - /// get a point - Point3 point(Key j) const { return at(j); } - - /// insert a number of initial point values by backprojecting - void insertBackprojections(const SimpleCamera& c, const Vector& J, - const Matrix& Z, double depth); - - /// perturb all points using normally distributed noise - void perturbPoints(double sigma, int32_t seed = 42u); - - /// get all point coordinates in a matrix - Matrix points() const; - - }; - - /** - * Non-linear factor graph for vanilla visual SLAM - */ - struct Graph: public pose3SLAM::Graph { - - /// shared pointer to this type of graph - typedef boost::shared_ptr shared_graph; - - /// Default constructor - Graph(){} - - /// Copy constructor given any other NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph): - pose3SLAM::Graph(graph) {} - - /// check if two graphs are equal - bool equals(const Graph& p, double tol = 1e-9) const { - return NonlinearFactorGraph::equals(p, tol); - } - - /** - * Add a constraint on a point (for now, *must* be satisfied in any Values) - * @param key variable key of the point - * @param p point around which soft prior is defined - */ - void addPointConstraint(Key pointKey, const Point3& p = Point3()); - - /** - * Add a prior on a point - * @param key variable key of the point - * @param p to which point to constrain it to - * @param model uncertainty model of this prior - */ - void addPointPrior(Key pointKey, const Point3& p = Point3(), - const SharedNoiseModel& model = noiseModel::Unit::Create(3)); - - /** - * Add a range prior to a point - * @param poseKey variable key of the camera pose - * @param pointKey variable key of the point - * @param range approximate range to point - * @param model uncertainty model of this prior - */ - void addRangeFactor(Key poseKey, Key pointKey, double range, - const SharedNoiseModel& model = noiseModel::Unit::Create(1)); - - /** - * Add a projection factor measurement (monocular) - * @param measured the measurement - * @param model the noise model for the measurement - * @param poseKey variable key for the camera pose - * @param pointKey variable key for the point - * @param K shared pointer to calibration object - */ - void addMeasurement(const Point2& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey, const shared_ptrK K); - - /** - * Add a number of measurements at the same time - * @param i variable key for the camera pose - * @param J variable keys for the point, KeyVector of size K - * @param Z the 2*K measurements as a matrix - * @param model the noise model for the measurement - * @param K shared pointer to calibration object - */ - void addMeasurements(Key i, const Vector& J, const Matrix& Z, - const SharedNoiseModel& model, const shared_ptrK K); - - /** - * Add a stereo factor measurement - * @param measured the measurement - * @param model the noise model for the measurement - * @param poseKey variable key for the camera pose - * @param pointKey variable key for the point - * @param K shared pointer to stereo calibration object - */ - void addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey, const shared_ptrKStereo K); - - /// Return a 2*K Matrix of reprojection errors - Matrix reprojectionErrors(const Values& values) const; - - }; // Graph - - /** - * Non-linear ISAM for vanilla incremental visual SLAM inference - */ - typedef gtsam::NonlinearISAM ISAM; - -} // visualSLAM - -/** - * Backwards compatibility and wrap use only, avoid using - */ -namespace visualSLAM { - typedef gtsam::NonlinearEquality PoseConstraint; ///< \deprecated typedef for backwards compatibility - typedef gtsam::NonlinearEquality PointConstraint; ///< \deprecated typedef for backwards compatibility - typedef gtsam::PriorFactor PosePrior; ///< \deprecated typedef for backwards compatibility - typedef gtsam::PriorFactor PointPrior; ///< \deprecated typedef for backwards compatibility - typedef gtsam::RangeFactor RangeFactor; ///< \deprecated typedef for backwards compatibility - typedef gtsam::GenericProjectionFactor ProjectionFactor; ///< \deprecated typedef for backwards compatibility - typedef gtsam::GenericStereoFactor StereoFactor; ///< \deprecated typedef for backwards compatibility -} - diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index 1e1dae189..a69410e34 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -45,8 +45,8 @@ TEST( InvDepthFactor, Project2) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 1., M_PI/4., atan(1/sqrt(2))); - LieScalar inv_depth(1/sqrt(3)); + LieVector diag_landmark(5, 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))); + LieScalar inv_depth(1/sqrt(3.0)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); } @@ -60,8 +60,8 @@ TEST( InvDepthFactor, Project3) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 0., M_PI/4., atan(2./sqrt(2))); - LieScalar inv_depth( 1./sqrt(1+1+4)); + LieVector diag_landmark(5, 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))); + LieScalar inv_depth( 1./sqrt(1.0+1+4)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); } @@ -75,8 +75,8 @@ TEST( InvDepthFactor, Project4) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 0., atan(4/1), atan(2./sqrt(1+16))); - LieScalar inv_depth(1./sqrt(1+16+4)); + LieVector diag_landmark(5, 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))); + LieScalar inv_depth(1./sqrt(1.+16.+4.)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp index d15bcd150..9c0438e49 100644 --- a/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp @@ -6,7 +6,8 @@ #include #include #include -#include +#include +#include #include diff --git a/gtsam_unstable/slam/tests/testPosePriors.cpp b/gtsam_unstable/slam/tests/testPosePriors.cpp index 4aa835266..1a40dd335 100644 --- a/gtsam_unstable/slam/tests/testPosePriors.cpp +++ b/gtsam_unstable/slam/tests/testPosePriors.cpp @@ -9,12 +9,12 @@ #include +#include +#include + #include #include -#include -#include - #include using namespace gtsam; diff --git a/matlab.h b/matlab.h new file mode 100644 index 000000000..1a9af8396 --- /dev/null +++ b/matlab.h @@ -0,0 +1,129 @@ +/* ---------------------------------------------------------------------------- + + * 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 matlab.h + * @brief Contains *generic* global functions designed particularly for the matlab interface + * @author Stephen Williams + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace gtsam { + + namespace utilities { + + /// Extract all Point2 values into a single matrix [x y] + Matrix extractPoint2(const Values& values) { + size_t j=0; + Values::ConstFiltered points = values.filter(); + Matrix result(points.size(),2); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; + } + + /// Extract all Point3 values into a single matrix [x y z] + Matrix extractPoint3(const Values& values) { + size_t j=0; + Values::ConstFiltered points = values.filter(); + Matrix result(points.size(),3); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + result.row(j++) = key_value.value.vector(); + return result; + } + + /// Extract all Pose2 values into a single matrix [x y theta] + Matrix extractPose2(const Values& values) { + size_t j=0; + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(),3); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) + result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); + return result; + } + + /// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] + Matrix extractPose3(const Values& values) { + size_t j=0; + Values::ConstFiltered poses = values.filter(); + Matrix result(poses.size(),12); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { + result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); + result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); + result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); + result.row(j).tail(3) = key_value.value.translation().vector(); + j++; + } + return result; + } + + + /// perturb all Point2 using normally distributed noise + void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } + } + + /// perturb all Point3 using normally distributed noise + void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); + } + } + + /// insert a number of initial point values by backprojecting + void insertBackprojections(Values& values, const SimpleCamera& camera, const Vector& J, const Matrix& Z, double depth) { + if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); + if (Z.cols() != J.size()) throw std::invalid_argument("insertBackProjections: J and Z must have same number of entries"); + for(int k=0;k >(f)) ++K; + // now fill + Matrix errors(2,K); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { + boost::shared_ptr > p = boost::dynamic_pointer_cast >(f); + if (p) errors.col(k++) = p->unwhitenedError(values); + } + return errors; + } + + } + +} + diff --git a/matlab/CHECK.m b/matlab/+gtsam/CHECK.m similarity index 100% rename from matlab/CHECK.m rename to matlab/+gtsam/CHECK.m diff --git a/matlab/EQUALITY.m b/matlab/+gtsam/EQUALITY.m similarity index 100% rename from matlab/EQUALITY.m rename to matlab/+gtsam/EQUALITY.m diff --git a/matlab/VisualISAMGenerateData.m b/matlab/+gtsam/VisualISAMGenerateData.m similarity index 100% rename from matlab/VisualISAMGenerateData.m rename to matlab/+gtsam/VisualISAMGenerateData.m diff --git a/matlab/VisualISAMInitialize.m b/matlab/+gtsam/VisualISAMInitialize.m similarity index 51% rename from matlab/VisualISAMInitialize.m rename to matlab/+gtsam/VisualISAMInitialize.m index cbcb09c5c..1e408d3d9 100644 --- a/matlab/VisualISAMInitialize.m +++ b/matlab/+gtsam/VisualISAMInitialize.m @@ -1,65 +1,75 @@ -function [noiseModels,isam,result] = VisualISAMInitialize(data,truth,options) +function [noiseModels,isam,result,nextPoseIndex] = VisualISAMInitialize(data,truth,options) % VisualInitialize: initialize visualSLAM::iSAM object and noise parameters % Authors: Duy Nguyen Ta, Frank Dellaert and Alex Cunningham %% Initialize iSAM -isam = visualSLAM.ISAM(options.reorderInterval); +import gtsam.* +params = gtsam.ISAM2Params; +if options.alwaysRelinearize + params.setRelinearizeSkip(1); +end +isam = ISAM2; %% Set Noise parameters import gtsam.* noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); -noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); +%noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); +noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.05 0.05 0.2 0.2 0.2]'); noiseModels.point = noiseModel.Isotropic.Sigma(3, 0.1); noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0); %% Add constraints/priors % TODO: should not be from ground truth! -newFactors = visualSLAM.Graph; -initialEstimates = visualSLAM.Values; +import gtsam.* +newFactors = NonlinearFactorGraph; +initialEstimates = Values; for i=1:2 ii = symbol('x',i); if i==1 if options.hardConstraint % add hard constraint - newFactors.addPoseConstraint(ii,truth.cameras{1}.pose); + newFactors.add(NonlinearEqualityPose3(ii,truth.cameras{1}.pose)); else - newFactors.addPosePrior(ii,truth.cameras{i}.pose, noiseModels.pose); + newFactors.add(PriorFactorPose3(ii,truth.cameras{i}.pose, noiseModels.pose)); end end - initialEstimates.insertPose(ii,truth.cameras{i}.pose); + initialEstimates.insert(ii,truth.cameras{i}.pose); end +nextPoseIndex = 3; + %% Add visual measurement factors from two first poses and initialize observed landmarks +import gtsam.* for i=1:2 ii = symbol('x',i); for k=1:length(data.Z{i}) j = data.J{i}{k}; jj = symbol('l',data.J{i}{k}); - newFactors.addMeasurement(data.Z{i}{k}, noiseModels.measurement, ii, jj, data.K); + newFactors.add(GenericProjectionFactorCal3_S2(data.Z{i}{k}, noiseModels.measurement, ii, jj, data.K)); % TODO: initial estimates should not be from ground truth! if ~initialEstimates.exists(jj) - initialEstimates.insertPoint(jj, truth.points{j}); + initialEstimates.insert(jj, truth.points{j}); end if options.pointPriors % add point priors - newFactors.addPointPrior(jj, truth.points{j}, noiseModels.point); + newFactors.add(priorFactorPoint3(jj, truth.points{j}, noiseModels.point)); end end end %% Add odometry between frames 1 and 2 -newFactors.addRelativePose(symbol('x',1), symbol('x',2), data.odometry{1}, noiseModels.odometry); +import gtsam.* +newFactors.add(BetweenFactorPose3(symbol('x',1), symbol('x',2), data.odometry{1}, noiseModels.odometry)); %% Update ISAM +import gtsam.* if options.batchInitialization % Do a full optimize for first two poses - fullyOptimized = newFactors.optimize(initialEstimates,0); + batchOptimizer = LevenbergMarquardtOptimizer(newFactors, initialEstimates); + fullyOptimized = batchOptimizer.optimize(); isam.update(newFactors, fullyOptimized); else isam.update(newFactors, initialEstimates); end % figure(1);tic; % t=toc; plot(frame_i,t,'r.'); tic -result = isam.estimate(); +result = isam.calculateEstimate(); % t=toc; plot(frame_i,t,'g.'); -if options.alwaysRelinearize % re-linearize - isam.reorder_relinearize(); -end diff --git a/matlab/VisualISAMPlot.m b/matlab/+gtsam/VisualISAMPlot.m similarity index 55% rename from matlab/VisualISAMPlot.m rename to matlab/+gtsam/VisualISAMPlot.m index 1f2b7d710..5ef64cf62 100644 --- a/matlab/VisualISAMPlot.m +++ b/matlab/+gtsam/VisualISAMPlot.m @@ -1,39 +1,33 @@ function VisualISAMPlot(truth, data, isam, result, options) -% VisualISAMPlot: plot current state of visualSLAM::iSAM object +% VisualISAMPlot: plot current state of ISAM2 object % Authors: Duy Nguyen Ta and Frank Dellaert -M = result.nrPoses; -N = result.nrPoints; - h=gca; cla(h); hold on; %% Plot points % Can't use data because current frame might not see all points -pointKeys = result.allPoints().keys(); -for j=0:N-1 % NOTE: uses indexing directly from a C++ vector, so zero-indexed - jj = pointKeys.at(j); - point_j = result.point(jj); - plot3(point_j.x, point_j.y, point_j.z,'marker','o'); - P = isam.marginalCovariance(jj); - covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); -end +marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); % TODO - this is slow +gtsam.plot3DPoints(result, [], marginals); %% Plot cameras import gtsam.* -for i=1:options.cameraInterval:M - ii = symbol('x',i); - pose_i = result.pose(ii); - if options.hardConstraint && (i==1) - plotPose3(pose_i,[],10); +M = 1; +while result.exists(symbol('x',M)) + ii = symbol('x',M); + pose_i = result.at(ii); + if options.hardConstraint && (M==1) + gtsam.plotPose3(pose_i,[],10); else - P = isam.marginalCovariance(ii); - plotPose3(pose_i,P,10); + P = marginals.marginalCovariance(ii); + gtsam.plotPose3(pose_i,P,10); end if options.drawTruePoses % show ground truth - plotPose3(truth.cameras{i}.pose,[],10); + gtsam.plotPose3(truth.cameras{M}.pose,[],10); end + + M = M + options.cameraInterval; end %% draw diff --git a/matlab/+gtsam/VisualISAMStep.m b/matlab/+gtsam/VisualISAMStep.m new file mode 100644 index 000000000..71b1c849c --- /dev/null +++ b/matlab/+gtsam/VisualISAMStep.m @@ -0,0 +1,44 @@ +function [isam,result,nextPoseIndex] = VisualISAMStep(data,noiseModels,isam,result,truth,nextPoseIndex) +% VisualISAMStep: execute one update step of visualSLAM::iSAM object +% Authors: Duy Nguyen Ta and Frank Dellaert + +% iSAM expects us to give it a new set of factors +% along with initial estimates for any new variables introduced. +import gtsam.* +newFactors = NonlinearFactorGraph; +initialEstimates = Values; + +%% Add odometry +import gtsam.* +odometry = data.odometry{nextPoseIndex-1}; +newFactors.add(BetweenFactorPose3(symbol('x',nextPoseIndex-1), symbol('x',nextPoseIndex), odometry, noiseModels.odometry)); + +%% Add visual measurement factors and initializations as necessary +import gtsam.* +for k=1:length(data.Z{nextPoseIndex}) + zij = data.Z{nextPoseIndex}{k}; + j = data.J{nextPoseIndex}{k}; + jj = symbol('l', j); + newFactors.add(GenericProjectionFactorCal3_S2(zij, noiseModels.measurement, symbol('x',nextPoseIndex), jj, data.K)); + % TODO: initialize with something other than truth + if ~result.exists(jj) && ~initialEstimates.exists(jj) + lmInit = truth.points{j}; + initialEstimates.insert(jj, lmInit); + end +end + +%% Initial estimates for the new pose. +import gtsam.* +prevPose = result.at(symbol('x',nextPoseIndex-1)); +initialEstimates.insert(symbol('x',nextPoseIndex), prevPose.compose(odometry)); + +%% Update ISAM +% figure(1);tic; +isam.update(newFactors, initialEstimates); +% t=toc; plot(frame_i,t,'r.'); tic +result = isam.calculateEstimate(); +% t=toc; plot(frame_i,t,'g.'); + +% Update nextPoseIndex to return +nextPoseIndex = nextPoseIndex + 1; + diff --git a/matlab/+gtsam/circlePose2.m b/matlab/+gtsam/circlePose2.m new file mode 100644 index 000000000..0679c16b3 --- /dev/null +++ b/matlab/+gtsam/circlePose2.m @@ -0,0 +1,30 @@ +function values = circlePose2(numPoses, radius, symbolChar) +% circlePose3: generate a set of poses in a circle. This function +% returns those poses inside a gtsam.Values object, with sequential +% keys starting from 0. An optional character may be provided, which +% will be stored in the msb of each key (i.e. gtsam.Symbol). +% +% We use aerospace/navlab convention, X forward, Y right, Z down +% First pose will be at (R,0,0) +% ^y ^ X +% | | +% z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) +% Vehicle at p0 is looking towards y axis (X-axis points towards world y) + +if nargin<3,symbolChar=0;end +if nargin<2,radius=1.0;end +if nargin<1,numPoses=8;end + +% Force symbolChar to be a single character +symbolChar = char(symbolChar); +symbolChar = symbolChar(1); + +values = gtsam.Values; +theta = 0.0; +dtheta = 2*pi()/numPoses; +for i = 1:numPoses + key = gtsam.symbol(symbolChar, i-1); + pose = gtsam.Pose2(radius*cos(theta), radius*sin(theta), pi()/2 + theta); + values.insert(key, pose); + theta = theta + dtheta; +end diff --git a/matlab/+gtsam/circlePose3.m b/matlab/+gtsam/circlePose3.m new file mode 100644 index 000000000..9085ab0d0 --- /dev/null +++ b/matlab/+gtsam/circlePose3.m @@ -0,0 +1,33 @@ +function values = circlePose3(numPoses, radius, symbolChar) +% circlePose3: generate a set of poses in a circle. This function +% returns those poses inside a gtsam.Values object, with sequential +% keys starting from 0. An optional character may be provided, which +% will be stored in the msb of each key (i.e. gtsam.Symbol). +% +% We use aerospace/navlab convention, X forward, Y right, Z down +% First pose will be at (R,0,0) +% ^y ^ X +% | | +% z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) +% Vehicle at p0 is looking towards y axis (X-axis points towards world y) + +if nargin<3,symbolChar=0;end +if nargin<2,radius=1.0;end +if nargin<1,numPoses=8;end + +% Force symbolChar to be a single character +symbolChar = char(symbolChar); +symbolChar = symbolChar(1); + +values = gtsam.Values; +theta = 0.0; +dtheta = 2*pi()/numPoses; +gRo = gtsam.Rot3([0, 1, 0 ; 1, 0, 0 ; 0, 0, -1]); +for i = 1:numPoses + key = gtsam.symbol(symbolChar, i-1); + gti = gtsam.Point3(radius*cos(theta), radius*sin(theta), 0); + oRi = gtsam.Rot3.Yaw(-theta); % negative yaw goes counterclockwise, with Z down ! + gTi = gtsam.Pose3(gRo.compose(oRi), gti); + values.insert(key, gTi); + theta = theta + dtheta; +end diff --git a/matlab/covarianceEllipse.m b/matlab/+gtsam/covarianceEllipse.m similarity index 96% rename from matlab/covarianceEllipse.m rename to matlab/+gtsam/covarianceEllipse.m index 491e099c3..b4579862e 100644 --- a/matlab/covarianceEllipse.m +++ b/matlab/+gtsam/covarianceEllipse.m @@ -1,34 +1,34 @@ -function covarianceEllipse(x,P,color) -% covarianceEllipse: plot a Gaussian as an uncertainty ellipse -% Based on Maybeck Vol 1, page 366 -% k=2.296 corresponds to 1 std, 68.26% of all probability -% k=11.82 corresponds to 3 std, 99.74% of all probability -% -% covarianceEllipse(x,P,color) -% it is assumed x and y are the first two components of state x - -[e,s] = eig(P(1:2,1:2)); -s1 = s(1,1); -s2 = s(2,2); -k = 2.296; -[ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) ); -line(ex,ey,'color',color); - - function [x,y] = ellipse(a,b,c); - % ellipse: return the x and y coordinates for an ellipse - % [x,y] = ellipse(a,b,c); - % a, and b are the axes. c is the center - - global ellipse_x ellipse_y - if ~exist('elipse_x') - q =0:2*pi/25:2*pi; - ellipse_x = cos(q); - ellipse_y = sin(q); - end - - points = a*ellipse_x + b*ellipse_y; - x = c(1) + points(1,:); - y = c(2) + points(2,:); - end - +function covarianceEllipse(x,P,color) +% covarianceEllipse: plot a Gaussian as an uncertainty ellipse +% Based on Maybeck Vol 1, page 366 +% k=2.296 corresponds to 1 std, 68.26% of all probability +% k=11.82 corresponds to 3 std, 99.74% of all probability +% +% covarianceEllipse(x,P,color) +% it is assumed x and y are the first two components of state x + +[e,s] = eig(P(1:2,1:2)); +s1 = s(1,1); +s2 = s(2,2); +k = 2.296; +[ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) ); +line(ex,ey,'color',color); + + function [x,y] = ellipse(a,b,c); + % ellipse: return the x and y coordinates for an ellipse + % [x,y] = ellipse(a,b,c); + % a, and b are the axes. c is the center + + global ellipse_x ellipse_y + if ~exist('elipse_x') + q =0:2*pi/25:2*pi; + ellipse_x = cos(q); + ellipse_y = sin(q); + end + + points = a*ellipse_x + b*ellipse_y; + x = c(1) + points(1,:); + y = c(2) + points(2,:); + end + end \ No newline at end of file diff --git a/matlab/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m similarity index 91% rename from matlab/covarianceEllipse3D.m rename to matlab/+gtsam/covarianceEllipse3D.m index 6d6ed9127..d3ef1f8a7 100644 --- a/matlab/covarianceEllipse3D.m +++ b/matlab/+gtsam/covarianceEllipse3D.m @@ -11,7 +11,7 @@ k = 11.82; radii = k*sqrt(diag(s)); % generate data for "unrotated" ellipsoid -[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3)); +[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8); % rotate data with orientation matrix U and center M data = kron(e(:,1),xc) + kron(e(:,2),yc) + kron(e(:,3),zc); diff --git a/matlab/+gtsam/findExampleDataFile.m b/matlab/+gtsam/findExampleDataFile.m new file mode 100644 index 000000000..9c40960b9 --- /dev/null +++ b/matlab/+gtsam/findExampleDataFile.m @@ -0,0 +1,19 @@ +function datafile = findExampleDataFile(datasetName) +%FINDEXAMPLEDATAFILE Find a dataset in the examples folder + +[ myPath, ~, ~ ] = fileparts(mfilename('fullpath')); +searchPath = { ... + fullfile(myPath, [ '../../examples/Data/' datasetName ]) ... + fullfile(myPath, [ '../gtsam_examples/Data/' datasetName ]) }; +datafile = []; +for path = searchPath + if exist(path{:}, 'file') + datafile = path{:}; + end +end +if isempty(datafile) + error([ 'Could not find example data file ' datasetName ]); +end + +end + diff --git a/matlab/load3D.m b/matlab/+gtsam/load3D.m similarity index 79% rename from matlab/load3D.m rename to matlab/+gtsam/load3D.m index 5c9d08146..c0ccd545f 100644 --- a/matlab/load3D.m +++ b/matlab/+gtsam/load3D.m @@ -3,6 +3,8 @@ function [graph,initial] = load3D(filename,model,successive,N) % cannot read noise model from file yet, uses specified model % if [successive] is tru, constructs initial estimate from odometry +import gtsam.* + if nargin<3, successive=false; end fid = fopen(filename); if fid < 0 @@ -15,10 +17,10 @@ fclose(fid); lines=columns{1}; % loop over lines and add vertices -graph = pose3SLAM.Graph; -initial = pose3SLAM.Values; +graph = NonlinearFactorGraph; +initial = Values; origin=gtsam.Pose3; -initial.insertPose(0,origin); +initial.insert(0,origin); n=size(lines,1); if nargin<4, N=n;end @@ -30,7 +32,7 @@ for i=1:n if (~successive && i1i1 - initial.insertPose(i2,initial.pose(i1).compose(dpose)); + initial.insert(i2,initial.at(i1).compose(dpose)); else - initial.insertPose(i1,initial.pose(i2).compose(dpose.inverse)); + initial.insert(i1,initial.at(i2).compose(dpose.inverse)); end end end diff --git a/matlab/+gtsam/plot2DPoints.m b/matlab/+gtsam/plot2DPoints.m new file mode 100644 index 000000000..d4703a5d7 --- /dev/null +++ b/matlab/+gtsam/plot2DPoints.m @@ -0,0 +1,37 @@ +function plot2DPoints(values, linespec, marginals) +%PLOT2DPOINTS Plots the Point2's in a values, with optional covariances +% Finds all the Point2 objects in the given Values object and plots them. +% If a Marginals object is given, this function will also plot marginal +% covariance ellipses for each point. + +import gtsam.* + +if ~exist('linespec', 'var') || isempty(linespec) + linespec = 'g'; +end +haveMarginals = exist('marginals', 'var'); +keys = KeyVector(values.keys); + +holdstate = ishold; +hold on + +% Plot points and covariance matrices +for i = 0:keys.size-1 + key = keys.at(i); + p = values.at(key); + if isa(p, 'gtsam.Point2') + if haveMarginals + P = marginals.marginalCovariance(key); + gtsam.plotPoint2(p, linespec, P); + else + gtsam.plotPoint2(p, linespec); + end + end +end + +if ~holdstate + hold off +end + +end + diff --git a/matlab/+gtsam/plot2DTrajectory.m b/matlab/+gtsam/plot2DTrajectory.m new file mode 100644 index 000000000..329026398 --- /dev/null +++ b/matlab/+gtsam/plot2DTrajectory.m @@ -0,0 +1,54 @@ +function plot2DTrajectory(values, linespec, marginals) +%PLOT2DTRAJECTORY Plots the Pose2's in a values, with optional covariances +% Finds all the Pose2 objects in the given Values object and plots them +% in increasing key order, connecting consecutive poses with a line. If +% a Marginals object is given, this function will also plot marginal +% covariance ellipses for each pose. + +import gtsam.* + +if ~exist('linespec', 'var') || isempty(linespec) + linespec = 'k*-'; +end + +haveMarginals = exist('marginals', 'var'); +keys = KeyVector(values.keys); + +holdstate = ishold; +hold on + +% Plot poses and covariance matrices +lastIndex = []; +for i = 0:keys.size-1 + key = keys.at(i); + x = values.at(key); + if isa(x, 'gtsam.Pose2') + if ~isempty(lastIndex) + % Draw line from last pose then covariance ellipse on top of + % last pose. + lastKey = keys.at(lastIndex); + lastPose = values.at(lastKey); + plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec); + if haveMarginals + P = marginals.marginalCovariance(lastKey); + gtsam.plotPose2(lastPose, 'g', P); + end + end + lastIndex = i; + end +end + +% Draw final covariance ellipse +if ~isempty(lastIndex) && haveMarginals + lastKey = keys.at(lastIndex); + lastPose = values.at(lastKey); + P = marginals.marginalCovariance(lastKey); + gtsam.plotPose2(lastPose, 'g', P); +end + +if ~holdstate + hold off +end + +end + diff --git a/matlab/+gtsam/plot3DPoints.m b/matlab/+gtsam/plot3DPoints.m new file mode 100644 index 000000000..8feab1744 --- /dev/null +++ b/matlab/+gtsam/plot3DPoints.m @@ -0,0 +1,36 @@ +function plot3DPoints(values, linespec, marginals) +%PLOT3DPOINTS Plots the Point3's in a values, with optional covariances +% Finds all the Point3 objects in the given Values object and plots them. +% If a Marginals object is given, this function will also plot marginal +% covariance ellipses for each point. + +import gtsam.* + +if ~exist('linespec', 'var') || isempty(linespec) + linespec = 'g'; +end +haveMarginals = exist('marginals', 'var'); +keys = KeyVector(values.keys); + +holdstate = ishold; +hold on + +% Plot points and covariance matrices +for i = 0:keys.size-1 + key = keys.at(i); + p = values.at(key); + if isa(p, 'gtsam.Point3') + if haveMarginals + P = marginals.marginalCovariance(key); + gtsam.plotPoint3(p, linespec, P); + else + gtsam.plotPoint3(p, linespec); + end + end +end + +if ~holdstate + hold off +end + +end diff --git a/matlab/+gtsam/plot3DTrajectory.m b/matlab/+gtsam/plot3DTrajectory.m new file mode 100644 index 000000000..1cd35c22f --- /dev/null +++ b/matlab/+gtsam/plot3DTrajectory.m @@ -0,0 +1,53 @@ +function plot3DTrajectory(values,linespec,frames,scale,marginals) +% plot3DTrajectory +if ~exist('scale','var') || isempty(scale), scale=1; end +if ~exist('frames','var'), scale=[]; end + +import gtsam.* + +haveMarginals = exist('marginals', 'var'); +keys = KeyVector(values.keys); + +holdstate = ishold; +hold on + +% Plot poses and covariance matrices +lastIndex = []; +for i = 0:keys.size-1 + key = keys.at(i); + x = values.at(key); + if isa(x, 'gtsam.Pose3') + if ~isempty(lastIndex) + % Draw line from last pose then covariance ellipse on top of + % last pose. + lastKey = keys.at(lastIndex); + lastPose = values.at(lastKey); + plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec); + if haveMarginals + P = marginals.marginalCovariance(lastKey); + else + P = []; + end + gtsam.plotPose3(lastPose, P, scale); + end + lastIndex = i; + end +end + +% Draw final pose +if ~isempty(lastIndex) + lastKey = keys.at(lastIndex); + lastPose = values.at(lastKey); + if haveMarginals + P = marginals.marginalCovariance(lastKey); + else + P = []; + end + gtsam.plotPose3(lastPose, P, scale); +end + +if ~holdstate + hold off +end + +end \ No newline at end of file diff --git a/matlab/plotCamera.m b/matlab/+gtsam/plotCamera.m similarity index 100% rename from matlab/plotCamera.m rename to matlab/+gtsam/plotCamera.m diff --git a/matlab/+gtsam/plotPoint2.m b/matlab/+gtsam/plotPoint2.m new file mode 100644 index 000000000..e62804ce3 --- /dev/null +++ b/matlab/+gtsam/plotPoint2.m @@ -0,0 +1,10 @@ +function plotPoint2(p,color,P) +% plotPoint2: show a Point2, possibly with covariance matrix +if size(color,2)==1 + plot(p.x,p.y,[color '*']); +else + plot(p.x,p.y,color); +end +if exist('P', 'var') + gtsam.covarianceEllipse([p.x;p.y],P,color(1)); +end \ No newline at end of file diff --git a/matlab/+gtsam/plotPoint3.m b/matlab/+gtsam/plotPoint3.m new file mode 100644 index 000000000..390b44939 --- /dev/null +++ b/matlab/+gtsam/plotPoint3.m @@ -0,0 +1,13 @@ +function plotPoint3(p, color, P) +%PLOTPOINT3 Plot a Point3 with an optional covariance matrix +if size(color,2)==1 + plot3(p.x,p.y,p.z,[color '*']); +else + plot3(p.x,p.y,p.z,color); +end +if exist('P', 'var') + gtsam.covarianceEllipse3D([p.x;p.y;p.z],P); +end + +end + diff --git a/matlab/plotPose2.m b/matlab/+gtsam/plotPose2.m similarity index 85% rename from matlab/plotPose2.m rename to matlab/+gtsam/plotPose2.m index 54b1e68f0..c7fba28ed 100644 --- a/matlab/plotPose2.m +++ b/matlab/+gtsam/plotPose2.m @@ -9,5 +9,5 @@ quiver(pose.x,pose.y,c,s,axisLength,color); if nargin>2 pPp = P(1:2,1:2); % covariance matrix in pose coordinate frame gRp = [c -s;s c]; % rotation from pose to global - covarianceEllipse([pose.x;pose.y],gRp*pPp*gRp',color); + gtsam.covarianceEllipse([pose.x;pose.y],gRp*pPp*gRp',color); end \ No newline at end of file diff --git a/matlab/plotPose3.m b/matlab/+gtsam/plotPose3.m similarity index 54% rename from matlab/plotPose3.m rename to matlab/+gtsam/plotPose3.m index 262d1dfc1..e2c254e39 100644 --- a/matlab/plotPose3.m +++ b/matlab/+gtsam/plotPose3.m @@ -6,24 +6,26 @@ if nargin<3,axisLength=0.1;end gRp = pose.rotation().matrix(); % rotation from pose to global C = pose.translation().vector(); -% draw the camera axes -xAxis = C+gRp(:,1)*axisLength; -L = [C xAxis]'; -line(L(:,1),L(:,2),L(:,3),'Color','r'); - -yAxis = C+gRp(:,2)*axisLength; -L = [C yAxis]'; -line(L(:,1),L(:,2),L(:,3),'Color','g'); - -zAxis = C+gRp(:,3)*axisLength; -L = [C zAxis]'; -line(L(:,1),L(:,2),L(:,3),'Color','b'); +if ~isempty(axisLength) + % draw the camera axes + xAxis = C+gRp(:,1)*axisLength; + L = [C xAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','r'); + + yAxis = C+gRp(:,2)*axisLength; + L = [C yAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','g'); + + zAxis = C+gRp(:,3)*axisLength; + L = [C zAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','b'); +end % plot the covariance if (nargin>2) && (~isempty(P)) pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame - covarianceEllipse3D(C,gPp); + gtsam.covarianceEllipse3D(C,gPp); end end diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 3e77de602..b4e43d3d6 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -10,16 +10,17 @@ install(FILES ${matlab_utils} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/) # Tests message(STATUS "Installing Matlab Toolbox Tests") -file(GLOB matlab_tests "${GTSAM_SOURCE_ROOT_DIR}/matlab/tests/*.m") -install(FILES ${matlab_tests} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_tests) +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_tests" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") # Examples message(STATUS "Installing Matlab Toolbox Examples") # Matlab files: *.m and *.fig -file(GLOB matlab_examples_m "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/*.m") -file(GLOB matlab_examples_fig "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/*.fig") -set(matlab_examples ${matlab_examples_m} ${matlab_examples_fig}) -install(FILES ${matlab_examples} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples) +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.fig") + +# Utilities +message(STATUS "Installing Matlab Toolbox Utilities") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") message(STATUS "Installing Matlab Toolbox Examples (Data)") # Data files: *.graph and *.txt diff --git a/matlab/VisualISAMStep.m b/matlab/VisualISAMStep.m deleted file mode 100644 index ef056553e..000000000 --- a/matlab/VisualISAMStep.m +++ /dev/null @@ -1,44 +0,0 @@ -function [isam,result] = VisualISAMStep(data,noiseModels,isam,result,truth, options) -% VisualISAMStep: execute one update step of visualSLAM::iSAM object -% Authors: Duy Nguyen Ta and Frank Dellaert - -% iSAM expects us to give it a new set of factors -% along with initial estimates for any new variables introduced. -newFactors = visualSLAM.Graph; -initialEstimates = visualSLAM.Values; - -%% Add odometry -import gtsam.* -i = result.nrPoses+1; -odometry = data.odometry{i-1}; -newFactors.addRelativePose(symbol('x',i-1), symbol('x',i), odometry, noiseModels.odometry); - -%% Add visual measurement factors and initializations as necessary -import gtsam.* -for k=1:length(data.Z{i}) - zij = data.Z{i}{k}; - j = data.J{i}{k}; - jj = symbol('l', j); - newFactors.addMeasurement(zij, noiseModels.measurement, symbol('x',i), jj, data.K); - % TODO: initialize with something other than truth - if ~result.exists(jj) && ~initialEstimates.exists(jj) - lmInit = truth.points{j}; - initialEstimates.insertPoint(jj, lmInit); - end -end - -%% Initial estimates for the new pose. -import gtsam.* -prevPose = result.pose(symbol('x',i-1)); -initialEstimates.insertPose(symbol('x',i), prevPose.compose(odometry)); - -%% Update ISAM -% figure(1);tic; -isam.update(newFactors, initialEstimates); -% t=toc; plot(frame_i,t,'r.'); tic -result = isam.estimate(); -% t=toc; plot(frame_i,t,'g.'); - -if options.alwaysRelinearize % re-linearize - isam.reorder_relinearize(); -end diff --git a/matlab/VisualISAM_gui.fig b/matlab/VisualISAM_gui.fig deleted file mode 100644 index 7140cbf73..000000000 Binary files a/matlab/VisualISAM_gui.fig and /dev/null differ diff --git a/matlab/examples/Pose2SLAMExample_circle.m b/matlab/examples/Pose2SLAMExample_circle.m deleted file mode 100644 index 8f281d34f..000000000 --- a/matlab/examples/Pose2SLAMExample_circle.m +++ /dev/null @@ -1,50 +0,0 @@ -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% GTSAM Copyright 2010, Georgia Tech Research Corporation, -% Atlanta, Georgia 30332-0415 -% All Rights Reserved -% Authors: Frank Dellaert, et al. (see THANKS for the full author list) -% -% See LICENSE for the license information -% -% @brief Read graph from file and perform GraphSLAM -% @author Frank Dellaert -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%% Create a hexagon of poses -hexagon = pose2SLAM.Values.Circle(6,1.0); -p0 = hexagon.pose(0); -p1 = hexagon.pose(1); - -%% create a Pose graph with one equality constraint and one measurement -fg = pose2SLAM.Graph; -fg.addPoseConstraint(0, p0); -delta = p0.between(p1); -covariance = gtsam.noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); -fg.addRelativePose(0,1, delta, covariance); -fg.addRelativePose(1,2, delta, covariance); -fg.addRelativePose(2,3, delta, covariance); -fg.addRelativePose(3,4, delta, covariance); -fg.addRelativePose(4,5, delta, covariance); -fg.addRelativePose(5,0, delta, covariance); - -%% Create initial config -initial = pose2SLAM.Values; -initial.insertPose(0, p0); -initial.insertPose(1, hexagon.pose(1).retract([-0.1, 0.1,-0.1]')); -initial.insertPose(2, hexagon.pose(2).retract([ 0.1,-0.1, 0.1]')); -initial.insertPose(3, hexagon.pose(3).retract([-0.1, 0.1,-0.1]')); -initial.insertPose(4, hexagon.pose(4).retract([ 0.1,-0.1, 0.1]')); -initial.insertPose(5, hexagon.pose(5).retract([-0.1, 0.1,-0.1]')); - -%% Plot Initial Estimate -figure(1);clf -P=initial.poses; -plot(P(:,1),P(:,2),'g-*'); axis equal - -%% optimize -result = fg.optimize(initial,1); - -%% Show Result -P=result.poses; -hold on; plot(P(:,1),P(:,2),'b-*') -result.print(sprintf('\nFinal result:\n')); diff --git a/matlab/examples/Pose2SLAMExample_graph.m b/matlab/examples/Pose2SLAMExample_graph.m deleted file mode 100644 index 0481f021e..000000000 --- a/matlab/examples/Pose2SLAMExample_graph.m +++ /dev/null @@ -1,48 +0,0 @@ -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% GTSAM Copyright 2010, Georgia Tech Research Corporation, -% Atlanta, Georgia 30332-0415 -% All Rights Reserved -% Authors: Frank Dellaert, et al. (see THANKS for the full author list) -% -% See LICENSE for the license information -% -% @brief Read graph from file and perform GraphSLAM -% @author Frank Dellaert -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%% Initialize graph, initial estimate, and odometry noise -import gtsam.* -model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 1*pi/180]); -maxID=0; -addNoise=false; -smart=true; -[graph,initial]=load2D('Data/w10000-odom.graph',model,maxID,addNoise,smart); -initial.print(sprintf('Initial estimate:\n')); - -%% Add a Gaussian prior on pose x_1 -priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin -priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]); -graph.addPosePrior(0, priorMean, priorNoise); % add directly to graph - -%% Plot Initial Estimate -figure(1);clf -P=initial.poses; -plot(P(:,1),P(:,2),'g-*'); axis equal - -%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -tic -result = graph.optimize(initial,1); -toc -P=result.poses; -hold on; plot(P(:,1),P(:,2),'b-*') -result.print(sprintf('\nFinal result:\n')); - -%% Plot Covariance Ellipses -marginals = graph.marginals(result); -P={}; -for i=1:result.size()-1 - pose_i = result.pose(i); - P{i}=marginals.marginalCovariance(i); - plotPose2(pose_i,'b',P{i}) -end -fprintf(1,'%.5f %.5f %.5f\n',P{99}) \ No newline at end of file diff --git a/matlab/examples/Pose3SLAMExample.m b/matlab/examples/Pose3SLAMExample.m deleted file mode 100644 index d4a8baf99..000000000 --- a/matlab/examples/Pose3SLAMExample.m +++ /dev/null @@ -1,54 +0,0 @@ -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% GTSAM Copyright 2010, Georgia Tech Research Corporation, -% Atlanta, Georgia 30332-0415 -% All Rights Reserved -% Authors: Frank Dellaert, et al. (see THANKS for the full author list) -% -% See LICENSE for the license information -% -% @brief Read graph from file and perform GraphSLAM -% @author Frank Dellaert -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%% Create a hexagon of poses -hexagon = pose3SLAM.Values.Circle(6,1.0); -p0 = hexagon.pose(0); -p1 = hexagon.pose(1); - -%% create a Pose graph with one equality constraint and one measurement -fg = pose3SLAM.Graph; -fg.addPoseConstraint(0, p0); -delta = p0.between(p1); -covariance = gtsam.noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); -fg.addRelativePose(0,1, delta, covariance); -fg.addRelativePose(1,2, delta, covariance); -fg.addRelativePose(2,3, delta, covariance); -fg.addRelativePose(3,4, delta, covariance); -fg.addRelativePose(4,5, delta, covariance); -fg.addRelativePose(5,0, delta, covariance); - -%% Create initial config -initial = pose3SLAM.Values; -s = 0.10; -initial.insertPose(0, p0); -initial.insertPose(1, hexagon.pose(1).retract(s*randn(6,1))); -initial.insertPose(2, hexagon.pose(2).retract(s*randn(6,1))); -initial.insertPose(3, hexagon.pose(3).retract(s*randn(6,1))); -initial.insertPose(4, hexagon.pose(4).retract(s*randn(6,1))); -initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1))); - -%% Plot Initial Estimate -cla -T=initial.translations(); -plot3(T(:,1),T(:,2),T(:,3),'g-*'); - -%% optimize -result = fg.optimize(initial,1); - -%% Show Result -hold on; plot3DTrajectory(result,'b-*', true, 0.3); -axis([-2 2 -2 2 -1 1]); -axis equal -view(-37,40) -colormap hot -result.print(sprintf('\nFinal result:\n')); diff --git a/matlab/examples/Pose3SLAMExample_graph.m b/matlab/examples/Pose3SLAMExample_graph.m deleted file mode 100644 index cace357ae..000000000 --- a/matlab/examples/Pose3SLAMExample_graph.m +++ /dev/null @@ -1,32 +0,0 @@ -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% GTSAM Copyright 2010, Georgia Tech Research Corporation, -% Atlanta, Georgia 30332-0415 -% All Rights Reserved -% Authors: Frank Dellaert, et al. (see THANKS for the full author list) -% -% See LICENSE for the license information -% -% @brief Read graph from file and perform GraphSLAM -% @author Frank Dellaert -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -N = 2500; -% filename = '../../examples/Data/sphere_smallnoise.graph'; -% filename = '../../examples/Data/sphere2500_groundtruth.txt'; -filename = '../../examples/Data/sphere2500.txt'; - -%% Initialize graph, initial estimate, and odometry noise -model = gtsam.noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); -[graph,initial]=load3D(filename,model,true,N); - -%% Plot Initial Estimate -figure(1);clf -first = initial.pose(0); -plot3(first.x(),first.y(),first.z(),'r*'); hold on -plot3DTrajectory(initial,'g-',false); - -%% Read again, now with all constraints, and optimize -graph = load3D(filename,model,false,N); -graph.addPoseConstraint(0, first); -result = graph.optimize(initial); -plot3DTrajectory(result,'r-',false); axis equal; diff --git a/matlab/examples/gtsamExamples.fig b/matlab/examples/gtsamExamples.fig deleted file mode 100644 index be72daf9c..000000000 Binary files a/matlab/examples/gtsamExamples.fig and /dev/null differ diff --git a/matlab/examples/LocalizationExample.m b/matlab/gtsam_examples/LocalizationExample.m similarity index 65% rename from matlab/examples/LocalizationExample.m rename to matlab/gtsam_examples/LocalizationExample.m index 0a54111b0..c877c70c7 100644 --- a/matlab/examples/LocalizationExample.m +++ b/matlab/gtsam_examples/LocalizationExample.m @@ -16,51 +16,47 @@ % - The robot is on a grid, moving 2 meters each step %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) -graph = pose2SLAM.Graph; +graph = gtsam.NonlinearFactorGraph; %% Add two odometry factors import gtsam.* odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta -graph.addRelativePose(1, 2, odometry, odometryNoise); -graph.addRelativePose(2, 3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); %% Add three "GPS" measurements import gtsam.* % We use Pose2 Priors here with high variance on theta priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]); -graph.addPosePrior(1, Pose2(0.0, 0.0, 0.0), priorNoise); -graph.addPosePrior(2, Pose2(2.0, 0.0, 0.0), priorNoise); -graph.addPosePrior(3, Pose2(4.0, 0.0, 0.0), priorNoise); +graph.add(PriorFactorPose2(1, Pose2(0.0, 0.0, 0.0), priorNoise)); +graph.add(PriorFactorPose2(2, Pose2(2.0, 0.0, 0.0), priorNoise)); +graph.add(PriorFactorPose2(3, Pose2(4.0, 0.0, 0.0), priorNoise)); %% print graph.print(sprintf('\nFactor graph:\n')); %% Initialize to noisy points import gtsam.* -initialEstimate = pose2SLAM.Values; -initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); +initialEstimate = Values; +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(sprintf('\nInitial estimate:\n ')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd import gtsam.* -result = graph.optimize(initialEstimate,1); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); result.print(sprintf('\nFinal result:\n ')); -%% Plot Covariance Ellipses +%% Plot trajectory and covariance ellipses import gtsam.* cla; -X=result.poses(); -plot(X(:,1),X(:,2),'k*-'); hold on -marginals = graph.marginals(result); -P={}; -for i=1:result.size() - pose_i = result.pose(i); - P{i}=marginals.marginalCovariance(i); - plotPose2(pose_i,'g',P{i}) -end +hold on; + +gtsam.plot2DTrajectory(result, [], Marginals(graph, result)); + axis([-0.6 4.8 -1 1]) axis equal view(2) diff --git a/matlab/examples/OdometryExample.m b/matlab/gtsam_examples/OdometryExample.m similarity index 68% rename from matlab/examples/OdometryExample.m rename to matlab/gtsam_examples/OdometryExample.m index 297702b51..b913b187a 100644 --- a/matlab/examples/OdometryExample.m +++ b/matlab/gtsam_examples/OdometryExample.m @@ -16,48 +16,44 @@ % - The robot is on a grid, moving 2 meters each step %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) -graph = pose2SLAM.Graph; +graph = gtsam.NonlinearFactorGraph; %% Add a Gaussian prior on pose x_1 import gtsam.* priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta -graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph %% Add two odometry factors import gtsam.* odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta -graph.addRelativePose(1, 2, odometry, odometryNoise); -graph.addRelativePose(2, 3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); %% print graph.print(sprintf('\nFactor graph:\n')); %% Initialize to noisy points import gtsam.* -initialEstimate = pose2SLAM.Values; -initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); +initialEstimate = Values; +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(sprintf('\nInitial estimate:\n ')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,1); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); result.print(sprintf('\nFinal result:\n ')); -%% Plot Covariance Ellipses +%% Plot trajectory and covariance ellipses import gtsam.* cla; -X=result.poses(); -plot(X(:,1),X(:,2),'k*-'); hold on -marginals = graph.marginals(result); -P={}; -for i=1:result.size() - pose_i = result.pose(i); - P{i}=marginals.marginalCovariance(i); - plotPose2(pose_i,'g',P{i}) -end +hold on; + +gtsam.plot2DTrajectory(result, [], Marginals(graph, result)); + axis([-0.6 4.8 -1 1]) axis equal view(2) diff --git a/matlab/examples/PlanarSLAMExample.m b/matlab/gtsam_examples/PlanarSLAMExample.m similarity index 57% rename from matlab/examples/PlanarSLAMExample.m rename to matlab/gtsam_examples/PlanarSLAMExample.m index e664d66fc..e37326275 100644 --- a/matlab/examples/PlanarSLAMExample.m +++ b/matlab/gtsam_examples/PlanarSLAMExample.m @@ -25,72 +25,59 @@ i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); j1 = symbol('l',1); j2 = symbol('l',2); %% Create graph container and add factors to it -graph = planarSLAM.Graph; +graph = gtsam.NonlinearFactorGraph; %% Add prior import gtsam.* priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); -graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph %% Add odometry import gtsam.* odometry = Pose2(2.0, 0.0, 0.0); odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(i1, i2, odometry, odometryNoise); -graph.addRelativePose(i2, i3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); %% Add bearing/range measurement factors import gtsam.* degrees = pi/180; brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); -graph.addBearingRange(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise); -graph.addBearingRange(i2, j1, Rot2(90*degrees), 2, brNoise); -graph.addBearingRange(i3, j2, Rot2(90*degrees), 2, brNoise); +graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); +graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); +graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); % print graph.print(sprintf('\nFull graph:\n')); %% Initialize to noisy points import gtsam.* -initialEstimate = planarSLAM.Values; -initialEstimate.insertPose(i1, Pose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(i2, Pose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(i3, Pose2(4.1, 0.1, 0.1)); -initialEstimate.insertPoint(j1, Point2(1.8, 2.1)); -initialEstimate.insertPoint(j2, Point2(4.1, 1.8)); +initialEstimate = Values; +initialEstimate.insert(i1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insert(i2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insert(i3, Pose2(4.1, 0.1, 0.1)); +initialEstimate.insert(j1, Point2(1.8, 2.1)); +initialEstimate.insert(j2, Point2(4.1, 1.8)); initialEstimate.print(sprintf('\nInitial estimate:\n')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,1); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses import gtsam.* cla;hold on -marginals = graph.marginals(result); -for i=1:3 - key = symbol('x',i); - pose{i} = result.pose(key); - P{i}=marginals.marginalCovariance(key); - if i>1 - plot([pose{i-1}.x;pose{i}.x],[pose{i-1}.y;pose{i}.y],'r-'); - end -end -for i=1:3 - plotPose2(pose{i},'g',P{i}) -end -point = {}; -for j=1:2 - key = symbol('l',j); - point{j} = result.point(key); - Q{j}=marginals.marginalCovariance(key); - plotPoint2(point{j},'b',Q{j}) -end -plot([pose{1}.x;point{1}.x],[pose{1}.y;point{1}.y],'c-'); -plot([pose{2}.x;point{1}.x],[pose{2}.y;point{1}.y],'c-'); -plot([pose{3}.x;point{2}.x],[pose{3}.y;point{2}.y],'c-'); + +marginals = Marginals(graph, result); +gtsam.plot2DTrajectory(result, [], marginals); +gtsam.plot2DPoints(result, [], marginals); + +plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-'); +plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-'); +plot([result.at(i3).x; result.at(j2).x],[result.at(i3).y; result.at(j2).y], 'c-'); axis([-0.6 4.8 -1 1]) axis equal view(2) diff --git a/matlab/examples/PlanarSLAMExample_sampling.m b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m similarity index 51% rename from matlab/examples/PlanarSLAMExample_sampling.m rename to matlab/gtsam_examples/PlanarSLAMExample_sampling.m index ec68d81e6..59c99df53 100644 --- a/matlab/examples/PlanarSLAMExample_sampling.m +++ b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m @@ -14,14 +14,14 @@ %% Create the same factor graph as in PlanarSLAMExample import gtsam.* i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); -graph = planarSLAM.Graph; +graph = NonlinearFactorGraph; priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); -graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph odometry = Pose2(2.0, 0.0, 0.0); odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(i1, i2, odometry, odometryNoise); -graph.addRelativePose(i2, i3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); %% Except, for measurements we offer a choice import gtsam.* @@ -29,53 +29,49 @@ j1 = symbol('l',1); j2 = symbol('l',2); degrees = pi/180; brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); if 1 - graph.addBearingRange(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise); - graph.addBearingRange(i2, j1, Rot2(90*degrees), 2, brNoise); + graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); + graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); else bearingModel = noiseModel.Diagonal.Sigmas(0.1); - graph.addBearing(i1, j1, Rot2(45*degrees), bearingModel); - graph.addBearing(i2, j1, Rot2(90*degrees), bearingModel); + graph.add(BearingFactor2D(i1, j1, Rot2(45*degrees), bearingModel)); + graph.add(BearingFactor2D(i2, j1, Rot2(90*degrees), bearingModel)); end -graph.addBearingRange(i3, j2, Rot2(90*degrees), 2, brNoise); +graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); %% Initialize MCMC sampler with ground truth -sample = planarSLAM.Values; -sample.insertPose(i1, Pose2(0,0,0)); -sample.insertPose(i2, Pose2(2,0,0)); -sample.insertPose(i3, Pose2(4,0,0)); -sample.insertPoint(j1, Point2(2,2)); -sample.insertPoint(j2, Point2(4,2)); +import gtsam.* +sample = Values; +sample.insert(i1, Pose2(0,0,0)); +sample.insert(i2, Pose2(2,0,0)); +sample.insert(i3, Pose2(4,0,0)); +sample.insert(j1, Point2(2,2)); +sample.insert(j2, Point2(4,2)); %% Calculate and plot Covariance Ellipses -figure(1);clf;hold on -marginals = graph.marginals(sample); -for i=1:3 - key = symbol('x',i); - pose{i} = sample.pose(key); - P{i}=marginals.marginalCovariance(key); - if i>1 - plot([pose{i-1}.x;pose{i}.x],[pose{i-1}.y;pose{i}.y],'r-'); - end -end -for i=1:3 - plotPose2(pose{i},'g',P{i}) -end +import gtsam.* +cla;hold on +marginals = Marginals(graph, sample); + +gtsam.plot2DTrajectory(sample, [], marginals); +gtsam.plot2DPoints(sample, [], marginals); + for j=1:2 key = symbol('l',j); - point{j} = sample.point(key); + point{j} = sample.at(key); Q{j}=marginals.marginalCovariance(key); S{j}=chol(Q{j}); % for sampling - plotPoint2(point{j},'b',Q{j}) end -plot([pose{1}.x;point{1}.x],[pose{1}.y;point{1}.y],'c-'); -plot([pose{2}.x;point{1}.x],[pose{2}.y;point{1}.y],'c-'); -plot([pose{3}.x;point{2}.x],[pose{3}.y;point{2}.y],'c-'); -axis equal + +plot([sample.at(i1).x; sample.at(j1).x],[sample.at(i1).y; sample.at(j1).y], 'c-'); +plot([sample.at(i2).x; sample.at(j1).x],[sample.at(i2).y; sample.at(j1).y], 'c-'); +plot([sample.at(i3).x; sample.at(j2).x],[sample.at(i3).y; sample.at(j2).y], 'c-'); +view(2); axis auto; axis equal %% Do Sampling on point 2 +import gtsam.* N=1000; for s=1:N delta = S{2}*randn(2,1); proposedPoint = Point2(point{2}.x+delta(1),point{2}.y+delta(2)); - plotPoint2(proposedPoint,'k.') + gtsam.plotPoint2(proposedPoint,'k.') end \ No newline at end of file diff --git a/matlab/examples/Pose2SLAMExample.m b/matlab/gtsam_examples/Pose2SLAMExample.m similarity index 59% rename from matlab/examples/Pose2SLAMExample.m rename to matlab/gtsam_examples/Pose2SLAMExample.m index 4bb072fa8..fccacc894 100644 --- a/matlab/examples/Pose2SLAMExample.m +++ b/matlab/gtsam_examples/Pose2SLAMExample.m @@ -19,58 +19,57 @@ % - The robot is on a grid, moving 2 meters each step %% Create graph container and add factors to it -graph = pose2SLAM.Graph; +graph = NonlinearFactorGraph; %% Add prior import gtsam.* % gaussian for prior priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); -graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph %% Add odometry import gtsam.* % general noisemodel for odometry odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); -graph.addRelativePose(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise); +graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise)); +graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise)); +graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise)); %% Add pose constraint import gtsam.* model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(5, 2, Pose2(2.0, 0.0, pi/2), model); +graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model)); % print graph.print(sprintf('\nFactor graph:\n')); %% Initialize to noisy points import gtsam.* -initialEstimate = pose2SLAM.Values; -initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2 )); -initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2 )); -initialEstimate.insertPose(3, Pose2(4.1, 0.1, pi/2)); -initialEstimate.insertPose(4, Pose2(4.0, 2.0, pi )); -initialEstimate.insertPose(5, Pose2(2.1, 2.1,-pi/2)); +initialEstimate = Values; +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, pi/2)); +initialEstimate.insert(4, Pose2(4.0, 2.0, pi )); +initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2)); initialEstimate.print(sprintf('\nInitial estimate:\n')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,1); +import gtsam.* +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses +import gtsam.* cla; -X=result.poses(); -plot(X(:,1),X(:,2),'k*-'); hold on -plot([result.pose(5).x;result.pose(2).x],[result.pose(5).y;result.pose(2).y],'r-'); -marginals = graph.marginals(result); +hold on +plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-','LineWidth',2); +marginals = Marginals(graph, result); + +gtsam.plot2DTrajectory(result, [], marginals); -for i=1:result.size() - pose_i = result.pose(i); - P = marginals.marginalCovariance(i) - plotPose2(pose_i,'g',P); -end axis([-0.6 4.8 -1 1]) axis equal view(2) diff --git a/matlab/examples/Pose2SLAMExample_advanced.m b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m similarity index 70% rename from matlab/examples/Pose2SLAMExample_advanced.m rename to matlab/gtsam_examples/Pose2SLAMExample_advanced.m index fc23d67f1..fab340f2a 100644 --- a/matlab/examples/Pose2SLAMExample_advanced.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m @@ -21,22 +21,22 @@ % - The robot is on a grid, moving 2 meters each step %% Create graph container and add factors to it -graph = pose2SLAM.Graph; +graph = NonlinearFactorGraph; %% Add prior import gtsam.* % gaussian for prior priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin -graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph %% Add odometry import gtsam.* % general noisemodel for odometry odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) -graph.addRelativePose(1, 2, odometry, odometryNoise); -graph.addRelativePose(2, 3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); %% Add measurements import gtsam.* @@ -48,23 +48,23 @@ graph.print('full graph'); %% Initialize to noisy points import gtsam.* -initialEstimate = pose2SLAM.Values; -initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); +initialEstimate = Values; +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('initial estimate'); %% set up solver, choose ordering and optimize -%params = NonlinearOptimizationParameters_newDecreaseThresholds(1e-15, 1e-15); -% -%ord = graph.orderingCOLAMD(initialEstimate); -% -%result = pose2SLAMOptimizer(graph,initialEstimate,ord,params); -%result.print('final result'); +params = DoglegParams; +params.setAbsoluteErrorTol(1e-15); +params.setRelativeErrorTol(1e-15); +params.setVerbosity('ERROR'); +params.setVerbosityDL('VERBOSE'); +params.setOrdering(graph.orderingCOLAMD(initialEstimate)); +optimizer = DoglegOptimizer(graph, initialEstimate, params); -%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,1); +result = optimizer.optimizeSafely(); result.print('final result'); %% Get the corresponding dense matrix @@ -77,4 +77,6 @@ IJS = gfg.sparseJacobian_(); Ab=sparse(IJS(1,:),IJS(2,:),IJS(3,:)); A = Ab(:,1:end-1); b = full(Ab(:,end)); +clf spy(A); +title('Non-zero entries in measurement Jacobian'); diff --git a/matlab/gtsam_examples/Pose2SLAMExample_circle.m b/matlab/gtsam_examples/Pose2SLAMExample_circle.m new file mode 100644 index 000000000..957a30df4 --- /dev/null +++ b/matlab/gtsam_examples/Pose2SLAMExample_circle.m @@ -0,0 +1,57 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 +% +% @brief Read graph from file and perform GraphSLAM +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%% Create a hexagon of poses +import gtsam.* +hexagon = gtsam.circlePose2(6,1.0); +p0 = hexagon.at(0); +p1 = hexagon.at(1); + +%% create a Pose graph with one equality constraint and one measurement +import gtsam.* +fg = NonlinearFactorGraph; +fg.add(NonlinearEqualityPose2(0, p0)); +delta = p0.between(p1); +covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); +fg.add(BetweenFactorPose2(0,1, delta, covariance)); +fg.add(BetweenFactorPose2(1,2, delta, covariance)); +fg.add(BetweenFactorPose2(2,3, delta, covariance)); +fg.add(BetweenFactorPose2(3,4, delta, covariance)); +fg.add(BetweenFactorPose2(4,5, delta, covariance)); +fg.add(BetweenFactorPose2(5,0, delta, covariance)); + +%% Create initial config +import gtsam.* +initial = Values; +initial.insert(0, p0); +initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]')); +initial.insert(2, hexagon.at(2).retract([ 0.1,-0.1, 0.1]')); +initial.insert(3, hexagon.at(3).retract([-0.1, 0.1,-0.1]')); +initial.insert(4, hexagon.at(4).retract([ 0.1,-0.1, 0.1]')); +initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]')); + +%% Plot Initial Estimate +import gtsam.* +cla +gtsam.plot2DTrajectory(initial, 'g*-'); axis equal + +%% optimize +import gtsam.* +optimizer = DoglegOptimizer(fg, initial); +result = optimizer.optimizeSafely; + +%% Show Result +import gtsam.* +hold on; gtsam.plot2DTrajectory(result, 'b*-'); +view(2); +axis([-1.5 1.5 -1.5 1.5]); +result.print(sprintf('\nFinal result:\n')); diff --git a/examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m similarity index 63% rename from examples/Pose2SLAMExample_graph.m rename to matlab/gtsam_examples/Pose2SLAMExample_graph.m index 65271c053..3eed090a2 100644 --- a/examples/Pose2SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m @@ -10,38 +10,45 @@ % @author Frank Dellaert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% Find data file +datafile = gtsam.findExampleDataFile('w100-odom.graph'); + %% Initialize graph, initial estimate, and odometry noise import gtsam.* model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); maxID = 0; addNoise = false; smart = true; -[graph,initial]=load2D('Data/w100-odom.graph',model,maxID,addNoise,smart); +[graph,initial] = gtsam.load2D(datafile, model, maxID, addNoise, smart); initial.print(sprintf('Initial estimate:\n')); %% Add a Gaussian prior on pose x_1 import gtsam.* priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]); -graph.addPosePrior(0, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(0, priorMean, priorNoise)); % add directly to graph %% Plot Initial Estimate -figure(1);clf -P=initial.poses; -plot(P(:,1),P(:,2),'g-*'); axis equal +import gtsam.* +cla +gtsam.plot2DTrajectory(initial, 'g-*'); axis equal %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initial,1); -P=result.poses; -hold on; plot(P(:,1),P(:,2),'b-*') +import gtsam.* +optimizer = LevenbergMarquardtOptimizer(graph, initial); +result = optimizer.optimizeSafely; +hold on; gtsam.plot2DTrajectory(result, 'b-*'); result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses -marginals = graph.marginals(result); +import gtsam.* +marginals = Marginals(graph, result); P={}; for i=1:result.size()-1 - pose_i = result.pose(i); + pose_i = result.at(i); P{i}=marginals.marginalCovariance(i); - plotPose2(pose_i,'b',P{i}) + gtsam.plotPose2(pose_i,'b',P{i}) end +view(2) +axis([-15 10 -15 10]); axis equal; fprintf(1,'%.5f %.5f %.5f\n',P{99}) \ No newline at end of file diff --git a/matlab/examples/Pose2SLAMwSPCG.m b/matlab/gtsam_examples/Pose2SLAMwSPCG.m similarity index 60% rename from matlab/examples/Pose2SLAMwSPCG.m rename to matlab/gtsam_examples/Pose2SLAMwSPCG.m index 5db736fab..dca31ac94 100644 --- a/matlab/examples/Pose2SLAMwSPCG.m +++ b/matlab/gtsam_examples/Pose2SLAMwSPCG.m @@ -17,41 +17,42 @@ % - The robot is on a grid, moving 2 meters each step %% Create graph container and add factors to it -graph = pose2SLAM.Graph; +graph = NonlinearFactorGraph; %% Add prior import gtsam.* % gaussian for prior priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); -graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph %% Add odometry import gtsam.* % general noisemodel for odometry odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); -graph.addRelativePose(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise); +graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise)); +graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise)); +graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise)); %% Add pose constraint import gtsam.* model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(5, 2, Pose2(2.0, 0.0, pi/2), model); +graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model)); % print graph.print(sprintf('\nFactor graph:\n')); %% Initialize to noisy points -initialEstimate = pose2SLAM.Values; -initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2 )); -initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2 )); -initialEstimate.insertPose(3, Pose2(4.1, 0.1, pi/2)); -initialEstimate.insertPose(4, Pose2(4.0, 2.0, pi )); -initialEstimate.insertPose(5, Pose2(2.1, 2.1,-pi/2)); +initialEstimate = Values; +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, pi/2)); +initialEstimate.insert(4, Pose2(4.0, 2.0, pi )); +initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2)); initialEstimate.print(sprintf('\nInitial estimate:\n')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimizeSPCG(initialEstimate); +optimizer = DoglegOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); result.print(sprintf('\nFinal result:\n')); \ No newline at end of file diff --git a/matlab/gtsam_examples/Pose3SLAMExample.m b/matlab/gtsam_examples/Pose3SLAMExample.m new file mode 100644 index 000000000..b1521ff5b --- /dev/null +++ b/matlab/gtsam_examples/Pose3SLAMExample.m @@ -0,0 +1,60 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 +% +% @brief Read graph from file and perform GraphSLAM +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%% Create a hexagon of poses +import gtsam.* +hexagon = gtsam.circlePose3(6,1.0); +p0 = hexagon.at(0); +p1 = hexagon.at(1); + +%% create a Pose graph with one equality constraint and one measurement +import gtsam.* +fg = NonlinearFactorGraph; +fg.add(NonlinearEqualityPose3(0, p0)); +delta = p0.between(p1); +covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); +fg.add(BetweenFactorPose3(0,1, delta, covariance)); +fg.add(BetweenFactorPose3(1,2, delta, covariance)); +fg.add(BetweenFactorPose3(2,3, delta, covariance)); +fg.add(BetweenFactorPose3(3,4, delta, covariance)); +fg.add(BetweenFactorPose3(4,5, delta, covariance)); +fg.add(BetweenFactorPose3(5,0, delta, covariance)); + +%% Create initial config +import gtsam.* +initial = Values; +s = 0.10; +initial.insert(0, p0); +initial.insert(1, hexagon.at(1).retract(s*randn(6,1))); +initial.insert(2, hexagon.at(2).retract(s*randn(6,1))); +initial.insert(3, hexagon.at(3).retract(s*randn(6,1))); +initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); +initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); + +%% Plot Initial Estimate +import gtsam.* +cla +gtsam.plot3DTrajectory(initial, 'g-*'); + +%% optimize +import gtsam.* +optimizer = DoglegOptimizer(fg, initial); +result = optimizer.optimizeSafely(); + +%% Show Result +import gtsam.* +hold on; gtsam.plot3DTrajectory(result, 'b-*', true, 0.3); +axis([-2 2 -2 2 -1 1]); +axis equal +view(-37,40) +colormap hot +result.print(sprintf('\nFinal result:\n')); diff --git a/matlab/gtsam_examples/Pose3SLAMExample_graph.m b/matlab/gtsam_examples/Pose3SLAMExample_graph.m new file mode 100644 index 000000000..19cc31439 --- /dev/null +++ b/matlab/gtsam_examples/Pose3SLAMExample_graph.m @@ -0,0 +1,42 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 +% +% @brief Read graph from file and perform GraphSLAM +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%% Find data file +N = 2500; +% dataset = 'sphere_smallnoise.graph'; +% dataset = 'sphere2500_groundtruth.txt'; +dataset = 'sphere2500.txt'; + +datafile = gtsam.findExampleDataFile(dataset); + +%% Initialize graph, initial estimate, and odometry noise +import gtsam.* +model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); +[graph,initial]=gtsam.load3D(datafile,model,true,N); + +%% Plot Initial Estimate +import gtsam.* +cla +first = initial.at(0); +plot3(first.x(),first.y(),first.z(),'r*'); hold on +gtsam.plot3DTrajectory(initial,'g-',false); +drawnow; + +%% Read again, now with all constraints, and optimize +import gtsam.* +graph = gtsam.load3D(datafile, model, false, N); +graph.add(NonlinearEqualityPose3(0, first)); +optimizer = LevenbergMarquardtOptimizer(graph, initial); +result = optimizer.optimizeSafely(); +gtsam.plot3DTrajectory(result, 'r-', false); axis equal; + +view(3); axis equal; \ No newline at end of file diff --git a/matlab/examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m similarity index 80% rename from matlab/examples/SBAExample.m rename to matlab/gtsam_examples/SBAExample.m index a0f003eeb..7e4ac4031 100644 --- a/matlab/examples/SBAExample.m +++ b/matlab/gtsam_examples/SBAExample.m @@ -22,7 +22,7 @@ options.nrCameras = 10; options.showImages = false; %% Generate data -[data,truth] = VisualISAMGenerateData(options); +[data,truth] = gtsam.VisualISAMGenerateData(options); measurementNoiseSigma = 1.0; pointNoiseSigma = 0.1; @@ -30,7 +30,8 @@ cameraNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1 ... 0.001*ones(1,5)]'; %% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph) -graph = sparseBA.Graph; +import gtsam.* +graph = NonlinearFactorGraph; %% Add factors for all measurements @@ -39,7 +40,7 @@ measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma); for i=1:length(data.Z) for k=1:length(data.Z{i}) j = data.J{i}{k}; - graph.addSimpleCameraMeasurement(data.Z{i}{k}, measurementNoise, symbol('c',i), symbol('p',j)); + graph.add(GeneralSFMFactorCal3_S2(data.Z{i}{k}, measurementNoise, symbol('c',i), symbol('p',j))); end end @@ -47,10 +48,10 @@ end import gtsam.* cameraPriorNoise = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas); firstCamera = SimpleCamera(truth.cameras{1}.pose, truth.K); -graph.addSimpleCameraPrior(symbol('c',1), firstCamera, cameraPriorNoise); +graph.add(PriorFactorSimpleCamera(symbol('c',1), firstCamera, cameraPriorNoise)); pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); -graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise); +graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise)); %% Print the graph graph.print(sprintf('\nFactor graph:\n')); @@ -58,15 +59,15 @@ graph.print(sprintf('\nFactor graph:\n')); %% Initialize cameras and points close to ground truth in this example import gtsam.* -initialEstimate = sparseBA.Values; +initialEstimate = Values; for i=1:size(truth.cameras,2) pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1)); camera_i = SimpleCamera(pose_i, truth.K); - initialEstimate.insertSimpleCamera(symbol('c',i), camera_i); + initialEstimate.insert(symbol('c',i), camera_i); end for j=1:size(truth.points,2) point_j = truth.points{j}.retract(0.1*randn(3,1)); - initialEstimate.insertPoint(symbol('p',j), point_j); + initialEstimate.insert(symbol('p',j), point_j); end initialEstimate.print(sprintf('\nInitial estimate:\n ')); @@ -77,7 +78,7 @@ parameters = LevenbergMarquardtParams; parameters.setlambdaInitial(1.0); parameters.setVerbosityLM('trylambda'); -optimizer = graph.optimizer(initialEstimate, parameters); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, parameters); for i=1:5 optimizer.iterate(); diff --git a/matlab/examples/SFMExample.m b/matlab/gtsam_examples/SFMExample.m similarity index 70% rename from matlab/examples/SFMExample.m rename to matlab/gtsam_examples/SFMExample.m index 7c4baa75b..92eeb482b 100644 --- a/matlab/examples/SFMExample.m +++ b/matlab/gtsam_examples/SFMExample.m @@ -22,14 +22,15 @@ options.nrCameras = 10; options.showImages = false; %% Generate data -[data,truth] = VisualISAMGenerateData(options); +[data,truth] = gtsam.VisualISAMGenerateData(options); measurementNoiseSigma = 1.0; pointNoiseSigma = 0.1; poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; %% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph) -graph = visualSLAM.Graph; +import gtsam.* +graph = NonlinearFactorGraph; %% Add factors for all measurements import gtsam.* @@ -37,29 +38,30 @@ measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma); for i=1:length(data.Z) for k=1:length(data.Z{i}) j = data.J{i}{k}; - graph.addMeasurement(data.Z{i}{k}, measurementNoise, symbol('x',i), symbol('p',j), data.K); + graph.add(GenericProjectionFactorCal3_S2(data.Z{i}{k}, measurementNoise, symbol('x',i), symbol('p',j), data.K)); end end %% Add Gaussian priors for a pose and a landmark to constrain the system import gtsam.* posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); -graph.addPosePrior(symbol('x',1), truth.cameras{1}.pose, posePriorNoise); +graph.add(PriorFactorPose3(symbol('x',1), truth.cameras{1}.pose, posePriorNoise)); pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); -graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise); +graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise)); %% Print the graph graph.print(sprintf('\nFactor graph:\n')); %% Initialize cameras and points close to ground truth in this example -initialEstimate = visualSLAM.Values; +import gtsam.* +initialEstimate = Values; for i=1:size(truth.cameras,2) pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1)); - initialEstimate.insertPose(symbol('x',i), pose_i); + initialEstimate.insert(symbol('x',i), pose_i); end for j=1:size(truth.points,2) point_j = truth.points{j}.retract(0.1*randn(3,1)); - initialEstimate.insertPoint(symbol('p',j), point_j); + initialEstimate.insert(symbol('p',j), point_j); end initialEstimate.print(sprintf('\nInitial estimate:\n ')); @@ -70,7 +72,7 @@ parameters = LevenbergMarquardtParams; parameters.setlambdaInitial(1.0); parameters.setVerbosityLM('trylambda'); -optimizer = graph.optimizer(initialEstimate, parameters); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, parameters); for i=1:5 optimizer.iterate(); @@ -79,21 +81,14 @@ result = optimizer.values(); result.print(sprintf('\nFinal result:\n ')); %% Plot results with covariance ellipses -marginals = graph.marginals(result); +import gtsam.* +marginals = Marginals(graph, result); cla hold on; -for j=1:result.nrPoints - P = marginals.marginalCovariance(symbol('p',j)); - point_j = result.point(symbol('p',j)); - plot3(point_j.x, point_j.y, point_j.z,'marker','o'); - covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); -end -for i=1:result.nrPoses - P = marginals.marginalCovariance(symbol('x',i)); - pose_i = result.pose(symbol('x',i)); - plotPose3(pose_i,P,10); -end +gtsam.plot3DPoints(result, [], marginals); +gtsam.plot3DTrajectory(result, '*', 1, 8, marginals); + axis([-40 40 -40 40 -10 20]);axis equal view(3) colormap('hot') diff --git a/matlab/examples/StereoVOExample.m b/matlab/gtsam_examples/StereoVOExample.m similarity index 53% rename from matlab/examples/StereoVOExample.m rename to matlab/gtsam_examples/StereoVOExample.m index 98e1b5845..94c4b3f37 100644 --- a/matlab/examples/StereoVOExample.m +++ b/matlab/gtsam_examples/StereoVOExample.m @@ -18,16 +18,18 @@ % - No noise on measurements %% Create keys for variables +import gtsam.* x1 = symbol('x',1); x2 = symbol('x',2); l1 = symbol('l',1); l2 = symbol('l',2); l3 = symbol('l',3); %% Create graph container and add factors to it -graph = visualSLAM.Graph; +import gtsam.* +graph = NonlinearFactorGraph; %% add a constraint on the starting pose import gtsam.* first_pose = Pose3(); -graph.addPoseConstraint(x1, first_pose); +graph.add(NonlinearEqualityPose3(x1, first_pose)); %% Create realistic calibration and measurement noise model % format: fx fy skew cx cy baseline @@ -38,48 +40,41 @@ stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]); %% Add measurements import gtsam.* % pose 1 -graph.addStereoMeasurement(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K); -graph.addStereoMeasurement(StereoPoint2(120, 80, 440), stereo_model, x1, l2, K); -graph.addStereoMeasurement(StereoPoint2(320, 280, 140), stereo_model, x1, l3, K); +graph.add(GenericStereoFactor3D(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K)); +graph.add(GenericStereoFactor3D(StereoPoint2(120, 80, 440), stereo_model, x1, l2, K)); +graph.add(GenericStereoFactor3D(StereoPoint2(320, 280, 140), stereo_model, x1, l3, K)); %pose 2 -graph.addStereoMeasurement(StereoPoint2(570, 520, 490), stereo_model, x2, l1, K); -graph.addStereoMeasurement(StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K); -graph.addStereoMeasurement(StereoPoint2(320, 270, 115), stereo_model, x2, l3, K); +graph.add(GenericStereoFactor3D(StereoPoint2(570, 520, 490), stereo_model, x2, l1, K)); +graph.add(GenericStereoFactor3D(StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K)); +graph.add(GenericStereoFactor3D(StereoPoint2(320, 270, 115), stereo_model, x2, l3, K)); %% Create initial estimate for camera poses and landmarks import gtsam.* -initialEstimate = visualSLAM.Values; -initialEstimate.insertPose(x1, first_pose); +initialEstimate = Values; +initialEstimate.insert(x1, first_pose); % noisy estimate for pose 2 -initialEstimate.insertPose(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1))); -initialEstimate.insertPoint(l1, Point3( 1, 1, 5)); -initialEstimate.insertPoint(l2, Point3(-1, 1, 5)); -initialEstimate.insertPoint(l3, Point3( 0,-.5, 5)); +initialEstimate.insert(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1))); +initialEstimate.insert(l1, Point3( 1, 1, 5)); +initialEstimate.insert(l2, Point3(-1, 1, 5)); +initialEstimate.insert(l3, Point3( 0,-.5, 5)); %% optimize fprintf(1,'Optimizing\n'); tic -result = graph.optimize(initialEstimate,1); +import gtsam.* +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); toc %% visualize initial trajectory, final trajectory, and final points cla; hold on; axis normal -axis([-1 6 -2 2 -1.5 1.5]); +axis([-1.5 1.5 -2 2 -1 6]); axis equal view(-38,12) +camup([0;1;0]); -% initial trajectory in red (rotated so Z is up) -T=initialEstimate.translations; -plot3(T(:,3),-T(:,1),-T(:,2), '-*r','LineWidth',2); - -% final trajectory in green (rotated so Z is up) -T=result.translations; -plot3(T(:,3),-T(:,1),-T(:,2), '-*g','LineWidth',2); -xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); - -% optimized 3D points (rotated so Z is up) -P = result.points(); -plot3(P(:,3),-P(:,1),-P(:,2),'*'); - +gtsam.plot3DTrajectory(initialEstimate, 'r', 1, 0.3); +gtsam.plot3DTrajectory(result, 'g', 1, 0.3); +gtsam.plot3DPoints(result); diff --git a/matlab/examples/StereoVOExample_large.m b/matlab/gtsam_examples/StereoVOExample_large.m similarity index 58% rename from matlab/examples/StereoVOExample_large.m rename to matlab/gtsam_examples/StereoVOExample_large.m index 7d670c99c..850af4f80 100644 --- a/matlab/examples/StereoVOExample_large.m +++ b/matlab/gtsam_examples/StereoVOExample_large.m @@ -13,75 +13,67 @@ %% Load calibration import gtsam.* % format: fx fy skew cx cy baseline -calib = dlmread('Data/VO_calibration.txt'); +calib = dlmread(gtsam.findExampleDataFile('VO_calibration.txt')); K = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]); %% create empty graph and values -graph = visualSLAM.Graph; -initial = visualSLAM.Values; +graph = NonlinearFactorGraph; +initial = Values; %% load the initial poses from VO % row format: camera_id 4x4 pose (row, major) import gtsam.* fprintf(1,'Reading data\n'); -cameras = dlmread('Data/VO_camera_poses_large.txt'); +cameras = dlmread(gtsam.findExampleDataFile('VO_camera_poses_large.txt')); for i=1:size(cameras,1) pose = Pose3(reshape(cameras(i,2:17),4,4)'); - initial.insertPose(symbol('x',cameras(i,1)),pose); + initial.insert(symbol('x',cameras(i,1)),pose); end %% load stereo measurements and initialize landmarks % camera_id landmark_id uL uR v X Y Z import gtsam.* -measurements = dlmread('Data/VO_stereo_factors_large.txt'); +measurements = dlmread(gtsam.findExampleDataFile('VO_stereo_factors_large.txt')); fprintf(1,'Creating Graph\n'); tic for i=1:size(measurements,1) sf = measurements(i,:); - graph.addStereoMeasurement(StereoPoint2(sf(3),sf(4),sf(5)), stereo_model, ... - symbol('x', sf(1)), symbol('l', sf(2)), K); + graph.add(GenericStereoFactor3D(StereoPoint2(sf(3),sf(4),sf(5)), stereo_model, ... + symbol('x', sf(1)), symbol('l', sf(2)), K)); if ~initial.exists(symbol('l',sf(2))) % 3D landmarks are stored in camera coordinates: transform % to world coordinates using the respective initial pose - pose = initial.pose(symbol('x', sf(1))); + pose = initial.at(symbol('x', sf(1))); world_point = pose.transform_from(Point3(sf(6),sf(7),sf(8))); - initial.insertPoint(symbol('l',sf(2)), world_point); + initial.insert(symbol('l',sf(2)), world_point); end end toc %% add a constraint on the starting pose +import gtsam.* key = symbol('x',1); -first_pose = initial.pose(key); -graph.addPoseConstraint(key, first_pose); +first_pose = initial.at(key); +graph.add(NonlinearEqualityPose3(key, first_pose)); %% optimize +import gtsam.* fprintf(1,'Optimizing\n'); tic -result = graph.optimize(initial,1); +optimizer = LevenbergMarquardtOptimizer(graph, initial); +result = optimizer.optimizeSafely(); toc %% visualize initial trajectory, final trajectory, and final points -figure(1); clf; hold on; +cla; hold on; -% initial trajectory in red (rotated so Z is up) -P = initial.translations; -plot3(P(:,3),-P(:,1),-P(:,2), '-*r','LineWidth',2); +gtsam.plot3DTrajectory(initial, 'r', 1, 0.5); +gtsam.plot3DTrajectory(result, 'g', 1, 0.5); +gtsam.plot3DPoints(result); -% final trajectory in green (rotated so Z is up) -P = result.translations; -plot3(P(:,3),-P(:,1),-P(:,2), '-*g','LineWidth',2); -xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); - -% switch to XZ view -view([0 0]); - -% optimized 3D points (rotated so Z is up) -points = result.points(); -plot3(points(:,3),-points(:,1),-points(:,2),'.'); - -axis([0 100 -20 20 -5 20]); +axis([-5 20 -20 20 0 100]); axis equal view(3) +camup([0;1;0]); diff --git a/matlab/examples/VisualISAMDemo.m b/matlab/gtsam_examples/VisualISAMDemo.m similarity index 100% rename from matlab/examples/VisualISAMDemo.m rename to matlab/gtsam_examples/VisualISAMDemo.m diff --git a/matlab/examples/VisualISAMExample.m b/matlab/gtsam_examples/VisualISAMExample.m similarity index 76% rename from matlab/examples/VisualISAMExample.m rename to matlab/gtsam_examples/VisualISAMExample.m index dc6a95ae2..31c240029 100644 --- a/matlab/examples/VisualISAMExample.m +++ b/matlab/gtsam_examples/VisualISAMExample.m @@ -32,17 +32,17 @@ options.saveFigures = false; options.saveDotFiles = false; %% Generate data -[data,truth] = VisualISAMGenerateData(options); +[data,truth] = gtsam.VisualISAMGenerateData(options); %% Initialize iSAM with the first pose and points -[noiseModels,isam,result] = VisualISAMInitialize(data,truth,options); +[noiseModels,isam,result,nextPose] = gtsam.VisualISAMInitialize(data,truth,options); cla; -VisualISAMPlot(truth, data, isam, result, options) +gtsam.VisualISAMPlot(truth, data, isam, result, options) %% Main loop for iSAM: stepping through all poses for frame_i=3:options.nrCameras - [isam,result] = VisualISAMStep(data,noiseModels,isam,result,truth,options); + [isam,result,nextPose] = gtsam.VisualISAMStep(data,noiseModels,isam,result,truth,nextPose); if mod(frame_i,options.drawInterval)==0 - VisualISAMPlot(truth, data, isam, result, options) + gtsam.VisualISAMPlot(truth, data, isam, result, options) end end diff --git a/matlab/gtsam_examples/VisualISAM_gui.fig b/matlab/gtsam_examples/VisualISAM_gui.fig new file mode 100644 index 000000000..3c8adb46c Binary files /dev/null and b/matlab/gtsam_examples/VisualISAM_gui.fig differ diff --git a/matlab/VisualISAM_gui.m b/matlab/gtsam_examples/VisualISAM_gui.m similarity index 92% rename from matlab/VisualISAM_gui.m rename to matlab/gtsam_examples/VisualISAM_gui.m index cdb2ddbaf..bc08e7abf 100644 --- a/matlab/VisualISAM_gui.m +++ b/matlab/gtsam_examples/VisualISAM_gui.m @@ -224,32 +224,32 @@ function saveGraphsCB_Callback(hObject, ~, handles) % --- Executes on button press in intializeButton. function intializeButton_Callback(hObject, ~, handles) -global frame_i truth data noiseModels isam result options +global frame_i truth data noiseModels isam result nextPoseIndex options % initialize global options initOptions(handles) % Generate Data -[data,truth] = VisualISAMGenerateData(options); +[data,truth] = gtsam.VisualISAMGenerateData(options); % Initialize and plot -[noiseModels,isam,result] = VisualISAMInitialize(data,truth,options); +[noiseModels,isam,result,nextPoseIndex] = gtsam.VisualISAMInitialize(data,truth,options); cla -VisualISAMPlot(truth, data, isam, result, options) +gtsam.VisualISAMPlot(truth, data, isam, result, options) frame_i = 2; showFramei(hObject, handles) % --- Executes on button press in runButton. function runButton_Callback(hObject, ~, handles) -global frame_i truth data noiseModels isam result options +global frame_i truth data noiseModels isam result nextPoseIndex options while (frame_i2 - pPp = P(1:2,1:2); % covariance matrix in pose coordinate frame - covarianceEllipse([p.x;p.y],pPp,color(1)); -end \ No newline at end of file diff --git a/matlab/symbol.m b/matlab/symbol.m deleted file mode 100644 index 10a733593..000000000 --- a/matlab/symbol.m +++ /dev/null @@ -1,4 +0,0 @@ -function key = symbol(c,i) -% generate a key corresponding to a symbol -s = gtsam.Symbol(c,i); -key = s.key(); \ No newline at end of file diff --git a/matlab/symbolChr.m b/matlab/symbolChr.m deleted file mode 100644 index 6cabdb2b2..000000000 --- a/matlab/symbolChr.m +++ /dev/null @@ -1,4 +0,0 @@ -function c = symbolChr(key) -% generate the chr from a key -s = gtsam.Symbol(key); -c = s.chr(); \ No newline at end of file diff --git a/matlab/symbolIndex.m b/matlab/symbolIndex.m deleted file mode 100644 index ad6eaf97d..000000000 --- a/matlab/symbolIndex.m +++ /dev/null @@ -1,4 +0,0 @@ -function i = symbolIndex(key) -% generate the index from a key -s = gtsam.Symbol(key); -i = s.index(); \ No newline at end of file diff --git a/matlab/tests/testPose3SLAMExample.m b/matlab/tests/testPose3SLAMExample.m deleted file mode 100644 index ac3c0d5bb..000000000 --- a/matlab/tests/testPose3SLAMExample.m +++ /dev/null @@ -1,47 +0,0 @@ -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% GTSAM Copyright 2010, Georgia Tech Research Corporation, -% Atlanta, Georgia 30332-0415 -% All Rights Reserved -% Authors: Frank Dellaert, et al. (see THANKS for the full author list) -% -% See LICENSE for the license information -% -% @brief Read graph from file and perform GraphSLAM -% @author Frank Dellaert -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%% Create a hexagon of poses -hexagon = pose3SLAM.Values.Circle(6,1.0); -p0 = hexagon.pose(0); -p1 = hexagon.pose(1); - -%% create a Pose graph with one equality constraint and one measurement -import gtsam.* -fg = pose3SLAM.Graph; -fg.addPoseConstraint(0, p0); -delta = p0.between(p1); -covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); -fg.addRelativePose(0,1, delta, covariance); -fg.addRelativePose(1,2, delta, covariance); -fg.addRelativePose(2,3, delta, covariance); -fg.addRelativePose(3,4, delta, covariance); -fg.addRelativePose(4,5, delta, covariance); -fg.addRelativePose(5,0, delta, covariance); - -%% Create initial config -initial = pose3SLAM.Values; -s = 0.10; -initial.insertPose(0, p0); -initial.insertPose(1, hexagon.pose(1).retract(s*randn(6,1))); -initial.insertPose(2, hexagon.pose(2).retract(s*randn(6,1))); -initial.insertPose(3, hexagon.pose(3).retract(s*randn(6,1))); -initial.insertPose(4, hexagon.pose(4).retract(s*randn(6,1))); -initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1))); - -%% optimize -result = fg.optimize(initial,0); - -pose_1 = result.pose(1); -CHECK('pose_1.equals(Pose3,1e-4)',pose_1.equals(p1,1e-4)); - - diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 9a4f6bf25..3c5409af7 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -15,14 +15,13 @@ * @author Richard Roberts */ -#include #include #include #include #include #include #include -#include +#include #include #include diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 2c932680d..67fa287d3 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -6,15 +6,21 @@ #include #include +#include +#include #include -#include #include #include #include #include +#include +#include +#include #include +#include +#include +#include #include -#include #include @@ -39,21 +45,21 @@ SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas(Vector_(2, M_PI/100.0, 0.1 ISAM2 createSlamlikeISAM2( boost::optional init_values = boost::none, - boost::optional full_graph = boost::none, + boost::optional full_graph = boost::none, const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)) { // These variables will be reused and accumulate factors and values ISAM2 isam(params); Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; // i keeps track of the time step size_t i = 0; // Add a prior at time 0 and update isam { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + NonlinearFactorGraph newfactors; + newfactors.add(PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -65,8 +71,8 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 0 to time 5 for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -78,10 +84,10 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 5 to 6 and landmark measurement at time 5 { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); + newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); fullgraph.push_back(newfactors); Values init; @@ -98,8 +104,8 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 6 to time 10 for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -111,10 +117,10 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 10 to 11 and landmark measurement at time 10 { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); + newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); fullgraph.push_back(newfactors); Values init; @@ -298,8 +304,8 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) { // typedef GaussianISAM2::Impl Impl; // // Ordering ordering; ordering += (0), (0), (1); -// planarSLAM::Graph graph; -// graph.addPosePrior((0), Pose2(), noiseModel::Unit::Create(Pose2::dimension)); +// NonlinearFactorGraph graph; +// graph.add(PriorFactor((0), Pose2(), noiseModel::Unit::Create(Pose2::dimension)); // graph.addRange((0), (0), 1.0, noiseModel::Unit::Create(1)); // // FastSet expected; @@ -378,7 +384,7 @@ TEST(ISAM2, optimize2) { } /* ************************************************************************* */ -bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { +bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { TestResult& result_ = result; const std::string name_ = test.getName(); @@ -439,7 +445,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) { // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Compare solutions @@ -451,7 +457,7 @@ TEST(ISAM2, slamlike_solution_dogleg) { // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); // Compare solutions @@ -463,7 +469,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr) { // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR)); // Compare solutions @@ -475,7 +481,7 @@ TEST(ISAM2, slamlike_solution_dogleg_qr) { // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR)); // Compare solutions @@ -584,13 +590,13 @@ TEST(ISAM2, removeFactors) // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Remove the 2nd measurement on landmark 0 (Key 100) FastVector toRemove; toRemove.push_back(12); - isam.update(planarSLAM::Graph(), Values(), toRemove); + isam.update(NonlinearFactorGraph(), Values(), toRemove); // Remove the factor from the full system fullgraph.remove(12); @@ -604,14 +610,14 @@ TEST_UNSAFE(ISAM2, removeVariables) { // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Remove the measurement on landmark 0 (Key 100) FastVector toRemove; toRemove.push_back(7); toRemove.push_back(14); - isam.update(planarSLAM::Graph(), Values(), toRemove); + isam.update(NonlinearFactorGraph(), Values(), toRemove); // Remove the factors and variable from the full system fullgraph.remove(7); @@ -629,7 +635,7 @@ TEST_UNSAFE(ISAM2, swapFactors) // then swaps the 2nd-to-last landmark measurement with a different one Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph); // Remove the measurement on landmark 0 and replace with a different one @@ -639,15 +645,15 @@ TEST_UNSAFE(ISAM2, swapFactors) toRemove.push_back(swap_idx); fullgraph.remove(swap_idx); - planarSLAM::Graph swapfactors; -// swapfactors.addBearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); // original factor - swapfactors.addBearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise); + NonlinearFactorGraph swapfactors; +// swapfactors.add(BearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); // original factor + swapfactors.add(BearingRangeFactor(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise)); fullgraph.push_back(swapfactors); isam.update(swapfactors, Values(), toRemove); } // Compare solutions - EXPECT(assert_equal(fullgraph, planarSLAM::Graph(isam.getFactorsUnsafe()))); + EXPECT(assert_equal(fullgraph, NonlinearFactorGraph(isam.getFactorsUnsafe()))); EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); // Check gradient at each node @@ -685,7 +691,7 @@ TEST(ISAM2, constrained_ordering) // These variables will be reused and accumulate factors and values ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; // We will constrain x3 and x4 to the end FastMap constrained; @@ -697,8 +703,8 @@ TEST(ISAM2, constrained_ordering) // Add a prior at time 0 and update isam { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + NonlinearFactorGraph newfactors; + newfactors.add(PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -712,8 +718,8 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 0 to time 5 for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -728,10 +734,10 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 5 to 6 and landmark measurement at time 5 { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); + newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); fullgraph.push_back(newfactors); Values init; @@ -748,8 +754,8 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 6 to time 10 for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -761,10 +767,10 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 10 to 11 and landmark measurement at time 10 { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); + newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); fullgraph.push_back(newfactors); Values init; @@ -816,7 +822,7 @@ TEST(ISAM2, slamlike_solution_partial_relinearization_check) // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false); params.enablePartialRelinearizationCheck = true; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, params); diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 883011da2..422e52377 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -16,14 +16,18 @@ */ #include -#include -#include +#include +#include +#include +#include +#include #include #include #include #include #include -#include +#include +#include #include #include #include @@ -128,29 +132,29 @@ TEST( GaussianJunctionTreeB, optimizeMultiFrontal2) /* ************************************************************************* */ TEST(GaussianJunctionTreeB, slamlike) { Values init; - planarSLAM::Graph newfactors; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph newfactors; + NonlinearFactorGraph fullgraph; SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas(Vector_(2, M_PI/100.0, 0.1)); size_t i = 0; - newfactors = planarSLAM::Graph(); - newfactors.addPosePrior(X(0), Pose2(0.0, 0.0, 0.0), odoNoise); + newfactors = NonlinearFactorGraph(); + newfactors.add(PriorFactor(X(0), Pose2(0.0, 0.0, 0.0), odoNoise)); init.insert(X(0), Pose2(0.01, 0.01, 0.01)); fullgraph.push_back(newfactors); for( ; i<5; ++i) { - newfactors = planarSLAM::Graph(); - newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors = NonlinearFactorGraph(); + newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullgraph.push_back(newfactors); } - newfactors = planarSLAM::Graph(); - newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + newfactors = NonlinearFactorGraph(); + newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); + newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); init.insert(X(i+1), Pose2(1.01, 0.01, 0.01)); init.insert(L(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); init.insert(L(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); @@ -158,16 +162,16 @@ TEST(GaussianJunctionTreeB, slamlike) { ++ i; for( ; i<5; ++i) { - newfactors = planarSLAM::Graph(); - newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors = NonlinearFactorGraph(); + newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullgraph.push_back(newfactors); } - newfactors = planarSLAM::Graph(); - newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors = NonlinearFactorGraph(); + newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); + newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); init.insert(X(i+1), Pose2(6.9, 0.1, 0.01)); fullgraph.push_back(newfactors); ++ i; @@ -193,9 +197,9 @@ TEST(GaussianJunctionTreeB, simpleMarginal) { typedef BayesTree GaussianBayesTree; // Create a simple graph - pose2SLAM::Graph fg; - fg.addPosePrior(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0)); - fg.addRelativePose(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0))); + NonlinearFactorGraph fg; + fg.add(PriorFactor(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0))); + fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0)))); Values init; init.insert(X(0), Pose2()); diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index bb7743bd9..4f556eb0f 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -5,8 +5,12 @@ * @date Jun 11, 2012 */ -#include +#include +#include #include +#include +#include +#include #include @@ -19,43 +23,67 @@ using namespace std; using namespace gtsam; -boost::tuple generateProblem() { +boost::tuple generateProblem() { // 1. Create graph container and add factors to it - pose2SLAM::Graph graph ; + NonlinearFactorGraph graph ; // 2a. Add Gaussian prior Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - graph.addPosePrior(1, priorMean, priorNoise); + graph.add(PriorFactor(1, priorMean, priorNoise)); // 2b. Add odometry factors SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); - graph.addRelativePose(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addRelativePose(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addRelativePose(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); + graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.add(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); // 2c. Add pose constraint SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addRelativePose(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); + graph.add(BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty)); // 3. Create the data structure to hold the initialEstimate estinmate to the solution - pose2SLAM::Values initialEstimate; - Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1); - Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2); - Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insertPose(3, x3); - Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insertPose(4, x4); - Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insertPose(5, x5); + Values initialEstimate; + Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insert(1, x1); + Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insert(2, x2); + Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insert(3, x3); + Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insert(4, x4); + Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insert(5, x5); return boost::tie(graph, initialEstimate); } +/* ************************************************************************* */ +TEST(optimize, GradientDescentOptimizer) { + + NonlinearFactorGraph graph; + Values initialEstimate; + + boost::tie(graph, initialEstimate) = generateProblem(); + // cout << "initial error = " << graph.error(initialEstimate) << endl ; + + // Single Step Optimization using Levenberg-Marquardt + NonlinearOptimizerParams param; + param.maxIterations = 500; /* requires a larger number of iterations to converge */ + param.verbosity = NonlinearOptimizerParams::SILENT; + + NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); + Values result = optimizer.optimize(); +// cout << "gd1 solver final error = " << graph.error(result) << endl; + + /* the optimality of the solution is not comparable to the */ + DOUBLES_EQUAL(0.0, graph.error(result), 1e-2); + + CHECK(1); +} + /* ************************************************************************* */ TEST(optimize, ConjugateGradientOptimizer) { - pose2SLAM::Graph graph ; - pose2SLAM::Values initialEstimate; + NonlinearFactorGraph graph; + Values initialEstimate; boost::tie(graph, initialEstimate) = generateProblem(); // cout << "initial error = " << graph.error(initialEstimate) << endl ; diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 5f99d60c6..93ecd6dbe 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -16,8 +16,10 @@ * @brief unit test for graph-inl.h */ -#include -#include +#include +#include +#include +#include #include @@ -75,13 +77,13 @@ TEST( Graph, predecessorMap2Graph ) /* ************************************************************************* */ TEST( Graph, composePoses ) { - pose2SLAM::Graph graph; + NonlinearFactorGraph graph; SharedNoiseModel cov = noiseModel::Unit::Create(3); Pose2 p1(1.0, 2.0, 0.3), p2(4.0, 5.0, 0.6), p3(7.0, 8.0, 0.9), p4(2.0, 2.0, 2.9); Pose2 p12=p1.between(p2), p23=p2.between(p3), p43=p4.between(p3); - graph.addRelativePose(1,2, p12, cov); - graph.addRelativePose(2,3, p23, cov); - graph.addRelativePose(4,3, p43, cov); + graph.add(BetweenFactor(1,2, p12, cov)); + graph.add(BetweenFactor(2,3, p23, cov)); + graph.add(BetweenFactor(4,3, p43, cov)); PredecessorMap tree; tree.insert(1,2); @@ -91,8 +93,7 @@ TEST( Graph, composePoses ) Pose2 rootPose = p2; - boost::shared_ptr actual = composePoses (graph, tree, rootPose); + boost::shared_ptr actual = composePoses, Pose2, Key> (graph, tree, rootPose); Values expected; expected.insert(1, p1); diff --git a/tests/testInferenceB.cpp b/tests/testInferenceB.cpp index 3e68780c4..9b8127473 100644 --- a/tests/testInferenceB.cpp +++ b/tests/testInferenceB.cpp @@ -17,10 +17,17 @@ #include +#include +#include +#include +#include +#include #include #include #include -#include +#include +#include +#include #include @@ -54,16 +61,16 @@ TEST( inference, marginals ) /* ************************************************************************* */ TEST( inference, marginals2) { - planarSLAM::Graph fg; + NonlinearFactorGraph fg; SharedDiagonal poseModel(noiseModel::Isotropic::Sigma(3, 0.1)); SharedDiagonal pointModel(noiseModel::Isotropic::Sigma(3, 0.1)); - fg.addPosePrior(X(0), Pose2(), poseModel); - fg.addRelativePose(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel); - fg.addRelativePose(X(1), X(2), Pose2(1.0,0.0,0.0), poseModel); - fg.addBearingRange(X(0), L(0), Rot2(), 1.0, pointModel); - fg.addBearingRange(X(1), L(0), Rot2(), 1.0, pointModel); - fg.addBearingRange(X(2), L(0), Rot2(), 1.0, pointModel); + fg.add(PriorFactor(X(0), Pose2(), poseModel)); + fg.add(BetweenFactor(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel)); + fg.add(BetweenFactor(X(1), X(2), Pose2(1.0,0.0,0.0), poseModel)); + fg.add(BearingRangeFactor(X(0), L(0), Rot2(), 1.0, pointModel)); + fg.add(BearingRangeFactor(X(1), L(0), Rot2(), 1.0, pointModel)); + fg.add(BearingRangeFactor(X(2), L(0), Rot2(), 1.0, pointModel)); Values init; init.insert(X(0), Pose2(0.0,0.0,0.0)); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 1efc40cad..2b580560f 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -17,12 +17,17 @@ #include #include -#include +#include #include #include #include #include +#include #include +#include +#include +#include +#include #include @@ -510,7 +515,7 @@ static Cal3_S2 K(fov,w,h); static boost::shared_ptr shK(new Cal3_S2(K)); // typedefs for visual SLAM example -typedef visualSLAM::Graph VGraph; +typedef NonlinearFactorGraph VGraph; // factors for visual slam typedef NonlinearEquality2 Point3Equality; @@ -537,13 +542,13 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { VGraph graph; // create equality constraints for poses - graph.addPoseConstraint(x1, camera1.pose()); - graph.addPoseConstraint(x2, camera2.pose()); + graph.add(NonlinearEquality(x1, camera1.pose())); + graph.add(NonlinearEquality(x2, camera2.pose())); // create factors SharedDiagonal vmodel = noiseModel::Unit::Create(3); - graph.addMeasurement(camera1.project(landmark), vmodel, x1, l1, shK); - graph.addMeasurement(camera2.project(landmark), vmodel, x2, l2, shK); + graph.add(GenericProjectionFactor(camera1.project(landmark), vmodel, x1, l1, shK)); + graph.add(GenericProjectionFactor(camera2.project(landmark), vmodel, x2, l2, shK)); // add equality constraint graph.add(Point3Equality(l1, l2)); diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 980ea8033..142fba7eb 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -5,13 +5,17 @@ #include -#include +#include +#include #include +#include +#include #include -#include +#include +#include + using namespace gtsam; -using namespace planarSLAM; typedef NonlinearISAM PlanarISAM; @@ -27,22 +31,22 @@ TEST(testNonlinearISAM, markov_chain ) { // create initial graph Pose2 cur_pose; // start at origin - Graph start_factors; - start_factors.addPoseConstraint(0, cur_pose); + NonlinearFactorGraph start_factors; + start_factors.add(NonlinearEquality(0, cur_pose)); - planarSLAM::Values init; - planarSLAM::Values expected; - init.insertPose(0, cur_pose); - expected.insertPose(0, cur_pose); + Values init; + Values expected; + init.insert(0, cur_pose); + expected.insert(0, cur_pose); isam.update(start_factors, init); // loop for a period of time to verify memory usage size_t nrPoses = 21; Pose2 z(1.0, 2.0, 0.1); for (size_t i=1; i<=nrPoses; ++i) { - Graph new_factors; - new_factors.addRelativePose(i-1, i, z, model); - planarSLAM::Values new_init; + NonlinearFactorGraph new_factors; + new_factors.add(BetweenFactor(i-1, i, z, model)); + Values new_init; // perform a check on changing orderings if (i == 5) { @@ -60,15 +64,15 @@ TEST(testNonlinearISAM, markov_chain ) { } cur_pose = cur_pose.compose(z); - new_init.insertPose(i, cur_pose.retract(sampler.sample())); - expected.insertPose(i, cur_pose); + new_init.insert(i, cur_pose.retract(sampler.sample())); + expected.insert(i, cur_pose); isam.update(new_factors, new_init); } // verify values - all but the last one should be very close - planarSLAM::Values actual = isam.estimate(); + Values actual = isam.estimate(); for (size_t i=0; i(i), actual.at(i), tol)); } } diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index eb7585420..cb5e8f482 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -16,7 +16,10 @@ */ #include -#include +#include +#include +#include +#include #include #include #include @@ -24,6 +27,7 @@ #include #include #include +#include #include #include @@ -174,9 +178,9 @@ TEST( NonlinearOptimizer, Factorization ) config.insert(X(1), Pose2(0.,0.,0.)); config.insert(X(2), Pose2(1.5,0.,0.)); - pose2SLAM::Graph graph; - graph.addPosePrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); - graph.addRelativePose(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); + NonlinearFactorGraph graph; + graph.add(PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10))); + graph.add(BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1))); Ordering ordering; ordering.push_back(X(1)); diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index d9094111e..4f9664e20 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -17,12 +17,37 @@ */ #include - -#include -#include +//#include +#include +#include +#include +//#include +#include +//#include +#include +#include +#include +#include +#include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include #include @@ -30,17 +55,79 @@ using namespace std; using namespace gtsam; using namespace gtsam::serializationTestHelpers; +// Creating as many permutations of factors as possible +typedef PriorFactor PriorFactorLieVector; +typedef PriorFactor PriorFactorLieMatrix; +typedef PriorFactor PriorFactorPoint2; +typedef PriorFactor PriorFactorStereoPoint2; +typedef PriorFactor PriorFactorPoint3; +typedef PriorFactor PriorFactorRot2; +typedef PriorFactor PriorFactorRot3; +typedef PriorFactor PriorFactorPose2; +typedef PriorFactor PriorFactorPose3; +typedef PriorFactor PriorFactorCal3_S2; +typedef PriorFactor PriorFactorCal3DS2; +typedef PriorFactor PriorFactorCalibratedCamera; +typedef PriorFactor PriorFactorSimpleCamera; +typedef PriorFactor PriorFactorStereoCamera; + +typedef BetweenFactor BetweenFactorLieVector; +typedef BetweenFactor BetweenFactorLieMatrix; +typedef BetweenFactor BetweenFactorPoint2; +typedef BetweenFactor BetweenFactorPoint3; +typedef BetweenFactor BetweenFactorRot2; +typedef BetweenFactor BetweenFactorRot3; +typedef BetweenFactor BetweenFactorPose2; +typedef BetweenFactor BetweenFactorPose3; + +typedef NonlinearEquality NonlinearEqualityLieVector; +typedef NonlinearEquality NonlinearEqualityLieMatrix; +typedef NonlinearEquality NonlinearEqualityPoint2; +typedef NonlinearEquality NonlinearEqualityStereoPoint2; +typedef NonlinearEquality NonlinearEqualityPoint3; +typedef NonlinearEquality NonlinearEqualityRot2; +typedef NonlinearEquality NonlinearEqualityRot3; +typedef NonlinearEquality NonlinearEqualityPose2; +typedef NonlinearEquality NonlinearEqualityPose3; +typedef NonlinearEquality NonlinearEqualityCal3_S2; +typedef NonlinearEquality NonlinearEqualityCal3DS2; +typedef NonlinearEquality NonlinearEqualityCalibratedCamera; +typedef NonlinearEquality NonlinearEqualitySimpleCamera; +typedef NonlinearEquality NonlinearEqualityStereoCamera; + +typedef RangeFactor RangeFactorPosePoint2; +typedef RangeFactor RangeFactorPosePoint3; +typedef RangeFactor RangeFactorPose2; +typedef RangeFactor RangeFactorPose3; +typedef RangeFactor RangeFactorCalibratedCameraPoint; +typedef RangeFactor RangeFactorSimpleCameraPoint; +typedef RangeFactor RangeFactorCalibratedCamera; +typedef RangeFactor RangeFactorSimpleCamera; + +typedef BearingFactor BearingFactor2D; +typedef BearingFactor BearingFactor3D; + +typedef BearingRangeFactor BearingRangeFactor2D; +typedef BearingRangeFactor BearingRangeFactor3D; + +typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; +typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; + +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +//typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; + +typedef gtsam::GeneralSFMFactor2 GeneralSFMFactor2Cal3_S2; + +typedef gtsam::GenericStereoFactor GenericStereoFactor3D; + + // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::L; -/* Create GUIDs for factors */ -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); +/* Create GUIDs for Noisemodels */ /* ************************************************************************* */ -// Export Noisemodels BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); @@ -50,6 +137,93 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropi BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +/* Create GUIDs for geometry */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT(gtsam::LieVector); +BOOST_CLASS_EXPORT(gtsam::LieMatrix); +BOOST_CLASS_EXPORT(gtsam::Point2); +BOOST_CLASS_EXPORT(gtsam::StereoPoint2); +BOOST_CLASS_EXPORT(gtsam::Point3); +BOOST_CLASS_EXPORT(gtsam::Rot2); +BOOST_CLASS_EXPORT(gtsam::Rot3); +BOOST_CLASS_EXPORT(gtsam::Pose2); +BOOST_CLASS_EXPORT(gtsam::Pose3); +BOOST_CLASS_EXPORT(gtsam::Cal3_S2); +BOOST_CLASS_EXPORT(gtsam::Cal3DS2); +BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo); +BOOST_CLASS_EXPORT(gtsam::CalibratedCamera); +BOOST_CLASS_EXPORT(gtsam::SimpleCamera); +BOOST_CLASS_EXPORT(gtsam::StereoCamera); + + +/* Create GUIDs for factors */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); + +BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector"); +BOOST_CLASS_EXPORT_GUID(PriorFactorLieMatrix, "gtsam::PriorFactorLieMatrix"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3"); +BOOST_CLASS_EXPORT_GUID(PriorFactorRot2, "gtsam::PriorFactorRot2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorRot3, "gtsam::PriorFactorRot3"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPose2, "gtsam::PriorFactorPose2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3"); +BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera"); +BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera"); +BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera"); + +BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorLieMatrix, "gtsam::BetweenFactorLieMatrix"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3"); + +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieVector, "gtsam::NonlinearEqualityLieVector"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieMatrix, "gtsam::NonlinearEqualityLieMatrix"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot2, "gtsam::NonlinearEqualityRot2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot3, "gtsam::NonlinearEqualityRot3"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose2, "gtsam::NonlinearEqualityPose2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); + +BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); +BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint"); +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); +BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); + +BOOST_CLASS_EXPORT_GUID(BearingFactor2D, "gtsam::BearingFactor2D"); + +BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); + +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2"); +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); + +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2"); +//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2"); + +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2"); + +BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); + + /* ************************************************************************* */ TEST (Serialization, smallExample_linear) { using namespace example; @@ -85,12 +259,11 @@ TEST (Serialization, gaussianISAM) { BOOST_CLASS_EXPORT_GUID(simulated2D::Prior, "gtsam::simulated2D::Prior" ) BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" ) BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement") -BOOST_CLASS_EXPORT(gtsam::Point2) /* ************************************************************************* */ -TEST (Serialization, smallExample) { +TEST (Serialization, smallExample_nonlinear) { using namespace example; - Graph nfg = createNonlinearFactorGraph(); + NonlinearFactorGraph nfg = createNonlinearFactorGraph(); Values c1 = createValues(); EXPECT(equalsObj(nfg)); EXPECT(equalsXML(nfg)); @@ -100,113 +273,321 @@ TEST (Serialization, smallExample) { } /* ************************************************************************* */ -/* Create GUIDs for factors */ -BOOST_CLASS_EXPORT_GUID(planarSLAM::Prior, "gtsam::planarSLAM::Prior"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Bearing, "gtsam::planarSLAM::Bearing"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Range, "gtsam::planarSLAM::Range"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::BearingRange,"gtsam::planarSLAM::BearingRange"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Odometry, "gtsam::planarSLAM::Odometry"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint, "gtsam::planarSLAM::Constraint"); +TEST (Serialization, factors) { -BOOST_CLASS_EXPORT(gtsam::Pose2) + LieVector lieVector(4, 1.0, 2.0, 3.0, 4.0); + LieMatrix lieMatrix(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0); + Point2 point2(1.0, 2.0); + StereoPoint2 stereoPoint2(1.0, 2.0, 3.0); + Point3 point3(1.0, 2.0, 3.0); + Rot2 rot2(1.0); + Rot3 rot3(Rot3::RzRyRx(1.0, 2.0, 3.0)); + Pose2 pose2(rot2, point2); + Pose3 pose3(rot3, point3); + Cal3_S2 cal3_s2(1.0, 2.0, 3.0, 4.0, 5.0); + Cal3DS2 cal3ds2(1.0, 2.0, 3.0, 4.0, 5.0,6.0, 7.0, 8.0, 9.0); + Cal3_S2Stereo cal3_s2stereo(1.0, 2.0, 3.0, 4.0, 5.0, 1.0); + CalibratedCamera calibratedCamera(pose3); + SimpleCamera simpleCamera(pose3, cal3_s2); + StereoCamera stereoCamera(pose3, boost::make_shared(cal3_s2stereo)); + + + Symbol a01('a',1), a02('a',2), a03('a',3), a04('a',4), a05('a',5), + a06('a',6), a07('a',7), a08('a',8), a09('a',9), a10('a',10), + a11('a',11), a12('a',12), a13('a',13), a14('a',14), a15('a',15); + Symbol b01('b',1), b02('b',2), b03('b',3), b04('b',4), b05('b',5), + b06('b',6), b07('b',7), b08('b',8), b09('b',9), b10('b',10), + b11('b',11), b12('b',12), b13('b',13), b14('b',14), b15('b',15); + + Values values; + values.insert(a01, lieVector); + values.insert(a02, lieMatrix); + values.insert(a03, point2); + values.insert(a04, stereoPoint2); + values.insert(a05, point3); + values.insert(a06, rot2); + values.insert(a07, rot3); + values.insert(a08, pose2); + values.insert(a09, pose3); + values.insert(a10, cal3_s2); + values.insert(a11, cal3ds2); + values.insert(a12, calibratedCamera); + values.insert(a13, simpleCamera); + values.insert(a14, stereoCamera); -/* ************************************************************************* */ -TEST (Serialization, planar_system) { - using namespace planarSLAM; - planarSLAM::Values values; - Symbol i2('x',2), i3('x',3), i4('x',4), i9('x',9), j3('l',3), j5('l',5), j9('l',9); - values.insert(j3, Point2(1.0, 2.0)); - values.insert(i4, Pose2(1.0, 2.0, 0.3)); SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3); SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); + SharedNoiseModel model4 = noiseModel::Isotropic::Sigma(4, 0.3); + SharedNoiseModel model5 = noiseModel::Isotropic::Sigma(5, 0.3); + SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3); + SharedNoiseModel model9 = noiseModel::Isotropic::Sigma(9, 0.3); + SharedNoiseModel model11 = noiseModel::Isotropic::Sigma(11, 0.3); - Prior prior(i3, Pose2(0.1,-0.3, 0.2), model1); - Bearing bearing(i3, j5, Rot2::fromDegrees(0.5), model1); - Range range(i2, j9, 7.0, model1); - BearingRange bearingRange(i2, j3, Rot2::fromDegrees(0.6), 2.0, model2); - Odometry odometry(i2, i3, Pose2(1.0, 2.0, 0.3), model3); - Constraint constraint(i9, Pose2(2.0,-1.0, 0.2)); - Graph graph; - graph.add(prior); - graph.add(bearing); - graph.add(range); - graph.add(bearingRange); - graph.add(odometry); - graph.add(constraint); + + PriorFactorLieVector priorFactorLieVector(a01, lieVector, model4); + PriorFactorLieMatrix priorFactorLieMatrix(a02, lieMatrix, model6); + PriorFactorPoint2 priorFactorPoint2(a03, point2, model2); + PriorFactorStereoPoint2 priorFactorStereoPoint2(a04, stereoPoint2, model3); + PriorFactorPoint3 priorFactorPoint3(a05, point3, model3); + PriorFactorRot2 priorFactorRot2(a06, rot2, model1); + PriorFactorRot3 priorFactorRot3(a07, rot3, model3); + PriorFactorPose2 priorFactorPose2(a08, pose2, model3); + PriorFactorPose3 priorFactorPose3(a09, pose3, model6); + PriorFactorCal3_S2 priorFactorCal3_S2(a10, cal3_s2, model5); + PriorFactorCal3DS2 priorFactorCal3DS2(a11, cal3ds2, model9); + PriorFactorCalibratedCamera priorFactorCalibratedCamera(a12, calibratedCamera, model6); + PriorFactorSimpleCamera priorFactorSimpleCamera(a13, simpleCamera, model11); + PriorFactorStereoCamera priorFactorStereoCamera(a14, stereoCamera, model11); + + BetweenFactorLieVector betweenFactorLieVector(a01, b01, lieVector, model4); + BetweenFactorLieMatrix betweenFactorLieMatrix(a02, b02, lieMatrix, model6); + BetweenFactorPoint2 betweenFactorPoint2(a03, b03, point2, model2); + BetweenFactorPoint3 betweenFactorPoint3(a05, b05, point3, model3); + BetweenFactorRot2 betweenFactorRot2(a06, b06, rot2, model1); + BetweenFactorRot3 betweenFactorRot3(a07, b07, rot3, model3); + BetweenFactorPose2 betweenFactorPose2(a08, b08, pose2, model3); + BetweenFactorPose3 betweenFactorPose3(a09, b09, pose3, model6); + + NonlinearEqualityLieVector nonlinearEqualityLieVector(a01, lieVector); + NonlinearEqualityLieMatrix nonlinearEqualityLieMatrix(a02, lieMatrix); + NonlinearEqualityPoint2 nonlinearEqualityPoint2(a03, point2); + NonlinearEqualityStereoPoint2 nonlinearEqualityStereoPoint2(a04, stereoPoint2); + NonlinearEqualityPoint3 nonlinearEqualityPoint3(a05, point3); + NonlinearEqualityRot2 nonlinearEqualityRot2(a06, rot2); + NonlinearEqualityRot3 nonlinearEqualityRot3(a07, rot3); + NonlinearEqualityPose2 nonlinearEqualityPose2(a08, pose2); + NonlinearEqualityPose3 nonlinearEqualityPose3(a09, pose3); + NonlinearEqualityCal3_S2 nonlinearEqualityCal3_S2(a10, cal3_s2); + NonlinearEqualityCal3DS2 nonlinearEqualityCal3DS2(a11, cal3ds2); + NonlinearEqualityCalibratedCamera nonlinearEqualityCalibratedCamera(a12, calibratedCamera); + NonlinearEqualitySimpleCamera nonlinearEqualitySimpleCamera(a13, simpleCamera); + NonlinearEqualityStereoCamera nonlinearEqualityStereoCamera(a14, stereoCamera); + + RangeFactorPosePoint2 rangeFactorPosePoint2(a08, a03, 2.0, model1); + RangeFactorPosePoint3 rangeFactorPosePoint3(a09, a05, 2.0, model1); + RangeFactorPose2 rangeFactorPose2(a08, b08, 2.0, model1); + RangeFactorPose3 rangeFactorPose3(a09, b09, 2.0, model1); + RangeFactorCalibratedCameraPoint rangeFactorCalibratedCameraPoint(a12, a05, 2.0, model1); + RangeFactorSimpleCameraPoint rangeFactorSimpleCameraPoint(a13, a05, 2.0, model1); + RangeFactorCalibratedCamera rangeFactorCalibratedCamera(a12, b12, 2.0, model1); + RangeFactorSimpleCamera rangeFactorSimpleCamera(a13, b13, 2.0, model1); + + BearingFactor2D bearingFactor2D(a08, a03, rot2, model1); + + BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2); + + GenericProjectionFactorCal3_S2 genericProjectionFactorCal3_S2(point2, model2, a09, a05, boost::make_shared(cal3_s2)); + GenericProjectionFactorCal3DS2 genericProjectionFactorCal3DS2(point2, model2, a09, a05, boost::make_shared(cal3ds2)); + + GeneralSFMFactorCal3_S2 generalSFMFactorCal3_S2(point2, model2, a13, a05); + + GeneralSFMFactor2Cal3_S2 generalSFMFactor2Cal3_S2(point2, model2, a09, a05, a10); + + GenericStereoFactor3D genericStereoFactor3D(stereoPoint2, model3, a09, a05, boost::make_shared(cal3_s2stereo)); + + + NonlinearFactorGraph graph; + graph.add(priorFactorLieVector); + graph.add(priorFactorLieMatrix); + graph.add(priorFactorPoint2); + graph.add(priorFactorStereoPoint2); + graph.add(priorFactorPoint3); + graph.add(priorFactorRot2); + graph.add(priorFactorRot3); + graph.add(priorFactorPose2); + graph.add(priorFactorPose3); + graph.add(priorFactorCal3_S2); + graph.add(priorFactorCal3DS2); + graph.add(priorFactorCalibratedCamera); + graph.add(priorFactorSimpleCamera); + graph.add(priorFactorStereoCamera); + + graph.add(betweenFactorLieVector); + graph.add(betweenFactorLieMatrix); + graph.add(betweenFactorPoint2); + graph.add(betweenFactorPoint3); + graph.add(betweenFactorRot2); + graph.add(betweenFactorRot3); + graph.add(betweenFactorPose2); + graph.add(betweenFactorPose3); + + graph.add(nonlinearEqualityLieVector); + graph.add(nonlinearEqualityLieMatrix); + graph.add(nonlinearEqualityPoint2); + graph.add(nonlinearEqualityStereoPoint2); + graph.add(nonlinearEqualityPoint3); + graph.add(nonlinearEqualityRot2); + graph.add(nonlinearEqualityRot3); + graph.add(nonlinearEqualityPose2); + graph.add(nonlinearEqualityPose3); + graph.add(nonlinearEqualityCal3_S2); + graph.add(nonlinearEqualityCal3DS2); + graph.add(nonlinearEqualityCalibratedCamera); + graph.add(nonlinearEqualitySimpleCamera); + graph.add(nonlinearEqualityStereoCamera); + + graph.add(rangeFactorPosePoint2); + graph.add(rangeFactorPosePoint3); + graph.add(rangeFactorPose2); + graph.add(rangeFactorPose3); + graph.add(rangeFactorCalibratedCameraPoint); + graph.add(rangeFactorSimpleCameraPoint); + graph.add(rangeFactorCalibratedCamera); + graph.add(rangeFactorSimpleCamera); + + graph.add(bearingFactor2D); + + graph.add(bearingRangeFactor2D); + + graph.add(genericProjectionFactorCal3_S2); + graph.add(genericProjectionFactorCal3DS2); + + graph.add(generalSFMFactorCal3_S2); + + graph.add(generalSFMFactor2Cal3_S2); + + graph.add(genericStereoFactor3D); + // text - EXPECT(equalsObj(i2)); - EXPECT(equalsObj(j3)); - EXPECT(equalsObj(values)); - EXPECT(equalsObj(prior)); - EXPECT(equalsObj(bearing)); - EXPECT(equalsObj(bearingRange)); - EXPECT(equalsObj(range)); - EXPECT(equalsObj(odometry)); - EXPECT(equalsObj(constraint)); - EXPECT(equalsObj(graph)); + EXPECT(equalsObj(a01)); + EXPECT(equalsObj(b02)); + EXPECT(equalsObj(values)); + EXPECT(equalsObj(graph)); + + EXPECT(equalsObj(priorFactorLieVector)); + EXPECT(equalsObj(priorFactorLieMatrix)); + EXPECT(equalsObj(priorFactorPoint2)); + EXPECT(equalsObj(priorFactorStereoPoint2)); + EXPECT(equalsObj(priorFactorPoint3)); + EXPECT(equalsObj(priorFactorRot2)); + EXPECT(equalsObj(priorFactorRot3)); + EXPECT(equalsObj(priorFactorPose2)); + EXPECT(equalsObj(priorFactorPose3)); + EXPECT(equalsObj(priorFactorCal3_S2)); + EXPECT(equalsObj(priorFactorCal3DS2)); + EXPECT(equalsObj(priorFactorCalibratedCamera)); + EXPECT(equalsObj(priorFactorSimpleCamera)); + EXPECT(equalsObj(priorFactorStereoCamera)); + + EXPECT(equalsObj(betweenFactorLieVector)); + EXPECT(equalsObj(betweenFactorLieMatrix)); + EXPECT(equalsObj(betweenFactorPoint2)); + EXPECT(equalsObj(betweenFactorPoint3)); + EXPECT(equalsObj(betweenFactorRot2)); + EXPECT(equalsObj(betweenFactorRot3)); + EXPECT(equalsObj(betweenFactorPose2)); + EXPECT(equalsObj(betweenFactorPose3)); + + EXPECT(equalsObj(nonlinearEqualityLieVector)); + EXPECT(equalsObj(nonlinearEqualityLieMatrix)); + EXPECT(equalsObj(nonlinearEqualityPoint2)); + EXPECT(equalsObj(nonlinearEqualityStereoPoint2)); + EXPECT(equalsObj(nonlinearEqualityPoint3)); + EXPECT(equalsObj(nonlinearEqualityRot2)); + EXPECT(equalsObj(nonlinearEqualityRot3)); + EXPECT(equalsObj(nonlinearEqualityPose2)); + EXPECT(equalsObj(nonlinearEqualityPose3)); + EXPECT(equalsObj(nonlinearEqualityCal3_S2)); + EXPECT(equalsObj(nonlinearEqualityCal3DS2)); + EXPECT(equalsObj(nonlinearEqualityCalibratedCamera)); + EXPECT(equalsObj(nonlinearEqualitySimpleCamera)); + EXPECT(equalsObj(nonlinearEqualityStereoCamera)); + + EXPECT(equalsObj(rangeFactorPosePoint2)); + EXPECT(equalsObj(rangeFactorPosePoint3)); + EXPECT(equalsObj(rangeFactorPose2)); + EXPECT(equalsObj(rangeFactorPose3)); + EXPECT(equalsObj(rangeFactorCalibratedCameraPoint)); + EXPECT(equalsObj(rangeFactorSimpleCameraPoint)); + EXPECT(equalsObj(rangeFactorCalibratedCamera)); + EXPECT(equalsObj(rangeFactorSimpleCamera)); + + EXPECT(equalsObj(bearingFactor2D)); + + EXPECT(equalsObj(bearingRangeFactor2D)); + + EXPECT(equalsObj(genericProjectionFactorCal3_S2)); + EXPECT(equalsObj(genericProjectionFactorCal3DS2)); + + EXPECT(equalsObj(generalSFMFactorCal3_S2)); + + EXPECT(equalsObj(generalSFMFactor2Cal3_S2)); + + EXPECT(equalsObj(genericStereoFactor3D)); + // xml - EXPECT(equalsXML(i2)); - EXPECT(equalsXML(j3)); - EXPECT(equalsXML(values)); - EXPECT(equalsXML(prior)); - EXPECT(equalsXML(bearing)); - EXPECT(equalsXML(bearingRange)); - EXPECT(equalsXML(range)); - EXPECT(equalsXML(odometry)); - EXPECT(equalsXML(constraint)); - EXPECT(equalsXML(graph)); + EXPECT(equalsXML(a01)); + EXPECT(equalsXML(b02)); + EXPECT(equalsXML(values)); + EXPECT(equalsXML(graph)); + + EXPECT(equalsXML(priorFactorLieVector)); + EXPECT(equalsXML(priorFactorLieMatrix)); + EXPECT(equalsXML(priorFactorPoint2)); + EXPECT(equalsXML(priorFactorStereoPoint2)); + EXPECT(equalsXML(priorFactorPoint3)); + EXPECT(equalsXML(priorFactorRot2)); + EXPECT(equalsXML(priorFactorRot3)); + EXPECT(equalsXML(priorFactorPose2)); + EXPECT(equalsXML(priorFactorPose3)); + EXPECT(equalsXML(priorFactorCal3_S2)); + EXPECT(equalsXML(priorFactorCal3DS2)); + EXPECT(equalsXML(priorFactorCalibratedCamera)); + EXPECT(equalsXML(priorFactorSimpleCamera)); + EXPECT(equalsXML(priorFactorStereoCamera)); + + EXPECT(equalsXML(betweenFactorLieVector)); + EXPECT(equalsXML(betweenFactorLieMatrix)); + EXPECT(equalsXML(betweenFactorPoint2)); + EXPECT(equalsXML(betweenFactorPoint3)); + EXPECT(equalsXML(betweenFactorRot2)); + EXPECT(equalsXML(betweenFactorRot3)); + EXPECT(equalsXML(betweenFactorPose2)); + EXPECT(equalsXML(betweenFactorPose3)); + + EXPECT(equalsXML(nonlinearEqualityLieVector)); + EXPECT(equalsXML(nonlinearEqualityLieMatrix)); + EXPECT(equalsXML(nonlinearEqualityPoint2)); + EXPECT(equalsXML(nonlinearEqualityStereoPoint2)); + EXPECT(equalsXML(nonlinearEqualityPoint3)); + EXPECT(equalsXML(nonlinearEqualityRot2)); + EXPECT(equalsXML(nonlinearEqualityRot3)); + EXPECT(equalsXML(nonlinearEqualityPose2)); + EXPECT(equalsXML(nonlinearEqualityPose3)); + EXPECT(equalsXML(nonlinearEqualityCal3_S2)); + EXPECT(equalsXML(nonlinearEqualityCal3DS2)); + EXPECT(equalsXML(nonlinearEqualityCalibratedCamera)); + EXPECT(equalsXML(nonlinearEqualitySimpleCamera)); + EXPECT(equalsXML(nonlinearEqualityStereoCamera)); + + EXPECT(equalsXML(rangeFactorPosePoint2)); + EXPECT(equalsXML(rangeFactorPosePoint3)); + EXPECT(equalsXML(rangeFactorPose2)); + EXPECT(equalsXML(rangeFactorPose3)); + EXPECT(equalsXML(rangeFactorCalibratedCameraPoint)); + EXPECT(equalsXML(rangeFactorSimpleCameraPoint)); + EXPECT(equalsXML(rangeFactorCalibratedCamera)); + EXPECT(equalsXML(rangeFactorSimpleCamera)); + + EXPECT(equalsXML(bearingFactor2D)); + + EXPECT(equalsXML(bearingRangeFactor2D)); + + EXPECT(equalsXML(genericProjectionFactorCal3_S2)); + EXPECT(equalsXML(genericProjectionFactorCal3DS2)); + + EXPECT(equalsXML(generalSFMFactorCal3_S2)); + + EXPECT(equalsXML(generalSFMFactor2Cal3_S2)); + + EXPECT(equalsXML(genericStereoFactor3D)); } -/* ************************************************************************* */ -/* Create GUIDs for factors */ -BOOST_CLASS_EXPORT_GUID(visualSLAM::PoseConstraint, "gtsam::visualSLAM::PoseConstraint"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PointConstraint, "gtsam::visualSLAM::PointConstraint"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PosePrior, "gtsam::visualSLAM::PosePrior"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PointPrior, "gtsam::visualSLAM::PointPrior"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::ProjectionFactor,"gtsam::visualSLAM::ProjectionFactor"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoFactor"); - -BOOST_CLASS_EXPORT(gtsam::Pose3) -BOOST_CLASS_EXPORT(gtsam::Point3) - -static Point3 pt3(1.0, 2.0, 3.0); -static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); -static Pose3 pose3(rt3, pt3); -static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); - -/* ************************************************************************* */ -TEST (Serialization, visual_system) { - using namespace visualSLAM; - visualSLAM::Values values; - Symbol x1('x',1), x2('x',2); - Symbol l1('l',1), l2('l',2); - Pose3 pose1 = pose3, pose2 = pose3.inverse(); - Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0); - values.insert(x1, pose1); - values.insert(l1, pt1); - SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); - SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); - SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3); - boost::shared_ptr K(new Cal3_S2(cal1)); - - Graph graph; - graph.addMeasurement(Point2(1.0, 2.0), model2, x1, l1, K); - graph.addPointConstraint(l1, pt1); - graph.addPointPrior(l1, pt2, model3); - graph.addPoseConstraint(x1, pose1); - graph.addPosePrior(x1, pose2, model6); - - EXPECT(equalsObj(values)); - EXPECT(equalsObj(graph)); - - EXPECT(equalsXML(values)); - EXPECT(equalsXML(graph)); -} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 00fdba011..c1fac2c0b 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -217,7 +217,7 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wra } /* ************************************************************************* */ -vector expandArgumentListsTemplate(const vector& argLists, const string& templateArg, const vector& instName) { +vector expandArgumentListsTemplate(const vector& argLists, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { vector result; BOOST_FOREACH(const ArgumentList& argList, argLists) { ArgumentList instArgList; @@ -226,6 +226,9 @@ vector expandArgumentListsTemplate(const vector& arg if(arg.type == templateArg) { instArg.namespaces.assign(instName.begin(), instName.end()-1); instArg.type = instName.back(); + } else if(arg.type == "This") { + instArg.namespaces.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); + instArg.type = expandedClassName; } instArgList.push_back(instArg); } @@ -236,23 +239,29 @@ vector expandArgumentListsTemplate(const vector& arg /* ************************************************************************* */ template -map expandMethodTemplate(const map& methods, const string& templateArg, const vector& instName) { +map expandMethodTemplate(const map& methods, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { map result; typedef pair Name_Method; BOOST_FOREACH(const Name_Method& name_method, methods) { const METHOD& method = name_method.second; METHOD instMethod = method; - instMethod.argLists = expandArgumentListsTemplate(method.argLists, templateArg, instName); + instMethod.argLists = expandArgumentListsTemplate(method.argLists, templateArg, instName, expandedClassNamespace, expandedClassName); instMethod.returnVals.clear(); BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) { ReturnValue instRetVal = retVal; if(retVal.type1 == templateArg) { instRetVal.namespaces1.assign(instName.begin(), instName.end()-1); instRetVal.type1 = instName.back(); + } else if(retVal.type1 == "This") { + instRetVal.namespaces1.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); + instRetVal.type1 = expandedClassName; } if(retVal.type2 == templateArg) { instRetVal.namespaces2.assign(instName.begin(), instName.end()-1); instRetVal.type2 = instName.back(); + } else if(retVal.type1 == "This") { + instRetVal.namespaces2.assign(expandedClassNamespace.begin(), expandedClassNamespace.end()); + instRetVal.type2 = expandedClassName; } instMethod.returnVals.push_back(instRetVal); } @@ -262,18 +271,18 @@ map expandMethodTemplate(const map& methods, con } /* ************************************************************************* */ -Class expandClassTemplate(const Class& cls, const string& templateArg, const vector& instName) { +Class expandClassTemplate(const Class& cls, const string& templateArg, const vector& instName, const std::vector& expandedClassNamespace, const string& expandedClassName) { Class inst; inst.name = cls.name; inst.templateArgs = cls.templateArgs; inst.typedefName = cls.typedefName; inst.isVirtual = cls.isVirtual; inst.qualifiedParent = cls.qualifiedParent; - inst.methods = expandMethodTemplate(cls.methods, templateArg, instName); - inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg, instName); + inst.methods = expandMethodTemplate(cls.methods, templateArg, instName, expandedClassNamespace, expandedClassName); + inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg, instName, expandedClassNamespace, expandedClassName); inst.namespaces = cls.namespaces; inst.constructor = cls.constructor; - inst.constructor.args_list = expandArgumentListsTemplate(cls.constructor.args_list, templateArg, instName); + inst.constructor.args_list = expandArgumentListsTemplate(cls.constructor.args_list, templateArg, instName, expandedClassNamespace, expandedClassName); inst.constructor.name = inst.name; inst.deconstructor = cls.deconstructor; inst.deconstructor.name = inst.name; @@ -285,8 +294,9 @@ Class expandClassTemplate(const Class& cls, const string& templateArg, const vec vector Class::expandTemplate(const string& templateArg, const vector >& instantiations) const { vector result; BOOST_FOREACH(const vector& instName, instantiations) { - Class inst = expandClassTemplate(*this, templateArg, instName); - inst.name = name + instName.back(); + const string expandedName = name + instName.back(); + Class inst = expandClassTemplate(*this, templateArg, instName, this->namespaces, expandedName); + inst.name = expandedName; inst.templateArgs.clear(); inst.typedefName = qualifiedName("::") + "<" + wrap::qualifiedName("::", instName) + ">"; result.push_back(inst); @@ -295,8 +305,8 @@ vector Class::expandTemplate(const string& templateArg, const vector& instantiation) const { - return expandClassTemplate(*this, templateArg, instantiation); +Class Class::expandTemplate(const string& templateArg, const vector& instantiation, const std::vector& expandedClassNamespace, const string& expandedClassName) const { + return expandClassTemplate(*this, templateArg, instantiation, expandedClassNamespace, expandedClassName); } /* ************************************************************************* */ diff --git a/wrap/Class.h b/wrap/Class.h index 2d10d691a..6b60f8219 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -57,7 +57,7 @@ struct Class { std::string qualifiedName(const std::string& delim = "") const; ///< creates a namespace-qualified name, optional delimiter std::vector expandTemplate(const std::string& templateArg, const std::vector >& instantiations) const; - Class expandTemplate(const std::string& templateArg, const std::vector& instantiation) const; + Class expandTemplate(const std::string& templateArg, const std::vector& instantiation, const std::vector& expandedClassNamespace, const std::string& expandedClassName) const; // The typedef line for this class, if this class is a typedef, otherwise returns an empty string. std::string getTypedef() const; diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 8c564e92f..b2f061838 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -416,6 +416,9 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co vector validTypes = GenerateValidTypes(expandedClasses, forward_declarations); // Check that all classes have been defined somewhere + verifyArguments(validTypes, global_functions); + verifyReturnTypes(validTypes, global_functions); + BOOST_FOREACH(const Class& cls, expandedClasses) { // verify all of the function arguments //TODO:verifyArguments(validTypes, cls.constructor.args_list); @@ -558,13 +561,15 @@ void Module::WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::st const string cppName = cls.qualifiedName("::"); const string collectorType = "Collector_" + matlabUniqueName; const string collectorName = "collector_" + matlabUniqueName; + // The extra curly-braces around the for loops work around a limitation in MSVC (existing + // since 2005!) preventing more than 248 blocks. wrapperFile.oss << - " for(" << collectorType << "::iterator iter = " << collectorName << ".begin();\n" + " { for(" << collectorType << "::iterator iter = " << collectorName << ".begin();\n" " iter != " << collectorName << ".end(); ) {\n" " delete *iter;\n" " " << collectorName << ".erase(iter++);\n" " anyDeleted = true;\n" - " }\n"; + " } }\n"; } wrapperFile.oss << " if(anyDeleted)\n" diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 3def66b0f..61b54434c 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -131,8 +131,6 @@ string StaticMethod::wrapper_fragment(FileWriter& file, // unwrap arguments, see Argument.cpp args.matlab_unwrap(file,0); // We start at 0 because there is no self object - file.oss << " "; - // call method with default type and wrap result if (returnVal.type1!="void") returnVal.wrap_result(cppClassName+"::"+name+"("+args.names()+")", file, typeAttributes); diff --git a/wrap/TemplateInstantiationTypedef.cpp b/wrap/TemplateInstantiationTypedef.cpp index da1e8f616..b822f732b 100644 --- a/wrap/TemplateInstantiationTypedef.cpp +++ b/wrap/TemplateInstantiationTypedef.cpp @@ -42,7 +42,7 @@ namespace wrap { // Instantiate it Class classInst = *clsIt; for(size_t i = 0; i < typeList.size(); ++i) - classInst = classInst.expandTemplate(classInst.templateArgs[i], typeList[i]); + classInst = classInst.expandTemplate(classInst.templateArgs[i], typeList[i], namespaces, name); // Fix class properties classInst.name = name; diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index d18ad2ec1..b546aa65f 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -19,24 +19,24 @@ void _deleteAllObjects() std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - for(Collector_Point2::iterator iter = collector_Point2.begin(); + { for(Collector_Point2::iterator iter = collector_Point2.begin(); iter != collector_Point2.end(); ) { delete *iter; collector_Point2.erase(iter++); anyDeleted = true; - } - for(Collector_Point3::iterator iter = collector_Point3.begin(); + } } + { for(Collector_Point3::iterator iter = collector_Point3.begin(); iter != collector_Point3.end(); ) { delete *iter; collector_Point3.erase(iter++); anyDeleted = true; - } - for(Collector_Test::iterator iter = collector_Test.begin(); + } } + { for(Collector_Test::iterator iter = collector_Test.begin(); iter != collector_Test.end(); ) { delete *iter; collector_Test.erase(iter++); anyDeleted = true; - } + } } if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -227,14 +227,14 @@ void Point3_StaticFunctionRet_15(int nargout, mxArray *out[], int nargin, const typedef boost::shared_ptr Shared; checkArguments("Point3.StaticFunctionRet",nargout,nargin,1); double z = unwrap< double >(in[0]); - out[0] = wrap_shared_ptr(SharedPoint3(new Point3(Point3::StaticFunctionRet(z))),"Point3", false); + out[0] = wrap_shared_ptr(SharedPoint3(new Point3(Point3::StaticFunctionRet(z))),"Point3", false); } void Point3_staticFunction_16(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr Shared; checkArguments("Point3.staticFunction",nargout,nargin,0); - out[0] = wrap< double >(Point3::staticFunction()); + out[0] = wrap< double >(Point3::staticFunction()); } void Test_collectorInsertAndMakeBase_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) diff --git a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp index c75c6a388..d199e59ab 100644 --- a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp +++ b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp @@ -29,42 +29,42 @@ void _deleteAllObjects() std::streambuf *outbuf = std::cout.rdbuf(&mout); bool anyDeleted = false; - for(Collector_ns1ClassA::iterator iter = collector_ns1ClassA.begin(); + { for(Collector_ns1ClassA::iterator iter = collector_ns1ClassA.begin(); iter != collector_ns1ClassA.end(); ) { delete *iter; collector_ns1ClassA.erase(iter++); anyDeleted = true; - } - for(Collector_ns1ClassB::iterator iter = collector_ns1ClassB.begin(); + } } + { for(Collector_ns1ClassB::iterator iter = collector_ns1ClassB.begin(); iter != collector_ns1ClassB.end(); ) { delete *iter; collector_ns1ClassB.erase(iter++); anyDeleted = true; - } - for(Collector_ns2ClassA::iterator iter = collector_ns2ClassA.begin(); + } } + { for(Collector_ns2ClassA::iterator iter = collector_ns2ClassA.begin(); iter != collector_ns2ClassA.end(); ) { delete *iter; collector_ns2ClassA.erase(iter++); anyDeleted = true; - } - for(Collector_ns2ns3ClassB::iterator iter = collector_ns2ns3ClassB.begin(); + } } + { for(Collector_ns2ns3ClassB::iterator iter = collector_ns2ns3ClassB.begin(); iter != collector_ns2ns3ClassB.end(); ) { delete *iter; collector_ns2ns3ClassB.erase(iter++); anyDeleted = true; - } - for(Collector_ns2ClassC::iterator iter = collector_ns2ClassC.begin(); + } } + { for(Collector_ns2ClassC::iterator iter = collector_ns2ClassC.begin(); iter != collector_ns2ClassC.end(); ) { delete *iter; collector_ns2ClassC.erase(iter++); anyDeleted = true; - } - for(Collector_ClassD::iterator iter = collector_ClassD.begin(); + } } + { for(Collector_ClassD::iterator iter = collector_ClassD.begin(); iter != collector_ClassD.end(); ) { delete *iter; collector_ClassD.erase(iter++); anyDeleted = true; - } + } } if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -230,7 +230,7 @@ void ns2ClassA_afunction_12(int nargout, mxArray *out[], int nargin, const mxArr { typedef boost::shared_ptr Shared; checkArguments("ns2ClassA.afunction",nargout,nargin,0); - out[0] = wrap< double >(ns2::ClassA::afunction()); + out[0] = wrap< double >(ns2::ClassA::afunction()); } void ns2ns3ClassB_collectorInsertAndMakeBase_13(int nargout, mxArray *out[], int nargin, const mxArray *in[])