From 2db224df3c6846b710a352a87438a8570dfcd3b1 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Sun, 29 Jan 2012 22:10:35 +0000 Subject: [PATCH] All compiled! Only SPCG and linear/SubgraphSolver are not fixed. --- examples/CameraResectioning.cpp | 15 ++++++------ examples/Makefile.am | 4 ++-- examples/PlanarSLAMExample_easy.cpp | 2 +- examples/PlanarSLAMSelfContained_advanced.cpp | 24 ++++++++----------- examples/Pose2SLAMExample_advanced.cpp | 4 ++-- examples/Pose2SLAMExample_easy.cpp | 4 ++-- examples/Pose2SLAMwSPCG_easy.cpp | 2 +- examples/SimpleRotation.cpp | 10 ++++---- examples/easyPoint2KalmanFilter.cpp | 16 ++++++------- examples/elaboratePoint2KalmanFilter.cpp | 18 +++++++------- examples/vSLAMexample/vISAMexample.cpp | 14 +++++------ examples/vSLAMexample/vSFMexample.cpp | 10 ++++---- gtsam/linear/SubgraphSolver-inl.h | 18 +++++++------- gtsam/linear/SubgraphSolver.h | 11 +++++---- gtsam/nonlinear/ExtendedKalmanFilter-inl.h | 22 ++++++++--------- gtsam/nonlinear/ExtendedKalmanFilter.h | 10 ++++---- gtsam/nonlinear/NonlinearISAM-inl.h | 24 +++++++++---------- gtsam/nonlinear/NonlinearISAM.h | 11 ++++----- gtsam/nonlinear/NonlinearOptimization-inl.h | 10 ++++---- gtsam/slam/pose2SLAM.h | 4 ++-- tests/timeMultifrontalOnDataset.cpp | 2 +- tests/timeSequentialOnDataset.cpp | 2 +- 22 files changed, 113 insertions(+), 124 deletions(-) diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index aee073df7..9727ceacb 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include #include @@ -28,13 +27,12 @@ using namespace gtsam; typedef TypedSymbol PoseKey; -typedef Values PoseValues; /** * Unary factor for the pose. */ -class ResectioningFactor: public NonlinearFactor1 { - typedef NonlinearFactor1 Base; +class ResectioningFactor: public NonlinearFactor1 { + typedef NonlinearFactor1 Base; shared_ptrK K_; // camera's intrinsic parameters Point3 P_; // 3D point on the calibration rig @@ -46,6 +44,8 @@ public: Base(model, key), K_(calib), P_(P), p_(p) { } + virtual ~ResectioningFactor() {} + virtual Vector evaluateError(const Pose3& X, boost::optional H = boost::none) const { SimpleCamera camera(*K_, X); @@ -72,7 +72,7 @@ int main(int argc, char* argv[]) { PoseKey X(1); /* 1. create graph */ - NonlinearFactorGraph graph; + NonlinearFactorGraph graph; /* 2. add factors to the graph */ // add measurement factors @@ -92,14 +92,13 @@ int main(int argc, char* argv[]) { graph.push_back(factor); /* 3. Create an initial estimate for the camera pose */ - PoseValues initial; + DynamicValues initial; initial.insert(X, Pose3(Rot3(1.,0.,0., 0.,-1.,0., 0.,0.,-1.), Point3(0.,0.,2.0))); /* 4. Optimize the graph using Levenberg-Marquardt*/ - PoseValues result = optimize , PoseValues> ( - graph, initial); + DynamicValues result = optimize (graph, initial); result.print("Final result: "); return 0; diff --git a/examples/Makefile.am b/examples/Makefile.am index 14b6fb935..d7ec2aaea 100644 --- a/examples/Makefile.am +++ b/examples/Makefile.am @@ -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 += 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 += 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_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 += 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 += CameraResectioning diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp index d697ae261..b73a989d4 100644 --- a/examples/PlanarSLAMExample_easy.cpp +++ b/examples/PlanarSLAMExample_easy.cpp @@ -86,7 +86,7 @@ int main(int argc, char** argv) { initialEstimate.print("initial estimate"); // optimize using Levenberg-Marquardt optimization with an ordering from colamd - planarSLAM::Values result = optimize(graph, initialEstimate); + planarSLAM::Values result = optimize(graph, initialEstimate); result.print("final result"); return 0; diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index d915a9ebd..10b1b3576 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -34,7 +34,6 @@ #include // implementations for structures - needed if self-contained, and these should be included last -#include #include #include #include @@ -43,12 +42,9 @@ // Main typedefs typedef gtsam::TypedSymbol PoseKey; // Key for poses, with type included typedef gtsam::TypedSymbol PointKey; // Key for points, with type included -typedef gtsam::Values PoseValues; // config type for poses -typedef gtsam::Values PointValues; // config type for points -typedef gtsam::TupleValues2 PlanarValues; // main config with two variable classes -typedef gtsam::NonlinearFactorGraph Graph; // graph structure -typedef gtsam::NonlinearOptimizer OptimizerSeqential; // optimization engine for this domain -typedef gtsam::NonlinearOptimizer OptimizerMultifrontal; // optimization engine for this domain +typedef gtsam::NonlinearFactorGraph Graph; // graph structure +typedef gtsam::NonlinearOptimizer OptimizerSeqential; // optimization engine for this domain +typedef gtsam::NonlinearOptimizer OptimizerMultifrontal; // optimization engine for this domain using namespace std; using namespace gtsam; @@ -74,7 +70,7 @@ int main(int argc, char** argv) { // gaussian for prior 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 - PriorFactor posePrior(x1, prior_measurement, prior_model); // create the factor + PriorFactor posePrior(x1, prior_measurement, prior_model); // create the factor graph->add(posePrior); // add the factor to the graph /* 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)); 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 - BetweenFactor odom12(x1, x2, odom_measurement, odom_model); - BetweenFactor odom23(x2, x3, odom_measurement, odom_model); + BetweenFactor odom12(x1, x2, odom_measurement, odom_model); + BetweenFactor odom23(x2, x3, odom_measurement, odom_model); graph->add(odom12); // add both to graph graph->add(odom23); @@ -100,9 +96,9 @@ int main(int argc, char** argv) { range32 = 2.0; // create bearing/range factors - BearingRangeFactor meas11(x1, l1, bearing11, range11, meas_model); - BearingRangeFactor meas21(x2, l1, bearing21, range21, meas_model); - BearingRangeFactor meas32(x3, l2, bearing32, range32, meas_model); + BearingRangeFactor meas11(x1, l1, bearing11, range11, meas_model); + BearingRangeFactor meas21(x2, l1, bearing21, range21, meas_model); + BearingRangeFactor meas32(x3, l2, bearing32, range32, meas_model); // add the factors graph->add(meas11); @@ -112,7 +108,7 @@ int main(int argc, char** argv) { graph->print("Full Graph"); // initialize to noisy points - boost::shared_ptr initial(new PlanarValues); + boost::shared_ptr initial(new DynamicValues); initial->insert(x1, Pose2(0.5, 0.0, 0.2)); initial->insert(x2, Pose2(2.3, 0.1,-0.2)); initial->insert(x3, Pose2(4.1, 0.1, 0.1)); diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index 703f6a47d..b6bfe7303 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -57,7 +57,7 @@ int main(int argc, char** argv) { /* 3. Create the data structure to hold the initial estimate to the solution * initialize to noisy points */ - shared_ptr initial(new pose2SLAM::Values); + shared_ptr initial(new DynamicValues); initial->insert(x1, Pose2(0.5, 0.0, 0.2)); initial->insert(x2, Pose2(2.3, 0.1,-0.2)); 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_result = optimizer.levenbergMarquardt(); - pose2SLAM::Values result = *optimizer_result.values(); + DynamicValues result = *optimizer_result.values(); result.print("final result"); /* Get covariances */ diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp index 68ece651b..fb98ebd97 100644 --- a/examples/Pose2SLAMExample_easy.cpp +++ b/examples/Pose2SLAMExample_easy.cpp @@ -55,7 +55,7 @@ int main(int argc, char** argv) { /* 3. Create the data structure to hold the initial estinmate to the solution * initialize to noisy points */ - pose2SLAM::Values initial; + DynamicValues initial; initial.insert(x1, Pose2(0.5, 0.0, 0.2)); initial.insert(x2, Pose2(2.3, 0.1,-0.2)); initial.insert(x3, Pose2(4.1, 0.1, 0.1)); @@ -63,7 +63,7 @@ int main(int argc, char** argv) { /* 4 Single Step Optimization * optimize using Levenberg-Marquardt optimization with an ordering from colamd */ - pose2SLAM::Values result = optimize(graph, initial); + DynamicValues result = optimize(graph, initial); result.print("final result"); diff --git a/examples/Pose2SLAMwSPCG_easy.cpp b/examples/Pose2SLAMwSPCG_easy.cpp index 3cce81d48..51be80841 100644 --- a/examples/Pose2SLAMwSPCG_easy.cpp +++ b/examples/Pose2SLAMwSPCG_easy.cpp @@ -27,7 +27,7 @@ using namespace gtsam; using namespace pose2SLAM; Graph graph; -pose2SLAM::Values initial, result; +DynamicValues initial, result; /* ************************************************************************* */ int main(void) { diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 9d897dd39..bd00f4a04 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -53,8 +52,7 @@ using namespace gtsam; * and 2D poses. */ typedef TypedSymbol Key; /// Variable labels for a specific type -typedef Values 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 Graph; /// Graph container for constraints - needs to know type of variables const double degree = M_PI / 180; @@ -83,7 +81,7 @@ int main() { prior.print("goal angle"); SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree); Key key(1); - PriorFactor factor(key, prior, model); + PriorFactor factor(key, prior, model); /** * 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 * are multiple types of variables. */ - RotValues initial; + DynamicValues initial; initial.insert(key, Rot2::fromAngle(20 * degree)); initial.print("initial estimate"); @@ -126,7 +124,7 @@ int main() { * initial estimate. This will yield a new RotValues structure * with the final state of the optimization. */ - RotValues result = optimize(graph, initial); + DynamicValues result = optimize(graph, initial); result.print("final result"); return 0; diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index f22d65f95..d190c38dc 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include using namespace std; @@ -32,7 +31,6 @@ using namespace gtsam; // Define Types for Linear System Test typedef TypedSymbol LinearKey; -typedef Values LinearValues; typedef Point2 LinearMeasurement; int main() { @@ -42,7 +40,7 @@ int main() { SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // Create an ExtendedKalmanFilter object - ExtendedKalmanFilter ekf(x_initial, P_initial); + ExtendedKalmanFilter ekf(x_initial, P_initial); @@ -67,7 +65,7 @@ int main() { // Predict delta based on controls Point2 difference(1,0); // Create Factor - BetweenFactor factor1(x0, x1, difference, Q); + BetweenFactor factor1(x0, x1, difference, Q); // Predict the new value with the EKF class Point2 x1_predict = ekf.predict(factor1); @@ -88,7 +86,7 @@ int main() { // This simple measurement can be modeled with a PriorFactor Point2 z1(1.0, 0.0); - PriorFactor factor2(x1, z1, R); + PriorFactor factor2(x1, z1, R); // Update the Kalman Filter with the measurement Point2 x1_update = ekf.update(factor2); @@ -100,13 +98,13 @@ int main() { // Predict LinearKey x2(2); difference = Point2(1,0); - BetweenFactor factor3(x1, x2, difference, Q); + BetweenFactor factor3(x1, x2, difference, Q); Point2 x2_predict = ekf.predict(factor1); x2_predict.print("X2 Predict"); // Update Point2 z2(2.0, 0.0); - PriorFactor factor4(x2, z2, R); + PriorFactor factor4(x2, z2, R); Point2 x2_update = ekf.update(factor4); x2_update.print("X2 Update"); @@ -116,13 +114,13 @@ int main() { // Predict LinearKey x3(3); difference = Point2(1,0); - BetweenFactor factor5(x2, x3, difference, Q); + BetweenFactor factor5(x2, x3, difference, Q); Point2 x3_predict = ekf.predict(factor5); x3_predict.print("X3 Predict"); // Update Point2 z3(3.0, 0.0); - PriorFactor factor6(x3, z3, R); + PriorFactor factor6(x3, z3, R); Point2 x3_update = ekf.update(factor6); x3_update.print("X3 Update"); diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 8c8cbe147..d001016c4 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -22,7 +22,6 @@ #include #include -#include #include #include #include @@ -35,7 +34,6 @@ using namespace std; using namespace gtsam; typedef TypedSymbol Key; /// Variable labels for a specific type -typedef Values KalmanValues; /// Class to store values - acts as a state for the system int main() { @@ -48,7 +46,7 @@ int main() { Ordering::shared_ptr ordering(new Ordering); // Create a structure to hold the linearization points - KalmanValues linearizationPoints; + DynamicValues linearizationPoints; // Ground truth example // 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 Point2 x_initial(0,0); SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - PriorFactor factor1(x0, x_initial, P_initial); + PriorFactor factor1(x0, x_initial, P_initial); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x0, x_initial); linearFactorGraph->push_back(factor1.linearize(linearizationPoints, *ordering)); @@ -95,7 +93,7 @@ int main() { Point2 difference(1,0); SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - BetweenFactor factor2(x0, x1, difference, Q); + BetweenFactor factor2(x0, x1, difference, Q); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x1, x_initial); 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. Point2 z1(1.0, 0.0); SharedDiagonal R1 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); - PriorFactor factor4(x1, z1, R1); + PriorFactor factor4(x1, z1, R1); // Linearize the factor and add it to the linear factor graph linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering)); @@ -225,7 +223,7 @@ int main() { // Create a nonlinear factor describing the motion model difference = Point2(1,0); Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - BetweenFactor factor6(x1, x2, difference, Q); + BetweenFactor factor6(x1, x2, difference, Q); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x2, x1_update); @@ -263,7 +261,7 @@ int main() { // And update using z2 ... Point2 z2(2.0, 0.0); SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); - PriorFactor factor8(x2, z2, R2); + PriorFactor factor8(x2, z2, R2); // Linearize the factor and add it to the linear factor graph linearFactorGraph->push_back(factor8.linearize(linearizationPoints, *ordering)); @@ -314,7 +312,7 @@ int main() { // Create a nonlinear factor describing the motion model difference = Point2(1,0); Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - BetweenFactor factor10(x2, x3, difference, Q); + BetweenFactor factor10(x2, x3, difference, Q); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x3, x2_update); @@ -352,7 +350,7 @@ int main() { // And update using z3 ... Point2 z3(3.0, 0.0); SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); - PriorFactor factor12(x3, z3, R3); + PriorFactor factor12(x3, z3, R3); // Linearize the factor and add it to the linear factor graph linearFactorGraph->push_back(factor12.linearize(linearizationPoints, *ordering)); diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index 6cdc34f17..ce88276c9 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -80,7 +80,7 @@ void readAllDataISAM() { /** * Setup newFactors and initialValues for each new pose and set of measurements at each frame. */ -void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, +void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, int pose_id, const Pose3& pose, const std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { // Create a graph of newFactors with new measurements @@ -101,10 +101,10 @@ void createNewFactors(shared_ptr& newFactors, boost::shared_ptr (new visualSLAM::Values()); - initialValues->insert(pose_id, pose); + initialValues = shared_ptr (new DynamicValues()); + initialValues->insert(PoseKey(pose_id), pose); 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 int relinearizeInterval = 3; - NonlinearISAM isam(relinearizeInterval); + NonlinearISAM<> isam(relinearizeInterval); // At each frame (poseId) with new camera pose and set of associated measurements, // 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) { const int poseId = features.first; shared_ptr newFactors; - shared_ptr initialValues; + shared_ptr initialValues; createNewFactors(newFactors, initialValues, poseId, g_poses[poseId], features.second, measurementSigma, g_calib); isam.update(*newFactors, *initialValues); - visualSLAM::Values currentEstimate = isam.estimate(); + DynamicValues currentEstimate = isam.estimate(); cout << "****************************************************" << endl; currentEstimate.print("Current estimate: "); } diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp index 9dc8bfe34..1edb4a9ae 100644 --- a/examples/vSLAMexample/vSFMexample.cpp +++ b/examples/vSLAMexample/vSFMexample.cpp @@ -103,17 +103,17 @@ Graph setupGraph(std::vector& measurements, SharedNoiseModel measurem * 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. */ -visualSLAM::Values initialize(std::map landmarks, std::map poses) { +DynamicValues initialize(std::map landmarks, std::map poses) { - visualSLAM::Values initValues; + DynamicValues initValues; // Initialize landmarks 3D positions. for (map::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++) - initValues.insert(lmit->first, lmit->second); + initValues.insert(PointKey(lmit->first), lmit->second); // Initialize camera poses. for (map::iterator poseit = poses.begin(); poseit != poses.end(); poseit++) - initValues.insert(poseit->first, poseit->second); + initValues.insert(PoseKey(poseit->first), poseit->second); return initValues; } @@ -136,7 +136,7 @@ int main(int argc, char* argv[]) { boost::shared_ptr graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib))); // Create an initial Values structure using groundtruth values as the initial estimates - boost::shared_ptr initialEstimates(new visualSLAM::Values(initialize(g_landmarks, g_poses))); + boost::shared_ptr initialEstimates(new DynamicValues(initialize(g_landmarks, g_poses))); cout << "*******************************************************" << endl; initialEstimates->print("INITIAL ESTIMATES: "); diff --git a/gtsam/linear/SubgraphSolver-inl.h b/gtsam/linear/SubgraphSolver-inl.h index 63a497ac1..78fa4fdfe 100644 --- a/gtsam/linear/SubgraphSolver-inl.h +++ b/gtsam/linear/SubgraphSolver-inl.h @@ -20,8 +20,8 @@ using namespace std; namespace gtsam { -template -void SubgraphSolver::replaceFactors(const typename LINEAR::shared_ptr &graph) { +template +void SubgraphSolver::replaceFactors(const typename LINEAR::shared_ptr &graph) { GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(); GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared(); @@ -46,8 +46,8 @@ void SubgraphSolver::replaceFactors(const typename LINEAR:: Ab1->dynamicCastFactors >(), Ab2->dynamicCastFactors >(),Rc1,xbar); } -template -VectorValues::shared_ptr SubgraphSolver::optimize() const { +template +VectorValues::shared_ptr SubgraphSolver::optimize() const { // preconditioned conjugate gradient VectorValues zeros = pc_->zero(); @@ -59,18 +59,18 @@ VectorValues::shared_ptr SubgraphSolver::optimize() const { return xbar; } -template -void SubgraphSolver::initialize(const GRAPH& G, const VALUES& theta0) { +template +void SubgraphSolver::initialize(const GRAPH& G, const DynamicValues& theta0) { // generate spanning tree - PredecessorMap tree_ = gtsam::findMinimumSpanningTree(G); + PredecessorMap tree_ = gtsam::findMinimumSpanningTree(G); // make the ordering - list keys = predecessorMap2Keys(tree_); + list keys = predecessorMap2Keys(tree_); ordering_ = boost::make_shared(list(keys.begin(), keys.end())); // build factor pairs pairs_.clear(); - typedef pair EG ; + typedef pair EG ; BOOST_FOREACH( const EG &eg, tree_ ) { Symbol key1 = Symbol(eg.first), key2 = Symbol(eg.second) ; diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 539f102e6..6bb592005 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -16,6 +16,7 @@ #include #include #include +#include namespace gtsam { @@ -31,11 +32,11 @@ bool split(const std::map &M, * linearize: G * T -> L * solve : L -> VectorValues */ -template +template class SubgraphSolver : public IterativeSolver { private: - typedef typename VALUES::Key Key; +// typedef typename VALUES::Key Key; typedef typename GRAPH::Pose Pose; typedef typename GRAPH::Constraint Constraint; @@ -43,7 +44,7 @@ private: typedef boost::shared_ptr shared_ordering ; typedef boost::shared_ptr shared_graph ; typedef boost::shared_ptr shared_linear ; - typedef boost::shared_ptr shared_values ; + typedef boost::shared_ptr shared_values ; typedef boost::shared_ptr shared_preconditioner ; typedef std::map mapPairIndex ; @@ -61,7 +62,7 @@ private: public: - SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters ¶meters = Parameters(), bool useQR = false): + SubgraphSolver(const GRAPH& G, const DynamicValues& theta0, const Parameters ¶meters = Parameters(), bool useQR = false): IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); } SubgraphSolver(const LINEAR& GFG) { @@ -89,7 +90,7 @@ public: shared_ordering ordering() const { return ordering_; } protected: - void initialize(const GRAPH& G, const VALUES& theta0); + void initialize(const GRAPH& G, const DynamicValues& theta0); private: SubgraphSolver():IterativeSolver(){} diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 823853f6d..7d3314d52 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -27,10 +27,10 @@ namespace gtsam { /* ************************************************************************* */ - template - typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_( + template + typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_( const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering, - const VALUES& linearizationPoints, const KEY& lastKey, + const DynamicValues& linearizationPoints, const KEY& lastKey, JacobianFactor::shared_ptr& newPrior) const { // Extract the Index of the provided last key @@ -66,8 +66,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - ExtendedKalmanFilter::ExtendedKalmanFilter(T x_initial, + template + ExtendedKalmanFilter::ExtendedKalmanFilter(T x_initial, noiseModel::Gaussian::shared_ptr P_initial) { // Set the initial linearization point to the provided mean @@ -82,8 +82,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename ExtendedKalmanFilter::T ExtendedKalmanFilter::predict( + template + typename ExtendedKalmanFilter::T ExtendedKalmanFilter::predict( const MotionFactor& motionFactor) { // TODO: This implementation largely ignores the actual factor symbols. @@ -100,7 +100,7 @@ namespace gtsam { ordering.insert(x1, 1); // Create a set of linearization points - VALUES linearizationPoints; + DynamicValues linearizationPoints; linearizationPoints.insert(x0, x_); linearizationPoints.insert(x1, x_); // TODO should this really be x_ ? @@ -122,8 +122,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename ExtendedKalmanFilter::T ExtendedKalmanFilter::update( + template + typename ExtendedKalmanFilter::T ExtendedKalmanFilter::update( const MeasurementFactor& measurementFactor) { // TODO: This implementation largely ignores the actual factor symbols. @@ -138,7 +138,7 @@ namespace gtsam { ordering.insert(x0, 0); // Create a set of linearization points - VALUES linearizationPoints; + DynamicValues linearizationPoints; linearizationPoints.insert(x0, x_); // Create a Gaussian Factor Graph diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index 9717307bf..3bf8c1887 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -39,21 +39,21 @@ namespace gtsam { * TODO: a "predictAndUpdate" that combines both steps for some computational savings. */ - template + template class ExtendedKalmanFilter { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; typedef typename KEY::Value T; - typedef NonlinearFactor2 MotionFactor; - typedef NonlinearFactor1 MeasurementFactor; + typedef NonlinearFactor2 MotionFactor; + typedef NonlinearFactor1 MeasurementFactor; protected: T x_; // linearization point JacobianFactor::shared_ptr priorFactor_; // density 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; public: diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index 16ec47247..b98a0a958 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -29,9 +29,9 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -template -void NonlinearISAM::update(const Factors& newFactors, - const Values& initialValues) { +template +void NonlinearISAM::update(const Factors& newFactors, + const DynamicValues& initialValues) { if(newFactors.size() > 0) { @@ -62,14 +62,14 @@ void NonlinearISAM::update(const Factors& newFactors, } /* ************************************************************************* */ -template -void NonlinearISAM::reorder_relinearize() { +template +void NonlinearISAM::reorder_relinearize() { // cout << "Reordering, relinearizing..." << endl; if(factors_.size() > 0) { // Obtain the new linearization point - const Values newLinPoint = estimate(); + const DynamicValues newLinPoint = estimate(); isam_.clear(); @@ -89,8 +89,8 @@ void NonlinearISAM::reorder_relinearize() { } /* ************************************************************************* */ -template -VALUES NonlinearISAM::estimate() const { +template +DynamicValues NonlinearISAM::estimate() const { if(isam_.size() > 0) return linPoint_.retract(optimize(isam_), ordering_); else @@ -98,14 +98,14 @@ VALUES NonlinearISAM::estimate() const { } /* ************************************************************************* */ -template -Matrix NonlinearISAM::marginalCovariance(const Symbol& key) const { +template +Matrix NonlinearISAM::marginalCovariance(const Symbol& key) const { return isam_.marginalCovariance(ordering_[key]); } /* ************************************************************************* */ -template -void NonlinearISAM::print(const std::string& s) const { +template +void NonlinearISAM::print(const std::string& s) const { cout << "ISAM - " << s << ":" << endl; cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl; isam_.print("GaussianISAM"); diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 8c6d2499d..827bb30e1 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -24,11 +24,10 @@ namespace gtsam { /** * Wrapper class to manage ISAM in a nonlinear context */ -template > +template class NonlinearISAM { public: - typedef VALUES Values; typedef GRAPH Factors; protected: @@ -37,7 +36,7 @@ protected: gtsam::GaussianISAM isam_; /** The current linearization point */ - Values linPoint_; + DynamicValues linPoint_; /** The ordering */ gtsam::Ordering ordering_; @@ -61,10 +60,10 @@ public: NonlinearISAM(int reorderInterval = 1) : reorderInterval_(reorderInterval), reorderCounter_(0) {} /** 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 */ - Values estimate() const; + DynamicValues estimate() const; /** Relinearization and reordering of variables */ void reorder_relinearize(); @@ -84,7 +83,7 @@ public: const GaussianISAM& bayesTree() const { return isam_; } /** Return the current linearization point */ - const Values& getLinearizationPoint() const { return linPoint_; } + const DynamicValues& getLinearizationPoint() const { return linPoint_; } /** Get the ordering */ const gtsam::Ordering& getOrdering() const { return ordering_; } diff --git a/gtsam/nonlinear/NonlinearOptimization-inl.h b/gtsam/nonlinear/NonlinearOptimization-inl.h index 97da0c367..e1896557b 100644 --- a/gtsam/nonlinear/NonlinearOptimization-inl.h +++ b/gtsam/nonlinear/NonlinearOptimization-inl.h @@ -39,7 +39,7 @@ namespace gtsam { Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate); // Create an nonlinear Optimizer that uses a Sequential Solver - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; Optimizer optimizer(boost::make_shared(graph), boost::make_shared(initialEstimate), ordering, boost::make_shared(parameters)); @@ -62,7 +62,7 @@ namespace gtsam { Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate); // Create an nonlinear Optimizer that uses a Multifrontal Solver - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; Optimizer optimizer(boost::make_shared(graph), boost::make_shared(initialEstimate), ordering, boost::make_shared(parameters)); @@ -85,7 +85,7 @@ namespace gtsam { // initial optimization state is the same in both cases tested typedef SubgraphSolver Solver; typedef boost::shared_ptr shared_Solver; - typedef NonlinearOptimizer SPCGOptimizer; + typedef NonlinearOptimizer SPCGOptimizer; shared_Solver solver = boost::make_shared( graph, initialEstimate, IterativeOptimizationParameters()); SPCGOptimizer optimizer( @@ -111,10 +111,10 @@ namespace gtsam { const NonlinearOptimizationMethod& nonlinear_method) { switch (solver) { case SEQUENTIAL: - return optimizeSequential(graph, initialEstimate, parameters, + return optimizeSequential(graph, initialEstimate, parameters, nonlinear_method == LM); case MULTIFRONTAL: - return optimizeMultiFrontal(graph, initialEstimate, parameters, + return optimizeMultiFrontal(graph, initialEstimate, parameters, nonlinear_method == LM); case SPCG: // return optimizeSPCG(graph, initialEstimate, parameters, diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 88110c40e..cc688c597 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -74,10 +74,10 @@ namespace gtsam { }; /// The sequential optimizer - typedef NonlinearOptimizer OptimizerSequential; + typedef NonlinearOptimizer OptimizerSequential; /// The multifrontal optimizer - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; } // pose2SLAM diff --git a/tests/timeMultifrontalOnDataset.cpp b/tests/timeMultifrontalOnDataset.cpp index c4d23ad73..a21096051 100644 --- a/tests/timeMultifrontalOnDataset.cpp +++ b/tests/timeMultifrontalOnDataset.cpp @@ -44,7 +44,7 @@ int main(int argc, char *argv[]) { else if (argc == 4) nrTrials = strtoul(argv[3], NULL, 10); - pair, shared_ptr > data = load2D(dataset(datasetname)); + pair, shared_ptr > data = load2D(dataset(datasetname)); // Add a prior on the first pose if (soft_prior) diff --git a/tests/timeSequentialOnDataset.cpp b/tests/timeSequentialOnDataset.cpp index 063ecd651..8845b901e 100644 --- a/tests/timeSequentialOnDataset.cpp +++ b/tests/timeSequentialOnDataset.cpp @@ -44,7 +44,7 @@ int main(int argc, char *argv[]) { else if (argc == 4) nrTrials = strtoul(argv[3], NULL, 10); - pair, shared_ptr > data = load2D(dataset(datasetname)); + pair, shared_ptr > data = load2D(dataset(datasetname)); // Add a prior on the first pose if (soft_prior)