12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged from branch 'branches/remove_slam_namespaces' into 'trunk'

release/4.3a0
Richard Roberts 2012-08-04 16:46:22 +00:00
parent a9864dab23 6f0c0fc7c5
commit f080d34dc0
140 changed files with 3138 additions and 4954 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

185
examples/LocalizationExample.cpp
View File

@ -15,87 +15,166 @@
* @author Frank Dellaert
*/
// pull in the 2D PoseSLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/pose2SLAM.h>
/**
* 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 <gtsam/geometry/Pose2.h>
// 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 <gtsam/nonlinear/Key.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 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 <gtsam/slam/BetweenFactor.h>
// 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 <gtsam/nonlinear/NonlinearFactorGraph.h>
// 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 <gtsam/nonlinear/Values.h>
// 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 <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
// Once the optimized values have been calculated, we can also calculate the marginal covariance
// of desired variables
#include <gtsam/nonlinear/Marginals.h>
#include <iomanip>
#include <iostream>
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 <gtsam/nonlinear/NonlinearFactor.h>
class UnaryFactor: public NoiseModelFactor1<Pose2> {
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<UnaryFactor> 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<Pose2>(model, j), mx_(x), my_(y) {}
NoiseModelFactor1<Pose2>(model, j), measurement_(x, y) {}
virtual ~UnaryFactor() {}
Vector evaluateError(const Pose2& q,
boost::optional<Matrix&> 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<Matrix&> 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>(
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<const UnaryFactor*> (&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<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.add(BetweenFactor<Pose2>(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<UnaryFactor>(1, 0, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(2, 2, 0, noiseModel));
graph.push_back(boost::make_shared<UnaryFactor>(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 ");
// use an explicit Optimizer object
// 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);
pose2SLAM::Values result = optimizer.optimize();
result.print("\nFinal result:\n ");
Values result = optimizer.optimize();
result.print("Final Result:\n");
// Query the marginals
// 5. Calculate and print marginal covariances for all variables
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;
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;
return 0;
}

115
examples/OdometryExample.cpp
View File

@ -15,62 +15,91 @@
* @author Frank Dellaert
*/
// pull in the 2D PoseSLAM domain with all typedefs and helper functions defined
#include <gtsam/slam/pose2SLAM.h>
// include this for marginals
#include <gtsam/nonlinear/Marginals.h>
#include <iomanip>
#include <iostream>
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 <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
// 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 <gtsam/nonlinear/Symbol.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 Between factors for the relative motion described by odometry measurements.
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// 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 <gtsam/nonlinear/NonlinearFactorGraph.h>
// 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 <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
// Once the optimized values have been calculated, we can also calculate the marginal covariance
// of desired variables
#include <gtsam/nonlinear/Marginals.h>
// 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 <gtsam/nonlinear/Values.h>
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<Pose2>(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<Pose2>(Symbol('x', 1), Symbol('x', 2), Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.add(BetweenFactor<Pose2>(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 ");
// 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;
// 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;
return 0;
}

138
examples/PlanarSLAMExample.cpp
View File

@ -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 <gtsam/slam/planarSLAM.h>
// we will use Symbol keys
#include <gtsam/nonlinear/Symbol.h>
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 <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
// 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 <gtsam/nonlinear/Symbol.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 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 <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
// 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 <gtsam/nonlinear/NonlinearFactorGraph.h>
// 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 <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
// Once the optimized values have been calculated, we can also calculate the marginal covariance
// of desired variables
#include <gtsam/nonlinear/Marginals.h>
// 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 <gtsam/nonlinear/Values.h>
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<Pose2>(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<Pose2>(x1, x2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(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<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(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;
}

137
examples/PlanarSLAMExample_selfcontained.cpp
View File

@ -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 <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
// for all nonlinear keys
#include <gtsam/nonlinear/Symbol.h>
// implementations for structures - needed if self-contained, and these should be included last
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
// for modeling measurement uncertainty - all models included here
#include <gtsam/linear/NoiseModel.h>
// for points and poses
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <cmath>
#include <iostream>
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<Pose2> 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<Pose2> odom12(i1, i2, odometry, odometryNoise);
BetweenFactor<Pose2> 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<Pose2, Point2> meas11(i1, j1, bearing11, range11, measurementNoise);
BearingRangeFactor<Pose2, Point2> meas21(i2, j1, bearing21, range21, measurementNoise);
BearingRangeFactor<Pose2, Point2> 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;
}

133
examples/Pose2SLAMExample.cpp
View File

@ -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 <gtsam/slam/pose2SLAM.h>
#include <cmath>
/**
* 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 <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
// 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 <gtsam/nonlinear/Key.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 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 <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// 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 <gtsam/nonlinear/NonlinearFactorGraph.h>
// 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 <gtsam/nonlinear/GaussNewtonOptimizer.h>
// Once the optimized values have been calculated, we can also calculate the marginal covariance
// of desired variables
#include <gtsam/nonlinear/Marginals.h>
// 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 <gtsam/nonlinear/Values.h>
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<Pose2>(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<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(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<Pose2>(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;
}

82
examples/Pose2SLAMExample_advanced.cpp
View File

@ -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 <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <boost/shared_ptr.hpp>
#include <cmath>
#include <iostream>
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;
}

19
examples/Pose2SLAMExample_graph.cpp
View File

@ -17,32 +17,35 @@
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Pose2.h>
#include <boost/tuple/tuple.hpp>
#include <cmath>
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<Pose2>(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

128
examples/Pose2SLAMwSPCG.cpp
View File

@ -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 <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
// 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 <gtsam/nonlinear/Key.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 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 <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// 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 <gtsam/nonlinear/NonlinearFactorGraph.h>
// 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 <gtsam/nonlinear/Values.h>
// ???
#include <gtsam/linear/SimpleSPCGSolver.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/pose2SLAM.h>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
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<Pose2>(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<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(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<Pose2>(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<SimpleSPCGSolverParameters>();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, param);
parameters.iterativeParams = boost::make_shared<SimpleSPCGSolverParameters>();
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<SubgraphSolverParameters>();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, param);
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
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;
}

54
examples/SimpleRotation.cpp
View File

@ -17,20 +17,41 @@
* @author Alex Cunningham
*/
#include <cmath>
#include <iostream>
#include <gtsam/slam/PriorFactor.h>
/**
* 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 <gtsam/geometry/Rot2.h>
#include <gtsam/linear/NoiseModel.h>
// 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 <gtsam/nonlinear/Symbol.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.
// We will apply a simple prior on the rotation
#include <gtsam/slam/PriorFactor.h>
// 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 <gtsam/nonlinear/NonlinearFactorGraph.h>
// 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 <gtsam/nonlinear/Values.h>
// 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 <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
/*
* 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<Rot2> 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

159
examples/VisualISAM2Example.cpp Normal file
View File

@ -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 <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/SimpleCamera.h>
// 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 <gtsam/nonlinear/Symbol.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 <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
// We want to use iSAM2 to solve the structure-from-motion problem incrementally, so
// include iSAM2 here
#include <gtsam/nonlinear/ISAM2.h>
// 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 <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <vector>
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<gtsam::Point3> 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<gtsam::Pose3> 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<Pose3, Point3, Cal3_S2>(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<Pose3>(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<Point3>(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;
}
/* ************************************************************************* */

168
examples/VisualISAMExample.cpp
View File

@ -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 <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/SimpleCamera.h>
// 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 <gtsam/nonlinear/Symbol.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 <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
// We want to use iSAM to solve the structure-from-motion problem incrementally, so
// include iSAM here
#include <gtsam/nonlinear/NonlinearISAM.h>
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/BetweenFactor.h>
#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 <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <vector>
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<gtsam::Point3> 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<gtsam::Pose3> 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;
// First pose with prior factor
newFactors.addPosePrior(X(0), data.poses[0], data.noiseX);
// Second pose with odometry measurement
newFactors.addRelativePose(X(0), X(1), data.odometry, data.noiseX);
// Visual measurements at both poses
for (size_t i=0; i<2; ++i) {
for (size_t j=0; j<data.z[i].size(); ++j) {
newFactors.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK);
}
// 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<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
}
// 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 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))));
// Initial values for the landmarks
for (size_t j=0; j<data.points.size(); ++j)
initials.insert(L(j), data.points[j]);
// 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<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Update ISAM the first time and obtain the current estimate
isam.update(newFactors, initials);
currentEstimate = isam.estimate();
cout << "Frame 0 and 1: " << endl;
currentEstimate.print("Current estimate: ");
}
// Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
// Subsequent frames: Add new odometry and measurement factors and initial values,
// then update ISAM at each frame
for (size_t i=2; i<data.poses.size(); ++i) {
visualSLAM::Graph newFactors;
// Factor for odometry measurements, simulated by adding Gaussian noise to the ground-truth.
Pose3 odoMeasurement = data.odometry;
newFactors.addRelativePose(X(i-1), X(i), data.odometry, data.noiseX);
// Factors for visual measurements
for (size_t j=0; j<data.z[i].size(); ++j) {
newFactors.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK);
}
// 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 estimates for the new node Xi, simulated by Gaussian noises
Values initials;
initials.insert(X(i), currentEstimate.at<Pose3>(X(i-1))*data.odometry);
// update ISAM
isam.update(newFactors, initials);
currentEstimate = isam.estimate();
} else {
// Update iSAM with the new factors
isam.update(graph, initialEstimate);
Values currentEstimate = isam.estimate();
cout << "****************************************************" << endl;
cout << "Frame " << i << ": " << endl;
currentEstimate.print("Current estimate: ");
// Clear the factor graph and values for the next iteration
graph.resize(0);
initialEstimate.clear();
}
}
return 0;
}
/* ************************************************************************* */

89
examples/VisualSLAMData.h
View File

@ -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 <vector>
#include <map>
#include <gtsam/geometry/SimpleCamera.h>
/* ************************************************************************* */
/**
* 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<gtsam::Pose3> poses; // ground-truth camera poses
gtsam::Pose3 odometry; // ground-truth odometry between 2 consecutive poses (simulated data for iSAM)
std::vector<gtsam::Point3> points; // ground-truth landmarks
std::map<int, std::vector<gtsam::Point2> > 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; i<n; ++i, theta += 2*M_PI/n) {
gtsam::Point3 C = gtsam::Point3(r*cos(theta), r*sin(theta), 0.0);
gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(C, gtsam::Point3(), gtsam::Point3(0,0,1));
data.poses.push_back(camera.pose());
}
data.odometry = data.poses[0].between(data.poses[1]);
// Simulated measurements, possibly with Gaussian noise
data.noiseZ = gtsam::noiseModel::Isotropic::Sigma(2, 1.0);
for (size_t i=0; i<data.poses.size(); ++i) {
for (size_t j=0; j<data.points.size(); ++j) {
gtsam::SimpleCamera camera(data.poses[i], *data.sK);
data.z[i].push_back(camera.project(data.points[j])
/*+ gtsam::Point2(data.noiseZ->sample()))*/); // 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;
}
};

134
examples/VisualSLAMExample.cpp
View File

@ -15,49 +15,123 @@
* @author Duy-Nguyen Ta
*/
#include <gtsam/slam/visualSLAM.h>
/**
* 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 <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/SimpleCamera.h>
// 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 <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#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 <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
// 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 <gtsam/nonlinear/NonlinearFactorGraph.h>
// 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 <gtsam/nonlinear/DoglegOptimizer.h>
// 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 <gtsam/nonlinear/Values.h>
#include <vector>
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<data.poses.size(); ++i) {
for (size_t j=0; j<data.points.size(); ++j)
graph.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK);
// Create the set of ground-truth landmarks
std::vector<gtsam::Point3> 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<gtsam::Pose3> 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; i<data.poses.size(); ++i)
initial.insert(X(i), data.poses[i]/* *Pose3::Expmap(data.noiseX->sample())*/); // you can add noise if you want
for (size_t j=0; j<data.points.size(); ++j)
initial.insert(L(j), data.points[j] /*+ Point3(data.noiseL->sample())*/); // 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<Pose3>(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<Pose3, Point3, Cal3_S2>(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<Point3>(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;
}

2
examples/easyPoint2KalmanFilter.cpp
View File

@ -21,7 +21,7 @@
* @author Stephen Williams
*/
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
#include <gtsam/nonlinear/ExtendedKalmanFilter.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>

547
gtsam.h
View File

@ -66,6 +66,9 @@
* or with typedefs, e.g.
* template<T, U> class Class2 { ... };
* typedef Class2<Type1, Type2> 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<CALIBRATION = {gtsam::Cal3DS2}>
//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<gtsam::Point2,bool> 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 <gtsam/nonlinear/Symbol.h>
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 <gtsam/nonlinear/Ordering.h>
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<Key>
@ -1137,6 +1173,7 @@ class KeyVector {
void push_back(size_t key) const;
};
#include <gtsam/nonlinear/Marginals.h>
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 <gtsam/nonlinear/GaussNewtonOptimizer.h>
virtual class GaussNewtonParams : gtsam::SuccessiveLinearizationParams {
GaussNewtonParams();
};
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
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 <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h>
template<T = {gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera}>
virtual class PriorFactor : gtsam::NonlinearFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
};
#include <gtsam/slam/BetweenFactor.h>
template<T = {gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
virtual class BetweenFactor : gtsam::NonlinearFactor {
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
};
#include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera}>
virtual class NonlinearEquality : gtsam::NonlinearFactor {
// Constructor - forces exact evaluation
@ -1386,6 +1449,7 @@ virtual class NonlinearEquality : gtsam::NonlinearFactor {
};
#include <gtsam/slam/RangeFactor.h>
template<POSE, POINT>
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<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimple
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
template<POSE, POINT, ROT>
#include <gtsam/slam/BearingFactor.h>
template<POSE, POINT, ROTATION>
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<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
#include <gtsam/slam/BearingRangeFactor.h>
template<POSE, POINT, ROTATION>
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<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingRangeFactor2D;
#include <gtsam/slam/ProjectionFactor.h>
template<POSE, LANDMARK, CALIBRATION>
virtual class GenericProjectionFactor : gtsam::NonlinearFactor {
@ -1417,415 +1492,65 @@ virtual class GenericProjectionFactor : gtsam::NonlinearFactor {
CALIBRATION* calibration() const;
};
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> GenericProjectionFactorCal3_S2;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
// FIXME: Add Cal3DS2 when it has a 'calibrate' function
//typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
#include <gtsam/slam/GeneralSFMFactor.h>
template<CAMERA, LANDMARK>
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<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
// FIXME: Add Cal3DS2 when it has a 'calibrate' function
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
// FIXME: Add Cal3DS2 when it has a 'calibrate' function
template<CALIBRATION = {gtsam::Cal3_S2}>
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 <gtsam/slam/StereoFactor.h>
template<POSE, LANDMARK>
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<gtsam::Pose3, gtsam::Point3> GenericStereoFactor3D;
#include <gtsam/slam/dataset.h>
pair<pose2SLAM::Graph*, pose2SLAM::Values*> load2D(string filename,
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model);
} //\namespace gtsam
//*************************************************************************
// Pose2SLAM
// Utilities
//*************************************************************************
namespace pose2SLAM {
namespace utilities {
#include <gtsam/slam/pose2SLAM.h>
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 <matlab.h>
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 <gtsam/slam/pose2SLAM.h>
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 <gtsam/slam/pose3SLAM.h>
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 <gtsam/slam/pose3SLAM.h>
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 <gtsam/slam/planarSLAM.h>
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 <gtsam/slam/planarSLAM.h>
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 <gtsam/slam/planarSLAM.h>
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 <gtsam/slam/visualSLAM.h>
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 <gtsam/slam/visualSLAM.h>
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 <gtsam/slam/visualSLAM.h>
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 <gtsam/slam/visualSLAM.h>
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 <gtsam/slam/sparseBA.h>
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 <gtsam/slam/sparseBA.h>
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 <gtsam/slam/sparseBA.h>
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
}

14
gtsam/base/LieMatrix.h
View File

@ -22,6 +22,7 @@
#include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/DerivedValue.h>
#include <boost/serialization/nvp.hpp>
namespace gtsam {
@ -135,5 +136,18 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
static inline Vector Logmap(const LieMatrix& p) {
return Eigen::Map<const Vector>(&p(0,0), p.dim()); }
private:
// Serialization function
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("LieMatrix",
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Matrix",
boost::serialization::base_object<Matrix>(*this));
}
};
} // \namespace gtsam

12
gtsam/base/LieVector.h
View File

@ -119,5 +119,17 @@ struct LieVector : public Vector, public DerivedValue<LieVector> {
/** 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<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("LieVector",
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Vector",
boost::serialization::base_object<Vector>(*this));
}
};
} // \namespace gtsam

2
gtsam/geometry/StereoCamera.h
View File

@ -141,6 +141,8 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("StereoCamera",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
ar & BOOST_SERIALIZATION_NVP(K_);
}

2
gtsam/inference/graph.h
View File

@ -108,3 +108,5 @@ namespace gtsam {
} // namespace gtsam
#include <gtsam/inference/graph-inl.h>

1
gtsam/nonlinear/DoglegOptimizer.h
View File

@ -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;
};

2
gtsam/nonlinear/EasyFactorGraph.cpp
View File

@ -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<SimpleSPCGSolverParameters>();
return optimizer(initialEstimate, p).optimizeSafely();
}

2
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -90,3 +90,5 @@ namespace gtsam {
};
} // namespace
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>

1
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -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;
};

1
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -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;
};

4
gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp
View File

@ -34,8 +34,8 @@ void SuccessiveLinearizationParams::print(const std::string& str) const {
case CHOLMOD:
std::cout << " linear solver type: CHOLMOD\n";
break;
case CG:
std::cout << " linear solver type: CG\n";
case CONJUGATE_GRADIENT:
std::cout << " linear solver type: CONJUGATE GRADIENT\n";
break;
default:
std::cout << " linear solver type: (invalid)\n";

43
gtsam/nonlinear/SuccessiveLinearizationOptimizer.h
View File

@ -31,7 +31,7 @@ public:
MULTIFRONTAL_QR,
SEQUENTIAL_CHOLESKY,
SEQUENTIAL_QR,
CG, /* Experimental Flag */
CONJUGATE_GRADIENT, /* Experimental Flag */
CHOLMOD, /* Experimental Flag */
};
@ -43,20 +43,14 @@ public:
virtual ~SuccessiveLinearizationParams() {}
inline bool isMultifrontal() const {
return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR);
}
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 */

9
gtsam/nonlinear/Symbol.h
View File

@ -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); }

4
gtsam/slam/BearingFactor.h
View File

@ -25,12 +25,12 @@ namespace gtsam {
/**
* Binary factor for a bearing measurement
*/
template<class POSE, class POINT, class ROT = typename POSE::Rotation>
template<class POSE, class POINT, class ROTATION = typename POSE::Rotation>
class BearingFactor: public NoiseModelFactor2<POSE, POINT> {
private:
typedef POSE Pose;
typedef ROT Rot;
typedef ROTATION Rot;
typedef POINT Point;
typedef BearingFactor<POSE, POINT> This;

4
gtsam/slam/BearingRangeFactor.h
View File

@ -27,12 +27,12 @@ namespace gtsam {
/**
* Binary factor for a bearing measurement
*/
template<class POSE, class POINT>
template<class POSE, class POINT, class ROTATION = typename POSE::Rotation>
class BearingRangeFactor: public NoiseModelFactor2<POSE, POINT> {
private:
typedef POSE Pose;
typedef typename POSE::Rotation Rot;
typedef ROTATION Rot;
typedef POINT Point;
typedef BearingRangeFactor<POSE, POINT> This;

5
gtsam/slam/GeneralSFMFactor.h
View File

@ -111,6 +111,8 @@ namespace gtsam {
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*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<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};

5
gtsam/slam/StereoFactor.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/StereoCamera.h>
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<const GenericStereoFactor*> (&f);
return p && Base::equals(f) && measured_.equals(p->measured_, tol);
}
@ -102,6 +103,8 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(K_);
}

68
gtsam/slam/dataset.cpp
View File

@ -20,8 +20,10 @@
#include <sstream>
#include <cstdlib>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
using namespace std;
@ -30,71 +32,23 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset,
const string& user_path) {
string path = user_path, set = dataset;
boost::optional<SharedDiagonal> null_model;
boost::optional<SharedDiagonal> identity(noiseModel::Unit::Create(3));
boost::optional<SharedDiagonal> 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<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
pair<string, boost::optional<SharedDiagonal> > dataset,
pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
pair<string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset,
int maxID, bool addNoise, bool smart) {
return load2D(dataset.first, dataset.second, maxID, addNoise, smart);
}
/* ************************************************************************* */
pair<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
const string& filename, boost::optional<SharedDiagonal> model, int maxID,
pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
const string& filename, boost::optional<noiseModel::Diagonal::shared_ptr> 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<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
if (!poses->exists(id2))
poses->insert(id2, poses->at<Pose2>(id1) * l1Xl2);
pose2SLAM::Graph::sharedFactor factor(
NonlinearFactor::shared_ptr factor(
new BetweenFactor<Pose2>(id1, id2, l1Xl2, *model));
graph->push_back(factor);
}
@ -169,8 +123,8 @@ pair<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> 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);

25
gtsam/slam/dataset.h
View File

@ -18,19 +18,12 @@
#pragma once
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
#include <string>
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<std::string, boost::optional<gtsam::SharedDiagonal> >
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<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
std::pair<std::string, boost::optional<SharedDiagonal> > dataset,
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
std::pair<std::string, boost::optional<noiseModel::Diagonal::shared_ptr> > 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<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
const std::string& filename,
boost::optional<gtsam::SharedDiagonal> 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

63
gtsam/slam/planarSLAM.cpp
View File

@ -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 <gtsam/slam/planarSLAM.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
// Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM {
/* ************************************************************************* */
Matrix Values::points() const {
size_t j=0;
ConstFiltered<Point2> points = filter<Point2>();
Matrix result(points.size(),2);
BOOST_FOREACH(const ConstFiltered<Point2>::KeyValuePair& keyValue, points)
result.row(j++) = keyValue.value.vector();
return result;
}
/* ************************************************************************* */
void Graph::addPointConstraint(Key pointKey, const Point2& p) {
push_back(boost::make_shared<NonlinearEquality<Point2> >(pointKey, p));
}
/* ************************************************************************* */
void Graph::addPointPrior(Key pointKey, const Point2& p, const SharedNoiseModel& model) {
push_back(boost::make_shared<PriorFactor<Point2> >(pointKey, p, model));
}
/* ************************************************************************* */
void Graph::addBearing(Key i, Key j, const Rot2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<BearingFactor<Pose2, Point2> >(i, j, z, model));
}
/* ************************************************************************* */
void Graph::addRange(Key i, Key j, double z, const SharedNoiseModel& model) {
push_back(boost::make_shared<RangeFactor<Pose2, Point2> >(i, j, z, model));
}
/* ************************************************************************* */
void Graph::addBearingRange(Key i, Key j, const Rot2& z1,
double z2, const SharedNoiseModel& model) {
push_back(boost::make_shared<BearingRangeFactor<Pose2, Point2> >(i, j, z1, z2, model));
}
/* ************************************************************************* */
} // planarSLAM

122
gtsam/slam/planarSLAM.h
View File

@ -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 <gtsam/slam/pose2SLAM.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/BearingFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/geometry/Pose2.h>
// 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<Values> shared_ptr;
typedef gtsam::Values::ConstFiltered<Pose2> PoseFiltered;
typedef gtsam::Values::ConstFiltered<Point2> 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<Pose2>(); } ///< 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<Point2>(); } ///< 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<Point2>(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<Pose2> Constraint; ///< \deprecated typedef for backwards compatibility
typedef gtsam::PriorFactor<Pose2> Prior; ///< \deprecated typedef for backwards compatibility
typedef gtsam::BetweenFactor<Pose2> Odometry; ///< \deprecated typedef for backwards compatibility
typedef gtsam::BearingFactor<Pose2, Point2> Bearing; ///< \deprecated typedef for backwards compatibility
typedef gtsam::RangeFactor<Pose2, Point2> Range; ///< \deprecated typedef for backwards compatibility
typedef gtsam::BearingRangeFactor<Pose2, Point2> BearingRange; ///< \deprecated typedef for backwards compatibility
}

66
gtsam/slam/pose2SLAM.cpp
View File

@ -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 <gtsam/slam/pose2SLAM.h>
// 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<Pose2> poses = filter<Pose2>();
Matrix result(poses.size(),3);
BOOST_FOREACH(const ConstFiltered<Pose2>::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<Pose2>(i, p));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPosePrior(Key i, const Pose2& p, const SharedNoiseModel& model) {
sharedFactor factor(new PriorFactor<Pose2>(i, p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addRelativePose(Key i1, Key i2, const Pose2& z,
const SharedNoiseModel& model) {
sharedFactor factor(new BetweenFactor<Pose2>(i1, i2, z, model));
push_back(factor);
}
/* ************************************************************************* */
} // pose2SLAM

107
gtsam/slam/pose2SLAM.h
View File

@ -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 <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/EasyFactorGraph.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/geometry/Pose2.h>
// 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<Values> 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<Pose2>(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<Graph> 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<Pose2> HardConstraint; ///< \deprecated typedef for backwards compatibility
typedef gtsam::PriorFactor<Pose2> Prior; ///< \deprecated typedef for backwards compatibility
typedef gtsam::BetweenFactor<Pose2> Odometry; ///< \deprecated typedef for backwards compatibility
}

73
gtsam/slam/pose3SLAM.cpp
View File

@ -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 <gtsam/slam/pose3SLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// 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<Pose3> poses = filter<Pose3>();
Matrix result(poses.size(),3);
BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
result.row(j++) = keyValue.value.translation().vector();
return result;
}
/* ************************************************************************* */
void Graph::addPoseConstraint(Key i, const Pose3& p) {
sharedFactor factor(new NonlinearEquality<Pose3>(i, p));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPosePrior(Key i, const Pose3& p, const SharedNoiseModel& model) {
sharedFactor factor(new PriorFactor<Pose3>(i, p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addRelativePose(Key i1, Key i2, const Pose3& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<BetweenFactor<Pose3> >(i1, i2, z, model));
}
/* ************************************************************************* */
} // pose3SLAM

120
gtsam/slam/pose3SLAM.h
View File

@ -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 <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/EasyFactorGraph.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/geometry/Pose3.h>
/// 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<Values> 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<Pose3>(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<Pose3> Prior; ///< \deprecated typedef for backwards compatibility
typedef gtsam::BetweenFactor<Pose3> Constraint; ///< \deprecated typedef for backwards compatibility
typedef gtsam::NonlinearEquality<Pose3> 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
}

60
gtsam/slam/sparseBA.cpp
View File

@ -1,60 +0,0 @@
/**
* @file sparseBA.cpp
* @brief
* @date Jul 6, 2012
* @author Yong-Dian Jian
*/
#include <gtsam/slam/sparseBA.h>
namespace sparseBA {
/* ************************************************************************* */
void Graph::addSimpleCameraConstraint(Key cameraKey, const SimpleCamera &camera) {
addCameraConstraint<SimpleCamera>(cameraKey, camera);
}
/* ************************************************************************* */
void Graph::addSimpleCameraPrior(Key cameraKey, const SimpleCamera &camera, SharedNoiseModel &model) {
addCameraPrior<SimpleCamera>(cameraKey, camera, model);
}
/* ************************************************************************* */
void Graph::addSimpleCameraMeasurement(const Point2 &z, SharedNoiseModel& model, Index cameraKey, Index pointKey) {
addMeasurement<SimpleCamera>(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<SimpleCamera, Point3> SFMFactor;
typedef GeneralSFMFactor2<Cal3_S2> SFMFactor2;
// first count
size_t K = 0, k=0;
BOOST_FOREACH(const sharedFactor& f, *this)
if (boost::dynamic_pointer_cast<const SFMFactor>(f)) ++K;
else if (boost::dynamic_pointer_cast<const SFMFactor2>(f)) ++K;
// now fill
Matrix errors(2,K);
BOOST_FOREACH(const sharedFactor& f, *this) {
boost::shared_ptr<const SFMFactor> p = boost::dynamic_pointer_cast<const SFMFactor>(f);
if (p) {
errors.col(k++) = p->unwhitenedError(values);
continue;
}
boost::shared_ptr<const SFMFactor2> p2 = boost::dynamic_pointer_cast<const SFMFactor2>(f);
if (p2) {
errors.col(k++) = p2->unwhitenedError(values);
}
}
return errors;
}
/* ************************************************************************* */
}

143
gtsam/slam/sparseBA.h
View File

@ -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 <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/SimpleCamera.h>
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<Values> shared_ptr;
typedef gtsam::Values::ConstFiltered<SimpleCamera> SimpleCameraFiltered;
typedef gtsam::Values::ConstFiltered<Cal3_S2> 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<SimpleCamera>(); } ///< 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<SimpleCamera>(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 <typename Camera>
void addCameraPrior(Key cameraKey, const Camera &camera, SharedNoiseModel &model = noiseModel::Unit::Create(Camera::Dim())) {
sharedFactor factor(new PriorFactor<Camera>(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 <typename Camera>
void addCameraConstraint(Key cameraKey, const Camera &camera) {
sharedFactor factor(new NonlinearEquality<Camera>(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 <typename Camera>
void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index cameraKey, const Index pointKey) {
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
boost::shared_ptr<SFMFactor> 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 <typename Calib>
void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index posekey, const Index pointkey, const Index calibkey) {
typedef GeneralSFMFactor2<Calib> SFMFactor;
boost::shared_ptr<SFMFactor> 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);
};
}

14
gtsam/slam/tests/testAntiFactor.cpp
View File

@ -17,10 +17,12 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/pose3SLAM.h>
#include <gtsam/slam/AntiFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/geometry/Pose3.h>
@ -95,12 +97,12 @@ TEST( AntiFactor, EquivalentBayesNet)
Pose3 z(Rot3(), Point3(1, 1, 1));
SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim()));
boost::shared_ptr<pose3SLAM::Graph> 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<Pose3>(1, pose1, sigma));
graph->add(BetweenFactor<Pose3>(1, 2, pose1.between(pose2), sigma));
// Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values());
Values::shared_ptr values(new Values());
values->insert(1, pose1);
values->insert(2, pose2);

218
gtsam/slam/tests/testPlanarSLAM.cpp
View File

@ -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 <gtsam/slam/planarSLAM.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/base/TestableAssertions.h>
#include <iostream>
#include <CppUnitLite/TestHarness.h>
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<Pose2> 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<Pose2, Point2> 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<Pose2, Point2>
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<Pose2, Point2> 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<Pose2, Point2> 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<Pose2, Point2> 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<Pose2, Point2>
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<Pose2,Pose2> PoseBearingRange;
PoseBearingRange actual(2, 3, Rot2::fromDegrees(60.0), 12.3, sigma2);
}
/* ************************************************************************* */
TEST( planarSLAM, PoseConstraint_equals )
{
NonlinearEquality<Pose2> 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<NoiseModelFactor>(G[0])->unwhitenedError(c)));
EXPECT(assert_equal(expected1, boost::dynamic_pointer_cast<NoiseModelFactor>(G[1])->unwhitenedError(c)));
EXPECT(assert_equal(expected2, boost::dynamic_pointer_cast<NoiseModelFactor>(G[2])->unwhitenedError(c)));
EXPECT(assert_equal(expected3, boost::dynamic_pointer_cast<NoiseModelFactor>(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<Key> expected, actual;
expected += j3, i2, i3;
actual = c.keys();
CHECK(expected == actual);
}
{
FastList<Key> expected, actual;
expected += i2, i3;
actual = c.poseKeys();
CHECK(expected == actual);
}
{
FastList<Key> expected, actual;
expected += j3;
actual = c.pointKeys();
CHECK(expected == actual);
}
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

608
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -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 <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/base/numericalDerivative.h>
using namespace gtsam;
#include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp>
using namespace boost;
using namespace boost::assign;
#include <iostream>
using namespace std;
typedef BetweenFactor<Pose2> 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<FactorGraph<GaussianFactor> > lfg_linearized = graph.linearize(config, ordering);
//lfg_linearized->print("lfg_actual");
// the expected linear factor
FactorGraph<GaussianFactor> 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<Pose2>(5).between(actual.at<Pose2>(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<pose2SLAM::pose2SLAM::PoseKey> tree =
// G.findMinimumSpanningTree<pose2SLAM::pose2SLAM::PoseKey, Pose2Factor>();
// 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<pose2SLAM::pose2SLAM::PoseKey> tree;
// tree.insert(1,2);
// tree.insert(2,2);
// tree.insert(3,2);
//
// G.split<pose2SLAM::pose2SLAM::PoseKey, Pose2Factor>(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<Pose2> factor(1, p1, sigmas);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
boost::shared_ptr<JacobianFactor> linear =
boost::dynamic_pointer_cast<JacobianFactor>(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<Pose2> 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<JacobianFactor> actual =
boost::dynamic_pointer_cast<JacobianFactor>(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<JacobianFactor> linear =
boost::dynamic_pointer_cast<JacobianFactor>(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<JacobianFactor> linear =
boost::dynamic_pointer_cast<JacobianFactor>(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<JacobianFactor> actual =
boost::dynamic_pointer_cast<JacobianFactor>(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);
}
/* ************************************************************************* */

211
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -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 <gtsam/slam/pose3SLAM.h>
#include <gtsam/slam/PartialPriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost;
using namespace boost::assign;
#include <iostream>
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<Pose3>(0), gT1 = hexagon.at<Pose3>(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<Pose3>(1).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial.insert(2, hexagon.at<Pose3>(2).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial.insert(3, hexagon.at<Pose3>(3).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial.insert(4, hexagon.at<Pose3>(4).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial.insert(5, hexagon.at<Pose3>(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<Pose3>(5).between(actual.at<Pose3>(0)),1e-5));
}
/* ************************************************************************* */
TEST(Pose3Graph, partial_prior_height) {
typedef PartialPriorFactor<Pose3> 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<Pose3>(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<Pose3> 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<Pose3> 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<size_t> 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<Pose3>(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);
}
/* ************************************************************************* */

8
gtsam/slam/tests/testProjectionFactor.cpp
View File

@ -16,9 +16,13 @@
* @date Nov 2009
*/
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <CppUnitLite/TestHarness.h>
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<GaussianFactor> expected_lfg;
expected_lfg.push_back(actual);

193
gtsam/slam/tests/testSparseBA.cpp
View File

@ -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 <gtsam/slam/sparseBA.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp>
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<Cal3_S2> 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<Camera>(z11, sigma, C(1), L(1));
g.addMeasurement<Camera>(z12, sigma, C(1), L(2));
g.addMeasurement<Camera>(z13, sigma, C(1), L(3));
g.addMeasurement<Camera>(z14, sigma, C(1), L(4));
g.addMeasurement<Camera>(z21, sigma, C(2), L(1));
g.addMeasurement<Camera>(z22, sigma, C(2), L(2));
g.addMeasurement<Camera>(z23, sigma, C(2), L(3));
g.addMeasurement<Camera>(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<Cal3_S2>(z11, sigma, X(1), L(1), K(1));
g.addMeasurement<Cal3_S2>(z12, sigma, X(1), L(2), K(1));
g.addMeasurement<Cal3_S2>(z13, sigma, X(1), L(3), K(1));
g.addMeasurement<Cal3_S2>(z14, sigma, X(1), L(4), K(1));
g.addMeasurement<Cal3_S2>(z21, sigma, X(2), L(1), K(1));
g.addMeasurement<Cal3_S2>(z22, sigma, X(2), L(2), K(1));
g.addMeasurement<Cal3_S2>(z23, sigma, X(2), L(3), K(1));
g.addMeasurement<Cal3_S2>(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); }
/* ************************************************************************* */

16
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -15,13 +15,16 @@
* @author Chris Beall
*/
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <CppUnitLite/TestHarness.h>
@ -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<Cal3_S2Stereo> 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<Pose3, Point3> MyStereoFactor;
/* ************************************************************************* */
TEST( StereoFactor, singlePoint)
{
//Cal3_S2 K(625, 625, 0, 320, 240, 0.5);
boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));
NonlinearFactorGraph graph;
graph.add(NonlinearEquality<Pose3>(X(1), camera1));
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<Pose3, Point3>(z14, sigma, X(1), L(1), K));
// Create a configuration corresponding to the ground truth
Values values;

344
gtsam/slam/tests/testVisualSLAM.cpp
View File

@ -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 <gtsam/slam/visualSLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Symbol.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp>
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<visualSLAM::Values,GaussianBayesNet> actualJoint = isam.jointPrediction(); // 4*4
// std::pair<visualSLAM::Values,GaussianBayesNet> actualMarginals = isam.individualPredictions(); // 2 times 2*2
// std::pair<visualSLAM::Values,GaussianBayesNet> 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<Key> expected, actual;
expected += L(2), X(1), X(2);
actual = c.keys();
CHECK(expected == actual);
}
{
FastList<Key> expected, actual;
expected += X(1), X(2);
actual = c.poseKeys();
CHECK(expected == actual);
}
{
FastList<Key> 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); }
/* ************************************************************************* */

116
gtsam/slam/visualSLAM.cpp
View File

@ -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 <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/Sampler.h>
#include <boost/make_shared.hpp>
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<Z.cols();k++) {
Point2 p(Z(0,k),Z(1,k));
Point3 P = camera.backproject(p, depth);
insertPoint(J(k), P);
}
}
/* ************************************************************************* */
void Values::perturbPoints(double sigma, int32_t seed) {
ConstFiltered<Point3> points = allPoints();
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma);
Sampler sampler(model, seed);
BOOST_FOREACH(const ConstFiltered<Point3>::KeyValuePair& keyValue, points) {
update(keyValue.key, keyValue.value.retract(sampler.sample()));
}
}
/* ************************************************************************* */
Matrix Values::points() const {
size_t j=0;
ConstFiltered<Point3> points = allPoints();
Matrix result(points.size(),3);
BOOST_FOREACH(const ConstFiltered<Point3>::KeyValuePair& keyValue, points)
result.row(j++) = keyValue.value.vector();
return result;
}
/* ************************************************************************* */
void Graph::addPointConstraint(Key pointKey, const Point3& p) {
push_back(make_shared<NonlinearEquality<Point3> >(pointKey, p));
}
/* ************************************************************************* */
void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) {
push_back(make_shared<PriorFactor<Point3> >(pointKey, p, model));
}
/* ************************************************************************* */
void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrK K) {
push_back(
make_shared<GenericProjectionFactor<Pose3, Point3> >
(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<GenericStereoFactor<Pose3, Point3> >(measured, model, poseKey, pointKey, K));
}
/* ************************************************************************* */
void Graph::addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model) {
push_back(make_shared<gtsam::RangeFactor<Pose3, Point3> >(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<const ProjectionFactor>(f)) ++K;
// now fill
Matrix errors(2,K);
BOOST_FOREACH(const sharedFactor& f, *this) {
boost::shared_ptr<const ProjectionFactor> p =
boost::dynamic_pointer_cast<const ProjectionFactor>(f);
if (p) errors.col(k++) = p->unwhitenedError(values);
}
return errors;
}
/* ************************************************************************* */
}

186
gtsam/slam/visualSLAM.h
View File

@ -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 <gtsam/slam/pose3SLAM.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/NonlinearISAM.h>
#include <gtsam/geometry/SimpleCamera.h>
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<Values> shared_ptr;
typedef gtsam::Values::ConstFiltered<Pose3> PoseFiltered;
typedef gtsam::Values::ConstFiltered<Point3> 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<Pose3>(); } ///< 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<Point3>(); } ///< 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<Point3>(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<Graph> 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<Pose3> PoseConstraint; ///< \deprecated typedef for backwards compatibility
typedef gtsam::NonlinearEquality<Point3> PointConstraint; ///< \deprecated typedef for backwards compatibility
typedef gtsam::PriorFactor<Pose3> PosePrior; ///< \deprecated typedef for backwards compatibility
typedef gtsam::PriorFactor<Point3> PointPrior; ///< \deprecated typedef for backwards compatibility
typedef gtsam::RangeFactor<Pose3, Point3> RangeFactor; ///< \deprecated typedef for backwards compatibility
typedef gtsam::GenericProjectionFactor<Pose3, Point3> ProjectionFactor; ///< \deprecated typedef for backwards compatibility
typedef gtsam::GenericStereoFactor<Pose3, Point3> StereoFactor; ///< \deprecated typedef for backwards compatibility
}

12
gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
View File

@ -45,8 +45,8 @@ TEST( InvDepthFactor, Project2) {
Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> 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<Cal3_S2> 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<Cal3_S2> 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));
}

3
gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp
View File

@ -6,7 +6,8 @@
#include <boost/assign/std/vector.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>

6
gtsam_unstable/slam/tests/testPosePriors.cpp
View File

@ -9,12 +9,12 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam_unstable/slam/PoseRotationPrior.h>
#include <gtsam_unstable/slam/PoseTranslationPrior.h>
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/nonlinear/Symbol.h>
using namespace gtsam;

129
matlab.h Normal file
View File

@ -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 <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <exception>
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<Point2> points = values.filter<Point2>();
Matrix result(points.size(),2);
BOOST_FOREACH(const Values::ConstFiltered<Point2>::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<Point3> points = values.filter<Point3>();
Matrix result(points.size(),3);
BOOST_FOREACH(const Values::ConstFiltered<Point3>::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<Pose2> poses = values.filter<Pose2>();
Matrix result(poses.size(),3);
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::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<Pose3> poses = values.filter<Pose3>();
Matrix result(poses.size(),12);
BOOST_FOREACH(const Values::ConstFiltered<Pose3>::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<Point2>::KeyValuePair& key_value, values.filter<Point2>()) {
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<Point3>::KeyValuePair& key_value, values.filter<Point3>()) {
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<Z.cols();k++) {
Point2 p(Z(0,k),Z(1,k));
Point3 P = camera.backproject(p, depth);
values.insert(J(k), P);
}
}
/// calculate the errors of all projection factors in a graph
Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) {
// first count
size_t K = 0, k=0;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph)
if (boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(f)) ++K;
// now fill
Matrix errors(2,K);
BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) {
boost::shared_ptr<const GenericProjectionFactor<Pose3, Point3> > p = boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(f);
if (p) errors.col(k++) = p->unwhitenedError(values);
}
return errors;
}
}
}

0
matlab/CHECK.m → matlab/+gtsam/CHECK.m
View File

0
matlab/EQUALITY.m → matlab/+gtsam/EQUALITY.m
View File

0
matlab/VisualISAMGenerateData.m → matlab/+gtsam/VisualISAMGenerateData.m
View File

44
matlab/VisualISAMInitialize.m → matlab/+gtsam/VisualISAMInitialize.m
View File

@ -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

34
matlab/VisualISAMPlot.m → matlab/+gtsam/VisualISAMPlot.m
View File

@ -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

44
matlab/+gtsam/VisualISAMStep.m Normal file
View File

@ -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;

30
matlab/+gtsam/circlePose2.m Normal file
View File

@ -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

33
matlab/+gtsam/circlePose3.m Normal file
View File

@ -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

0
matlab/covarianceEllipse.m → matlab/+gtsam/covarianceEllipse.m
View File

2
matlab/covarianceEllipse3D.m → matlab/+gtsam/covarianceEllipse3D.m
View File

@ -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);

19
matlab/+gtsam/findExampleDataFile.m Normal file
View File

@ -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

16
matlab/load3D.m → matlab/+gtsam/load3D.m
View File

@ -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 && i1<N || successive && i1==0)
t = gtsam.Point3(v{3}, v{4}, v{5});
R = gtsam.Rot3.Ypr(v{8}, -v{7}, v{6});
initial.insertPose(i1, gtsam.Pose3(R,t));
initial.insert(i1, gtsam.Pose3(R,t));
end
elseif strcmp('EDGE3',line_i(1:5))
e = textscan(line_i,'%s %d %d %f %f %f %f %f %f',1);
@ -41,12 +43,12 @@ for i=1:n
t = gtsam.Point3(e{4}, e{5}, e{6});
R = gtsam.Rot3.Ypr(e{9}, e{8}, e{7});
dpose = gtsam.Pose3(R,t);
graph.addRelativePose(i1, i2, dpose, model);
graph.add(BetweenFactorPose3(i1, i2, dpose, model));
if successive
if i2>i1
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

37
matlab/+gtsam/plot2DPoints.m Normal file
View File

@ -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

54
matlab/+gtsam/plot2DTrajectory.m Normal file
View File

@ -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

36
matlab/+gtsam/plot3DPoints.m Normal file
View File

@ -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

53
matlab/+gtsam/plot3DTrajectory.m Normal file
View File

@ -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

0
matlab/plotCamera.m → matlab/+gtsam/plotCamera.m
View File

10
matlab/+gtsam/plotPoint2.m Normal file
View File

@ -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

13
matlab/+gtsam/plotPoint3.m Normal file
View File

@ -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

2
matlab/plotPose2.m → matlab/+gtsam/plotPose2.m
View File

@ -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

4
matlab/plotPose3.m → matlab/+gtsam/plotPose3.m
View File

@ -6,6 +6,7 @@ if nargin<3,axisLength=0.1;end
gRp = pose.rotation().matrix(); % rotation from pose to global
C = pose.translation().vector();
if ~isempty(axisLength)
% draw the camera axes
xAxis = C+gRp(:,1)*axisLength;
L = [C xAxis]';
@ -18,12 +19,13 @@ 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

13
matlab/CMakeLists.txt
View File

@ -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

44
matlab/VisualISAMStep.m
View File

@ -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

BIN
matlab/VisualISAM_gui.fig
View File

Binary file not shown.

50
matlab/examples/Pose2SLAMExample_circle.m
View File

@ -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'));

48
matlab/examples/Pose2SLAMExample_graph.m
View File

@ -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})

54
matlab/examples/Pose3SLAMExample.m
View File

@ -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'));

32
matlab/examples/Pose3SLAMExample_graph.m
View File

@ -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;

BIN
matlab/examples/gtsamExamples.fig
View File

Binary file not shown.

38
matlab/examples/LocalizationExample.m → matlab/gtsam_examples/LocalizationExample.m
View File

@ -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)

34
matlab/examples/OdometryExample.m → matlab/gtsam_examples/OdometryExample.m
View File

@ -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)

59
matlab/examples/PlanarSLAMExample.m → matlab/gtsam_examples/PlanarSLAMExample.m
View File

@ -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)

66
matlab/examples/PlanarSLAMExample_sampling.m → matlab/gtsam_examples/PlanarSLAMExample_sampling.m
View File

@ -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

45
matlab/examples/Pose2SLAMExample.m → matlab/gtsam_examples/Pose2SLAMExample.m
View File

@ -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)

34
matlab/examples/Pose2SLAMExample_advanced.m → matlab/gtsam_examples/Pose2SLAMExample_advanced.m
View File

@ -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');

57
matlab/gtsam_examples/Pose2SLAMExample_circle.m Normal file
View File

@ -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'));

29
examples/Pose2SLAMExample_graph.m → matlab/gtsam_examples/Pose2SLAMExample_graph.m
View File

@ -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})

29
matlab/examples/Pose2SLAMwSPCG.m → matlab/gtsam_examples/Pose2SLAMwSPCG.m
View File

@ -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'));

60
matlab/gtsam_examples/Pose3SLAMExample.m Normal file
View File

@ -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'));

42
matlab/gtsam_examples/Pose3SLAMExample_graph.m Normal file
View File

@ -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;

19
matlab/examples/SBAExample.m → matlab/gtsam_examples/SBAExample.m
View File

@ -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();

37
matlab/examples/SFMExample.m → matlab/gtsam_examples/SFMExample.m
View File

@ -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')

53
matlab/examples/StereoVOExample.m → matlab/gtsam_examples/StereoVOExample.m
View File

@ -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);

52
matlab/examples/StereoVOExample_large.m → matlab/gtsam_examples/StereoVOExample_large.m
View File

@ -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]);

0
matlab/examples/VisualISAMDemo.m → matlab/gtsam_examples/VisualISAMDemo.m
View File

Some files were not shown because too many files have changed in this diff Show More