diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 9727ceacb..511e7eb3f 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -92,13 +92,13 @@ int main(int argc, char* argv[]) { graph.push_back(factor); /* 3. Create an initial estimate for the camera pose */ - DynamicValues 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*/ - DynamicValues result = optimize (graph, initial); + Values result = optimize (graph, initial); result.print("Final result: "); return 0; diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp index b73a989d4..c84924816 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 10b1b3576..fd6d2cbfe 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -108,7 +108,7 @@ int main(int argc, char** argv) { graph->print("Full Graph"); // initialize to noisy points - boost::shared_ptr initial(new DynamicValues); + 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 b6bfe7303..0e60c46df 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 DynamicValues); + 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(); - DynamicValues 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 fb98ebd97..e4281aa09 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 */ - DynamicValues 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 */ - DynamicValues 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 b298ed92a..adc9f6780 100644 --- a/examples/Pose2SLAMwSPCG_advanced.cpp +++ b/examples/Pose2SLAMwSPCG_advanced.cpp @@ -27,17 +27,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) { @@ -47,7 +47,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 51be80841..aa11fd959 100644 --- a/examples/Pose2SLAMwSPCG_easy.cpp +++ b/examples/Pose2SLAMwSPCG_easy.cpp @@ -27,7 +27,7 @@ using namespace gtsam; using namespace pose2SLAM; Graph graph; -DynamicValues initial, result; +Values initial, result; /* ************************************************************************* */ int main(void) { diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index bd00f4a04..821b177dc 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -112,7 +112,7 @@ int main() { * on the type of key used to find the appropriate value map if there * are multiple types of variables. */ - DynamicValues initial; + Values initial; initial.insert(key, Rot2::fromAngle(20 * degree)); initial.print("initial estimate"); @@ -124,7 +124,7 @@ int main() { * initial estimate. This will yield a new RotValues structure * with the final state of the optimization. */ - DynamicValues result = optimize(graph, initial); + Values result = optimize(graph, initial); result.print("final result"); return 0; diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index d001016c4..30d77aedc 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -46,7 +46,7 @@ int main() { Ordering::shared_ptr ordering(new Ordering); // Create a structure to hold the linearization points - DynamicValues linearizationPoints; + Values linearizationPoints; // Ground truth example // Start at origin, move to the right (x-axis): 0,0 0,1 0,2 diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index ce88276c9..f197399df 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -80,7 +80,7 @@ void readAllDataISAM() { /** * Setup newFactors and initialValues for each new pose and set of measurements at each frame. */ -void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, +void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, int pose_id, const Pose3& pose, const std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { // Create a graph of newFactors with new measurements @@ -101,7 +101,7 @@ void createNewFactors(shared_ptr& newFactors, boost::shared_ptr (new DynamicValues()); + initialValues = shared_ptr (new Values()); initialValues->insert(PoseKey(pose_id), pose); for (size_t i = 0; i < measurements.size(); i++) { initialValues->insert(PointKey(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]); @@ -132,12 +132,12 @@ int main(int argc, char* argv[]) { BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) { const int poseId = features.first; shared_ptr newFactors; - shared_ptr initialValues; + shared_ptr initialValues; createNewFactors(newFactors, initialValues, poseId, g_poses[poseId], features.second, measurementSigma, g_calib); isam.update(*newFactors, *initialValues); - DynamicValues 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 1edb4a9ae..70ffaf05a 100644 --- a/examples/vSLAMexample/vSFMexample.cpp +++ b/examples/vSLAMexample/vSFMexample.cpp @@ -103,9 +103,9 @@ 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. */ -DynamicValues initialize(std::map landmarks, std::map poses) { +Values initialize(std::map landmarks, std::map poses) { - DynamicValues initValues; + Values initValues; // Initialize landmarks 3D positions. for (map::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++) @@ -136,7 +136,7 @@ int main(int argc, char* argv[]) { boost::shared_ptr graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib))); // Create an initial Values structure using groundtruth values as the initial estimates - boost::shared_ptr initialEstimates(new DynamicValues(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 08727f9ef..0d6c3db12 100644 --- a/gtsam.h +++ b/gtsam.h @@ -321,9 +321,9 @@ class Graph { void print(string s) const; - double error(const planarSLAM::Values& values) const; - Ordering* orderingCOLAMD(const planarSLAM::Values& values) const; - GaussianFactorGraph* linearize(const planarSLAM::Values& values, + double error(const Values& values) const; + Ordering* orderingCOLAMD(const Values& values) const; + GaussianFactorGraph* linearize(const Values& values, const Ordering& ordering) const; void addPrior(int key, const Pose2& pose, const SharedNoiseModel& noiseModel); @@ -333,14 +333,14 @@ class Graph { void addRange(int poseKey, int pointKey, double range, const SharedNoiseModel& noiseModel); void addBearingRange(int poseKey, int pointKey, const Rot2& bearing, double range, const SharedNoiseModel& noiseModel); - planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate); + Values optimize(const Values& initialEstimate); }; class Odometry { Odometry(int key1, int key2, const Pose2& measured, const SharedNoiseModel& model); void print(string s) const; - GaussianFactor* linearize(const planarSLAM::Values& center, const Ordering& ordering) const; + GaussianFactor* linearize(const Values& center, const Ordering& ordering) const; }; }///\namespace planarSLAM diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 5fc784a3e..be4ddf840 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -143,11 +143,11 @@ 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 { KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); @@ -160,7 +160,7 @@ public: /* ************************************************************************* */ template -boost::shared_ptr composePoses(const G& graph, const PredecessorMap& tree, +boost::shared_ptr composePoses(const G& graph, const PredecessorMap& tree, const POSE& rootPose) { //TODO: change edge_weight_t to edge_pose_t @@ -207,7 +207,7 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorM } // compose poses - boost::shared_ptr config(new DynamicValues); + boost::shared_ptr config(new Values); KEY rootKey = boost::get(boost::vertex_name, g, root); config->insert(rootKey, rootPose); compose_key_visitor vis(config); diff --git a/gtsam/inference/graph.h b/gtsam/inference/graph.h index 6e51a9f6c..98d41940b 100644 --- a/gtsam/inference/graph.h +++ b/gtsam/inference/graph.h @@ -25,7 +25,7 @@ #include #include #include -#include +#include namespace gtsam { @@ -89,7 +89,7 @@ namespace gtsam { * Compose the poses by following the chain specified by the spanning tree */ template - boost::shared_ptr + 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 78fa4fdfe..573d97f04 100644 --- a/gtsam/linear/SubgraphSolver-inl.h +++ b/gtsam/linear/SubgraphSolver-inl.h @@ -60,7 +60,7 @@ VectorValues::shared_ptr SubgraphSolver::optimize() const { } template -void SubgraphSolver::initialize(const GRAPH& G, const DynamicValues& theta0) { +void SubgraphSolver::initialize(const GRAPH& G, const Values& theta0) { // generate spanning tree PredecessorMap tree_ = gtsam::findMinimumSpanningTree(G); diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 6bb592005..30acb8fc4 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -16,7 +16,7 @@ #include #include #include -#include +#include namespace gtsam { @@ -44,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 ; @@ -62,7 +62,7 @@ private: public: - SubgraphSolver(const GRAPH& G, const DynamicValues& 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) { @@ -90,7 +90,7 @@ public: shared_ordering ordering() const { return ordering_; } protected: - void initialize(const GRAPH& G, const DynamicValues& theta0); + void initialize(const GRAPH& G, const Values& theta0); private: SubgraphSolver():IterativeSolver(){} diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 7d3314d52..2c4e382a1 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -30,7 +30,7 @@ namespace gtsam { template typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_( const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering, - const DynamicValues& linearizationPoints, const KEY& lastKey, + const Values& linearizationPoints, const KEY& lastKey, JacobianFactor::shared_ptr& newPrior) const { // Extract the Index of the provided last key @@ -100,7 +100,7 @@ namespace gtsam { ordering.insert(x1, 1); // Create a set of linearization points - DynamicValues linearizationPoints; + Values linearizationPoints; linearizationPoints.insert(x0, x_); linearizationPoints.insert(x1, x_); // TODO should this really be x_ ? @@ -138,7 +138,7 @@ namespace gtsam { ordering.insert(x0, 0); // Create a set of linearization points - DynamicValues 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 3bf8c1887..2963638bf 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -53,7 +53,7 @@ namespace gtsam { JacobianFactor::shared_ptr priorFactor_; // density T solve_(const GaussianFactorGraph& linearFactorGraph, - const Ordering& ordering, const DynamicValues& linearizationPoints, + const Ordering& ordering, const Values& linearizationPoints, const KEY& x, JacobianFactor::shared_ptr& newPrior) const; public: diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index 7ddd2f8c6..c37892654 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -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 DynamicValues& newTheta, DynamicValues& 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(DynamicValues& values, const Permuted& delta, + static void ExpmapMasked(Values& values, const Permuted& delta, const Ordering& ordering, const std::vector& mask, boost::optional&> invalidateIfDebug = boost::optional&>()); @@ -136,7 +136,7 @@ struct _VariableAdder { /* ************************************************************************* */ template void ISAM2::Impl::AddVariables( - const DynamicValues& newTheta, DynamicValues& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes) { + const Values& newTheta, Values& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes) { const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); @@ -259,7 +259,7 @@ struct _SelectiveExpmapAndClear { /* ************************************************************************* */ template -void ISAM2::Impl::ExpmapMasked(DynamicValues& values, const Permuted& delta, +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 diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 231fbdea7..6435331ba 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -580,10 +580,10 @@ ISAM2Result ISAM2::update( /* ************************************************************************* */ template -DynamicValues ISAM2::calculateEstimate() const { +Values ISAM2::calculateEstimate() const { // We use ExpmapMasked here instead of regular expmap because the former // handles Permuted - DynamicValues ret(theta_); + Values ret(theta_); vector mask(ordering_.nVars(), true); Impl::ExpmapMasked(ret, delta_, ordering_, mask); return ret; @@ -600,7 +600,7 @@ typename KEY::Value ISAM2::calculateEstimate(const KEY& key) /* ************************************************************************* */ template -DynamicValues ISAM2::calculateBestEstimate() const { +Values ISAM2::calculateBestEstimate() const { VectorValues delta(theta_.dims(ordering_)); optimize2(this->root(), delta); return theta_.retract(delta, ordering_); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 85db8b691..62a86cf64 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -271,7 +271,7 @@ class ISAM2: public BayesTree > { protected: /** The current linearization point */ - DynamicValues theta_; + Values theta_; /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */ VariableIndex variableIndex_; @@ -314,7 +314,7 @@ public: typedef BayesTree > Base; ///< The BayesTree base class typedef ISAM2 This; ///< This class - typedef DynamicValues Values; + typedef Values Values; typedef GRAPH Graph; /** Create an empty ISAM2 instance */ @@ -367,19 +367,19 @@ public: * (Params::relinearizeSkip). * @return An ISAM2Result struct containing information about the update */ - ISAM2Result update(const GRAPH& newFactors = GRAPH(), const DynamicValues& newTheta = DynamicValues(), + 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 DynamicValues& 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&). */ - DynamicValues 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 @@ -398,7 +398,7 @@ public: /** Compute an estimate using a complete delta computed by a full back-substitution. */ - DynamicValues calculateBestEstimate() const; + Values calculateBestEstimate() const; /** Access the current delta, computed during the last call to update */ const Permuted& getDelta() const { return delta_; } diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index 1ad683047..923fbda8a 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -16,9 +16,9 @@ check_PROGRAMS = #---------------------------------------------------------------------------------------------------- # Lie Groups -headers += DynamicValues.h DynamicValues-inl.h -sources += DynamicValues.cpp -check_PROGRAMS += tests/testDynamicValues tests/testKey tests/testOrdering tests/testNonlinearFactor +headers += Values-inl.h +sources += Values.cpp +check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering tests/testNonlinearFactor # Nonlinear nonlinear headers += Key.h diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index e0066f21a..82e830990 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -108,7 +108,7 @@ namespace gtsam { } /** actual error function calculation */ - virtual double error(const DynamicValues& 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_)) { @@ -135,7 +135,7 @@ namespace gtsam { } // Linearize is over-written, because base linearization tries to whiten - virtual GaussianFactor::shared_ptr linearize(const DynamicValues& 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); diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index c392c05d7..2e35b97c1 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -31,7 +31,7 @@ #include #include -#include +#include #include namespace gtsam { @@ -107,7 +107,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 DynamicValues& 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; @@ -122,11 +122,11 @@ public: * when the constraint is *NOT* fulfilled. * @return true if the constraint is active */ - virtual bool active(const DynamicValues& c) const { return true; } + virtual bool active(const Values& c) const { return true; } /** linearize to a GaussianFactor */ virtual boost::shared_ptr - linearize(const DynamicValues& 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 @@ -231,13 +231,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 DynamicValues& 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 DynamicValues& c) const { + Vector whitenedError(const Values& c) const { return noiseModel_->whiten(unwhitenedError(c)); } @@ -247,7 +247,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 DynamicValues& c) const { + virtual double error(const Values& c) const { if (this->active(c)) return 0.5 * noiseModel_->distance(unwhitenedError(c)); else @@ -259,7 +259,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 DynamicValues& 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(); @@ -343,7 +343,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 DynamicValues& 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) { @@ -427,7 +427,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 DynamicValues& 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_]; @@ -519,7 +519,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 DynamicValues& 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]); @@ -617,7 +617,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 DynamicValues& 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]); @@ -721,7 +721,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 DynamicValues& 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]); @@ -832,7 +832,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 DynamicValues& 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.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 880cb2fb9..11f8e84d9 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -28,7 +28,7 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - double NonlinearFactorGraph::probPrime(const DynamicValues& c) const { + double NonlinearFactorGraph::probPrime(const Values& c) const { return exp(-0.5 * error(c)); } @@ -38,7 +38,7 @@ namespace gtsam { } /* ************************************************************************* */ - double NonlinearFactorGraph::error(const DynamicValues& 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_) { @@ -60,7 +60,7 @@ namespace gtsam { /* ************************************************************************* */ Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD( - const DynamicValues& config) const { + const Values& config) const { // Create symbolic graph and initial (iterator) ordering SymbolicFactorGraph::shared_ptr symbolic; @@ -102,7 +102,7 @@ namespace gtsam { /* ************************************************************************* */ pair NonlinearFactorGraph::symbolic( - const DynamicValues& config) const { + const Values& config) const { // Generate an initial key ordering in iterator order Ordering::shared_ptr ordering(config.orderingArbitrary()); return make_pair(symbolic(*ordering), ordering); @@ -110,7 +110,7 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize( - const DynamicValues& config, const Ordering& ordering) const { + const Values& config, const Ordering& ordering) const { // create an empty linear FG GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 1b1bba892..fa87baedf 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -50,10 +50,10 @@ namespace gtsam { std::set keys() const; /** unnormalized error */ - double error(const DynamicValues& c) const; + double error(const Values& c) const; /** Unnormalized probability. O(n) */ - double probPrime(const DynamicValues& c) const; + double probPrime(const Values& c) const; template void add(const F& factor) { @@ -72,20 +72,20 @@ namespace gtsam { * ordering is found. */ std::pair - symbolic(const DynamicValues& 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 DynamicValues& config) const; + Ordering::shared_ptr orderingCOLAMD(const Values& config) const; /** * linearize a nonlinear factor graph */ boost::shared_ptr - linearize(const DynamicValues& config, const Ordering& ordering) const; + linearize(const Values& config, const Ordering& ordering) const; private: diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index b98a0a958..03d2832ce 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -31,7 +31,7 @@ using namespace gtsam; /* ************************************************************************* */ template void NonlinearISAM::update(const Factors& newFactors, - const DynamicValues& initialValues) { + const Values& initialValues) { if(newFactors.size() > 0) { @@ -69,7 +69,7 @@ void NonlinearISAM::reorder_relinearize() { if(factors_.size() > 0) { // Obtain the new linearization point - const DynamicValues newLinPoint = estimate(); + const Values newLinPoint = estimate(); isam_.clear(); @@ -90,7 +90,7 @@ void NonlinearISAM::reorder_relinearize() { /* ************************************************************************* */ template -DynamicValues NonlinearISAM::estimate() const { +Values NonlinearISAM::estimate() const { if(isam_.size() > 0) return linPoint_.retract(optimize(isam_), ordering_); else diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 827bb30e1..a8b34b14c 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -36,7 +36,7 @@ protected: gtsam::GaussianISAM isam_; /** The current linearization point */ - DynamicValues linPoint_; + Values linPoint_; /** The ordering */ gtsam::Ordering ordering_; @@ -60,10 +60,10 @@ public: NonlinearISAM(int reorderInterval = 1) : reorderInterval_(reorderInterval), reorderCounter_(0) {} /** Add new factors along with their initial linearization points */ - void update(const Factors& newFactors, const DynamicValues& initialValues); + void update(const Factors& newFactors, const Values& initialValues); /** Return the current solution estimate */ - DynamicValues estimate() const; + Values estimate() const; /** Relinearization and reordering of variables */ void reorder_relinearize(); @@ -83,7 +83,7 @@ public: const GaussianISAM& bayesTree() const { return isam_; } /** Return the current linearization point */ - const DynamicValues& getLinearizationPoint() const { return linPoint_; } + const Values& getLinearizationPoint() const { return linPoint_; } /** Get the ordering */ const gtsam::Ordering& getOrdering() const { return ordering_; } diff --git a/gtsam/nonlinear/NonlinearOptimization-inl.h b/gtsam/nonlinear/NonlinearOptimization-inl.h index e1896557b..450626293 100644 --- a/gtsam/nonlinear/NonlinearOptimization-inl.h +++ b/gtsam/nonlinear/NonlinearOptimization-inl.h @@ -32,7 +32,7 @@ namespace gtsam { * The Elimination solver */ template - DynamicValues optimizeSequential(const G& graph, const DynamicValues& initialEstimate, + Values optimizeSequential(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters, bool useLM) { // Use a variable ordering from COLAMD @@ -41,7 +41,7 @@ namespace gtsam { // Create an nonlinear Optimizer that uses a Sequential Solver 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. @@ -55,7 +55,7 @@ namespace gtsam { * The multifrontal solver */ template - DynamicValues optimizeMultiFrontal(const G& graph, const DynamicValues& initialEstimate, + Values optimizeMultiFrontal(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters, bool useLM) { // Use a variable ordering from COLAMD @@ -64,7 +64,7 @@ namespace gtsam { // Create an nonlinear Optimizer that uses a Multifrontal Solver 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 @@ -78,19 +78,19 @@ namespace gtsam { * The sparse preconditioned conjugate gradient solver */ template - DynamicValues optimizeSPCG(const G& graph, const DynamicValues& initialEstimate, + 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; 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)); @@ -106,7 +106,7 @@ namespace gtsam { * optimization that returns the values */ template - DynamicValues optimize(const G& graph, const DynamicValues& initialEstimate, const NonlinearOptimizationParameters& parameters, + Values optimize(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters, const LinearSolver& solver, const NonlinearOptimizationMethod& nonlinear_method) { switch (solver) { @@ -117,7 +117,7 @@ namespace gtsam { return optimizeMultiFrontal(graph, initialEstimate, parameters, nonlinear_method == LM); 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"); } diff --git a/gtsam/nonlinear/NonlinearOptimization.h b/gtsam/nonlinear/NonlinearOptimization.h index 86ab7a3c8..4a8ab15d2 100644 --- a/gtsam/nonlinear/NonlinearOptimization.h +++ b/gtsam/nonlinear/NonlinearOptimization.h @@ -43,7 +43,7 @@ namespace gtsam { * optimization that returns the values */ template - DynamicValues optimize(const G& graph, const DynamicValues& initialEstimate, + 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 2e7a83f76..213ab8daf 100644 --- a/gtsam/nonlinear/NonlinearOptimizer-inl.h +++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h @@ -86,7 +86,7 @@ namespace gtsam { if (verbosity >= Parameters::DELTA) delta.print("delta"); // take old values and update it - shared_values newValues(new DynamicValues(values_->retract(delta, *ordering_))); + shared_values newValues(new Values(values_->retract(delta, *ordering_))); // maybe show output if (verbosity >= Parameters::VALUES) newValues->print("newValues"); @@ -178,7 +178,7 @@ namespace gtsam { if (verbosity >= Parameters::TRYDELTA) delta.print("delta"); // update values - shared_values newValues(new DynamicValues(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); @@ -306,7 +306,7 @@ namespace gtsam { 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 DynamicValues(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); } diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index e87b44a5d..d57477653 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -37,19 +37,19 @@ 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 $DynamicValues$, + * 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 $DynamicValues$ 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 * DynamicValues -> L - * solve : L -> DynamicValues + * 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, $DynamicValues$ can be Pose2Values, + * For example, in a 2D case, $G$ can be Pose2Graph, $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 @@ -62,7 +62,7 @@ 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 @@ -222,7 +222,7 @@ public: /** * linearize and optimize - * This returns an VectorValues, i.e., vectors in tangent space of DynamicValues + * This returns an VectorValues, i.e., vectors in tangent space of Values */ VectorValues linearizeAndOptimizeForDelta() const { return *createSolver()->optimize(); @@ -309,18 +309,18 @@ public: * Static interface to LM optimization (no shared_ptr arguments) - see above */ static shared_values optimizeLM(const G& graph, - const DynamicValues& 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)); } static shared_values optimizeLM(const G& graph, - const DynamicValues& values, + const Values& values, Parameters::verbosityLevel verbosity) { return optimizeLM(boost::make_shared(graph), - boost::make_shared(values), + boost::make_shared(values), verbosity); } @@ -360,18 +360,18 @@ public: * Static interface to Dogleg optimization (no shared_ptr arguments) - see above */ static shared_values optimizeDogLeg(const G& graph, - const DynamicValues& 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)); } static shared_values optimizeDogLeg(const G& graph, - const DynamicValues& values, + const Values& values, Parameters::verbosityLevel verbosity) { return optimizeDogLeg(boost::make_shared(graph), - boost::make_shared(values), + boost::make_shared(values), verbosity); } @@ -398,9 +398,9 @@ public: /** * Static interface to GN optimization (no shared_ptr arguments) - see above */ - static shared_values optimizeGN(const G& graph, const DynamicValues& 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 9f1cbebae..000000000 --- a/gtsam/nonlinear/TupleValues.h +++ /dev/null @@ -1,524 +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. - */ - 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; - - /** 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) {} - - /** 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); - } - - /** - * 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 Key1& key, const Value1& value) {first_.insert(key, value);} - - /** - * Insert a complete values at a time. - * Note: if the key is already in the values, the values will not be changed. - * Use update() to allow for changing existing values. - * @param values is a full values to add - */ - template - void insert(const TupleValues& values) { second_.insert(values); } - void insert(const TupleValues& values) { - first_.insert(values.first_); - second_.insert(values.second_); - } - - /** - * Update function for whole valuess - this will change existing values - * @param values is a values to add - */ - template - void update(const TupleValues& values) { second_.update(values); } - void update(const TupleValues& values) { - first_.update(values.first_); - second_.update(values.second_); - } - - /** - * Update function for single key/value pairs - will change existing values - * @param key is the variable identifier - * @param value is the variable value to update - */ - template - void update(const KEY& key, const VALUE& value) { second_.update(key, value); } - void update(const Key1& key, const Value1& value) { first_.update(key, value); } - - /** - * Insert a subvalues - * @param values is the values to insert - */ - template - void insertSub(const CFG& values) { second_.insertSub(values); } - void insertSub(const VALUES1& values) { first_.insert(values); } - - /** erase an element by key */ - template - void erase(const KEY& j) { second_.erase(j); } - void erase(const Key1& j) { first_.erase(j); } - - /** clears the values */ - void clear() { first_.clear(); second_.clear(); } - - /** determine whether an element exists */ - template - bool exists(const KEY& j) const { return second_.exists(j); } - bool exists(const Key1& j) const { return first_.exists(j); } - - /** a variant of exists */ - template - boost::optional exists_(const KEY& j) const { return second_.exists_(j); } - boost::optional exists_(const Key1& j) const { return first_.exists_(j); } - - /** access operator */ - template - const typename KEY::Value & operator[](const KEY& j) const { return second_[j]; } - const Value1& operator[](const Key1& j) const { return first_[j]; } - - /** at access function */ - template - const typename KEY::Value & at(const KEY& j) const { return second_.at(j); } - const Value1& at(const Key1& j) const { return first_.at(j); } - - /** direct values access */ - const VALUES1& values() const { return first_; } - const VALUES2& rest() const { return second_; } - - /** zero: create VectorValues of appropriate structure */ - VectorValues zero(const Ordering& ordering) const { - return VectorValues::Zero(this->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(); } - - /** @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; - } - - /** - * 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; - } - - /** 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); - } - - /** - * 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); - } - template - void apply(A& operation) const { - first_.apply(operation); - second_.apply(operation); - } - - 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/DynamicValues-inl.h b/gtsam/nonlinear/Values-inl.h similarity index 81% rename from gtsam/nonlinear/DynamicValues-inl.h rename to gtsam/nonlinear/Values-inl.h index 7686c2293..b49049a02 100644 --- a/gtsam/nonlinear/DynamicValues-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file DynamicValues.h + * @file Values.h * @author Richard Roberts * * @brief A non-templated config holding any types of Manifold-group elements @@ -24,7 +24,7 @@ #include -#include // Only so Eclipse finds class definition +#include // Only so Eclipse finds class definition namespace gtsam { @@ -39,17 +39,17 @@ namespace gtsam { /* ************************************************************************* */ template - const ValueType& DynamicValues::at(const Symbol& j) const { + const ValueType& Values::at(const Symbol& j) const { // Find the item const_iterator item = values_.find(j); // Throw exception if it does not exist if(item == values_.end()) - throw DynamicValuesKeyDoesNotExist("retrieve", j); + throw ValuesKeyDoesNotExist("retrieve", j); // Check the type and throw exception if incorrect if(typeid(*item->second) != typeid(ValueType)) - throw DynamicValuesIncorrectType(j, 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); @@ -57,7 +57,7 @@ namespace gtsam { /* ************************************************************************* */ template - const typename TypedKey::Value& DynamicValues::at(const TypedKey& j) const { + const typename TypedKey::Value& Values::at(const TypedKey& j) const { // Convert to Symbol const Symbol symbol(j.symbol()); @@ -67,14 +67,14 @@ namespace gtsam { /* ************************************************************************* */ template - boost::optional DynamicValues::exists(const Symbol& j) const { + boost::optional Values::exists(const Symbol& j) const { // Find the item const_iterator item = values_.find(j); if(item != values_.end()) { // Check the type and throw exception if incorrect if(typeid(*item->second) != typeid(ValueType)) - throw DynamicValuesIncorrectType(j, 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); @@ -85,7 +85,7 @@ namespace gtsam { /* ************************************************************************* */ template - boost::optional DynamicValues::exists(const TypedKey& j) const { + boost::optional Values::exists(const TypedKey& j) const { // Convert to Symbol const Symbol symbol(j.symbol()); diff --git a/gtsam/nonlinear/DynamicValues.cpp b/gtsam/nonlinear/Values.cpp similarity index 80% rename from gtsam/nonlinear/DynamicValues.cpp rename to gtsam/nonlinear/Values.cpp index 57d760b59..c5190cbc8 100644 --- a/gtsam/nonlinear/DynamicValues.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file DynamicValues.h + * @file Values.h * @author Richard Roberts * * @brief A non-templated config holding any types of Manifold-group elements @@ -22,7 +22,7 @@ * which is also a manifold element, and hence supports operations dim, retract, and localCoordinates. */ -#include +#include #include @@ -35,12 +35,12 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - DynamicValues::DynamicValues(const DynamicValues& other) { + Values::Values(const Values& other) { this->insert(other); } /* ************************************************************************* */ - void DynamicValues::print(const string& str) const { + void Values::print(const string& str) const { cout << str << "Values with " << size() << " values:\n" << endl; for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) { cout << " " << (string)key_value->first << ": "; @@ -49,7 +49,7 @@ namespace gtsam { } /* ************************************************************************* */ - bool DynamicValues::equals(const DynamicValues& other, double tol) const { + 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) { @@ -64,18 +64,18 @@ namespace gtsam { } /* ************************************************************************* */ - bool DynamicValues::exists(const Symbol& j) const { + bool Values::exists(const Symbol& j) const { return values_.find(j) != values_.end(); } /* ************************************************************************* */ - VectorValues DynamicValues::zeroVectors(const Ordering& ordering) const { + VectorValues Values::zeroVectors(const Ordering& ordering) const { return VectorValues::Zero(this->dims(ordering)); } /* ************************************************************************* */ - DynamicValues DynamicValues::retract(const VectorValues& delta, const Ordering& ordering) const { - DynamicValues result; + Values Values::retract(const VectorValues& delta, const Ordering& ordering) const { + Values result; for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) { const SubVector& singleDelta = delta[ordering[key_value->first]]; // Delta for this value @@ -88,14 +88,14 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValues DynamicValues::localCoordinates(const DynamicValues& cp, const Ordering& ordering) const { + VectorValues Values::localCoordinates(const Values& cp, const Ordering& ordering) const { VectorValues result(this->dims(ordering)); localCoordinates(cp, ordering, result); return result; } /* ************************************************************************* */ - void DynamicValues::localCoordinates(const DynamicValues& cp, const Ordering& ordering, VectorValues& result) const { + 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) { @@ -107,15 +107,15 @@ namespace gtsam { } /* ************************************************************************* */ - void DynamicValues::insert(const Symbol& j, const Value& val) { + 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 DynamicValuesKeyAlreadyExists(j); + throw ValuesKeyAlreadyExists(j); } /* ************************************************************************* */ - void DynamicValues::insert(const DynamicValues& values) { + void Values::insert(const Values& values) { for(KeyValueMap::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); @@ -123,36 +123,36 @@ namespace gtsam { } /* ************************************************************************* */ - void DynamicValues::update(const Symbol& j, const Value& val) { + void Values::update(const Symbol& j, const Value& val) { // Find the value to update iterator item = values_.find(j); if(item == values_.end()) - throw DynamicValuesKeyDoesNotExist("update", j); + throw ValuesKeyDoesNotExist("update", j); // Cast to the derived type if(typeid(*item->second) != typeid(val)) - throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(val)); + throw ValuesIncorrectType(j, typeid(*item->second), typeid(val)); values_.replace(item, val.clone_()); } /* ************************************************************************* */ - void DynamicValues::update(const DynamicValues& values) { + void Values::update(const Values& values) { for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { this->update(key_value->first, *key_value->second); } } /* ************************************************************************* */ - void DynamicValues::erase(const Symbol& j) { + void Values::erase(const Symbol& j) { iterator item = values_.find(j); if(item == values_.end()) - throw DynamicValuesKeyDoesNotExist("erase", j); + throw ValuesKeyDoesNotExist("erase", j); values_.erase(item); } /* ************************************************************************* */ - FastList DynamicValues::keys() const { + FastList Values::keys() const { FastList result; for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) result.push_back(key_value->first); @@ -160,14 +160,14 @@ namespace gtsam { } /* ************************************************************************* */ - DynamicValues& DynamicValues::operator=(const DynamicValues& rhs) { + Values& Values::operator=(const Values& rhs) { this->clear(); this->insert(rhs); return *this; } /* ************************************************************************* */ - vector DynamicValues::dims(const Ordering& ordering) const { + vector Values::dims(const Ordering& ordering) const { // vector result(values_.size()); // // Transform with Value::dim(auto_ptr::get(KeyValuePair::second)) // result.assign( @@ -182,7 +182,7 @@ namespace gtsam { } /* ************************************************************************* */ - Ordering::shared_ptr DynamicValues::orderingArbitrary(Index firstVar) const { + Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const { Ordering::shared_ptr ordering(new Ordering); for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) { ordering->insert(key_value->first, firstVar++); @@ -191,7 +191,7 @@ namespace gtsam { } /* ************************************************************************* */ - const char* DynamicValuesKeyAlreadyExists::what() const throw() { + 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."; @@ -199,7 +199,7 @@ namespace gtsam { } /* ************************************************************************* */ - const char* DynamicValuesKeyDoesNotExist::what() const throw() { + const char* ValuesKeyDoesNotExist::what() const throw() { if(message_.empty()) message_ = "Attempting to " + std::string(operation_) + " the key \"" + @@ -208,10 +208,10 @@ namespace gtsam { } /* ************************************************************************* */ - const char* DynamicValuesIncorrectType::what() const throw() { + const char* ValuesIncorrectType::what() const throw() { if(message_.empty()) message_ = - "Attempting to retrieve value with key \"" + (std::string)key_ + "\", type stored in DynamicValues is " + + "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/DynamicValues.h b/gtsam/nonlinear/Values.h similarity index 88% rename from gtsam/nonlinear/DynamicValues.h rename to gtsam/nonlinear/Values.h index 4f866749e..27da4d7e4 100644 --- a/gtsam/nonlinear/DynamicValues.h +++ b/gtsam/nonlinear/Values.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file DynamicValues.h + * @file Values.h * @author Richard Roberts * * @brief A non-templated config holding any types of Manifold-group elements @@ -63,7 +63,7 @@ namespace gtsam { * manifold element, and hence supports operations dim, retract, and * localCoordinates. */ - class DynamicValues { + class Values { private: @@ -91,11 +91,11 @@ namespace gtsam { typedef KeyValueMap::reverse_iterator reverse_iterator; typedef KeyValueMap::const_reverse_iterator const_reverse_iterator; - /** Default constructor creates an empty DynamicValues class */ - DynamicValues() {} + /** Default constructor creates an empty Values class */ + Values() {} /** Copy constructor duplicates all keys and values */ - DynamicValues(const DynamicValues& other); + Values(const Values& other); /// @name Testable /// @{ @@ -104,7 +104,7 @@ namespace gtsam { void print(const std::string& str = "") const; /** Test whether the sets of keys and values are identical */ - bool equals(const DynamicValues& other, double tol=1e-9) const; + bool equals(const Values& other, double tol=1e-9) const; /// @} @@ -178,13 +178,13 @@ namespace gtsam { /// @{ /** Add a delta config to current config and returns a new config */ - DynamicValues retract(const VectorValues& delta, const Ordering& ordering) const; + Values retract(const VectorValues& delta, const Ordering& ordering) const; /** Get a delta config about a linearization point c0 (*this) */ - VectorValues localCoordinates(const DynamicValues& cp, const Ordering& ordering) const; + VectorValues localCoordinates(const Values& cp, const Ordering& ordering) const; /** Get a delta config about a linearization point c0 (*this) */ - void localCoordinates(const DynamicValues& cp, const Ordering& ordering, VectorValues& delta) const; + void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const; ///@} @@ -194,13 +194,13 @@ namespace gtsam { void insert(const Symbol& j, const Value& val); /** Add a set of variables, throws KeyAlreadyExists if a key is already present */ - void insert(const DynamicValues& values); + 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 DynamicValues& values); + void update(const Values& values); /** Remove a variable from the config, throws KeyDoesNotExist if j is not present */ void erase(const Symbol& j); @@ -218,7 +218,7 @@ namespace gtsam { FastList keys() const; /** Replace all keys and variables */ - DynamicValues& operator=(const DynamicValues& rhs); + Values& operator=(const Values& rhs); /** Remove all variables from the config */ void clear() { values_.clear(); } @@ -254,7 +254,7 @@ namespace gtsam { }; /* ************************************************************************* */ - class DynamicValuesKeyAlreadyExists : public std::exception { + class ValuesKeyAlreadyExists : public std::exception { protected: const Symbol key_; ///< The key that already existed @@ -263,10 +263,10 @@ namespace gtsam { public: /// Construct with the key-value pair attemped to be added - DynamicValuesKeyAlreadyExists(const Symbol& key) throw() : + ValuesKeyAlreadyExists(const Symbol& key) throw() : key_(key) {} - virtual ~DynamicValuesKeyAlreadyExists() throw() {} + virtual ~ValuesKeyAlreadyExists() throw() {} /// The duplicate key that was attemped to be added const Symbol& key() const throw() { return key_; } @@ -276,7 +276,7 @@ namespace gtsam { }; /* ************************************************************************* */ - class DynamicValuesKeyDoesNotExist : public std::exception { + 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 @@ -286,10 +286,10 @@ namespace gtsam { public: /// Construct with the key that does not exist in the values - DynamicValuesKeyDoesNotExist(const char* operation, const Symbol& key) throw() : + ValuesKeyDoesNotExist(const char* operation, const Symbol& key) throw() : operation_(operation), key_(key) {} - virtual ~DynamicValuesKeyDoesNotExist() throw() {} + virtual ~ValuesKeyDoesNotExist() throw() {} /// The key that was attempted to be accessed that does not exist const Symbol& key() const throw() { return key_; } @@ -299,7 +299,7 @@ namespace gtsam { }; /* ************************************************************************* */ - class DynamicValuesIncorrectType : public std::exception { + class ValuesIncorrectType : public std::exception { protected: const Symbol key_; ///< The key requested const std::type_info& storedTypeId_; @@ -310,16 +310,16 @@ namespace gtsam { public: /// Construct with the key that does not exist in the values - DynamicValuesIncorrectType(const Symbol& key, + ValuesIncorrectType(const Symbol& key, const std::type_info& storedTypeId, const std::type_info& requestedTypeId) throw() : key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {} - virtual ~DynamicValuesIncorrectType() throw() {} + 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 DynamicValues + /// The typeid of the value stores in the Values const std::type_info& storedTypeId() const { return storedTypeId_; } /// The requested typeid @@ -338,10 +338,10 @@ namespace gtsam { virtual ~DynamicValuesMismatched() throw() {} virtual const char* what() const throw() { - return "The Values 'this' and the argument passed to DynamicValues::localCoordinates have mismatched keys and values"; + return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values"; } }; } -#include +#include diff --git a/gtsam/nonlinear/ValuesOld-inl.h b/gtsam/nonlinear/ValuesOld-inl.h deleted file mode 100644 index 5f53e32e8..000000000 --- a/gtsam/nonlinear/ValuesOld-inl.h +++ /dev/null @@ -1,231 +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 ValuesOld.cpp - * @author Richard Roberts - */ - -#pragma once - -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ - template - void ValuesOld::print(const string &s) const { - cout << "ValuesOld " << s << ", size " << values_.size() << "\n"; - BOOST_FOREACH(const KeyValuePair& v, values_) { - gtsam::print(v.second, (string)(v.first)); - } - } - - /* ************************************************************************* */ - template - bool ValuesOld::equals(const ValuesOld& 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 typename J::Value& ValuesOld::at(const J& j) const { - const_iterator it = values_.find(j); - if (it == values_.end()) throw KeyDoesNotExist("retrieve", j); - else return it->second; - } - - /* ************************************************************************* */ - template - size_t ValuesOld::dim() const { - size_t n = 0; - BOOST_FOREACH(const KeyValuePair& value, values_) - n += value.second.dim(); - return n; - } - - /* ************************************************************************* */ - template - VectorValues ValuesOld::zero(const Ordering& ordering) const { - return VectorValues::Zero(this->dims(ordering)); - } - - /* ************************************************************************* */ - template - void ValuesOld::insert(const J& name, const typename J::Value& val) { - if(!values_.insert(make_pair(name, val)).second) - throw KeyAlreadyExists(name, val); - } - - /* ************************************************************************* */ - template - void ValuesOld::insert(const ValuesOld& cfg) { - BOOST_FOREACH(const KeyValuePair& v, cfg.values_) - insert(v.first, v.second); - } - - /* ************************************************************************* */ - template - void ValuesOld::update(const ValuesOld& cfg) { - BOOST_FOREACH(const KeyValuePair& v, values_) { - boost::optional t = cfg.exists_(v.first); - if (t) values_[v.first] = *t; - } - } - - /* ************************************************************************* */ - template - void ValuesOld::update(const J& j, const typename J::Value& val) { - values_[j] = val; - } - - /* ************************************************************************* */ - template - std::list ValuesOld::keys() const { - std::list ret; - BOOST_FOREACH(const KeyValuePair& v, values_) - ret.push_back(v.first); - return ret; - } - - /* ************************************************************************* */ - template - void ValuesOld::erase(const J& j) { - size_t dim; // unused - erase(j, dim); - } - - /* ************************************************************************* */ - template - void ValuesOld::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 - ValuesOld ValuesOld::retract(const VectorValues& delta, const Ordering& ordering) const { - ValuesOld 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 ValuesOld::dims(const Ordering& ordering) const { - _ValuesDimensionCollector dimCollector(ordering); - this->apply(dimCollector); - return dimCollector.dimensions; - } - - /* ************************************************************************* */ - template - Ordering::shared_ptr ValuesOld::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 -// ValuesOld ValuesOld::retract(const Vector& delta) const { -// if(delta.size() != dim()) { -// cout << "ValuesOld::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl; -// throw invalid_argument("Delta vector length does not match config dimensionality."); -// } -// ValuesOld 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 ValuesOld::localCoordinates(const ValuesOld& cp, const Ordering& ordering) const { - VectorValues delta(this->dims(ordering)); - localCoordinates(cp, ordering, delta); - return delta; - } - - /* ************************************************************************* */ - template - void ValuesOld::localCoordinates(const ValuesOld& 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); - } - } - - /* ************************************************************************* */ - 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 ValuesOld."; - return message_.c_str(); - } - - /* ************************************************************************* */ - 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(); - } - -} - - diff --git a/gtsam/nonlinear/ValuesOld.h b/gtsam/nonlinear/ValuesOld.h deleted file mode 100644 index 88595a5dd..000000000 --- a/gtsam/nonlinear/ValuesOld.h +++ /dev/null @@ -1,301 +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 ValuesOld.h - * @author Richard Roberts - * - * @brief A templated config for 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 ValuesOld 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 - -namespace boost { template class optional; } -namespace gtsam { class VectorValues; class Ordering; } - -namespace gtsam { - - // Forward declarations - template class KeyDoesNotExist; - template class KeyAlreadyExists; - - /** - * 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 - class ValuesOld { - - 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) - - KeyValueMap values_; - - public: - - ValuesOld() {} - ValuesOld(const ValuesOld& config) : - values_(config.values_) {} - template - ValuesOld(const ValuesOld& other) {} // do nothing when initializing with wrong type - virtual ~ValuesOld() {} - - /// @name Testable - /// @{ - - /** print */ - void print(const std::string &s="") const; - - /** Test whether configs are identical in keys and values */ - bool equals(const ValuesOld& expected, double tol=1e-9) const; - - /// @} - - /** Retrieve a variable by j, throws KeyDoesNotExist if not found */ - const Value& at(const J& j) const; - - /** operator[] syntax for get, throws KeyDoesNotExist if not found */ - const Value& operator[](const J& 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 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; - } - - /** The number of variables in this config */ - size_t size() const { return values_.size(); } - - /** whether the config is empty */ - bool empty() const { return values_.empty(); } - - /** Get a zero VectorValues of the correct structure */ - VectorValues zero(const Ordering& ordering) const; - - const_iterator begin() const { return values_.begin(); } - const_iterator end() const { return values_.end(); } - iterator begin() { return values_.begin(); } - iterator end() { return values_.end(); } - - /// @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 */ - ValuesOld retract(const VectorValues& delta, const Ordering& ordering) const; - - /** Get a delta config about a linearization point c0 (*this) */ - VectorValues localCoordinates(const ValuesOld& cp, const Ordering& ordering) const; - - /** Get a delta config about a linearization point c0 (*this) */ - void localCoordinates(const ValuesOld& cp, const Ordering& ordering, VectorValues& delta) const; - - /// @} - - // imperative methods: - - /** Add a variable with the given j, throws KeyAlreadyExists 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 ValuesOld& cfg); - - /** update the current available values without adding new ones */ - void update(const ValuesOld& 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 */ - ValuesOld& operator=(const ValuesOld& 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 ValuesOld, (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); - } - - /** Create an array of variable dimensions using the given ordering */ - std::vector dims(const Ordering& ordering) const; - - /** - * 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; - - 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; - } - }; - - -/* ************************************************************************* */ -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/testDynamicValues.cpp b/gtsam/nonlinear/tests/testDynamicValues.cpp deleted file mode 100644 index 51ffd0320..000000000 --- a/gtsam/nonlinear/tests/testDynamicValues.cpp +++ /dev/null @@ -1,265 +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 testDynamicValues.cpp - * @author Richard Roberts - */ - -#include -#include -#include -#include // for operator += -using namespace boost::assign; - -#include -#include -#include -#include - -using namespace gtsam; -using namespace std; -static double inf = std::numeric_limits::infinity(); - -typedef TypedSymbol VecKey; - -VecKey key1(1), key2(2), key3(3), key4(4); - -/* ************************************************************************* */ -TEST( DynamicValues, equals1 ) -{ - DynamicValues expected; - LieVector v(3, 5.0, 6.0, 7.0); - expected.insert(key1,v); - DynamicValues actual; - actual.insert(key1,v); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( DynamicValues, equals2 ) -{ - DynamicValues 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)); - CHECK(!cfg2.equals(cfg1)); -} - -/* ************************************************************************* */ -TEST( DynamicValues, equals_nan ) -{ - DynamicValues 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)); - CHECK(!cfg2.equals(cfg1)); -} - -/* ************************************************************************* */ -TEST( DynamicValues, insert_good ) -{ - DynamicValues 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); - - cfg1.insert(cfg2); - - expected.insert(key1, v1); - expected.insert(key2, v2); - expected.insert(key3, v4); - - CHECK(assert_equal(expected, cfg1)); -} - -/* ************************************************************************* */ -TEST( DynamicValues, insert_bad ) -{ - DynamicValues 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), DynamicValuesKeyAlreadyExists); -} - -/* ************************************************************************* */ -TEST( DynamicValues, update_element ) -{ - DynamicValues 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); - CHECK(assert_equal(v1, cfg.at(key1))); - - cfg.update(key1, v2); - CHECK(cfg.size() == 1); - CHECK(assert_equal(v2, cfg.at(key1))); -} - -///* ************************************************************************* */ -//TEST(DynamicValues, dim_zero) -//{ -// DynamicValues 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)); -// expected.insert(key2, zero(3)); -// CHECK(assert_equal(expected, config0.zero())); -//} - -/* ************************************************************************* */ -TEST(DynamicValues, expmap_a) -{ - DynamicValues 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); - - DynamicValues 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(DynamicValues, expmap_b) -{ - DynamicValues 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(VectorValues::Zero(config0.dims(ordering))); - increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5); - - DynamicValues 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(DynamicValues, expmap_c) -//{ -// DynamicValues 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 = LieVector(6, -// 1.0, 1.1, 1.2, -// 1.3, 1.4, 1.5); -// -// DynamicValues 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(DynamicValues, expmap_d) -{ - DynamicValues 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)); - - typedef TypedSymbol PoseKey; - DynamicValues 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(DynamicValues, extract_keys) -{ - typedef TypedSymbol PoseKey; - DynamicValues config; - - config.insert(PoseKey(1), Pose2()); - config.insert(PoseKey(2), Pose2()); - config.insert(PoseKey(4), Pose2()); - config.insert(PoseKey(5), Pose2()); - - FastList expected, actual; - expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5); - actual = config.keys(); - - CHECK(actual.size() == expected.size()); - FastList::const_iterator itAct = actual.begin(), itExp = expected.begin(); - for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) { - CHECK(assert_equal(*itExp, *itAct)); - } -} - -/* ************************************************************************* */ -TEST(DynamicValues, exists_) -{ - DynamicValues config0; - config0.insert(key1, LieVector(Vector_(1, 1.))); - config0.insert(key2, LieVector(Vector_(1, 2.))); - - boost::optional v = config0.exists(key1); - CHECK(assert_equal(Vector_(1, 1.),*v)); -} - -/* ************************************************************************* */ -TEST(DynamicValues, update) -{ - DynamicValues config0; - config0.insert(key1, LieVector(1, 1.)); - config0.insert(key2, LieVector(1, 2.)); - - DynamicValues superset; - superset.insert(key1, LieVector(1, -1.)); - superset.insert(key2, LieVector(1, -2.)); - config0.update(superset); - - DynamicValues expected; - expected.insert(key1, LieVector(1, -1.)); - expected.insert(key2, LieVector(1, -2.)); - CHECK(assert_equal(expected,config0)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testNonlinearFactor.cpp b/gtsam/nonlinear/tests/testNonlinearFactor.cpp index 558c40036..6a210d6ed 100644 --- a/gtsam/nonlinear/tests/testNonlinearFactor.cpp +++ b/gtsam/nonlinear/tests/testNonlinearFactor.cpp @@ -76,7 +76,7 @@ public: /* ************************************ */ TEST(NonlinearFactor, NonlinearFactor4) { TestFactor4 tf; - DynamicValues tv; + 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)); @@ -124,7 +124,7 @@ public: /* ************************************ */ TEST(NonlinearFactor, NonlinearFactor5) { TestFactor5 tf; - DynamicValues tv; + 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)); @@ -177,7 +177,7 @@ public: /* ************************************ */ TEST(NonlinearFactor, NonlinearFactor6) { TestFactor6 tf; - DynamicValues tv; + 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)); 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/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 274c0d83d..f3df76774 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -57,7 +57,7 @@ struct BoundingConstraint1: public NonlinearFactor1 { boost::none) const = 0; /** active when constraint *NOT* met */ - bool active(const DynamicValues& 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_; @@ -125,7 +125,7 @@ struct BoundingConstraint2: public NonlinearFactor2 { boost::optional H2 = boost::none) const = 0; /** active when constraint *NOT* met */ - bool active(const DynamicValues& 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/dataset.cpp b/gtsam/slam/dataset.cpp index d94749b7f..1428d9d8b 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -30,7 +30,7 @@ using namespace gtsam; #define LINESIZE 81920 typedef boost::shared_ptr sharedPose2Graph; -typedef boost::shared_ptr sharedPose2Values; +typedef boost::shared_ptr sharedPose2Values; namespace gtsam { @@ -81,7 +81,7 @@ pair load2D(const string& filename, exit(-1); } - sharedPose2Values poses(new DynamicValues); + sharedPose2Values poses(new Values); sharedPose2Graph graph(new Pose2Graph); string tag; @@ -149,14 +149,14 @@ pair load2D(const string& filename, } /* ************************************************************************* */ -void save2D(const Pose2Graph& graph, const DynamicValues& config, const SharedDiagonal model, +void save2D(const Pose2Graph& graph, const Values& config, const SharedDiagonal model, const string& filename) { -// typedef DynamicValues::Key Key; +// typedef Values::Key Key; fstream stream(filename.c_str(), fstream::out); // save poses - for (DynamicValues::const_iterator it = config.begin(); it != config.end(); ++it) { + for (Values::const_iterator it = config.begin(); it != config.end(); ++it) { Pose2 pose = config.at(it->first); stream << "VERTEX2 " << it->first.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 2b339db18..7dafc58b4 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -41,16 +41,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 gtsam::Pose2Graph& graph, const DynamicValues& config, const gtsam::SharedDiagonal model, +void save2D(const gtsam::Pose2Graph& graph, const Values& config, const gtsam::SharedDiagonal model, const std::string& filename); /** diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 15f9ff61f..1ba91c4af 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -40,37 +40,6 @@ namespace gtsam { /// Typedef for a PointKey with Point2 data and 'l' symbol typedef TypedSymbol PointKey; - /// Values class, inherited from TupleValues2, using PoseKeys and PointKeys - struct Values: public DynamicValues { - - /// Default constructor - Values() {} - - /// Copy constructor - Values(const DynamicValues& values) : - DynamicValues(values) { - } - - /// From sub-values -// Values(const DynamicValues& poses, const DynamicValues& points) : -// DynamicValues(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 */ @@ -120,7 +89,7 @@ namespace gtsam { const Rot2& bearing, double range, const SharedNoiseModel& model); /// Optimize - Values optimize(const DynamicValues& initialEstimate) { + Values optimize(const Values& initialEstimate) { typedef NonlinearOptimizer Optimizer; return *Optimizer::optimizeLM(*this, initialEstimate, NonlinearOptimizationParameters::LAMBDA); diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index 090607d72..7fc1e8516 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -27,8 +27,8 @@ namespace gtsam { namespace pose2SLAM { /* ************************************************************************* */ - DynamicValues circle(size_t n, double R) { - DynamicValues x; + Values circle(size_t n, double R) { + Values x; double theta = 0, dtheta = 2 * M_PI / n; for (size_t i = 0; i < n; i++, theta += dtheta) x.insert(Key(i), Pose2(cos(theta), sin(theta), M_PI_2 + theta)); diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index cc688c597..dfa4e7fa3 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -43,7 +43,7 @@ namespace gtsam { * @param c character to use for keys * @return circle of n 2D poses */ - DynamicValues circle(size_t n, double R); + Values circle(size_t n, double R); /// A prior factor on Key with Pose2 data type. typedef PriorFactor Prior; diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index 29eb14ba9..4d063c12b 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -25,8 +25,8 @@ namespace gtsam { namespace pose3SLAM { /* ************************************************************************* */ - DynamicValues circle(size_t n, double radius) { - DynamicValues x; + Values circle(size_t n, double radius) { + Values x; double theta = 0, dtheta = 2 * M_PI / n; // We use aerospace/navlab convention, X forward, Y right, Z down // First pose will be at (R,0,0) diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h index 544dcb5f0..686f6bd0f 100644 --- a/gtsam/slam/pose3SLAM.h +++ b/gtsam/slam/pose3SLAM.h @@ -39,7 +39,7 @@ namespace gtsam { * @param R radius of circle * @return circle of n 3D poses */ - DynamicValues circle(size_t n, double R); + Values circle(size_t n, double R); /// A prior factor on Key with Pose3 data type. typedef PriorFactor Prior; diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index 291bde25e..c0cab4078 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -32,58 +32,6 @@ namespace gtsam { typedef TypedSymbol PoseKey; typedef TypedSymbol PointKey; - /** - * Custom Values class that holds poses and points - */ - class Values: public DynamicValues { - size_t nrPoses_; - size_t nrPoints_; - public: - typedef DynamicValues Base; ///< base class - typedef boost::shared_ptr sharedPoint; ///< shortcut to shared Point type - - /// Constructor - Values() : nrPoses_(0), nrPoints_(0) { - } - - /// Copy constructor - Values(const Base& base) : - Base(base), nrPoses_(0), nrPoints_(0) { - } - - /// Insert a pose - void insertPose(const simulated2D::PoseKey& i, const Point2& p) { - insert(i, p); - nrPoses_++; - } - - /// Insert a point - void insertPoint(const simulated2D::PointKey& j, const Point2& p) { - insert(j, p); - nrPoints_++; - } - - /// Number of poses - int nrPoses() const { - return nrPoses_; - } - - /// Number of points - int nrPoints() const { - return nrPoints_; - } - - /// 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) { return x; diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index d54924c12..28770d6d9 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -32,30 +32,6 @@ namespace gtsam { typedef TypedSymbol PointKey; typedef TypedSymbol PoseKey; - /// Specialized Values structure with syntactic sugar for - /// compatibility with matlab - class Values: public DynamicValues { - int nrPoses_, nrPoints_; - public: - Values() : nrPoses_(0), nrPoints_(0) {} - - void insertPose(const PoseKey& i, const Pose2& p) { - insert(i, p); - nrPoses_++; - } - - void insertPoint(const PointKey& j, const Point2& p) { - insert(j, p); - nrPoints_++; - } - - int nrPoses() const { return nrPoses_; } - int nrPoints() const { return nrPoints_; } - - 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 /// Prior on a single pose diff --git a/gtsam/slam/smallExample.h b/gtsam/slam/smallExample.h index 2380d86f2..96dcbaf09 100644 --- a/gtsam/slam/smallExample.h +++ b/gtsam/slam/smallExample.h @@ -28,7 +28,6 @@ namespace gtsam { namespace example { - typedef simulated2D::Values Values; typedef NonlinearFactorGraph Graph; /** diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index a62122705..8291b17ca 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -97,7 +97,7 @@ TEST( GeneralSFMFactor, error ) { boost::shared_ptr factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); // For the following configuration, the factor predicts 320,240 - DynamicValues values; + Values values; Rot3 R; Point3 t1(0,0,-6); Pose3 x1(R,t1); @@ -170,7 +170,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new DynamicValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert(CameraKey((int)i), X[i]) ; @@ -209,7 +209,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new DynamicValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert(CameraKey((int)i), X[i]) ; @@ -255,7 +255,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new DynamicValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert(CameraKey((int)i), X[i]) ; @@ -298,7 +298,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new DynamicValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) { const double rot_noise = 1e-5, @@ -359,7 +359,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new DynamicValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert(CameraKey((int)i), X[i]) ; diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 89b5e5f34..65126d3d0 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -97,7 +97,7 @@ TEST( GeneralSFMFactor, error ) { boost::shared_ptr factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); // For the following configuration, the factor predicts 320,240 - DynamicValues values; + Values values; Rot3 R; Point3 t1(0,0,-6); Pose3 x1(R,t1); @@ -170,7 +170,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new DynamicValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert(CameraKey((int)i), X[i]) ; @@ -209,7 +209,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new DynamicValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert(CameraKey((int)i), X[i]) ; @@ -255,7 +255,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new DynamicValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert(CameraKey((int)i), X[i]) ; @@ -298,7 +298,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new DynamicValues); + 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, @@ -355,7 +355,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new DynamicValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) values->insert(CameraKey((int)i), X[i]) ; diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp index 3241a1e99..a74f79d49 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -51,7 +51,7 @@ TEST( planarSLAM, BearingFactor ) planarSLAM::Bearing factor(2, 3, z, sigma); // create config - DynamicValues c; + Values c; c.insert(PoseKey(2), x2); c.insert(PointKey(3), l3); @@ -79,7 +79,7 @@ TEST( planarSLAM, RangeFactor ) planarSLAM::Range factor(2, 3, z, sigma); // create config - DynamicValues c; + Values c; c.insert(PoseKey(2), x2); c.insert(PointKey(3), l3); @@ -106,7 +106,7 @@ TEST( planarSLAM, BearingRangeFactor ) planarSLAM::BearingRange factor(2, 3, r, b, sigma2); // create config - DynamicValues c; + Values c; c.insert(PoseKey(2), x2); c.insert(PointKey(3), l3); @@ -139,7 +139,7 @@ TEST( planarSLAM, PoseConstraint_equals ) TEST( planarSLAM, constructor ) { // create config - DynamicValues c; + Values c; c.insert(PoseKey(2), x2); c.insert(PoseKey(3), x3); c.insert(PointKey(3), l3); diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index c17c20348..84878e869 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -116,7 +116,7 @@ TEST( Pose2SLAM, linearization ) // Choose a linearization point Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2) - DynamicValues config; + Values config; config.insert(Key(1),p1); config.insert(Key(2),p2); // Linearize @@ -152,7 +152,7 @@ TEST(Pose2Graph, optimize) { fg->addConstraint(0, 1, Pose2(1,2,M_PI_2), covariance); // Create initial config - boost::shared_ptr initial(new DynamicValues()); + boost::shared_ptr initial(new Values()); initial->insert(Key(0), Pose2(0,0,0)); initial->insert(Key(1), Pose2(0,0,0)); @@ -166,7 +166,7 @@ TEST(Pose2Graph, optimize) { Optimizer optimizer = optimizer0.levenbergMarquardt(); // Check with expected config - DynamicValues expected; + Values expected; expected.insert(Key(0), Pose2(0,0,0)); expected.insert(Key(1), Pose2(1,2,M_PI_2)); CHECK(assert_equal(expected, *optimizer.values())); @@ -177,7 +177,7 @@ TEST(Pose2Graph, optimize) { TEST(Pose2Graph, optimizeThreePoses) { // Create a hexagon of poses - DynamicValues hexagon = pose2SLAM::circle(3,1.0); + Values hexagon = pose2SLAM::circle(3,1.0); Pose2 p0 = hexagon[Key(0)], p1 = hexagon[Key(1)]; // create a Pose graph with one equality constraint and one measurement @@ -189,7 +189,7 @@ TEST(Pose2Graph, optimizeThreePoses) { fg->addConstraint(2, 0, delta, covariance); // Create initial config - boost::shared_ptr initial(new DynamicValues()); + boost::shared_ptr initial(new Values()); initial->insert(Key(0), p0); initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(3,-0.1, 0.1,-0.1))); initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(3, 0.1,-0.1, 0.1))); @@ -203,7 +203,7 @@ TEST(Pose2Graph, optimizeThreePoses) { pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params); pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); - DynamicValues actual = *optimizer.values(); + Values actual = *optimizer.values(); // Check with ground truth CHECK(assert_equal(hexagon, actual)); @@ -214,7 +214,7 @@ TEST(Pose2Graph, optimizeThreePoses) { TEST_UNSAFE(Pose2Graph, optimizeCircle) { // Create a hexagon of poses - DynamicValues hexagon = pose2SLAM::circle(6,1.0); + Values hexagon = pose2SLAM::circle(6,1.0); Pose2 p0 = hexagon[Key(0)], p1 = hexagon[Key(1)]; // create a Pose graph with one equality constraint and one measurement @@ -229,7 +229,7 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) { fg->addConstraint(5, 0, delta, covariance); // Create initial config - boost::shared_ptr initial(new DynamicValues()); + boost::shared_ptr initial(new Values()); initial->insert(Key(0), p0); initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(3,-0.1, 0.1,-0.1))); initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(3, 0.1,-0.1, 0.1))); @@ -246,7 +246,7 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) { pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params); pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); - DynamicValues actual = *optimizer.values(); + Values actual = *optimizer.values(); // Check with ground truth CHECK(assert_equal(hexagon, actual)); @@ -280,7 +280,7 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) { // // myOptimizer.update(x); // -// DynamicValues expected; +// Values expected; // expected.insert(0, Pose2(0.,0.,0.)); // expected.insert(1, Pose2(1.,0.,0.)); // expected.insert(2, Pose2(2.,0.,0.)); @@ -341,38 +341,38 @@ TEST(Pose2Graph, optimize2) { using namespace pose2SLAM; /* ************************************************************************* */ -TEST( DynamicValues, pose2Circle ) +TEST( Values, pose2Circle ) { // expected is 4 poses tangent to circle with radius 1m - DynamicValues expected; + Values expected; expected.insert(Key(0), Pose2( 1, 0, M_PI_2)); expected.insert(Key(1), Pose2( 0, 1, - M_PI )); expected.insert(Key(2), Pose2(-1, 0, - M_PI_2)); expected.insert(Key(3), Pose2( 0, -1, 0 )); - DynamicValues actual = pose2SLAM::circle(4,1.0); + Values actual = pose2SLAM::circle(4,1.0); CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( DynamicValues, expmap ) +TEST( Values, expmap ) { // expected is circle shifted to right - DynamicValues expected; + Values expected; expected.insert(Key(0), Pose2( 1.1, 0, M_PI_2)); expected.insert(Key(1), Pose2( 0.1, 1, - M_PI )); expected.insert(Key(2), Pose2(-0.9, 0, - M_PI_2)); expected.insert(Key(3), Pose2( 0.1, -1, 0 )); // Note expmap coordinates are in local coordinates, so shifting to right requires thought !!! - DynamicValues circle(pose2SLAM::circle(4,1.0)); + Values circle(pose2SLAM::circle(4,1.0)); Ordering ordering(*circle.orderingArbitrary()); VectorValues delta(circle.dims(ordering)); delta[ordering[Key(0)]] = Vector_(3, 0.0,-0.1,0.0); delta[ordering[Key(1)]] = Vector_(3, -0.1,0.0,0.0); delta[ordering[Key(2)]] = Vector_(3, 0.0,0.1,0.0); delta[ordering[Key(3)]] = Vector_(3, 0.1,0.0,0.0); - DynamicValues actual = circle.retract(delta, ordering); + Values actual = circle.retract(delta, ordering); CHECK(assert_equal(expected,actual)); } @@ -385,7 +385,7 @@ TEST( Pose2Prior, error ) { // Choose a linearization point Pose2 p1(1, 0, 0); // robot at (1,0) - DynamicValues x0; + Values x0; x0.insert(Key(1), p1); // Create factor @@ -407,7 +407,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; - DynamicValues 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))); @@ -429,7 +429,7 @@ LieVector hprior(const Pose2& p1) { TEST( Pose2Prior, linearize ) { // Choose a linearization point at ground truth - DynamicValues x0; + Values x0; x0.insert(Key(1),priorVal); // Actual linearization @@ -449,7 +449,7 @@ TEST( Pose2Factor, error ) // Choose a linearization point Pose2 p1; // robot at origin Pose2 p2(1, 0, 0); // robot at (1,0) - DynamicValues x0; + Values x0; x0.insert(Key(1), p1); x0.insert(Key(2), p2); @@ -473,7 +473,7 @@ TEST( Pose2Factor, error ) // Check error after increasing p2 VectorValues plus = delta; plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0); - DynamicValues 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))); @@ -490,7 +490,7 @@ 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) - DynamicValues x0; + Values x0; x0.insert(Key(1),p1); x0.insert(Key(2),p2); @@ -520,7 +520,7 @@ TEST( Pose2Factor, linearize ) // Choose a linearization point at ground truth Pose2 p1(1,2,M_PI_2); Pose2 p2(-1,4,M_PI); - DynamicValues x0; + Values x0; x0.insert(Key(1),p1); x0.insert(Key(2),p2); diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 574d05b6d..94a3bd72f 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -49,7 +49,7 @@ TEST(Pose3Graph, optimizeCircle) { // Create a hexagon of poses double radius = 10; - DynamicValues hexagon = pose3SLAM::circle(6,radius); + 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 @@ -66,7 +66,7 @@ TEST(Pose3Graph, optimizeCircle) { fg->addConstraint(5,0, _0T1, covariance); // Create initial config - boost::shared_ptr initial(new DynamicValues()); + 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))); @@ -81,7 +81,7 @@ TEST(Pose3Graph, optimizeCircle) { pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params); pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); - DynamicValues actual = *optimizer.values(); + Values actual = *optimizer.values(); // Check with ground truth CHECK(assert_equal(hexagon, actual,1e-4)); @@ -110,13 +110,13 @@ TEST(Pose3Graph, partial_prior_height) { pose3SLAM::Graph graph; graph.add(height); - DynamicValues values; + Values values; values.insert(key, init); // linearization EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); - DynamicValues 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); } @@ -134,7 +134,7 @@ TEST( Pose3Factor, error ) Pose3Factor factor(1,2, z, I6); // Create config - DynamicValues x; + Values x; x.insert(Key(1),t1); x.insert(Key(2),t2); @@ -165,10 +165,10 @@ TEST(Pose3Graph, partial_prior_xy) { pose3SLAM::Graph graph; graph.add(priorXY); - DynamicValues values; + Values values; values.insert(key, init); - DynamicValues 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); } @@ -181,23 +181,23 @@ 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( DynamicValues, pose3Circle ) +TEST( Values, pose3Circle ) { // expected is 4 poses tangent to circle with radius 1m - DynamicValues expected; + 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))); - DynamicValues actual = pose3SLAM::circle(4,1.0); + Values actual = pose3SLAM::circle(4,1.0); CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( DynamicValues, expmap ) +TEST( Values, expmap ) { - DynamicValues expected; + 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))); @@ -210,7 +210,7 @@ TEST( DynamicValues, 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); - DynamicValues 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 f5f41fd2e..1d8616c30 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -52,7 +52,7 @@ TEST( ProjectionFactor, error ) factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); // For the following values structure, the factor predicts 320,240 - DynamicValues config; + 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.) @@ -81,13 +81,13 @@ TEST( ProjectionFactor, error ) CHECK(assert_equal(expected_lfg,*actual_lfg)); // expmap on a config - DynamicValues expected_config; + 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.); - DynamicValues 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 c9b2bfd61..50b302bd2 100644 --- a/gtsam/slam/tests/testSimulated2D.cpp +++ b/gtsam/slam/tests/testSimulated2D.cpp @@ -25,13 +25,14 @@ using namespace std; using namespace gtsam; +using namespace gtsam::simulated2D; /* ************************************************************************* */ TEST( simulated2D, Simulated2DValues ) { - simulated2D::Values actual; - actual.insertPose(1,Point2(1,1)); - actual.insertPoint(2,Point2(2,2)); + Values actual; + actual.insert(PoseKey(1),Point2(1,1)); + actual.insert(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 57951f6e1..c482281b7 100644 --- a/gtsam/slam/tests/testSimulated2DOriented.cpp +++ b/gtsam/slam/tests/testSimulated2DOriented.cpp @@ -56,7 +56,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 5a90f2489..fb0dbaf88 100644 --- a/gtsam/slam/tests/testSimulated3D.cpp +++ b/gtsam/slam/tests/testSimulated3D.cpp @@ -28,7 +28,7 @@ using namespace gtsam; /* ************************************************************************* */ TEST( simulated3D, Values ) { - DynamicValues 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 1fd23b57f..29bca829c 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -60,7 +60,7 @@ 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 DynamicValues()); + 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); diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp index 9281e51dc..98c369e4f 100644 --- a/gtsam/slam/tests/testVSLAM.cpp +++ b/gtsam/slam/tests/testVSLAM.cpp @@ -92,7 +92,7 @@ TEST( Graph, optimizeLM) graph->addPointConstraint(3, landmark3); // Create an initial values structure corresponding to the ground truth - boost::shared_ptr initialEstimate(new DynamicValues); + boost::shared_ptr initialEstimate(new Values); initialEstimate->insert(PoseKey(1), camera1); initialEstimate->insert(PoseKey(2), camera2); initialEstimate->insert(PointKey(1), landmark1); @@ -129,7 +129,7 @@ TEST( Graph, optimizeLM2) graph->addPoseConstraint(2, camera2); // Create an initial values structure corresponding to the ground truth - boost::shared_ptr initialEstimate(new DynamicValues); + boost::shared_ptr initialEstimate(new Values); initialEstimate->insert(PoseKey(1), camera1); initialEstimate->insert(PoseKey(2), camera2); initialEstimate->insert(PointKey(1), landmark1); @@ -166,7 +166,7 @@ TEST( Graph, CHECK_ORDERING) graph->addPoseConstraint(2, camera2); // Create an initial values structure corresponding to the ground truth - boost::shared_ptr initialEstimate(new DynamicValues); + boost::shared_ptr initialEstimate(new Values); initialEstimate->insert(PoseKey(1), camera1); initialEstimate->insert(PoseKey(2), camera2); initialEstimate->insert(PointKey(1), landmark1); @@ -194,23 +194,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 - DynamicValues init; + Values init; init.insert(PoseKey(1), Pose3()); init.insert(PointKey(1), Point3(1.0, 2.0, 3.0)); - DynamicValues expected; + 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; - DynamicValues largeValues = init; + 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); - DynamicValues 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 958337677..4bf5d7b35 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -38,7 +38,7 @@ namespace gtsam { */ typedef TypedSymbol PoseKey; ///< The key type used for poses typedef TypedSymbol PointKey; ///< The key type used for points - typedef boost::shared_ptr shared_values; ///< shared pointer to 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 diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index d6bda8472..e1f8f574a 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -33,7 +33,7 @@ SharedDiagonal hard_model1 = noiseModel::Constrained::All(1); typedef NonlinearFactorGraph Graph; typedef boost::shared_ptr shared_graph; -typedef boost::shared_ptr shared_values; +typedef boost::shared_ptr shared_values; typedef NonlinearOptimizer Optimizer; // some simple inequality constraints @@ -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); - DynamicValues 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); - DynamicValues 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 1ff5fe047..a53f716b5 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -170,7 +170,7 @@ public: * calculate the error of the factor * Override for systems with unusual noise models */ - virtual double error(const DynamicValues& c) const { + virtual double error(const Values& c) const { Vector w = whitenedError(c); return 0.5 * w.dot(w); } @@ -181,7 +181,7 @@ public: } /** Vector of errors, whitened ! */ - Vector whitenedError(const DynamicValues& c) const { + Vector whitenedError(const Values& c) const { return QInvSqrt(c[key1_])*unwhitenedError(c); } @@ -190,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 DynamicValues& 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; @@ -307,7 +307,7 @@ public: * calculate the error of the factor * Override for systems with unusual noise models */ - virtual double error(const DynamicValues& c) const { + virtual double error(const Values& c) const { Vector w = whitenedError(c); return 0.5 * w.dot(w); } @@ -318,7 +318,7 @@ public: } /** Vector of errors, whitened ! */ - Vector whitenedError(const DynamicValues& c) const { + Vector whitenedError(const Values& c) const { return RInvSqrt(c[key_])*unwhitenedError(c); } @@ -327,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 DynamicValues& 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); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 7c39b8c60..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 - DynamicValues theta; + Values theta; theta.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3)); theta.insert(planarSLAM::PointKey(0), Point2(.4, .5)); - DynamicValues newTheta; + Values newTheta; newTheta.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8)); VectorValues deltaUnpermuted; @@ -60,7 +60,7 @@ TEST(ISAM2, AddVariables) { EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[planarSLAM::PoseKey(0)]])); // Create expected state - DynamicValues 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)); @@ -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 - DynamicValues theta; + Values theta; theta.insert(planarSLAM::PoseKey(0), Pose2(0.01, 0.01, 0.01)); // Create conditional @@ -180,15 +180,15 @@ TEST(ISAM2, optimize2) { } /* ************************************************************************* */ -bool isam_check(const planarSLAM::Graph& fullgraph, const DynamicValues& fullinit, const GaussianISAM2<>& isam) { - DynamicValues 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); - DynamicValues expected = fullinit.retract(delta, ordering); + Values expected = fullinit.retract(delta, ordering); return assert_equal(expected, actual); } @@ -211,7 +211,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) // These variables will be reused and accumulate factors and values GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - DynamicValues fullinit; + 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); - DynamicValues 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); - DynamicValues 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); - DynamicValues 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); - DynamicValues 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); - DynamicValues 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)); @@ -344,7 +344,7 @@ TEST(ISAM2, slamlike_solution_dogleg) // These variables will be reused and accumulate factors and values GaussianISAM2<> isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); - DynamicValues fullinit; + 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); - DynamicValues 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); - DynamicValues 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); - DynamicValues 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); - DynamicValues 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); - DynamicValues 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)); @@ -472,7 +472,7 @@ TEST(ISAM2, clone) { // These variables will be reused and accumulate factors and values GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); - DynamicValues fullinit; + 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); - DynamicValues 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); - DynamicValues 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); - DynamicValues 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); - DynamicValues 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); - DynamicValues 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)); @@ -645,7 +645,7 @@ TEST(ISAM2, removeFactors) // These variables will be reused and accumulate factors and values GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - DynamicValues fullinit; + 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); - DynamicValues 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); - DynamicValues 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); - DynamicValues 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); - DynamicValues 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 - DynamicValues 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,7 +731,7 @@ TEST(ISAM2, removeFactors) // Remove the measurement on landmark 0 FastVector toRemove; toRemove.push_back(result.newFactorsIndices[1]); - isam.update(planarSLAM::Graph(), DynamicValues(), toRemove); + isam.update(planarSLAM::Graph(), Values(), toRemove); } // Compare solutions @@ -784,7 +784,7 @@ TEST(ISAM2, constrained_ordering) // These variables will be reused and accumulate factors and values GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - DynamicValues fullinit; + 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); - DynamicValues 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); - DynamicValues 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); - DynamicValues 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); - DynamicValues 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); - DynamicValues 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)); diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index fc9662519..ee601e24f 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,7 +198,7 @@ TEST(GaussianJunctionTree, simpleMarginal) { fg.addPrior(pose2SLAM::Key(0), Pose2(), sharedSigma(3, 10.0)); fg.addConstraint(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0))); - DynamicValues init; + Values init; init.insert(pose2SLAM::Key(0), Pose2()); init.insert(pose2SLAM::Key(1), Pose2(1.0, 0.0, 0.0)); diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index f6d8629bf..80c15c611 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -94,10 +94,10 @@ TEST( Graph, composePoses ) Pose2 rootPose = p2; - boost::shared_ptr actual = composePoses actual = composePoses (graph, tree, rootPose); - DynamicValues expected; + Values expected; expected.insert(pose2SLAM::Key(1), p1); expected.insert(pose2SLAM::Key(2), p2); expected.insert(pose2SLAM::Key(3), p3); 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 b917185e9..328b26083 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -44,7 +44,7 @@ PoseKey key(1); /* ************************************************************************* */ TEST ( NonlinearEquality, linearization ) { Pose2 value = Pose2(2.1, 1.0, 2.0); - DynamicValues linearize; + Values linearize; linearize.insert(key, value); // create a nonlinear equality constraint @@ -62,7 +62,7 @@ TEST ( NonlinearEquality, linearization_pose ) { PoseKey key(1); Pose2 value; - DynamicValues config; + Values config; config.insert(key, value); // create a nonlinear equality constraint @@ -76,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); - DynamicValues bad_linearize; + Values bad_linearize; bad_linearize.insert(key, wrong); // create a nonlinear equality constraint @@ -92,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); - DynamicValues bad_linearize; + Values bad_linearize; bad_linearize.insert(key, wrong); // create a nonlinear equality constraint @@ -108,7 +108,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) { PoseKey key(1); Pose2 value, wrong(2.0, 3.0, 4.0); - DynamicValues bad_linearize; + Values bad_linearize; bad_linearize.insert(key, wrong); // create a nonlinear equality constraint @@ -122,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); - DynamicValues feasible, bad_linearize; + Values feasible, bad_linearize; feasible.insert(key, value); bad_linearize.insert(key, wrong); @@ -167,7 +167,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { EXPECT(assert_equal(expVec, actVec, 1e-5)); // the actual error should have a gain on it - DynamicValues config; + Values config; config.insert(key1, badPoint1); double actError = nle.error(config); DOUBLES_EQUAL(500.0, actError, 1e-9); @@ -194,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 DynamicValues()); + boost::shared_ptr init(new Values()); init->insert(key1, initPose); // optimize @@ -205,7 +205,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) { PoseOptimizer result = optimizer.levenbergMarquardt(); // verify - DynamicValues expected; + Values expected; expected.insert(key1, feasible1); EXPECT(assert_equal(expected, *result.values())); } @@ -218,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 DynamicValues()); + boost::shared_ptr init(new Values()); Pose2 initPose(0.0, 2.0, 3.0); init->insert(key1, initPose); @@ -241,7 +241,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { PoseOptimizer result = optimizer.levenbergMarquardt(); // verify - DynamicValues expected; + Values expected; expected.insert(key1, feasible1); EXPECT(assert_equal(expected, *result.values())); } @@ -252,7 +252,7 @@ SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0); typedef NonlinearFactorGraph Graph; typedef boost::shared_ptr shared_graph; -typedef boost::shared_ptr shared_values; +typedef boost::shared_ptr shared_values; typedef NonlinearOptimizer Optimizer; /* ************************************************************************* */ @@ -262,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)); @@ -287,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); @@ -319,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)); - DynamicValues expected; + Values expected; expected.insert(key, truth_pt); EXPECT(constraint->active(expected)); EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol); @@ -341,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)); @@ -349,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); @@ -369,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); @@ -378,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); @@ -417,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); - DynamicValues expected; + Values expected; expected.insert(key1, truth_pt1); expected.insert(key2, truth_pt2); CHECK(assert_equal(expected, *actual, tol)); @@ -454,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 @@ -462,7 +462,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate); - DynamicValues expected; + Values expected; expected.insert(x1, pt_x1); expected.insert(l1, Point2(1.0, 6.0)); expected.insert(l2, Point2(1.0, 6.0)); @@ -497,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 @@ -506,7 +506,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { // optimize Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate); - DynamicValues 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)); @@ -521,7 +521,7 @@ Cal3_S2 K(fov,w,h); boost::shared_ptr shK(new Cal3_S2(K)); // typedefs for visual SLAM example -typedef boost::shared_ptr shared_vconfig; +typedef boost::shared_ptr shared_vconfig; typedef visualSLAM::Graph VGraph; typedef NonlinearOptimizer VOptimizer; @@ -565,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 DynamicValues()); + shared_vconfig initValues(new Values()); initValues->insert(x1, pose1); initValues->insert(x2, pose2); initValues->insert(l1, landmark1); @@ -575,7 +575,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { VOptimizer::shared_values actual = VOptimizer::optimizeLM(graph, initValues); // create config - DynamicValues 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 c9fa05c45..490692c90 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -82,7 +82,7 @@ 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]; @@ -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()); @@ -181,7 +181,7 @@ TEST( NonlinearFactor, size ) 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], @@ -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()); @@ -262,7 +262,7 @@ public: /* ************************************ */ TEST(NonlinearFactor, NonlinearFactor4) { TestFactor4 tf; - DynamicValues tv; + 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)); @@ -309,7 +309,7 @@ public: /* ************************************ */ TEST(NonlinearFactor, NonlinearFactor5) { TestFactor5 tf; - DynamicValues tv; + 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)); @@ -361,7 +361,7 @@ public: /* ************************************ */ TEST(NonlinearFactor, NonlinearFactor6) { TestFactor6 tf; - DynamicValues tv; + 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)); diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 8606c027f..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 ); } @@ -89,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); @@ -101,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 66fd410b6..0ad4e03ae 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -30,8 +30,8 @@ TEST(testNonlinearISAM, markov_chain ) { Graph start_factors; start_factors.addPoseConstraint(key, cur_pose); - DynamicValues init; - DynamicValues 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 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); - DynamicValues actualMFQR = optimize( + Values actualMFQR = optimize( fg, c0, *NonlinearOptimizationParameters().newFactorization(true), MULTIFRONTAL, LM); DOUBLES_EQUAL(0,fg.error(actualMFQR),tol); - DynamicValues actualMFLDL = optimize( + Values actualMFLDL = optimize( fg, c0, *NonlinearOptimizationParameters().newFactorization(false), MULTIFRONTAL, LM); DOUBLES_EQUAL(0,fg.error(actualMFLDL),tol); } @@ -255,7 +255,7 @@ TEST( NonlinearOptimizer, Factorization ) { typedef NonlinearOptimizer Optimizer; - boost::shared_ptr config(new DynamicValues); + boost::shared_ptr config(new Values); config->insert(pose2SLAM::Key(1), Pose2(0.,0.,0.)); config->insert(pose2SLAM::Key(2), Pose2(1.5,0.,0.)); @@ -270,7 +270,7 @@ TEST( NonlinearOptimizer, Factorization ) Optimizer optimizer(graph, config, ordering); Optimizer optimized = optimizer.iterateLM(); - DynamicValues expected; + Values expected; expected.insert(pose2SLAM::Key(1), Pose2(0.,0.,0.)); expected.insert(pose2SLAM::Key(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 64585ff75..439228c79 100644 --- a/tests/testPose2SLAMwSPCG.cpp +++ b/tests/testPose2SLAMwSPCG.cpp @@ -40,7 +40,7 @@ TEST(testPose2SLAMwSPCG, example1) { graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ; graph.addPrior(x1, Pose2(0,0,0), sigma) ; - DynamicValues 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)); @@ -51,7 +51,7 @@ TEST(testPose2SLAMwSPCG, example1) { initial.insert(x8, Pose2(3.9, 2.1, 0.01)); initial.insert(x9, Pose2(4.1, 3.9,-0.01)); - DynamicValues 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)); @@ -62,7 +62,7 @@ TEST(testPose2SLAMwSPCG, example1) { expected.insert(x8, Pose2(4.0, 2.0, 0.0)); expected.insert(x9, Pose2(4.0, 4.0, 0.0)); - DynamicValues 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 1e57a0797..285428dab 100644 --- a/tests/testRot3Optimization.cpp +++ b/tests/testRot3Optimization.cpp @@ -38,8 +38,8 @@ typedef NonlinearFactorGraph Graph; TEST(Rot3, optimize) { // Optimize a circle - DynamicValues truth; - DynamicValues initial; + Values truth; + Values initial; Graph fg; fg.add(Prior(0, Rot3(), sharedSigma(3, 0.01))); for(int j=0; j<6; ++j) { @@ -49,7 +49,7 @@ TEST(Rot3, optimize) { } NonlinearOptimizationParameters params; - DynamicValues 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 31012aa44..f2e740180 100644 --- a/tests/testSerialization.cpp +++ b/tests/testSerialization.cpp @@ -445,7 +445,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::simulated2D::Measurement, "gtsam::simulated2D::Me TEST (Serialization, smallExample) { using namespace example; Graph nfg = createNonlinearFactorGraph(); - example::Values c1 = createValues(); + Values c1 = createValues(); EXPECT(equalsObj(nfg)); EXPECT(equalsXML(nfg)); @@ -464,7 +464,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::planarSLAM::Constraint, "gtsam::planarSLAM::Cons /* ************************************************************************* */ 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)); @@ -490,7 +490,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)); @@ -502,7 +502,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)); @@ -524,7 +524,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::visualSLAM::StereoFactor, "gtsam::visualSLAM:: /* ************************************************************************* */ 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 a21096051..106b16ed1 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 8845b901e..4d6d3660c 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)