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

All compiled! Only SPCG and linear/SubgraphSolver are not fixed.

release/4.3a0
Duy-Nguyen Ta 2012-01-29 22:10:35 +00:00
parent 5b5bbfdfff
commit 2db224df3c
22 changed files with 113 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
examples/CameraResectioning.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization.h> #include <gtsam/nonlinear/NonlinearOptimization.h>
@ -28,13 +27,12 @@
using namespace gtsam; using namespace gtsam;
typedef TypedSymbol<Pose3, 'x'> PoseKey; typedef TypedSymbol<Pose3, 'x'> PoseKey;
typedef Values<PoseKey> PoseValues;
/** /**
* Unary factor for the pose. * Unary factor for the pose.
*/ */
class ResectioningFactor: public NonlinearFactor1<PoseValues, PoseKey> { class ResectioningFactor: public NonlinearFactor1<PoseKey> {
typedef NonlinearFactor1<PoseValues, PoseKey> Base; typedef NonlinearFactor1<PoseKey> Base;
shared_ptrK K_; // camera's intrinsic parameters shared_ptrK K_; // camera's intrinsic parameters
Point3 P_; // 3D point on the calibration rig Point3 P_; // 3D point on the calibration rig
@ -46,6 +44,8 @@ public:
Base(model, key), K_(calib), P_(P), p_(p) { Base(model, key), K_(calib), P_(P), p_(p) {
} }
virtual ~ResectioningFactor() {}
virtual Vector evaluateError(const Pose3& X, boost::optional<Matrix&> H = virtual Vector evaluateError(const Pose3& X, boost::optional<Matrix&> H =
boost::none) const { boost::none) const {
SimpleCamera camera(*K_, X); SimpleCamera camera(*K_, X);
@ -72,7 +72,7 @@ int main(int argc, char* argv[]) {
PoseKey X(1); PoseKey X(1);
/* 1. create graph */ /* 1. create graph */
NonlinearFactorGraph<PoseValues> graph; NonlinearFactorGraph graph;
/* 2. add factors to the graph */ /* 2. add factors to the graph */
// add measurement factors // add measurement factors
@ -92,14 +92,13 @@ int main(int argc, char* argv[]) {
graph.push_back(factor); graph.push_back(factor);
/* 3. Create an initial estimate for the camera pose */ /* 3. Create an initial estimate for the camera pose */
PoseValues initial; DynamicValues initial;
initial.insert(X, Pose3(Rot3(1.,0.,0., initial.insert(X, Pose3(Rot3(1.,0.,0.,
0.,-1.,0., 0.,-1.,0.,
0.,0.,-1.), Point3(0.,0.,2.0))); 0.,0.,-1.), Point3(0.,0.,2.0)));
/* 4. Optimize the graph using Levenberg-Marquardt*/ /* 4. Optimize the graph using Levenberg-Marquardt*/
PoseValues result = optimize<NonlinearFactorGraph<PoseValues> , PoseValues> ( DynamicValues result = optimize<NonlinearFactorGraph> (graph, initial);
graph, initial);
result.print("Final result: "); result.print("Final result: ");
return 0; return 0;

4
examples/Makefile.am
View File

@ -16,8 +16,8 @@ noinst_PROGRAMS += PlanarSLAMExample_easy # Solves SLAM example from tutorial
noinst_PROGRAMS += PlanarSLAMSelfContained_advanced # Solves SLAM example from tutorial with all typedefs in the script noinst_PROGRAMS += PlanarSLAMSelfContained_advanced # Solves SLAM example from tutorial with all typedefs in the script
noinst_PROGRAMS += Pose2SLAMExample_easy # Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface noinst_PROGRAMS += Pose2SLAMExample_easy # Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface
noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface
noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver #noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver
noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver #noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver
noinst_PROGRAMS += elaboratePoint2KalmanFilter # simple linear Kalman filter on a moving 2D point, but done using factor graphs noinst_PROGRAMS += elaboratePoint2KalmanFilter # simple linear Kalman filter on a moving 2D point, but done using factor graphs
noinst_PROGRAMS += easyPoint2KalmanFilter # uses the cool generic templated Kalman filter class to do the same noinst_PROGRAMS += easyPoint2KalmanFilter # uses the cool generic templated Kalman filter class to do the same
noinst_PROGRAMS += CameraResectioning noinst_PROGRAMS += CameraResectioning

2
examples/PlanarSLAMExample_easy.cpp
View File

@ -86,7 +86,7 @@ int main(int argc, char** argv) {
initialEstimate.print("initial estimate"); initialEstimate.print("initial estimate");
// optimize using Levenberg-Marquardt optimization with an ordering from colamd // optimize using Levenberg-Marquardt optimization with an ordering from colamd
planarSLAM::Values result = optimize<Graph, planarSLAM::Values>(graph, initialEstimate); planarSLAM::Values result = optimize<Graph>(graph, initialEstimate);
result.print("final result"); result.print("final result");
return 0; return 0;

24
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -34,7 +34,6 @@
#include <gtsam/slam/BearingRangeFactor.h> #include <gtsam/slam/BearingRangeFactor.h>
// implementations for structures - needed if self-contained, and these should be included last // implementations for structures - needed if self-contained, and these should be included last
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h> #include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/linear/GaussianSequentialSolver.h>
@ -43,12 +42,9 @@
// Main typedefs // Main typedefs
typedef gtsam::TypedSymbol<gtsam::Pose2, 'x'> PoseKey; // Key for poses, with type included typedef gtsam::TypedSymbol<gtsam::Pose2, 'x'> PoseKey; // Key for poses, with type included
typedef gtsam::TypedSymbol<gtsam::Point2,'l'> PointKey; // Key for points, with type included typedef gtsam::TypedSymbol<gtsam::Point2,'l'> PointKey; // Key for points, with type included
typedef gtsam::Values<PoseKey> PoseValues; // config type for poses typedef gtsam::NonlinearFactorGraph Graph; // graph structure
typedef gtsam::Values<PointKey> PointValues; // config type for points typedef gtsam::NonlinearOptimizer<Graph,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential; // optimization engine for this domain
typedef gtsam::TupleValues2<PoseValues, PointValues> PlanarValues; // main config with two variable classes typedef gtsam::NonlinearOptimizer<Graph,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal; // optimization engine for this domain
typedef gtsam::NonlinearFactorGraph<PlanarValues> Graph; // graph structure
typedef gtsam::NonlinearOptimizer<Graph,PlanarValues,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential; // optimization engine for this domain
typedef gtsam::NonlinearOptimizer<Graph,PlanarValues,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal; // optimization engine for this domain
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -74,7 +70,7 @@ int main(int argc, char** argv) {
// gaussian for prior // gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
PriorFactor<PlanarValues, PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor PriorFactor<PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor
graph->add(posePrior); // add the factor to the graph graph->add(posePrior); // add the factor to the graph
/* add odometry */ /* add odometry */
@ -82,8 +78,8 @@ int main(int argc, char** argv) {
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
// create between factors to represent odometry // create between factors to represent odometry
BetweenFactor<PlanarValues,PoseKey> odom12(x1, x2, odom_measurement, odom_model); BetweenFactor<PoseKey> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<PlanarValues,PoseKey> odom23(x2, x3, odom_measurement, odom_model); BetweenFactor<PoseKey> odom23(x2, x3, odom_measurement, odom_model);
graph->add(odom12); // add both to graph graph->add(odom12); // add both to graph
graph->add(odom23); graph->add(odom23);
@ -100,9 +96,9 @@ int main(int argc, char** argv) {
range32 = 2.0; range32 = 2.0;
// create bearing/range factors // create bearing/range factors
BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model); BearingRangeFactor<PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model);
BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model); BearingRangeFactor<PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model); BearingRangeFactor<PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model);
// add the factors // add the factors
graph->add(meas11); graph->add(meas11);
@ -112,7 +108,7 @@ int main(int argc, char** argv) {
graph->print("Full Graph"); graph->print("Full Graph");
// initialize to noisy points // initialize to noisy points
boost::shared_ptr<PlanarValues> initial(new PlanarValues); boost::shared_ptr<DynamicValues> initial(new DynamicValues);
initial->insert(x1, Pose2(0.5, 0.0, 0.2)); initial->insert(x1, Pose2(0.5, 0.0, 0.2));
initial->insert(x2, Pose2(2.3, 0.1,-0.2)); initial->insert(x2, Pose2(2.3, 0.1,-0.2));
initial->insert(x3, Pose2(4.1, 0.1, 0.1)); initial->insert(x3, Pose2(4.1, 0.1, 0.1));

4
examples/Pose2SLAMExample_advanced.cpp
View File

@ -57,7 +57,7 @@ int main(int argc, char** argv) {
/* 3. Create the data structure to hold the initial estimate to the solution /* 3. Create the data structure to hold the initial estimate to the solution
* initialize to noisy points */ * initialize to noisy points */
shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values); shared_ptr<DynamicValues> initial(new DynamicValues);
initial->insert(x1, Pose2(0.5, 0.0, 0.2)); initial->insert(x1, Pose2(0.5, 0.0, 0.2));
initial->insert(x2, Pose2(2.3, 0.1,-0.2)); initial->insert(x2, Pose2(2.3, 0.1,-0.2));
initial->insert(x3, Pose2(4.1, 0.1, 0.1)); initial->insert(x3, Pose2(4.1, 0.1, 0.1));
@ -72,7 +72,7 @@ int main(int argc, char** argv) {
Optimizer optimizer(graph, initial, ordering, params); Optimizer optimizer(graph, initial, ordering, params);
Optimizer optimizer_result = optimizer.levenbergMarquardt(); Optimizer optimizer_result = optimizer.levenbergMarquardt();
pose2SLAM::Values result = *optimizer_result.values(); DynamicValues result = *optimizer_result.values();
result.print("final result"); result.print("final result");
/* Get covariances */ /* Get covariances */

4
examples/Pose2SLAMExample_easy.cpp
View File

@ -55,7 +55,7 @@ int main(int argc, char** argv) {
/* 3. Create the data structure to hold the initial estinmate to the solution /* 3. Create the data structure to hold the initial estinmate to the solution
* initialize to noisy points */ * initialize to noisy points */
pose2SLAM::Values initial; DynamicValues initial;
initial.insert(x1, Pose2(0.5, 0.0, 0.2)); initial.insert(x1, Pose2(0.5, 0.0, 0.2));
initial.insert(x2, Pose2(2.3, 0.1,-0.2)); initial.insert(x2, Pose2(2.3, 0.1,-0.2));
initial.insert(x3, Pose2(4.1, 0.1, 0.1)); initial.insert(x3, Pose2(4.1, 0.1, 0.1));
@ -63,7 +63,7 @@ int main(int argc, char** argv) {
/* 4 Single Step Optimization /* 4 Single Step Optimization
* optimize using Levenberg-Marquardt optimization with an ordering from colamd */ * optimize using Levenberg-Marquardt optimization with an ordering from colamd */
pose2SLAM::Values result = optimize<Graph, pose2SLAM::Values>(graph, initial); DynamicValues result = optimize<Graph>(graph, initial);
result.print("final result"); result.print("final result");

2
examples/Pose2SLAMwSPCG_easy.cpp
View File

@ -27,7 +27,7 @@ using namespace gtsam;
using namespace pose2SLAM; using namespace pose2SLAM;
Graph graph; Graph graph;
pose2SLAM::Values initial, result; DynamicValues initial, result;
/* ************************************************************************* */ /* ************************************************************************* */
int main(void) { int main(void) {

10
examples/SimpleRotation.cpp
View File

@ -23,7 +23,6 @@
#include <gtsam/geometry/Rot2.h> #include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/Key.h> #include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization.h> #include <gtsam/nonlinear/NonlinearOptimization.h>
@ -53,8 +52,7 @@ using namespace gtsam;
* and 2D poses. * and 2D poses.
*/ */
typedef TypedSymbol<Rot2, 'x'> Key; /// Variable labels for a specific type typedef TypedSymbol<Rot2, 'x'> Key; /// Variable labels for a specific type
typedef Values<Key> RotValues; /// Class to store values - acts as a state for the system typedef NonlinearFactorGraph Graph; /// Graph container for constraints - needs to know type of variables
typedef NonlinearFactorGraph<RotValues> Graph; /// Graph container for constraints - needs to know type of variables
const double degree = M_PI / 180; const double degree = M_PI / 180;
@ -83,7 +81,7 @@ int main() {
prior.print("goal angle"); prior.print("goal angle");
SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree); SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree);
Key key(1); Key key(1);
PriorFactor<RotValues, Key> factor(key, prior, model); PriorFactor<Key> factor(key, prior, model);
/** /**
* Step 3: create a graph container and add the factor to it * Step 3: create a graph container and add the factor to it
@ -114,7 +112,7 @@ int main() {
* on the type of key used to find the appropriate value map if there * on the type of key used to find the appropriate value map if there
* are multiple types of variables. * are multiple types of variables.
*/ */
RotValues initial; DynamicValues initial;
initial.insert(key, Rot2::fromAngle(20 * degree)); initial.insert(key, Rot2::fromAngle(20 * degree));
initial.print("initial estimate"); initial.print("initial estimate");
@ -126,7 +124,7 @@ int main() {
* initial estimate. This will yield a new RotValues structure * initial estimate. This will yield a new RotValues structure
* with the final state of the optimization. * with the final state of the optimization.
*/ */
RotValues result = optimize<Graph, RotValues>(graph, initial); DynamicValues result = optimize<Graph>(graph, initial);
result.print("final result"); result.print("final result");
return 0; return 0;

16
examples/easyPoint2KalmanFilter.cpp
View File

@ -24,7 +24,6 @@
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h> #include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
using namespace std; using namespace std;
@ -32,7 +31,6 @@ using namespace gtsam;
// Define Types for Linear System Test // Define Types for Linear System Test
typedef TypedSymbol<Point2, 'x'> LinearKey; typedef TypedSymbol<Point2, 'x'> LinearKey;
typedef Values<LinearKey> LinearValues;
typedef Point2 LinearMeasurement; typedef Point2 LinearMeasurement;
int main() { int main() {
@ -42,7 +40,7 @@ int main() {
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
// Create an ExtendedKalmanFilter object // Create an ExtendedKalmanFilter object
ExtendedKalmanFilter<LinearValues,LinearKey> ekf(x_initial, P_initial); ExtendedKalmanFilter<LinearKey> ekf(x_initial, P_initial);
@ -67,7 +65,7 @@ int main() {
// Predict delta based on controls // Predict delta based on controls
Point2 difference(1,0); Point2 difference(1,0);
// Create Factor // Create Factor
BetweenFactor<LinearValues,LinearKey> factor1(x0, x1, difference, Q); BetweenFactor<LinearKey> factor1(x0, x1, difference, Q);
// Predict the new value with the EKF class // Predict the new value with the EKF class
Point2 x1_predict = ekf.predict(factor1); Point2 x1_predict = ekf.predict(factor1);
@ -88,7 +86,7 @@ int main() {
// This simple measurement can be modeled with a PriorFactor // This simple measurement can be modeled with a PriorFactor
Point2 z1(1.0, 0.0); Point2 z1(1.0, 0.0);
PriorFactor<LinearValues,LinearKey> factor2(x1, z1, R); PriorFactor<LinearKey> factor2(x1, z1, R);
// Update the Kalman Filter with the measurement // Update the Kalman Filter with the measurement
Point2 x1_update = ekf.update(factor2); Point2 x1_update = ekf.update(factor2);
@ -100,13 +98,13 @@ int main() {
// Predict // Predict
LinearKey x2(2); LinearKey x2(2);
difference = Point2(1,0); difference = Point2(1,0);
BetweenFactor<LinearValues,LinearKey> factor3(x1, x2, difference, Q); BetweenFactor<LinearKey> factor3(x1, x2, difference, Q);
Point2 x2_predict = ekf.predict(factor1); Point2 x2_predict = ekf.predict(factor1);
x2_predict.print("X2 Predict"); x2_predict.print("X2 Predict");
// Update // Update
Point2 z2(2.0, 0.0); Point2 z2(2.0, 0.0);
PriorFactor<LinearValues,LinearKey> factor4(x2, z2, R); PriorFactor<LinearKey> factor4(x2, z2, R);
Point2 x2_update = ekf.update(factor4); Point2 x2_update = ekf.update(factor4);
x2_update.print("X2 Update"); x2_update.print("X2 Update");
@ -116,13 +114,13 @@ int main() {
// Predict // Predict
LinearKey x3(3); LinearKey x3(3);
difference = Point2(1,0); difference = Point2(1,0);
BetweenFactor<LinearValues,LinearKey> factor5(x2, x3, difference, Q); BetweenFactor<LinearKey> factor5(x2, x3, difference, Q);
Point2 x3_predict = ekf.predict(factor5); Point2 x3_predict = ekf.predict(factor5);
x3_predict.print("X3 Predict"); x3_predict.print("X3 Predict");
// Update // Update
Point2 z3(3.0, 0.0); Point2 z3(3.0, 0.0);
PriorFactor<LinearValues,LinearKey> factor6(x3, z3, R); PriorFactor<LinearKey> factor6(x3, z3, R);
Point2 x3_update = ekf.update(factor6); Point2 x3_update = ekf.update(factor6);
x3_update.print("X3 Update"); x3_update.print("X3 Update");

18
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -22,7 +22,6 @@
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Key.h> #include <gtsam/nonlinear/Key.h>
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam/linear/GaussianSequentialSolver.h>
@ -35,7 +34,6 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
typedef TypedSymbol<Point2, 'x'> Key; /// Variable labels for a specific type typedef TypedSymbol<Point2, 'x'> Key; /// Variable labels for a specific type
typedef Values<Key> KalmanValues; /// Class to store values - acts as a state for the system
int main() { int main() {
@ -48,7 +46,7 @@ int main() {
Ordering::shared_ptr ordering(new Ordering); Ordering::shared_ptr ordering(new Ordering);
// Create a structure to hold the linearization points // Create a structure to hold the linearization points
KalmanValues linearizationPoints; DynamicValues linearizationPoints;
// Ground truth example // Ground truth example
// Start at origin, move to the right (x-axis): 0,0 0,1 0,2 // Start at origin, move to the right (x-axis): 0,0 0,1 0,2
@ -64,7 +62,7 @@ int main() {
// This is equivalent to x_0 and P_0 // This is equivalent to x_0 and P_0
Point2 x_initial(0,0); Point2 x_initial(0,0);
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
PriorFactor<KalmanValues, Key> factor1(x0, x_initial, P_initial); PriorFactor<Key> factor1(x0, x_initial, P_initial);
// Linearize the factor and add it to the linear factor graph // Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x0, x_initial); linearizationPoints.insert(x0, x_initial);
linearFactorGraph->push_back(factor1.linearize(linearizationPoints, *ordering)); linearFactorGraph->push_back(factor1.linearize(linearizationPoints, *ordering));
@ -95,7 +93,7 @@ int main() {
Point2 difference(1,0); Point2 difference(1,0);
SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
BetweenFactor<KalmanValues, Key> factor2(x0, x1, difference, Q); BetweenFactor<Key> factor2(x0, x1, difference, Q);
// Linearize the factor and add it to the linear factor graph // Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x1, x_initial); linearizationPoints.insert(x1, x_initial);
linearFactorGraph->push_back(factor2.linearize(linearizationPoints, *ordering)); linearFactorGraph->push_back(factor2.linearize(linearizationPoints, *ordering));
@ -173,7 +171,7 @@ int main() {
// This can be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R. // This can be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R.
Point2 z1(1.0, 0.0); Point2 z1(1.0, 0.0);
SharedDiagonal R1 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); SharedDiagonal R1 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
PriorFactor<KalmanValues, Key> factor4(x1, z1, R1); PriorFactor<Key> factor4(x1, z1, R1);
// Linearize the factor and add it to the linear factor graph // Linearize the factor and add it to the linear factor graph
linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering)); linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering));
@ -225,7 +223,7 @@ int main() {
// Create a nonlinear factor describing the motion model // Create a nonlinear factor describing the motion model
difference = Point2(1,0); difference = Point2(1,0);
Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
BetweenFactor<KalmanValues, Key> factor6(x1, x2, difference, Q); BetweenFactor<Key> factor6(x1, x2, difference, Q);
// Linearize the factor and add it to the linear factor graph // Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x2, x1_update); linearizationPoints.insert(x2, x1_update);
@ -263,7 +261,7 @@ int main() {
// And update using z2 ... // And update using z2 ...
Point2 z2(2.0, 0.0); Point2 z2(2.0, 0.0);
SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
PriorFactor<KalmanValues, Key> factor8(x2, z2, R2); PriorFactor<Key> factor8(x2, z2, R2);
// Linearize the factor and add it to the linear factor graph // Linearize the factor and add it to the linear factor graph
linearFactorGraph->push_back(factor8.linearize(linearizationPoints, *ordering)); linearFactorGraph->push_back(factor8.linearize(linearizationPoints, *ordering));
@ -314,7 +312,7 @@ int main() {
// Create a nonlinear factor describing the motion model // Create a nonlinear factor describing the motion model
difference = Point2(1,0); difference = Point2(1,0);
Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
BetweenFactor<KalmanValues, Key> factor10(x2, x3, difference, Q); BetweenFactor<Key> factor10(x2, x3, difference, Q);
// Linearize the factor and add it to the linear factor graph // Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x3, x2_update); linearizationPoints.insert(x3, x2_update);
@ -352,7 +350,7 @@ int main() {
// And update using z3 ... // And update using z3 ...
Point2 z3(3.0, 0.0); Point2 z3(3.0, 0.0);
SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
PriorFactor<KalmanValues, Key> factor12(x3, z3, R3); PriorFactor<Key> factor12(x3, z3, R3);
// Linearize the factor and add it to the linear factor graph // Linearize the factor and add it to the linear factor graph
linearFactorGraph->push_back(factor12.linearize(linearizationPoints, *ordering)); linearFactorGraph->push_back(factor12.linearize(linearizationPoints, *ordering));

14
examples/vSLAMexample/vISAMexample.cpp
View File

@ -80,7 +80,7 @@ void readAllDataISAM() {
/** /**
* Setup newFactors and initialValues for each new pose and set of measurements at each frame. * Setup newFactors and initialValues for each new pose and set of measurements at each frame.
*/ */
void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<visualSLAM::Values>& initialValues, void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<DynamicValues>& initialValues,
int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
// Create a graph of newFactors with new measurements // Create a graph of newFactors with new measurements
@ -101,10 +101,10 @@ void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<visualSLA
} }
// Create initial values for all nodes in the newFactors // Create initial values for all nodes in the newFactors
initialValues = shared_ptr<visualSLAM::Values> (new visualSLAM::Values()); initialValues = shared_ptr<DynamicValues> (new DynamicValues());
initialValues->insert(pose_id, pose); initialValues->insert(PoseKey(pose_id), pose);
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
initialValues->insert(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]); initialValues->insert(PointKey(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
} }
} }
@ -124,7 +124,7 @@ int main(int argc, char* argv[]) {
// Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates // Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates
int relinearizeInterval = 3; int relinearizeInterval = 3;
NonlinearISAM<visualSLAM::Values> isam(relinearizeInterval); NonlinearISAM<> isam(relinearizeInterval);
// At each frame (poseId) with new camera pose and set of associated measurements, // At each frame (poseId) with new camera pose and set of associated measurements,
// create a graph of new factors and update ISAM // create a graph of new factors and update ISAM
@ -132,12 +132,12 @@ int main(int argc, char* argv[]) {
BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) { BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) {
const int poseId = features.first; const int poseId = features.first;
shared_ptr<Graph> newFactors; shared_ptr<Graph> newFactors;
shared_ptr<visualSLAM::Values> initialValues; shared_ptr<DynamicValues> initialValues;
createNewFactors(newFactors, initialValues, poseId, g_poses[poseId], createNewFactors(newFactors, initialValues, poseId, g_poses[poseId],
features.second, measurementSigma, g_calib); features.second, measurementSigma, g_calib);
isam.update(*newFactors, *initialValues); isam.update(*newFactors, *initialValues);
visualSLAM::Values currentEstimate = isam.estimate(); DynamicValues currentEstimate = isam.estimate();
cout << "****************************************************" << endl; cout << "****************************************************" << endl;
currentEstimate.print("Current estimate: "); currentEstimate.print("Current estimate: ");
} }

10
examples/vSLAMexample/vSFMexample.cpp
View File

@ -103,17 +103,17 @@ Graph setupGraph(std::vector<Feature2D>& measurements, SharedNoiseModel measurem
* Create a structure of initial estimates for all nodes (landmarks and poses) in the graph. * Create a structure of initial estimates for all nodes (landmarks and poses) in the graph.
* The returned Values structure contains all initial values for all nodes. * The returned Values structure contains all initial values for all nodes.
*/ */
visualSLAM::Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) { DynamicValues initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
visualSLAM::Values initValues; DynamicValues initValues;
// Initialize landmarks 3D positions. // Initialize landmarks 3D positions.
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++) for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
initValues.insert(lmit->first, lmit->second); initValues.insert(PointKey(lmit->first), lmit->second);
// Initialize camera poses. // Initialize camera poses.
for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++) for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
initValues.insert(poseit->first, poseit->second); initValues.insert(PoseKey(poseit->first), poseit->second);
return initValues; return initValues;
} }
@ -136,7 +136,7 @@ int main(int argc, char* argv[]) {
boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib))); boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
// Create an initial Values structure using groundtruth values as the initial estimates // Create an initial Values structure using groundtruth values as the initial estimates
boost::shared_ptr<visualSLAM::Values> initialEstimates(new visualSLAM::Values(initialize(g_landmarks, g_poses))); boost::shared_ptr<DynamicValues> initialEstimates(new DynamicValues(initialize(g_landmarks, g_poses)));
cout << "*******************************************************" << endl; cout << "*******************************************************" << endl;
initialEstimates->print("INITIAL ESTIMATES: "); initialEstimates->print("INITIAL ESTIMATES: ");

18
gtsam/linear/SubgraphSolver-inl.h
View File

@ -20,8 +20,8 @@ using namespace std;
namespace gtsam { namespace gtsam {
template<class GRAPH, class LINEAR, class VALUES> template<class GRAPH, class LINEAR, class KEY>
void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::shared_ptr &graph) { void SubgraphSolver<GRAPH,LINEAR,KEY>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>(); GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>();
GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared<GaussianFactorGraph>(); GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared<GaussianFactorGraph>();
@ -46,8 +46,8 @@ void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::
Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(), Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),Rc1,xbar); Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(), Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),Rc1,xbar);
} }
template<class GRAPH, class LINEAR, class VALUES> template<class GRAPH, class LINEAR, class KEY>
VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() const { VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,KEY>::optimize() const {
// preconditioned conjugate gradient // preconditioned conjugate gradient
VectorValues zeros = pc_->zero(); VectorValues zeros = pc_->zero();
@ -59,18 +59,18 @@ VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() const {
return xbar; return xbar;
} }
template<class GRAPH, class LINEAR, class VALUES> template<class GRAPH, class LINEAR, class KEY>
void SubgraphSolver<GRAPH,LINEAR,VALUES>::initialize(const GRAPH& G, const VALUES& theta0) { void SubgraphSolver<GRAPH,LINEAR,KEY>::initialize(const GRAPH& G, const DynamicValues& theta0) {
// generate spanning tree // generate spanning tree
PredecessorMap<Key> tree_ = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G); PredecessorMap<KEY> tree_ = gtsam::findMinimumSpanningTree<GRAPH, KEY, Constraint>(G);
// make the ordering // make the ordering
list<Key> keys = predecessorMap2Keys(tree_); list<KEY> keys = predecessorMap2Keys(tree_);
ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end())); ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end()));
// build factor pairs // build factor pairs
pairs_.clear(); pairs_.clear();
typedef pair<Key,Key> EG ; typedef pair<KEY,KEY> EG ;
BOOST_FOREACH( const EG &eg, tree_ ) { BOOST_FOREACH( const EG &eg, tree_ ) {
Symbol key1 = Symbol(eg.first), Symbol key1 = Symbol(eg.first),
key2 = Symbol(eg.second) ; key2 = Symbol(eg.second) ;

11
gtsam/linear/SubgraphSolver.h
View File

@ -16,6 +16,7 @@
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/IterativeSolver.h> #include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h> #include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/nonlinear/DynamicValues.h>
namespace gtsam { namespace gtsam {
@ -31,11 +32,11 @@ bool split(const std::map<Index, Index> &M,
* linearize: G * T -> L * linearize: G * T -> L
* solve : L -> VectorValues * solve : L -> VectorValues
*/ */
template<class GRAPH, class LINEAR, class VALUES> template<class GRAPH, class LINEAR, class KEY>
class SubgraphSolver : public IterativeSolver { class SubgraphSolver : public IterativeSolver {
private: private:
typedef typename VALUES::Key Key; // typedef typename VALUES::Key Key;
typedef typename GRAPH::Pose Pose; typedef typename GRAPH::Pose Pose;
typedef typename GRAPH::Constraint Constraint; typedef typename GRAPH::Constraint Constraint;
@ -43,7 +44,7 @@ private:
typedef boost::shared_ptr<Ordering> shared_ordering ; typedef boost::shared_ptr<Ordering> shared_ordering ;
typedef boost::shared_ptr<GRAPH> shared_graph ; typedef boost::shared_ptr<GRAPH> shared_graph ;
typedef boost::shared_ptr<LINEAR> shared_linear ; typedef boost::shared_ptr<LINEAR> shared_linear ;
typedef boost::shared_ptr<VALUES> shared_values ; typedef boost::shared_ptr<DynamicValues> shared_values ;
typedef boost::shared_ptr<SubgraphPreconditioner> shared_preconditioner ; typedef boost::shared_ptr<SubgraphPreconditioner> shared_preconditioner ;
typedef std::map<Index,Index> mapPairIndex ; typedef std::map<Index,Index> mapPairIndex ;
@ -61,7 +62,7 @@ private:
public: public:
SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters &parameters = Parameters(), bool useQR = false): SubgraphSolver(const GRAPH& G, const DynamicValues& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); } IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); }
SubgraphSolver(const LINEAR& GFG) { SubgraphSolver(const LINEAR& GFG) {
@ -89,7 +90,7 @@ public:
shared_ordering ordering() const { return ordering_; } shared_ordering ordering() const { return ordering_; }
protected: protected:
void initialize(const GRAPH& G, const VALUES& theta0); void initialize(const GRAPH& G, const DynamicValues& theta0);
private: private:
SubgraphSolver():IterativeSolver(){} SubgraphSolver():IterativeSolver(){}

22
gtsam/nonlinear/ExtendedKalmanFilter-inl.h
View File

@ -27,10 +27,10 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES, class KEY> template<class KEY>
typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::solve_( typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::solve_(
const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering, const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering,
const VALUES& linearizationPoints, const KEY& lastKey, const DynamicValues& linearizationPoints, const KEY& lastKey,
JacobianFactor::shared_ptr& newPrior) const { JacobianFactor::shared_ptr& newPrior) const {
// Extract the Index of the provided last key // Extract the Index of the provided last key
@ -66,8 +66,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES, class KEY> template<class KEY>
ExtendedKalmanFilter<VALUES, KEY>::ExtendedKalmanFilter(T x_initial, ExtendedKalmanFilter<KEY>::ExtendedKalmanFilter(T x_initial,
noiseModel::Gaussian::shared_ptr P_initial) { noiseModel::Gaussian::shared_ptr P_initial) {
// Set the initial linearization point to the provided mean // Set the initial linearization point to the provided mean
@ -82,8 +82,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES, class KEY> template<class KEY>
typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::predict( typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::predict(
const MotionFactor& motionFactor) { const MotionFactor& motionFactor) {
// TODO: This implementation largely ignores the actual factor symbols. // TODO: This implementation largely ignores the actual factor symbols.
@ -100,7 +100,7 @@ namespace gtsam {
ordering.insert(x1, 1); ordering.insert(x1, 1);
// Create a set of linearization points // Create a set of linearization points
VALUES linearizationPoints; DynamicValues linearizationPoints;
linearizationPoints.insert(x0, x_); linearizationPoints.insert(x0, x_);
linearizationPoints.insert(x1, x_); // TODO should this really be x_ ? linearizationPoints.insert(x1, x_); // TODO should this really be x_ ?
@ -122,8 +122,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES, class KEY> template<class KEY>
typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::update( typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::update(
const MeasurementFactor& measurementFactor) { const MeasurementFactor& measurementFactor) {
// TODO: This implementation largely ignores the actual factor symbols. // TODO: This implementation largely ignores the actual factor symbols.
@ -138,7 +138,7 @@ namespace gtsam {
ordering.insert(x0, 0); ordering.insert(x0, 0);
// Create a set of linearization points // Create a set of linearization points
VALUES linearizationPoints; DynamicValues linearizationPoints;
linearizationPoints.insert(x0, x_); linearizationPoints.insert(x0, x_);
// Create a Gaussian Factor Graph // Create a Gaussian Factor Graph

10
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -39,21 +39,21 @@ namespace gtsam {
* TODO: a "predictAndUpdate" that combines both steps for some computational savings. * TODO: a "predictAndUpdate" that combines both steps for some computational savings.
*/ */
template<class VALUES, class KEY> template<class KEY>
class ExtendedKalmanFilter { class ExtendedKalmanFilter {
public: public:
typedef boost::shared_ptr<ExtendedKalmanFilter<VALUES, KEY> > shared_ptr; typedef boost::shared_ptr<ExtendedKalmanFilter<KEY> > shared_ptr;
typedef typename KEY::Value T; typedef typename KEY::Value T;
typedef NonlinearFactor2<VALUES, KEY, KEY> MotionFactor; typedef NonlinearFactor2<KEY, KEY> MotionFactor;
typedef NonlinearFactor1<VALUES, KEY> MeasurementFactor; typedef NonlinearFactor1<KEY> MeasurementFactor;
protected: protected:
T x_; // linearization point T x_; // linearization point
JacobianFactor::shared_ptr priorFactor_; // density JacobianFactor::shared_ptr priorFactor_; // density
T solve_(const GaussianFactorGraph& linearFactorGraph, T solve_(const GaussianFactorGraph& linearFactorGraph,
const Ordering& ordering, const VALUES& linearizationPoints, const Ordering& ordering, const DynamicValues& linearizationPoints,
const KEY& x, JacobianFactor::shared_ptr& newPrior) const; const KEY& x, JacobianFactor::shared_ptr& newPrior) const;
public: public:

24
gtsam/nonlinear/NonlinearISAM-inl.h
View File

@ -29,9 +29,9 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES, class GRAPH> template<class GRAPH>
void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors, void NonlinearISAM<GRAPH>::update(const Factors& newFactors,
const Values& initialValues) { const DynamicValues& initialValues) {
if(newFactors.size() > 0) { if(newFactors.size() > 0) {
@ -62,14 +62,14 @@ void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors,
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES, class GRAPH> template<class GRAPH>
void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() { void NonlinearISAM<GRAPH>::reorder_relinearize() {
// cout << "Reordering, relinearizing..." << endl; // cout << "Reordering, relinearizing..." << endl;
if(factors_.size() > 0) { if(factors_.size() > 0) {
// Obtain the new linearization point // Obtain the new linearization point
const Values newLinPoint = estimate(); const DynamicValues newLinPoint = estimate();
isam_.clear(); isam_.clear();
@ -89,8 +89,8 @@ void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES, class GRAPH> template<class GRAPH>
VALUES NonlinearISAM<VALUES,GRAPH>::estimate() const { DynamicValues NonlinearISAM<GRAPH>::estimate() const {
if(isam_.size() > 0) if(isam_.size() > 0)
return linPoint_.retract(optimize(isam_), ordering_); return linPoint_.retract(optimize(isam_), ordering_);
else else
@ -98,14 +98,14 @@ VALUES NonlinearISAM<VALUES,GRAPH>::estimate() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES, class GRAPH> template<class GRAPH>
Matrix NonlinearISAM<VALUES,GRAPH>::marginalCovariance(const Symbol& key) const { Matrix NonlinearISAM<GRAPH>::marginalCovariance(const Symbol& key) const {
return isam_.marginalCovariance(ordering_[key]); return isam_.marginalCovariance(ordering_[key]);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES, class GRAPH> template<class GRAPH>
void NonlinearISAM<VALUES,GRAPH>::print(const std::string& s) const { void NonlinearISAM<GRAPH>::print(const std::string& s) const {
cout << "ISAM - " << s << ":" << endl; cout << "ISAM - " << s << ":" << endl;
cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl; cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl;
isam_.print("GaussianISAM"); isam_.print("GaussianISAM");

11
gtsam/nonlinear/NonlinearISAM.h
View File

@ -24,11 +24,10 @@ namespace gtsam {
/** /**
* Wrapper class to manage ISAM in a nonlinear context * Wrapper class to manage ISAM in a nonlinear context
*/ */
template<class VALUES, class GRAPH = gtsam::NonlinearFactorGraph<VALUES> > template<class GRAPH = gtsam::NonlinearFactorGraph >
class NonlinearISAM { class NonlinearISAM {
public: public:
typedef VALUES Values;
typedef GRAPH Factors; typedef GRAPH Factors;
protected: protected:
@ -37,7 +36,7 @@ protected:
gtsam::GaussianISAM isam_; gtsam::GaussianISAM isam_;
/** The current linearization point */ /** The current linearization point */
Values linPoint_; DynamicValues linPoint_;
/** The ordering */ /** The ordering */
gtsam::Ordering ordering_; gtsam::Ordering ordering_;
@ -61,10 +60,10 @@ public:
NonlinearISAM(int reorderInterval = 1) : reorderInterval_(reorderInterval), reorderCounter_(0) {} NonlinearISAM(int reorderInterval = 1) : reorderInterval_(reorderInterval), reorderCounter_(0) {}
/** Add new factors along with their initial linearization points */ /** Add new factors along with their initial linearization points */
void update(const Factors& newFactors, const Values& initialValues); void update(const Factors& newFactors, const DynamicValues& initialValues);
/** Return the current solution estimate */ /** Return the current solution estimate */
Values estimate() const; DynamicValues estimate() const;
/** Relinearization and reordering of variables */ /** Relinearization and reordering of variables */
void reorder_relinearize(); void reorder_relinearize();
@ -84,7 +83,7 @@ public:
const GaussianISAM& bayesTree() const { return isam_; } const GaussianISAM& bayesTree() const { return isam_; }
/** Return the current linearization point */ /** Return the current linearization point */
const Values& getLinearizationPoint() const { return linPoint_; } const DynamicValues& getLinearizationPoint() const { return linPoint_; }
/** Get the ordering */ /** Get the ordering */
const gtsam::Ordering& getOrdering() const { return ordering_; } const gtsam::Ordering& getOrdering() const { return ordering_; }

10
gtsam/nonlinear/NonlinearOptimization-inl.h
View File

@ -39,7 +39,7 @@ namespace gtsam {
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate); Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
// Create an nonlinear Optimizer that uses a Sequential Solver // Create an nonlinear Optimizer that uses a Sequential Solver
typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, GaussianSequentialSolver> Optimizer; typedef NonlinearOptimizer<G, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
Optimizer optimizer(boost::make_shared<const G>(graph), Optimizer optimizer(boost::make_shared<const G>(graph),
boost::make_shared<const DynamicValues>(initialEstimate), ordering, boost::make_shared<const DynamicValues>(initialEstimate), ordering,
boost::make_shared<NonlinearOptimizationParameters>(parameters)); boost::make_shared<NonlinearOptimizationParameters>(parameters));
@ -62,7 +62,7 @@ namespace gtsam {
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate); Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
// Create an nonlinear Optimizer that uses a Multifrontal Solver // Create an nonlinear Optimizer that uses a Multifrontal Solver
typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer; typedef NonlinearOptimizer<G, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
Optimizer optimizer(boost::make_shared<const G>(graph), Optimizer optimizer(boost::make_shared<const G>(graph),
boost::make_shared<const DynamicValues>(initialEstimate), ordering, boost::make_shared<const DynamicValues>(initialEstimate), ordering,
boost::make_shared<NonlinearOptimizationParameters>(parameters)); boost::make_shared<NonlinearOptimizationParameters>(parameters));
@ -85,7 +85,7 @@ namespace gtsam {
// initial optimization state is the same in both cases tested // initial optimization state is the same in both cases tested
typedef SubgraphSolver<G,GaussianFactorGraph,DynamicValues> Solver; typedef SubgraphSolver<G,GaussianFactorGraph,DynamicValues> Solver;
typedef boost::shared_ptr<Solver> shared_Solver; typedef boost::shared_ptr<Solver> shared_Solver;
typedef NonlinearOptimizer<G, DynamicValues, GaussianFactorGraph, Solver> SPCGOptimizer; typedef NonlinearOptimizer<G, GaussianFactorGraph, Solver> SPCGOptimizer;
shared_Solver solver = boost::make_shared<Solver>( shared_Solver solver = boost::make_shared<Solver>(
graph, initialEstimate, IterativeOptimizationParameters()); graph, initialEstimate, IterativeOptimizationParameters());
SPCGOptimizer optimizer( SPCGOptimizer optimizer(
@ -111,10 +111,10 @@ namespace gtsam {
const NonlinearOptimizationMethod& nonlinear_method) { const NonlinearOptimizationMethod& nonlinear_method) {
switch (solver) { switch (solver) {
case SEQUENTIAL: case SEQUENTIAL:
return optimizeSequential<G,DynamicValues>(graph, initialEstimate, parameters, return optimizeSequential<G>(graph, initialEstimate, parameters,
nonlinear_method == LM); nonlinear_method == LM);
case MULTIFRONTAL: case MULTIFRONTAL:
return optimizeMultiFrontal<G,DynamicValues>(graph, initialEstimate, parameters, return optimizeMultiFrontal<G>(graph, initialEstimate, parameters,
nonlinear_method == LM); nonlinear_method == LM);
case SPCG: case SPCG:
// return optimizeSPCG<G,DynamicValues>(graph, initialEstimate, parameters, // return optimizeSPCG<G,DynamicValues>(graph, initialEstimate, parameters,

4
gtsam/slam/pose2SLAM.h
View File

@ -74,10 +74,10 @@ namespace gtsam {
}; };
/// The sequential optimizer /// The sequential optimizer
typedef NonlinearOptimizer<Graph, DynamicValues, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential; typedef NonlinearOptimizer<Graph, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential;
/// The multifrontal optimizer /// The multifrontal optimizer
typedef NonlinearOptimizer<Graph, DynamicValues, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer; typedef NonlinearOptimizer<Graph, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
} // pose2SLAM } // pose2SLAM

2
tests/timeMultifrontalOnDataset.cpp
View File

@ -44,7 +44,7 @@ int main(int argc, char *argv[]) {
else if (argc == 4) else if (argc == 4)
nrTrials = strtoul(argv[3], NULL, 10); nrTrials = strtoul(argv[3], NULL, 10);
pair<shared_ptr<Pose2Graph>, shared_ptr<Pose2Values> > data = load2D(dataset(datasetname)); pair<shared_ptr<Pose2Graph>, shared_ptr<DynamicValues> > data = load2D(dataset(datasetname));
// Add a prior on the first pose // Add a prior on the first pose
if (soft_prior) if (soft_prior)

2
tests/timeSequentialOnDataset.cpp
View File

@ -44,7 +44,7 @@ int main(int argc, char *argv[]) {
else if (argc == 4) else if (argc == 4)
nrTrials = strtoul(argv[3], NULL, 10); nrTrials = strtoul(argv[3], NULL, 10);
pair<shared_ptr<Pose2Graph>, shared_ptr<Pose2Values> > data = load2D(dataset(datasetname)); pair<shared_ptr<Pose2Graph>, shared_ptr<DynamicValues> > data = load2D(dataset(datasetname));
// Add a prior on the first pose // Add a prior on the first pose
if (soft_prior) if (soft_prior)