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

Renamed DynamicValues to Values and removed specialized derived Values classes

release/4.3a0
Richard Roberts 2012-02-02 16:16:46 +00:00
parent f3a3f8ebf6
commit 26cdf28421
82 changed files with 526 additions and 2140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/CameraResectioning.cpp
View File

@ -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<NonlinearFactorGraph> (graph, initial);
Values result = optimize<NonlinearFactorGraph> (graph, initial);
result.print("Final result: ");
return 0;

4
examples/PlanarSLAMExample_easy.cpp
View File

@ -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>(graph, initialEstimate);
Values result = optimize<Graph>(graph, initialEstimate);
result.print("final result");
return 0;

2
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -108,7 +108,7 @@ int main(int argc, char** argv) {
graph->print("Full Graph");
// initialize to noisy points
boost::shared_ptr<DynamicValues> initial(new DynamicValues);
boost::shared_ptr<Values> 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));

4
examples/Pose2SLAMExample_advanced.cpp
View File

@ -57,7 +57,7 @@ int main(int argc, char** argv) {
/* 3. Create the data structure to hold the initial estimate to the solution
* initialize to noisy points */
shared_ptr<DynamicValues> initial(new DynamicValues);
shared_ptr<Values> 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 */

4
examples/Pose2SLAMExample_easy.cpp
View File

@ -55,7 +55,7 @@ int main(int argc, char** argv) {
/* 3. Create the data structure to hold the initial estinmate to the solution
* 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>(graph, initial);
Values result = optimize<Graph>(graph, initial);
result.print("final result");

10
examples/Pose2SLAMwSPCG_advanced.cpp
View File

@ -27,17 +27,17 @@ using namespace gtsam;
using namespace pose2SLAM;
typedef boost::shared_ptr<Graph> sharedGraph ;
typedef boost::shared_ptr<pose2SLAM::Values> sharedValue ;
typedef boost::shared_ptr<Values> sharedValue ;
//typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, SubgraphSolver<Graph,Values> > SPCGOptimizer;
typedef SubgraphSolver<Graph, GaussianFactorGraph, pose2SLAM::Values> Solver;
typedef SubgraphSolver<Graph, GaussianFactorGraph, Values> Solver;
typedef boost::shared_ptr<Solver> sharedSolver ;
typedef NonlinearOptimizer<Graph, pose2SLAM::Values, GaussianFactorGraph, Solver> SPCGOptimizer;
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, Solver> 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<Graph>() ;
initial = boost::make_shared<pose2SLAM::Values>() ;
initial = boost::make_shared<Values>() ;
// create a 3 by 3 grid
// x3 x6 x9

2
examples/Pose2SLAMwSPCG_easy.cpp
View File

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

4
examples/SimpleRotation.cpp
View File

@ -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>(graph, initial);
Values result = optimize<Graph>(graph, initial);
result.print("final result");
return 0;

2
examples/elaboratePoint2KalmanFilter.cpp
View File

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

8
examples/vSLAMexample/vISAMexample.cpp
View File

@ -80,7 +80,7 @@ void readAllDataISAM() {
/**
* Setup newFactors and initialValues for each new pose and set of measurements at each frame.
*/
void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<DynamicValues>& initialValues,
void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
// Create a graph of newFactors with new measurements
@ -101,7 +101,7 @@ void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<DynamicVa
}
// Create initial values for all nodes in the newFactors
initialValues = shared_ptr<DynamicValues> (new DynamicValues());
initialValues = shared_ptr<Values> (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<Graph> newFactors;
shared_ptr<DynamicValues> initialValues;
shared_ptr<Values> 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: ");
}

6
examples/vSLAMexample/vSFMexample.cpp
View File

@ -103,9 +103,9 @@ Graph setupGraph(std::vector<Feature2D>& 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<int, Point3> landmarks, std::map<int, Pose3> poses) {
Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
DynamicValues initValues;
Values initValues;
// Initialize landmarks 3D positions.
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
@ -136,7 +136,7 @@ int main(int argc, char* argv[]) {
boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
// Create an initial Values structure using groundtruth values as the initial estimates
boost::shared_ptr<DynamicValues> initialEstimates(new DynamicValues(initialize(g_landmarks, g_poses)));
boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses)));
cout << "*******************************************************" << endl;
initialEstimates->print("INITIAL ESTIMATES: ");

10
gtsam.h
View File

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

8
gtsam/inference/graph-inl.h
View File

@ -143,11 +143,11 @@ template <class V, class POSE, class KEY>
class compose_key_visitor : public boost::default_bfs_visitor {
private:
boost::shared_ptr<DynamicValues> config_;
boost::shared_ptr<Values> config_;
public:
compose_key_visitor(boost::shared_ptr<DynamicValues> config_in) {config_ = config_in;}
compose_key_visitor(boost::shared_ptr<Values> config_in) {config_ = config_in;}
template <typename Edge, typename Graph> 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<class G, class Factor, class POSE, class KEY>
boost::shared_ptr<DynamicValues> composePoses(const G& graph, const PredecessorMap<KEY>& tree,
boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>& tree,
const POSE& rootPose) {
//TODO: change edge_weight_t to edge_pose_t
@ -207,7 +207,7 @@ boost::shared_ptr<DynamicValues> composePoses(const G& graph, const PredecessorM
}
// compose poses
boost::shared_ptr<DynamicValues> config(new DynamicValues);
boost::shared_ptr<Values> config(new Values);
KEY rootKey = boost::get(boost::vertex_name, g, root);
config->insert(rootKey, rootPose);
compose_key_visitor<PoseVertex, POSE, KEY> vis(config);

4
gtsam/inference/graph.h
View File

@ -25,7 +25,7 @@
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/shared_ptr.hpp>
#include <gtsam/nonlinear/DynamicValues.h>
#include <gtsam/nonlinear/Values.h>
namespace gtsam {
@ -89,7 +89,7 @@ namespace gtsam {
* Compose the poses by following the chain specified by the spanning tree
*/
template<class G, class Factor, class POSE, class KEY>
boost::shared_ptr<DynamicValues>
boost::shared_ptr<Values>
composePoses(const G& graph, const PredecessorMap<KEY>& tree, const POSE& rootPose);

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

@ -60,7 +60,7 @@ VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,KEY>::optimize() const {
}
template<class GRAPH, class LINEAR, class KEY>
void SubgraphSolver<GRAPH,LINEAR,KEY>::initialize(const GRAPH& G, const DynamicValues& theta0) {
void SubgraphSolver<GRAPH,LINEAR,KEY>::initialize(const GRAPH& G, const Values& theta0) {
// generate spanning tree
PredecessorMap<KEY> tree_ = gtsam::findMinimumSpanningTree<GRAPH, KEY, Constraint>(G);

8
gtsam/linear/SubgraphSolver.h
View File

@ -16,7 +16,7 @@
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/nonlinear/DynamicValues.h>
#include <gtsam/nonlinear/Values.h>
namespace gtsam {
@ -44,7 +44,7 @@ private:
typedef boost::shared_ptr<Ordering> shared_ordering ;
typedef boost::shared_ptr<GRAPH> shared_graph ;
typedef boost::shared_ptr<LINEAR> shared_linear ;
typedef boost::shared_ptr<DynamicValues> shared_values ;
typedef boost::shared_ptr<Values> shared_values ;
typedef boost::shared_ptr<SubgraphPreconditioner> shared_preconditioner ;
typedef std::map<Index,Index> mapPairIndex ;
@ -62,7 +62,7 @@ private:
public:
SubgraphSolver(const GRAPH& G, const DynamicValues& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
SubgraphSolver(const GRAPH& G, const Values& theta0, const Parameters &parameters = 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(){}

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

@ -30,7 +30,7 @@ namespace gtsam {
template<class KEY>
typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::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

2
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

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

8
gtsam/nonlinear/ISAM2-impl-inl.h
View File

@ -45,7 +45,7 @@ struct ISAM2<CONDITIONAL, GRAPH>::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<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& 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<CONDITIONAL, GRAPH>::Impl {
* where we might expmap something twice, or expmap it but then not
* recalculate its delta.
*/
static void ExpmapMasked(DynamicValues& values, const Permuted<VectorValues>& delta,
static void ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
const Ordering& ordering, const std::vector<bool>& mask,
boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>());
@ -136,7 +136,7 @@ struct _VariableAdder {
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
const DynamicValues& newTheta, DynamicValues& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) {
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
theta.insert(newTheta);
@ -259,7 +259,7 @@ struct _SelectiveExpmapAndClear {
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(DynamicValues& values, const Permuted<VectorValues>& delta,
void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
const Ordering& ordering, const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug) {
// If debugging, invalidate if requested, otherwise do not invalidate.
// Invalidating means setting expmapped entries to Inf, to trigger assertions

6
gtsam/nonlinear/ISAM2-inl.h
View File

@ -580,10 +580,10 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
DynamicValues ISAM2<CONDITIONAL, GRAPH>::calculateEstimate() const {
Values ISAM2<CONDITIONAL, GRAPH>::calculateEstimate() const {
// We use ExpmapMasked here instead of regular expmap because the former
// handles Permuted<VectorValues>
DynamicValues ret(theta_);
Values ret(theta_);
vector<bool> mask(ordering_.nVars(), true);
Impl::ExpmapMasked(ret, delta_, ordering_, mask);
return ret;
@ -600,7 +600,7 @@ typename KEY::Value ISAM2<CONDITIONAL, GRAPH>::calculateEstimate(const KEY& key)
/* ************************************************************************* */
template<class CONDITIONAL, class GRAPH>
DynamicValues ISAM2<CONDITIONAL, GRAPH>::calculateBestEstimate() const {
Values ISAM2<CONDITIONAL, GRAPH>::calculateBestEstimate() const {
VectorValues delta(theta_.dims(ordering_));
optimize2(this->root(), delta);
return theta_.retract(delta, ordering_);

12
gtsam/nonlinear/ISAM2.h
View File

@ -271,7 +271,7 @@ class ISAM2: public BayesTree<CONDITIONAL, ISAM2Clique<CONDITIONAL> > {
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<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class
typedef ISAM2<CONDITIONAL> 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<size_t>& removeFactorIndices = FastVector<size_t>(),
const boost::optional<FastSet<Symbol> >& 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<VectorValues>& getDelta() const { return delta_; }

6
gtsam/nonlinear/Makefile.am
View File

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

4
gtsam/nonlinear/NonlinearEquality.h
View File

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

28
gtsam/nonlinear/NonlinearFactor.h
View File

@ -31,7 +31,7 @@
#include <gtsam/linear/SharedNoiseModel.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/DynamicValues.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Ordering.h>
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<GaussianFactor>
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<std::vector<Matrix>&> H = boost::none) const = 0;
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> 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<GaussianFactor> linearize(const DynamicValues& x, const Ordering& ordering) const {
boost::shared_ptr<GaussianFactor> linearize(const Values& x, const Ordering& ordering) const {
// Only linearize if the factor is active
if (!this->active(x))
return boost::shared_ptr<JacobianFactor>();
@ -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<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> 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<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> 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<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> 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<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> 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<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> 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<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> 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]);

10
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -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<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> 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);

10
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -50,10 +50,10 @@ namespace gtsam {
std::set<Symbol> 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<class F>
void add(const F& factor) {
@ -72,20 +72,20 @@ namespace gtsam {
* ordering is found.
*/
std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
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<GaussianFactorGraph >
linearize(const DynamicValues& config, const Ordering& ordering) const;
linearize(const Values& config, const Ordering& ordering) const;
private:

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

@ -31,7 +31,7 @@ using namespace gtsam;
/* ************************************************************************* */
template<class GRAPH>
void NonlinearISAM<GRAPH>::update(const Factors& newFactors,
const DynamicValues& initialValues) {
const Values& initialValues) {
if(newFactors.size() > 0) {
@ -69,7 +69,7 @@ void NonlinearISAM<GRAPH>::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<GRAPH>::reorder_relinearize() {
/* ************************************************************************* */
template<class GRAPH>
DynamicValues NonlinearISAM<GRAPH>::estimate() const {
Values NonlinearISAM<GRAPH>::estimate() const {
if(isam_.size() > 0)
return linPoint_.retract(optimize(isam_), ordering_);
else

8
gtsam/nonlinear/NonlinearISAM.h
View File

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

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

@ -32,7 +32,7 @@ namespace gtsam {
* The Elimination solver
*/
template<class G>
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<G, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
Optimizer optimizer(boost::make_shared<const G>(graph),
boost::make_shared<const DynamicValues>(initialEstimate), ordering,
boost::make_shared<const Values>(initialEstimate), ordering,
boost::make_shared<NonlinearOptimizationParameters>(parameters));
// Now optimize using either LM or GN methods.
@ -55,7 +55,7 @@ namespace gtsam {
* The multifrontal solver
*/
template<class G>
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<G, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
Optimizer optimizer(boost::make_shared<const G>(graph),
boost::make_shared<const DynamicValues>(initialEstimate), ordering,
boost::make_shared<const Values>(initialEstimate), ordering,
boost::make_shared<NonlinearOptimizationParameters>(parameters));
// now optimize using either LM or GN methods
@ -78,19 +78,19 @@ namespace gtsam {
* The sparse preconditioned conjugate gradient solver
*/
template<class G>
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<G,GaussianFactorGraph,DynamicValues> Solver;
typedef SubgraphSolver<G,GaussianFactorGraph,Values> Solver;
typedef boost::shared_ptr<Solver> shared_Solver;
typedef NonlinearOptimizer<G, GaussianFactorGraph, Solver> SPCGOptimizer;
shared_Solver solver = boost::make_shared<Solver>(
graph, initialEstimate, IterativeOptimizationParameters());
SPCGOptimizer optimizer(
boost::make_shared<const G>(graph),
boost::make_shared<const DynamicValues>(initialEstimate),
boost::make_shared<const Values>(initialEstimate),
solver->ordering(),
solver,
boost::make_shared<NonlinearOptimizationParameters>(parameters));
@ -106,7 +106,7 @@ namespace gtsam {
* optimization that returns the values
*/
template<class G>
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<G>(graph, initialEstimate, parameters,
nonlinear_method == LM);
case SPCG:
// return optimizeSPCG<G,DynamicValues>(graph, initialEstimate, parameters,
// return optimizeSPCG<G,Values>(graph, initialEstimate, parameters,
// nonlinear_method == LM);
throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint");
}

2
gtsam/nonlinear/NonlinearOptimization.h
View File

@ -43,7 +43,7 @@ namespace gtsam {
* optimization that returns the values
*/
template<class G>
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);

6
gtsam/nonlinear/NonlinearOptimizer-inl.h
View File

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

36
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -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<G,DynamicValues,L> implements
* linearize: G * DynamicValues -> L
* solve : L -> DynamicValues
* Concept NonLinearSolver<G,Values,L> 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<Pose2Graph, Pose2Values>.
* 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<const DynamicValues> shared_values; ///Prevent memory leaks in Values
typedef boost::shared_ptr<const Values> shared_values; ///Prevent memory leaks in Values
typedef boost::shared_ptr<const G> shared_graph; /// Prevent memory leaks in Graph
typedef boost::shared_ptr<L> shared_linear; /// Not sure
typedef boost::shared_ptr<const Ordering> 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<const G>(graph),
boost::make_shared<const DynamicValues>(values),
boost::make_shared<const Values>(values),
boost::make_shared<Parameters>(parameters));
}
static shared_values optimizeLM(const G& graph,
const DynamicValues& values,
const Values& values,
Parameters::verbosityLevel verbosity) {
return optimizeLM(boost::make_shared<const G>(graph),
boost::make_shared<const DynamicValues>(values),
boost::make_shared<const Values>(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<const G>(graph),
boost::make_shared<const DynamicValues>(values),
boost::make_shared<const Values>(values),
boost::make_shared<Parameters>(parameters));
}
static shared_values optimizeDogLeg(const G& graph,
const DynamicValues& values,
const Values& values,
Parameters::verbosityLevel verbosity) {
return optimizeDogLeg(boost::make_shared<const G>(graph),
boost::make_shared<const DynamicValues>(values),
boost::make_shared<const Values>(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<const G>(graph),
boost::make_shared<const DynamicValues>(values),
boost::make_shared<const Values>(values),
boost::make_shared<Parameters>(parameters));
}
};

165
gtsam/nonlinear/TupleValues-inl.h
View File

@ -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<class VALUES1>
TupleValues1<VALUES1>::TupleValues1(const TupleValues1<VALUES1>& values) :
TupleValuesEnd<VALUES1> (values) {}
template<class VALUES1>
TupleValues1<VALUES1>::TupleValues1(const VALUES1& cfg1) :
TupleValuesEnd<VALUES1> (cfg1) {}
template<class VALUES1>
TupleValues1<VALUES1>::TupleValues1(const TupleValuesEnd<VALUES1>& values) :
TupleValuesEnd<VALUES1>(values) {}
/* ************************************************************************* */
/** TupleValues 2 */
template<class VALUES1, class VALUES2>
TupleValues2<VALUES1, VALUES2>::TupleValues2(const TupleValues2<VALUES1, VALUES2>& values) :
TupleValues<VALUES1, TupleValuesEnd<VALUES2> >(values) {}
template<class VALUES1, class VALUES2>
TupleValues2<VALUES1, VALUES2>::TupleValues2(const VALUES1& cfg1, const VALUES2& cfg2) :
TupleValues<VALUES1, TupleValuesEnd<VALUES2> >(
cfg1, TupleValuesEnd<VALUES2>(cfg2)) {}
template<class VALUES1, class VALUES2>
TupleValues2<VALUES1, VALUES2>::TupleValues2(const TupleValues<VALUES1, TupleValuesEnd<VALUES2> >& values) :
TupleValues<VALUES1, TupleValuesEnd<VALUES2> >(values) {}
/* ************************************************************************* */
/** TupleValues 3 */
template<class VALUES1, class VALUES2, class VALUES3>
TupleValues3<VALUES1, VALUES2, VALUES3>::TupleValues3(
const TupleValues3<VALUES1, VALUES2, VALUES3>& values) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >(values) {}
template<class VALUES1, class VALUES2, class VALUES3>
TupleValues3<VALUES1, VALUES2, VALUES3>::TupleValues3(
const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >(
cfg1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> >(
cfg2, TupleValuesEnd<VALUES3>(cfg3))) {}
template<class VALUES1, class VALUES2, class VALUES3>
TupleValues3<VALUES1, VALUES2, VALUES3>::TupleValues3(
const TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >& values) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >(values) {}
/* ************************************************************************* */
/** TupleValues 4 */
template<class VALUES1, class VALUES2, class VALUES3, class VALUES4>
TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>::TupleValues4(
const TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>& values) :
TupleValues<VALUES1, TupleValues<VALUES2,
TupleValues<VALUES3, TupleValuesEnd<VALUES4> > > >(values) {}
template<class VALUES1, class VALUES2, class VALUES3, class VALUES4>
TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>::TupleValues4(
const VALUES1& cfg1, const VALUES2& cfg2,
const VALUES3& cfg3,const VALUES4& cfg4) :
TupleValues<VALUES1, TupleValues<VALUES2,
TupleValues<VALUES3, TupleValuesEnd<VALUES4> > > >(
cfg1, TupleValues<VALUES2, TupleValues<VALUES3, TupleValuesEnd<VALUES4> > >(
cfg2, TupleValues<VALUES3, TupleValuesEnd<VALUES4> >(
cfg3, TupleValuesEnd<VALUES4>(cfg4)))) {}
template<class VALUES1, class VALUES2, class VALUES3, class VALUES4>
TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>::TupleValues4(
const TupleValues<VALUES1, TupleValues<VALUES2,
TupleValues<VALUES3, TupleValuesEnd<VALUES4> > > >& values) :
TupleValues<VALUES1, TupleValues<VALUES2,TupleValues<VALUES3,
TupleValuesEnd<VALUES4> > > >(values) {}
/* ************************************************************************* */
/** TupleValues 5 */
template<class VALUES1, class VALUES2, class VALUES3, class VALUES4, class VALUES5>
TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>::TupleValues5(
const TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>& values) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > > >(values) {}
template<class VALUES1, class VALUES2, class VALUES3, class VALUES4, class VALUES5>
TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>::TupleValues5(
const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3,
const VALUES4& cfg4, const VALUES5& cfg5) :
TupleValues<VALUES1, TupleValues<VALUES2,
TupleValues<VALUES3, TupleValues<VALUES4,
TupleValuesEnd<VALUES5> > > > >(
cfg1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > >(
cfg2, TupleValues<VALUES3, TupleValues<VALUES4, TupleValuesEnd<VALUES5> > >(
cfg3, TupleValues<VALUES4, TupleValuesEnd<VALUES5> >(
cfg4, TupleValuesEnd<VALUES5>(cfg5))))) {}
template<class VALUES1, class VALUES2, class VALUES3, class VALUES4, class VALUES5>
TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>::TupleValues5(
const TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > > >& values) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > > >(values) {}
/* ************************************************************************* */
/** TupleValues 6 */
template<class VALUES1, class VALUES2, class VALUES3,
class VALUES4, class VALUES5, class VALUES6>
TupleValues6<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5, VALUES6>::TupleValues6(
const TupleValues6<VALUES1, VALUES2, VALUES3,
VALUES4, VALUES5, VALUES6>& values) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValues<VALUES5,
TupleValuesEnd<VALUES6> > > > > >(values) {}
template<class VALUES1, class VALUES2, class VALUES3,
class VALUES4, class VALUES5, class VALUES6>
TupleValues6<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5, VALUES6>::TupleValues6(
const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3,
const VALUES4& cfg4, const VALUES5& cfg5, const VALUES6& cfg6) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValues<VALUES5, TupleValuesEnd<VALUES6> > > > > >(
cfg1, TupleValues<VALUES2, TupleValues<VALUES3, TupleValues<VALUES4,
TupleValues<VALUES5, TupleValuesEnd<VALUES6> > > > >(
cfg2, TupleValues<VALUES3, TupleValues<VALUES4, TupleValues<VALUES5,
TupleValuesEnd<VALUES6> > > >(
cfg3, TupleValues<VALUES4, TupleValues<VALUES5,
TupleValuesEnd<VALUES6> > >(
cfg4, TupleValues<VALUES5, TupleValuesEnd<VALUES6> >(
cfg5, TupleValuesEnd<VALUES6>(cfg6)))))) {}
template<class VALUES1, class VALUES2, class VALUES3,
class VALUES4, class VALUES5, class VALUES6>
TupleValues6<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5, VALUES6>::TupleValues6(
const TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValues<VALUES5,
TupleValuesEnd<VALUES6> > > > > >& values) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValues<VALUES5,
TupleValuesEnd<VALUES6> > > > > >(values) {}
}

524
gtsam/nonlinear/TupleValues.h
View File

@ -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 <gtsam/nonlinear/Values.h>
#include <gtsam/linear/VectorValues.h>
#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 VALUES1, class VALUES2>
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<VALUES1, VALUES2>& 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<VALUES1, VALUES2>& 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<class KEY, class VALUE>
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<class CFG1, class CFG2>
void insert(const TupleValues<CFG1, CFG2>& values) { second_.insert(values); }
void insert(const TupleValues<VALUES1, VALUES2>& 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<class CFG1, class CFG2>
void update(const TupleValues<CFG1, CFG2>& values) { second_.update(values); }
void update(const TupleValues<VALUES1, VALUES2>& 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<class KEY, class VALUE>
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<class CFG>
void insertSub(const CFG& values) { second_.insertSub(values); }
void insertSub(const VALUES1& values) { first_.insert(values); }
/** erase an element by key */
template<class KEY>
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<class KEY>
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<class KEY>
boost::optional<typename KEY::Value> exists_(const KEY& j) const { return second_.exists_(j); }
boost::optional<Value1> exists_(const Key1& j) const { return first_.exists_(j); }
/** access operator */
template<class KEY>
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<class KEY>
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<size_t> 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<VALUES1, VALUES2> 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<VALUES1, VALUES2>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering));
localCoordinates(cp, ordering, delta);
return delta;
}
/** logmap each element */
void localCoordinates(const TupleValues<VALUES1, VALUES2>& 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 <key,value> pair. The operator must be able to handle such an
* iterator for every type in the Values, (i.e. through templating).
*/
template<typename A>
void apply(A& operation) {
first_.apply(operation);
second_.apply(operation);
}
template<typename A>
void apply(A& operation) const {
first_.apply(operation);
second_.apply(operation);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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 VALUES>
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>& values) :
first_(values.first_) {}
TupleValuesEnd(const VALUES& cfg) :
first_(cfg) {}
void print(const std::string& s = "") const {
first_.print();
}
bool equals(const TupleValuesEnd<VALUES>& 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>& values) {first_.insert(values.first_); }
void update(const TupleValuesEnd<VALUES>& 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<Value1> 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<VALUES> retract(const VectorValues& delta, const Ordering& ordering) const {
return TupleValuesEnd(first_.retract(delta, ordering));
}
VectorValues localCoordinates(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering));
localCoordinates(cp, ordering, delta);
return delta;
}
void localCoordinates(const TupleValuesEnd<VALUES>& 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 <key,value> pair. The operator must be able to handle such an
* iterator for every type in the Values, (i.e. through templating).
*/
template<typename A>
void apply(A& operation) {
first_.apply(operation);
}
template<typename A>
void apply(A& operation) const {
first_.apply(operation);
}
/** Create an array of variable dimensions using the given ordering */
std::vector<size_t> 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<class ARCHIVE>
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 C1>
class TupleValues1 : public TupleValuesEnd<C1> {
public:
// typedefs
typedef C1 Values1;
typedef TupleValuesEnd<C1> Base;
typedef TupleValues1<C1> 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 C1, class C2>
class TupleValues2 : public TupleValues<C1, TupleValuesEnd<C2> > {
public:
// typedefs
typedef C1 Values1;
typedef C2 Values2;
typedef TupleValues<C1, TupleValuesEnd<C2> > Base;
typedef TupleValues2<C1, C2> 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 C1, class C2, class C3>
class TupleValues3 : public TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > > {
public:
// typedefs
typedef C1 Values1;
typedef C2 Values2;
typedef C3 Values3;
typedef TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > > Base;
typedef TupleValues3<C1, C2, C3> 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 C1, class C2, class C3, class C4>
class TupleValues4 : public TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > {
public:
// typedefs
typedef C1 Values1;
typedef C2 Values2;
typedef C3 Values3;
typedef C4 Values4;
typedef TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > Base;
typedef TupleValues4<C1, C2, C3, C4> 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 C1, class C2, class C3, class C4, class C5>
class TupleValues5 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > > {
public:
typedef C1 Values1;
typedef C2 Values2;
typedef C3 Values3;
typedef C4 Values4;
typedef C5 Values5;
typedef TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > > Base;
typedef TupleValues5<C1, C2, C3, C4, C5> 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 C1, class C2, class C3, class C4, class C5, class C6>
class TupleValues6 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > > {
public:
typedef C1 Values1;
typedef C2 Values2;
typedef C3 Values3;
typedef C4 Values4;
typedef C5 Values5;
typedef C6 Values6;
typedef TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > > Base;
typedef TupleValues6<C1, C2, C3, C4, C5, C6> 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 <gtsam/nonlinear/TupleValues-inl.h>

18
gtsam/nonlinear/DynamicValues-inl.h → gtsam/nonlinear/Values-inl.h
View File

@ -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 <utility>
#include <gtsam/nonlinear/DynamicValues.h> // Only so Eclipse finds class definition
#include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition
namespace gtsam {
@ -39,17 +39,17 @@ namespace gtsam {
/* ************************************************************************* */
template<typename ValueType>
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<const ValueType&>(*item->second);
@ -57,7 +57,7 @@ namespace gtsam {
/* ************************************************************************* */
template<typename TypedKey>
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<typename ValueType>
boost::optional<const ValueType&> DynamicValues::exists(const Symbol& j) const {
boost::optional<const ValueType&> 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<const ValueType&>(*item->second);
@ -85,7 +85,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class TypedKey>
boost::optional<const typename TypedKey::Value&> DynamicValues::exists(const TypedKey& j) const {
boost::optional<const typename TypedKey::Value&> Values::exists(const TypedKey& j) const {
// Convert to Symbol
const Symbol symbol(j.symbol());

56
gtsam/nonlinear/DynamicValues.cpp → gtsam/nonlinear/Values.cpp
View File

@ -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 <gtsam/nonlinear/DynamicValues.h>
#include <gtsam/nonlinear/Values.h>
#include <list>
@ -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<iterator,bool> 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<Symbol> DynamicValues::keys() const {
FastList<Symbol> Values::keys() const {
FastList<Symbol> 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<size_t> DynamicValues::dims(const Ordering& ordering) const {
vector<size_t> Values::dims(const Ordering& ordering) const {
// vector<size_t> 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();
}

48
gtsam/nonlinear/DynamicValues.h → gtsam/nonlinear/Values.h
View File

@ -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<J> 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<J> if j is not present */
void erase(const Symbol& j);
@ -218,7 +218,7 @@ namespace gtsam {
FastList<Symbol> 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 <gtsam/nonlinear/DynamicValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>

231
gtsam/nonlinear/ValuesOld-inl.h
View File

@ -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 <utility>
#include <iostream>
#include <stdexcept>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/Lie-inl.h>
#include <gtsam/nonlinear/Ordering.h>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class J>
void ValuesOld<J>::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<class J>
bool ValuesOld<J>::equals(const ValuesOld<J>& 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<class J>
const typename J::Value& ValuesOld<J>::at(const J& j) const {
const_iterator it = values_.find(j);
if (it == values_.end()) throw KeyDoesNotExist<J>("retrieve", j);
else return it->second;
}
/* ************************************************************************* */
template<class J>
size_t ValuesOld<J>::dim() const {
size_t n = 0;
BOOST_FOREACH(const KeyValuePair& value, values_)
n += value.second.dim();
return n;
}
/* ************************************************************************* */
template<class J>
VectorValues ValuesOld<J>::zero(const Ordering& ordering) const {
return VectorValues::Zero(this->dims(ordering));
}
/* ************************************************************************* */
template<class J>
void ValuesOld<J>::insert(const J& name, const typename J::Value& val) {
if(!values_.insert(make_pair(name, val)).second)
throw KeyAlreadyExists<J>(name, val);
}
/* ************************************************************************* */
template<class J>
void ValuesOld<J>::insert(const ValuesOld<J>& cfg) {
BOOST_FOREACH(const KeyValuePair& v, cfg.values_)
insert(v.first, v.second);
}
/* ************************************************************************* */
template<class J>
void ValuesOld<J>::update(const ValuesOld<J>& cfg) {
BOOST_FOREACH(const KeyValuePair& v, values_) {
boost::optional<typename J::Value> t = cfg.exists_(v.first);
if (t) values_[v.first] = *t;
}
}
/* ************************************************************************* */
template<class J>
void ValuesOld<J>::update(const J& j, const typename J::Value& val) {
values_[j] = val;
}
/* ************************************************************************* */
template<class J>
std::list<J> ValuesOld<J>::keys() const {
std::list<J> ret;
BOOST_FOREACH(const KeyValuePair& v, values_)
ret.push_back(v.first);
return ret;
}
/* ************************************************************************* */
template<class J>
void ValuesOld<J>::erase(const J& j) {
size_t dim; // unused
erase(j, dim);
}
/* ************************************************************************* */
template<class J>
void ValuesOld<J>::erase(const J& j, size_t& dim) {
iterator it = values_.find(j);
if (it == values_.end())
throw KeyDoesNotExist<J>("erase", j);
dim = it->second.dim();
values_.erase(it);
}
/* ************************************************************************* */
// todo: insert for every element is inefficient
template<class J>
ValuesOld<J> ValuesOld<J>::retract(const VectorValues& delta, const Ordering& ordering) const {
ValuesOld<J> newValues;
typedef pair<J,typename J::Value> 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<class J>
std::vector<size_t> ValuesOld<J>::dims(const Ordering& ordering) const {
_ValuesDimensionCollector dimCollector(ordering);
this->apply(dimCollector);
return dimCollector.dimensions;
}
/* ************************************************************************* */
template<class J>
Ordering::shared_ptr ValuesOld<J>::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<class J>
// ValuesOld<J> ValuesOld<J>::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<J> newValues;
// int delta_offset = 0;
// typedef pair<J,typename J::Value> 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<class J>
inline VectorValues ValuesOld<J>::localCoordinates(const ValuesOld<J>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering));
localCoordinates(cp, ordering, delta);
return delta;
}
/* ************************************************************************* */
template<class J>
void ValuesOld<J>::localCoordinates(const ValuesOld<J>& cp, const Ordering& ordering, VectorValues& delta) const {
typedef pair<J,typename J::Value> KeyValue;
BOOST_FOREACH(const KeyValue& value, cp) {
assert(this->exists(value.first));
delta[ordering[value.first]] = this->at(value.first).localCoordinates(value.second);
}
}
/* ************************************************************************* */
template<class J>
const char* KeyDoesNotExist<J>::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<class J>
const char* KeyAlreadyExists<J>::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();
}
}

301
gtsam/nonlinear/ValuesOld.h
View File

@ -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 <set>
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/nonlinear/Ordering.h>
namespace boost { template<class T> class optional; }
namespace gtsam { class VectorValues; class Ordering; }
namespace gtsam {
// Forward declarations
template<class J> class KeyDoesNotExist;
template<class J> 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 J>
class ValuesOld {
public:
/**
* Typedefs
*/
typedef J Key;
typedef typename J::Value Value;
typedef std::map<Key, Value, std::less<Key>, boost::fast_pool_allocator<std::pair<const Key, Value> > > KeyValueMap;
// typedef FastMap<J,Value> 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<class J_ALT>
ValuesOld(const ValuesOld<J_ALT>& 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<J> if not found */
const Value& at(const J& j) const;
/** operator[] syntax for get, throws KeyDoesNotExist<J> 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<Value> 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<J> if j is already present */
void insert(const J& j, const Value& val);
/** Add a set of variables, throws KeyAlreadyExists<J> 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<J> 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<J> 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<J> 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 <key,value> pair. The operator must be able to handle such an
* iterator for every type in the ValuesOld, (i.e. through templating).
*/
template<typename A>
void apply(A& operation) {
for(iterator it = begin(); it != end(); ++it)
operation(it);
}
template<typename A>
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<size_t> 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<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(values_);
}
};
struct _ValuesDimensionCollector {
const Ordering& ordering;
std::vector<size_t> dimensions;
_ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {}
template<typename I> 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<typename I> void operator()(const I& key_value) {
ordering->insert(key_value->first, var);
++ var;
}
};
/* ************************************************************************* */
template<class J>
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 J>
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 <gtsam/nonlinear/ValuesOld-inl.h>

265
gtsam/nonlinear/tests/testDynamicValues.cpp
View File

@ -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 <CppUnitLite/TestHarness.h>
#include <stdexcept>
#include <limits>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/DynamicValues.h>
using namespace gtsam;
using namespace std;
static double inf = std::numeric_limits<double>::infinity();
typedef TypedSymbol<LieVector, 'v'> 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<Pose2, 'p'> 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<Pose2, 'x'> PoseKey;
DynamicValues config;
config.insert(PoseKey(1), Pose2());
config.insert(PoseKey(2), Pose2());
config.insert(PoseKey(4), Pose2());
config.insert(PoseKey(5), Pose2());
FastList<Symbol> expected, actual;
expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5);
actual = config.keys();
CHECK(actual.size() == expected.size());
FastList<Symbol>::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<const LieVector&> 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); }
/* ************************************************************************* */

6
gtsam/nonlinear/tests/testNonlinearFactor.cpp
View File

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

221
gtsam/nonlinear/tests/testValues.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file testValues.cpp
* @file testDynamicValues.cpp
* @author Richard Roberts
*/
@ -22,6 +22,7 @@ using namespace boost::assign;
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/Values.h>
using namespace gtsam;
@ -29,27 +30,26 @@ using namespace std;
static double inf = std::numeric_limits<double>::infinity();
typedef TypedSymbol<LieVector, 'v'> VecKey;
typedef Values<VecKey> 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<VecKey>);
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<string,Pose2> poseconfig;
poseconfig.insert("p1", Pose2(1,2,3));
poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
//poseconfig.print("poseconfig");
typedef TypedSymbol<Pose2, 'p'> 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<Pose2, 'x'> PoseKey;
TestValues<PoseKey, Pose2> config;
Values config;
config.insert(PoseKey(1), Pose2());
config.insert(PoseKey(2), Pose2());
config.insert(PoseKey(4), Pose2());
config.insert(PoseKey(5), Pose2());
list<PoseKey> expected, actual;
FastList<Symbol> expected, actual;
expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5);
actual = config.keys();
CHECK(actual.size() == expected.size());
list<PoseKey>::const_iterator itAct = actual.begin(), itExp = expected.begin();
FastList<Symbol>::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<LieVector> v = config0.exists_(key1);
boost::optional<const LieVector&> 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<LieVector, 'z'> Key;
typedef Values<Key> 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); }
/* ************************************************************************* */

4
gtsam/slam/BoundingConstraint.h
View File

@ -57,7 +57,7 @@ struct BoundingConstraint1: public NonlinearFactor1<KEY> {
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<KEY1, KEY2> {
boost::optional<Matrix&> 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_;

10
gtsam/slam/dataset.cpp
View File

@ -30,7 +30,7 @@ using namespace gtsam;
#define LINESIZE 81920
typedef boost::shared_ptr<Pose2Graph> sharedPose2Graph;
typedef boost::shared_ptr<DynamicValues> sharedPose2Values;
typedef boost::shared_ptr<Values> sharedPose2Values;
namespace gtsam {
@ -81,7 +81,7 @@ pair<sharedPose2Graph, sharedPose2Values> 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<sharedPose2Graph, sharedPose2Values> 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<Pose2>(it->first);
stream << "VERTEX2 " << it->first.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl;
}

6
gtsam/slam/dataset.h
View File

@ -41,16 +41,16 @@ std::pair<std::string, boost::optional<gtsam::SharedDiagonal> >
* @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<gtsam::Pose2Graph>, boost::shared_ptr<DynamicValues> > load2D(
std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<Values> > load2D(
std::pair<std::string, boost::optional<SharedDiagonal> > dataset,
int maxID = 0, bool addNoise=false, bool smart=true);
std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<DynamicValues> > load2D(
std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<Values> > load2D(
const std::string& filename,
boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(),
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);
/**

33
gtsam/slam/planarSLAM.h
View File

@ -40,37 +40,6 @@ namespace gtsam {
/// Typedef for a PointKey with Point2 data and 'l' symbol
typedef TypedSymbol<Point2, 'l'> 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<Graph> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA);

4
gtsam/slam/pose2SLAM.cpp
View File

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

2
gtsam/slam/pose2SLAM.h
View File

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

4
gtsam/slam/pose3SLAM.cpp
View File

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

2
gtsam/slam/pose3SLAM.h
View File

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

52
gtsam/slam/simulated2D.h
View File

@ -32,58 +32,6 @@ namespace gtsam {
typedef TypedSymbol<Point2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> 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<Point2> 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;

24
gtsam/slam/simulated2DOriented.h
View File

@ -32,30 +32,6 @@ namespace gtsam {
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'x'> 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

1
gtsam/slam/smallExample.h
View File

@ -28,7 +28,6 @@
namespace gtsam {
namespace example {
typedef simulated2D::Values Values;
typedef NonlinearFactorGraph Graph;
/**

12
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -97,7 +97,7 @@ TEST( GeneralSFMFactor, error ) {
boost::shared_ptr<Projection>
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<DynamicValues> values(new DynamicValues);
boost::shared_ptr<Values> 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<DynamicValues> values(new DynamicValues);
boost::shared_ptr<Values> 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<DynamicValues> values(new DynamicValues);
boost::shared_ptr<Values> 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<DynamicValues> values(new DynamicValues);
boost::shared_ptr<Values> 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<DynamicValues> values(new DynamicValues);
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(CameraKey((int)i), X[i]) ;

12
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -97,7 +97,7 @@ TEST( GeneralSFMFactor, error ) {
boost::shared_ptr<Projection>
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<DynamicValues> values(new DynamicValues);
boost::shared_ptr<Values> 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<DynamicValues> values(new DynamicValues);
boost::shared_ptr<Values> 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<DynamicValues> values(new DynamicValues);
boost::shared_ptr<Values> 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<DynamicValues> values(new DynamicValues);
boost::shared_ptr<Values> 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<DynamicValues> values(new DynamicValues);
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(CameraKey((int)i), X[i]) ;

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

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

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

@ -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<DynamicValues> initial(new DynamicValues());
boost::shared_ptr<Values> 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<DynamicValues> initial(new DynamicValues());
boost::shared_ptr<Values> 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<DynamicValues> initial(new DynamicValues());
boost::shared_ptr<Values> 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);

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

@ -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<DynamicValues> initial(new DynamicValues());
boost::shared_ptr<Values> 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));
}

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

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

7
gtsam/slam/tests/testSimulated2D.cpp
View File

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

2
gtsam/slam/tests/testSimulated2DOriented.cpp
View File

@ -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<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary());

2
gtsam/slam/tests/testSimulated3D.cpp
View File

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

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

@ -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<DynamicValues> values(new DynamicValues());
boost::shared_ptr<Values> values(new Values());
values->insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin
Point3 l1(0, 0, 0);

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

@ -92,7 +92,7 @@ TEST( Graph, optimizeLM)
graph->addPointConstraint(3, landmark3);
// Create an initial values structure corresponding to the ground truth
boost::shared_ptr<DynamicValues> initialEstimate(new DynamicValues);
boost::shared_ptr<Values> 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<DynamicValues> initialEstimate(new DynamicValues);
boost::shared_ptr<Values> 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<DynamicValues> initialEstimate(new DynamicValues);
boost::shared_ptr<Values> 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));
}

2
gtsam/slam/visualSLAM.h
View File

@ -38,7 +38,7 @@ namespace gtsam {
*/
typedef TypedSymbol<Pose3,'x'> PoseKey; ///< The key type used for poses
typedef TypedSymbol<Point3,'l'> PointKey; ///< The key type used for points
typedef boost::shared_ptr<DynamicValues> shared_values; ///< shared pointer to values data structure
typedef boost::shared_ptr<Values> shared_values; ///< shared pointer to values data structure
typedef NonlinearEquality<PoseKey> PoseConstraint; ///< put a hard constraint on a pose
typedef NonlinearEquality<PointKey> PointConstraint; ///< put a hard constraint on a point

30
tests/testBoundingConstraint.cpp
View File

@ -33,7 +33,7 @@ SharedDiagonal hard_model1 = noiseModel::Constrained::All(1);
typedef NonlinearFactorGraph Graph;
typedef boost::shared_ptr<Graph> shared_graph;
typedef boost::shared_ptr<simulated2D::Values> shared_values;
typedef boost::shared_ptr<Values> shared_values;
typedef NonlinearOptimizer<Graph> 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);

4
tests/testDoglegOptimizer.cpp
View File

@ -385,7 +385,7 @@ TEST(DoglegOptimizer, Iterate) {
// config far from minimum
Point2 x0(3,0);
boost::shared_ptr<simulated2D::Values> config(new example::Values);
boost::shared_ptr<Values> 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
}

12
tests/testExtendedKalmanFilter.cpp
View File

@ -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<GaussianFactor> linearize(const DynamicValues& c, const Ordering& ordering) const {
boost::shared_ptr<GaussianFactor> 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<GaussianFactor> linearize(const DynamicValues& c, const Ordering& ordering) const {
boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
const X& x1 = c[key_];
Matrix A1;
Vector b = -evaluateError(x1, A1);

80
tests/testGaussianISAM2.cpp
View File

@ -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<planarSLAM::Values>::Impl Impl;
// typedef GaussianISAM2<Values>::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<planarSLAM::Values>::Impl Impl;
// typedef GaussianISAM2<Values>::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<size_t> 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));

10
tests/testGaussianJunctionTree.cpp
View File

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

4
tests/testGraph.cpp
View File

@ -94,10 +94,10 @@ TEST( Graph, composePoses )
Pose2 rootPose = p2;
boost::shared_ptr<DynamicValues> actual = composePoses<Pose2Graph, Pose2Factor,
boost::shared_ptr<Values> actual = composePoses<Pose2Graph, Pose2Factor,
Pose2, pose2SLAM::Key> (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);

2
tests/testInference.cpp
View File

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

62
tests/testNonlinearEquality.cpp
View File

@ -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<DynamicValues> init(new DynamicValues());
boost::shared_ptr<Values> 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<DynamicValues> init(new DynamicValues());
boost::shared_ptr<Values> 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<Graph> shared_graph;
typedef boost::shared_ptr<DynamicValues> shared_values;
typedef boost::shared_ptr<Values> shared_values;
typedef NonlinearOptimizer<Graph> 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<Cal3_S2> shK(new Cal3_S2(K));
// typedefs for visual SLAM example
typedef boost::shared_ptr<DynamicValues> shared_vconfig;
typedef boost::shared_ptr<Values> shared_vconfig;
typedef visualSLAM::Graph VGraph;
typedef NonlinearOptimizer<VGraph> 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);

22
tests/testNonlinearFactor.cpp
View File

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

8
tests/testNonlinearFactorGraph.cpp
View File

@ -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<FactorGraph<GaussianFactor> > linearized = fg.linearize(initial, *initial.orderingArbitrary());
FactorGraph<GaussianFactor> expected = createGaussianFactorGraph(*initial.orderingArbitrary());
CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations

8
tests/testNonlinearISAM.cpp
View File

@ -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<nrPoses; ++i) {
PoseKey cur_key(i);
EXPECT(assert_equal(expected[cur_key], actual[cur_key], tol));

28
tests/testNonlinearOptimizer.cpp
View File

@ -113,7 +113,7 @@ TEST( NonlinearOptimizer, iterateLM )
// config far from minimum
Point2 x0(3,0);
boost::shared_ptr<example::Values> config(new example::Values);
boost::shared_ptr<Values> 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<example::Values> c0(new example::Values);
boost::shared_ptr<Values> 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<example::Values> c0(new example::Values);
boost::shared_ptr<Values> 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<example::Values> c0(new example::Values);
boost::shared_ptr<Values> 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<example::Graph>(
Values actualMFQR = optimize<example::Graph>(
fg, c0, *NonlinearOptimizationParameters().newFactorization(true), MULTIFRONTAL, LM);
DOUBLES_EQUAL(0,fg.error(actualMFQR),tol);
DynamicValues actualMFLDL = optimize<example::Graph>(
Values actualMFLDL = optimize<example::Graph>(
fg, c0, *NonlinearOptimizationParameters().newFactorization(false), MULTIFRONTAL, LM);
DOUBLES_EQUAL(0,fg.error(actualMFLDL),tol);
}
@ -255,7 +255,7 @@ TEST( NonlinearOptimizer, Factorization )
{
typedef NonlinearOptimizer<Pose2Graph, GaussianFactorGraph, GaussianSequentialSolver > Optimizer;
boost::shared_ptr<DynamicValues> config(new DynamicValues);
boost::shared_ptr<Values> 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<example::Values> c0(new example::Values);
boost::shared_ptr<Values> c0(new Values);
c0->insert(simulated2D::PoseKey(1), x0);
DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3);

6
tests/testPose2SLAMwSPCG.cpp
View File

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

6
tests/testRot3Optimization.cpp
View File

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

10
tests/testSerialization.cpp
View File

@ -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>(PoseKey(2)));
EXPECT(equalsObj<PointKey>(PointKey(3)));
EXPECT(equalsObj<planarSLAM::Values>(values));
EXPECT(equalsObj<Values>(values));
EXPECT(equalsObj<Prior>(prior));
EXPECT(equalsObj<Bearing>(bearing));
EXPECT(equalsObj<BearingRange>(bearingRange));
@ -502,7 +502,7 @@ TEST (Serialization, planar_system) {
// xml
EXPECT(equalsXML<PoseKey>(PoseKey(2)));
EXPECT(equalsXML<PointKey>(PointKey(3)));
EXPECT(equalsXML<planarSLAM::Values>(values));
EXPECT(equalsXML<Values>(values));
EXPECT(equalsXML<Prior>(prior));
EXPECT(equalsXML<Bearing>(bearing));
EXPECT(equalsXML<BearingRange>(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();

2
tests/timeMultifrontalOnDataset.cpp
View File

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

2
tests/timeSequentialOnDataset.cpp
View File

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