diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index aee073df7..511e7eb3f 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; + Values 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); + Values 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 d29b21fa9..b71b80876 100644 --- a/examples/PlanarSLAMExample_easy.cpp +++ b/examples/PlanarSLAMExample_easy.cpp @@ -76,7 +76,7 @@ int main(int argc, char** argv) { graph.print("full graph"); // initialize to noisy points - planarSLAM::Values initialEstimate; + 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)); @@ -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); + 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..fd6d2cbfe 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 Values); 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 bb7f60516..398fec627 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 Values); 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(); + Values result = *optimizer_result.values(); result.print("final result"); /* Get covariances */ diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp index ab23436b0..402a3d30f 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; + Values 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); + Values result = optimize(graph, initial); result.print("final result"); diff --git a/examples/Pose2SLAMwSPCG_advanced.cpp b/examples/Pose2SLAMwSPCG_advanced.cpp index a04083986..12d17a6e9 100644 --- a/examples/Pose2SLAMwSPCG_advanced.cpp +++ b/examples/Pose2SLAMwSPCG_advanced.cpp @@ -31,17 +31,17 @@ using namespace gtsam; using namespace pose2SLAM; typedef boost::shared_ptr sharedGraph ; -typedef boost::shared_ptr sharedValue ; +typedef boost::shared_ptr sharedValue ; //typedef NonlinearOptimizer > SPCGOptimizer; -typedef SubgraphSolver Solver; +typedef SubgraphSolver Solver; typedef boost::shared_ptr sharedSolver ; -typedef NonlinearOptimizer SPCGOptimizer; +typedef NonlinearOptimizer SPCGOptimizer; sharedGraph graph; sharedValue initial; -pose2SLAM::Values result; +Values result; /* ************************************************************************* */ int main(void) { @@ -51,7 +51,7 @@ int main(void) { Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9); graph = boost::make_shared() ; - initial = boost::make_shared() ; + initial = boost::make_shared() ; // create a 3 by 3 grid // x3 x6 x9 diff --git a/examples/Pose2SLAMwSPCG_easy.cpp b/examples/Pose2SLAMwSPCG_easy.cpp index 131b19443..e610d327c 100644 --- a/examples/Pose2SLAMwSPCG_easy.cpp +++ b/examples/Pose2SLAMwSPCG_easy.cpp @@ -31,7 +31,7 @@ using namespace gtsam; using namespace pose2SLAM; Graph graph; -pose2SLAM::Values initial, result; +Values initial, result; /* ************************************************************************* */ int main(void) { diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 9d897dd39..821b177dc 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; + Values 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); + Values 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..30d77aedc 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; + Values 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 8027f3088..89ee250f9 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -79,7 +79,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 @@ -100,10 +100,10 @@ void createNewFactors(shared_ptr& newFactors, boost::shared_ptr (new visualSLAM::Values()); - initialValues->insert(pose_id, pose); + initialValues = shared_ptr (new Values()); + 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]); } } @@ -123,7 +123,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 @@ -131,12 +131,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(); + Values currentEstimate = isam.estimate(); cout << "****************************************************" << endl; currentEstimate.print("Current estimate: "); } diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp index 8aaf7be0a..0ecd2e3e9 100644 --- a/examples/vSLAMexample/vSFMexample.cpp +++ b/examples/vSLAMexample/vSFMexample.cpp @@ -102,17 +102,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) { +Values initialize(std::map landmarks, std::map poses) { - visualSLAM::Values initValues; + Values 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; } @@ -135,7 +135,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 Values(initialize(g_landmarks, g_poses))); cout << "*******************************************************" << endl; initialEstimates->print("INITIAL ESTIMATES: "); diff --git a/gtsam.h b/gtsam.h index b8aba73ff..97dea0b31 100644 --- a/gtsam.h +++ b/gtsam.h @@ -427,14 +427,14 @@ class Graph { void addRange(int poseKey, int pointKey, double range, const gtsam::SharedNoiseModel& noiseModel); void addBearingRange(int poseKey, int pointKey, const gtsam::Rot2& bearing, double range, const gtsam::SharedNoiseModel& noiseModel); - planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate); + Values optimize(const Values& initialEstimate); }; class Odometry { Odometry(int key1, int key2, const gtsam::Pose2& measured, const gtsam::SharedNoiseModel& model); void print(string s) const; - gtsam::GaussianFactor* linearize(const planarSLAM::Values& center, const gtsam::Ordering& ordering) const; + gtsam::GaussianFactor* linearize(const Values& center, const Ordering& ordering) const; }; class Optimizer { @@ -637,8 +637,8 @@ class Graph { // GaussianFactor* linearize(const gtsam::Pose2Values& config) const; //}; // -//class gtsam::Pose2Graph{ -// Pose2Graph(); +//class gtsam::pose2SLAM::Graph{ +// pose2SLAM::Graph(); // void print(string s) const; // GaussianFactorGraph* linearize_(const gtsam::Pose2Values& config) const; // void push_back(Pose2Factor* factor); diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h new file mode 100644 index 000000000..f59303675 --- /dev/null +++ b/gtsam/base/DerivedValue.h @@ -0,0 +1,95 @@ +/* + * DerivedValue.h + * + * Created on: Jan 26, 2012 + * Author: thduynguyen + */ + +#pragma once + +#include +#include +namespace gtsam { + +template +class DerivedValue : public Value { +private: + /// Fake Tag struct for singleton pool allocator. In fact, it is never used! + struct PoolTag { }; + +protected: + DerivedValue() {} + +public: + + virtual ~DerivedValue() {} + + /** + * Create a duplicate object returned as a pointer to the generic Value interface. + * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator + */ + virtual Value* clone_() const { + void *place = boost::singleton_pool::malloc(); + DERIVED* ptr = new(place) DERIVED(static_cast(*this)); + return ptr; + } + + /** + * Destroy and deallocate this object, only if it was originally allocated using clone_(). + */ + virtual void deallocate_() const { + this->~Value(); + boost::singleton_pool::free((void*)this); + } + + /// equals implementing generic Value interface + virtual bool equals_(const Value& p, double tol = 1e-9) const { + // Cast the base class Value pointer to a derived class pointer + const DERIVED& derivedValue2 = dynamic_cast(p); + + // Return the result of calling equals on the derived class + return (static_cast(this))->equals(derivedValue2, tol); + } + + /// Generic Value interface version of retract + virtual Value* retract_(const Vector& delta) const { + // Call retract on the derived class + const DERIVED retractResult = (static_cast(this))->retract(delta); + + // Create a Value pointer copy of the result + void* resultAsValuePlace = boost::singleton_pool::malloc(); + Value* resultAsValue = new(resultAsValuePlace) DERIVED(retractResult); + + // Return the pointer to the Value base class + return resultAsValue; + } + + /// Generic Value interface version of localCoordinates + virtual Vector localCoordinates_(const Value& value2) const { + // Cast the base class Value pointer to a derived class pointer + const DERIVED& derivedValue2 = dynamic_cast(value2); + + // Return the result of calling localCoordinates on the derived class + return (static_cast(this))->localCoordinates(derivedValue2); + } + + /// Assignment operator + virtual Value& operator=(const Value& rhs) { + // Cast the base class Value pointer to a derived class pointer + const DERIVED& derivedRhs = dynamic_cast(rhs); + + // Do the assignment and return the result + return (static_cast(this))->operator=(derivedRhs); + } + +protected: + /// Assignment operator, protected because only the Value or DERIVED + /// assignment operators should be used. + DerivedValue& operator=(const DerivedValue& rhs) { + // Nothing to do, do not call base class assignment operator + return *this; + } + +}; + +} /* namespace gtsam */ diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 5a1b3d580..6a6b3408d 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -19,13 +19,14 @@ #include #include +#include namespace gtsam { /** * LieVector is a wrapper around vector to allow it to be a Lie type */ -struct LieVector : public Vector { +struct LieVector : public Vector, public DerivedValue { /** default constructor - should be unnecessary */ LieVector() {} diff --git a/gtsam/base/Makefile.am b/gtsam/base/Makefile.am index 0d4189dd7..aa02ffec9 100644 --- a/gtsam/base/Makefile.am +++ b/gtsam/base/Makefile.am @@ -26,7 +26,7 @@ sources += timing.cpp debug.cpp check_PROGRAMS += tests/testDebug tests/testTestableAssertions # Manifolds and Lie Groups -headers += Manifold.h Group.h +headers += DerivedValue.h Value.h Manifold.h Group.h headers += Lie.h Lie-inl.h lieProxies.h LieScalar.h sources += LieVector.cpp check_PROGRAMS += tests/testLieVector tests/testLieScalar diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h new file mode 100644 index 000000000..8d0b2dd05 --- /dev/null +++ b/gtsam/base/Value.h @@ -0,0 +1,147 @@ +/* ---------------------------------------------------------------------------- + + * 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 Value.h + * @brief The interface class for any variable that can be optimized or used in a factor. + * @author Richard Roberts + * @date Jan 14, 2012 + */ + +#pragma once + +#include + +#include + +namespace gtsam { + + /** + * This is the interface class for any value that may be used as a variable + * assignment in a factor graph, and which you must derive to create new + * variable types to use with gtsam. Examples of built-in classes + * implementing this are mainly in geometry, including Rot3, Pose2, etc. + * + * This interface specifies pure virtual retract_(), localCoordinates_() and + * equals_() functions that work with pointers and references to this interface + * class, i.e. the base class. These functions allow containers, such as + * Values can operate generically on Value objects, retracting or computing + * local coordinates for many Value objects of different types. + * + * When you implement retract_(), localCoordinates_(), and equals_(), we + * suggest first implementing versions of these functions that work directly + * with derived objects, then using the provided helper functions to + * implement the generic Value versions. This makes your implementation + * easier, and also improves performance in situations where the derived type + * is in fact known, such as in most implementations of \c evaluateError() in + * classes derived from NonlinearFactor. + * + * Using the above practice, here is an example of implementing a typical + * class derived from Value: + * \code + class Rot3 : public Value { + public: + // Constructor, there is never a need to call the Value base class constructor. + Rot3() { ... } + + // Print for unit tests and debugging (virtual, implements Value::print()) + virtual void print(const std::string& str = "") const; + + // Equals working directly with Rot3 objects (non-virtual, non-overriding!) + bool equals(const Rot3& other, double tol = 1e-9) const; + + // Tangent space dimensionality (virtual, implements Value::dim()) + virtual size_t dim() const { + return 3; + } + + // retract working directly with Rot3 objects (non-virtual, non-overriding!) + Rot3 retract(const Vector& delta) const { + // Math to implement a 3D rotation retraction e.g. exponential map + return Rot3(result); + } + + // localCoordinates working directly with Rot3 objects (non-virtual, non-overriding!) + Vector localCoordinates(const Rot3& r2) const { + // Math to implement 3D rotation localCoordinates, e.g. logarithm map + return Vector(result); + } + + // Equals implementing the generic Value interface (virtual, implements Value::equals_()) + virtual bool equals_(const Value& other, double tol = 1e-9) const { + // Call our provided helper function to call your Rot3-specific + // equals with appropriate casting. + return CallDerivedEquals(this, other, tol); + } + + // retract implementing the generic Value interface (virtual, implements Value::retract_()) + virtual std::auto_ptr retract_(const Vector& delta) const { + // Call our provided helper function to call your Rot3-specific + // retract and do the appropriate casting and allocation. + return CallDerivedRetract(this, delta); + } + + // localCoordinates implementing the generic Value interface (virtual, implements Value::localCoordinates_()) + virtual Vector localCoordinates_(const Value& value) const { + // Call our provided helper function to call your Rot3-specific + // localCoordinates and do the appropriate casting. + return CallDerivedLocalCoordinates(this, value); + } + }; + \endcode + */ + class Value { + public: + + /** Allocate and construct a clone of this value */ + virtual Value* clone_() const = 0; + + /** Deallocate a raw pointer of this value */ + virtual void deallocate_() const = 0; + + /** Compare this Value with another for equality. */ + virtual bool equals_(const Value& other, double tol = 1e-9) const = 0; + + /** Print this value, for debugging and unit tests */ + virtual void print(const std::string& str = "") const = 0; + + /** Return the dimensionality of the tangent space of this value. This is + * the dimensionality of \c delta passed into retract() and of the vector + * returned by localCoordinates(). + * @return The dimensionality of the tangent space + */ + virtual size_t dim() const = 0; + + /** Increment the value, by mapping from the vector delta in the tangent + * space of the current value back to the manifold to produce a new, + * incremented value. + * @param delta The delta vector in the tangent space of this value, by + * which to increment this value. + */ + virtual Value* retract_(const Vector& delta) const = 0; + + /** Compute the coordinates in the tangent space of this value that + * retract() would map to \c value. + * @param value The value whose coordinates should be determined in the + * tangent space of the value on which this function is called. + * @return The coordinates of \c value in the tangent space of \c this. + */ + virtual Vector localCoordinates_(const Value& value) const = 0; + + /** Assignment operator */ + virtual Value& operator=(const Value& rhs) = 0; + + /** Virutal destructor */ + virtual ~Value() {} + + }; + +} /* namespace gtsam */ diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 9c7561f61..65dd1a19a 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -18,6 +18,7 @@ #pragma once +#include #include namespace gtsam { @@ -27,7 +28,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ -class Cal3Bundler { +class Cal3Bundler : public DerivedValue { private: double f_, k1_, k2_ ; @@ -94,7 +95,7 @@ public: Vector localCoordinates(const Cal3Bundler& T2) const ; ///TODO: comment - int dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + virtual size_t dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) ///TODO: comment static size_t Dim() { return 3; } //TODO: make a final dimension variable diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 437a68a68..0044f1c35 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -18,6 +18,7 @@ #pragma once +#include #include namespace gtsam { @@ -27,7 +28,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ -class Cal3DS2 { +class Cal3DS2 : public DerivedValue { private: @@ -97,7 +98,7 @@ public: Vector localCoordinates(const Cal3DS2& T2) const ; ///TODO: comment - int dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + virtual size_t dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) ///TODO: comment static size_t Dim() { return 9; } //TODO: make a final dimension variable diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index d7640cc15..910994df7 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -21,6 +21,7 @@ #pragma once +#include #include namespace gtsam { @@ -30,7 +31,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class Cal3_S2 { + class Cal3_S2 : public DerivedValue { private: double fx_, fy_, s_, u0_, v0_; diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 21fe526a1..5458b88f0 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include @@ -35,7 +36,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class CalibratedCamera { + class CalibratedCamera : public DerivedValue { private: Pose3 pose_; // 6DOF pose @@ -61,9 +62,10 @@ namespace gtsam { /// @name Testable /// @{ - void print(const std::string& s="") const { - pose_.print(); - } + virtual void print(const std::string& s = "") const { + pose_.print(s); + } + /// check equality to another camera bool equals (const CalibratedCamera &camera, double tol = 1e-9) const { return pose_.equals(camera.pose(), tol) ; diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index b8c6428bb..7d6cb218d 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -35,7 +36,7 @@ namespace gtsam { * \nosubgrouping */ template - class PinholeCamera { + class PinholeCamera : public DerivedValue > { private: Pose3 pose_; Calibration k_; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index fe86cc865..898307334 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -18,6 +18,8 @@ #pragma once #include + +#include #include #include @@ -30,7 +32,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ -class Point2 { +class Point2 : public DerivedValue { public: /// dimension of the variable - used to autodetect sizes static const size_t dimension = 2; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index c2667d3b4..8233b341a 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -24,6 +24,7 @@ #include #include +#include #include namespace gtsam { @@ -33,7 +34,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class Point3 { + class Point3 : public DerivedValue { public: /// dimension of the variable - used to autodetect sizes static const size_t dimension = 3; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 3a8f69899..2bcf65181 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -32,7 +33,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ -class Pose2 { +class Pose2 : public DerivedValue { public: static const size_t dimension = 3; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 640016970..f052d86f1 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -22,6 +22,7 @@ #define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER #endif +#include #include #include @@ -34,7 +35,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class Pose3 { + class Pose3 : public DerivedValue { public: static const size_t dimension = 6; diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index f3062f5c1..2adbae4bd 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -19,6 +19,8 @@ #pragma once #include + +#include #include namespace gtsam { @@ -29,7 +31,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class Rot2 { + class Rot2 : public DerivedValue { public: /** get the dimension by the type */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 2e16f869c..b640ac615 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -32,6 +32,7 @@ #endif #endif +#include #include #include @@ -49,7 +50,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class Rot3 { + class Rot3 : public DerivedValue { public: static const size_t dimension = 3; @@ -92,6 +93,9 @@ namespace gtsam { */ Rot3(const Quaternion& q); + /** Virtual destructor */ + virtual ~Rot3() {} + /* Static member function to generate some well known rotations */ /// Rotation around X axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index cd3dbd19e..f5e07e66c 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -18,6 +18,8 @@ #pragma once #include + +#include #include #include #include diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 0bdf37e0e..63538b6d5 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -18,6 +18,7 @@ #pragma once +#include #include namespace gtsam { @@ -27,7 +28,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class StereoPoint2 { + class StereoPoint2 : public DerivedValue { public: static const size_t dimension = 3; private: diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 5692d9a92..be4ddf840 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -139,19 +139,19 @@ predecessorMap2Graph(const PredecessorMap& p_map) { } /* ************************************************************************* */ -template +template class compose_key_visitor : public boost::default_bfs_visitor { private: - boost::shared_ptr config_; + boost::shared_ptr config_; public: - compose_key_visitor(boost::shared_ptr config_in) {config_ = config_in;} + compose_key_visitor(boost::shared_ptr config_in) {config_ = config_in;} template void tree_edge(Edge edge, const Graph& g) const { - typename VALUES::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); - typename VALUES::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g)); + KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); + KEY key_to = boost::get(boost::vertex_name, g, boost::target(edge, g)); POSE relativePose = boost::get(boost::edge_weight, g, edge); config_->insert(key_to, (*config_)[key_from].compose(relativePose)); } @@ -159,23 +159,23 @@ public: }; /* ************************************************************************* */ -template -boost::shared_ptr composePoses(const G& graph, const PredecessorMap& tree, +template +boost::shared_ptr composePoses(const G& graph, const PredecessorMap& tree, const POSE& rootPose) { //TODO: change edge_weight_t to edge_pose_t typedef typename boost::adjacency_list< boost::vecS, boost::vecS, boost::directedS, - boost::property, + boost::property, boost::property > PoseGraph; typedef typename boost::graph_traits::vertex_descriptor PoseVertex; typedef typename boost::graph_traits::edge_descriptor PoseEdge; PoseGraph g; PoseVertex root; - map key2vertex; + map key2vertex; boost::tie(g, root, key2vertex) = - predecessorMap2Graph(tree); + predecessorMap2Graph(tree); // attach the relative poses to the edges PoseEdge edge12, edge21; @@ -189,8 +189,8 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap factor = boost::dynamic_pointer_cast(nl_factor); if (!factor) continue; - typename VALUES::Key key1 = factor->key1(); - typename VALUES::Key key2 = factor->key2(); + KEY key1 = factor->key1(); + KEY key2 = factor->key2(); PoseVertex v1 = key2vertex.find(key1)->second; PoseVertex v2 = key2vertex.find(key2)->second; @@ -207,10 +207,10 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap config(new VALUES); - typename VALUES::Key rootKey = boost::get(boost::vertex_name, g, root); + boost::shared_ptr config(new Values); + KEY rootKey = boost::get(boost::vertex_name, g, root); config->insert(rootKey, rootPose); - compose_key_visitor vis(config); + compose_key_visitor vis(config); boost::breadth_first_search(g, root, boost::visitor(vis)); return config; diff --git a/gtsam/inference/graph.h b/gtsam/inference/graph.h index 7a34159ca..98d41940b 100644 --- a/gtsam/inference/graph.h +++ b/gtsam/inference/graph.h @@ -25,6 +25,7 @@ #include #include #include +#include namespace gtsam { @@ -87,9 +88,9 @@ namespace gtsam { /** * Compose the poses by following the chain specified by the spanning tree */ - template - boost::shared_ptr - composePoses(const G& graph, const PredecessorMap& tree, const POSE& rootPose); + template + boost::shared_ptr + composePoses(const G& graph, const PredecessorMap& tree, const POSE& rootPose); /** diff --git a/gtsam/linear/SubgraphSolver-inl.h b/gtsam/linear/SubgraphSolver-inl.h index 4aa12532e..9d53c5741 100644 --- a/gtsam/linear/SubgraphSolver-inl.h +++ b/gtsam/linear/SubgraphSolver-inl.h @@ -21,8 +21,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(); @@ -47,8 +47,8 @@ void SubgraphSolver::replaceFactors(const typename LINEAR:: Ab1->dynamicCastFactors >(), Ab2->dynamicCastFactors >(),Rc1,xbar); } -template -VectorValues::shared_ptr SubgraphSolver::optimize() { +template +VectorValues::shared_ptr SubgraphSolver::optimize() const { // preconditioned conjugate gradient VectorValues zeros = pc_->zero(); @@ -60,18 +60,18 @@ VectorValues::shared_ptr SubgraphSolver::optimize() { return xbar; } -template -void SubgraphSolver::initialize(const GRAPH& G, const VALUES& theta0) { +template +void SubgraphSolver::initialize(const GRAPH& G, const Values& 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 9e0f0a9d7..9ae669931 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 Values& theta0, const Parameters ¶meters = Parameters(), bool useQR = false): IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); } SubgraphSolver(const LINEAR& GFG) { @@ -85,11 +86,11 @@ public: IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc), useQR_(useQR) {} void replaceFactors(const typename LINEAR::shared_ptr &graph); - VectorValues::shared_ptr optimize() ; + VectorValues::shared_ptr optimize() const; shared_ordering ordering() const { return ordering_; } protected: - void initialize(const GRAPH& G, const VALUES& theta0); + void initialize(const GRAPH& G, const Values& theta0); private: SubgraphSolver():IterativeSolver(){} diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 33351ceb7..0be2cad19 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -175,7 +175,7 @@ namespace gtsam { /// @name Advanced Constructors /// @{ - /** Construct from a container of variable dimensions (in variable order). */ + /** Construct from a container of variable dimensions (in variable order), without initializing any values. */ template VectorValues(const CONTAINER& dimensions) { append(dimensions); } diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index daab0c66e..5923baa18 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -7,7 +7,7 @@ #include #include // To get optimize(BayesTree) -#include +//#include #include namespace gtsam { diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 823853f6d..2c4e382a1 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 Values& 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; + Values 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; + Values linearizationPoints; linearizationPoints.insert(x0, x_); // Create a Gaussian Factor Graph diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index 8f88979a1..6e85d6e27 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -40,21 +40,21 @@ namespace gtsam { * \nosubgrouping */ - 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 Values& linearizationPoints, const KEY& x, JacobianFactor::shared_ptr& newPrior) const; public: diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index 26049ec69..d1a4bef9f 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -36,19 +36,19 @@ namespace gtsam { * variables. * @tparam GRAPH The NonlinearFactorGraph structure to store factors. Defaults to standard NonlinearFactorGraph */ -template > -class GaussianISAM2 : public ISAM2 { - typedef ISAM2 Base; +template +class GaussianISAM2 : public ISAM2 { + typedef ISAM2 Base; public: /// @name Standard Constructors /// @{ /** Create an empty ISAM2 instance */ - GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} + GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ - GaussianISAM2() : ISAM2() {} + GaussianISAM2() : ISAM2() {} /// @} /// @name Advanced Interface diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index fc48720f9..eae39bf52 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -19,10 +19,10 @@ namespace gtsam { using namespace std; -template -struct ISAM2::Impl { +template +struct ISAM2::Impl { - typedef ISAM2 ISAM2Type; + typedef ISAM2 ISAM2Type; struct PartialSolveResult { typename ISAM2Type::sharedClique bayesTree; @@ -45,7 +45,7 @@ struct ISAM2::Impl { * @param ordering Current ordering to be augmented with new variables * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables */ - static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes); + static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes); /** * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol @@ -95,7 +95,7 @@ struct ISAM2::Impl { * where we might expmap something twice, or expmap it but then not * recalculate its delta. */ - static void ExpmapMasked(VALUES& values, const Permuted& delta, + static void ExpmapMasked(Values& values, const Permuted& delta, const Ordering& ordering, const std::vector& mask, boost::optional&> invalidateIfDebug = boost::optional&>()); @@ -118,25 +118,9 @@ struct ISAM2::Impl { }; /* ************************************************************************* */ -struct _VariableAdder { - Ordering& ordering; - Permuted& vconfig; - Index nextVar; - _VariableAdder(Ordering& _ordering, Permuted& _vconfig, Index _nextVar) : ordering(_ordering), vconfig(_vconfig), nextVar(_nextVar){} - template - void operator()(I xIt) { - const bool debug = ISDEBUG("ISAM2 AddVariables"); - vconfig.permutation()[nextVar] = nextVar; - ordering.insert(xIt->first, nextVar); - if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << nextVar << endl; - ++ nextVar; - } -}; - -/* ************************************************************************* */ -template -void ISAM2::Impl::AddVariables( - const VALUES& newTheta, VALUES& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes) { +template +void ISAM2::Impl::AddVariables( + const Values& newTheta, Values& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes) { const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); @@ -151,8 +135,13 @@ void ISAM2::Impl::AddVariables( delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); delta.permutation().resize(originalnVars + newTheta.size()); { - _VariableAdder vadder(ordering, delta, originalnVars); - newTheta.apply(vadder); + Index nextVar = originalnVars; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { + delta.permutation()[nextVar] = nextVar; + ordering.insert(key_value.first, nextVar); + if(debug) cout << "Adding variable " << (string)key_value.first << " with order " << nextVar << endl; + ++ nextVar; + } assert(delta.permutation().size() == delta.container().size()); assert(ordering.nVars() == delta.size()); assert(ordering.size() == delta.size()); @@ -162,10 +151,10 @@ void ISAM2::Impl::AddVariables( } /* ************************************************************************* */ -template -FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) { +template +FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) { FastSet indices; - BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) { + BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) { BOOST_FOREACH(const Symbol& key, factor->keys()) { indices.insert(ordering[key]); } @@ -174,8 +163,8 @@ FastSet ISAM2::Impl::IndicesFromFactors(const O } /* ************************************************************************* */ -template -FastSet ISAM2::Impl::CheckRelinearization(const Permuted& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { +template +FastSet ISAM2::Impl::CheckRelinearization(const Permuted& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { FastSet relinKeys; if(relinearizeThreshold.type() == typeid(double)) { @@ -202,8 +191,8 @@ FastSet ISAM2::Impl::CheckRelinearization(const } /* ************************************************************************* */ -template -void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { +template +void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { static const bool debug = false; // does the separator contain any of the variables? bool found = false; @@ -223,41 +212,8 @@ void ISAM2::Impl::FindAll(typename ISAM2Clique& delta; - const Ordering& ordering; - const vector& mask; - const boost::optional&> invalidate; - _SelectiveExpmapAndClear(const Permuted& _delta, const Ordering& _ordering, const vector& _mask, boost::optional&> _invalidate) : - delta(_delta), ordering(_ordering), mask(_mask), invalidate(_invalidate) {} - template - void operator()(I it_x) { - Index var = ordering[it_x->first]; - if(ISDEBUG("ISAM2 update verbose")) { - if(mask[var]) - cout << "expmap " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; - else - cout << " " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; - } - assert(delta[var].size() == (int)it_x->second.dim()); - assert(delta[var].unaryExpr(&isfinite).all()); - if(mask[var]) { - it_x->second = it_x->second.retract(delta[var]); - if(invalidate) - (*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) - } - } -}; - -/* ************************************************************************* */ -template -void ISAM2::Impl::ExpmapMasked(VALUES& values, const Permuted& delta, +template +void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted& delta, const Ordering& ordering, const vector& mask, boost::optional&> invalidateIfDebug) { // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions @@ -266,14 +222,35 @@ void ISAM2::Impl::ExpmapMasked(VALUES& values, const invalidateIfDebug = boost::optional&>(); #endif - _SelectiveExpmapAndClear selectiveExpmapper(delta, ordering, mask, invalidateIfDebug); - values.apply(selectiveExpmapper); + assert(values.size() == ordering.size()); + Values::iterator key_value; + Ordering::const_iterator key_index; + for(key_value = values.begin(), key_index = ordering.begin(); + key_value != values.end() && key_index != ordering.end(); ++key_value, ++key_index) { + assert(key_value->first == key_index->first); + const Index var = key_index->second; + if(ISDEBUG("ISAM2 update verbose")) { + if(mask[var]) + cout << "expmap " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; + else + cout << " " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; + } + assert(delta[var].size() == (int)key_value->second.dim()); + assert(delta[var].unaryExpr(&isfinite).all()); + if(mask[var]) { + Value* retracted = key_value->second.retract_(delta[var]); + key_value->second = *retracted; + retracted->deallocate_(); + if(invalidateIfDebug) + (*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) + } + } } /* ************************************************************************* */ -template -typename ISAM2::Impl::PartialSolveResult -ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, +template +typename ISAM2::Impl::PartialSolveResult +ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode) { static const bool debug = ISDEBUG("ISAM2 recalculate"); diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 8a2fc6e35..5c87d1dda 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -39,8 +39,8 @@ static const bool disableReordering = false; static const double batchThreshold = 0.65; /* ************************************************************************* */ -template -ISAM2::ISAM2(const ISAM2Params& params): +template +ISAM2::ISAM2(const ISAM2Params& params): delta_(Permutation(), deltaUnpermuted_), params_(params) { // See note in gtsam/base/boost_variant_with_workaround.h if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) @@ -48,8 +48,8 @@ ISAM2::ISAM2(const ISAM2Params& params): } /* ************************************************************************* */ -template -ISAM2::ISAM2(): +template +ISAM2::ISAM2(): delta_(Permutation(), deltaUnpermuted_) { // See note in gtsam/base/boost_variant_with_workaround.h if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) @@ -57,14 +57,14 @@ ISAM2::ISAM2(): } /* ************************************************************************* */ -template -FastList ISAM2::getAffectedFactors(const FastList& keys) const { +template +FastList ISAM2::getAffectedFactors(const FastList& keys) const { static const bool debug = false; if(debug) cout << "Getting affected factors for "; if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } } if(debug) cout << endl; - FactorGraph > allAffected; + FactorGraph allAffected; FastList indices; BOOST_FOREACH(const Index key, keys) { // const list l = nonlinearFactors_.factors(key); @@ -86,9 +86,9 @@ FastList ISAM2::getAffectedFactors(const Fas /* ************************************************************************* */ // retrieve all factors that ONLY contain the affected variables // (note that the remaining stuff is summarized in the cached factors) -template +template FactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { +ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { tic(1,"getAffectedFactors"); FastList candidates = getAffectedFactors(affectedKeys); @@ -125,9 +125,9 @@ ISAM2::relinearizeAffectedFactors(const FastList -FactorGraph::CacheFactor> -ISAM2::getCachedBoundaryFactors(Cliques& orphans) { +template +FactorGraph::CacheFactor> +ISAM2::getCachedBoundaryFactors(Cliques& orphans) { static const bool debug = false; @@ -151,8 +151,8 @@ ISAM2::getCachedBoundaryFactors(Cliques& orphans) { return cachedBoundary; } -template -boost::shared_ptr > ISAM2::recalculate( +template +boost::shared_ptr > ISAM2::recalculate( const FastSet& markedKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, const boost::optional >& constrainKeys, ISAM2Result& result) { @@ -395,8 +395,8 @@ boost::shared_ptr > ISAM2::recalculat } /* ************************************************************************* */ -template -ISAM2Result ISAM2::update( +template +ISAM2Result ISAM2::update( const GRAPH& newFactors, const Values& newTheta, const FastVector& removeFactorIndices, const boost::optional >& constrainedKeys, bool force_relinearize) { @@ -579,36 +579,36 @@ ISAM2Result ISAM2::update( } /* ************************************************************************* */ -template -VALUES ISAM2::calculateEstimate() const { +template +Values ISAM2::calculateEstimate() const { // We use ExpmapMasked here instead of regular expmap because the former // handles Permuted - VALUES ret(theta_); + Values ret(theta_); vector mask(ordering_.nVars(), true); Impl::ExpmapMasked(ret, delta_, ordering_, mask); return ret; } /* ************************************************************************* */ -template +template template -typename KEY::Value ISAM2::calculateEstimate(const KEY& key) const { +typename KEY::Value ISAM2::calculateEstimate(const KEY& key) const { const Index index = getOrdering()[key]; const SubVector delta = getDelta()[index]; return getLinearizationPoint()[key].retract(delta); } /* ************************************************************************* */ -template -VALUES ISAM2::calculateBestEstimate() const { +template +Values ISAM2::calculateBestEstimate() const { VectorValues delta(theta_.dims(ordering_)); optimize2(this->root(), delta); return theta_.retract(delta, ordering_); } /* ************************************************************************* */ -template -VectorValues optimize(const ISAM2& isam) { +template +VectorValues optimize(const ISAM2& isam) { VectorValues delta = *allocateVectorValues(isam); optimize2(isam.root(), delta); return delta; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index a97d9d4ed..1bde623a8 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -266,13 +266,13 @@ private: * estimate of all variables. * */ -template > +template class ISAM2: public BayesTree > { protected: /** The current linearization point */ - VALUES theta_; + Values theta_; /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */ VariableIndex variableIndex_; @@ -314,8 +314,8 @@ private: public: typedef BayesTree > Base; ///< The BayesTree base class - typedef ISAM2 This; ///< This class - typedef VALUES Values; + typedef ISAM2 This; ///< This class + typedef Values Values; typedef GRAPH Graph; /** Create an empty ISAM2 instance */ @@ -368,19 +368,19 @@ public: * (Params::relinearizeSkip). * @return An ISAM2Result struct containing information about the update */ - ISAM2Result update(const GRAPH& newFactors = GRAPH(), const VALUES& newTheta = VALUES(), + ISAM2Result update(const GRAPH& newFactors = GRAPH(), const Values& newTheta = Values(), const FastVector& removeFactorIndices = FastVector(), const boost::optional >& constrainedKeys = boost::none, bool force_relinearize = false); /** Access the current linearization point */ - const VALUES& getLinearizationPoint() const {return theta_;} + const Values& getLinearizationPoint() const {return theta_;} /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only * a single variable is needed, it is faster to call calculateEstimate(const KEY&). */ - VALUES calculateEstimate() const; + Values calculateEstimate() const; /** Compute an estimate for a single variable using its incomplete linear delta computed * during the last update. This is faster than calling the no-argument version of @@ -399,7 +399,7 @@ public: /** Compute an estimate using a complete delta computed by a full back-substitution. */ - VALUES calculateBestEstimate() const; + Values calculateBestEstimate() const; /** Access the current delta, computed during the last call to update */ const Permuted& getDelta() const { return delta_; } @@ -435,8 +435,8 @@ private: }; // ISAM2 /** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ -template -VectorValues optimize(const ISAM2& isam); +template +VectorValues optimize(const ISAM2& isam); } /// namespace gtsam diff --git a/gtsam/nonlinear/Key.h b/gtsam/nonlinear/Key.h index 1e28f51e6..9bff0511d 100644 --- a/gtsam/nonlinear/Key.h +++ b/gtsam/nonlinear/Key.h @@ -31,6 +31,9 @@ namespace gtsam { + // Forward declarations + class Symbol; + /** * TypedSymbol key class is templated on * 1) the type T it is supposed to retrieve, for extra type checking @@ -76,6 +79,7 @@ public: std::string latex() const { return (boost::format("%c_{%d}") % C % j_).str(); } + Symbol symbol() const; // logic: @@ -298,6 +302,9 @@ public: std::string label_s = (boost::format("%1%") % label_).str(); return (boost::format("%c%s_{%d}") % C % label_s % this->j_).str(); } + Symbol symbol() const { + return Symbol(*this); + } // Needed for conversion to LabeledSymbol size_t convertLabel() const { @@ -354,5 +361,11 @@ private: } }; +/* ************************************************************************* */ +template +Symbol TypedSymbol::symbol() const { + return Symbol(*this); +} + } // namespace gtsam diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index 6d5f126c8..76a41247c 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -16,15 +16,16 @@ check_PROGRAMS = #---------------------------------------------------------------------------------------------------- # Lie Groups -headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h +headers += Values-inl.h +sources += Values.cpp check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering # Nonlinear nonlinear headers += Key.h -headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h +headers += NonlinearFactorGraph.h headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h headers += NonlinearFactor.h -sources += NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp NonlinearOptimizationParameters.cpp +sources += NonlinearFactorGraph.cpp NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp NonlinearOptimizationParameters.cpp headers += DoglegOptimizer.h DoglegOptimizer-inl.h # Nonlinear iSAM(2) @@ -62,7 +63,7 @@ AM_CXXFLAGS = #---------------------------------------------------------------------------------------------------- TESTS = $(check_PROGRAMS) AM_LDFLAGS += $(boost_serialization) -LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../base/libbase.la ../3rdparty/libccolamd.la +LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../geometry/libgeometry.la ../base/libbase.la ../3rdparty/libccolamd.la LDADD += ../../CppUnitLite/libCppUnitLite.a AM_DEFAULT_SOURCE_EXT = .cpp diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 7537ea192..e01623601 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -47,8 +47,8 @@ namespace gtsam { * * \nosubgrouping */ - template - class NonlinearEquality: public NonlinearFactor1 { + template + class NonlinearEquality: public NonlinearFactor1 { public: typedef typename KEY::Value T; @@ -71,7 +71,7 @@ namespace gtsam { */ bool (*compare_)(const T& a, const T& b); - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 Base; /** default constructor - only for serialization */ NonlinearEquality() {} @@ -110,7 +110,7 @@ namespace gtsam { } /** Check if two factors are equal */ - bool equals(const NonlinearEquality& f, double tol = 1e-9) const { + bool equals(const NonlinearEquality& f, double tol = 1e-9) const { if (!Base::equals(f)) return false; return feasible_.equals(f.feasible_, tol) && fabs(error_gain_ - f.error_gain_) < tol; @@ -121,7 +121,7 @@ namespace gtsam { /// @{ /** actual error function calculation */ - virtual double error(const VALUES& c) const { + virtual double error(const Values& c) const { const T& xj = c[this->key_]; Vector e = this->unwhitenedError(c); if (allow_error_ || !compare_(xj, feasible_)) { @@ -148,7 +148,7 @@ namespace gtsam { } // Linearize is over-written, because base linearization tries to whiten - virtual GaussianFactor::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const { + virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const { const T& xj = x[this->key_]; Matrix A; Vector b = evaluateError(xj, A); @@ -177,14 +177,14 @@ namespace gtsam { /** * Simple unary equality constraint - fixes a value for a variable */ - template - class NonlinearEquality1 : public NonlinearFactor1 { + template + class NonlinearEquality1 : public NonlinearFactor1 { public: typedef typename KEY::Value X; protected: - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 Base; /** default constructor to allow for serialization */ NonlinearEquality1() {} @@ -196,7 +196,7 @@ namespace gtsam { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; ///TODO: comment NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0) @@ -236,13 +236,13 @@ namespace gtsam { * Simple binary equality constraint - this constraint forces two factors to * be the same. */ - template - class NonlinearEquality2 : public NonlinearFactor2 { + template + class NonlinearEquality2 : public NonlinearFactor2 { public: typedef typename KEY::Value X; protected: - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; GTSAM_CONCEPT_MANIFOLD_TYPE(X); @@ -251,7 +251,7 @@ namespace gtsam { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; ///TODO: comment NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index ad137a5d9..07334eac9 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -31,6 +31,7 @@ #include #include +#include #include namespace gtsam { @@ -57,18 +58,17 @@ inline void __fill_from_tuple(std::vector& vec * which are objects in non-linear manifolds (Lie groups). * \nosubgrouping */ -template class NonlinearFactor: public Factor { protected: // Some handy typedefs typedef Factor Base; - typedef NonlinearFactor This; + typedef NonlinearFactor This; public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ @@ -120,7 +120,7 @@ public: * This is typically equal to log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian. * You can override this for systems with unusual noise models. */ - virtual double error(const VALUES& c) const = 0; + virtual double error(const Values& c) const = 0; /** get the dimension of the factor (number of rows on linearization) */ virtual size_t dim() const = 0; @@ -135,11 +135,11 @@ public: * when the constraint is *NOT* fulfilled. * @return true if the constraint is active */ - virtual bool active(const VALUES& c) const { return true; } + virtual bool active(const Values& c) const { return true; } /** linearize to a GaussianFactor */ virtual boost::shared_ptr - linearize(const VALUES& c, const Ordering& ordering) const = 0; + linearize(const Values& c, const Ordering& ordering) const = 0; /** * Create a symbolic factor using the given ordering to determine the @@ -165,20 +165,19 @@ public: * The noise model is typically Gaussian, but robust and constrained error models are also supported. */ -template -class NoiseModelFactor: public NonlinearFactor { +class NoiseModelFactor: public NonlinearFactor { protected: // handy typedefs - typedef NonlinearFactor Base; - typedef NoiseModelFactor This; + typedef NonlinearFactor Base; + typedef NoiseModelFactor This; SharedNoiseModel noiseModel_; /** Noise model */ public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr shared_ptr; /** Default constructor for I/O only */ NoiseModelFactor() { @@ -225,7 +224,7 @@ public: } /** Check if two factors are equal */ - virtual bool equals(const NoiseModelFactor& f, double tol = 1e-9) const { + virtual bool equals(const NoiseModelFactor& f, double tol = 1e-9) const { return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, tol); } @@ -245,13 +244,13 @@ public: * If any of the optional Matrix reference arguments are specified, it should compute * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...). */ - virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const = 0; + virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const = 0; /** * Vector of errors, whitened * This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian */ - Vector whitenedError(const VALUES& c) const { + Vector whitenedError(const Values& c) const { return noiseModel_->whiten(unwhitenedError(c)); } @@ -261,7 +260,7 @@ public: * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. */ - virtual double error(const VALUES& c) const { + virtual double error(const Values& c) const { if (this->active(c)) return 0.5 * noiseModel_->distance(unwhitenedError(c)); else @@ -273,7 +272,7 @@ public: * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ - boost::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const { + boost::shared_ptr linearize(const Values& x, const Ordering& ordering) const { // Only linearize if the factor is active if (!this->active(x)) return boost::shared_ptr(); @@ -321,8 +320,8 @@ private: /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 1 * variable. To derive from this class, implement evaluateError(). */ -template -class NonlinearFactor1: public NoiseModelFactor { +template +class NonlinearFactor1: public NoiseModelFactor { public: @@ -334,8 +333,8 @@ protected: // The value of the key. Not const to allow serialization KEY key_; - typedef NoiseModelFactor Base; - typedef NonlinearFactor1 This; + typedef NoiseModelFactor Base; + typedef NonlinearFactor1 This; public: @@ -357,7 +356,7 @@ public: /** Calls the 1-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { const X& x1 = x[key_]; if(H) { @@ -400,8 +399,8 @@ private: /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 2 * variables. To derive from this class, implement evaluateError(). */ -template -class NonlinearFactor2: public NoiseModelFactor { +template +class NonlinearFactor2: public NoiseModelFactor { public: @@ -415,8 +414,8 @@ protected: KEY1 key1_; KEY2 key2_; - typedef NoiseModelFactor Base; - typedef NonlinearFactor2 This; + typedef NoiseModelFactor Base; + typedef NonlinearFactor2 This; public: @@ -441,7 +440,7 @@ public: /** Calls the 2-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { const X1& x1 = x[key1_]; const X2& x2 = x[key2_]; @@ -488,8 +487,8 @@ private: /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 3 * variables. To derive from this class, implement evaluateError(). */ -template -class NonlinearFactor3: public NoiseModelFactor { +template +class NonlinearFactor3: public NoiseModelFactor { public: @@ -505,8 +504,8 @@ protected: KEY2 key2_; KEY3 key3_; - typedef NoiseModelFactor Base; - typedef NonlinearFactor3 This; + typedef NoiseModelFactor Base; + typedef NonlinearFactor3 This; public: @@ -533,7 +532,7 @@ public: /** Calls the 3-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { if(H) return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]); @@ -582,8 +581,8 @@ private: /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 4 * variables. To derive from this class, implement evaluateError(). */ -template -class NonlinearFactor4: public NoiseModelFactor { +template +class NonlinearFactor4: public NoiseModelFactor { public: @@ -601,8 +600,8 @@ protected: KEY3 key3_; KEY4 key4_; - typedef NoiseModelFactor Base; - typedef NonlinearFactor4 This; + typedef NoiseModelFactor Base; + typedef NonlinearFactor4 This; public: @@ -631,7 +630,7 @@ public: /** Calls the 4-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { if(H) return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]); @@ -682,8 +681,8 @@ private: /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 5 * variables. To derive from this class, implement evaluateError(). */ -template -class NonlinearFactor5: public NoiseModelFactor { +template +class NonlinearFactor5: public NoiseModelFactor { public: @@ -703,8 +702,8 @@ protected: KEY4 key4_; KEY5 key5_; - typedef NoiseModelFactor Base; - typedef NonlinearFactor5 This; + typedef NoiseModelFactor Base; + typedef NonlinearFactor5 This; public: @@ -735,7 +734,7 @@ public: /** Calls the 5-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { if(H) return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); @@ -789,8 +788,8 @@ private: /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 6 * variables. To derive from this class, implement evaluateError(). */ -template -class NonlinearFactor6: public NoiseModelFactor { +template +class NonlinearFactor6: public NoiseModelFactor { public: @@ -812,8 +811,8 @@ protected: KEY5 key5_; KEY6 key6_; - typedef NoiseModelFactor Base; - typedef NonlinearFactor6 This; + typedef NoiseModelFactor Base; + typedef NonlinearFactor6 This; public: @@ -846,7 +845,7 @@ public: /** Calls the 6-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { if(H) return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); diff --git a/gtsam/nonlinear/NonlinearFactorGraph-inl.h b/gtsam/nonlinear/NonlinearFactorGraph.cpp similarity index 78% rename from gtsam/nonlinear/NonlinearFactorGraph-inl.h rename to gtsam/nonlinear/NonlinearFactorGraph.cpp index 473107429..11f8e84d9 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph-inl.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -10,39 +10,35 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearFactorGraph-inl.h + * @file NonlinearFactorGraph.cpp * @brief Factor Graph Consisting of non-linear factors * @author Frank Dellaert * @author Carlos Nieto * @author Christian Potthast */ -#pragma once - +#include +#include #include #include -#include -#include +#include using namespace std; namespace gtsam { /* ************************************************************************* */ - template - double NonlinearFactorGraph::probPrime(const VALUES& c) const { + double NonlinearFactorGraph::probPrime(const Values& c) const { return exp(-0.5 * error(c)); } /* ************************************************************************* */ - template - void NonlinearFactorGraph::print(const std::string& str) const { + void NonlinearFactorGraph::print(const std::string& str) const { Base::print(str); } /* ************************************************************************* */ - template - double NonlinearFactorGraph::error(const VALUES& c) const { + double NonlinearFactorGraph::error(const Values& c) const { double total_error = 0.; // iterate over all the factors_ to accumulate the log probabilities BOOST_FOREACH(const sharedFactor& factor, this->factors_) { @@ -53,8 +49,7 @@ namespace gtsam { } /* ************************************************************************* */ - template - std::set NonlinearFactorGraph::keys() const { + std::set NonlinearFactorGraph::keys() const { std::set keys; BOOST_FOREACH(const sharedFactor& factor, this->factors_) { if(factor) @@ -64,9 +59,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD( - const VALUES& config) const { + Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD( + const Values& config) const { // Create symbolic graph and initial (iterator) ordering SymbolicFactorGraph::shared_ptr symbolic; @@ -91,8 +85,7 @@ namespace gtsam { } /* ************************************************************************* */ - template - SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const { + SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const { // Generate the symbolic factor graph SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph); symbolicfg->reserve(this->size()); @@ -108,21 +101,19 @@ namespace gtsam { } /* ************************************************************************* */ - template - pair NonlinearFactorGraph< - VALUES>::symbolic(const VALUES& config) const { + pair NonlinearFactorGraph::symbolic( + const Values& config) const { // Generate an initial key ordering in iterator order Ordering::shared_ptr ordering(config.orderingArbitrary()); return make_pair(symbolic(*ordering), ordering); } /* ************************************************************************* */ - template - typename GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize( - const VALUES& config, const Ordering& ordering) const { + GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize( + const Values& config, const Ordering& ordering) const { // create an empty linear FG - typename GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph); + GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph); linearFG->reserve(this->size()); // linearize all factors diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index b572c7d84..fa87baedf 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -28,42 +28,37 @@ namespace gtsam { /** - * A non-linear factor graph is templated on a values structure, but the factor type - * is fixed as a NonlinearFactor. The values structures are typically (in SAM) more general + * A non-linear factor graph is a graph of non-Gaussian, i.e. non-linear factors, + * which derive from NonlinearFactor. The values structures are typically (in SAM) more general * than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds. * Linearizing the non-linear factor graph creates a linear factor graph on the * tangent vector space at the linearization point. Because the tangent space is a true * vector space, the config type will be an VectorValues in that linearized factor graph. - * \nosubgrouping */ - template - class NonlinearFactorGraph: public FactorGraph > { + class NonlinearFactorGraph: public FactorGraph { public: - typedef VALUES Values; - typedef FactorGraph > Base; - typedef boost::shared_ptr > shared_ptr; - typedef boost::shared_ptr > sharedFactor; - - /// @name Testable - /// @{ + typedef FactorGraph Base; + typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr sharedFactor; /** print just calls base class */ void print(const std::string& str = "NonlinearFactorGraph: ") const; - /// @} - /// @name Standard Interface - /// @{ - /** return keys in some random order */ std::set keys() const; /** unnormalized error */ - double error(const VALUES& c) const; + double error(const Values& c) const; /** Unnormalized probability. O(n) */ - double probPrime(const VALUES& c) const; + double probPrime(const Values& c) const; + + template + void add(const F& factor) { + this->push_back(boost::shared_ptr(new F(factor))); + } /** * Create a symbolic factor graph using an existing ordering @@ -77,31 +72,20 @@ namespace gtsam { * ordering is found. */ std::pair - symbolic(const VALUES& config) const; + symbolic(const Values& config) const; /** * Compute a fill-reducing ordering using COLAMD. This returns the * ordering and a VariableIndex, which can later be re-used to save * computation. */ - Ordering::shared_ptr orderingCOLAMD(const VALUES& config) const; + Ordering::shared_ptr orderingCOLAMD(const Values& config) const; /** * linearize a nonlinear factor graph */ boost::shared_ptr - linearize(const VALUES& config, const Ordering& ordering) const; - - /// @} - /// @name Advanced Interface - /// @{ - - template - void add(const F& factor) { - this->push_back(boost::shared_ptr(new F(factor))); - } - - /// @} + linearize(const Values& config, const Ordering& ordering) const; private: @@ -116,4 +100,3 @@ namespace gtsam { } // namespace -#include diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index 16ec47247..03d2832ce 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -29,8 +29,8 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -template -void NonlinearISAM::update(const Factors& newFactors, +template +void NonlinearISAM::update(const Factors& newFactors, const Values& initialValues) { if(newFactors.size() > 0) { @@ -62,8 +62,8 @@ void NonlinearISAM::update(const Factors& newFactors, } /* ************************************************************************* */ -template -void NonlinearISAM::reorder_relinearize() { +template +void NonlinearISAM::reorder_relinearize() { // cout << "Reordering, relinearizing..." << endl; @@ -89,8 +89,8 @@ void NonlinearISAM::reorder_relinearize() { } /* ************************************************************************* */ -template -VALUES NonlinearISAM::estimate() const { +template +Values 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 9f082330c..f74cff5ea 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: diff --git a/gtsam/nonlinear/NonlinearOptimization-inl.h b/gtsam/nonlinear/NonlinearOptimization-inl.h index 105a9de19..361de59e2 100644 --- a/gtsam/nonlinear/NonlinearOptimization-inl.h +++ b/gtsam/nonlinear/NonlinearOptimization-inl.h @@ -34,17 +34,17 @@ namespace gtsam { /** * The Elimination solver */ - template - T optimizeSequential(const G& graph, const T& initialEstimate, + template + Values optimizeSequential(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters, bool useLM) { // Use a variable ordering from COLAMD 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(initialEstimate), ordering, boost::make_shared(parameters)); // Now optimize using either LM or GN methods. @@ -57,17 +57,17 @@ namespace gtsam { /** * The multifrontal solver */ - template - T optimizeMultiFrontal(const G& graph, const T& initialEstimate, + template + Values optimizeMultiFrontal(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters, bool useLM) { // Use a variable ordering from COLAMD 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(initialEstimate), ordering, boost::make_shared(parameters)); // now optimize using either LM or GN methods @@ -81,20 +81,20 @@ namespace gtsam { /** * The sparse preconditioned conjugate gradient solver */ - template - T optimizeSPCG(const G& graph, const T& initialEstimate, + template + Values optimizeSPCG(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(), bool useLM = true) { // initial optimization state is the same in both cases tested - typedef SubgraphSolver Solver; + 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( boost::make_shared(graph), - boost::make_shared(initialEstimate), + boost::make_shared(initialEstimate), solver->ordering(), solver, boost::make_shared(parameters)); @@ -110,20 +110,20 @@ namespace gtsam { /** * optimization that returns the values */ - template - T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters, + template + Values optimize(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters, const LinearSolver& solver, 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); #if ENABLE_SPCG case SPCG: -// return optimizeSPCG(graph, initialEstimate, parameters, +// return optimizeSPCG(graph, initialEstimate, parameters, // nonlinear_method == LM); throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint"); #endif diff --git a/gtsam/nonlinear/NonlinearOptimization.h b/gtsam/nonlinear/NonlinearOptimization.h index b563796eb..6521ded8e 100644 --- a/gtsam/nonlinear/NonlinearOptimization.h +++ b/gtsam/nonlinear/NonlinearOptimization.h @@ -44,8 +44,8 @@ namespace gtsam { /** * optimization that returns the values */ - template - T optimize(const G& graph, const T& initialEstimate, + template + Values optimize(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(), const LinearSolver& solver = MULTIFRONTAL, const NonlinearOptimizationMethod& nonlinear_method = LM); diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h index a98f4b455..f788eac2a 100644 --- a/gtsam/nonlinear/NonlinearOptimizer-inl.h +++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h @@ -30,8 +30,8 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - template - NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, + template + NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, shared_values values, shared_ordering ordering, shared_parameters parameters) : graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering), parameters_(parameters), iterations_(0), @@ -47,8 +47,8 @@ namespace gtsam { /* ************************************************************************* */ // FIXME: remove this constructor - template - NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, + template + NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, shared_values values, shared_ordering ordering, shared_solver spcg_solver, shared_parameters parameters) : graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering), @@ -66,8 +66,8 @@ namespace gtsam { /* ************************************************************************* */ // One iteration of Gauss Newton /* ************************************************************************* */ - template - NonlinearOptimizer NonlinearOptimizer::iterate() const { + template + NonlinearOptimizer NonlinearOptimizer::iterate() const { Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; @@ -86,7 +86,7 @@ namespace gtsam { if (verbosity >= Parameters::DELTA) delta.print("delta"); // take old values and update it - shared_values newValues(new C(values_->retract(delta, *ordering_))); + shared_values newValues(new Values(values_->retract(delta, *ordering_))); // maybe show output if (verbosity >= Parameters::VALUES) newValues->print("newValues"); @@ -99,8 +99,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - NonlinearOptimizer NonlinearOptimizer::gaussNewton() const { + template + NonlinearOptimizer NonlinearOptimizer::gaussNewton() const { static W writer(error_); if (error_ < parameters_->sumError_ ) { @@ -130,8 +130,8 @@ namespace gtsam { // TODO: in theory we can't infinitely recurse, but maybe we should put a max. // Reminder: the parameters are Graph type $G$, Values class type $T$, // linear system class $L$, the non linear solver type $S$, and the writer type $W$ - template - NonlinearOptimizer NonlinearOptimizer::try_lambda(const L& linearSystem) { + template + NonlinearOptimizer NonlinearOptimizer::try_lambda(const L& linearSystem) { const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; const Parameters::LambdaMode lambdaMode = parameters_->lambdaMode_ ; @@ -178,7 +178,7 @@ namespace gtsam { if (verbosity >= Parameters::TRYDELTA) delta.print("delta"); // update values - shared_values newValues(new T(values_->retract(delta, *ordering_))); + shared_values newValues(new Values(values_->retract(delta, *ordering_))); // create new optimization state with more adventurous lambda double error = graph_->error(*newValues); @@ -228,8 +228,8 @@ namespace gtsam { // One iteration of Levenberg Marquardt // Reminder: the parameters are Graph type $G$, Values class type $T$, // linear system class $L$, the non linear solver type $S$, and the writer type $W$ - template - NonlinearOptimizer NonlinearOptimizer::iterateLM() { + template + NonlinearOptimizer NonlinearOptimizer::iterateLM() { const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; const double lambda = parameters_->lambda_ ; @@ -253,8 +253,8 @@ namespace gtsam { /* ************************************************************************* */ // Reminder: the parameters are Graph type $G$, Values class type $T$, // linear system class $L$, the non linear solver type $S$, and the writer type $W$ - template - NonlinearOptimizer NonlinearOptimizer::levenbergMarquardt() { + template + NonlinearOptimizer NonlinearOptimizer::levenbergMarquardt() { // Initialize bool converged = false; @@ -299,20 +299,20 @@ namespace gtsam { } /* ************************************************************************* */ - template - NonlinearOptimizer NonlinearOptimizer::iterateDogLeg() { + template + NonlinearOptimizer NonlinearOptimizer::iterateDogLeg() { S solver(*graph_->linearize(*values_, *ordering_), parameters_->useQR_); DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate( parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(), *graph_, *values_, *ordering_, error_, parameters_->verbosity_ > Parameters::ERROR); - shared_values newValues(new T(values_->retract(result.dx_d, *ordering_))); + shared_values newValues(new Values(values_->retract(result.dx_d, *ordering_))); return newValuesErrorLambda_(newValues, result.f_error, result.Delta); } /* ************************************************************************* */ - template - NonlinearOptimizer NonlinearOptimizer::dogLeg() { + template + NonlinearOptimizer NonlinearOptimizer::dogLeg() { static W writer(error_); // check if we're already close enough diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 00647b2d8..5a2caed64 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -37,20 +37,20 @@ public: * and then one of the optimization routines is called. These iterate * until convergence. All methods are functional and return a new state. * - * The class is parameterized by the Graph type $G$, Values class type $T$, + * The class is parameterized by the Graph type $G$, Values class type $Values$, * linear system class $L$, the non linear solver type $S$, and the writer type $W$ * - * The values class type $T$ is in order to be able to optimize over non-vector values structures. + * The values class type $Values$ is in order to be able to optimize over non-vector values structures. * * A nonlinear system solver $S$ - * Concept NonLinearSolver implements - * linearize: G * T -> L - * solve : L -> T + * Concept NonLinearSolver implements + * linearize: G * Values -> L + * solve : L -> Values * * The writer $W$ generates output to disk or the screen. * - * For example, in a 2D case, $G$ can be Pose2Graph, $T$ can be Pose2Values, - * $L$ can be GaussianFactorGraph and $S$ can be Factorization. + * For example, in a 2D case, $G$ can be pose2SLAM::Graph, $Values$ can be Pose2Values, + * $L$ can be GaussianFactorGraph and $S$ can be Factorization. * The solver class has two main functions: linearize and optimize. The first one * linearizes the nonlinear cost function around the current estimate, and the second * one optimizes the linearized system using various methods. @@ -58,12 +58,12 @@ public: * To use the optimizer in code, include in your cpp file * \nosubgrouping */ -template +template class NonlinearOptimizer { public: // For performance reasons in recursion, we store values in a shared_ptr - typedef boost::shared_ptr shared_values; ///Prevent memory leaks in Values + typedef boost::shared_ptr shared_values; ///Prevent memory leaks in Values typedef boost::shared_ptr shared_graph; /// Prevent memory leaks in Graph typedef boost::shared_ptr shared_linear; /// Not sure typedef boost::shared_ptr shared_ordering; ///ordering parameters @@ -74,7 +74,7 @@ public: private: - typedef NonlinearOptimizer This; + typedef NonlinearOptimizer This; typedef boost::shared_ptr > shared_dimensions; /// keep a reference to const version of the graph @@ -182,7 +182,7 @@ public: /** * Copy constructor */ - NonlinearOptimizer(const NonlinearOptimizer &optimizer) : + NonlinearOptimizer(const NonlinearOptimizer &optimizer) : graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_), ordering_(optimizer.ordering_), parameters_(optimizer.parameters_), iterations_(optimizer.iterations_), dimensions_(optimizer.dimensions_), structure_(optimizer.structure_) {} @@ -248,7 +248,7 @@ public: /** * linearize and optimize - * This returns an VectorValues, i.e., vectors in tangent space of T + * This returns an VectorValues, i.e., vectors in tangent space of Values */ VectorValues linearizeAndOptimizeForDelta() const { return *createSolver()->optimize(); @@ -340,19 +340,19 @@ public: * Static interface to LM optimization (no shared_ptr arguments) - see above */ static shared_values optimizeLM(const G& graph, - const T& values, + const Values& values, const Parameters parameters = Parameters()) { return optimizeLM(boost::make_shared(graph), - boost::make_shared(values), + boost::make_shared(values), boost::make_shared(parameters)); } ///TODO: comment static shared_values optimizeLM(const G& graph, - const T& values, + const Values& values, Parameters::verbosityLevel verbosity) { return optimizeLM(boost::make_shared(graph), - boost::make_shared(values), + boost::make_shared(values), verbosity); } @@ -392,19 +392,19 @@ public: * Static interface to Dogleg optimization (no shared_ptr arguments) - see above */ static shared_values optimizeDogLeg(const G& graph, - const T& values, + const Values& values, const Parameters parameters = Parameters()) { return optimizeDogLeg(boost::make_shared(graph), - boost::make_shared(values), + boost::make_shared(values), boost::make_shared(parameters)); } ///TODO: comment static shared_values optimizeDogLeg(const G& graph, - const T& values, + const Values& values, Parameters::verbosityLevel verbosity) { return optimizeDogLeg(boost::make_shared(graph), - boost::make_shared(values), + boost::make_shared(values), verbosity); } @@ -431,9 +431,9 @@ public: /** * Static interface to GN optimization (no shared_ptr arguments) - see above */ - static shared_values optimizeGN(const G& graph, const T& values, const Parameters parameters = Parameters()) { + static shared_values optimizeGN(const G& graph, const Values& values, const Parameters parameters = Parameters()) { return optimizeGN(boost::make_shared(graph), - boost::make_shared(values), + boost::make_shared(values), boost::make_shared(parameters)); } }; diff --git a/gtsam/nonlinear/TupleValues-inl.h b/gtsam/nonlinear/TupleValues-inl.h deleted file mode 100644 index a9d6300c1..000000000 --- a/gtsam/nonlinear/TupleValues-inl.h +++ /dev/null @@ -1,165 +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 TupleValues-inl.h - * @author Richard Roberts - * @author Manohar Paluri - * @author Alex Cunningham - */ - -#pragma once - -namespace gtsam { - -/* ************************************************************************* */ -/** TupleValuesN Implementations */ -/* ************************************************************************* */ - -/* ************************************************************************* */ -/** TupleValues 1 */ -template -TupleValues1::TupleValues1(const TupleValues1& values) : - TupleValuesEnd (values) {} - -template -TupleValues1::TupleValues1(const VALUES1& cfg1) : - TupleValuesEnd (cfg1) {} - -template -TupleValues1::TupleValues1(const TupleValuesEnd& values) : - TupleValuesEnd(values) {} - -/* ************************************************************************* */ -/** TupleValues 2 */ -template -TupleValues2::TupleValues2(const TupleValues2& values) : - TupleValues >(values) {} - -template -TupleValues2::TupleValues2(const VALUES1& cfg1, const VALUES2& cfg2) : - TupleValues >( - cfg1, TupleValuesEnd(cfg2)) {} - -template -TupleValues2::TupleValues2(const TupleValues >& values) : - TupleValues >(values) {} - -/* ************************************************************************* */ -/** TupleValues 3 */ -template -TupleValues3::TupleValues3( - const TupleValues3& values) : - TupleValues > >(values) {} - -template -TupleValues3::TupleValues3( - const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3) : - TupleValues > >( - cfg1, TupleValues >( - cfg2, TupleValuesEnd(cfg3))) {} - -template -TupleValues3::TupleValues3( - const TupleValues > >& values) : - TupleValues > >(values) {} - -/* ************************************************************************* */ -/** TupleValues 4 */ -template -TupleValues4::TupleValues4( - const TupleValues4& values) : - TupleValues > > >(values) {} - -template -TupleValues4::TupleValues4( - const VALUES1& cfg1, const VALUES2& cfg2, - const VALUES3& cfg3,const VALUES4& cfg4) : - TupleValues > > >( - cfg1, TupleValues > >( - cfg2, TupleValues >( - cfg3, TupleValuesEnd(cfg4)))) {} - -template -TupleValues4::TupleValues4( - const TupleValues > > >& values) : - TupleValues > > >(values) {} - -/* ************************************************************************* */ -/** TupleValues 5 */ -template -TupleValues5::TupleValues5( - const TupleValues5& values) : - TupleValues > > > >(values) {} - -template -TupleValues5::TupleValues5( - const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3, - const VALUES4& cfg4, const VALUES5& cfg5) : - TupleValues > > > >( - cfg1, TupleValues > > >( - cfg2, TupleValues > >( - cfg3, TupleValues >( - cfg4, TupleValuesEnd(cfg5))))) {} - -template -TupleValues5::TupleValues5( - const TupleValues > > > >& values) : - TupleValues > > > >(values) {} - -/* ************************************************************************* */ -/** TupleValues 6 */ -template -TupleValues6::TupleValues6( - const TupleValues6& values) : - TupleValues > > > > >(values) {} - -template -TupleValues6::TupleValues6( - const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3, - const VALUES4& cfg4, const VALUES5& cfg5, const VALUES6& cfg6) : - TupleValues > > > > >( - cfg1, TupleValues > > > >( - cfg2, TupleValues > > >( - cfg3, TupleValues > >( - cfg4, TupleValues >( - cfg5, TupleValuesEnd(cfg6)))))) {} - -template -TupleValues6::TupleValues6( - const TupleValues > > > > >& values) : - TupleValues > > > > >(values) {} - -} diff --git a/gtsam/nonlinear/TupleValues.h b/gtsam/nonlinear/TupleValues.h deleted file mode 100644 index df3eab51b..000000000 --- a/gtsam/nonlinear/TupleValues.h +++ /dev/null @@ -1,550 +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 TupleValues.h - * @author Richard Roberts - * @author Manohar Paluri - * @author Alex Cunningham - */ - -#include -#include - -#pragma once - -namespace gtsam { - - /** - * TupleValues are a structure to manage heterogenous Values, so as to - * enable different types of variables/keys to be used simultaneously. We - * do this with recursive templates (instead of blind pointer casting) to - * reduce run-time overhead and keep static type checking. The interface - * mimics that of a single Values. - * - * This uses a recursive structure of values pairs to form a lisp-like - * list, with a special case (TupleValuesEnd) that contains only one values - * at the end. Because this recursion is done at compile time, there is no - * runtime performance hit to using this structure. - * - * For an easy interface, there are TupleValuesN classes, which wrap - * the recursive TupleValues together as a single class, so you can have - * mixed-type classes from 2-6 types. Note that a TupleValues2 is equivalent - * to the previously used PairValues. - * - * Design and extension note: - * To implement a recursively templated data structure, note that most operations - * have two versions: one with templates and one without. The templated one allows - * for the arguments to be passed to the next values, while the specialized one - * operates on the "first" values. TupleValuesEnd contains only the specialized version. - * \nosubgrouping - */ - template - class TupleValues { - - protected: - // Data for internal valuess - VALUES1 first_; /// Arbitrary values - VALUES2 second_; /// A TupleValues or TupleValuesEnd, which wraps an arbitrary values - - /** concept checks */ - GTSAM_CONCEPT_TESTABLE_TYPE(VALUES1) - GTSAM_CONCEPT_TESTABLE_TYPE(VALUES2) - - public: - // typedefs for values subtypes - typedef typename VALUES1::Key Key1; - typedef typename VALUES1::Value Value1; - - /// @name Standard Constructors - /// @{ - - /** default constructor */ - TupleValues() {} - - /** Copy constructor */ - TupleValues(const TupleValues& values) : - first_(values.first_), second_(values.second_) {} - - /** Construct from valuess */ - TupleValues(const VALUES1& cfg1, const VALUES2& cfg2) : - first_(cfg1), second_(cfg2) {} - - /// @} - /// @name Testable - /// @{ - - /** Print */ - void print(const std::string& s = "") const { - first_.print(s); - second_.print(); - } - - /** Equality with tolerance for keys and values */ - bool equals(const TupleValues& c, double tol=1e-9) const { - return first_.equals(c.first_, tol) && second_.equals(c.second_, tol); - } - - /// @} - /// @name Advanced Interface - /// @{ - - /** - * Insert a key/value pair to the values. - * Note: if the key is already in the values, the values will not be changed. - * Use update() to allow for changing existing values. - * @param key is the key - can be an int (second version) if the can can be initialized from an int - * @param value is the value to insert - */ - template - void insert(const KEY& key, const VALUE& value) {second_.insert(key, value);} - void insert(int key, const Value1& value) {first_.insert(Key1(key), value);} /// - void insert(const TupleValues& values) { second_.insert(values); } - void insert(const TupleValues& values) { /// - void update(const TupleValues& values) { second_.update(values); } - void update(const TupleValues& values) { /// - void update(const KEY& key, const VALUE& value) { second_.update(key, value); } - void update(const Key1& key, const Value1& value) { first_.update(key, value); } /// - void insertSub(const CFG& values) { second_.insertSub(values); } - void insertSub(const VALUES1& values) { first_.insert(values); } /// - void erase(const KEY& j) { second_.erase(j); } - void erase(const Key1& j) { first_.erase(j); } /// - bool exists(const KEY& j) const { return second_.exists(j); } - bool exists(const Key1& j) const { return first_.exists(j); } /// - boost::optional exists_(const KEY& j) const { return second_.exists_(j); } - boost::optional exists_(const Key1& j) const { return first_.exists_(j); } /// - const typename KEY::Value & operator[](const KEY& j) const { return second_[j]; } - const Value1& operator[](const Key1& j) const { return first_[j]; } /// - const typename KEY::Value & at(const KEY& j) const { return second_.at(j); } - const Value1& at(const Key1& j) const { return first_.at(j); } ///dims(ordering)); - } - - /** @return number of key/value pairs stored */ - size_t size() const { return first_.size() + second_.size(); } - - /** @return true if values is empty */ - bool empty() const { return first_.empty() && second_.empty(); } - - /** - * Generate a default ordering, simply in key sort order. To instead - * create a fill-reducing ordering, use - * NonlinearFactorGraph::orderingCOLAMD(). Alternatively, you may permute - * this ordering yourself (as orderingCOLAMD() does internally). - */ - Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const { - // Generate an initial key ordering in iterator order - _ValuesKeyOrderer keyOrderer(firstVar); - this->apply(keyOrderer); - return keyOrderer.ordering; - } - - /** - * Apply a class with an application operator() to a const_iterator over - * every pair. The operator must be able to handle such an - * iterator for every type in the Values, (i.e. through templating). - */ - template - void apply(A& operation) { - first_.apply(operation); - second_.apply(operation); - } - - /** - * TODO: comment - */ - template - void apply(A& operation) const { - first_.apply(operation); - second_.apply(operation); - } - - /// @} - /// @name Manifold - /// @{ - - /** @return The dimensionality of the tangent space */ - size_t dim() const { return first_.dim() + second_.dim(); } - - /** Create an array of variable dimensions using the given ordering */ - std::vector dims(const Ordering& ordering) const { - _ValuesDimensionCollector dimCollector(ordering); - this->apply(dimCollector); - return dimCollector.dimensions; - } - - /** Expmap */ - TupleValues retract(const VectorValues& delta, const Ordering& ordering) const { - return TupleValues(first_.retract(delta, ordering), second_.retract(delta, ordering)); - } - - /** logmap each element */ - VectorValues localCoordinates(const TupleValues& cp, const Ordering& ordering) const { - VectorValues delta(this->dims(ordering)); - localCoordinates(cp, ordering, delta); - return delta; - } - - /** logmap each element */ - void localCoordinates(const TupleValues& cp, const Ordering& ordering, VectorValues& delta) const { - first_.localCoordinates(cp.first_, ordering, delta); - second_.localCoordinates(cp.second_, ordering, delta); - } - - /// @} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(first_); - ar & BOOST_SERIALIZATION_NVP(second_); - } - - }; - - /** - * End of a recursive TupleValues - contains only one values - * - * Do not use this class directly - it should only be used as a part - * of a recursive structure - */ - template - class TupleValuesEnd { - - protected: - // Data for internal valuess - VALUES first_; - - public: - // typedefs - typedef typename VALUES::Key Key1; - typedef typename VALUES::Value Value1; - - TupleValuesEnd() {} - - TupleValuesEnd(const TupleValuesEnd& values) : - first_(values.first_) {} - - TupleValuesEnd(const VALUES& cfg) : - first_(cfg) {} - - void print(const std::string& s = "") const { - first_.print(); - } - - bool equals(const TupleValuesEnd& c, double tol=1e-9) const { - return first_.equals(c.first_, tol); - } - - void insert(const Key1& key, const Value1& value) {first_.insert(key, value); } - void insert(int key, const Value1& value) {first_.insert(Key1(key), value);} - - void insert(const TupleValuesEnd& values) {first_.insert(values.first_); } - - void update(const TupleValuesEnd& values) {first_.update(values.first_); } - - void update(const Key1& key, const Value1& value) { first_.update(key, value); } - - void insertSub(const VALUES& values) {first_.insert(values); } - - const Value1& operator[](const Key1& j) const { return first_[j]; } - - const VALUES& values() const { return first_; } - - void erase(const Key1& j) { first_.erase(j); } - - void clear() { first_.clear(); } - - bool empty() const { return first_.empty(); } - - bool exists(const Key1& j) const { return first_.exists(j); } - - boost::optional exists_(const Key1& j) const { return first_.exists_(j); } - - const Value1& at(const Key1& j) const { return first_.at(j); } - - VectorValues zero(const Ordering& ordering) const { - return VectorValues::Zero(this->dims(ordering)); - } - - size_t size() const { return first_.size(); } - - size_t dim() const { return first_.dim(); } - - TupleValuesEnd retract(const VectorValues& delta, const Ordering& ordering) const { - return TupleValuesEnd(first_.retract(delta, ordering)); - } - - VectorValues localCoordinates(const TupleValuesEnd& cp, const Ordering& ordering) const { - VectorValues delta(this->dims(ordering)); - localCoordinates(cp, ordering, delta); - return delta; - } - - void localCoordinates(const TupleValuesEnd& cp, const Ordering& ordering, VectorValues& delta) const { - first_.localCoordinates(cp.first_, ordering, delta); - } - - /** - * Apply a class with an application operator() to a const_iterator over - * every pair. The operator must be able to handle such an - * iterator for every type in the Values, (i.e. through templating). - */ - template - void apply(A& operation) { - first_.apply(operation); - } - template - void apply(A& operation) const { - first_.apply(operation); - } - - /** Create an array of variable dimensions using the given ordering */ - std::vector dims(const Ordering& ordering) const { - _ValuesDimensionCollector dimCollector(ordering); - this->apply(dimCollector); - return dimCollector.dimensions; - } - - /** - * Generate a default ordering, simply in key sort order. To instead - * create a fill-reducing ordering, use - * NonlinearFactorGraph::orderingCOLAMD(). Alternatively, you may permute - * this ordering yourself (as orderingCOLAMD() does internally). - */ - Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const { - // Generate an initial key ordering in iterator order - _ValuesKeyOrderer keyOrderer(firstVar); - this->apply(keyOrderer); - return keyOrderer.ordering; - } - - private: - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(first_); - } - }; - - /** - * Wrapper classes to act as containers for valuess. Note that these can be cascaded - * recursively, as they are TupleValues, and are primarily a short form of the values - * structure to make use of the TupleValues easier. - * - * The interface is designed to mimic PairValues, but for 2-6 values types. - */ - - template - class TupleValues1 : public TupleValuesEnd { - public: - // typedefs - typedef C1 Values1; - - typedef TupleValuesEnd Base; - typedef TupleValues1 This; - - TupleValues1() {} - TupleValues1(const This& values); - TupleValues1(const Base& values); - TupleValues1(const Values1& cfg1); - - // access functions - inline const Values1& first() const { return this->values(); } - }; - - template - class TupleValues2 : public TupleValues > { - public: - // typedefs - typedef C1 Values1; - typedef C2 Values2; - - typedef TupleValues > Base; - typedef TupleValues2 This; - - TupleValues2() {} - TupleValues2(const This& values); - TupleValues2(const Base& values); - TupleValues2(const Values1& cfg1, const Values2& cfg2); - - // access functions - inline const Values1& first() const { return this->values(); } - inline const Values2& second() const { return this->rest().values(); } - }; - - template - class TupleValues3 : public TupleValues > > { - public: - // typedefs - typedef C1 Values1; - typedef C2 Values2; - typedef C3 Values3; - - typedef TupleValues > > Base; - typedef TupleValues3 This; - - TupleValues3() {} - TupleValues3(const Base& values); - TupleValues3(const This& values); - TupleValues3(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3); - - // access functions - inline const Values1& first() const { return this->values(); } - inline const Values2& second() const { return this->rest().values(); } - inline const Values3& third() const { return this->rest().rest().values(); } - }; - - template - class TupleValues4 : public TupleValues > > > { - public: - // typedefs - typedef C1 Values1; - typedef C2 Values2; - typedef C3 Values3; - typedef C4 Values4; - - typedef TupleValues > > > Base; - typedef TupleValues4 This; - - TupleValues4() {} - TupleValues4(const This& values); - TupleValues4(const Base& values); - TupleValues4(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,const Values4& cfg4); - - // access functions - inline const Values1& first() const { return this->values(); } - inline const Values2& second() const { return this->rest().values(); } - inline const Values3& third() const { return this->rest().rest().values(); } - inline const Values4& fourth() const { return this->rest().rest().rest().values(); } - }; - - template - class TupleValues5 : public TupleValues > > > > { - public: - typedef C1 Values1; - typedef C2 Values2; - typedef C3 Values3; - typedef C4 Values4; - typedef C5 Values5; - - typedef TupleValues > > > > Base; - typedef TupleValues5 This; - - TupleValues5() {} - TupleValues5(const This& values); - TupleValues5(const Base& values); - TupleValues5(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3, - const Values4& cfg4, const Values5& cfg5); - - // access functions - inline const Values1& first() const { return this->values(); } - inline const Values2& second() const { return this->rest().values(); } - inline const Values3& third() const { return this->rest().rest().values(); } - inline const Values4& fourth() const { return this->rest().rest().rest().values(); } - inline const Values5& fifth() const { return this->rest().rest().rest().rest().values(); } - }; - - template - class TupleValues6 : public TupleValues > > > > > { - public: - typedef C1 Values1; - typedef C2 Values2; - typedef C3 Values3; - typedef C4 Values4; - typedef C5 Values5; - typedef C6 Values6; - - typedef TupleValues > > > > > Base; - typedef TupleValues6 This; - - TupleValues6() {} - TupleValues6(const This& values); - TupleValues6(const Base& values); - TupleValues6(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3, - const Values4& cfg4, const Values5& cfg5, const Values6& cfg6); - // access functions - inline const Values1& first() const { return this->values(); } - inline const Values2& second() const { return this->rest().values(); } - inline const Values3& third() const { return this->rest().rest().values(); } - inline const Values4& fourth() const { return this->rest().rest().rest().values(); } - inline const Values5& fifth() const { return this->rest().rest().rest().rest().values(); } - inline const Values6& sixth() const { return this->rest().rest().rest().rest().rest().values(); } - }; - -} - -#include diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 28f08bfb5..76a1ce035 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -10,222 +10,87 @@ * -------------------------------------------------------------------------- */ /** - * @file Values.cpp + * @file Values.h * @author Richard Roberts + * + * @brief A non-templated config holding any types of Manifold-group elements + * + * Detailed story: + * A values structure is a map from keys to values. It is used to specify the value of a bunch + * of variables in a factor graph. A Values is a values structure which can hold variables that + * are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type + * which is also a manifold element, and hence supports operations dim, retract, and localCoordinates. */ -#pragma once - #include -#include -#include -#include -#include - -#include -#include -#include -#include - -using namespace std; +#include // Only so Eclipse finds class definition namespace gtsam { -/* ************************************************************************* */ - template - void Values::print(const string &s) const { - cout << "Values " << s << ", size " << values_.size() << "\n"; - BOOST_FOREACH(const KeyValuePair& v, values_) { - gtsam::print(v.second, (string)(v.first)); - } - } + /* ************************************************************************* */ + class ValueCloneAllocator { + public: + static Value* allocate_clone(const Value& a) { return a.clone_(); } + static void deallocate_clone(const Value* a) { a->deallocate_(); } + private: + ValueCloneAllocator() {} + }; /* ************************************************************************* */ - template - bool Values::equals(const Values& expected, double tol) const { - if (values_.size() != expected.values_.size()) return false; - BOOST_FOREACH(const KeyValuePair& v, values_) { - if (!expected.exists(v.first)) return false; - if(!gtsam::equal(v.second, expected[v.first], tol)) - return false; - } - return true; + template + const ValueType& Values::at(const Symbol& j) const { + // Find the item + KeyValueMap::const_iterator item = values_.find(j); + + // Throw exception if it does not exist + if(item == values_.end()) + throw ValuesKeyDoesNotExist("retrieve", j); + + // Check the type and throw exception if incorrect + if(typeid(*item->second) != typeid(ValueType)) + throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); + + // We have already checked the type, so do a "blind" static_cast, not dynamic_cast + return static_cast(*item->second); } /* ************************************************************************* */ - template - const typename J::Value& Values::at(const J& j) const { - const_iterator it = values_.find(j); - if (it == values_.end()) throw KeyDoesNotExist("retrieve", j); - else return it->second; + template + const typename TypedKey::Value& Values::at(const TypedKey& j) const { + // Convert to Symbol + const Symbol symbol(j.symbol()); + + // Call at with the Value type from the key + return at(symbol); } /* ************************************************************************* */ - template - size_t Values::dim() const { - size_t n = 0; - BOOST_FOREACH(const KeyValuePair& value, values_) - n += value.second.dim(); - return n; - } + template + boost::optional Values::exists(const Symbol& j) const { + // Find the item + KeyValueMap::const_iterator item = values_.find(j); - /* ************************************************************************* */ - template - VectorValues Values::zero(const Ordering& ordering) const { - return VectorValues::Zero(this->dims(ordering)); - } + if(item != values_.end()) { + // Check the type and throw exception if incorrect + if(typeid(*item->second) != typeid(ValueType)) + throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); - /* ************************************************************************* */ - template - void Values::insert(const J& name, const typename J::Value& val) { - if(!values_.insert(make_pair(name, val)).second) - throw KeyAlreadyExists(name, val); - } - - /* ************************************************************************* */ - template - void Values::insert(const Values& cfg) { - BOOST_FOREACH(const KeyValuePair& v, cfg.values_) - insert(v.first, v.second); - } - - /* ************************************************************************* */ - template - void Values::update(const Values& cfg) { - BOOST_FOREACH(const KeyValuePair& v, values_) { - boost::optional t = cfg.exists_(v.first); - if (t) values_[v.first] = *t; - } - } - - /* ************************************************************************* */ - template - void Values::update(const J& j, const typename J::Value& val) { - values_[j] = val; - } - - /* ************************************************************************* */ - template - std::list Values::keys() const { - std::list ret; - BOOST_FOREACH(const KeyValuePair& v, values_) - ret.push_back(v.first); - return ret; - } - - /* ************************************************************************* */ - template - void Values::erase(const J& j) { - size_t dim; // unused - erase(j, dim); - } - - /* ************************************************************************* */ - template - void Values::erase(const J& j, size_t& dim) { - iterator it = values_.find(j); - if (it == values_.end()) - throw KeyDoesNotExist("erase", j); - dim = it->second.dim(); - values_.erase(it); - } - - /* ************************************************************************* */ - // todo: insert for every element is inefficient - template - Values Values::retract(const VectorValues& delta, const Ordering& ordering) const { - Values newValues; - typedef pair KeyValue; - BOOST_FOREACH(const KeyValue& value, this->values_) { - const J& j = value.first; - const typename J::Value& pj = value.second; - Index index; - if(ordering.tryAt(j,index)) { - newValues.insert(j, pj.retract(delta[index])); - } else - newValues.insert(j, pj); - } - return newValues; - } - - /* ************************************************************************* */ - template - std::vector Values::dims(const Ordering& ordering) const { - _ValuesDimensionCollector dimCollector(ordering); - this->apply(dimCollector); - return dimCollector.dimensions; - } - - /* ************************************************************************* */ - template - Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const { - // Generate an initial key ordering in iterator order - _ValuesKeyOrderer keyOrderer(firstVar); - this->apply(keyOrderer); - return keyOrderer.ordering; - } - -// /* ************************************************************************* */ -// // todo: insert for every element is inefficient -// template -// Values Values::retract(const Vector& delta) const { -// if(delta.size() != dim()) { -// cout << "Values::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl; -// throw invalid_argument("Delta vector length does not match config dimensionality."); -// } -// Values newValues; -// int delta_offset = 0; -// typedef pair KeyValue; -// BOOST_FOREACH(const KeyValue& value, this->values_) { -// const J& j = value.first; -// const typename J::Value& pj = value.second; -// int cur_dim = pj.dim(); -// newValues.insert(j,pj.retract(sub(delta, delta_offset, delta_offset+cur_dim))); -// delta_offset += cur_dim; -// } -// return newValues; -// } - - /* ************************************************************************* */ - // todo: insert for every element is inefficient - // todo: currently only logmaps elements in both configs - template - inline VectorValues Values::localCoordinates(const Values& cp, const Ordering& ordering) const { - VectorValues delta(this->dims(ordering)); - localCoordinates(cp, ordering, delta); - return delta; - } - - /* ************************************************************************* */ - template - void Values::localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const { - typedef pair KeyValue; - BOOST_FOREACH(const KeyValue& value, cp) { - assert(this->exists(value.first)); - delta[ordering[value.first]] = this->at(value.first).localCoordinates(value.second); + // We have already checked the type, so do a "blind" static_cast, not dynamic_cast + return static_cast(*item->second); + } else { + return boost::none; } } /* ************************************************************************* */ - template - const char* KeyDoesNotExist::what() const throw() { - if(message_.empty()) - message_ = - "Attempting to " + std::string(operation_) + " the key \"" + - (std::string)key_ + "\", which does not exist in the Values."; - return message_.c_str(); - } + template + boost::optional Values::exists(const TypedKey& j) const { + // Convert to Symbol + const Symbol symbol(j.symbol()); - /* ************************************************************************* */ - template - const char* KeyAlreadyExists::what() const throw() { - if(message_.empty()) - message_ = - "Attempting to add a key-value pair with key \"" + (std::string)key_ + "\", key already exists."; - return message_.c_str(); + // Call exists with the Value type from the key + return exists(symbol); } } - - diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp new file mode 100644 index 000000000..2d8b72120 --- /dev/null +++ b/gtsam/nonlinear/Values.cpp @@ -0,0 +1,213 @@ +/* ---------------------------------------------------------------------------- + + * 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 Values.h + * @author Richard Roberts + * + * @brief A non-templated config holding any types of Manifold-group elements + * + * Detailed story: + * A values structure is a map from keys to values. It is used to specify the value of a bunch + * of variables in a factor graph. A Values is a values structure which can hold variables that + * are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type + * which is also a manifold element, and hence supports operations dim, retract, and localCoordinates. + */ + +#include + +#include + +#include +#include +#include + +using namespace std; + +namespace gtsam { + + /* ************************************************************************* */ + Values::Values(const Values& other) { + this->insert(other); + } + + /* ************************************************************************* */ + void Values::print(const string& str) const { + cout << str << "Values with " << size() << " values:\n" << endl; + for(const_iterator key_value = begin(); key_value != end(); ++key_value) { + cout << " " << (string)key_value->first << ": "; + key_value->second.print(""); + } + } + + /* ************************************************************************* */ + bool Values::equals(const Values& other, double tol) const { + if(this->size() != other.size()) + return false; + for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) { + if(typeid(it1->second) != typeid(it2->second)) + return false; + if(it1->first != it2->first) + return false; + if(!it1->second.equals_(it2->second, tol)) + return false; + } + return true; // We return false earlier if we find anything that does not match + } + + /* ************************************************************************* */ + bool Values::exists(const Symbol& j) const { + return values_.find(j) != values_.end(); + } + + /* ************************************************************************* */ + VectorValues Values::zeroVectors(const Ordering& ordering) const { + return VectorValues::Zero(this->dims(ordering)); + } + + /* ************************************************************************* */ + Values Values::retract(const VectorValues& delta, const Ordering& ordering) const { + Values result; + + for(const_iterator key_value = begin(); key_value != end(); ++key_value) { + const SubVector& singleDelta = delta[ordering[key_value->first]]; // Delta for this value + Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument + Value* retractedValue(key_value->second.retract_(singleDelta)); // Retract + result.values_.insert(key, retractedValue); // Add retracted result directly to result values + } + + return result; + } + + /* ************************************************************************* */ + VectorValues Values::localCoordinates(const Values& cp, const Ordering& ordering) const { + VectorValues result(this->dims(ordering)); + localCoordinates(cp, ordering, result); + return result; + } + + /* ************************************************************************* */ + void Values::localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& result) const { + if(this->size() != cp.size()) + throw DynamicValuesMismatched(); + for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) { + if(it1->first != it2->first) + throw DynamicValuesMismatched(); // If keys do not match + // Will throw a dynamic_cast exception if types do not match + result.insert(ordering[it1->first], it1->second.localCoordinates_(it2->second)); + } + } + + /* ************************************************************************* */ + void Values::insert(const Symbol& j, const Value& val) { + Symbol key = j; // Non-const duplicate to deal with non-const insert argument + std::pair insertResult = values_.insert(key, val.clone_()); + if(!insertResult.second) + throw ValuesKeyAlreadyExists(j); + } + + /* ************************************************************************* */ + void Values::insert(const Values& values) { + for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { + Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument + insert(key, key_value->second); + } + } + + /* ************************************************************************* */ + void Values::update(const Symbol& j, const Value& val) { + // Find the value to update + KeyValueMap::iterator item = values_.find(j); + if(item == values_.end()) + throw ValuesKeyDoesNotExist("update", j); + + // Cast to the derived type + if(typeid(*item->second) != typeid(val)) + throw ValuesIncorrectType(j, typeid(*item->second), typeid(val)); + + values_.replace(item, val.clone_()); + } + + /* ************************************************************************* */ + void Values::update(const Values& values) { + for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { + this->update(key_value->first, key_value->second); + } + } + + /* ************************************************************************* */ + void Values::erase(const Symbol& j) { + KeyValueMap::iterator item = values_.find(j); + if(item == values_.end()) + throw ValuesKeyDoesNotExist("erase", j); + values_.erase(item); + } + + /* ************************************************************************* */ + FastList Values::keys() const { + FastList result; + for(const_iterator key_value = begin(); key_value != end(); ++key_value) + result.push_back(key_value->first); + return result; + } + + /* ************************************************************************* */ + Values& Values::operator=(const Values& rhs) { + this->clear(); + this->insert(rhs); + return *this; + } + + /* ************************************************************************* */ + vector Values::dims(const Ordering& ordering) const { + vector result(values_.size()); + BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) { + result[ordering[key_value.first]] = key_value.second.dim(); + } + return result; + } + + /* ************************************************************************* */ + Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const { + Ordering::shared_ptr ordering(new Ordering); + for(const_iterator key_value = begin(); key_value != end(); ++key_value) { + ordering->insert(key_value->first, firstVar++); + } + return ordering; + } + + /* ************************************************************************* */ + const char* ValuesKeyAlreadyExists::what() const throw() { + if(message_.empty()) + message_ = + "Attempting to add a key-value pair with key \"" + (std::string)key_ + "\", key already exists."; + return message_.c_str(); + } + + /* ************************************************************************* */ + const char* ValuesKeyDoesNotExist::what() const throw() { + if(message_.empty()) + message_ = + "Attempting to " + std::string(operation_) + " the key \"" + + (std::string)key_ + "\", which does not exist in the Values."; + return message_.c_str(); + } + + /* ************************************************************************* */ + const char* ValuesIncorrectType::what() const throw() { + if(message_.empty()) + message_ = + "Attempting to retrieve value with key \"" + (std::string)key_ + "\", type stored in Values is " + + std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name()); + return message_.c_str(); + } + +} diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 4c98e8a58..9f154609d 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -13,115 +13,159 @@ * @file Values.h * @author Richard Roberts * - * @brief A templated config for Manifold-group elements + * @brief A non-templated config holding any types of Manifold-group elements * * Detailed story: * A values structure is a map from keys to values. It is used to specify the value of a bunch * of variables in a factor graph. A Values is a values structure which can hold variables that * are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type * which is also a manifold element, and hence supports operations dim, retract, and localCoordinates. - * \nosubgrouping */ #pragma once -#include +#include +#include -#include +#include +#include +#include +#include +#include + +#include #include -#include -#include +#include +#include #include -namespace boost { template class optional; } -namespace gtsam { class VectorValues; class Ordering; } - namespace gtsam { // Forward declarations - template class KeyDoesNotExist; - template class KeyAlreadyExists; + class ValueCloneAllocator; - /** - * Manifold type values structure - * Takes two template types - * J: a key type to look up values in the values structure (need to be sortable) - * - * Key concept: - * The key will be assumed to be sortable, and must have a - * typedef called "Value" with the type of the value the key - * labels (example: Pose2, Point2, etc) - */ - template +/** + * A non-templated config holding any types of Manifold-group elements. A + * values structure is a map from keys to values. It is used to specify the + * value of a bunch of variables in a factor graph. A Values is a values + * structure which can hold variables that are elements on manifolds, not just + * vectors. It then, as a whole, implements a aggregate type which is also a + * manifold element, and hence supports operations dim, retract, and + * localCoordinates. + */ class Values { - public: - - /** - * Typedefs - */ - typedef J Key; - typedef typename J::Value Value; - typedef std::map, boost::fast_pool_allocator > > KeyValueMap; - // typedef FastMap KeyValueMap; - typedef typename KeyValueMap::value_type KeyValuePair; - typedef typename KeyValueMap::iterator iterator; - typedef typename KeyValueMap::const_iterator const_iterator; - private: - /** concept check */ - GTSAM_CONCEPT_TESTABLE_TYPE(Value) - GTSAM_CONCEPT_MANIFOLD_TYPE(Value) + // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined + // below) to clone and deallocate the Value objects, and a boost + // fast_pool_allocator to allocate map nodes. In this way, all memory is + // allocated in a boost memory pool. + typedef boost::ptr_map< + Symbol, + Value, + std::less, + ValueCloneAllocator, + boost::fast_pool_allocator > > KeyValueMap; + // The member to store the values, see just above KeyValueMap values_; + // Types obtained by iterating + typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair; + typedef KeyValueMap::iterator::value_type KeyValuePtrPair; + public: - /// @name Standard Constructors - /// @{ + /// A shared_ptr to this class + typedef boost::shared_ptr shared_ptr; - ///TODO comment + /// A pair of const references to the key and value, the dereferenced type of the const_iterator and const_reverse_iterator + typedef std::pair ConstKeyValuePair; + + /// A pair of references to the key and value, the dereferenced type of the iterator and reverse_iterator + typedef std::pair KeyValuePair; + + /// Mutable forward iterator, with value type KeyValuePair + typedef boost::transform_iterator< + boost::function1, KeyValueMap::iterator> iterator; + + /// Const forward iterator, with value type ConstKeyValuePair + typedef boost::transform_iterator< + boost::function1, KeyValueMap::const_iterator> const_iterator; + + /// Mutable reverse iterator, with value type KeyValuePair + typedef boost::transform_iterator< + boost::function1, KeyValueMap::reverse_iterator> reverse_iterator; + + /// Const reverse iterator, with value type ConstKeyValuePair + typedef boost::transform_iterator< + boost::function1, KeyValueMap::const_reverse_iterator> const_reverse_iterator; + + /** Default constructor creates an empty Values class */ Values() {} - ///TODO: comment - Values(const Values& config) : - values_(config.values_) {} + /** Copy constructor duplicates all keys and values */ + Values(const Values& other); - ///TODO: comment - template - Values(const Values& other) {} // do nothing when initializing with wrong type - virtual ~Values() {} - - /// @} /// @name Testable /// @{ - /** print */ - void print(const std::string &s="") const; + /** print method for testing and debugging */ + void print(const std::string& str = "") const; - /** Test whether configs are identical in keys and values */ - bool equals(const Values& expected, double tol=1e-9) const; + /** Test whether the sets of keys and values are identical */ + bool equals(const Values& other, double tol=1e-9) const; /// @} - /// @name Standard Interface - /// @{ - /** Retrieve a variable by j, throws KeyDoesNotExist if not found */ - const Value& at(const J& j) const; + /** Retrieve a variable by key \c j. The type of the value associated with + * this key is supplied as a template argument to this function. + * @param j Retrieve the value associated with this key + * @tparam Value The type of the value stored with this key, this method + * throws DynamicValuesIncorrectType if this requested type is not correct. + * @return A const reference to the stored value + */ + template + const ValueType& at(const Symbol& j) const; - /** operator[] syntax for get, throws KeyDoesNotExist if not found */ - const Value& operator[](const J& j) const { + /** Retrieve a variable using a special key (typically TypedSymbol), which + * contains the type of the value associated with the key, and which must + * be conversion constructible to a Symbol, e.g. + * Symbol(const TypedKey&). Throws DynamicValuesKeyDoesNotExist + * the key is not found, and DynamicValuesIncorrectType if the value type + * associated with the requested key does not match the stored value type. + */ + template + const typename TypedKey::Value& at(const TypedKey& j) const; + + /** operator[] syntax for at(const TypedKey& j) */ + template + const typename TypedKey::Value& operator[](const TypedKey& j) const { return at(j); } - /** Check if a variable exists */ - bool exists(const J& i) const { return values_.find(i)!=values_.end(); } + /** Check if a value exists with key \c j. See exists<>(const Symbol& j) + * and exists(const TypedKey& j) for versions that return the value if it + * exists. */ + bool exists(const Symbol& j) const; - /** Check if a variable exists and return it if so */ - boost::optional exists_(const J& i) const { - const_iterator it = values_.find(i); - if (it==values_.end()) return boost::none; else return it->second; - } + /** Check if a value with key \c j exists, returns the value with type + * \c Value if the key does exist, or boost::none if it does not exist. + * Throws DynamicValuesIncorrectType if the value type associated with the + * requested key does not match the stored value type. */ + template + boost::optional exists(const Symbol& j) const; + + /** Check if a value with key \c j exists, returns the value with type + * \c Value if the key does exist, or boost::none if it does not exist. + * Uses a special key (typically TypedSymbol), which contains the type of + * the value associated with the key, and which must be conversion + * constructible to a Symbol, e.g. Symbol(const TypedKey&). Throws + * DynamicValuesIncorrectType if the value type associated with the + * requested key does not match the stored value type. + */ + template + boost::optional exists(const TypedKey& j) const; /** The number of variables in this config */ size_t size() const { return values_.size(); } @@ -130,10 +174,64 @@ namespace gtsam { bool empty() const { return values_.empty(); } /** Get a zero VectorValues of the correct structure */ - VectorValues zero(const Ordering& ordering) const; + VectorValues zeroVectors(const Ordering& ordering) const; - const_iterator begin() const { return values_.begin(); } /// make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) { + return std::make_pair(key_value.first, *key_value.second); } + static std::pair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) { + return std::make_pair(key_value.first, *key_value.second); } + + public: + const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); } + const_iterator end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); } + iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); } + iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); } + const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); } + const_reverse_iterator rend() const { return boost::make_transform_iterator(values_.rend(), &make_const_deref_pair); } + reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); } + reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); } + + /// @name Manifold Operations + /// @{ + + /** Add a delta config to current config and returns a new config */ + Values retract(const VectorValues& delta, const Ordering& ordering) const; + + /** Get a delta config about a linearization point c0 (*this) */ + VectorValues localCoordinates(const Values& cp, const Ordering& ordering) const; + + /** Get a delta config about a linearization point c0 (*this) */ + void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const; + + ///@} + + /** Add a variable with the given j, throws KeyAlreadyExists if j is already present */ + void insert(const Symbol& j, const Value& val); + + /** Add a set of variables, throws KeyAlreadyExists if a key is already present */ + void insert(const Values& values); + + /** single element change of existing element */ + void update(const Symbol& j, const Value& val); + + /** update the current available values without adding new ones */ + void update(const Values& values); + + /** Remove a variable from the config, throws KeyDoesNotExist if j is not present */ + void erase(const Symbol& j); + + /** + * Returns a set of keys in the config + * Note: by construction, the list is ordered + */ + FastList keys() const; + + /** Replace all keys and variables */ + Values& operator=(const Values& rhs); + + /** Remove all variables from the config */ + void clear() { values_.clear(); } /** Create an array of variable dimensions using the given ordering */ std::vector dims(const Ordering& ordering) const; @@ -146,174 +244,97 @@ namespace gtsam { */ Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const; - /// @} - /// @name Manifold Operations - /// @{ - - /** The dimensionality of the tangent space */ - size_t dim() const; - - /** Add a delta config to current config and returns a new config */ - Values retract(const VectorValues& delta, const Ordering& ordering) const; - - /** Get a delta config about a linearization point c0 (*this) */ - VectorValues localCoordinates(const Values& cp, const Ordering& ordering) const; - - /** Get a delta config about a linearization point c0 (*this) */ - void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const; - - /// @} - /// @name Advanced Interface - /// @{ - - // imperative methods: - - iterator begin() { return values_.begin(); } /// if j is already present */ - void insert(const J& j, const Value& val); - - /** Add a set of variables, throws KeyAlreadyExists if a key is already present */ - void insert(const Values& cfg); - - /** update the current available values without adding new ones */ - void update(const Values& cfg); - - /** single element change of existing element */ - void update(const J& j, const Value& val); - - /** Remove a variable from the config, throws KeyDoesNotExist if j is not present */ - void erase(const J& j); - - /** Remove a variable from the config while returning the dimensionality of - * the removed element (normally not needed by user code), throws - * KeyDoesNotExist if j is not present. - */ - void erase(const J& j, size_t& dim); - - /** - * Returns a set of keys in the config - * Note: by construction, the list is ordered - */ - std::list keys() const; - - /** Replace all keys and variables */ - Values& operator=(const Values& rhs) { - values_ = rhs.values_; - return (*this); - } - - /** Remove all variables from the config */ - void clear() { - values_.clear(); - } - - /** - * Apply a class with an application operator() to a const_iterator over - * every pair. The operator must be able to handle such an - * iterator for every type in the Values, (i.e. through templating). - */ - template - void apply(A& operation) { - for(iterator it = begin(); it != end(); ++it) - operation(it); - } - template - void apply(A& operation) const { - for(const_iterator it = begin(); it != end(); ++it) - operation(it); - } - - /// @} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(values_); - } - - }; - - struct _ValuesDimensionCollector { - const Ordering& ordering; - std::vector dimensions; - _ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {} - template void operator()(const I& key_value) { - Index var; - if(ordering.tryAt(key_value->first, var)) { - assert(var < dimensions.size()); - dimensions[var] = key_value->second.dim(); - } - } }; /* ************************************************************************* */ - struct _ValuesKeyOrderer { - Index var; - Ordering::shared_ptr ordering; - _ValuesKeyOrderer(Index firstVar) : var(firstVar), ordering(new Ordering) {} - template void operator()(const I& key_value) { - ordering->insert(key_value->first, var); - ++ var; + class ValuesKeyAlreadyExists : public std::exception { + protected: + const Symbol key_; ///< The key that already existed + + private: + mutable std::string message_; + + public: + /// Construct with the key-value pair attemped to be added + ValuesKeyAlreadyExists(const Symbol& key) throw() : + key_(key) {} + + virtual ~ValuesKeyAlreadyExists() throw() {} + + /// The duplicate key that was attemped to be added + const Symbol& key() const throw() { return key_; } + + /// The message to be displayed to the user + virtual const char* what() const throw(); + }; + + /* ************************************************************************* */ + class ValuesKeyDoesNotExist : public std::exception { + protected: + const char* operation_; ///< The operation that attempted to access the key + const Symbol key_; ///< The key that does not exist + + private: + mutable std::string message_; + + public: + /// Construct with the key that does not exist in the values + ValuesKeyDoesNotExist(const char* operation, const Symbol& key) throw() : + operation_(operation), key_(key) {} + + virtual ~ValuesKeyDoesNotExist() throw() {} + + /// The key that was attempted to be accessed that does not exist + const Symbol& key() const throw() { return key_; } + + /// The message to be displayed to the user + virtual const char* what() const throw(); + }; + + /* ************************************************************************* */ + class ValuesIncorrectType : public std::exception { + protected: + const Symbol key_; ///< The key requested + const std::type_info& storedTypeId_; + const std::type_info& requestedTypeId_; + + private: + mutable std::string message_; + + public: + /// Construct with the key that does not exist in the values + ValuesIncorrectType(const Symbol& key, + const std::type_info& storedTypeId, const std::type_info& requestedTypeId) throw() : + key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {} + + virtual ~ValuesIncorrectType() throw() {} + + /// The key that was attempted to be accessed that does not exist + const Symbol& key() const throw() { return key_; } + + /// The typeid of the value stores in the Values + const std::type_info& storedTypeId() const { return storedTypeId_; } + + /// The requested typeid + const std::type_info& requestedTypeId() const { return requestedTypeId_; } + + /// The message to be displayed to the user + virtual const char* what() const throw(); + }; + + /* ************************************************************************* */ + class DynamicValuesMismatched : public std::exception { + + public: + DynamicValuesMismatched() throw() {} + + virtual ~DynamicValuesMismatched() throw() {} + + virtual const char* what() const throw() { + return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values"; } }; - -/* ************************************************************************* */ -template -class KeyAlreadyExists : public std::exception { -protected: - const J key_; ///< The key that already existed - const typename J::Value value_; ///< The value attempted to be inserted - -private: - mutable std::string message_; - -public: - /// Construct with the key-value pair attemped to be added - KeyAlreadyExists(const J& key, const typename J::Value& value) throw() : - key_(key), value_(value) {} - - virtual ~KeyAlreadyExists() throw() {} - - /// The duplicate key that was attemped to be added - const J& key() const throw() { return key_; } - - /// The value that was attempted to be added - const typename J::Value& value() const throw() { return value_; } - - /// The message to be displayed to the user - virtual const char* what() const throw(); -}; - -/* ************************************************************************* */ -template -class KeyDoesNotExist : public std::exception { -protected: - const char* operation_; ///< The operation that attempted to access the key - const J key_; ///< The key that does not exist - -private: - mutable std::string message_; - -public: - /// Construct with the key that does not exist in the values - KeyDoesNotExist(const char* operation, const J& key) throw() : - operation_(operation), key_(key) {} - - virtual ~KeyDoesNotExist() throw() {} - - /// The key that was attempted to be accessed that does not exist - const J& key() const throw() { return key_; } - - /// The message to be displayed to the user - virtual const char* what() const throw(); -}; - -} // \namespace gtsam +} #include - diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 3bf76d681..c66e59ead 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testValues.cpp + * @file testDynamicValues.cpp * @author Richard Roberts */ @@ -22,6 +22,7 @@ using namespace boost::assign; #include #include +#include #include using namespace gtsam; @@ -29,27 +30,26 @@ using namespace std; static double inf = std::numeric_limits::infinity(); typedef TypedSymbol VecKey; -typedef Values TestValues; VecKey key1(1), key2(2), key3(3), key4(4); /* ************************************************************************* */ -TEST( TestValues, equals1 ) +TEST( Values, equals1 ) { - TestValues expected; - Vector v = Vector_(3, 5.0, 6.0, 7.0); + Values expected; + LieVector v(3, 5.0, 6.0, 7.0); expected.insert(key1,v); - TestValues actual; + Values actual; actual.insert(key1,v); CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( TestValues, equals2 ) +TEST( Values, equals2 ) { - TestValues cfg1, cfg2; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, 5.0, 6.0, 8.0); + Values cfg1, cfg2; + LieVector v1(3, 5.0, 6.0, 7.0); + LieVector v2(3, 5.0, 6.0, 8.0); cfg1.insert(key1, v1); cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); @@ -57,11 +57,11 @@ TEST( TestValues, equals2 ) } /* ************************************************************************* */ -TEST( TestValues, equals_nan ) +TEST( Values, equals_nan ) { - TestValues cfg1, cfg2; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, inf, inf, inf); + Values cfg1, cfg2; + LieVector v1(3, 5.0, 6.0, 7.0); + LieVector v2(3, inf, inf, inf); cfg1.insert(key1, v1); cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); @@ -69,13 +69,13 @@ TEST( TestValues, equals_nan ) } /* ************************************************************************* */ -TEST( TestValues, insert_good ) +TEST( Values, insert_good ) { - TestValues cfg1, cfg2, expected; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, 8.0, 9.0, 1.0); - Vector v3 = Vector_(3, 2.0, 4.0, 3.0); - Vector v4 = Vector_(3, 8.0, 3.0, 7.0); + Values cfg1, cfg2, expected; + LieVector v1(3, 5.0, 6.0, 7.0); + LieVector v2(3, 8.0, 9.0, 1.0); + LieVector v3(3, 2.0, 4.0, 3.0); + LieVector v4(3, 8.0, 3.0, 7.0); cfg1.insert(key1, v1); cfg1.insert(key2, v2); cfg2.insert(key3, v4); @@ -86,31 +86,31 @@ TEST( TestValues, insert_good ) expected.insert(key2, v2); expected.insert(key3, v4); - CHECK(assert_equal(cfg1, expected)); + CHECK(assert_equal(expected, cfg1)); } /* ************************************************************************* */ -TEST( TestValues, insert_bad ) +TEST( Values, insert_bad ) { - TestValues cfg1, cfg2; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, 8.0, 9.0, 1.0); - Vector v3 = Vector_(3, 2.0, 4.0, 3.0); - Vector v4 = Vector_(3, 8.0, 3.0, 7.0); + Values cfg1, cfg2; + LieVector v1(3, 5.0, 6.0, 7.0); + LieVector v2(3, 8.0, 9.0, 1.0); + LieVector v3(3, 2.0, 4.0, 3.0); + LieVector v4(3, 8.0, 3.0, 7.0); cfg1.insert(key1, v1); cfg1.insert(key2, v2); cfg2.insert(key2, v3); cfg2.insert(key3, v4); - CHECK_EXCEPTION(cfg1.insert(cfg2), const KeyAlreadyExists); + CHECK_EXCEPTION(cfg1.insert(cfg2), ValuesKeyAlreadyExists); } /* ************************************************************************* */ -TEST( TestValues, update_element ) +TEST( Values, update_element ) { - TestValues cfg; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, 8.0, 9.0, 1.0); + Values cfg; + LieVector v1(3, 5.0, 6.0, 7.0); + LieVector v2(3, 8.0, 9.0, 1.0); cfg.insert(key1, v1); CHECK(cfg.size() == 1); @@ -122,12 +122,12 @@ TEST( TestValues, update_element ) } ///* ************************************************************************* */ -//TEST(TestValues, dim_zero) +//TEST(Values, dim_zero) //{ -// TestValues config0; -// config0.insert(key1, Vector_(2, 2.0, 3.0)); -// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); -// LONGS_EQUAL(5,config0.dim()); +// Values config0; +// config0.insert(key1, LieVector(2, 2.0, 3.0)); +// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); +// LONGS_EQUAL(5, config0.dim()); // // VectorValues expected; // expected.insert(key1, zero(2)); @@ -136,151 +136,130 @@ TEST( TestValues, update_element ) //} /* ************************************************************************* */ -TEST(TestValues, expmap_a) +TEST(Values, expmap_a) { - TestValues config0; - config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); - config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); + Values config0; + config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); + config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); Ordering ordering(*config0.orderingArbitrary()); VectorValues increment(config0.dims(ordering)); increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2); increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); - TestValues expected; - expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); - expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); + Values expected; + expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); + expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); CHECK(assert_equal(expected, config0.retract(increment, ordering))); } -///* ************************************************************************* */ -//TEST(TestValues, expmap_b) -//{ -// TestValues config0; -// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); -// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); -// -// Ordering ordering(*config0.orderingArbitrary()); -// VectorValues increment(config0.dims(ordering)); -// increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); -// -// TestValues expected; -// expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); -// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); -// -// CHECK(assert_equal(expected, config0.retract(increment, ordering))); -//} +/* ************************************************************************* */ +TEST(Values, expmap_b) +{ + Values config0; + config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); + config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); -///* ************************************************************************* */ -//TEST(TestValues, expmap_c) + Ordering ordering(*config0.orderingArbitrary()); + VectorValues increment(VectorValues::Zero(config0.dims(ordering))); + increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5); + + Values expected; + expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); + expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); + + CHECK(assert_equal(expected, config0.retract(increment, ordering))); +} + +/* ************************************************************************* */ +//TEST(Values, expmap_c) //{ -// TestValues config0; -// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); -// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); +// Values config0; +// config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); +// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); // -// Vector increment = Vector_(6, +// Vector increment = LieVector(6, // 1.0, 1.1, 1.2, // 1.3, 1.4, 1.5); // -// TestValues expected; -// expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); -// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); +// Values expected; +// expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); +// expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); // // CHECK(assert_equal(expected, config0.retract(increment))); //} /* ************************************************************************* */ -/*TEST(TestValues, expmap_d) +TEST(Values, expmap_d) { - TestValues config0; - config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); - config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); + Values config0; + config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); + config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); //config0.print("config0"); CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); - TestValues poseconfig; - poseconfig.insert("p1", Pose2(1,2,3)); - poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5)); - //poseconfig.print("poseconfig"); + typedef TypedSymbol PoseKey; + Values poseconfig; + poseconfig.insert(PoseKey(1), Pose2(1,2,3)); + poseconfig.insert(PoseKey(2), Pose2(0.3, 0.4, 0.5)); + CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); -}*/ +} /* ************************************************************************* */ -/*TEST(TestValues, extract_keys) +TEST(Values, extract_keys) { typedef TypedSymbol PoseKey; - TestValues config; + Values config; config.insert(PoseKey(1), Pose2()); config.insert(PoseKey(2), Pose2()); config.insert(PoseKey(4), Pose2()); config.insert(PoseKey(5), Pose2()); - list expected, actual; + FastList expected, actual; expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5); actual = config.keys(); CHECK(actual.size() == expected.size()); - list::const_iterator itAct = actual.begin(), itExp = expected.begin(); + FastList::const_iterator itAct = actual.begin(), itExp = expected.begin(); for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) { CHECK(assert_equal(*itExp, *itAct)); } -}*/ +} /* ************************************************************************* */ -TEST(TestValues, exists_) +TEST(Values, exists_) { - TestValues config0; - config0.insert(key1, Vector_(1, 1.)); - config0.insert(key2, Vector_(1, 2.)); + Values config0; + config0.insert(key1, LieVector(Vector_(1, 1.))); + config0.insert(key2, LieVector(Vector_(1, 2.))); - boost::optional v = config0.exists_(key1); + boost::optional v = config0.exists(key1); CHECK(assert_equal(Vector_(1, 1.),*v)); } /* ************************************************************************* */ -TEST(TestValues, update) +TEST(Values, update) { - TestValues config0; - config0.insert(key1, Vector_(1, 1.)); - config0.insert(key2, Vector_(1, 2.)); + Values config0; + config0.insert(key1, LieVector(1, 1.)); + config0.insert(key2, LieVector(1, 2.)); - TestValues superset; - superset.insert(key1, Vector_(1, -1.)); - superset.insert(key2, Vector_(1, -2.)); - superset.insert(key3, Vector_(1, -3.)); + Values superset; + superset.insert(key1, LieVector(1, -1.)); + superset.insert(key2, LieVector(1, -2.)); config0.update(superset); - TestValues expected; - expected.insert(key1, Vector_(1, -1.)); - expected.insert(key2, Vector_(1, -2.)); + Values expected; + expected.insert(key1, LieVector(1, -1.)); + expected.insert(key2, LieVector(1, -2.)); CHECK(assert_equal(expected,config0)); } -/* ************************************************************************* */ -TEST(TestValues, dummy_initialization) -{ - typedef TypedSymbol Key; - typedef Values Values1; - - Values1 init1; - init1.insert(Key(1), Vector_(2, 1.0, 2.0)); - init1.insert(Key(2), Vector_(2, 4.0, 3.0)); - - TestValues init2; - init2.insert(key1, Vector_(2, 1.0, 2.0)); - init2.insert(key2, Vector_(2, 4.0, 3.0)); - - Values1 actual1(init2); - TestValues actual2(init1); - - EXPECT(actual1.empty()); - EXPECT(actual2.empty()); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index ad33a67cd..8ed7f284f 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -27,27 +27,26 @@ namespace gtsam { * linearized Hessian matrices of the original factor, but with the opposite sign. This effectively * cancels out any affects of the original factor during optimization." */ - template - class AntiFactor: public NonlinearFactor { + class AntiFactor: public NonlinearFactor { private: - typedef AntiFactor This; - typedef NonlinearFactor Base; - typedef typename NonlinearFactor::shared_ptr sharedFactor; + typedef AntiFactor This; + typedef NonlinearFactor Base; + typedef NonlinearFactor::shared_ptr sharedFactor; sharedFactor factor_; public: // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; /** default constructor - only use for serialization */ AntiFactor() {} /** constructor - Creates the equivalent AntiFactor from an existing factor */ - AntiFactor(typename NonlinearFactor::shared_ptr factor) : Base(factor->begin(), factor->end()), factor_(factor) {} + AntiFactor(NonlinearFactor::shared_ptr factor) : Base(factor->begin(), factor->end()), factor_(factor) {} virtual ~AntiFactor() {} @@ -60,7 +59,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol); } @@ -72,7 +71,7 @@ namespace gtsam { * For the AntiFactor, this will have the same magnitude of the original factor, * but the opposite sign. */ - double error(const VALUES& c) const { return -factor_->error(c); } + double error(const Values& c) const { return -factor_->error(c); } /** get the dimension of the factor (same as the original factor) */ size_t dim() const { return factor_->dim(); } @@ -81,7 +80,7 @@ namespace gtsam { * Checks whether this factor should be used based on a set of values. * The AntiFactor will have the same 'active' profile as the original factor. */ - bool active(const VALUES& c) const { return factor_->active(c); } + bool active(const Values& c) const { return factor_->active(c); } /** * Linearize to a GaussianFactor. The AntiFactor always returns a Hessian Factor @@ -90,7 +89,7 @@ namespace gtsam { * effectively cancels the effect of the original factor on the factor graph. */ boost::shared_ptr - linearize(const VALUES& c, const Ordering& ordering) const { + linearize(const Values& c, const Ordering& ordering) const { // Generate the linearized factor from the contained nonlinear factor GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c, ordering); diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index d17dd135e..eaaf3d4e9 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -25,16 +25,16 @@ namespace gtsam { /** * Binary factor for a bearing measurement */ - template - class BearingFactor: public NonlinearFactor2 { + template + class BearingFactor: public NonlinearFactor2 { private: typedef typename POSEKEY::Value Pose; typedef typename POSEKEY::Value::Rotation Rot; typedef typename POINTKEY::Value Point; - typedef BearingFactor This; - typedef NonlinearFactor2 Base; + typedef BearingFactor This; + typedef NonlinearFactor2 Base; Rot z_; /** measurement */ @@ -68,7 +68,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && this->z_.equals(e->z_, tol); } diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 833708b5d..ef71f8f13 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -27,16 +27,16 @@ namespace gtsam { /** * Binary factor for a bearing measurement */ - template - class BearingRangeFactor: public NonlinearFactor2 { + template + class BearingRangeFactor: public NonlinearFactor2 { private: typedef typename POSEKEY::Value Pose; typedef typename POSEKEY::Value::Rotation Rot; typedef typename POINTKEY::Value Point; - typedef BearingRangeFactor This; - typedef NonlinearFactor2 Base; + typedef BearingRangeFactor This; + typedef NonlinearFactor2 Base; // the measurement Rot bearing_; @@ -68,7 +68,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && fabs(this->range_ - e->range_) < tol && diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index dee18ebc7..707dc3350 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -29,13 +29,13 @@ namespace gtsam { * KEY1::Value is the Lie Group type * T is the measurement type, by default the same */ - template - class BetweenFactor: public NonlinearFactor2 { + template + class BetweenFactor: public NonlinearFactor2 { private: - typedef BetweenFactor This; - typedef NonlinearFactor2 Base; + typedef BetweenFactor This; + typedef NonlinearFactor2 Base; T measured_; /** The measurement */ @@ -71,7 +71,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); } @@ -114,15 +114,15 @@ namespace gtsam { * This constraint requires the underlying type to a Lie type * */ - template - class BetweenConstraint : public BetweenFactor { + template + class BetweenConstraint : public BetweenFactor { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; /** Syntactic sugar for constrained version */ BetweenConstraint(const typename KEY::Value& measured, const KEY& key1, const KEY& key2, double mu = 1000.0) - : BetweenFactor(key1, key2, measured, + : BetweenFactor(key1, key2, measured, noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {} private: @@ -132,7 +132,7 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("BetweenFactor", - boost::serialization::base_object >(*this)); + boost::serialization::base_object >(*this)); } }; // \class BetweenConstraint diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index e8f59cb2d..f3df76774 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -28,11 +28,11 @@ namespace gtsam { * will need to have its value function implemented to return * a scalar for comparison. */ -template -struct BoundingConstraint1: public NonlinearFactor1 { +template +struct BoundingConstraint1: public NonlinearFactor1 { typedef typename KEY::Value X; - typedef NonlinearFactor1 Base; - typedef boost::shared_ptr > shared_ptr; + typedef NonlinearFactor1 Base; + typedef boost::shared_ptr > shared_ptr; double threshold_; bool isGreaterThan_; /// flag for greater/less than @@ -57,7 +57,7 @@ struct BoundingConstraint1: public NonlinearFactor1 { boost::none) const = 0; /** active when constraint *NOT* met */ - bool active(const VALUES& c) const { + bool active(const Values& c) const { // note: still active at equality to avoid zigzagging double x = value(c[this->key_]); return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; @@ -95,13 +95,13 @@ private: * Binary scalar inequality constraint, with a similar value() function * to implement for specific systems */ -template -struct BoundingConstraint2: public NonlinearFactor2 { +template +struct BoundingConstraint2: public NonlinearFactor2 { typedef typename KEY1::Value X1; typedef typename KEY2::Value X2; - typedef NonlinearFactor2 Base; - typedef boost::shared_ptr > shared_ptr; + typedef NonlinearFactor2 Base; + typedef boost::shared_ptr > shared_ptr; double threshold_; bool isGreaterThan_; /// flag for greater/less than @@ -125,7 +125,7 @@ struct BoundingConstraint2: public NonlinearFactor2 { boost::optional H2 = boost::none) const = 0; /** active when constraint *NOT* met */ - bool active(const VALUES& c) const { + bool active(const Values& c) const { // note: still active at equality to avoid zigzagging double x = value(c[this->key1_], c[this->key2_]); return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index ee3bdb38d..c0cd9e6c3 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -29,21 +29,21 @@ namespace gtsam { /** * Non-linear factor for a constraint derived from a 2D measurement. The calibration is unknown here compared to GenericProjectionFactor */ - template + template class GeneralSFMFactor: - public NonlinearFactor2 { + public NonlinearFactor2 { protected: Point2 z_; ///< the 2D measurement public: typedef typename CamK::Value Cam; ///< typedef for camera type - typedef GeneralSFMFactor Self ; ///< typedef for this object - typedef NonlinearFactor2 Base; ///< typedef for the base class + typedef GeneralSFMFactor Self ; ///< typedef for this object + typedef NonlinearFactor2 Base; ///< typedef for the base class typedef Point2 Measurement; ///< typedef for the measurement // shorthand for a smart pointer to a factor - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; /** * Constructor @@ -72,7 +72,7 @@ namespace gtsam { /** * equals */ - bool equals(const GeneralSFMFactor &p, double tol = 1e-9) const { + bool equals(const GeneralSFMFactor &p, double tol = 1e-9) const { return Base::equals(p, tol) && this->z_.equals(p.z_, tol) ; } diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index eb1c4614c..651e55500 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.h @@ -38,16 +38,16 @@ namespace gtsam { * For practical use, it would be good to subclass this factor and have the class type * construct the mask. */ - template - class PartialPriorFactor: public NonlinearFactor1 { + template + class PartialPriorFactor: public NonlinearFactor1 { public: typedef typename KEY::Value T; protected: - typedef NonlinearFactor1 Base; - typedef PartialPriorFactor This; + typedef NonlinearFactor1 Base; + typedef PartialPriorFactor This; Vector prior_; ///< measurement on logmap parameters, in compressed form std::vector mask_; ///< indices of values to constrain in compressed prior vector @@ -95,7 +95,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) && diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 2d7b631cc..fa33a9bc7 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -28,15 +28,15 @@ namespace gtsam { * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so * a simple type like int will not work */ - template - class PriorFactor: public NonlinearFactor1 { + template + class PriorFactor: public NonlinearFactor1 { public: typedef typename KEY::Value T; private: - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 Base; T prior_; /** The measurement */ @@ -69,9 +69,8 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const PriorFactor *e = dynamic_cast*> (&expected); + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const PriorFactor *e = dynamic_cast*> (&expected); return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol); } diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 73d18a7e6..dcf425aaa 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -29,8 +29,8 @@ namespace gtsam { * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. * i.e. the main building block for visual SLAM. */ - template - class GenericProjectionFactor: public NonlinearFactor2 { + template + class GenericProjectionFactor: public NonlinearFactor2 { protected: // Keep a copy of measurement and calibration for I/O @@ -40,10 +40,10 @@ namespace gtsam { public: /// shorthand for base class type - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; /// Default constructor GenericProjectionFactor() : @@ -73,7 +73,7 @@ namespace gtsam { } /// equals - bool equals(const GenericProjectionFactor& p + bool equals(const GenericProjectionFactor& p , double tol = 1e-9) const { return Base::equals(p, tol) && this->z_.equals(p.z_, tol) && this->K_->equals(*p.K_, tol); diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 93cef86f1..b762c5690 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -25,14 +25,14 @@ namespace gtsam { /** * Binary factor for a range measurement */ - template - class RangeFactor: public NonlinearFactor2 { + template + class RangeFactor: public NonlinearFactor2 { private: double z_; /** measurement */ - typedef RangeFactor This; - typedef NonlinearFactor2 Base; + typedef RangeFactor This; + typedef NonlinearFactor2 Base; typedef typename POSEKEY::Value Pose; typedef typename POINTKEY::Value Point; @@ -64,7 +64,7 @@ namespace gtsam { } /** equals specialized to this factor */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && fabs(this->z_ - e->z_) < tol; } diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 729b9fff1..7f1e8077d 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -22,8 +22,8 @@ namespace gtsam { -template -class GenericStereoFactor: public NonlinearFactor2 { +template +class GenericStereoFactor: public NonlinearFactor2 { private: // Keep a copy of measurement and calibration for I/O @@ -33,7 +33,7 @@ private: public: // shorthand for base class type - typedef NonlinearFactor2 Base; ///< typedef for base class + typedef NonlinearFactor2 Base; ///< typedef for base class typedef boost::shared_ptr shared_ptr; ///< typedef for shared pointer to this object typedef typename KEY1::Value CamPose; ///< typedef for Pose Lie Value type diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index af964c0ab..73fee971e 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -29,7 +29,6 @@ using namespace gtsam; #define LINESIZE 81920 typedef boost::shared_ptr sharedPose2Graph; -typedef boost::shared_ptr sharedPose2Values; typedef pose2SLAM::Odometry Pose2Factor; ///< Typedef for Constraint class for backwards compatibility namespace gtsam { @@ -66,13 +65,13 @@ pair > dataset(const string& dataset, c /* ************************************************************************* */ -pair load2D( +pair load2D( pair > dataset, int maxID, bool addNoise, bool smart) { return load2D(dataset.first, dataset.second, maxID, addNoise, smart); } -pair load2D(const string& filename, +pair load2D(const string& filename, boost::optional model, int maxID, bool addNoise, bool smart) { cout << "Will try to read " << filename << endl; ifstream is(filename.c_str()); @@ -81,7 +80,7 @@ pair load2D(const string& filename, exit(-1); } - sharedPose2Values poses(new pose2SLAM::Values); + Values::shared_ptr poses(new Values); sharedPose2Graph graph(new pose2SLAM::Graph); string tag; @@ -96,7 +95,7 @@ pair load2D(const string& filename, is >> id >> x >> y >> yaw; // optional filter if (maxID && id >= maxID) continue; - poses->insert(id, Pose2(x, y, yaw)); + poses->insert(pose2SLAM::PoseKey(id), Pose2(x, y, yaw)); } is.ignore(LINESIZE, '\n'); } @@ -133,8 +132,8 @@ pair load2D(const string& filename, l1Xl2 = l1Xl2.retract((*model)->sample()); // Insert vertices if pure odometry file - if (!poses->exists(id1)) poses->insert(id1, Pose2()); - if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * l1Xl2); + if (!poses->exists(pose2SLAM::PoseKey(id1))) poses->insert(pose2SLAM::PoseKey(id1), Pose2()); + if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at(pose2SLAM::PoseKey(id1)) * l1Xl2); pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model)); graph->push_back(factor); @@ -149,26 +148,25 @@ pair load2D(const string& filename, } /* ************************************************************************* */ -void save2D(const pose2SLAM::Graph& graph, const pose2SLAM::Values& config, const SharedDiagonal model, +void save2D(const pose2SLAM::Graph& graph, const Values& config, const SharedDiagonal model, const string& filename) { - typedef pose2SLAM::Values::Key Key; fstream stream(filename.c_str(), fstream::out); // save poses - pose2SLAM::Values::Key key; - Pose2 pose; - BOOST_FOREACH(boost::tie(key, pose), config) - stream << "VERTEX2 " << key.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) { + const Pose2& pose = dynamic_cast(key_value.second); + stream << "VERTEX2 " << key_value.first.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; + } // save edges Matrix R = model->R(); Matrix RR = trans(R)*R;//prod(trans(R),R); - BOOST_FOREACH(boost::shared_ptr > factor_, graph) { + BOOST_FOREACH(boost::shared_ptr factor_, graph) { boost::shared_ptr factor = boost::dynamic_pointer_cast(factor_); if (!factor) continue; - pose = factor->measured().inverse(); + Pose2 pose = factor->measured().inverse(); stream << "EDGE2 " << factor->key2().index() << " " << factor->key1().index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 99e7f7eef..5e06e583d 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -40,16 +40,16 @@ std::pair > * @param maxID, if non-zero cut out vertices >= maxID * @param smart: try to reduce complexity of covariance to cheapest model */ -std::pair, boost::shared_ptr > load2D( +std::pair, boost::shared_ptr > load2D( std::pair > dataset, int maxID = 0, bool addNoise=false, bool smart=true); -std::pair, boost::shared_ptr > load2D( +std::pair, boost::shared_ptr > load2D( const std::string& filename, boost::optional model = boost::optional(), int maxID = 0, bool addNoise=false, bool smart=true); /** save 2d graph */ -void save2D(const pose2SLAM::Graph& graph, const pose2SLAM::Values& config, const gtsam::SharedDiagonal model, +void save2D(const pose2SLAM::Graph& graph, const Values& config, const gtsam::SharedDiagonal model, const std::string& filename); /** diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index 6942ab429..e8d674f6c 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -23,8 +23,8 @@ // Use planarSLAM namespace for specific SLAM instance namespace planarSLAM { - Graph::Graph(const NonlinearFactorGraph& graph) : - NonlinearFactorGraph(graph) {} + Graph::Graph(const NonlinearFactorGraph& graph) : + NonlinearFactorGraph(graph) {} void Graph::addPrior(const PoseKey& i, const Pose2& p, const SharedNoiseModel& model) { diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index b8e887f8a..5740d893f 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -22,7 +22,6 @@ #include #include #include -#include #include #include #include @@ -39,107 +38,63 @@ namespace planarSLAM { /// Typedef for a PointKey with Point2 data and 'l' symbol typedef TypedSymbol PointKey; - - /// Typedef for Values structure with PoseKey type - typedef Values PoseValues; - - /// Typedef for Values structure with PointKey type - typedef Values PointValues; - - /// Values class, inherited from TupleValues2, using PoseKeys and PointKeys - struct Values: public TupleValues2 { - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const TupleValues2& values) : - TupleValues2(values) { - } - - /// Copy constructor - Values(const TupleValues2::Base& values) : - TupleValues2(values) { - } - - /// From sub-values - Values(const PoseValues& poses, const PointValues& points) : - TupleValues2(poses, points) { - } - - // Convenience for MATLAB wrapper, which does not allow for identically named methods - - /// get a pose - Pose2 pose(int key) const { return (*this)[PoseKey(key)]; } - - /// get a point - Point2 point(int key) const { return (*this)[PointKey(key)]; } - - /// insert a pose - void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); } - - /// insert a point - void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); } - }; - /** * List of typedefs for factors */ + /// A hard constraint for PoseKeys to enforce particular values + typedef NonlinearEquality Constraint; + /// A prior factor to bias the value of a PoseKey + typedef PriorFactor Prior; + /// A factor between two PoseKeys set with a Pose2 + typedef BetweenFactor Odometry; + /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) + typedef BearingFactor Bearing; + /// A factor between a PoseKey and a PointKey to express distance between them (set with a double) + typedef RangeFactor Range; + /// A factor between a PoseKey and a PointKey to express difference in rotation and location + typedef BearingRangeFactor BearingRange; - /// A hard constraint for PoseKeys to enforce particular values - typedef NonlinearEquality Constraint; - /// A prior factor to bias the value of a PoseKey - typedef PriorFactor Prior; - /// A factor between two PoseKeys set with a Pose2 - typedef BetweenFactor Odometry; - /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) - typedef BearingFactor Bearing; - /// A factor between a PoseKey and a PointKey to express distance between them (set with a double) - typedef RangeFactor Range; - /// A factor between a PoseKey and a PointKey to express difference in rotation and location - typedef BearingRangeFactor BearingRange; + /// Creates a NonlinearFactorGraph with the Values type + struct Graph: public NonlinearFactorGraph { - /// Creates a NonlinearFactorGraph with the Values type - struct Graph: public NonlinearFactorGraph { + /// Default constructor for a NonlinearFactorGraph + Graph(){} - /// Default constructor for a NonlinearFactorGraph - Graph(){} + /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph + Graph(const NonlinearFactorGraph& graph); - /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph); + /// Biases the value of PoseKey key with Pose2 p given a noise model + void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel); - /// Biases the value of PoseKey key with Pose2 p given a noise model - void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel); + /// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value + void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose); - /// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value - void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose); + /// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement) + void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry, + const SharedNoiseModel& model); - /// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement) - void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry, - const SharedNoiseModel& model); + /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation + void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing, + const SharedNoiseModel& model); - /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation - void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing, - const SharedNoiseModel& model); + /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location + void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range, + const SharedNoiseModel& model); - /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location - void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range, - const SharedNoiseModel& model); + /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location + void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey, + const Rot2& bearing, double range, const SharedNoiseModel& model); - /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location - void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey, - const Rot2& bearing, double range, const SharedNoiseModel& model); + /// Optimize + Values optimize(const Values& initialEstimate) { + typedef NonlinearOptimizer Optimizer; + return *Optimizer::optimizeLM(*this, initialEstimate, + NonlinearOptimizationParameters::LAMBDA); + } + }; - /// Optimize - Values optimize(const Values& initialEstimate) { - typedef NonlinearOptimizer Optimizer; - return *Optimizer::optimizeLM(*this, initialEstimate, - NonlinearOptimizationParameters::LAMBDA); - } - }; - - /// Optimizer - typedef NonlinearOptimizer Optimizer; + /// Optimizer + typedef NonlinearOptimizer Optimizer; } // planarSLAM diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index e078505a6..a425f8fb9 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -21,7 +21,7 @@ // Use pose2SLAM namespace for specific SLAM instance -template class gtsam::NonlinearOptimizer; + template class gtsam::NonlinearOptimizer; namespace pose2SLAM { @@ -30,7 +30,7 @@ namespace pose2SLAM { 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)); + x.insert(PoseKey(i), Pose2(cos(theta), sin(theta), M_PI_2 + theta)); return x; } diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 82a5a6efd..89a3b7ec6 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -18,10 +18,10 @@ #pragma once #include -#include #include #include #include +#include #include #include #include @@ -36,25 +36,6 @@ namespace pose2SLAM { /// Keys with Pose2 and symbol 'x' typedef TypedSymbol PoseKey; - /// Values class, inherited from Values, using PoseKeys - struct Values: public gtsam::Values { - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const gtsam::Values& values) : - gtsam::Values(values) { - } - - // Convenience for MATLAB wrapper, which does not allow for identically named methods - - /// get a pose - Pose2 pose(int key) const { return (*this)[PoseKey(key)]; } - - /// insert a pose - void insertPose(int key, const Pose2& pose) { insert(PoseKey(key), pose); } - }; /** * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) @@ -70,20 +51,20 @@ namespace pose2SLAM { */ /// A hard constraint to enforce a specific value for a pose - typedef NonlinearEquality HardConstraint; + typedef NonlinearEquality HardConstraint; /// A prior factor on a pose with Pose2 data type. - typedef PriorFactor Prior; + typedef PriorFactor Prior; /// A factor to add an odometry measurement between two poses. - typedef BetweenFactor Odometry; + typedef BetweenFactor Odometry; /// Graph - struct Graph: public NonlinearFactorGraph { + struct Graph: public NonlinearFactorGraph { /// Default constructor for a NonlinearFactorGraph Graph(){} /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph); + Graph(const NonlinearFactorGraph& graph); /// Adds a Pose2 prior with a noise model to one of the keys in the nonlinear factor graph void addPrior(const PoseKey& i, const Pose2& p, const SharedNoiseModel& model); @@ -97,18 +78,18 @@ namespace pose2SLAM { /// Optimize Values optimize(const Values& initialEstimate) { - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; return *Optimizer::optimizeLM(*this, initialEstimate, NonlinearOptimizationParameters::LAMBDA); } }; /// The sequential optimizer - typedef NonlinearOptimizer OptimizerSequential; /// The multifrontal optimizer - typedef NonlinearOptimizer Optimizer; } // pose2SLAM diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index 102652813..4d063c12b 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -39,7 +39,7 @@ namespace gtsam { 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); + x.insert(Key(i), gTi); } return x; } diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h index f4516e647..686f6bd0f 100644 --- a/gtsam/slam/pose3SLAM.h +++ b/gtsam/slam/pose3SLAM.h @@ -18,7 +18,6 @@ #pragma once #include -#include #include #include #include @@ -33,8 +32,6 @@ namespace gtsam { /// Creates a Key with data Pose3 and symbol 'x' typedef TypedSymbol Key; - /// Creates a Values structure with type 'Key' - typedef Values Values; /** * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) @@ -45,14 +42,14 @@ namespace gtsam { Values circle(size_t n, double R); /// A prior factor on Key with Pose3 data type. - typedef PriorFactor Prior; + typedef PriorFactor Prior; /// A factor to put constraints between two factors. - typedef BetweenFactor Constraint; + typedef BetweenFactor Constraint; /// A hard constraint would enforce that the given key would have the input value in the results. - typedef NonlinearEquality HardConstraint; + typedef NonlinearEquality HardConstraint; /// Graph - struct Graph: public NonlinearFactorGraph { + struct Graph: public NonlinearFactorGraph { /// Adds a factor between keys of the same type void addPrior(const Key& i, const Pose3& p, @@ -67,14 +64,13 @@ namespace gtsam { }; /// Optimizer - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; } // pose3SLAM /** * Backwards compatibility */ - typedef pose3SLAM::Values Pose3Values; ///< Typedef for Values class for backwards compatibility typedef pose3SLAM::Prior Pose3Prior; ///< Typedef for Prior class for backwards compatibility typedef pose3SLAM::Constraint Pose3Factor; ///< Typedef for Constraint class for backwards compatibility typedef pose3SLAM::Graph Pose3Graph; ///< Typedef for Graph class for backwards compatibility diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index ffcfce04d..4798bae84 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -19,7 +19,6 @@ #pragma once #include -#include #include #include @@ -32,56 +31,6 @@ namespace simulated2D { // Simulated2D robots have no orientation, just a position typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; - typedef Values PoseValues; - typedef Values PointValues; - - /** - * Custom Values class that holds poses and points - */ - class Values: public TupleValues2 { - public: - typedef TupleValues2 Base; ///< base class - typedef boost::shared_ptr sharedPoint; ///< shortcut to shared Point type - - /// Constructor - Values() { - } - - /// Copy constructor - Values(const Base& base) : - Base(base) { - } - - /// Insert a pose - void insertPose(const simulated2D::PoseKey& i, const Point2& p) { - insert(i, p); - } - - /// Insert a point - void insertPoint(const simulated2D::PointKey& j, const Point2& p) { - insert(j, p); - } - - /// Number of poses - int nrPoses() const { - return this->first_.size(); - } - - /// Number of points - int nrPoints() const { - return this->second_.size(); - } - - /// Return pose i - Point2 pose(const simulated2D::PoseKey& i) const { - return (*this)[i]; - } - - /// Return point j - Point2 point(const simulated2D::PointKey& j) const { - return (*this)[j]; - } - }; /// Prior on a single pose inline Point2 prior(const Point2& x) { @@ -112,18 +61,18 @@ namespace simulated2D { /** * Unary factor encoding a soft prior on a vector */ - template - class GenericPrior: public NonlinearFactor1 { + template + class GenericPrior: public NonlinearFactor1 { public: - typedef NonlinearFactor1 Base; ///< base class - typedef boost::shared_ptr > shared_ptr; + typedef NonlinearFactor1 Base; ///< base class + typedef boost::shared_ptr > shared_ptr; typedef typename KEY::Value Pose; ///< shortcut to Pose type Pose z_; ///< prior mean /// Create generic prior GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) : - NonlinearFactor1(model, key), z_(z) { + NonlinearFactor1(model, key), z_(z) { } /// Return error and optional derivative @@ -150,11 +99,11 @@ namespace simulated2D { /** * Binary factor simulating "odometry" between two Vectors */ - template - class GenericOdometry: public NonlinearFactor2 { + template + class GenericOdometry: public NonlinearFactor2 { public: - typedef NonlinearFactor2 Base; ///< base class - typedef boost::shared_ptr > shared_ptr; + typedef NonlinearFactor2 Base; ///< base class + typedef boost::shared_ptr > shared_ptr; typedef typename KEY::Value Pose; ///< shortcut to Pose type Pose z_; ///< odometry measurement @@ -162,7 +111,7 @@ namespace simulated2D { /// Create odometry GenericOdometry(const Pose& z, const SharedNoiseModel& model, const KEY& i1, const KEY& i2) : - NonlinearFactor2(model, i1, i2), z_(z) { + NonlinearFactor2(model, i1, i2), z_(z) { } /// Evaluate error and optionally return derivatives @@ -190,11 +139,11 @@ namespace simulated2D { /** * Binary factor simulating "measurement" between two Vectors */ - template - class GenericMeasurement: public NonlinearFactor2 { + template + class GenericMeasurement: public NonlinearFactor2 { public: - typedef NonlinearFactor2 Base; ///< base class - typedef boost::shared_ptr > shared_ptr; + typedef NonlinearFactor2 Base; ///< base class + typedef boost::shared_ptr > shared_ptr; typedef typename XKEY::Value Pose; ///< shortcut to Pose type typedef typename LKEY::Value Point; ///< shortcut to Point type @@ -203,7 +152,7 @@ namespace simulated2D { /// Create measurement factor GenericMeasurement(const Point& z, const SharedNoiseModel& model, const XKEY& i, const LKEY& j) : - NonlinearFactor2(model, i, j), z_(z) { + NonlinearFactor2(model, i, j), z_(z) { } /// Evaluate error and optionally return derivatives @@ -229,13 +178,13 @@ namespace simulated2D { }; /** Typedefs for regular use */ - typedef GenericPrior Prior; - typedef GenericOdometry Odometry; - typedef GenericMeasurement Measurement; + typedef GenericPrior Prior; + typedef GenericOdometry Odometry; + typedef GenericMeasurement Measurement; // Specialization of a graph for this example domain // TODO: add functions to add factor types - class Graph : public NonlinearFactorGraph { + class Graph : public NonlinearFactorGraph { public: Graph() {} }; diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h index 2521e86e4..ffd4e54fb 100644 --- a/gtsam/slam/simulated2DConstraints.h +++ b/gtsam/slam/simulated2DConstraints.h @@ -33,13 +33,13 @@ namespace simulated2D { namespace equality_constraints { /** Typedefs for regular use */ - typedef NonlinearEquality1 UnaryEqualityConstraint; - typedef NonlinearEquality1 UnaryEqualityPointConstraint; - typedef BetweenConstraint OdoEqualityConstraint; + typedef NonlinearEquality1 UnaryEqualityConstraint; + typedef NonlinearEquality1 UnaryEqualityPointConstraint; + typedef BetweenConstraint OdoEqualityConstraint; /** Equality between variables */ - typedef NonlinearEquality2 PoseEqualityConstraint; - typedef NonlinearEquality2 PointEqualityConstraint; + typedef NonlinearEquality2 PoseEqualityConstraint; + typedef NonlinearEquality2 PointEqualityConstraint; } // \namespace equality_constraints @@ -51,10 +51,10 @@ namespace simulated2D { * @tparam KEY is the key type for the variable constrained * @tparam IDX is an index in tangent space to constrain, must be less than KEY::VALUE::Dim() */ - template - struct ScalarCoordConstraint1: public BoundingConstraint1 { - typedef BoundingConstraint1 Base; ///< Base class convenience typedef - typedef boost::shared_ptr > shared_ptr; ///< boost::shared_ptr convenience typedef + template + struct ScalarCoordConstraint1: public BoundingConstraint1 { + typedef BoundingConstraint1 Base; ///< Base class convenience typedef + typedef boost::shared_ptr > shared_ptr; ///< boost::shared_ptr convenience typedef typedef typename KEY::Value Point; ///< Constrained variable type virtual ~ScalarCoordConstraint1() {} @@ -68,7 +68,7 @@ namespace simulated2D { */ ScalarCoordConstraint1(const KEY& key, double c, bool isGreaterThan, double mu = 1000.0) : - Base(key, c, isGreaterThan, mu) { + Base(key, c, isGreaterThan, mu) { } /** @@ -94,8 +94,8 @@ namespace simulated2D { }; /** typedefs for use with simulated2D systems */ - typedef ScalarCoordConstraint1 PoseXInequality; ///< Simulated2D domain example factor constraining X - typedef ScalarCoordConstraint1 PoseYInequality; ///< Simulated2D domain example factor constraining Y + typedef ScalarCoordConstraint1 PoseXInequality; ///< Simulated2D domain example factor constraining X + typedef ScalarCoordConstraint1 PoseYInequality; ///< Simulated2D domain example factor constraining Y /** * Trait for distance constraints to provide distance @@ -114,9 +114,9 @@ namespace simulated2D { * @tparam VALUES is the variable set for the graph * @tparam KEY is the type of the keys for the variables constrained */ - template - struct MaxDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 Base; ///< Base class for factor + template + struct MaxDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 Base; ///< Base class for factor typedef typename KEY::Value Point; ///< Type of variable constrained virtual ~MaxDistanceConstraint() {} @@ -129,7 +129,7 @@ namespace simulated2D { * @param mu is the gain for the penalty function */ MaxDistanceConstraint(const KEY& key1, const KEY& key2, double range_bound, double mu = 1000.0) - : Base(key1, key2, range_bound, false, mu) {} + : Base(key1, key2, range_bound, false, mu) {} /** * computes the range with derivatives @@ -148,7 +148,7 @@ namespace simulated2D { } }; - typedef MaxDistanceConstraint PoseMaxDistConstraint; ///< Simulated2D domain example factor + typedef MaxDistanceConstraint PoseMaxDistConstraint; ///< Simulated2D domain example factor /** * Binary inequality constraint forcing a minimum range @@ -157,9 +157,9 @@ namespace simulated2D { * @tparam XKEY is the type of the pose key constrained * @tparam PKEY is the type of the point key constrained */ - template - struct MinDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 Base; ///< Base class for factor + template + struct MinDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 Base; ///< Base class for factor typedef typename XKEY::Value Pose; ///< Type of pose variable constrained typedef typename PKEY::Value Point; ///< Type of point variable constrained @@ -174,7 +174,7 @@ namespace simulated2D { */ MinDistanceConstraint(const XKEY& key1, const PKEY& key2, double range_bound, double mu = 1000.0) - : Base(key1, key2, range_bound, true, mu) {} + : Base(key1, key2, range_bound, true, mu) {} /** * computes the range with derivatives @@ -193,7 +193,7 @@ namespace simulated2D { } }; - typedef MinDistanceConstraint LandmarkAvoid; ///< Simulated2D domain example factor + typedef MinDistanceConstraint LandmarkAvoid; ///< Simulated2D domain example factor } // \namespace inequality_constraints diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index 18c0c96c2..51a2903fe 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -19,7 +19,6 @@ #pragma once #include -#include #include #include @@ -31,29 +30,6 @@ namespace simulated2DOriented { // The types that take an oriented pose2 rather than point2 typedef TypedSymbol PointKey; typedef TypedSymbol PoseKey; - typedef Values PoseValues; - typedef Values PointValues; - - /// Specialized Values structure with syntactic sugar for - /// compatibility with matlab - class Values: public TupleValues2 { - public: - Values() {} - - void insertPose(const PoseKey& i, const Pose2& p) { - insert(i, p); - } - - void insertPoint(const PointKey& j, const Point2& p) { - insert(j, p); - } - - int nrPoses() const { return this->first_.size(); } - int nrPoints() const { return this->second_.size(); } - - Pose2 pose(const PoseKey& i) const { return (*this)[i]; } - Point2 point(const PointKey& j) const { return (*this)[j]; } - }; //TODO:: point prior is not implemented right now @@ -75,15 +51,15 @@ namespace simulated2DOriented { boost::none, boost::optional H2 = boost::none); /// Unary factor encoding a soft prior on a vector - template - struct GenericPosePrior: public NonlinearFactor1 { + template + struct GenericPosePrior: public NonlinearFactor1 { Pose2 z_; ///< measurement /// Create generic pose prior GenericPosePrior(const Pose2& z, const SharedNoiseModel& model, const Key& key) : - NonlinearFactor1(model, key), z_(z) { + NonlinearFactor1(model, key), z_(z) { } /// Evaluate error and optionally derivative @@ -97,8 +73,8 @@ namespace simulated2DOriented { /** * Binary factor simulating "odometry" between two Vectors */ - template - struct GenericOdometry: public NonlinearFactor2 { + template + struct GenericOdometry: public NonlinearFactor2 { Pose2 z_; ///< Between measurement for odometry factor /** @@ -106,7 +82,7 @@ namespace simulated2DOriented { */ GenericOdometry(const Pose2& z, const SharedNoiseModel& model, const KEY& i1, const KEY& i2) : - NonlinearFactor2(model, i1, i2), z_(z) { + NonlinearFactor2(model, i1, i2), z_(z) { } /// Evaluate error and optionally derivative @@ -118,10 +94,10 @@ namespace simulated2DOriented { }; - typedef GenericOdometry Odometry; + typedef GenericOdometry Odometry; /// Graph specialization for syntactic sugar use with matlab - class Graph : public NonlinearFactorGraph { + class Graph : public NonlinearFactorGraph { public: Graph() {} // TODO: add functions to add factors diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h index 32f05e967..4a24bcaa6 100644 --- a/gtsam/slam/simulated3D.h +++ b/gtsam/slam/simulated3D.h @@ -24,7 +24,6 @@ #include #include #include -#include // \namespace @@ -40,10 +39,6 @@ namespace simulated3D { typedef gtsam::TypedSymbol PoseKey; typedef gtsam::TypedSymbol PointKey; -typedef Values PoseValues; -typedef Values PointValues; -typedef TupleValues2 Values; - /** * Prior on a single pose */ @@ -66,7 +61,7 @@ Point3 mea(const Point3& x, const Point3& l, /** * A prior factor on a single linear robot pose */ -struct PointPrior3D: public NonlinearFactor1 { +struct PointPrior3D: public NonlinearFactor1 { Point3 z_; ///< The prior pose value for the variable attached to this factor @@ -78,7 +73,7 @@ struct PointPrior3D: public NonlinearFactor1 { */ PointPrior3D(const Point3& z, const SharedNoiseModel& model, const PoseKey& j) : - NonlinearFactor1 (model, j), z_(z) { + NonlinearFactor1 (model, j), z_(z) { } /** @@ -97,8 +92,7 @@ struct PointPrior3D: public NonlinearFactor1 { /** * Models a linear 3D measurement between 3D points */ -struct Simulated3DMeasurement: public NonlinearFactor2 { +struct Simulated3DMeasurement: public NonlinearFactor2 { Point3 z_; ///< Linear displacement between a pose and landmark @@ -111,7 +105,7 @@ PoseKey, PointKey> { */ Simulated3DMeasurement(const Point3& z, const SharedNoiseModel& model, PoseKey& j1, PointKey j2) : - NonlinearFactor2 ( + NonlinearFactor2 ( model, j1, j2), z_(z) { } diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index 40d3e98ff..948616ea6 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -39,7 +39,7 @@ using namespace std; namespace gtsam { namespace example { - typedef boost::shared_ptr > shared; + typedef boost::shared_ptr shared; static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); @@ -195,14 +195,13 @@ namespace example { 0.0, cos(v.y())); } - struct UnaryFactor: public gtsam::NonlinearFactor1 { + struct UnaryFactor: public gtsam::NonlinearFactor1 { Point2 z_; UnaryFactor(const Point2& z, const SharedNoiseModel& model, const simulated2D::PoseKey& key) : - gtsam::NonlinearFactor1(model, key), z_(z) { + gtsam::NonlinearFactor1(model, key), z_(z) { } Vector evaluateError(const Point2& x, boost::optional A = @@ -423,7 +422,7 @@ namespace example { boost::tuple, Ordering, VectorValues> planarGraph(size_t N) { // create empty graph - NonlinearFactorGraph nlfg; + NonlinearFactorGraph nlfg; // Create almost hard constraint on x11, sigma=0 will work for PCG not for normal shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sharedSigma(2,1e-3), key(1,1))); diff --git a/gtsam/slam/smallExample.h b/gtsam/slam/smallExample.h index 1889ee1b5..96dcbaf09 100644 --- a/gtsam/slam/smallExample.h +++ b/gtsam/slam/smallExample.h @@ -28,8 +28,7 @@ namespace gtsam { namespace example { - typedef simulated2D::Values Values; - typedef NonlinearFactorGraph Graph; + typedef NonlinearFactorGraph Graph; /** * Create small example for non-linear factor graph diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index a7f91c320..99be5d09a 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -39,9 +39,9 @@ TEST( AntiFactor, NegativeHessian) SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); // Create a configuration corresponding to the ground truth - boost::shared_ptr values(new pose3SLAM::Values()); - values->insert(1, pose1); - values->insert(2, pose2); + boost::shared_ptr values(new Values()); + values->insert(pose3SLAM::Key(1), pose1); + values->insert(pose3SLAM::Key(2), pose2); // Define an elimination ordering Ordering::shared_ptr ordering(new Ordering()); @@ -50,7 +50,7 @@ TEST( AntiFactor, NegativeHessian) // Create a "standard" factor - BetweenFactor::shared_ptr originalFactor(new BetweenFactor(1, 2, z, sigma)); + BetweenFactor::shared_ptr originalFactor(new BetweenFactor(1, 2, z, sigma)); // Linearize it into a Jacobian Factor GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering); @@ -59,7 +59,7 @@ TEST( AntiFactor, NegativeHessian) HessianFactor::shared_ptr originalHessian = HessianFactor::shared_ptr(new HessianFactor(*originalJacobian)); // Create the AntiFactor version of the original nonlinear factor - AntiFactor::shared_ptr antiFactor(new AntiFactor(originalFactor)); + AntiFactor::shared_ptr antiFactor(new AntiFactor(originalFactor)); // Linearize the AntiFactor into a Hessian Factor GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(*values, *ordering); @@ -100,9 +100,9 @@ TEST( AntiFactor, EquivalentBayesNet) graph->addConstraint(1, 2, pose1.between(pose2), sigma); // Create a configuration corresponding to the ground truth - boost::shared_ptr values(new pose3SLAM::Values()); - values->insert(1, pose1); - values->insert(2, pose2); + boost::shared_ptr values(new Values()); + values->insert(pose3SLAM::Key(1), pose1); + values->insert(pose3SLAM::Key(2), pose2); // Define an elimination ordering Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); @@ -119,7 +119,7 @@ TEST( AntiFactor, EquivalentBayesNet) graph->push_back(f1); // Add the corresponding AntiFactor between Pose1 and Pose2 - AntiFactor::shared_ptr f2(new AntiFactor(f1)); + AntiFactor::shared_ptr f2(new AntiFactor(f1)); graph->push_back(f2); // Again, Eliminate into a BayesNet diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 0d5c7da9f..e887cac06 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -21,7 +21,6 @@ using namespace boost; #include #include #include -#include #include #include @@ -31,14 +30,11 @@ using namespace gtsam; typedef PinholeCamera GeneralCamera; typedef TypedSymbol CameraKey; typedef TypedSymbol PointKey; -typedef Values CameraConfig; -typedef Values PointConfig; -typedef TupleValues2 VisualValues; -typedef GeneralSFMFactor Projection; -typedef NonlinearEquality CameraConstraint; -typedef NonlinearEquality Point3Constraint; +typedef GeneralSFMFactor Projection; +typedef NonlinearEquality CameraConstraint; +typedef NonlinearEquality Point3Constraint; -class Graph: public NonlinearFactorGraph { +class Graph: public NonlinearFactorGraph { public: void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, i, j)); @@ -71,7 +67,7 @@ double getGaussian() return sqrt(-2.0f * (double)log(S) / S) * V1; } -typedef NonlinearOptimizer Optimizer; +typedef NonlinearOptimizer Optimizer; const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); @@ -99,12 +95,12 @@ TEST( GeneralSFMFactor, error ) { boost::shared_ptr factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); // For the following configuration, the factor predicts 320,240 - VisualValues values; + Values values; Rot3 R; Point3 t1(0,0,-6); Pose3 x1(R,t1); - values.insert(1, GeneralCamera(x1)); - Point3 l1; values.insert(1, l1); + values.insert(CameraKey(1), GeneralCamera(x1)); + Point3 l1; values.insert(PointKey(1), l1); EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); } @@ -172,15 +168,15 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(CameraKey((int)i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(i, pt) ; + values->insert(PointKey(i), pt) ; } graph->addCameraConstraint(0, X[0]); @@ -211,9 +207,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(CameraKey((int)i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { @@ -221,10 +217,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(i, pt) ; + values->insert(PointKey(i), pt) ; } else { - values->insert(i, L[i]) ; + values->insert(PointKey(i), L[i]) ; } } @@ -257,16 +253,16 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(CameraKey((int)i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); //Point3 pt(L[i].x(), L[i].y(), L[i].z()); - values->insert(i, pt) ; + values->insert(PointKey(i), pt) ; } for ( size_t i = 0 ; i < X.size() ; ++i ) @@ -300,7 +296,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) { const double rot_noise = 1e-5, @@ -308,7 +304,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { focal_noise = 1, skew_noise = 1e-5; if ( i == 0 ) { - values->insert((int)i, X[i]) ; + values->insert(CameraKey((int)i), X[i]) ; } else { @@ -319,12 +315,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { skew_noise, // s trans_noise, trans_noise // ux, uy ) ; - values->insert((int)i, X[i].retract(delta)) ; + values->insert(CameraKey((int)i), X[i].retract(delta)) ; } } for ( size_t i = 0 ; i < L.size() ; ++i ) { - values->insert(i, L[i]) ; + values->insert(PointKey(i), L[i]) ; } // fix X0 and all landmarks, allow only the X[1] to move @@ -362,16 +358,16 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(CameraKey((int)i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(i, pt) ; + values->insert(PointKey(i), pt) ; } graph->addCameraConstraint(0, X[0]); diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index b402b1d0b..5a626d095 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -21,7 +21,6 @@ using namespace boost; #include #include #include -#include #include #include @@ -31,15 +30,12 @@ using namespace gtsam; typedef PinholeCamera GeneralCamera; typedef TypedSymbol CameraKey; typedef TypedSymbol PointKey; -typedef Values CameraConfig; -typedef Values PointConfig; -typedef TupleValues2 VisualValues; -typedef GeneralSFMFactor Projection; -typedef NonlinearEquality CameraConstraint; -typedef NonlinearEquality Point3Constraint; +typedef GeneralSFMFactor Projection; +typedef NonlinearEquality CameraConstraint; +typedef NonlinearEquality Point3Constraint; /* ************************************************************************* */ -class Graph: public NonlinearFactorGraph { +class Graph: public NonlinearFactorGraph { public: void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, i, j)); @@ -72,7 +68,7 @@ double getGaussian() return sqrt(-2.0f * (double)log(S) / S) * V1; } -typedef NonlinearOptimizer Optimizer; +typedef NonlinearOptimizer Optimizer; const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); @@ -100,12 +96,12 @@ TEST( GeneralSFMFactor, error ) { boost::shared_ptr factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); // For the following configuration, the factor predicts 320,240 - VisualValues values; + Values values; Rot3 R; Point3 t1(0,0,-6); Pose3 x1(R,t1); - values.insert(1, GeneralCamera(x1)); - Point3 l1; values.insert(1, l1); + values.insert(CameraKey(1), GeneralCamera(x1)); + Point3 l1; values.insert(PointKey(1), l1); EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); } @@ -173,15 +169,15 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(CameraKey((int)i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(i, pt) ; + values->insert(PointKey(i), pt) ; } graph->addCameraConstraint(0, X[0]); @@ -212,9 +208,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(CameraKey((int)i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { @@ -222,10 +218,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(i, pt) ; + values->insert(PointKey(i), pt) ; } else { - values->insert(i, L[i]) ; + values->insert(PointKey(i), L[i]) ; } } @@ -258,16 +254,16 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(CameraKey((int)i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); //Point3 pt(L[i].x(), L[i].y(), L[i].z()); - values->insert(i, pt) ; + values->insert(PointKey(i), pt) ; } for ( size_t i = 0 ; i < X.size() ; ++i ) @@ -301,13 +297,13 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) { const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1, distort_noise = 1e-3; if ( i == 0 ) { - values->insert((int)i, X[i]) ; + values->insert(CameraKey((int)i), X[i]) ; } else { @@ -316,12 +312,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { trans_noise, trans_noise, trans_noise, // translation focal_noise, distort_noise, distort_noise // f, k1, k2 ) ; - values->insert((int)i, X[i].retract(delta)) ; + values->insert(CameraKey((int)i), X[i].retract(delta)) ; } } for ( size_t i = 0 ; i < L.size() ; ++i ) { - values->insert(i, L[i]) ; + values->insert(PointKey(i), L[i]) ; } // fix X0 and all landmarks, allow only the X[1] to move @@ -358,16 +354,16 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(CameraKey((int)i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(i, pt) ; + values->insert(PointKey(i), pt) ; } graph->addCameraConstraint(0, X[0]); diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp index 2362e0ae5..a74f79d49 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -23,6 +23,7 @@ using namespace std; using namespace gtsam; +using namespace planarSLAM; // some shared test values static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); @@ -50,9 +51,9 @@ TEST( planarSLAM, BearingFactor ) planarSLAM::Bearing factor(2, 3, z, sigma); // create config - planarSLAM::Values c; - c.insert(2, x2); - c.insert(3, l3); + Values c; + c.insert(PoseKey(2), x2); + c.insert(PointKey(3), l3); // Check error Vector actual = factor.unwhitenedError(c); @@ -78,9 +79,9 @@ TEST( planarSLAM, RangeFactor ) planarSLAM::Range factor(2, 3, z, sigma); // create config - planarSLAM::Values c; - c.insert(2, x2); - c.insert(3, l3); + Values c; + c.insert(PoseKey(2), x2); + c.insert(PointKey(3), l3); // Check error Vector actual = factor.unwhitenedError(c); @@ -105,9 +106,9 @@ TEST( planarSLAM, BearingRangeFactor ) planarSLAM::BearingRange factor(2, 3, r, b, sigma2); // create config - planarSLAM::Values c; - c.insert(2, x2); - c.insert(3, l3); + Values c; + c.insert(PoseKey(2), x2); + c.insert(PointKey(3), l3); // Check error Vector actual = factor.unwhitenedError(c); @@ -138,10 +139,10 @@ TEST( planarSLAM, PoseConstraint_equals ) TEST( planarSLAM, constructor ) { // create config - planarSLAM::Values c; - c.insert(2, x2); - c.insert(3, x3); - c.insert(3, l3); + Values c; + c.insert(PoseKey(2), x2); + c.insert(PoseKey(3), x3); + c.insert(PointKey(3), l3); // create graph planarSLAM::Graph G; @@ -165,8 +166,8 @@ TEST( planarSLAM, constructor ) Vector expected2 = Vector_(1, -0.1); Vector expected3 = Vector_(1, 0.22); // Get NoiseModelFactors - FactorGraph > GNM = - *G.dynamicCastFactors > >(); + FactorGraph GNM = + *G.dynamicCastFactors >(); EXPECT(assert_equal(expected0, GNM[0]->unwhitenedError(c))); EXPECT(assert_equal(expected1, GNM[1]->unwhitenedError(c))); EXPECT(assert_equal(expected2, GNM[2]->unwhitenedError(c))); diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index 3cbc7e082..9151dd6af 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -118,8 +118,8 @@ TEST( Pose2SLAM, linearization ) 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); + config.insert(pose2SLAM::PoseKey(1),p1); + config.insert(pose2SLAM::PoseKey(2),p2); // Linearize Ordering ordering(*config.orderingArbitrary()); boost::shared_ptr > lfg_linearized = graph.linearize(config, ordering); @@ -153,23 +153,23 @@ TEST(Pose2Graph, optimize) { fg->addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance); // Create initial config - boost::shared_ptr initial(new pose2SLAM::Values()); - initial->insert(0, Pose2(0,0,0)); - initial->insert(1, Pose2(0,0,0)); + boost::shared_ptr initial(new Values()); + initial->insert(pose2SLAM::PoseKey(0), Pose2(0,0,0)); + initial->insert(pose2SLAM::PoseKey(1), Pose2(0,0,0)); // Choose an ordering and optimize shared_ptr ordering(new Ordering); *ordering += "x0","x1"; - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); Optimizer optimizer0(fg, initial, ordering, params); Optimizer optimizer = optimizer0.levenbergMarquardt(); // Check with expected config - pose2SLAM::Values expected; - expected.insert(0, Pose2(0,0,0)); - expected.insert(1, Pose2(1,2,M_PI_2)); + Values expected; + expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0)); + expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI_2)); CHECK(assert_equal(expected, *optimizer.values())); } @@ -178,8 +178,8 @@ TEST(Pose2Graph, optimize) { TEST(Pose2Graph, optimizeThreePoses) { // Create a hexagon of poses - pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0); - Pose2 p0 = hexagon[0], p1 = hexagon[1]; + Values hexagon = pose2SLAM::circle(3,1.0); + Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)]; // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new pose2SLAM::Graph); @@ -190,10 +190,10 @@ TEST(Pose2Graph, optimizeThreePoses) { fg->addOdometry(2, 0, delta, covariance); // Create initial config - boost::shared_ptr initial(new pose2SLAM::Values()); - initial->insert(0, p0); - initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1))); + boost::shared_ptr initial(new Values()); + initial->insert(pose2SLAM::PoseKey(0), p0); + initial->insert(pose2SLAM::PoseKey(1), hexagon[pose2SLAM::PoseKey(1)].retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insert(pose2SLAM::PoseKey(2), hexagon[pose2SLAM::PoseKey(2)].retract(Vector_(3, 0.1,-0.1, 0.1))); // Choose an ordering shared_ptr ordering(new Ordering); @@ -204,7 +204,7 @@ TEST(Pose2Graph, optimizeThreePoses) { pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params); pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); - pose2SLAM::Values actual = *optimizer.values(); + Values actual = *optimizer.values(); // Check with ground truth CHECK(assert_equal(hexagon, actual)); @@ -215,8 +215,8 @@ TEST(Pose2Graph, optimizeThreePoses) { TEST_UNSAFE(Pose2SLAM, optimizeCircle) { // Create a hexagon of poses - pose2SLAM::Values hexagon = pose2SLAM::circle(6,1.0); - Pose2 p0 = hexagon[0], p1 = hexagon[1]; + Values hexagon = pose2SLAM::circle(6,1.0); + Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)]; // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new pose2SLAM::Graph); @@ -230,13 +230,13 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { fg->addOdometry(5, 0, delta, covariance); // Create initial config - boost::shared_ptr initial(new pose2SLAM::Values()); - initial->insert(0, p0); - initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1))); - initial->insert(3, hexagon[3].retract(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(4, hexagon[4].retract(Vector_(3, 0.1,-0.1, 0.1))); - initial->insert(5, hexagon[5].retract(Vector_(3,-0.1, 0.1,-0.1))); + boost::shared_ptr initial(new Values()); + initial->insert(pose2SLAM::PoseKey(0), p0); + initial->insert(pose2SLAM::PoseKey(1), hexagon[pose2SLAM::PoseKey(1)].retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insert(pose2SLAM::PoseKey(2), hexagon[pose2SLAM::PoseKey(2)].retract(Vector_(3, 0.1,-0.1, 0.1))); + initial->insert(pose2SLAM::PoseKey(3), hexagon[pose2SLAM::PoseKey(3)].retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insert(pose2SLAM::PoseKey(4), hexagon[pose2SLAM::PoseKey(4)].retract(Vector_(3, 0.1,-0.1, 0.1))); + initial->insert(pose2SLAM::PoseKey(5), hexagon[pose2SLAM::PoseKey(5)].retract(Vector_(3,-0.1, 0.1,-0.1))); // Choose an ordering shared_ptr ordering(new Ordering); @@ -247,13 +247,13 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params); pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); - pose2SLAM::Values actual = *optimizer.values(); + Values actual = *optimizer.values(); // Check with ground truth CHECK(assert_equal(hexagon, actual)); // Check loop closure - CHECK(assert_equal(delta,actual[5].between(actual[0]))); + CHECK(assert_equal(delta,actual[pose2SLAM::PoseKey(5)].between(actual[pose2SLAM::PoseKey(0)]))); // Pose2SLAMOptimizer myOptimizer("3"); @@ -281,7 +281,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { // // myOptimizer.update(x); // -// pose2SLAM::Values expected; +// Values expected; // expected.insert(0, Pose2(0.,0.,0.)); // expected.insert(1, Pose2(1.,0.,0.)); // expected.insert(2, Pose2(2.,0.,0.)); @@ -315,8 +315,8 @@ TEST(Pose2Graph, optimize2) { // G.addConstraint(1, 3, Pose2(0.,0.,0.), I3); // G.addConstraint(2, 3, Pose2(0.,0.,0.), I3); // -// PredecessorMap tree = -// G.findMinimumSpanningTree(); +// PredecessorMap tree = +// G.findMinimumSpanningTree(); // CHECK(tree[1] == 1); // CHECK(tree[2] == 1); // CHECK(tree[3] == 1); @@ -329,12 +329,12 @@ TEST(Pose2Graph, optimize2) { // G.addConstraint(1, 3, Pose2(0.,0.,0.), I3); // G.addConstraint(2, 3, Pose2(0.,0.,0.), I3); // -// PredecessorMap tree; +// PredecessorMap tree; // tree.insert(1,2); // tree.insert(2,2); // tree.insert(3,2); // -// G.split(tree, T, C); +// G.split(tree, T, C); // LONGS_EQUAL(2, T.size()); // LONGS_EQUAL(1, C.size()); //} @@ -345,13 +345,13 @@ using namespace pose2SLAM; TEST(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 )); + Values expected; + expected.insert(pose2SLAM::PoseKey(0), Pose2( 1, 0, M_PI_2)); + expected.insert(pose2SLAM::PoseKey(1), Pose2( 0, 1, - M_PI )); + expected.insert(pose2SLAM::PoseKey(2), Pose2(-1, 0, - M_PI_2)); + expected.insert(pose2SLAM::PoseKey(3), Pose2( 0, -1, 0 )); - pose2SLAM::Values actual = pose2SLAM::circle(4,1.0); + Values actual = pose2SLAM::circle(4,1.0); CHECK(assert_equal(expected,actual)); } @@ -359,21 +359,21 @@ TEST(Pose2Values, pose2Circle ) TEST(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 )); + Values expected; + expected.insert(pose2SLAM::PoseKey(0), Pose2( 1.1, 0, M_PI_2)); + expected.insert(pose2SLAM::PoseKey(1), Pose2( 0.1, 1, - M_PI )); + expected.insert(pose2SLAM::PoseKey(2), Pose2(-0.9, 0, - M_PI_2)); + expected.insert(pose2SLAM::PoseKey(3), Pose2( 0.1, -1, 0 )); // Note expmap coordinates are in local coordinates, so shifting to right requires thought !!! - pose2SLAM::Values circle(pose2SLAM::circle(4,1.0)); + Values circle(pose2SLAM::circle(4,1.0)); Ordering ordering(*circle.orderingArbitrary()); VectorValues delta(circle.dims(ordering)); - delta[ordering[PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0); - delta[ordering[PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0); - delta[ordering[PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0); - delta[ordering[PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0); - pose2SLAM::Values actual = circle.retract(delta, ordering); + delta[ordering[pose2SLAM::PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0); + delta[ordering[pose2SLAM::PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0); + delta[ordering[pose2SLAM::PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0); + delta[ordering[pose2SLAM::PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0); + Values actual = circle.retract(delta, ordering); CHECK(assert_equal(expected,actual)); } @@ -386,8 +386,8 @@ TEST( Pose2Prior, error ) { // Choose a linearization point Pose2 p1(1, 0, 0); // robot at (1,0) - pose2SLAM::Values x0; - x0.insert(1, p1); + Values x0; + x0.insert(pose2SLAM::PoseKey(1), p1); // Create factor pose2SLAM::Prior factor(1, p1, sigmas); @@ -408,7 +408,7 @@ TEST( Pose2Prior, error ) VectorValues addition(VectorValues::Zero(x0.dims(ordering))); addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0); VectorValues plus = delta + addition; - pose2SLAM::Values x1 = x0.retract(plus, ordering); + 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))); @@ -430,8 +430,8 @@ LieVector hprior(const Pose2& p1) { TEST( Pose2Prior, linearize ) { // Choose a linearization point at ground truth - pose2SLAM::Values x0; - x0.insert(1,priorVal); + Values x0; + x0.insert(pose2SLAM::PoseKey(1),priorVal); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -450,9 +450,9 @@ TEST( Pose2Factor, error ) // Choose a linearization point Pose2 p1; // robot at origin Pose2 p2(1, 0, 0); // robot at (1,0) - pose2SLAM::Values x0; - x0.insert(1, p1); - x0.insert(2, p2); + Values x0; + x0.insert(pose2SLAM::PoseKey(1), p1); + x0.insert(pose2SLAM::PoseKey(2), p2); // Create factor Pose2 z = p1.between(p2); @@ -474,7 +474,7 @@ TEST( Pose2Factor, error ) // Check error after increasing p2 VectorValues plus = delta; plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0); - pose2SLAM::Values x1 = x0.retract(plus, ordering); + 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))); @@ -491,9 +491,9 @@ TEST( 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); + Values x0; + x0.insert(pose2SLAM::PoseKey(1),p1); + x0.insert(pose2SLAM::PoseKey(2),p2); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -521,9 +521,9 @@ TEST( 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); + Values x0; + x0.insert(pose2SLAM::PoseKey(1),p1); + x0.insert(pose2SLAM::PoseKey(2),p2); // expected linearization Matrix expectedH1 = covariance->Whiten(Matrix_(3,3, diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index b2bd86d4d..03ca40dfa 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -36,6 +36,7 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +using namespace pose3SLAM; // common measurement covariance static Matrix covariance = eye(6); @@ -48,8 +49,8 @@ TEST(Pose3Graph, optimizeCircle) { // Create a hexagon of poses double radius = 10; - Pose3Values hexagon = pose3SLAM::circle(6,radius); - Pose3 gT0 = hexagon[0], gT1 = hexagon[1]; + Values hexagon = pose3SLAM::circle(6,radius); + Pose3 gT0 = hexagon[Key(0)], gT1 = hexagon[Key(1)]; // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new Pose3Graph); @@ -65,13 +66,13 @@ TEST(Pose3Graph, optimizeCircle) { fg->addConstraint(5,0, _0T1, covariance); // Create initial config - boost::shared_ptr initial(new Pose3Values()); - initial->insert(0, gT0); - initial->insert(1, hexagon[1].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial->insert(2, hexagon[2].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial->insert(3, hexagon[3].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial->insert(4, hexagon[4].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial->insert(5, hexagon[5].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + boost::shared_ptr initial(new Values()); + initial->insert(Key(0), gT0); + initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial->insert(Key(3), hexagon[Key(3)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(Key(4), hexagon[Key(4)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial->insert(Key(5), hexagon[Key(5)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); // Choose an ordering and optimize shared_ptr ordering(new Ordering); @@ -80,18 +81,18 @@ TEST(Pose3Graph, optimizeCircle) { pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params); pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); - Pose3Values actual = *optimizer.values(); + Values actual = *optimizer.values(); // Check with ground truth CHECK(assert_equal(hexagon, actual,1e-4)); // Check loop closure - CHECK(assert_equal(_0T1,actual[5].between(actual[0]),1e-5)); + CHECK(assert_equal(_0T1,actual[Key(5)].between(actual[Key(0)]),1e-5)); } /* ************************************************************************* */ TEST(Pose3Graph, partial_prior_height) { - typedef PartialPriorFactor Partial; + typedef PartialPriorFactor Partial; // reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3) @@ -109,13 +110,13 @@ TEST(Pose3Graph, partial_prior_height) { pose3SLAM::Graph graph; graph.add(height); - pose3SLAM::Values values; + Values values; values.insert(key, init); // linearization EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); - pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); + Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); EXPECT(assert_equal(expected, actual[key], tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } @@ -133,9 +134,9 @@ TEST( Pose3Factor, error ) Pose3Factor factor(1,2, z, I6); // Create config - Pose3Values x; - x.insert(1,t1); - x.insert(2,t2); + Values x; + x.insert(Key(1),t1); + x.insert(Key(2),t2); // Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2)) Vector actual = factor.unwhitenedError(x); @@ -145,7 +146,7 @@ TEST( Pose3Factor, error ) /* ************************************************************************* */ TEST(Pose3Graph, partial_prior_xy) { - typedef PartialPriorFactor Partial; + typedef PartialPriorFactor Partial; // XY prior - full mask interface pose3SLAM::Key key(1); @@ -164,10 +165,10 @@ TEST(Pose3Graph, partial_prior_xy) { pose3SLAM::Graph graph; graph.add(priorXY); - pose3SLAM::Values values; + Values values; values.insert(key, init); - pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); + Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); EXPECT(assert_equal(expected, actual[key], tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } @@ -180,27 +181,27 @@ 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( Pose3Values, pose3Circle ) +TEST( Values, pose3Circle ) { // expected is 4 poses tangent to circle with radius 1m - Pose3Values 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 expected; + expected.insert(Key(0), Pose3(R1, Point3( 1, 0, 0))); + expected.insert(Key(1), Pose3(R2, Point3( 0, 1, 0))); + expected.insert(Key(2), Pose3(R3, Point3(-1, 0, 0))); + expected.insert(Key(3), Pose3(R4, Point3( 0,-1, 0))); - Pose3Values actual = pose3SLAM::circle(4,1.0); + Values actual = pose3SLAM::circle(4,1.0); CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( Pose3Values, expmap ) +TEST( Values, expmap ) { - Pose3Values 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))); + Values expected; + expected.insert(Key(0), Pose3(R1, Point3( 1.0, 0.1, 0))); + expected.insert(Key(1), Pose3(R2, Point3(-0.1, 1.0, 0))); + expected.insert(Key(2), Pose3(R3, Point3(-1.0,-0.1, 0))); + expected.insert(Key(3), Pose3(R4, Point3( 0.1,-1.0, 0))); Ordering ordering(*expected.orderingArbitrary()); VectorValues delta(expected.dims(ordering)); @@ -209,7 +210,7 @@ TEST( Pose3Values, expmap ) 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); - Pose3Values actual = pose3SLAM::circle(4,1.0).retract(delta, ordering); + Values actual = pose3SLAM::circle(4,1.0).retract(delta, ordering); CHECK(assert_equal(expected,actual)); } diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 09fd612d7..1d8616c30 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -25,6 +25,7 @@ using namespace std; using namespace gtsam; +using namespace visualSLAM; // make cube static Point3 @@ -51,9 +52,9 @@ TEST( ProjectionFactor, error ) factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); // For the following values structure, the factor predicts 320,240 - visualSLAM::Values config; - Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1); - Point3 l1; config.insert(1, l1); + Values config; + Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(PoseKey(1), x1); + Point3 l1; config.insert(PointKey(1), l1); // Point should project to Point2(320.,240.) CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(config))); @@ -80,13 +81,13 @@ TEST( ProjectionFactor, error ) CHECK(assert_equal(expected_lfg,*actual_lfg)); // expmap on a config - visualSLAM::Values expected_config; - Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2); - Point3 l2(1,2,3); expected_config.insert(1, l2); + Values expected_config; + Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(PoseKey(1), x2); + Point3 l2(1,2,3); expected_config.insert(PointKey(1), l2); VectorValues delta(expected_config.dims(ordering)); delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.); delta[ordering["l1"]] = Vector_(3, 1.,2.,3.); - visualSLAM::Values actual_config = config.retract(delta, ordering); + Values actual_config = config.retract(delta, ordering); CHECK(assert_equal(expected_config,actual_config,1e-9)); } diff --git a/gtsam/slam/tests/testSimulated2D.cpp b/gtsam/slam/tests/testSimulated2D.cpp index 2d28f11c3..30c5561ce 100644 --- a/gtsam/slam/tests/testSimulated2D.cpp +++ b/gtsam/slam/tests/testSimulated2D.cpp @@ -30,9 +30,9 @@ using namespace simulated2D; /* ************************************************************************* */ TEST( simulated2D, Simulated2DValues ) { - simulated2D::Values actual; - actual.insertPose(1,Point2(1,1)); - actual.insertPoint(2,Point2(2,2)); + Values actual; + actual.insert(simulated2D::PoseKey(1),Point2(1,1)); + actual.insert(simulated2D::PointKey(2),Point2(2,2)); EXPECT(assert_equal(actual,actual,1e-9)); } diff --git a/gtsam/slam/tests/testSimulated2DOriented.cpp b/gtsam/slam/tests/testSimulated2DOriented.cpp index 64844cb2a..f5ebfa177 100644 --- a/gtsam/slam/tests/testSimulated2DOriented.cpp +++ b/gtsam/slam/tests/testSimulated2DOriented.cpp @@ -57,7 +57,7 @@ TEST( simulated2DOriented, constructor ) SharedDiagonal model(Vector_(3, 1., 1., 1.)); simulated2DOriented::Odometry factor(measurement, model, simulated2DOriented::PoseKey(1), simulated2DOriented::PoseKey(2)); - simulated2DOriented::Values config; + Values config; config.insert(simulated2DOriented::PoseKey(1), Pose2(1., 0., 0.2)); config.insert(simulated2DOriented::PoseKey(2), Pose2(2., 0., 0.1)); boost::shared_ptr lf = factor.linearize(config, *config.orderingArbitrary()); diff --git a/gtsam/slam/tests/testSimulated3D.cpp b/gtsam/slam/tests/testSimulated3D.cpp index b3086acf1..a48e797e5 100644 --- a/gtsam/slam/tests/testSimulated3D.cpp +++ b/gtsam/slam/tests/testSimulated3D.cpp @@ -29,7 +29,7 @@ using namespace simulated3D; /* ************************************************************************* */ TEST( simulated3D, Values ) { - simulated3D::Values actual; + Values actual; actual.insert(simulated3D::PointKey(1),Point3(1,1,1)); actual.insert(simulated3D::PoseKey(2),Point3(2,2,2)); EXPECT(assert_equal(actual,actual,1e-9)); diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 2955e1ef9..94a4d4060 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -29,6 +29,7 @@ using namespace std; using namespace gtsam; using namespace boost; +using namespace visualSLAM; Pose3 camera1(Matrix_(3,3, 1., 0., 0., @@ -58,15 +59,15 @@ TEST( StereoFactor, singlePoint) graph->add(visualSLAM::StereoFactor(z14,sigma, 1, 1, K)); // Create a configuration corresponding to the ground truth - boost::shared_ptr values(new visualSLAM::Values()); - values->insert(1, camera1); // add camera at z=6.25m looking towards origin + boost::shared_ptr values(new Values()); + values->insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin Point3 l1(0, 0, 0); - values->insert(1, l1); // add point at origin; + values->insert(PointKey(1), l1); // add point at origin; Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); - typedef gtsam::NonlinearOptimizer Optimizer; // optimization engine for this domain + typedef gtsam::NonlinearOptimizer Optimizer; // optimization engine for this domain double absoluteThreshold = 1e-9; double relativeThreshold = 1e-5; diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp index cb208bfeb..ccae629eb 100644 --- a/gtsam/slam/tests/testVSLAM.cpp +++ b/gtsam/slam/tests/testVSLAM.cpp @@ -32,6 +32,7 @@ using namespace boost; using namespace std; using namespace gtsam; using namespace boost; +using namespace visualSLAM; static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); @@ -90,13 +91,13 @@ TEST( Graph, optimizeLM) graph->addPointConstraint(3, landmark3); // Create an initial values structure corresponding to the ground truth - boost::shared_ptr initialEstimate(new visualSLAM::Values); - initialEstimate->insert(1, camera1); - initialEstimate->insert(2, camera2); - initialEstimate->insert(1, landmark1); - initialEstimate->insert(2, landmark2); - initialEstimate->insert(3, landmark3); - initialEstimate->insert(4, landmark4); + boost::shared_ptr initialEstimate(new Values); + initialEstimate->insert(PoseKey(1), camera1); + initialEstimate->insert(PoseKey(2), camera2); + initialEstimate->insert(PointKey(1), landmark1); + initialEstimate->insert(PointKey(2), landmark2); + initialEstimate->insert(PointKey(3), landmark3); + initialEstimate->insert(PointKey(4), landmark4); // Create an ordering of the variables shared_ptr ordering(new Ordering); @@ -127,13 +128,13 @@ TEST( Graph, optimizeLM2) graph->addPoseConstraint(2, camera2); // Create an initial values structure corresponding to the ground truth - boost::shared_ptr initialEstimate(new visualSLAM::Values); - initialEstimate->insert(1, camera1); - initialEstimate->insert(2, camera2); - initialEstimate->insert(1, landmark1); - initialEstimate->insert(2, landmark2); - initialEstimate->insert(3, landmark3); - initialEstimate->insert(4, landmark4); + boost::shared_ptr initialEstimate(new Values); + initialEstimate->insert(PoseKey(1), camera1); + initialEstimate->insert(PoseKey(2), camera2); + initialEstimate->insert(PointKey(1), landmark1); + initialEstimate->insert(PointKey(2), landmark2); + initialEstimate->insert(PointKey(3), landmark3); + initialEstimate->insert(PointKey(4), landmark4); // Create an ordering of the variables shared_ptr ordering(new Ordering); @@ -164,13 +165,13 @@ TEST( Graph, CHECK_ORDERING) graph->addPoseConstraint(2, camera2); // Create an initial values structure corresponding to the ground truth - boost::shared_ptr initialEstimate(new visualSLAM::Values); - initialEstimate->insert(1, camera1); - initialEstimate->insert(2, camera2); - initialEstimate->insert(1, landmark1); - initialEstimate->insert(2, landmark2); - initialEstimate->insert(3, landmark3); - initialEstimate->insert(4, landmark4); + boost::shared_ptr initialEstimate(new Values); + initialEstimate->insert(PoseKey(1), camera1); + initialEstimate->insert(PoseKey(2), camera2); + initialEstimate->insert(PointKey(1), landmark1); + initialEstimate->insert(PointKey(2), landmark2); + initialEstimate->insert(PointKey(3), landmark3); + initialEstimate->insert(PointKey(4), landmark4); Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initialEstimate); @@ -192,23 +193,23 @@ TEST( Graph, CHECK_ORDERING) TEST( Values, 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 - visualSLAM::Values init; - init.insert(1, Pose3()); - init.insert(1, Point3(1.0, 2.0, 3.0)); + Values init; + init.insert(PoseKey(1), Pose3()); + init.insert(PointKey(1), Point3(1.0, 2.0, 3.0)); - visualSLAM::Values expected; - expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); - expected.insert(1, Point3(1.1, 2.1, 3.1)); + Values expected; + expected.insert(PoseKey(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); + expected.insert(PointKey(1), Point3(1.1, 2.1, 3.1)); Ordering largeOrdering; - visualSLAM::Values largeValues = init; - largeValues.insert(2, Pose3()); + Values largeValues = init; + largeValues.insert(PoseKey(2), Pose3()); largeOrdering += "x1","l1","x2"; VectorValues delta(largeValues.dims(largeOrdering)); delta[largeOrdering["x1"]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); delta[largeOrdering["l1"]] = Vector_(3, 0.1, 0.1, 0.1); delta[largeOrdering["x2"]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1); - visualSLAM::Values actual = init.retract(delta, largeOrdering); + Values actual = init.retract(delta, largeOrdering); CHECK(assert_equal(expected,actual)); } diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index 931bcfaf8..c1a9123d9 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -25,7 +25,6 @@ #include #include #include -#include #include #include #include @@ -40,25 +39,22 @@ namespace visualSLAM { */ typedef TypedSymbol PoseKey; ///< The key type used for poses typedef TypedSymbol PointKey; ///< The key type used for points - typedef Values PoseValues; ///< Values used for poses - typedef Values PointValues; ///< Values used for points - typedef TupleValues2 Values; ///< Values data structure typedef boost::shared_ptr shared_values; ///< shared pointer to values data structure - typedef NonlinearEquality PoseConstraint; ///< put a hard constraint on a pose - typedef NonlinearEquality PointConstraint; ///< put a hard constraint on a point - typedef PriorFactor PosePrior; ///< put a soft prior on a Pose - typedef PriorFactor PointPrior; ///< put a soft prior on a point - typedef RangeFactor RangeFactor; ///< put a factor on the range from a pose to a point + typedef NonlinearEquality PoseConstraint; ///< put a hard constraint on a pose + typedef NonlinearEquality PointConstraint; ///< put a hard constraint on a point + typedef PriorFactor PosePrior; ///< put a soft prior on a Pose + typedef PriorFactor PointPrior; ///< put a soft prior on a point + typedef RangeFactor RangeFactor; ///< put a factor on the range from a pose to a point /// monocular and stereo camera typedefs for general use - typedef GenericProjectionFactor ProjectionFactor; - typedef GenericStereoFactor StereoFactor; + typedef GenericProjectionFactor ProjectionFactor; + typedef GenericStereoFactor StereoFactor; /** * Non-linear factor graph for vanilla visual SLAM */ - class Graph: public NonlinearFactorGraph { + class Graph: public NonlinearFactorGraph { public: /// shared pointer to this type of graph @@ -70,12 +66,12 @@ namespace visualSLAM { /// print out graph void print(const std::string& s = "") const { - NonlinearFactorGraph::print(s); + NonlinearFactorGraph::print(s); } /// check if two graphs are equal bool equals(const Graph& p, double tol = 1e-9) const { - return NonlinearFactorGraph::equals(p, tol); + return NonlinearFactorGraph::equals(p, tol); } /** @@ -148,6 +144,6 @@ namespace visualSLAM { }; // Graph /// typedef for Optimizer. The current default will use the multi-frontal solver - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; } // namespaces diff --git a/tests/Makefile.am b/tests/Makefile.am index 19378a83f..fb1172222 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -16,10 +16,10 @@ check_PROGRAMS += testGaussianJunctionTree check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph check_PROGRAMS += testNonlinearOptimizer testDoglegOptimizer check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph -check_PROGRAMS += testTupleValues +#check_PROGRAMS += testTupleValues check_PROGRAMS += testNonlinearISAM check_PROGRAMS += testBoundingConstraint -check_PROGRAMS += testPose2SLAMwSPCG +#check_PROGRAMS += testPose2SLAMwSPCG check_PROGRAMS += testGaussianISAM2 check_PROGRAMS += testExtendedKalmanFilter check_PROGRAMS += testRot3Optimization diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 522d2d9a8..c15070b4c 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -31,10 +31,10 @@ SharedDiagonal soft_model2 = noiseModel::Unit::Create(2); SharedDiagonal soft_model2_alt = noiseModel::Isotropic::Sigma(2, 0.1); SharedDiagonal hard_model1 = noiseModel::Constrained::All(1); -typedef NonlinearFactorGraph Graph; +typedef NonlinearFactorGraph Graph; typedef boost::shared_ptr shared_graph; -typedef boost::shared_ptr shared_values; -typedef NonlinearOptimizer Optimizer; +typedef boost::shared_ptr shared_values; +typedef NonlinearOptimizer Optimizer; // some simple inequality constraints simulated2D::PoseKey key(1); @@ -50,7 +50,7 @@ iq2D::PoseYInequality constraint4(key, 2.0, false, mu); /* ************************************************************************* */ TEST( testBoundingConstraint, unary_basics_inactive1 ) { Point2 pt1(2.0, 3.0); - simulated2D::Values config; + Values config; config.insert(key, pt1); EXPECT(!constraint1.active(config)); EXPECT(!constraint2.active(config)); @@ -69,7 +69,7 @@ TEST( testBoundingConstraint, unary_basics_inactive1 ) { /* ************************************************************************* */ TEST( testBoundingConstraint, unary_basics_inactive2 ) { Point2 pt2(-2.0, -3.0); - simulated2D::Values config; + Values config; config.insert(key, pt2); EXPECT(!constraint3.active(config)); EXPECT(!constraint4.active(config)); @@ -88,7 +88,7 @@ TEST( testBoundingConstraint, unary_basics_inactive2 ) { /* ************************************************************************* */ TEST( testBoundingConstraint, unary_basics_active1 ) { Point2 pt2(-2.0, -3.0); - simulated2D::Values config; + Values config; config.insert(key, pt2); EXPECT(constraint1.active(config)); EXPECT(constraint2.active(config)); @@ -103,7 +103,7 @@ TEST( testBoundingConstraint, unary_basics_active1 ) { /* ************************************************************************* */ TEST( testBoundingConstraint, unary_basics_active2 ) { Point2 pt1(2.0, 3.0); - simulated2D::Values config; + Values config; config.insert(key, pt1); EXPECT(constraint3.active(config)); EXPECT(constraint4.active(config)); @@ -118,7 +118,7 @@ TEST( testBoundingConstraint, unary_basics_active2 ) { /* ************************************************************************* */ TEST( testBoundingConstraint, unary_linearization_inactive) { Point2 pt1(2.0, 3.0); - simulated2D::Values config1; + Values config1; config1.insert(key, pt1); Ordering ordering; ordering += key; @@ -131,7 +131,7 @@ TEST( testBoundingConstraint, unary_linearization_inactive) { /* ************************************************************************* */ TEST( testBoundingConstraint, unary_linearization_active) { Point2 pt2(-2.0, -3.0); - simulated2D::Values config2; + Values config2; config2.insert(key, pt2); Ordering ordering; ordering += key; @@ -156,11 +156,11 @@ TEST( testBoundingConstraint, unary_simple_optimization1) { graph->add(iq2D::PoseYInequality(x1, 2.0, true)); graph->add(simulated2D::Prior(start_pt, soft_model2, x1)); - shared_values initValues(new simulated2D::Values()); + shared_values initValues(new Values()); initValues->insert(x1, start_pt); Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); - simulated2D::Values expected; + Values expected; expected.insert(x1, goal_pt); CHECK(assert_equal(expected, *actual, tol)); } @@ -178,11 +178,11 @@ TEST( testBoundingConstraint, unary_simple_optimization2) { graph->add(iq2D::PoseYInequality(x1, 2.0, false)); graph->add(simulated2D::Prior(start_pt, soft_model2, x1)); - shared_values initValues(new simulated2D::Values()); + shared_values initValues(new Values()); initValues->insert(x1, start_pt); Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); - simulated2D::Values expected; + Values expected; expected.insert(x1, goal_pt); CHECK(assert_equal(expected, *actual, tol)); } @@ -201,7 +201,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3))); EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4))); - simulated2D::Values config1; + Values config1; config1.insert(key1, pt1); config1.insert(key2, pt1); Ordering ordering; ordering += key1, key2; @@ -238,11 +238,11 @@ TEST( testBoundingConstraint, MaxDistance_simple_optimization) { graph.add(simulated2D::Prior(pt2_init, soft_model2_alt, x2)); graph.add(iq2D::PoseMaxDistConstraint(x1, x2, 2.0)); - simulated2D::Values initial_state; + Values initial_state; initial_state.insert(x1, pt1); initial_state.insert(x2, pt2_init); - simulated2D::Values expected; + Values expected; expected.insert(x1, pt1); expected.insert(x2, pt2_goal); @@ -268,7 +268,7 @@ TEST( testBoundingConstraint, avoid_demo) { graph.add(simulated2D::Odometry(odo, soft_model2_alt, x2, x3)); graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x3_pt, x3)); - simulated2D::Values init, expected; + Values init, expected; init.insert(x1, x1_pt); init.insert(x3, x3_pt); init.insert(l1, l1_pt); diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 2792d08b8..27070b801 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -385,7 +385,7 @@ TEST(DoglegOptimizer, Iterate) { // config far from minimum Point2 x0(3,0); - boost::shared_ptr config(new example::Values); + boost::shared_ptr config(new Values); config->insert(simulated2D::PoseKey(1), x0); // ordering @@ -404,7 +404,7 @@ TEST(DoglegOptimizer, Iterate) { DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, gbn, *fg, *config, *ord, fg->error(*config)); Delta = result.Delta; EXPECT(result.f_error < fg->error(*config)); // Check that error decreases - simulated2D::Values newConfig(config->retract(result.dx_d, *ord)); + Values newConfig(config->retract(result.dx_d, *ord)); (*config) = newConfig; DOUBLES_EQUAL(fg->error(*config), result.f_error, 1e-5); // Check that error is correctly filled in } diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index f747d71c8..5a87950d0 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -20,14 +20,12 @@ #include #include #include -#include #include using namespace gtsam; // Define Types for System Test typedef TypedSymbol TestKey; -typedef Values TestValues; /* ************************************************************************* */ TEST( ExtendedKalmanFilter, linear ) { @@ -37,7 +35,7 @@ TEST( ExtendedKalmanFilter, linear ) { 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); // Create the TestKeys for our example TestKey x0(0), x1(1), x2(2), x3(3); @@ -59,30 +57,30 @@ TEST( ExtendedKalmanFilter, linear ) { // Run iteration 1 // Create motion factor - BetweenFactor factor1(x0, x1, difference, Q); + BetweenFactor factor1(x0, x1, difference, Q); Point2 predict1 = ekf.predict(factor1); EXPECT(assert_equal(expected1,predict1)); // Create the measurement factor - PriorFactor factor2(x1, z1, R); + PriorFactor factor2(x1, z1, R); Point2 update1 = ekf.update(factor2); EXPECT(assert_equal(expected1,update1)); // Run iteration 2 - BetweenFactor factor3(x1, x2, difference, Q); + BetweenFactor factor3(x1, x2, difference, Q); Point2 predict2 = ekf.predict(factor3); EXPECT(assert_equal(expected2,predict2)); - PriorFactor factor4(x2, z2, R); + PriorFactor factor4(x2, z2, R); Point2 update2 = ekf.update(factor4); EXPECT(assert_equal(expected2,update2)); // Run iteration 3 - BetweenFactor factor5(x2, x3, difference, Q); + BetweenFactor factor5(x2, x3, difference, Q); Point2 predict3 = ekf.predict(factor5); EXPECT(assert_equal(expected3,predict3)); - PriorFactor factor6(x3, z3, R); + PriorFactor factor6(x3, z3, R); Point2 update3 = ekf.update(factor6); EXPECT(assert_equal(expected3,update3)); @@ -91,12 +89,12 @@ TEST( ExtendedKalmanFilter, linear ) { // Create Motion Model Factor -class NonlinearMotionModel : public NonlinearFactor2 { +class NonlinearMotionModel : public NonlinearFactor2 { public: typedef TestKey::Value T; private: - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; typedef NonlinearMotionModel This; protected: @@ -163,7 +161,7 @@ public: } /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ - virtual bool equals(const NonlinearFactor2& f, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor2& f, double tol = 1e-9) const { const This *e = dynamic_cast (&f); return (e != NULL) && (key1_ == e->key1_) && (key2_ == e->key2_); } @@ -172,7 +170,7 @@ public: * calculate the error of the factor * Override for systems with unusual noise models */ - virtual double error(const TestValues& c) const { + virtual double error(const Values& c) const { Vector w = whitenedError(c); return 0.5 * w.dot(w); } @@ -183,7 +181,7 @@ public: } /** Vector of errors, whitened ! */ - Vector whitenedError(const TestValues& c) const { + Vector whitenedError(const Values& c) const { return QInvSqrt(c[key1_])*unwhitenedError(c); } @@ -192,7 +190,7 @@ public: * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z * Hence b = z - h(x1,x2) = - error_vector(x) */ - boost::shared_ptr linearize(const TestValues& c, const Ordering& ordering) const { + boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { const X1& x1 = c[key1_]; const X2& x2 = c[key2_]; Matrix A1, A2; @@ -238,13 +236,13 @@ public: }; // Create Measurement Model Factor -class NonlinearMeasurementModel : public NonlinearFactor1 { +class NonlinearMeasurementModel : public NonlinearFactor1 { public: typedef TestKey::Value T; private: - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 Base; typedef NonlinearMeasurementModel This; protected: @@ -300,7 +298,7 @@ public: } /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ - virtual bool equals(const NonlinearFactor1& f, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor1& f, double tol = 1e-9) const { const This *e = dynamic_cast (&f); return (e != NULL) && (key_ == e->key_); } @@ -309,7 +307,7 @@ public: * calculate the error of the factor * Override for systems with unusual noise models */ - virtual double error(const TestValues& c) const { + virtual double error(const Values& c) const { Vector w = whitenedError(c); return 0.5 * w.dot(w); } @@ -320,7 +318,7 @@ public: } /** Vector of errors, whitened ! */ - Vector whitenedError(const TestValues& c) const { + Vector whitenedError(const Values& c) const { return RInvSqrt(c[key_])*unwhitenedError(c); } @@ -329,7 +327,7 @@ public: * Ax-b \approx h(x1+dx1)-z = h(x1) + A1*dx1 - z * Hence b = z - h(x1) = - error_vector(x) */ - boost::shared_ptr linearize(const TestValues& c, const Ordering& ordering) const { + boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { const X& x1 = c[key_]; Matrix A1; Vector b = -evaluateError(x1, A1); @@ -367,8 +365,8 @@ public: TEST( ExtendedKalmanFilter, nonlinear ) { // Create the set of expected output TestValues (generated using Matlab Kalman Filter) - Point2 expected_predict[10]; - Point2 expected_update[10]; + Point2 expected_predict[11]; + Point2 expected_update[11]; expected_predict[0] = Point2(0.81,0.99); expected_update[0] = Point2(0.824926197027,0.29509808); expected_predict[1] = Point2(0.680503230541,0.24343413); @@ -408,7 +406,7 @@ TEST( ExtendedKalmanFilter, nonlinear ) { 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); // Enter Predict-Update Loop Point2 x_predict, x_update; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index d28844cd9..c4f8c69fc 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -33,10 +33,10 @@ const double tol = 1e-4; TEST(ISAM2, AddVariables) { // Create initial state - planarSLAM::Values theta; + Values theta; theta.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3)); theta.insert(planarSLAM::PointKey(0), Point2(.4, .5)); - planarSLAM::Values newTheta; + Values newTheta; newTheta.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8)); VectorValues deltaUnpermuted; @@ -51,7 +51,7 @@ TEST(ISAM2, AddVariables) { Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0); - GaussianISAM2::Nodes nodes(2); + GaussianISAM2<>::Nodes nodes(2); // Verify initial state LONGS_EQUAL(0, ordering[planarSLAM::PointKey(0)]); @@ -60,7 +60,7 @@ TEST(ISAM2, AddVariables) { EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[planarSLAM::PoseKey(0)]])); // Create expected state - planarSLAM::Values thetaExpected; + Values thetaExpected; thetaExpected.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3)); thetaExpected.insert(planarSLAM::PointKey(0), Point2(.4, .5)); thetaExpected.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8)); @@ -79,11 +79,11 @@ TEST(ISAM2, AddVariables) { Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1); - GaussianISAM2::Nodes nodesExpected( - 3, GaussianISAM2::sharedClique()); + GaussianISAM2<>::Nodes nodesExpected( + 3, GaussianISAM2<>::sharedClique()); // Expand initial state - GaussianISAM2::Impl::AddVariables(newTheta, theta, delta, ordering, nodes); + GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, ordering, nodes); EXPECT(assert_equal(thetaExpected, theta)); EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted)); @@ -95,7 +95,7 @@ TEST(ISAM2, AddVariables) { //TEST(ISAM2, IndicesFromFactors) { // // using namespace gtsam::planarSLAM; -// typedef GaussianISAM2::Impl Impl; +// typedef GaussianISAM2::Impl Impl; // // Ordering ordering; ordering += PointKey(0), PoseKey(0), PoseKey(1); // planarSLAM::Graph graph; @@ -114,7 +114,7 @@ TEST(ISAM2, AddVariables) { /* ************************************************************************* */ //TEST(ISAM2, CheckRelinearization) { // -// typedef GaussianISAM2::Impl Impl; +// typedef GaussianISAM2::Impl Impl; // // // Create values where indices 1 and 3 are above the threshold of 0.1 // VectorValues values; @@ -148,7 +148,7 @@ TEST(ISAM2, AddVariables) { TEST(ISAM2, optimize2) { // Create initialization - planarSLAM::Values theta; + Values theta; theta.insert(planarSLAM::PoseKey(0), Pose2(0.01, 0.01, 0.01)); // Create conditional @@ -168,8 +168,8 @@ TEST(ISAM2, optimize2) { conditional->solveInPlace(expected); // Clique - GaussianISAM2::sharedClique clique( - GaussianISAM2::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr()))); + GaussianISAM2<>::sharedClique clique( + GaussianISAM2<>::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr()))); VectorValues actual(theta.dims(ordering)); conditional->rhs(actual); optimize2(clique, actual); @@ -180,15 +180,15 @@ TEST(ISAM2, optimize2) { } /* ************************************************************************* */ -bool isam_check(const planarSLAM::Graph& fullgraph, const planarSLAM::Values& fullinit, const GaussianISAM2& isam) { - planarSLAM::Values actual = isam.calculateEstimate(); +bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const GaussianISAM2<>& isam) { + Values actual = isam.calculateEstimate(); Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first; GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); // linearized.print("Expected linearized: "); GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); // gbn.print("Expected bayesnet: "); VectorValues delta = optimize(gbn); - planarSLAM::Values expected = fullinit.retract(delta, ordering); + Values expected = fullinit.retract(delta, ordering); return assert_equal(expected, actual); } @@ -210,8 +210,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton) SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - planarSLAM::Values fullinit; + GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + Values fullinit; planarSLAM::Graph fullgraph; // i keeps track of the time step @@ -223,7 +223,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); @@ -238,7 +238,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -253,7 +253,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); @@ -271,7 +271,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -286,7 +286,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); @@ -298,7 +298,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) CHECK(isam_check(fullgraph, fullinit, isam)); // Check gradient at each node - typedef GaussianISAM2::sharedClique sharedClique; + typedef GaussianISAM2<>::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; @@ -343,8 +343,8 @@ TEST(ISAM2, slamlike_solution_dogleg) SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); - planarSLAM::Values fullinit; + GaussianISAM2<> isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); + Values fullinit; planarSLAM::Graph fullgraph; // i keeps track of the time step @@ -356,7 +356,7 @@ TEST(ISAM2, slamlike_solution_dogleg) newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); @@ -371,7 +371,7 @@ TEST(ISAM2, slamlike_solution_dogleg) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -386,7 +386,7 @@ TEST(ISAM2, slamlike_solution_dogleg) newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); @@ -404,7 +404,7 @@ TEST(ISAM2, slamlike_solution_dogleg) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -419,7 +419,7 @@ TEST(ISAM2, slamlike_solution_dogleg) newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); @@ -431,7 +431,7 @@ TEST(ISAM2, slamlike_solution_dogleg) CHECK(isam_check(fullgraph, fullinit, isam)); // Check gradient at each node - typedef GaussianISAM2::sharedClique sharedClique; + typedef GaussianISAM2<>::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; @@ -471,8 +471,8 @@ TEST(ISAM2, clone) { SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); - planarSLAM::Values fullinit; + GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); + Values fullinit; planarSLAM::Graph fullgraph; // i keeps track of the time step @@ -484,7 +484,7 @@ TEST(ISAM2, clone) { newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); @@ -499,7 +499,7 @@ TEST(ISAM2, clone) { newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -514,7 +514,7 @@ TEST(ISAM2, clone) { newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); @@ -532,7 +532,7 @@ TEST(ISAM2, clone) { newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -547,7 +547,7 @@ TEST(ISAM2, clone) { newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); @@ -556,8 +556,8 @@ TEST(ISAM2, clone) { } // CLONING... - boost::shared_ptr > isam2 - = boost::shared_ptr >(new GaussianISAM2()); + boost::shared_ptr > isam2 + = boost::shared_ptr >(new GaussianISAM2<>()); isam.cloneTo(isam2); CHECK(assert_equal(isam, *isam2)); @@ -644,8 +644,8 @@ TEST(ISAM2, removeFactors) SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - planarSLAM::Values fullinit; + GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + Values fullinit; planarSLAM::Graph fullgraph; // i keeps track of the time step @@ -657,7 +657,7 @@ TEST(ISAM2, removeFactors) newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); @@ -672,7 +672,7 @@ TEST(ISAM2, removeFactors) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -687,7 +687,7 @@ TEST(ISAM2, removeFactors) newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); @@ -705,7 +705,7 @@ TEST(ISAM2, removeFactors) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -721,7 +721,7 @@ TEST(ISAM2, removeFactors) fullgraph.push_back(newfactors[0]); fullgraph.push_back(newfactors[2]); // Don't add measurement on landmark 0 - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); @@ -731,14 +731,14 @@ TEST(ISAM2, removeFactors) // Remove the measurement on landmark 0 FastVector toRemove; toRemove.push_back(result.newFactorsIndices[1]); - isam.update(planarSLAM::Graph(), planarSLAM::Values(), toRemove); + isam.update(planarSLAM::Graph(), Values(), toRemove); } // Compare solutions CHECK(isam_check(fullgraph, fullinit, isam)); // Check gradient at each node - typedef GaussianISAM2::sharedClique sharedClique; + typedef GaussianISAM2<>::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; @@ -783,8 +783,8 @@ TEST(ISAM2, constrained_ordering) SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - planarSLAM::Values fullinit; + GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + Values fullinit; planarSLAM::Graph fullgraph; // We will constrain x3 and x4 to the end @@ -799,7 +799,7 @@ TEST(ISAM2, constrained_ordering) newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); @@ -814,7 +814,7 @@ TEST(ISAM2, constrained_ordering) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -832,7 +832,7 @@ TEST(ISAM2, constrained_ordering) newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); @@ -850,7 +850,7 @@ TEST(ISAM2, constrained_ordering) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -865,7 +865,7 @@ TEST(ISAM2, constrained_ordering) newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); @@ -881,7 +881,7 @@ TEST(ISAM2, constrained_ordering) (isam.getOrdering()[planarSLAM::PoseKey(3)] == 13 && isam.getOrdering()[planarSLAM::PoseKey(4)] == 12)); // Check gradient at each node - typedef GaussianISAM2::sharedClique sharedClique; + typedef GaussianISAM2<>::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index da9a8aede..271f625b6 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -110,7 +110,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2) { // create a graph example::Graph nlfg = createNonlinearFactorGraph(); - example::Values noisy = createNoisyValues(); + Values noisy = createNoisyValues(); Ordering ordering; ordering += "x1","x2","l1"; GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering); @@ -128,7 +128,7 @@ TEST(GaussianJunctionTree, slamlike) { typedef planarSLAM::PoseKey PoseKey; typedef planarSLAM::PointKey PointKey; - planarSLAM::Values init; + Values init; planarSLAM::Graph newfactors; planarSLAM::Graph fullgraph; SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); @@ -179,11 +179,11 @@ TEST(GaussianJunctionTree, slamlike) { GaussianJunctionTree gjt(linearized); VectorValues deltaactual = gjt.optimize(&EliminateQR); - planarSLAM::Values actual = init.retract(deltaactual, ordering); + Values actual = init.retract(deltaactual, ordering); GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); VectorValues delta = optimize(gbn); - planarSLAM::Values expected = init.retract(delta, ordering); + Values expected = init.retract(delta, ordering); EXPECT(assert_equal(expected, actual)); } @@ -198,9 +198,9 @@ TEST(GaussianJunctionTree, simpleMarginal) { fg.addPrior(pose2SLAM::PoseKey(0), Pose2(), sharedSigma(3, 10.0)); fg.addOdometry(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0))); - pose2SLAM::Values init; - init.insert(0, Pose2()); - init.insert(1, Pose2(1.0, 0.0, 0.0)); + Values init; + init.insert(pose2SLAM::PoseKey(0), Pose2()); + init.insert(pose2SLAM::PoseKey(1), Pose2(1.0, 0.0, 0.0)); Ordering ordering; ordering += "x1", "x0"; diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index db9874c89..b41da9286 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -37,22 +37,22 @@ using namespace gtsam; // -> x3 -> x4 // -> x5 TEST ( Ordering, predecessorMap2Keys ) { - typedef TypedSymbol Key; - PredecessorMap p_map; + typedef TypedSymbol PoseKey; + PredecessorMap p_map; p_map.insert(1,1); p_map.insert(2,1); p_map.insert(3,1); p_map.insert(4,3); p_map.insert(5,1); - list expected; - expected += 4,5,3,2,1;//Key(4), Key(5), Key(3), Key(2), Key(1); + list expected; + expected += 4,5,3,2,1;//PoseKey(4), PoseKey(5), PoseKey(3), PoseKey(2), PoseKey(1); - list actual = predecessorMap2Keys(p_map); + list actual = predecessorMap2Keys(p_map); LONGS_EQUAL(expected.size(), actual.size()); - list::const_iterator it1 = expected.begin(); - list::const_iterator it2 = actual.begin(); + list::const_iterator it1 = expected.begin(); + list::const_iterator it2 = actual.begin(); for(; it1!=expected.end(); it1++, it2++) CHECK(*it1 == *it2) } @@ -86,22 +86,22 @@ TEST( Graph, composePoses ) graph.addOdometry(2,3, p23, cov); graph.addOdometry(4,3, p43, cov); - PredecessorMap tree; - tree.insert(1,2); - tree.insert(2,2); - tree.insert(3,2); - tree.insert(4,3); + PredecessorMap tree; + tree.insert(pose2SLAM::PoseKey(1),2); + tree.insert(pose2SLAM::PoseKey(2),2); + tree.insert(pose2SLAM::PoseKey(3),2); + tree.insert(pose2SLAM::PoseKey(4),3); Pose2 rootPose = p2; - boost::shared_ptr actual = composePoses (graph, tree, rootPose); + boost::shared_ptr actual = composePoses (graph, tree, rootPose); - pose2SLAM::Values expected; - expected.insert(1, p1); - expected.insert(2, p2); - expected.insert(3, p3); - expected.insert(4, p4); + Values expected; + expected.insert(pose2SLAM::PoseKey(1), p1); + expected.insert(pose2SLAM::PoseKey(2), p2); + expected.insert(pose2SLAM::PoseKey(3), p3); + expected.insert(pose2SLAM::PoseKey(4), p4); LONGS_EQUAL(4, actual->size()); CHECK(assert_equal(expected, *actual)); diff --git a/tests/testInference.cpp b/tests/testInference.cpp index 555f8a061..e530393f8 100644 --- a/tests/testInference.cpp +++ b/tests/testInference.cpp @@ -61,7 +61,7 @@ TEST( Inference, marginals2) fg.addBearingRange(planarSLAM::PoseKey(1), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel); fg.addBearingRange(planarSLAM::PoseKey(2), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel); - planarSLAM::Values init; + Values init; init.insert(planarSLAM::PoseKey(0), Pose2(0.0,0.0,0.0)); init.insert(planarSLAM::PoseKey(1), Pose2(1.0,0.0,0.0)); init.insert(planarSLAM::PoseKey(2), Pose2(2.0,0.0,0.0)); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index fd1459a17..bdbb3fdd2 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -32,20 +32,19 @@ namespace eq2D = simulated2D::equality_constraints; static const double tol = 1e-5; typedef TypedSymbol PoseKey; -typedef Values PoseValues; -typedef PriorFactor PosePrior; -typedef NonlinearEquality PoseNLE; +typedef PriorFactor PosePrior; +typedef NonlinearEquality PoseNLE; typedef boost::shared_ptr shared_poseNLE; -typedef NonlinearFactorGraph PoseGraph; -typedef NonlinearOptimizer PoseOptimizer; +typedef NonlinearFactorGraph PoseGraph; +typedef NonlinearOptimizer PoseOptimizer; PoseKey key(1); /* ************************************************************************* */ TEST ( NonlinearEquality, linearization ) { Pose2 value = Pose2(2.1, 1.0, 2.0); - PoseValues linearize; + Values linearize; linearize.insert(key, value); // create a nonlinear equality constraint @@ -63,7 +62,7 @@ TEST ( NonlinearEquality, linearization_pose ) { PoseKey key(1); Pose2 value; - PoseValues config; + Values config; config.insert(key, value); // create a nonlinear equality constraint @@ -77,7 +76,7 @@ TEST ( NonlinearEquality, linearization_pose ) { TEST ( NonlinearEquality, linearization_fail ) { Pose2 value = Pose2(2.1, 1.0, 2.0); Pose2 wrong = Pose2(2.1, 3.0, 4.0); - PoseValues bad_linearize; + Values bad_linearize; bad_linearize.insert(key, wrong); // create a nonlinear equality constraint @@ -93,7 +92,7 @@ TEST ( NonlinearEquality, linearization_fail_pose ) { PoseKey key(1); Pose2 value(2.0, 1.0, 2.0), wrong(2.0, 3.0, 4.0); - PoseValues bad_linearize; + Values bad_linearize; bad_linearize.insert(key, wrong); // create a nonlinear equality constraint @@ -109,7 +108,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) { PoseKey key(1); Pose2 value, wrong(2.0, 3.0, 4.0); - PoseValues bad_linearize; + Values bad_linearize; bad_linearize.insert(key, wrong); // create a nonlinear equality constraint @@ -123,7 +122,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) { TEST ( NonlinearEquality, error ) { Pose2 value = Pose2(2.1, 1.0, 2.0); Pose2 wrong = Pose2(2.1, 3.0, 4.0); - PoseValues feasible, bad_linearize; + Values feasible, bad_linearize; feasible.insert(key, value); bad_linearize.insert(key, wrong); @@ -168,7 +167,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { EXPECT(assert_equal(expVec, actVec, 1e-5)); // the actual error should have a gain on it - PoseValues config; + Values config; config.insert(key1, badPoint1); double actError = nle.error(config); DOUBLES_EQUAL(500.0, actError, 1e-9); @@ -195,7 +194,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) { // initialize away from the ideal Pose2 initPose(0.0, 2.0, 3.0); - boost::shared_ptr init(new PoseValues()); + boost::shared_ptr init(new Values()); init->insert(key1, initPose); // optimize @@ -206,7 +205,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) { PoseOptimizer result = optimizer.levenbergMarquardt(); // verify - PoseValues expected; + Values expected; expected.insert(key1, feasible1); EXPECT(assert_equal(expected, *result.values())); } @@ -219,7 +218,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { Pose2 feasible1(1.0, 2.0, 3.0); // initialize away from the ideal - boost::shared_ptr init(new PoseValues()); + boost::shared_ptr init(new Values()); Pose2 initPose(0.0, 2.0, 3.0); init->insert(key1, initPose); @@ -242,7 +241,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { PoseOptimizer result = optimizer.levenbergMarquardt(); // verify - PoseValues expected; + Values expected; expected.insert(key1, feasible1); EXPECT(assert_equal(expected, *result.values())); } @@ -251,10 +250,10 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { SharedDiagonal hard_model = noiseModel::Constrained::All(2); SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0); -typedef NonlinearFactorGraph Graph; +typedef NonlinearFactorGraph Graph; typedef boost::shared_ptr shared_graph; -typedef boost::shared_ptr shared_values; -typedef NonlinearOptimizer Optimizer; +typedef boost::shared_ptr shared_values; +typedef NonlinearOptimizer Optimizer; /* ************************************************************************* */ TEST( testNonlinearEqualityConstraint, unary_basics ) { @@ -263,14 +262,14 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) { double mu = 1000.0; eq2D::UnaryEqualityConstraint constraint(pt, key, mu); - simulated2D::Values config1; + Values config1; config1.insert(key, pt); EXPECT(constraint.active(config1)); EXPECT(assert_equal(zero(2), constraint.evaluateError(pt), tol)); EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); - simulated2D::Values config2; + Values config2; Point2 ptBad1(2.0, 2.0); config2.insert(key, ptBad1); EXPECT(constraint.active(config2)); @@ -288,13 +287,13 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { ordering += key; eq2D::UnaryEqualityConstraint constraint(pt, key, mu); - simulated2D::Values config1; + Values config1; config1.insert(key, pt); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering); GaussianFactor::shared_ptr expected1(new JacobianFactor(ordering[key], eye(2,2), zero(2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); - simulated2D::Values config2; + Values config2; Point2 ptBad(2.0, 2.0); config2.insert(key, ptBad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2, ordering); @@ -320,13 +319,13 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { graph->push_back(constraint); graph->push_back(factor); - shared_values initValues(new simulated2D::Values()); + shared_values initValues(new Values()); initValues->insert(key, badPt); // verify error values EXPECT(constraint->active(*initValues)); - simulated2D::Values expected; + Values expected; expected.insert(key, truth_pt); EXPECT(constraint->active(expected)); EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol); @@ -342,7 +341,7 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { double mu = 1000.0; eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); - simulated2D::Values config1; + Values config1; config1.insert(key1, x1); config1.insert(key2, x2); EXPECT(constraint.active(config1)); @@ -350,7 +349,7 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); - simulated2D::Values config2; + Values config2; Point2 x1bad(2.0, 2.0); Point2 x2bad(2.0, 2.0); config2.insert(key1, x1bad); @@ -370,7 +369,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { ordering += key1, key2; eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); - simulated2D::Values config1; + Values config1; config1.insert(key1, x1); config1.insert(key2, x2); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering); @@ -379,7 +378,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { eye(2,2), zero(2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); - simulated2D::Values config2; + Values config2; Point2 x1bad(2.0, 2.0); Point2 x2bad(2.0, 2.0); config2.insert(key1, x1bad); @@ -418,12 +417,12 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { graph->push_back(constraint2); graph->push_back(factor); - shared_values initValues(new simulated2D::Values()); + shared_values initValues(new Values()); initValues->insert(key1, Point2()); initValues->insert(key2, badPt); Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); - simulated2D::Values expected; + Values expected; expected.insert(key1, truth_pt1); expected.insert(key2, truth_pt2); CHECK(assert_equal(expected, *actual, tol)); @@ -455,7 +454,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { graph->add(eq2D::PointEqualityConstraint(l1, l2)); - shared_values initialEstimate(new simulated2D::Values()); + shared_values initialEstimate(new Values()); initialEstimate->insert(x1, pt_x1); initialEstimate->insert(x2, Point2()); initialEstimate->insert(l1, Point2(1.0, 6.0)); // ground truth @@ -463,7 +462,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate); - simulated2D::Values expected; + Values expected; expected.insert(x1, pt_x1); expected.insert(l1, Point2(1.0, 6.0)); expected.insert(l2, Point2(1.0, 6.0)); @@ -498,7 +497,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { graph->add(eq2D::PointEqualityConstraint(l1, l2)); // create an initial estimate - shared_values initialEstimate(new simulated2D::Values()); + shared_values initialEstimate(new Values()); initialEstimate->insert(x1, Point2( 1.0, 1.0)); initialEstimate->insert(l1, Point2( 1.0, 6.0)); initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame @@ -507,7 +506,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { // optimize Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate); - simulated2D::Values expected; + Values expected; expected.insert(x1, Point2(1.0, 1.0)); expected.insert(l1, Point2(1.0, 6.0)); expected.insert(l2, Point2(1.0, 6.0)); @@ -522,13 +521,12 @@ Cal3_S2 K(fov,w,h); boost::shared_ptr shK(new Cal3_S2(K)); // typedefs for visual SLAM example -typedef visualSLAM::Values VValues; -typedef boost::shared_ptr shared_vconfig; +typedef boost::shared_ptr shared_vconfig; typedef visualSLAM::Graph VGraph; -typedef NonlinearOptimizer VOptimizer; +typedef NonlinearOptimizer VOptimizer; // factors for visual slam -typedef NonlinearEquality2 Point3Equality; +typedef NonlinearEquality2 Point3Equality; /* ********************************************************************* */ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { @@ -567,7 +565,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { Point3 landmark1(0.5, 5.0, 0.0); Point3 landmark2(1.5, 5.0, 0.0); - shared_vconfig initValues(new VValues()); + shared_vconfig initValues(new Values()); initValues->insert(x1, pose1); initValues->insert(x2, pose2); initValues->insert(l1, landmark1); @@ -577,7 +575,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { VOptimizer::shared_values actual = VOptimizer::optimizeLM(graph, initValues); // create config - VValues truthValues; + Values truthValues; truthValues.insert(x1, camera1.pose()); truthValues.insert(x2, camera2.pose()); truthValues.insert(l1, landmark); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 095f562a4..490692c90 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -40,7 +40,7 @@ using namespace std; using namespace gtsam; using namespace example; -typedef boost::shared_ptr > shared_nlf; +typedef boost::shared_ptr shared_nlf; /* ************************************************************************* */ TEST( NonlinearFactor, equals ) @@ -82,18 +82,18 @@ TEST( NonlinearFactor, NonlinearFactor ) Graph fg = createNonlinearFactorGraph(); // create a values structure for the non linear factor graph - example::Values cfg = createNoisyValues(); + Values cfg = createNoisyValues(); // get the factor "f1" from the factor graph Graph::sharedFactor factor = fg[0]; // calculate the error_vector from the factor "f1" // error_vector = [0.1 0.1] - Vector actual_e = boost::dynamic_pointer_cast >(factor)->unwhitenedError(cfg); + Vector actual_e = boost::dynamic_pointer_cast(factor)->unwhitenedError(cfg); CHECK(assert_equal(0.1*ones(2),actual_e)); // error = 0.5 * [1 1] * [1;1] = 1 - double expected = 1.0; + double expected = 1.0; // calculate the error from the factor "f1" double actual = factor->error(cfg); @@ -103,7 +103,7 @@ TEST( NonlinearFactor, NonlinearFactor ) /* ************************************************************************* */ TEST( NonlinearFactor, linearize_f1 ) { - example::Values c = createNoisyValues(); + Values c = createNoisyValues(); // Grab a non-linear factor Graph nfg = createNonlinearFactorGraph(); @@ -125,7 +125,7 @@ TEST( NonlinearFactor, linearize_f1 ) /* ************************************************************************* */ TEST( NonlinearFactor, linearize_f2 ) { - example::Values c = createNoisyValues(); + Values c = createNoisyValues(); // Grab a non-linear factor Graph nfg = createNonlinearFactorGraph(); @@ -148,7 +148,7 @@ TEST( NonlinearFactor, linearize_f3 ) Graph::sharedFactor nlf = nfg[2]; // We linearize at noisy config from SmallExample - example::Values c = createNoisyValues(); + Values c = createNoisyValues(); GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); @@ -165,7 +165,7 @@ TEST( NonlinearFactor, linearize_f4 ) Graph::sharedFactor nlf = nfg[3]; // We linearize at noisy config from SmallExample - example::Values c = createNoisyValues(); + Values c = createNoisyValues(); GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); @@ -179,14 +179,14 @@ TEST( NonlinearFactor, size ) { // create a non linear factor graph Graph fg = createNonlinearFactorGraph(); - + // create a values structure for the non linear factor graph - example::Values cfg = createNoisyValues(); - + Values cfg = createNoisyValues(); + // get some factors from the graph Graph::sharedFactor factor1 = fg[0], factor2 = fg[1], factor3 = fg[2]; - + CHECK(factor1->size() == 1); CHECK(factor2->size() == 2); CHECK(factor3->size() == 2); @@ -201,7 +201,7 @@ TEST( NonlinearFactor, linearize_constraint1 ) Point2 mu(1., -1.); Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, 1)); - example::Values config; + Values config; config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary()); @@ -221,7 +221,7 @@ TEST( NonlinearFactor, linearize_constraint2 ) Point2 z3(1.,-1.); simulated2D::Measurement f0(z3, constraint, 1,1); - example::Values config; + Values config; config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); config.insert(simulated2D::PointKey(1), Point2(5.0, 4.0)); GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary()); @@ -236,12 +236,11 @@ TEST( NonlinearFactor, linearize_constraint2 ) /* ************************************************************************* */ typedef TypedSymbol TestKey; -typedef gtsam::Values TestValues; /* ************************************************************************* */ -class TestFactor4 : public NonlinearFactor4 { +class TestFactor4 : public NonlinearFactor4 { public: - typedef NonlinearFactor4 Base; + typedef NonlinearFactor4 Base; TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4) {} virtual Vector @@ -263,11 +262,11 @@ public: /* ************************************ */ TEST(NonlinearFactor, NonlinearFactor4) { TestFactor4 tf; - TestValues tv; - tv.insert(1, LieVector(1, 1.0)); - tv.insert(2, LieVector(1, 2.0)); - tv.insert(3, LieVector(1, 3.0)); - tv.insert(4, LieVector(1, 4.0)); + Values tv; + tv.insert(TestKey(1), LieVector(1, 1.0)); + tv.insert(TestKey(2), LieVector(1, 2.0)); + tv.insert(TestKey(3), LieVector(1, 3.0)); + tv.insert(TestKey(4), LieVector(1, 4.0)); EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4); @@ -284,9 +283,9 @@ TEST(NonlinearFactor, NonlinearFactor4) { } /* ************************************************************************* */ -class TestFactor5 : public NonlinearFactor5 { +class TestFactor5 : public NonlinearFactor5 { public: - typedef NonlinearFactor5 Base; + typedef NonlinearFactor5 Base; TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {} virtual Vector @@ -310,12 +309,12 @@ public: /* ************************************ */ TEST(NonlinearFactor, NonlinearFactor5) { TestFactor5 tf; - TestValues tv; - tv.insert(1, LieVector(1, 1.0)); - tv.insert(2, LieVector(1, 2.0)); - tv.insert(3, LieVector(1, 3.0)); - tv.insert(4, LieVector(1, 4.0)); - tv.insert(5, LieVector(1, 5.0)); + Values tv; + tv.insert(TestKey(1), LieVector(1, 1.0)); + tv.insert(TestKey(2), LieVector(1, 2.0)); + tv.insert(TestKey(3), LieVector(1, 3.0)); + tv.insert(TestKey(4), LieVector(1, 4.0)); + tv.insert(TestKey(5), LieVector(1, 5.0)); EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5); @@ -334,9 +333,9 @@ TEST(NonlinearFactor, NonlinearFactor5) { } /* ************************************************************************* */ -class TestFactor6 : public NonlinearFactor6 { +class TestFactor6 : public NonlinearFactor6 { public: - typedef NonlinearFactor6 Base; + typedef NonlinearFactor6 Base; TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {} virtual Vector @@ -362,13 +361,13 @@ public: /* ************************************ */ TEST(NonlinearFactor, NonlinearFactor6) { TestFactor6 tf; - TestValues tv; - tv.insert(1, LieVector(1, 1.0)); - tv.insert(2, LieVector(1, 2.0)); - tv.insert(3, LieVector(1, 3.0)); - tv.insert(4, LieVector(1, 4.0)); - tv.insert(5, LieVector(1, 5.0)); - tv.insert(6, LieVector(1, 6.0)); + Values tv; + tv.insert(TestKey(1), LieVector(1, 1.0)); + tv.insert(TestKey(2), LieVector(1, 2.0)); + tv.insert(TestKey(3), LieVector(1, 3.0)); + tv.insert(TestKey(4), LieVector(1, 4.0)); + tv.insert(TestKey(5), LieVector(1, 5.0)); + tv.insert(TestKey(6), LieVector(1, 6.0)); EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5), TestKey(6); diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 6b926c6d7..1cca4c988 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -51,11 +51,11 @@ TEST( Graph, equals ) TEST( Graph, error ) { Graph fg = createNonlinearFactorGraph(); - example::Values c1 = createValues(); + Values c1 = createValues(); double actual1 = fg.error(c1); DOUBLES_EQUAL( 0.0, actual1, 1e-9 ); - example::Values c2 = createNoisyValues(); + Values c2 = createNoisyValues(); double actual2 = fg.error(c2); DOUBLES_EQUAL( 5.625, actual2, 1e-9 ); } @@ -75,8 +75,12 @@ TEST( Graph, keys ) /* ************************************************************************* */ TEST( Graph, GET_ORDERING) { - Ordering expected; expected += "x1","l1","x2"; +// Ordering expected; expected += "x1","l1","x2"; // For starting with x1,x2,l1 + Ordering expected; expected += "l1","x2","x1"; // For starting with l1,x1,x2 Graph nlfg = createNonlinearFactorGraph(); + SymbolicFactorGraph::shared_ptr symbolic; + Ordering::shared_ptr ordering; + boost::tie(symbolic, ordering) = nlfg.symbolic(createNoisyValues()); Ordering actual = *nlfg.orderingCOLAMD(createNoisyValues()); CHECK(assert_equal(expected,actual)); } @@ -85,7 +89,7 @@ TEST( Graph, GET_ORDERING) TEST( Graph, probPrime ) { Graph fg = createNonlinearFactorGraph(); - example::Values cfg = createValues(); + Values cfg = createValues(); // evaluate the probability of the factor graph double actual = fg.probPrime(cfg); @@ -97,7 +101,7 @@ TEST( Graph, probPrime ) TEST( Graph, linearize ) { Graph fg = createNonlinearFactorGraph(); - example::Values initial = createNoisyValues(); + Values initial = createNoisyValues(); boost::shared_ptr > linearized = fg.linearize(initial, *initial.orderingArbitrary()); FactorGraph expected = createGaussianFactorGraph(*initial.orderingArbitrary()); CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index b90e98625..0ad4e03ae 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -12,7 +12,7 @@ using namespace gtsam; using namespace planarSLAM; -typedef NonlinearISAM PlanarISAM; +typedef NonlinearISAM<> PlanarISAM; const double tol=1e-5; @@ -30,8 +30,8 @@ TEST(testNonlinearISAM, markov_chain ) { Graph start_factors; start_factors.addPoseConstraint(key, cur_pose); - planarSLAM::Values init; - planarSLAM::Values expected; + Values init; + Values expected; init.insert(key, cur_pose); expected.insert(key, cur_pose); isam.update(start_factors, init); @@ -43,7 +43,7 @@ TEST(testNonlinearISAM, markov_chain ) { Graph new_factors; PoseKey key1(i-1), key2(i); new_factors.addOdometry(key1, key2, z, model); - planarSLAM::Values new_init; + Values new_init; // perform a check on changing orderings if (i == 5) { @@ -67,7 +67,7 @@ TEST(testNonlinearISAM, markov_chain ) { } // verify values - all but the last one should be very close - planarSLAM::Values actual = isam.estimate(); + Values actual = isam.estimate(); for (size_t i=0; i Optimizer; +typedef NonlinearOptimizer Optimizer; /* ************************************************************************* */ TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta ) @@ -113,7 +113,7 @@ TEST( NonlinearOptimizer, iterateLM ) // config far from minimum Point2 x0(3,0); - boost::shared_ptr config(new example::Values); + boost::shared_ptr config(new Values); config->insert(simulated2D::PoseKey(1), x0); // ordering @@ -149,13 +149,13 @@ TEST( NonlinearOptimizer, optimize ) // test error at minimum Point2 xstar(0,0); - example::Values cstar; + Values cstar; cstar.insert(simulated2D::PoseKey(1), xstar); DOUBLES_EQUAL(0.0,fg->error(cstar),0.0); // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = Point2 x0(3,3); - boost::shared_ptr c0(new example::Values); + boost::shared_ptr c0(new Values); c0->insert(simulated2D::PoseKey(1), x0); DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3); @@ -185,7 +185,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer ) example::createReallyNonlinearFactorGraph())); Point2 x0(3,3); - boost::shared_ptr c0(new example::Values); + boost::shared_ptr c0(new Values); c0->insert(simulated2D::PoseKey(1), x0); Optimizer::shared_values actual = Optimizer::optimizeLM(fg, c0); @@ -198,7 +198,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer_noshared ) example::Graph fg = example::createReallyNonlinearFactorGraph(); Point2 x0(3,3); - example::Values c0; + Values c0; c0.insert(simulated2D::PoseKey(1), x0); Optimizer::shared_values actual = Optimizer::optimizeLM(fg, c0); @@ -212,7 +212,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer ) example::createReallyNonlinearFactorGraph())); Point2 x0(3,3); - boost::shared_ptr c0(new example::Values); + boost::shared_ptr c0(new Values); c0->insert(simulated2D::PoseKey(1), x0); Optimizer::shared_values actual = Optimizer::optimizeGN(fg, c0); @@ -225,7 +225,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer_noshared ) example::Graph fg = example::createReallyNonlinearFactorGraph(); Point2 x0(3,3); - example::Values c0; + Values c0; c0.insert(simulated2D::PoseKey(1), x0); Optimizer::shared_values actual = Optimizer::optimizeGN(fg, c0); @@ -238,14 +238,14 @@ TEST( NonlinearOptimizer, optimization_method ) example::Graph fg = example::createReallyNonlinearFactorGraph(); Point2 x0(3,3); - example::Values c0; + Values c0; c0.insert(simulated2D::PoseKey(1), x0); - example::Values actualMFQR = optimize( + Values actualMFQR = optimize( fg, c0, *NonlinearOptimizationParameters().newFactorization(true), MULTIFRONTAL, LM); DOUBLES_EQUAL(0,fg.error(actualMFQR),tol); - example::Values actualMFLDL = optimize( + Values actualMFLDL = optimize( fg, c0, *NonlinearOptimizationParameters().newFactorization(false), MULTIFRONTAL, LM); DOUBLES_EQUAL(0,fg.error(actualMFLDL),tol); } @@ -253,11 +253,11 @@ TEST( NonlinearOptimizer, optimization_method ) /* ************************************************************************* */ TEST( NonlinearOptimizer, Factorization ) { - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; - boost::shared_ptr config(new pose2SLAM::Values); - config->insert(1, Pose2(0.,0.,0.)); - config->insert(2, Pose2(1.5,0.,0.)); + boost::shared_ptr config(new Values); + config->insert(pose2SLAM::PoseKey(1), Pose2(0.,0.,0.)); + config->insert(pose2SLAM::PoseKey(2), Pose2(1.5,0.,0.)); boost::shared_ptr graph(new pose2SLAM::Graph); graph->addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); @@ -270,9 +270,9 @@ TEST( NonlinearOptimizer, Factorization ) Optimizer optimizer(graph, config, ordering); Optimizer optimized = optimizer.iterateLM(); - pose2SLAM::Values expected; - expected.insert(1, Pose2(0.,0.,0.)); - expected.insert(2, Pose2(1.,0.,0.)); + Values expected; + expected.insert(pose2SLAM::PoseKey(1), Pose2(0.,0.,0.)); + expected.insert(pose2SLAM::PoseKey(2), Pose2(1.,0.,0.)); CHECK(assert_equal(expected, *optimized.values(), 1e-5)); } @@ -287,13 +287,13 @@ TEST_UNSAFE(NonlinearOptimizer, NullFactor) { // test error at minimum Point2 xstar(0,0); - example::Values cstar; + Values cstar; cstar.insert(simulated2D::PoseKey(1), xstar); DOUBLES_EQUAL(0.0,fg->error(cstar),0.0); // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = Point2 x0(3,3); - boost::shared_ptr c0(new example::Values); + boost::shared_ptr c0(new Values); c0->insert(simulated2D::PoseKey(1), x0); DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3); diff --git a/tests/testPose2SLAMwSPCG.cpp b/tests/testPose2SLAMwSPCG.cpp index c345b37b2..58020051d 100644 --- a/tests/testPose2SLAMwSPCG.cpp +++ b/tests/testPose2SLAMwSPCG.cpp @@ -41,7 +41,7 @@ TEST(testPose2SLAMwSPCG, example1) { graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ; graph.addPrior(x1, Pose2(0,0,0), sigma) ; - pose2SLAM::Values initial; + Values initial; initial.insert(x1, Pose2( 0, 0, 0)); initial.insert(x2, Pose2( 0, 2.1, 0.01)); initial.insert(x3, Pose2( 0, 3.9,-0.01)); @@ -52,7 +52,7 @@ TEST(testPose2SLAMwSPCG, example1) { initial.insert(x8, Pose2(3.9, 2.1, 0.01)); initial.insert(x9, Pose2(4.1, 3.9,-0.01)); - pose2SLAM::Values expected; + Values expected; expected.insert(x1, Pose2(0.0, 0.0, 0.0)); expected.insert(x2, Pose2(0.0, 2.0, 0.0)); expected.insert(x3, Pose2(0.0, 4.0, 0.0)); @@ -63,7 +63,7 @@ TEST(testPose2SLAMwSPCG, example1) { expected.insert(x8, Pose2(4.0, 2.0, 0.0)); expected.insert(x9, Pose2(4.0, 4.0, 0.0)); - pose2SLAM::Values actual = optimizeSPCG(graph, initial); + Values actual = optimizeSPCG(graph, initial); EXPECT(assert_equal(expected, actual, tol)); } diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp index a35165273..285428dab 100644 --- a/tests/testRot3Optimization.cpp +++ b/tests/testRot3Optimization.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include #include @@ -31,27 +30,26 @@ using namespace gtsam; typedef TypedSymbol Key; -typedef Values Rot3Values; -typedef PriorFactor Prior; -typedef BetweenFactor Between; -typedef NonlinearFactorGraph Graph; +typedef PriorFactor Prior; +typedef BetweenFactor Between; +typedef NonlinearFactorGraph Graph; /* ************************************************************************* */ TEST(Rot3, optimize) { // Optimize a circle - Rot3Values truth; - Rot3Values initial; + Values truth; + Values initial; Graph fg; fg.add(Prior(0, Rot3(), sharedSigma(3, 0.01))); for(int j=0; j<6; ++j) { - truth.insert(j, Rot3::Rz(M_PI/3.0 * double(j))); - initial.insert(j, Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); - fg.add(Between(j, (j+1)%6, Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01))); + truth.insert(Key(j), Rot3::Rz(M_PI/3.0 * double(j))); + initial.insert(Key(j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); + fg.add(Between(Key(j), Key((j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01))); } NonlinearOptimizationParameters params; - Rot3Values final = optimize(fg, initial, params); + Values final = optimize(fg, initial, params); EXPECT(assert_equal(truth, final, 1e-5)); } diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp index 72132deb6..2bd37ebbb 100644 --- a/tests/testSerialization.cpp +++ b/tests/testSerialization.cpp @@ -443,7 +443,7 @@ BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measureme TEST (Serialization, smallExample) { using namespace example; Graph nfg = createNonlinearFactorGraph(); - example::Values c1 = createValues(); + Values c1 = createValues(); EXPECT(equalsObj(nfg)); EXPECT(equalsXML(nfg)); @@ -462,7 +462,7 @@ BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint, "gtsam::planarSLAM::Constraint" /* ************************************************************************* */ TEST (Serialization, planar_system) { using namespace planarSLAM; - planarSLAM::Values values; + Values values; values.insert(PointKey(3), Point2(1.0, 2.0)); values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3)); @@ -488,7 +488,7 @@ TEST (Serialization, planar_system) { // text EXPECT(equalsObj(PoseKey(2))); EXPECT(equalsObj(PointKey(3))); - EXPECT(equalsObj(values)); + EXPECT(equalsObj(values)); EXPECT(equalsObj(prior)); EXPECT(equalsObj(bearing)); EXPECT(equalsObj(bearingRange)); @@ -500,7 +500,7 @@ TEST (Serialization, planar_system) { // xml EXPECT(equalsXML(PoseKey(2))); EXPECT(equalsXML(PointKey(3))); - EXPECT(equalsXML(values)); + EXPECT(equalsXML(values)); EXPECT(equalsXML(prior)); EXPECT(equalsXML(bearing)); EXPECT(equalsXML(bearingRange)); @@ -522,7 +522,7 @@ BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoF /* ************************************************************************* */ TEST (Serialization, visual_system) { using namespace visualSLAM; - visualSLAM::Values values; + Values values; PoseKey x1(1), x2(2); PointKey l1(1), l2(2); Pose3 pose1 = pose3, pose2 = pose3.inverse(); diff --git a/tests/timeMultifrontalOnDataset.cpp b/tests/timeMultifrontalOnDataset.cpp index 8fe127989..874ccf8bb 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 d9b71576f..9e1b986f9 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)