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

Changed LieValues->Values. Did not change Values typedefs in slam domains, just adjusted namespaces

release/4.3a0
Alex Cunningham 2011-11-06 19:08:42 +00:00
parent 929d5259c9
commit 5798868ab7
54 changed files with 385 additions and 445 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/CameraResectioning.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/LieValues.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization-inl.h> #include <gtsam/nonlinear/NonlinearOptimization-inl.h>
@ -28,7 +28,7 @@
using namespace gtsam; using namespace gtsam;
typedef TypedSymbol<Pose3, 'x'> PoseKey; typedef TypedSymbol<Pose3, 'x'> PoseKey;
typedef LieValues<PoseKey> PoseValues; typedef Values<PoseKey> PoseValues;
/** /**
* Unary factor for the pose. * Unary factor for the pose.

4
examples/PlanarSLAMExample_easy.cpp
View File

@ -76,7 +76,7 @@ int main(int argc, char** argv) {
graph.print("full graph"); graph.print("full graph");
// initialize to noisy points // initialize to noisy points
Values initialEstimate; planarSLAM::Values initialEstimate;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
@ -86,7 +86,7 @@ int main(int argc, char** argv) {
initialEstimate.print("initial estimate"); initialEstimate.print("initial estimate");
// optimize using Levenberg-Marquardt optimization with an ordering from colamd // optimize using Levenberg-Marquardt optimization with an ordering from colamd
Values result = optimize<Graph, Values>(graph, initialEstimate); planarSLAM::Values result = optimize<Graph, planarSLAM::Values>(graph, initialEstimate);
result.print("final result"); result.print("final result");
return 0; return 0;

26
examples/PlanarSLAMSelfContained_advanced.cpp
View File

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

4
examples/Pose2SLAMExample_advanced.cpp
View File

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

4
examples/Pose2SLAMExample_easy.cpp
View File

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

10
examples/Pose2SLAMwSPCG_advanced.cpp
View File

@ -27,17 +27,17 @@ using namespace gtsam;
using namespace pose2SLAM; using namespace pose2SLAM;
typedef boost::shared_ptr<Graph> sharedGraph ; typedef boost::shared_ptr<Graph> sharedGraph ;
typedef boost::shared_ptr<Values> sharedValue ; typedef boost::shared_ptr<pose2SLAM::Values> sharedValue ;
//typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, SubgraphSolver<Graph,Values> > SPCGOptimizer; //typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, SubgraphSolver<Graph,Values> > SPCGOptimizer;
typedef SubgraphSolver<Graph, GaussianFactorGraph, Values> Solver; typedef SubgraphSolver<Graph, GaussianFactorGraph, pose2SLAM::Values> Solver;
typedef boost::shared_ptr<Solver> sharedSolver ; typedef boost::shared_ptr<Solver> sharedSolver ;
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, Solver> SPCGOptimizer; typedef NonlinearOptimizer<Graph, pose2SLAM::Values, GaussianFactorGraph, Solver> SPCGOptimizer;
sharedGraph graph; sharedGraph graph;
sharedValue initial; sharedValue initial;
Values result; pose2SLAM::Values result;
/* ************************************************************************* */ /* ************************************************************************* */
int main(void) { 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); Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
graph = boost::make_shared<Graph>() ; graph = boost::make_shared<Graph>() ;
initial = boost::make_shared<Values>() ; initial = boost::make_shared<pose2SLAM::Values>() ;
// create a 3 by 3 grid // create a 3 by 3 grid
// x3 x6 x9 // x3 x6 x9

3
examples/Pose2SLAMwSPCG_easy.cpp
View File

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

28
examples/SimpleRotation.cpp
View File

@ -23,12 +23,12 @@
#include <gtsam/geometry/Rot2.h> #include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/Key.h> #include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/LieValues-inl.h> #include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h> #include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimization-inl.h> #include <gtsam/nonlinear/NonlinearOptimization-inl.h>
/* /*
* TODO: make factors independent of Values * TODO: make factors independent of RotValues
* TODO: make toplevel documentation * TODO: make toplevel documentation
* TODO: Clean up nonlinear optimization API * TODO: Clean up nonlinear optimization API
*/ */
@ -44,17 +44,17 @@ using namespace gtsam;
* *
* To create a domain: * To create a domain:
* - variable types need to have a key defined to act as a label in graphs * - variable types need to have a key defined to act as a label in graphs
* - a "Values" structure needs to be defined to store the system state * - a "RotValues" structure needs to be defined to store the system state
* - a graph container acting on a given Values * - a graph container acting on a given RotValues
* *
* In a typical scenario, these typedefs could be placed in a header * In a typical scenario, these typedefs could be placed in a header
* file and reused between projects. Also, LieValues can be combined to * file and reused between projects. Also, RotValues can be combined to
* form a "TupleValues" to enable multiple variable types, such as 2D points * form a "TupleValues" to enable multiple variable types, such as 2D points
* and 2D poses. * and 2D poses.
*/ */
typedef TypedSymbol<Rot2, 'x'> Key; /// Variable labels for a specific type typedef TypedSymbol<Rot2, 'x'> Key; /// Variable labels for a specific type
typedef LieValues<Key> Values; /// Class to store values - acts as a state for the system typedef Values<Key> RotValues; /// Class to store values - acts as a state for the system
typedef NonlinearFactorGraph<Values> Graph; /// Graph container for constraints - needs to know type of variables typedef NonlinearFactorGraph<RotValues> Graph; /// Graph container for constraints - needs to know type of variables
const double degree = M_PI / 180; const double degree = M_PI / 180;
@ -71,7 +71,7 @@ int main() {
* with a model of the noise on the measurement. * with a model of the noise on the measurement.
* *
* The "Key" created here is a label used to associate parts of the * The "Key" created here is a label used to associate parts of the
* state (stored in "Values") with particular factors. They require * state (stored in "RotValues") with particular factors. They require
* an index to allow for lookup, and should be unique. * an index to allow for lookup, and should be unique.
* *
* In general, creating a factor requires: * In general, creating a factor requires:
@ -83,7 +83,7 @@ int main() {
prior.print("goal angle"); prior.print("goal angle");
SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree); SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree);
Key key(1); Key key(1);
PriorFactor<Values, Key> factor(key, prior, model); PriorFactor<RotValues, Key> factor(key, prior, model);
/** /**
* Step 3: create a graph container and add the factor to it * Step 3: create a graph container and add the factor to it
@ -101,7 +101,7 @@ int main() {
/** /**
* Step 4: create an initial estimate * Step 4: create an initial estimate
* An initial estimate of the solution for the system is necessary to * An initial estimate of the solution for the system is necessary to
* start optimization. This system state is the "Values" structure, * start optimization. This system state is the "RotValues" structure,
* which is similar in structure to a STL map, in that it maps * which is similar in structure to a STL map, in that it maps
* keys (the label created in step 1) to specific values. * keys (the label created in step 1) to specific values.
* *
@ -110,11 +110,11 @@ int main() {
* all of the variables in the graph have a corresponding value in * all of the variables in the graph have a corresponding value in
* this structure. * this structure.
* *
* The interface to all Values types is the same, it only depends * The interface to all RotValues types is the same, it only depends
* on the type of key used to find the appropriate value map if there * on the type of key used to find the appropriate value map if there
* are multiple types of variables. * are multiple types of variables.
*/ */
Values initial; RotValues initial;
initial.insert(key, Rot2::fromAngle(20 * degree)); initial.insert(key, Rot2::fromAngle(20 * degree));
initial.print("initial estimate"); initial.print("initial estimate");
@ -123,10 +123,10 @@ int main() {
* After formulating the problem with a graph of constraints * After formulating the problem with a graph of constraints
* and an initial estimate, executing optimization is as simple * and an initial estimate, executing optimization is as simple
* as calling a general optimization function with the graph and * as calling a general optimization function with the graph and
* initial estimate. This will yield a new Values structure * initial estimate. This will yield a new RotValues structure
* with the final state of the optimization. * with the final state of the optimization.
*/ */
Values result = optimize<Graph, Values>(graph, initial); RotValues result = optimize<Graph, RotValues>(graph, initial);
result.print("final result"); result.print("final result");
return 0; return 0;

4
examples/easyPoint2KalmanFilter.cpp
View File

@ -24,7 +24,7 @@
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h> #include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LieValues-inl.h> #include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
using namespace std; using namespace std;
@ -32,7 +32,7 @@ using namespace gtsam;
// Define Types for Linear System Test // Define Types for Linear System Test
typedef TypedSymbol<Point2, 'x'> LinearKey; typedef TypedSymbol<Point2, 'x'> LinearKey;
typedef LieValues<LinearKey> LinearValues; typedef Values<LinearKey> LinearValues;
typedef Point2 LinearMeasurement; typedef Point2 LinearMeasurement;
int main() { int main() {

20
examples/elaboratePoint2KalmanFilter.cpp
View File

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

10
examples/vSLAMexample/vISAMexample.cpp
View File

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

8
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. * Create a structure of initial estimates for all nodes (landmarks and poses) in the graph.
* The returned Values structure contains all initial values for all nodes. * The returned Values structure contains all initial values for all nodes.
*/ */
Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) { visualSLAM::Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
Values initValues; visualSLAM::Values initValues;
// Initialize landmarks 3D positions. // Initialize landmarks 3D positions.
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++) for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
@ -136,7 +136,7 @@ int main(int argc, char* argv[]) {
boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib))); boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
// Create an initial Values structure using groundtruth values as the initial estimates // Create an initial Values structure using groundtruth values as the initial estimates
boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses))); boost::shared_ptr<visualSLAM::Values> initialEstimates(new visualSLAM::Values(initialize(g_landmarks, g_poses)));
cout << "*******************************************************" << endl; cout << "*******************************************************" << endl;
initialEstimates->print("INITIAL ESTIMATES: "); initialEstimates->print("INITIAL ESTIMATES: ");
@ -148,7 +148,7 @@ int main(int argc, char* argv[]) {
// Optimize the graph // Optimize the graph
cout << "*******************************************************" << endl; cout << "*******************************************************" << endl;
NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newVerbosity(Optimizer::Parameters::DAMPED); NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newVerbosity(Optimizer::Parameters::DAMPED);
Optimizer::shared_values result = Optimizer::optimizeGN(graph, initialEstimates, params); visualSLAM::Optimizer::shared_values result = visualSLAM::Optimizer::optimizeGN(graph, initialEstimates, params);
// Print final results // Print final results
cout << "*******************************************************" << endl; cout << "*******************************************************" << endl;

2
gtsam/nonlinear/DoglegOptimizer.h
View File

@ -15,7 +15,7 @@ namespace gtsam {
* This class is functional, meaning every method is \c const, and returns a new * This class is functional, meaning every method is \c const, and returns a new
* copy of the class. * copy of the class.
* *
* \tparam VALUES The LieValues or TupleValues type to hold the values to be * \tparam VALUES The Values or TupleValues type to hold the values to be
* estimated. * estimated.
* *
* \tparam GAUSSIAN_SOLVER The linear solver to use at each iteration, * \tparam GAUSSIAN_SOLVER The linear solver to use at each iteration,

4
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -18,7 +18,7 @@ namespace gtsam {
* here matches that in "trustregion.pdf" in gtsam_experimental/doc, see this * here matches that in "trustregion.pdf" in gtsam_experimental/doc, see this
* file for further explanation of the computations performed by this class. * file for further explanation of the computations performed by this class.
* *
* \tparam VALUES The LieValues or TupleValues type to hold the values to be * \tparam VALUES The Values or TupleValues type to hold the values to be
* estimated. * estimated.
* *
* \tparam GAUSSIAN_SOLVER The linear solver to use at each iteration, * \tparam GAUSSIAN_SOLVER The linear solver to use at each iteration,
@ -60,7 +60,7 @@ struct DoglegOptimizerImpl {
* @tparam M The type of the Bayes' net or tree, currently * @tparam M The type of the Bayes' net or tree, currently
* either BayesNet<GaussianConditional> (or GaussianBayesNet) or BayesTree<GaussianConditional>. * either BayesNet<GaussianConditional> (or GaussianBayesNet) or BayesTree<GaussianConditional>.
* @tparam F For normal usage this will be NonlinearFactorGraph<VALUES>. * @tparam F For normal usage this will be NonlinearFactorGraph<VALUES>.
* @tparam VALUES The LieValues or TupleValues to pass to F::error() to evaluate * @tparam VALUES The Values or TupleValues to pass to F::error() to evaluate
* the error function. * the error function.
* @param initialDelta The initial trust region radius. * @param initialDelta The initial trust region radius.
* @param Rd The Bayes' net or tree as described above. * @param Rd The Bayes' net or tree as described above.

2
gtsam/nonlinear/GaussianISAM2.h
View File

@ -24,7 +24,7 @@ namespace gtsam {
* GaussianConditional, and the values on which that GaussianISAM2 is * GaussianConditional, and the values on which that GaussianISAM2 is
* templated. * templated.
* *
* @tparam VALUES The LieValues or TupleValues\Emph{N} that contains the * @tparam VALUES The Values or TupleValues\Emph{N} that contains the
* variables. * variables.
*/ */
template <class VALUES> template <class VALUES>

4
gtsam/nonlinear/Makefile.am
View File

@ -16,8 +16,8 @@ check_PROGRAMS =
#---------------------------------------------------------------------------------------------------- #----------------------------------------------------------------------------------------------------
# Lie Groups # Lie Groups
headers += LieValues.h LieValues-inl.h TupleValues.h TupleValues-inl.h headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h
check_PROGRAMS += tests/testLieValues tests/testKey tests/testOrdering check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering
# Nonlinear nonlinear # Nonlinear nonlinear
headers += Key.h headers += Key.h

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

@ -18,7 +18,7 @@
#pragma once #pragma once
#include <gtsam/nonlinear/LieValues-inl.h> #include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/TupleValues.h> #include <gtsam/nonlinear/TupleValues.h>
// TupleValues instantiations for N = 1-6 // TupleValues instantiations for N = 1-6

6
gtsam/nonlinear/TupleValues.h
View File

@ -16,7 +16,7 @@
* @author Alex Cunningham * @author Alex Cunningham
*/ */
#include <gtsam/nonlinear/LieValues.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#pragma once #pragma once
@ -24,11 +24,11 @@
namespace gtsam { namespace gtsam {
/** /**
* TupleValues are a structure to manage heterogenous LieValues, so as to * TupleValues are a structure to manage heterogenous Values, so as to
* enable different types of variables/keys to be used simultaneously. We * enable different types of variables/keys to be used simultaneously. We
* do this with recursive templates (instead of blind pointer casting) to * do this with recursive templates (instead of blind pointer casting) to
* reduce run-time overhead and keep static type checking. The interface * reduce run-time overhead and keep static type checking. The interface
* mimics that of a single LieValues. * mimics that of a single Values.
* *
* This uses a recursive structure of values pairs to form a lisp-like * This uses a recursive structure of values pairs to form a lisp-like
* list, with a special case (TupleValuesEnd) that contains only one values * list, with a special case (TupleValuesEnd) that contains only one values

54
gtsam/nonlinear/LieValues-inl.h → gtsam/nonlinear/Values-inl.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file LieValues.cpp * @file Values.cpp
* @author Richard Roberts * @author Richard Roberts
*/ */
@ -28,9 +28,9 @@
#include <gtsam/base/Lie-inl.h> #include <gtsam/base/Lie-inl.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/LieValues.h> #include <gtsam/nonlinear/Values.h>
#define INSTANTIATE_LIE_VALUES(J) template class LieValues<J>; #define INSTANTIATE_VALUES(J) template class Values<J>;
using namespace std; using namespace std;
@ -38,8 +38,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void LieValues<J>::print(const string &s) const { void Values<J>::print(const string &s) const {
cout << "LieValues " << s << ", size " << values_.size() << "\n"; cout << "Values " << s << ", size " << values_.size() << "\n";
BOOST_FOREACH(const KeyValuePair& v, values_) { BOOST_FOREACH(const KeyValuePair& v, values_) {
gtsam::print(v.second, (string)(v.first)); gtsam::print(v.second, (string)(v.first));
} }
@ -47,7 +47,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
bool LieValues<J>::equals(const LieValues<J>& expected, double tol) const { bool Values<J>::equals(const Values<J>& expected, double tol) const {
if (values_.size() != expected.values_.size()) return false; if (values_.size() != expected.values_.size()) return false;
BOOST_FOREACH(const KeyValuePair& v, values_) { BOOST_FOREACH(const KeyValuePair& v, values_) {
if (!expected.exists(v.first)) return false; if (!expected.exists(v.first)) return false;
@ -59,15 +59,15 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
const typename J::Value& LieValues<J>::at(const J& j) const { const typename J::Value& Values<J>::at(const J& j) const {
const_iterator it = values_.find(j); const_iterator it = values_.find(j);
if (it == values_.end()) throw std::invalid_argument("LieValues::at() invalid j: " + (string)j); if (it == values_.end()) throw std::invalid_argument("Values::at() invalid j: " + (string)j);
else return it->second; else return it->second;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
size_t LieValues<J>::dim() const { size_t Values<J>::dim() const {
size_t n = 0; size_t n = 0;
BOOST_FOREACH(const KeyValuePair& value, values_) BOOST_FOREACH(const KeyValuePair& value, values_)
n += value.second.dim(); n += value.second.dim();
@ -76,26 +76,26 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
VectorValues LieValues<J>::zero(const Ordering& ordering) const { VectorValues Values<J>::zero(const Ordering& ordering) const {
return VectorValues::Zero(this->dims(ordering)); return VectorValues::Zero(this->dims(ordering));
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void LieValues<J>::insert(const J& name, const typename J::Value& val) { void Values<J>::insert(const J& name, const typename J::Value& val) {
values_.insert(make_pair(name, val)); values_.insert(make_pair(name, val));
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void LieValues<J>::insert(const LieValues<J>& cfg) { void Values<J>::insert(const Values<J>& cfg) {
BOOST_FOREACH(const KeyValuePair& v, cfg.values_) BOOST_FOREACH(const KeyValuePair& v, cfg.values_)
insert(v.first, v.second); insert(v.first, v.second);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void LieValues<J>::update(const LieValues<J>& cfg) { void Values<J>::update(const Values<J>& cfg) {
BOOST_FOREACH(const KeyValuePair& v, values_) { BOOST_FOREACH(const KeyValuePair& v, values_) {
boost::optional<typename J::Value> t = cfg.exists_(v.first); boost::optional<typename J::Value> t = cfg.exists_(v.first);
if (t) values_[v.first] = *t; if (t) values_[v.first] = *t;
@ -104,13 +104,13 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void LieValues<J>::update(const J& j, const typename J::Value& val) { void Values<J>::update(const J& j, const typename J::Value& val) {
values_[j] = val; values_[j] = val;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
std::list<J> LieValues<J>::keys() const { std::list<J> Values<J>::keys() const {
std::list<J> ret; std::list<J> ret;
BOOST_FOREACH(const KeyValuePair& v, values_) BOOST_FOREACH(const KeyValuePair& v, values_)
ret.push_back(v.first); ret.push_back(v.first);
@ -119,16 +119,16 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void LieValues<J>::erase(const J& j) { void Values<J>::erase(const J& j) {
size_t dim; // unused size_t dim; // unused
erase(j, dim); erase(j, dim);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void LieValues<J>::erase(const J& j, size_t& dim) { void Values<J>::erase(const J& j, size_t& dim) {
iterator it = values_.find(j); iterator it = values_.find(j);
if (it == values_.end()) throw std::invalid_argument("LieValues::erase() invalid j: " + (string)j); if (it == values_.end()) throw std::invalid_argument("Values::erase() invalid j: " + (string)j);
dim = it->second.dim(); dim = it->second.dim();
values_.erase(it); values_.erase(it);
} }
@ -136,8 +136,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// todo: insert for every element is inefficient // todo: insert for every element is inefficient
template<class J> template<class J>
LieValues<J> LieValues<J>::retract(const VectorValues& delta, const Ordering& ordering) const { Values<J> Values<J>::retract(const VectorValues& delta, const Ordering& ordering) const {
LieValues<J> newValues; Values<J> newValues;
typedef pair<J,typename J::Value> KeyValue; typedef pair<J,typename J::Value> KeyValue;
BOOST_FOREACH(const KeyValue& value, this->values_) { BOOST_FOREACH(const KeyValue& value, this->values_) {
const J& j = value.first; const J& j = value.first;
@ -153,7 +153,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
std::vector<size_t> LieValues<J>::dims(const Ordering& ordering) const { std::vector<size_t> Values<J>::dims(const Ordering& ordering) const {
_ValuesDimensionCollector dimCollector(ordering); _ValuesDimensionCollector dimCollector(ordering);
this->apply(dimCollector); this->apply(dimCollector);
return dimCollector.dimensions; return dimCollector.dimensions;
@ -161,7 +161,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
Ordering::shared_ptr LieValues<J>::orderingArbitrary(Index firstVar) const { Ordering::shared_ptr Values<J>::orderingArbitrary(Index firstVar) const {
// Generate an initial key ordering in iterator order // Generate an initial key ordering in iterator order
_ValuesKeyOrderer keyOrderer(firstVar); _ValuesKeyOrderer keyOrderer(firstVar);
this->apply(keyOrderer); this->apply(keyOrderer);
@ -171,12 +171,12 @@ namespace gtsam {
// /* ************************************************************************* */ // /* ************************************************************************* */
// // todo: insert for every element is inefficient // // todo: insert for every element is inefficient
// template<class J> // template<class J>
// LieValues<J> LieValues<J>::retract(const Vector& delta) const { // Values<J> Values<J>::retract(const Vector& delta) const {
// if(delta.size() != dim()) { // if(delta.size() != dim()) {
// cout << "LieValues::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl; // cout << "Values::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl;
// throw invalid_argument("Delta vector length does not match config dimensionality."); // throw invalid_argument("Delta vector length does not match config dimensionality.");
// } // }
// LieValues<J> newValues; // Values<J> newValues;
// int delta_offset = 0; // int delta_offset = 0;
// typedef pair<J,typename J::Value> KeyValue; // typedef pair<J,typename J::Value> KeyValue;
// BOOST_FOREACH(const KeyValue& value, this->values_) { // BOOST_FOREACH(const KeyValue& value, this->values_) {
@ -193,7 +193,7 @@ namespace gtsam {
// todo: insert for every element is inefficient // todo: insert for every element is inefficient
// todo: currently only logmaps elements in both configs // todo: currently only logmaps elements in both configs
template<class J> template<class J>
inline VectorValues LieValues<J>::localCoordinates(const LieValues<J>& cp, const Ordering& ordering) const { inline VectorValues Values<J>::localCoordinates(const Values<J>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering)); VectorValues delta(this->dims(ordering));
localCoordinates(cp, ordering, delta); localCoordinates(cp, ordering, delta);
return delta; return delta;
@ -201,7 +201,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void LieValues<J>::localCoordinates(const LieValues<J>& cp, const Ordering& ordering, VectorValues& delta) const { void Values<J>::localCoordinates(const Values<J>& cp, const Ordering& ordering, VectorValues& delta) const {
typedef pair<J,typename J::Value> KeyValue; typedef pair<J,typename J::Value> KeyValue;
BOOST_FOREACH(const KeyValue& value, cp) { BOOST_FOREACH(const KeyValue& value, cp) {
assert(this->exists(value.first)); assert(this->exists(value.first));

28
gtsam/nonlinear/LieValues.h → gtsam/nonlinear/Values.h
View File

@ -10,14 +10,14 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file LieValues.h * @file Values.h
* @author Richard Roberts * @author Richard Roberts
* *
* @brief A templated config for Lie-group elements * @brief A templated config for Lie-group elements
* *
* Detailed story: * Detailed story:
* A values structure is a map from keys to values. It is used to specify the value of a bunch * 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 LieValues is a values structure which can hold variables that * of variables in a factor graph. A Values is a values structure which can hold variables that
* are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type * 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. * which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
*/ */
@ -47,7 +47,7 @@ namespace gtsam {
* labels (example: Pose2, Point2, etc) * labels (example: Pose2, Point2, etc)
*/ */
template<class J> template<class J>
class LieValues { class Values {
public: public:
@ -72,18 +72,18 @@ namespace gtsam {
public: public:
LieValues() {} Values() {}
LieValues(const LieValues& config) : Values(const Values& config) :
values_(config.values_) {} values_(config.values_) {}
template<class J_ALT> template<class J_ALT>
LieValues(const LieValues<J_ALT>& other) {} // do nothing when initializing with wrong type Values(const Values<J_ALT>& other) {} // do nothing when initializing with wrong type
virtual ~LieValues() {} virtual ~Values() {}
/** print */ /** print */
void print(const std::string &s="") const; void print(const std::string &s="") const;
/** Test whether configs are identical in keys and values */ /** Test whether configs are identical in keys and values */
bool equals(const LieValues& expected, double tol=1e-9) const; bool equals(const Values& expected, double tol=1e-9) const;
/** Retrieve a variable by j, throws std::invalid_argument if not found */ /** Retrieve a variable by j, throws std::invalid_argument if not found */
const Value& at(const J& j) const; const Value& at(const J& j) const;
@ -120,13 +120,13 @@ namespace gtsam {
size_t dim() const; size_t dim() const;
/** Add a delta config to current config and returns a new config */ /** Add a delta config to current config and returns a new config */
LieValues 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) */ /** Get a delta config about a linearization point c0 (*this) */
VectorValues localCoordinates(const LieValues& cp, const Ordering& ordering) const; VectorValues localCoordinates(const Values& cp, const Ordering& ordering) const;
/** Get a delta config about a linearization point c0 (*this) */ /** Get a delta config about a linearization point c0 (*this) */
void localCoordinates(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const; void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const;
// imperative methods: // imperative methods:
@ -134,10 +134,10 @@ namespace gtsam {
void insert(const J& j, const Value& val); void insert(const J& j, const Value& val);
/** Add a set of variables - does note replace existing values */ /** Add a set of variables - does note replace existing values */
void insert(const LieValues& cfg); void insert(const Values& cfg);
/** update the current available values without adding new ones */ /** update the current available values without adding new ones */
void update(const LieValues& cfg); void update(const Values& cfg);
/** single element change of existing element */ /** single element change of existing element */
void update(const J& j, const Value& val); void update(const J& j, const Value& val);
@ -157,7 +157,7 @@ namespace gtsam {
std::list<J> keys() const; std::list<J> keys() const;
/** Replace all keys and variables */ /** Replace all keys and variables */
LieValues& operator=(const LieValues& rhs) { Values& operator=(const Values& rhs) {
values_ = rhs.values_; values_ = rhs.values_;
return (*this); return (*this);
} }

80
gtsam/nonlinear/tests/testLieValues.cpp → gtsam/nonlinear/tests/testValues.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file testLieValues.cpp * @file testValues.cpp
* @author Richard Roberts * @author Richard Roberts
*/ */
@ -22,32 +22,32 @@ using namespace boost::assign;
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/LieVector.h> #include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/LieValues-inl.h> #include <gtsam/nonlinear/Values-inl.h>
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
static double inf = std::numeric_limits<double>::infinity(); static double inf = std::numeric_limits<double>::infinity();
typedef TypedSymbol<LieVector, 'v'> VecKey; typedef TypedSymbol<LieVector, 'v'> VecKey;
typedef LieValues<VecKey> Values; typedef Values<VecKey> TestValues;
VecKey key1(1), key2(2), key3(3), key4(4); VecKey key1(1), key2(2), key3(3), key4(4);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieValues, equals1 ) TEST( TestValues, equals1 )
{ {
Values expected; TestValues expected;
Vector v = Vector_(3, 5.0, 6.0, 7.0); Vector v = Vector_(3, 5.0, 6.0, 7.0);
expected.insert(key1,v); expected.insert(key1,v);
Values actual; TestValues actual;
actual.insert(key1,v); actual.insert(key1,v);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieValues, equals2 ) TEST( TestValues, equals2 )
{ {
Values cfg1, cfg2; TestValues cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 5.0, 6.0, 8.0); Vector v2 = Vector_(3, 5.0, 6.0, 8.0);
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
@ -57,9 +57,9 @@ TEST( LieValues, equals2 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieValues, equals_nan ) TEST( TestValues, equals_nan )
{ {
Values cfg1, cfg2; TestValues cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, inf, inf, inf); Vector v2 = Vector_(3, inf, inf, inf);
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
@ -69,9 +69,9 @@ TEST( LieValues, equals_nan )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieValues, insert_config ) TEST( TestValues, insert_config )
{ {
Values cfg1, cfg2, expected; TestValues cfg1, cfg2, expected;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 8.0, 9.0, 1.0); Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
Vector v3 = Vector_(3, 2.0, 4.0, 3.0); Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
@ -92,9 +92,9 @@ TEST( LieValues, insert_config )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LieValues, update_element ) TEST( TestValues, update_element )
{ {
Values cfg; TestValues cfg;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0); Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 8.0, 9.0, 1.0); Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
@ -108,9 +108,9 @@ TEST( LieValues, update_element )
} }
///* ************************************************************************* */ ///* ************************************************************************* */
//TEST(LieValues, dim_zero) //TEST(TestValues, dim_zero)
//{ //{
// Values config0; // TestValues config0;
// config0.insert(key1, Vector_(2, 2.0, 3.0)); // config0.insert(key1, Vector_(2, 2.0, 3.0));
// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); // config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
// LONGS_EQUAL(5,config0.dim()); // LONGS_EQUAL(5,config0.dim());
@ -122,9 +122,9 @@ TEST( LieValues, update_element )
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieValues, expmap_a) TEST(TestValues, expmap_a)
{ {
Values config0; TestValues config0;
config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
@ -133,7 +133,7 @@ TEST(LieValues, expmap_a)
increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2); increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2);
increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
Values expected; TestValues expected;
expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
@ -141,9 +141,9 @@ TEST(LieValues, expmap_a)
} }
///* ************************************************************************* */ ///* ************************************************************************* */
//TEST(LieValues, expmap_b) //TEST(TestValues, expmap_b)
//{ //{
// Values config0; // TestValues config0;
// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); // config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); // config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
// //
@ -151,7 +151,7 @@ TEST(LieValues, expmap_a)
// VectorValues increment(config0.dims(ordering)); // VectorValues increment(config0.dims(ordering));
// increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); // increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
// //
// Values expected; // TestValues expected;
// expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); // expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); // expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
// //
@ -159,9 +159,9 @@ TEST(LieValues, expmap_a)
//} //}
///* ************************************************************************* */ ///* ************************************************************************* */
//TEST(LieValues, expmap_c) //TEST(TestValues, expmap_c)
//{ //{
// Values config0; // TestValues config0;
// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); // config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); // config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
// //
@ -169,7 +169,7 @@ TEST(LieValues, expmap_a)
// 1.0, 1.1, 1.2, // 1.0, 1.1, 1.2,
// 1.3, 1.4, 1.5); // 1.3, 1.4, 1.5);
// //
// Values expected; // TestValues expected;
// expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); // expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); // expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
// //
@ -177,16 +177,16 @@ TEST(LieValues, expmap_a)
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
/*TEST(LieValues, expmap_d) /*TEST(TestValues, expmap_d)
{ {
Values config0; TestValues config0;
config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
//config0.print("config0"); //config0.print("config0");
CHECK(equal(config0, config0)); CHECK(equal(config0, config0));
CHECK(config0.equals(config0)); CHECK(config0.equals(config0));
LieValues<string,Pose2> poseconfig; TestValues<string,Pose2> poseconfig;
poseconfig.insert("p1", Pose2(1,2,3)); poseconfig.insert("p1", Pose2(1,2,3));
poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5)); poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
//poseconfig.print("poseconfig"); //poseconfig.print("poseconfig");
@ -195,10 +195,10 @@ TEST(LieValues, expmap_a)
}*/ }*/
/* ************************************************************************* */ /* ************************************************************************* */
/*TEST(LieValues, extract_keys) /*TEST(TestValues, extract_keys)
{ {
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
LieValues<PoseKey, Pose2> config; TestValues<PoseKey, Pose2> config;
config.insert(PoseKey(1), Pose2()); config.insert(PoseKey(1), Pose2());
config.insert(PoseKey(2), Pose2()); config.insert(PoseKey(2), Pose2());
@ -217,9 +217,9 @@ TEST(LieValues, expmap_a)
}*/ }*/
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieValues, exists_) TEST(TestValues, exists_)
{ {
Values config0; TestValues config0;
config0.insert(key1, Vector_(1, 1.)); config0.insert(key1, Vector_(1, 1.));
config0.insert(key2, Vector_(1, 2.)); config0.insert(key2, Vector_(1, 2.));
@ -228,40 +228,40 @@ TEST(LieValues, exists_)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieValues, update) TEST(TestValues, update)
{ {
Values config0; TestValues config0;
config0.insert(key1, Vector_(1, 1.)); config0.insert(key1, Vector_(1, 1.));
config0.insert(key2, Vector_(1, 2.)); config0.insert(key2, Vector_(1, 2.));
Values superset; TestValues superset;
superset.insert(key1, Vector_(1, -1.)); superset.insert(key1, Vector_(1, -1.));
superset.insert(key2, Vector_(1, -2.)); superset.insert(key2, Vector_(1, -2.));
superset.insert(key3, Vector_(1, -3.)); superset.insert(key3, Vector_(1, -3.));
config0.update(superset); config0.update(superset);
Values expected; TestValues expected;
expected.insert(key1, Vector_(1, -1.)); expected.insert(key1, Vector_(1, -1.));
expected.insert(key2, Vector_(1, -2.)); expected.insert(key2, Vector_(1, -2.));
CHECK(assert_equal(expected,config0)); CHECK(assert_equal(expected,config0));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieValues, dummy_initialization) TEST(TestValues, dummy_initialization)
{ {
typedef TypedSymbol<LieVector, 'z'> Key; typedef TypedSymbol<LieVector, 'z'> Key;
typedef LieValues<Key> Values1; typedef Values<Key> Values1;
Values1 init1; Values1 init1;
init1.insert(Key(1), Vector_(2, 1.0, 2.0)); init1.insert(Key(1), Vector_(2, 1.0, 2.0));
init1.insert(Key(2), Vector_(2, 4.0, 3.0)); init1.insert(Key(2), Vector_(2, 4.0, 3.0));
Values init2; TestValues init2;
init2.insert(key1, Vector_(2, 1.0, 2.0)); init2.insert(key1, Vector_(2, 1.0, 2.0));
init2.insert(key2, Vector_(2, 4.0, 3.0)); init2.insert(key2, Vector_(2, 4.0, 3.0));
Values1 actual1(init2); Values1 actual1(init2);
Values actual2(init1); TestValues actual2(init1);
EXPECT(actual1.empty()); EXPECT(actual1.empty());
EXPECT(actual2.empty()); EXPECT(actual2.empty());

2
gtsam/slam/PartialPriorFactor.h
View File

@ -33,7 +33,7 @@ namespace gtsam {
* *
* It takes two template parameters: * It takes two template parameters:
* Key (typically TypedSymbol) is used to look up T's in a Values * Key (typically TypedSymbol) is used to look up T's in a Values
* Values where the T's are stored, typically LieValues<Key> or a TupleValues<...> * Values where the T's are stored, typically Values<Key> or a TupleValues<...>
* *
* For practical use, it would be good to subclass this factor and have the class type * For practical use, it would be good to subclass this factor and have the class type
* construct the mask. * construct the mask.

2
gtsam/slam/PriorFactor.h
View File

@ -24,7 +24,7 @@ namespace gtsam {
* A class for a soft prior on any Lie type * A class for a soft prior on any Lie type
* It takes two template parameters: * It takes two template parameters:
* Key (typically TypedSymbol) is used to look up T's in a Values * Key (typically TypedSymbol) is used to look up T's in a Values
* Values where the T's are stored, typically LieValues<Key> or a TupleValues<...> * Values where the T's are stored, typically Values<Key> or a TupleValues<...>
* The Key type is not arbitrary: we need to cast to a Symbol at linearize, so * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
* a simple type like int will not work * a simple type like int will not work
*/ */

7
gtsam/slam/Simulated3D.cpp
View File

@ -16,14 +16,15 @@
**/ **/
#include <gtsam/slam/Simulated3D.h> #include <gtsam/slam/Simulated3D.h>
#include <gtsam/nonlinear/LieValues-inl.h> #include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h> #include <gtsam/nonlinear/TupleValues-inl.h>
namespace gtsam { namespace gtsam {
INSTANTIATE_VALUES(simulated3D::PointKey)
INSTANTIATE_VALUES(simulated3D::PoseKey)
using namespace simulated3D; using namespace simulated3D;
INSTANTIATE_LIE_VALUES(PointKey)
INSTANTIATE_LIE_VALUES(PoseKey)
INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues) INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues)
namespace simulated3D { namespace simulated3D {

4
gtsam/slam/Simulated3D.h
View File

@ -40,8 +40,8 @@ namespace simulated3D {
typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey; typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
typedef gtsam::TypedSymbol<Point3, 'l'> PointKey; typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
typedef LieValues<PoseKey> PoseValues; typedef Values<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues; typedef Values<PointKey> PointValues;
typedef TupleValues2<PoseValues, PointValues> Values; typedef TupleValues2<PoseValues, PointValues> Values;
/** /**

9
gtsam/slam/planarSLAM.cpp
View File

@ -24,11 +24,10 @@
// Use planarSLAM namespace for specific SLAM instance // Use planarSLAM namespace for specific SLAM instance
namespace gtsam { namespace gtsam {
using namespace planarSLAM; INSTANTIATE_VALUES(planarSLAM::PointKey)
INSTANTIATE_LIE_VALUES(PointKey) INSTANTIATE_TUPLE_VALUES2(planarSLAM::PoseValues, planarSLAM::PointValues)
INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(planarSLAM::Values)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values) INSTANTIATE_NONLINEAR_OPTIMIZER(planarSLAM::Graph, planarSLAM::Values)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values)
namespace planarSLAM { namespace planarSLAM {

8
gtsam/slam/planarSLAM.h
View File

@ -41,11 +41,11 @@ namespace gtsam {
/// Typedef for a PointKey with Point2 data and 'l' symbol /// Typedef for a PointKey with Point2 data and 'l' symbol
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
/// Typedef for LieValues structure with PoseKey type /// Typedef for Values structure with PoseKey type
typedef LieValues<PoseKey> PoseValues; typedef Values<PoseKey> PoseValues;
/// Typedef for LieValues structure with PointKey type /// Typedef for Values structure with PointKey type
typedef LieValues<PointKey> PointValues; typedef Values<PointKey> PointValues;
/// Values class, inherited from TupleValues2, using PoseKeys and PointKeys /// Values class, inherited from TupleValues2, using PoseKeys and PointKeys
struct Values: public TupleValues2<PoseValues, PointValues> { struct Values: public TupleValues2<PoseValues, PointValues> {

11
gtsam/slam/pose2SLAM.cpp
View File

@ -16,18 +16,17 @@
**/ **/
#include <gtsam/slam/pose2SLAM.h> #include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/LieValues-inl.h> #include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h> #include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h> #include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
// Use pose2SLAM namespace for specific SLAM instance // Use pose2SLAM namespace for specific SLAM instance
namespace gtsam { namespace gtsam {
using namespace pose2SLAM; INSTANTIATE_VALUES(pose2SLAM::Key)
INSTANTIATE_LIE_VALUES(Key) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(pose2SLAM::Values)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values) INSTANTIATE_NONLINEAR_OPTIMIZER(pose2SLAM::Graph, pose2SLAM::Values)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values) template class NonlinearOptimizer<pose2SLAM::Graph, pose2SLAM::Values, GaussianFactorGraph, GaussianSequentialSolver>;
template class NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianSequentialSolver>;
namespace pose2SLAM { namespace pose2SLAM {

6
gtsam/slam/pose2SLAM.h
View File

@ -19,7 +19,7 @@
#include <gtsam/base/LieVector.h> #include <gtsam/base/LieVector.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LieValues.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h> #include <gtsam/nonlinear/Key.h>
@ -37,8 +37,8 @@ namespace gtsam {
/// Keys with Pose2 and symbol 'x' /// Keys with Pose2 and symbol 'x'
typedef TypedSymbol<Pose2, 'x'> Key; typedef TypedSymbol<Pose2, 'x'> Key;
/// LieValues with type 'Key' /// Values with type 'Key'
typedef LieValues<Key> Values; typedef Values<Key> Values;
/** /**
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)

9
gtsam/slam/pose3SLAM.cpp
View File

@ -16,17 +16,16 @@
**/ **/
#include <gtsam/slam/pose3SLAM.h> #include <gtsam/slam/pose3SLAM.h>
#include <gtsam/nonlinear/LieValues-inl.h> #include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h> #include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h> #include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
// Use pose3SLAM namespace for specific SLAM instance // Use pose3SLAM namespace for specific SLAM instance
namespace gtsam { namespace gtsam {
using namespace pose3SLAM; INSTANTIATE_VALUES(pose3SLAM::Key)
INSTANTIATE_LIE_VALUES(Key) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(pose3SLAM::Values)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values) INSTANTIATE_NONLINEAR_OPTIMIZER(pose3SLAM::Graph, pose3SLAM::Values)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values)
namespace pose3SLAM { namespace pose3SLAM {

6
gtsam/slam/pose3SLAM.h
View File

@ -18,7 +18,7 @@
#pragma once #pragma once
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/LieValues.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h> #include <gtsam/nonlinear/Key.h>
@ -33,8 +33,8 @@ namespace gtsam {
/// Creates a Key with data Pose3 and symbol 'x' /// Creates a Key with data Pose3 and symbol 'x'
typedef TypedSymbol<Pose3, 'x'> Key; typedef TypedSymbol<Pose3, 'x'> Key;
/// Creates a LieValues structure with type 'Key' /// Creates a Values structure with type 'Key'
typedef LieValues<Key> Values; typedef Values<Key> Values;
/** /**
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)

5
gtsam/slam/simulated2D.cpp
View File

@ -16,14 +16,15 @@
*/ */
#include <gtsam/slam/simulated2D.h> #include <gtsam/slam/simulated2D.h>
#include <gtsam/nonlinear/LieValues-inl.h> #include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h> #include <gtsam/nonlinear/TupleValues-inl.h>
namespace gtsam { namespace gtsam {
INSTANTIATE_VALUES(simulated2D::PoseKey)
using namespace simulated2D; using namespace simulated2D;
INSTANTIATE_LIE_VALUES(PoseKey)
INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues) INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues)
namespace simulated2D { namespace simulated2D {

4
gtsam/slam/simulated2D.h
View File

@ -31,8 +31,8 @@ namespace gtsam {
// Simulated2D robots have no orientation, just a position // Simulated2D robots have no orientation, just a position
typedef TypedSymbol<Point2, 'x'> PoseKey; typedef TypedSymbol<Point2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef LieValues<PoseKey> PoseValues; typedef Values<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues; typedef Values<PointKey> PointValues;
/** /**
* Custom Values class that holds poses and points * Custom Values class that holds poses and points

4
gtsam/slam/simulated2DOriented.h
View File

@ -31,8 +31,8 @@ namespace gtsam {
// The types that take an oriented pose2 rather than point2 // The types that take an oriented pose2 rather than point2
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef LieValues<PoseKey> PoseValues; typedef Values<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues; typedef Values<PointKey> PointValues;
typedef TupleValues2<PoseValues, PointValues> Values; typedef TupleValues2<PoseValues, PointValues> Values;
//TODO:: point prior is not implemented right now //TODO:: point prior is not implemented right now

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

@ -33,14 +33,14 @@ typedef Cal3_S2Camera GeneralCamera;
//typedef Cal3BundlerCamera GeneralCamera; //typedef Cal3BundlerCamera GeneralCamera;
typedef TypedSymbol<GeneralCamera, 'x'> CameraKey; typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
typedef TypedSymbol<Point3, 'l'> PointKey; typedef TypedSymbol<Point3, 'l'> PointKey;
typedef LieValues<CameraKey> CameraConfig; typedef Values<CameraKey> CameraConfig;
typedef LieValues<PointKey> PointConfig; typedef Values<PointKey> PointConfig;
typedef TupleValues2<CameraConfig, PointConfig> Values; typedef TupleValues2<CameraConfig, PointConfig> VisualValues;
typedef GeneralSFMFactor<Values, CameraKey, PointKey> Projection; typedef GeneralSFMFactor<VisualValues, CameraKey, PointKey> Projection;
typedef NonlinearEquality<Values, CameraKey> CameraConstraint; typedef NonlinearEquality<VisualValues, CameraKey> CameraConstraint;
typedef NonlinearEquality<Values, PointKey> Point3Constraint; typedef NonlinearEquality<VisualValues, PointKey> Point3Constraint;
class Graph: public NonlinearFactorGraph<Values> { class Graph: public NonlinearFactorGraph<VisualValues> {
public: public:
void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, i, j)); push_back(boost::make_shared<Projection>(z, model, i, j));
@ -73,7 +73,7 @@ double getGaussian()
return sqrt(-2.0f * (double)log(S) / S) * V1; return sqrt(-2.0f * (double)log(S) / S) * V1;
} }
typedef NonlinearOptimizer<Graph,Values> Optimizer; typedef NonlinearOptimizer<Graph,VisualValues> Optimizer;
const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
@ -90,7 +90,7 @@ TEST( GeneralSFMFactor, equals )
boost::shared_ptr<Projection> boost::shared_ptr<Projection>
factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
CHECK(assert_equal(*factor1, *factor2)); EXPECT(assert_equal(*factor1, *factor2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -101,18 +101,18 @@ TEST( GeneralSFMFactor, error ) {
boost::shared_ptr<Projection> boost::shared_ptr<Projection>
factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
// For the following configuration, the factor predicts 320,240 // For the following configuration, the factor predicts 320,240
Values values; VisualValues values;
Rot3 R; Rot3 R;
Point3 t1(0,0,-6); Point3 t1(0,0,-6);
Pose3 x1(R,t1); Pose3 x1(R,t1);
values.insert(1, GeneralCamera(x1)); values.insert(1, GeneralCamera(x1));
Point3 l1; values.insert(1, l1); Point3 l1; values.insert(1, l1);
CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
} }
static const double baseline = 5.0 ; static const double baseline = 5.0 ;
/* ************************************************************************* */
vector<Point3> genPoint3() { vector<Point3> genPoint3() {
const double z = 5; const double z = 5;
vector<Point3> L ; vector<Point3> L ;
@ -174,7 +174,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values); boost::shared_ptr<VisualValues> values(new VisualValues);
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ; values->insert((int)i, X[i]) ;
@ -189,20 +189,13 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// Create an ordering of the variables // Create an ordering of the variables
shared_ptr<Ordering> ordering = getOrdering(X,L); shared_ptr<Ordering> ordering = getOrdering(X,L);
//graph->print("graph") ; values->print("values") ;
NonlinearOptimizationParameters::sharedThis params ( NonlinearOptimizationParameters::sharedThis params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params); Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_defaultK::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt(); Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl; EXPECT(optimizer2.error() < 0.5 * 1e-5 * nMeasurements);
//optimizer2.values()->print("optimized") ;
CHECK(optimizer2.error() < 0.5 * 1e-5 * nMeasurements);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
vector<Point3> L = genPoint3(); vector<Point3> L = genPoint3();
@ -220,7 +213,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values); boost::shared_ptr<VisualValues> values(new VisualValues);
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ; values->insert((int)i, X[i]) ;
@ -244,13 +237,11 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
NonlinearOptimizationParameters::sharedThis params ( NonlinearOptimizationParameters::sharedThis params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params); Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_varK_SingleMeasurementError::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt(); Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl; EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
} }
/* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
vector<Point3> L = genPoint3(); vector<Point3> L = genPoint3();
@ -268,7 +259,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
const size_t nMeasurements = L.size()*X.size(); const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<Values> values(new Values); boost::shared_ptr<VisualValues> values(new VisualValues);
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ; values->insert((int)i, X[i]) ;
@ -290,14 +281,11 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params); Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_varK_FixCameras::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt(); Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl; EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
} }
/* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
vector<Point3> L = genPoint3(); vector<Point3> L = genPoint3();
@ -314,7 +302,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
const size_t nMeasurements = L.size()*X.size(); const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<Values> values(new Values); boost::shared_ptr<VisualValues> values(new VisualValues);
for ( size_t i = 0 ; i < X.size() ; ++i ) { for ( size_t i = 0 ; i < X.size() ; ++i ) {
const double const double
rot_noise = 1e-5, rot_noise = 1e-5,
@ -351,14 +339,10 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
shared_ptr<Ordering> ordering = getOrdering(X,L); shared_ptr<Ordering> ordering = getOrdering(X,L);
NonlinearOptimizationParameters::sharedThis params ( NonlinearOptimizationParameters::sharedThis params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT));
// new NonlinearOptimizationParameters(1e-7, 1e-7, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::TRYDELTA));
Optimizer optimizer(graph, values, ordering, params); Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_varK_FixLandmarks::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt(); Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl; EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -379,7 +363,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values); boost::shared_ptr<VisualValues> values(new VisualValues);
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ; values->insert((int)i, X[i]) ;
@ -399,14 +383,10 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
new NonlinearOptimizationParameters(1e-2, 1e-2, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); new NonlinearOptimizationParameters(1e-2, 1e-2, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params); Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_varK_BA::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt(); Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl; EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -32,14 +32,15 @@ using namespace gtsam;
typedef Cal3BundlerCamera GeneralCamera; typedef Cal3BundlerCamera GeneralCamera;
typedef TypedSymbol<GeneralCamera, 'x'> CameraKey; typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
typedef TypedSymbol<Point3, 'l'> PointKey; typedef TypedSymbol<Point3, 'l'> PointKey;
typedef LieValues<CameraKey> CameraConfig; typedef Values<CameraKey> CameraConfig;
typedef LieValues<PointKey> PointConfig; typedef Values<PointKey> PointConfig;
typedef TupleValues2<CameraConfig, PointConfig> Values; typedef TupleValues2<CameraConfig, PointConfig> VisualValues;
typedef GeneralSFMFactor<Values, CameraKey, PointKey> Projection; typedef GeneralSFMFactor<VisualValues, CameraKey, PointKey> Projection;
typedef NonlinearEquality<Values, CameraKey> CameraConstraint; typedef NonlinearEquality<VisualValues, CameraKey> CameraConstraint;
typedef NonlinearEquality<Values, PointKey> Point3Constraint; typedef NonlinearEquality<VisualValues, PointKey> Point3Constraint;
class Graph: public NonlinearFactorGraph<Values> { /* ************************************************************************* */
class Graph: public NonlinearFactorGraph<VisualValues> {
public: public:
void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, i, j)); push_back(boost::make_shared<Projection>(z, model, i, j));
@ -72,7 +73,7 @@ double getGaussian()
return sqrt(-2.0f * (double)log(S) / S) * V1; return sqrt(-2.0f * (double)log(S) / S) * V1;
} }
typedef NonlinearOptimizer<Graph,Values> Optimizer; typedef NonlinearOptimizer<Graph,VisualValues> Optimizer;
const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
@ -89,7 +90,7 @@ TEST( GeneralSFMFactor, equals )
boost::shared_ptr<Projection> boost::shared_ptr<Projection>
factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
CHECK(assert_equal(*factor1, *factor2)); EXPECT(assert_equal(*factor1, *factor2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -100,18 +101,19 @@ TEST( GeneralSFMFactor, error ) {
boost::shared_ptr<Projection> boost::shared_ptr<Projection>
factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
// For the following configuration, the factor predicts 320,240 // For the following configuration, the factor predicts 320,240
Values values; VisualValues values;
Rot3 R; Rot3 R;
Point3 t1(0,0,-6); Point3 t1(0,0,-6);
Pose3 x1(R,t1); Pose3 x1(R,t1);
values.insert(1, GeneralCamera(x1)); values.insert(1, GeneralCamera(x1));
Point3 l1; values.insert(1, l1); Point3 l1; values.insert(1, l1);
CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
} }
static const double baseline = 5.0 ; static const double baseline = 5.0 ;
/* ************************************************************************* */
vector<Point3> genPoint3() { vector<Point3> genPoint3() {
const double z = 5; const double z = 5;
vector<Point3> L ; vector<Point3> L ;
@ -172,7 +174,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values); boost::shared_ptr<VisualValues> values(new VisualValues);
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ; values->insert((int)i, X[i]) ;
@ -187,20 +189,13 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// Create an ordering of the variables // Create an ordering of the variables
shared_ptr<Ordering> ordering = getOrdering(X,L); shared_ptr<Ordering> ordering = getOrdering(X,L);
//graph->print("graph") ; values->print("values") ;
NonlinearOptimizationParameters::sharedThis params ( NonlinearOptimizationParameters::sharedThis params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params); Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_defaultK::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt(); Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl; EXPECT(optimizer2.error() < 0.5 * 1e-5 * nMeasurements);
//optimizer2.values()->print("optimized") ;
CHECK(optimizer2.error() < 0.5 * 1e-5 * nMeasurements);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
vector<Point3> L = genPoint3(); vector<Point3> L = genPoint3();
@ -218,7 +213,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values); boost::shared_ptr<VisualValues> values(new VisualValues);
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ; values->insert((int)i, X[i]) ;
@ -242,13 +237,11 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
NonlinearOptimizationParameters::sharedThis params ( NonlinearOptimizationParameters::sharedThis params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params); Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_varK_SingleMeasurementError::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt(); Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl; EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
} }
/* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
vector<Point3> L = genPoint3(); vector<Point3> L = genPoint3();
@ -266,7 +259,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
const size_t nMeasurements = L.size()*X.size(); const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<Values> values(new Values); boost::shared_ptr<VisualValues> values(new VisualValues);
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ; values->insert((int)i, X[i]) ;
@ -288,14 +281,11 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params); Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_varK_FixCameras::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt(); Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl; EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
} }
/* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
vector<Point3> L = genPoint3(); vector<Point3> L = genPoint3();
@ -312,7 +302,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
const size_t nMeasurements = L.size()*X.size(); const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<Values> values(new Values); boost::shared_ptr<VisualValues> values(new VisualValues);
for ( size_t i = 0 ; i < X.size() ; ++i ) { for ( size_t i = 0 ; i < X.size() ; ++i ) {
const double const double
rot_noise = 1e-5, trans_noise = 1e-3, rot_noise = 1e-5, trans_noise = 1e-3,
@ -345,14 +335,10 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
shared_ptr<Ordering> ordering = getOrdering(X,L); shared_ptr<Ordering> ordering = getOrdering(X,L);
NonlinearOptimizationParameters::sharedThis params ( NonlinearOptimizationParameters::sharedThis params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT)); new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT));
// new NonlinearOptimizationParameters(1e-7, 1e-7, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::TRYDELTA));
Optimizer optimizer(graph, values, ordering, params); Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_varK_FixLandmarks::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt(); Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl; EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -373,7 +359,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
// add initial // add initial
const double noise = baseline*0.1; const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values); boost::shared_ptr<VisualValues> values(new VisualValues);
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ; values->insert((int)i, X[i]) ;
@ -393,14 +379,10 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT)); new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params); Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_varK_BA::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt(); Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl; EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -42,8 +42,6 @@ static Matrix covariance = eye(6);
const double tol=1e-5; const double tol=1e-5;
using namespace pose3SLAM;
/* ************************************************************************* */ /* ************************************************************************* */
// test optimization with 6 poses arranged in a hexagon and a loop closure // test optimization with 6 poses arranged in a hexagon and a loop closure
TEST(Pose3Graph, optimizeCircle) { TEST(Pose3Graph, optimizeCircle) {
@ -79,8 +77,8 @@ TEST(Pose3Graph, optimizeCircle) {
shared_ptr<Ordering> ordering(new Ordering); shared_ptr<Ordering> ordering(new Ordering);
*ordering += "x0","x1","x2","x3","x4","x5"; *ordering += "x0","x1","x2","x3","x4","x5";
NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15);
Optimizer optimizer0(fg, initial, ordering, params); pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params);
Optimizer optimizer = optimizer0.levenbergMarquardt(); pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
Pose3Values actual = *optimizer.values(); Pose3Values actual = *optimizer.values();
@ -93,12 +91,12 @@ TEST(Pose3Graph, optimizeCircle) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3Graph, partial_prior_height) { TEST(Pose3Graph, partial_prior_height) {
typedef PartialPriorFactor<Values, Key> Partial; typedef PartialPriorFactor<pose3SLAM::Values, pose3SLAM::Key> Partial;
// reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3) // reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3)
// height prior - single element interface // height prior - single element interface
Key key(1); pose3SLAM::Key key(1);
double exp_height = 5.0; double exp_height = 5.0;
SharedDiagonal model = noiseModel::Unit::Create(1); SharedDiagonal model = noiseModel::Unit::Create(1);
Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height)); Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height));
@ -108,17 +106,17 @@ TEST(Pose3Graph, partial_prior_height) {
Matrix expA = Matrix_(1, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); Matrix expA = Matrix_(1, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
EXPECT(assert_equal(expA, actA)); EXPECT(assert_equal(expA, actA));
Graph graph; pose3SLAM::Graph graph;
// graph.add(height); // FAIL - on compile, can't initialize a reference? // graph.add(height); // FAIL - on compile, can't initialize a reference?
graph.push_back(boost::shared_ptr<Partial>(new Partial(height))); graph.push_back(boost::shared_ptr<Partial>(new Partial(height)));
Values values; pose3SLAM::Values values;
values.insert(key, init); values.insert(key, init);
// linearization // linearization
EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
Values actual = *Optimizer::optimizeLM(graph, values); pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
EXPECT(assert_equal(expected, actual[key], tol)); EXPECT(assert_equal(expected, actual[key], tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
} }
@ -148,10 +146,10 @@ TEST( Pose3Factor, error )
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3Graph, partial_prior_xy) { TEST(Pose3Graph, partial_prior_xy) {
typedef PartialPriorFactor<Values, Key> Partial; typedef PartialPriorFactor<pose3SLAM::Values, pose3SLAM::Key> Partial;
// XY prior - full mask interface // XY prior - full mask interface
Key key(1); pose3SLAM::Key key(1);
Vector exp_xy = Vector_(2, 3.0, 4.0); Vector exp_xy = Vector_(2, 3.0, 4.0);
SharedDiagonal model = noiseModel::Unit::Create(2); SharedDiagonal model = noiseModel::Unit::Create(2);
Pose3 init(Rot3(), Point3(1.0,-2.0, 3.0)), expected(Rot3(), Point3(3.0, 4.0, 3.0)); Pose3 init(Rot3(), Point3(1.0,-2.0, 3.0)), expected(Rot3(), Point3(3.0, 4.0, 3.0));
@ -164,13 +162,13 @@ TEST(Pose3Graph, partial_prior_xy) {
0.0, 0.0, 0.0, 0.0, 1.0, 0.0); 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
EXPECT(assert_equal(expA, actA)); EXPECT(assert_equal(expA, actA));
Graph graph; pose3SLAM::Graph graph;
graph.push_back(Partial::shared_ptr(new Partial(priorXY))); graph.push_back(Partial::shared_ptr(new Partial(priorXY)));
Values values; pose3SLAM::Values values;
values.insert(key, init); values.insert(key, init);
Values actual = *Optimizer::optimizeLM(graph, values); pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
EXPECT(assert_equal(expected, actual[key], tol)); EXPECT(assert_equal(expected, actual[key], tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
} }

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

@ -25,7 +25,6 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::visualSLAM;
// make cube // make cube
static Point3 static Point3
@ -48,11 +47,11 @@ TEST( ProjectionFactor, error )
// Create the factor with a measurement that is 3 pixels off in x // Create the factor with a measurement that is 3 pixels off in x
Point2 z(323.,240.); Point2 z(323.,240.);
int cameraFrameNumber=1, landmarkNumber=1; int cameraFrameNumber=1, landmarkNumber=1;
boost::shared_ptr<ProjectionFactor> boost::shared_ptr<visualSLAM::ProjectionFactor>
factor(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
// For the following values structure, the factor predicts 320,240 // For the following values structure, the factor predicts 320,240
Values config; visualSLAM::Values config;
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1); Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1);
Point3 l1; config.insert(1, l1); Point3 l1; config.insert(1, l1);
// Point should project to Point2(320.,240.) // Point should project to Point2(320.,240.)
@ -73,7 +72,7 @@ TEST( ProjectionFactor, error )
CHECK(assert_equal(expected,*actual,1e-3)); CHECK(assert_equal(expected,*actual,1e-3));
// linearize graph // linearize graph
Graph graph; visualSLAM::Graph graph;
graph.push_back(factor); graph.push_back(factor);
FactorGraph<GaussianFactor> expected_lfg; FactorGraph<GaussianFactor> expected_lfg;
expected_lfg.push_back(actual); expected_lfg.push_back(actual);
@ -81,13 +80,13 @@ TEST( ProjectionFactor, error )
CHECK(assert_equal(expected_lfg,*actual_lfg)); CHECK(assert_equal(expected_lfg,*actual_lfg));
// expmap on a config // expmap on a config
Values expected_config; visualSLAM::Values expected_config;
Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2); Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2);
Point3 l2(1,2,3); expected_config.insert(1, l2); Point3 l2(1,2,3); expected_config.insert(1, l2);
VectorValues delta(expected_config.dims(ordering)); VectorValues delta(expected_config.dims(ordering));
delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.); delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
delta[ordering["l1"]] = Vector_(3, 1.,2.,3.); delta[ordering["l1"]] = Vector_(3, 1.,2.,3.);
Values actual_config = config.retract(delta, ordering); visualSLAM::Values actual_config = config.retract(delta, ordering);
CHECK(assert_equal(expected_config,actual_config,1e-9)); CHECK(assert_equal(expected_config,actual_config,1e-9));
} }
@ -97,11 +96,11 @@ TEST( ProjectionFactor, equals )
// Create two identical factors and make sure they're equal // Create two identical factors and make sure they're equal
Vector z = Vector_(2,323.,240.); Vector z = Vector_(2,323.,240.);
int cameraFrameNumber=1, landmarkNumber=1; int cameraFrameNumber=1, landmarkNumber=1;
boost::shared_ptr<ProjectionFactor> boost::shared_ptr<visualSLAM::ProjectionFactor>
factor1(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); factor1(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
boost::shared_ptr<ProjectionFactor> boost::shared_ptr<visualSLAM::ProjectionFactor>
factor2(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); factor2(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
CHECK(assert_equal(*factor1, *factor2)); CHECK(assert_equal(*factor1, *factor2));
} }

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

@ -23,27 +23,26 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/slam/simulated2D.h> #include <gtsam/slam/simulated2D.h>
using namespace gtsam;
using namespace std; using namespace std;
using namespace simulated2D; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( simulated2D, Simulated2DValues ) TEST( simulated2D, Simulated2DValues )
{ {
Values actual; simulated2D::Values actual;
actual.insertPose(1,Point2(1,1)); actual.insertPose(1,Point2(1,1));
actual.insertPoint(2,Point2(2,2)); actual.insertPoint(2,Point2(2,2));
CHECK(assert_equal(actual,actual,1e-9)); EXPECT(assert_equal(actual,actual,1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( simulated2D, Dprior ) TEST( simulated2D, Dprior )
{ {
Point2 x(1,-9); Point2 x(1,-9);
Matrix numerical = numericalDerivative11(prior,x); Matrix numerical = numericalDerivative11(simulated2D::prior,x);
Matrix computed; Matrix computed;
prior(x,computed); simulated2D::prior(x,computed);
CHECK(assert_equal(numerical,computed,1e-9)); EXPECT(assert_equal(numerical,computed,1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -51,11 +50,11 @@ TEST( simulated2D, Dprior )
{ {
Point2 x1(1,-9),x2(-5,6); Point2 x1(1,-9),x2(-5,6);
Matrix H1,H2; Matrix H1,H2;
odo(x1,x2,H1,H2); simulated2D::odo(x1,x2,H1,H2);
Matrix A1 = numericalDerivative21(odo,x1,x2); Matrix A1 = numericalDerivative21(simulated2D::odo,x1,x2);
CHECK(assert_equal(A1,H1,1e-9)); EXPECT(assert_equal(A1,H1,1e-9));
Matrix A2 = numericalDerivative22(odo,x1,x2); Matrix A2 = numericalDerivative22(simulated2D::odo,x1,x2);
CHECK(assert_equal(A2,H2,1e-9)); EXPECT(assert_equal(A2,H2,1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -24,26 +24,16 @@
#include <gtsam/slam/simulated2D.h> #include <gtsam/slam/simulated2D.h>
#include <gtsam/slam/simulated2DOriented.h> #include <gtsam/slam/simulated2DOriented.h>
using namespace gtsam;
using namespace std; using namespace std;
using namespace simulated2DOriented; using namespace gtsam;
/* ************************************************************************* */
TEST( simulated2DOriented, Simulated2DValues )
{
simulated2D::Values actual;
actual.insertPose(1,Point2(1,1));
actual.insertPoint(2,Point2(2,2));
CHECK(assert_equal(actual,actual,1e-9));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( simulated2DOriented, Dprior ) TEST( simulated2DOriented, Dprior )
{ {
Pose2 x(1,-9, 0.1); Pose2 x(1,-9, 0.1);
Matrix numerical = numericalDerivative11(prior,x); Matrix numerical = numericalDerivative11(simulated2DOriented::prior,x);
Matrix computed; Matrix computed;
prior(x,computed); simulated2DOriented::prior(x,computed);
CHECK(assert_equal(numerical,computed,1e-9)); CHECK(assert_equal(numerical,computed,1e-9));
} }
@ -52,10 +42,10 @@ TEST( simulated2DOriented, Dprior )
{ {
Pose2 x1(1,-9,0.1),x2(-5,6,0.2); Pose2 x1(1,-9,0.1),x2(-5,6,0.2);
Matrix H1,H2; Matrix H1,H2;
odo(x1,x2,H1,H2); simulated2DOriented::odo(x1,x2,H1,H2);
Matrix A1 = numericalDerivative21(odo,x1,x2); Matrix A1 = numericalDerivative21(simulated2DOriented::odo,x1,x2);
CHECK(assert_equal(A1,H1,1e-9)); CHECK(assert_equal(A1,H1,1e-9));
Matrix A2 = numericalDerivative22(odo,x1,x2); Matrix A2 = numericalDerivative22(simulated2DOriented::odo,x1,x2);
CHECK(assert_equal(A2,H2,1e-9)); CHECK(assert_equal(A2,H2,1e-9));
} }
@ -64,11 +54,11 @@ TEST( simulated2DOriented, constructor )
{ {
Pose2 measurement(0.2, 0.3, 0.1); Pose2 measurement(0.2, 0.3, 0.1);
SharedDiagonal model(Vector_(3, 1., 1., 1.)); SharedDiagonal model(Vector_(3, 1., 1., 1.));
Odometry factor(measurement, model, PoseKey(1), PoseKey(2)); simulated2DOriented::Odometry factor(measurement, model, simulated2DOriented::PoseKey(1), simulated2DOriented::PoseKey(2));
Values config; simulated2DOriented::Values config;
config.insert(PoseKey(1), Pose2(1., 0., 0.2)); config.insert(simulated2DOriented::PoseKey(1), Pose2(1., 0., 0.2));
config.insert(PoseKey(2), Pose2(2., 0., 0.1)); config.insert(simulated2DOriented::PoseKey(2), Pose2(2., 0., 0.1));
boost::shared_ptr<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary()); boost::shared_ptr<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary());
} }

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

@ -24,14 +24,13 @@
#include <gtsam/slam/Simulated3D.h> #include <gtsam/slam/Simulated3D.h>
using namespace gtsam; using namespace gtsam;
using namespace simulated3D;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( simulated3D, Values ) TEST( simulated3D, Values )
{ {
Values actual; simulated3D::Values actual;
actual.insert(PointKey(1),Point3(1,1,1)); actual.insert(simulated3D::PointKey(1),Point3(1,1,1));
actual.insert(PoseKey(2),Point3(2,2,2)); actual.insert(simulated3D::PoseKey(2),Point3(2,2,2));
EXPECT(assert_equal(actual,actual,1e-9)); EXPECT(assert_equal(actual,actual,1e-9));
} }
@ -39,9 +38,9 @@ TEST( simulated3D, Values )
TEST( simulated3D, Dprior ) TEST( simulated3D, Dprior )
{ {
Point3 x(1,-9, 7); Point3 x(1,-9, 7);
Matrix numerical = numericalDerivative11<Point3, Point3>(boost::bind(prior, _1, boost::none),x); Matrix numerical = numericalDerivative11<Point3, Point3>(boost::bind(simulated3D::prior, _1, boost::none),x);
Matrix computed; Matrix computed;
prior(x,computed); simulated3D::prior(x,computed);
EXPECT(assert_equal(numerical,computed,1e-9)); EXPECT(assert_equal(numerical,computed,1e-9));
} }
@ -50,10 +49,10 @@ TEST( simulated3D, DOdo )
{ {
Point3 x1(1,-9,7),x2(-5,6,7); Point3 x1(1,-9,7),x2(-5,6,7);
Matrix H1,H2; Matrix H1,H2;
odo(x1,x2,H1,H2); simulated3D::odo(x1,x2,H1,H2);
Matrix A1 = numericalDerivative21<Point3, Point3, Point3>(boost::bind(odo, _1, _2, boost::none, boost::none),x1,x2); Matrix A1 = numericalDerivative21<Point3, Point3, Point3>(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2);
EXPECT(assert_equal(A1,H1,1e-9)); EXPECT(assert_equal(A1,H1,1e-9));
Matrix A2 = numericalDerivative22<Point3, Point3, Point3>(boost::bind(odo, _1, _2, boost::none, boost::none),x1,x2); Matrix A2 = numericalDerivative22<Point3, Point3, Point3>(boost::bind(simulated3D::odo, _1, _2, boost::none, boost::none),x1,x2);
EXPECT(assert_equal(A2,H2,1e-9)); EXPECT(assert_equal(A2,H2,1e-9));
} }

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

@ -29,9 +29,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::visualSLAM;
using namespace boost; using namespace boost;
typedef NonlinearOptimizer<Graph, Values> Optimizer;
Pose3 camera1(Matrix_(3,3, Pose3 camera1(Matrix_(3,3,
1., 0., 0., 1., 0., 0.,
@ -52,16 +50,16 @@ TEST( StereoFactor, singlePoint)
{ {
//Cal3_S2 K(625, 625, 0, 320, 240, 0.5); //Cal3_S2 K(625, 625, 0, 320, 240, 0.5);
boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));
boost::shared_ptr<Graph> graph(new Graph()); boost::shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph());
graph->add(PoseConstraint(1,camera1)); graph->add(visualSLAM::PoseConstraint(1,camera1));
StereoPoint2 z14(320,320.0-50, 240); StereoPoint2 z14(320,320.0-50, 240);
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m) // arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
graph->add(StereoFactor(z14,sigma, 1, 1, K)); graph->add(visualSLAM::StereoFactor(z14,sigma, 1, 1, K));
// Create a configuration corresponding to the ground truth // Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values()); boost::shared_ptr<visualSLAM::Values> values(new visualSLAM::Values());
values->insert(1, camera1); // add camera at z=6.25m looking towards origin values->insert(1, camera1); // add camera at z=6.25m looking towards origin
Point3 l1(0, 0, 0); Point3 l1(0, 0, 0);
@ -69,7 +67,7 @@ TEST( StereoFactor, singlePoint)
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
typedef gtsam::NonlinearOptimizer<Graph,Values,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> Optimizer; // optimization engine for this domain typedef gtsam::NonlinearOptimizer<visualSLAM::Graph,visualSLAM::Values,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> Optimizer; // optimization engine for this domain
double absoluteThreshold = 1e-9; double absoluteThreshold = 1e-9;
double relativeThreshold = 1e-5; double relativeThreshold = 1e-5;

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

@ -32,7 +32,6 @@ using namespace boost;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::visualSLAM;
using namespace boost; using namespace boost;
static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
@ -58,7 +57,7 @@ Pose3 camera2(Matrix_(3,3,
Point3(0,0,5.00)); Point3(0,0,5.00));
/* ************************************************************************* */ /* ************************************************************************* */
Graph testGraph() { visualSLAM::Graph testGraph() {
Point2 z11(-100, 100); Point2 z11(-100, 100);
Point2 z12(-100,-100); Point2 z12(-100,-100);
Point2 z13( 100,-100); Point2 z13( 100,-100);
@ -69,7 +68,7 @@ Graph testGraph() {
Point2 z24( 125, 125); Point2 z24( 125, 125);
shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0)); shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0));
Graph g; visualSLAM::Graph g;
g.addMeasurement(z11, sigma, 1, 1, sK); g.addMeasurement(z11, sigma, 1, 1, sK);
g.addMeasurement(z12, sigma, 1, 2, sK); g.addMeasurement(z12, sigma, 1, 2, sK);
g.addMeasurement(z13, sigma, 1, 3, sK); g.addMeasurement(z13, sigma, 1, 3, sK);
@ -85,14 +84,14 @@ Graph testGraph() {
TEST( Graph, optimizeLM) TEST( Graph, optimizeLM)
{ {
// build a graph // build a graph
shared_ptr<Graph> graph(new Graph(testGraph())); shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph(testGraph()));
// add 3 landmark constraints // add 3 landmark constraints
graph->addPointConstraint(1, landmark1); graph->addPointConstraint(1, landmark1);
graph->addPointConstraint(2, landmark2); graph->addPointConstraint(2, landmark2);
graph->addPointConstraint(3, landmark3); graph->addPointConstraint(3, landmark3);
// Create an initial values structure corresponding to the ground truth // Create an initial values structure corresponding to the ground truth
boost::shared_ptr<Values> initialEstimate(new Values); boost::shared_ptr<visualSLAM::Values> initialEstimate(new visualSLAM::Values);
initialEstimate->insert(1, camera1); initialEstimate->insert(1, camera1);
initialEstimate->insert(2, camera2); initialEstimate->insert(2, camera2);
initialEstimate->insert(1, landmark1); initialEstimate->insert(1, landmark1);
@ -106,12 +105,12 @@ TEST( Graph, optimizeLM)
// Create an optimizer and check its error // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
Optimizer optimizer(graph, initialEstimate, ordering); visualSLAM::Optimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started // Iterate once, and the config should not have changed because we started
// with the ground truth // with the ground truth
Optimizer afterOneIteration = optimizer.iterate(); visualSLAM::Optimizer afterOneIteration = optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct // check if correct
@ -123,13 +122,13 @@ TEST( Graph, optimizeLM)
TEST( Graph, optimizeLM2) TEST( Graph, optimizeLM2)
{ {
// build a graph // build a graph
shared_ptr<Graph> graph(new Graph(testGraph())); shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph(testGraph()));
// add 2 camera constraints // add 2 camera constraints
graph->addPoseConstraint(1, camera1); graph->addPoseConstraint(1, camera1);
graph->addPoseConstraint(2, camera2); graph->addPoseConstraint(2, camera2);
// Create an initial values structure corresponding to the ground truth // Create an initial values structure corresponding to the ground truth
boost::shared_ptr<Values> initialEstimate(new Values); boost::shared_ptr<visualSLAM::Values> initialEstimate(new visualSLAM::Values);
initialEstimate->insert(1, camera1); initialEstimate->insert(1, camera1);
initialEstimate->insert(2, camera2); initialEstimate->insert(2, camera2);
initialEstimate->insert(1, landmark1); initialEstimate->insert(1, landmark1);
@ -143,12 +142,12 @@ TEST( Graph, optimizeLM2)
// Create an optimizer and check its error // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
Optimizer optimizer(graph, initialEstimate, ordering); visualSLAM::Optimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started // Iterate once, and the config should not have changed because we started
// with the ground truth // with the ground truth
Optimizer afterOneIteration = optimizer.iterate(); visualSLAM::Optimizer afterOneIteration = optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct // check if correct
@ -160,13 +159,13 @@ TEST( Graph, optimizeLM2)
TEST( Graph, CHECK_ORDERING) TEST( Graph, CHECK_ORDERING)
{ {
// build a graph // build a graph
shared_ptr<Graph> graph(new Graph(testGraph())); shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph(testGraph()));
// add 2 camera constraints // add 2 camera constraints
graph->addPoseConstraint(1, camera1); graph->addPoseConstraint(1, camera1);
graph->addPoseConstraint(2, camera2); graph->addPoseConstraint(2, camera2);
// Create an initial values structure corresponding to the ground truth // Create an initial values structure corresponding to the ground truth
boost::shared_ptr<Values> initialEstimate(new Values); boost::shared_ptr<visualSLAM::Values> initialEstimate(new visualSLAM::Values);
initialEstimate->insert(1, camera1); initialEstimate->insert(1, camera1);
initialEstimate->insert(2, camera2); initialEstimate->insert(2, camera2);
initialEstimate->insert(1, landmark1); initialEstimate->insert(1, landmark1);
@ -178,12 +177,12 @@ TEST( Graph, CHECK_ORDERING)
// Create an optimizer and check its error // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
Optimizer optimizer(graph, initialEstimate, ordering); visualSLAM::Optimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started // Iterate once, and the config should not have changed because we started
// with the ground truth // with the ground truth
Optimizer afterOneIteration = optimizer.iterate(); visualSLAM::Optimizer afterOneIteration = optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct // check if correct
@ -194,23 +193,23 @@ TEST( Graph, CHECK_ORDERING)
TEST( Values, update_with_large_delta) { TEST( Values, update_with_large_delta) {
// this test ensures that if the update for delta is larger than // this test ensures that if the update for delta is larger than
// the size of the config, it only updates existing variables // the size of the config, it only updates existing variables
Values init; visualSLAM::Values init;
init.insert(1, Pose3()); init.insert(1, Pose3());
init.insert(1, Point3(1.0, 2.0, 3.0)); init.insert(1, Point3(1.0, 2.0, 3.0));
Values expected; visualSLAM::Values expected;
expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
expected.insert(1, Point3(1.1, 2.1, 3.1)); expected.insert(1, Point3(1.1, 2.1, 3.1));
Ordering largeOrdering; Ordering largeOrdering;
Values largeValues = init; visualSLAM::Values largeValues = init;
largeValues.insert(2, Pose3()); largeValues.insert(2, Pose3());
largeOrdering += "x1","l1","x2"; largeOrdering += "x1","l1","x2";
VectorValues delta(largeValues.dims(largeOrdering)); VectorValues delta(largeValues.dims(largeOrdering));
delta[largeOrdering["x1"]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); 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["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); delta[largeOrdering["x2"]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1);
Values actual = init.retract(delta, largeOrdering); visualSLAM::Values actual = init.retract(delta, largeOrdering);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }

4
gtsam/slam/visualSLAM.h
View File

@ -39,8 +39,8 @@ namespace gtsam {
*/ */
typedef TypedSymbol<Pose3,'x'> PoseKey; ///< The key type used for poses typedef TypedSymbol<Pose3,'x'> PoseKey; ///< The key type used for poses
typedef TypedSymbol<Point3,'l'> PointKey; ///< The key type used for points typedef TypedSymbol<Point3,'l'> PointKey; ///< The key type used for points
typedef LieValues<PoseKey> PoseValues; ///< Values used for poses typedef Values<PoseKey> PoseValues; ///< Values used for poses
typedef LieValues<PointKey> PointValues; ///< Values used for points typedef Values<PointKey> PointValues; ///< Values used for points
typedef TupleValues2<PoseValues, PointValues> Values; ///< Values data structure typedef TupleValues2<PoseValues, PointValues> Values; ///< Values data structure
typedef boost::shared_ptr<Values> shared_values; ///< shared pointer to values data structure typedef boost::shared_ptr<Values> shared_values; ///< shared pointer to values data structure

4
tests/testExtendedKalmanFilter.cpp
View File

@ -20,14 +20,14 @@
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h> #include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/LieValues-inl.h> #include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
using namespace gtsam; using namespace gtsam;
// Define Types for System Test // Define Types for System Test
typedef TypedSymbol<Point2, 'x'> TestKey; typedef TypedSymbol<Point2, 'x'> TestKey;
typedef LieValues<TestKey> TestValues; typedef Values<TestKey> TestValues;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ExtendedKalmanFilter, linear ) { TEST( ExtendedKalmanFilter, linear ) {

4
tests/testGaussianJunctionTree.cpp
View File

@ -109,8 +109,8 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal )
TEST( GaussianJunctionTree, optimizeMultiFrontal2) TEST( GaussianJunctionTree, optimizeMultiFrontal2)
{ {
// create a graph // create a graph
Graph nlfg = createNonlinearFactorGraph(); example::Graph nlfg = createNonlinearFactorGraph();
Values noisy = createNoisyValues(); example::Values noisy = createNoisyValues();
Ordering ordering; ordering += "x1","x2","l1"; Ordering ordering; ordering += "x1","x2","l1";
GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering); GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering);

28
tests/testInference.cpp
View File

@ -50,29 +50,27 @@ TEST( Inference, marginals )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Inference, marginals2) TEST( Inference, marginals2)
{ {
using namespace gtsam::planarSLAM; planarSLAM::Graph fg;
Graph fg;
SharedDiagonal poseModel(sharedSigma(3, 0.1)); SharedDiagonal poseModel(sharedSigma(3, 0.1));
SharedDiagonal pointModel(sharedSigma(3, 0.1)); SharedDiagonal pointModel(sharedSigma(3, 0.1));
fg.addPrior(PoseKey(0), Pose2(), poseModel); fg.addPrior(planarSLAM::PoseKey(0), Pose2(), poseModel);
fg.addOdometry(PoseKey(0), PoseKey(1), Pose2(1.0,0.0,0.0), poseModel); fg.addOdometry(planarSLAM::PoseKey(0), planarSLAM::PoseKey(1), Pose2(1.0,0.0,0.0), poseModel);
fg.addOdometry(PoseKey(1), PoseKey(2), Pose2(1.0,0.0,0.0), poseModel); fg.addOdometry(planarSLAM::PoseKey(1), planarSLAM::PoseKey(2), Pose2(1.0,0.0,0.0), poseModel);
fg.addBearingRange(PoseKey(0), PointKey(0), Rot2(), 1.0, pointModel); fg.addBearingRange(planarSLAM::PoseKey(0), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(PoseKey(1), PointKey(0), Rot2(), 1.0, pointModel); fg.addBearingRange(planarSLAM::PoseKey(1), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(PoseKey(2), PointKey(0), Rot2(), 1.0, pointModel); fg.addBearingRange(planarSLAM::PoseKey(2), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel);
Values init; planarSLAM::Values init;
init.insert(PoseKey(0), Pose2(0.0,0.0,0.0)); init.insert(planarSLAM::PoseKey(0), Pose2(0.0,0.0,0.0));
init.insert(PoseKey(1), Pose2(1.0,0.0,0.0)); init.insert(planarSLAM::PoseKey(1), Pose2(1.0,0.0,0.0));
init.insert(PoseKey(2), Pose2(2.0,0.0,0.0)); init.insert(planarSLAM::PoseKey(2), Pose2(2.0,0.0,0.0));
init.insert(PointKey(0), Point2(1.0,1.0)); init.insert(planarSLAM::PointKey(0), Point2(1.0,1.0));
Ordering ordering(*fg.orderingCOLAMD(init)); Ordering ordering(*fg.orderingCOLAMD(init));
FactorGraph<GaussianFactor>::shared_ptr gfg(fg.linearize(init, ordering)); FactorGraph<GaussianFactor>::shared_ptr gfg(fg.linearize(init, ordering));
GaussianMultifrontalSolver solver(*gfg); GaussianMultifrontalSolver solver(*gfg);
solver.marginalFactor(ordering[PointKey(0)]); solver.marginalFactor(ordering[planarSLAM::PointKey(0)]);
} }
/* ************************************************************************* */ /* ************************************************************************* */

4
tests/testNonlinearEquality.cpp
View File

@ -22,13 +22,13 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h> #include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/nonlinear/LieValues-inl.h> #include <gtsam/nonlinear/Values-inl.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef LieValues<PoseKey> PoseValues; typedef Values<PoseKey> PoseValues;
typedef PriorFactor<PoseValues, PoseKey> PosePrior; typedef PriorFactor<PoseValues, PoseKey> PosePrior;
typedef NonlinearEquality<PoseValues, PoseKey> PoseNLE; typedef NonlinearEquality<PoseValues, PoseKey> PoseNLE;
typedef boost::shared_ptr<PoseNLE> shared_poseNLE; typedef boost::shared_ptr<PoseNLE> shared_poseNLE;

22
tests/testNonlinearFactor.cpp
View File

@ -35,7 +35,7 @@
#include <gtsam/slam/simulated2D.h> #include <gtsam/slam/simulated2D.h>
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h> #include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/LieValues-inl.h> #include <gtsam/nonlinear/Values-inl.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -83,14 +83,14 @@ TEST( NonlinearFactor, NonlinearFactor )
Graph fg = createNonlinearFactorGraph(); Graph fg = createNonlinearFactorGraph();
// create a values structure for the non linear factor graph // create a values structure for the non linear factor graph
Values cfg = createNoisyValues(); example::Values cfg = createNoisyValues();
// get the factor "f1" from the factor graph // get the factor "f1" from the factor graph
Graph::sharedFactor factor = fg[0]; Graph::sharedFactor factor = fg[0];
// calculate the error_vector from the factor "f1" // calculate the error_vector from the factor "f1"
// error_vector = [0.1 0.1] // error_vector = [0.1 0.1]
Vector actual_e = boost::dynamic_pointer_cast<NoiseModelFactor<Values> >(factor)->unwhitenedError(cfg); Vector actual_e = boost::dynamic_pointer_cast<NoiseModelFactor<example::Values> >(factor)->unwhitenedError(cfg);
CHECK(assert_equal(0.1*ones(2),actual_e)); CHECK(assert_equal(0.1*ones(2),actual_e));
// error = 0.5 * [1 1] * [1;1] = 1 // error = 0.5 * [1 1] * [1;1] = 1
@ -104,7 +104,7 @@ TEST( NonlinearFactor, NonlinearFactor )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearFactor, linearize_f1 ) TEST( NonlinearFactor, linearize_f1 )
{ {
Values c = createNoisyValues(); example::Values c = createNoisyValues();
// Grab a non-linear factor // Grab a non-linear factor
Graph nfg = createNonlinearFactorGraph(); Graph nfg = createNonlinearFactorGraph();
@ -126,7 +126,7 @@ TEST( NonlinearFactor, linearize_f1 )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearFactor, linearize_f2 ) TEST( NonlinearFactor, linearize_f2 )
{ {
Values c = createNoisyValues(); example::Values c = createNoisyValues();
// Grab a non-linear factor // Grab a non-linear factor
Graph nfg = createNonlinearFactorGraph(); Graph nfg = createNonlinearFactorGraph();
@ -149,7 +149,7 @@ TEST( NonlinearFactor, linearize_f3 )
Graph::sharedFactor nlf = nfg[2]; Graph::sharedFactor nlf = nfg[2];
// We linearize at noisy config from SmallExample // We linearize at noisy config from SmallExample
Values c = createNoisyValues(); example::Values c = createNoisyValues();
GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary());
GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary());
@ -166,7 +166,7 @@ TEST( NonlinearFactor, linearize_f4 )
Graph::sharedFactor nlf = nfg[3]; Graph::sharedFactor nlf = nfg[3];
// We linearize at noisy config from SmallExample // We linearize at noisy config from SmallExample
Values c = createNoisyValues(); example::Values c = createNoisyValues();
GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary());
GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary());
@ -182,7 +182,7 @@ TEST( NonlinearFactor, size )
Graph fg = createNonlinearFactorGraph(); Graph fg = createNonlinearFactorGraph();
// create a values structure for the non linear factor graph // create a values structure for the non linear factor graph
Values cfg = createNoisyValues(); example::Values cfg = createNoisyValues();
// get some factors from the graph // get some factors from the graph
Graph::sharedFactor factor1 = fg[0], factor2 = fg[1], Graph::sharedFactor factor1 = fg[0], factor2 = fg[1],
@ -202,7 +202,7 @@ TEST( NonlinearFactor, linearize_constraint1 )
Point2 mu(1., -1.); Point2 mu(1., -1.);
Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, 1)); Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, 1));
Values config; example::Values config;
config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary()); GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary());
@ -222,7 +222,7 @@ TEST( NonlinearFactor, linearize_constraint2 )
Point2 z3(1.,-1.); Point2 z3(1.,-1.);
simulated2D::Measurement f0(z3, constraint, 1,1); simulated2D::Measurement f0(z3, constraint, 1,1);
Values config; example::Values config;
config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
config.insert(simulated2D::PointKey(1), Point2(5.0, 4.0)); config.insert(simulated2D::PointKey(1), Point2(5.0, 4.0));
GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary()); GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary());
@ -236,7 +236,7 @@ TEST( NonlinearFactor, linearize_constraint2 )
/* ************************************************************************* */ /* ************************************************************************* */
typedef TypedSymbol<LieVector, 'x'> TestKey; typedef TypedSymbol<LieVector, 'x'> TestKey;
typedef LieValues<TestKey> TestValues; typedef gtsam::Values<TestKey> TestValues;
/* ************************************************************************* */ /* ************************************************************************* */
class TestFactor4 : public NonlinearFactor4<TestValues, TestKey, TestKey, TestKey, TestKey> { class TestFactor4 : public NonlinearFactor4<TestValues, TestKey, TestKey, TestKey, TestKey> {

8
tests/testNonlinearFactorGraph.cpp
View File

@ -51,11 +51,11 @@ TEST( Graph, equals )
TEST( Graph, error ) TEST( Graph, error )
{ {
Graph fg = createNonlinearFactorGraph(); Graph fg = createNonlinearFactorGraph();
Values c1 = createValues(); example::Values c1 = createValues();
double actual1 = fg.error(c1); double actual1 = fg.error(c1);
DOUBLES_EQUAL( 0.0, actual1, 1e-9 ); DOUBLES_EQUAL( 0.0, actual1, 1e-9 );
Values c2 = createNoisyValues(); example::Values c2 = createNoisyValues();
double actual2 = fg.error(c2); double actual2 = fg.error(c2);
DOUBLES_EQUAL( 5.625, actual2, 1e-9 ); DOUBLES_EQUAL( 5.625, actual2, 1e-9 );
} }
@ -85,7 +85,7 @@ TEST( Graph, GET_ORDERING)
TEST( Graph, probPrime ) TEST( Graph, probPrime )
{ {
Graph fg = createNonlinearFactorGraph(); Graph fg = createNonlinearFactorGraph();
Values cfg = createValues(); example::Values cfg = createValues();
// evaluate the probability of the factor graph // evaluate the probability of the factor graph
double actual = fg.probPrime(cfg); double actual = fg.probPrime(cfg);
@ -97,7 +97,7 @@ TEST( Graph, probPrime )
TEST( Graph, linearize ) TEST( Graph, linearize )
{ {
Graph fg = createNonlinearFactorGraph(); Graph fg = createNonlinearFactorGraph();
Values initial = createNoisyValues(); example::Values initial = createNoisyValues();
boost::shared_ptr<FactorGraph<GaussianFactor> > linearized = fg.linearize(initial, *initial.orderingArbitrary()); boost::shared_ptr<FactorGraph<GaussianFactor> > linearized = fg.linearize(initial, *initial.orderingArbitrary());
FactorGraph<GaussianFactor> expected = createGaussianFactorGraph(*initial.orderingArbitrary()); FactorGraph<GaussianFactor> expected = createGaussianFactorGraph(*initial.orderingArbitrary());
CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations

10
tests/testNonlinearISAM.cpp
View File

@ -12,7 +12,7 @@
using namespace gtsam; using namespace gtsam;
using namespace planarSLAM; using namespace planarSLAM;
typedef NonlinearISAM<Values> PlanarISAM; typedef NonlinearISAM<planarSLAM::Values> PlanarISAM;
const double tol=1e-5; const double tol=1e-5;
@ -30,8 +30,8 @@ TEST(testNonlinearISAM, markov_chain ) {
Graph start_factors; Graph start_factors;
start_factors.addPoseConstraint(key, cur_pose); start_factors.addPoseConstraint(key, cur_pose);
Values init; planarSLAM::Values init;
Values expected; planarSLAM::Values expected;
init.insert(key, cur_pose); init.insert(key, cur_pose);
expected.insert(key, cur_pose); expected.insert(key, cur_pose);
isam.update(start_factors, init); isam.update(start_factors, init);
@ -43,7 +43,7 @@ TEST(testNonlinearISAM, markov_chain ) {
Graph new_factors; Graph new_factors;
PoseKey key1(i-1), key2(i); PoseKey key1(i-1), key2(i);
new_factors.addOdometry(key1, key2, z, model); new_factors.addOdometry(key1, key2, z, model);
Values new_init; planarSLAM::Values new_init;
// perform a check on changing orderings // perform a check on changing orderings
if (i == 5) { if (i == 5) {
@ -67,7 +67,7 @@ TEST(testNonlinearISAM, markov_chain ) {
} }
// verify values - all but the last one should be very close // verify values - all but the last one should be very close
Values actual = isam.estimate(); planarSLAM::Values actual = isam.estimate();
for (size_t i=0; i<nrPoses; ++i) { for (size_t i=0; i<nrPoses; ++i) {
PoseKey cur_key(i); PoseKey cur_key(i);
EXPECT(assert_equal(expected[cur_key], actual[cur_key], tol)); EXPECT(assert_equal(expected[cur_key], actual[cur_key], tol));

6
tests/testPose2SLAMwSPCG.cpp
View File

@ -40,7 +40,7 @@ TEST(testPose2SLAMwSPCG, example1) {
graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ; graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ;
graph.addPrior(x1, Pose2(0,0,0), sigma) ; graph.addPrior(x1, Pose2(0,0,0), sigma) ;
Values initial; pose2SLAM::Values initial;
initial.insert(x1, Pose2( 0, 0, 0)); initial.insert(x1, Pose2( 0, 0, 0));
initial.insert(x2, Pose2( 0, 2.1, 0.01)); initial.insert(x2, Pose2( 0, 2.1, 0.01));
initial.insert(x3, Pose2( 0, 3.9,-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(x8, Pose2(3.9, 2.1, 0.01));
initial.insert(x9, Pose2(4.1, 3.9,-0.01)); initial.insert(x9, Pose2(4.1, 3.9,-0.01));
Values expected; pose2SLAM::Values expected;
expected.insert(x1, Pose2(0.0, 0.0, 0.0)); expected.insert(x1, Pose2(0.0, 0.0, 0.0));
expected.insert(x2, Pose2(0.0, 2.0, 0.0)); expected.insert(x2, Pose2(0.0, 2.0, 0.0));
expected.insert(x3, Pose2(0.0, 4.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(x8, Pose2(4.0, 2.0, 0.0));
expected.insert(x9, Pose2(4.0, 4.0, 0.0)); expected.insert(x9, Pose2(4.0, 4.0, 0.0));
Values actual = optimizeSPCG(graph, initial); pose2SLAM::Values actual = optimizeSPCG(graph, initial);
EXPECT(assert_equal(expected, actual, tol)); EXPECT(assert_equal(expected, actual, tol));
} }

10
tests/testSerialization.cpp
View File

@ -445,7 +445,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::simulated2D::Measurement, "gtsam::simulated2D::Me
TEST (Serialization, smallExample) { TEST (Serialization, smallExample) {
using namespace example; using namespace example;
Graph nfg = createNonlinearFactorGraph(); Graph nfg = createNonlinearFactorGraph();
Values c1 = createValues(); example::Values c1 = createValues();
EXPECT(equalsObj(nfg)); EXPECT(equalsObj(nfg));
EXPECT(equalsXML(nfg)); EXPECT(equalsXML(nfg));
@ -464,7 +464,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::planarSLAM::Constraint, "gtsam::planarSLAM::Cons
/* ************************************************************************* */ /* ************************************************************************* */
TEST (Serialization, planar_system) { TEST (Serialization, planar_system) {
using namespace planarSLAM; using namespace planarSLAM;
Values values; planarSLAM::Values values;
values.insert(PointKey(3), Point2(1.0, 2.0)); values.insert(PointKey(3), Point2(1.0, 2.0));
values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3)); values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3));
@ -490,7 +490,7 @@ TEST (Serialization, planar_system) {
// text // text
EXPECT(equalsObj<PoseKey>(PoseKey(2))); EXPECT(equalsObj<PoseKey>(PoseKey(2)));
EXPECT(equalsObj<PointKey>(PointKey(3))); EXPECT(equalsObj<PointKey>(PointKey(3)));
EXPECT(equalsObj<Values>(values)); EXPECT(equalsObj<planarSLAM::Values>(values));
EXPECT(equalsObj<Prior>(prior)); EXPECT(equalsObj<Prior>(prior));
EXPECT(equalsObj<Bearing>(bearing)); EXPECT(equalsObj<Bearing>(bearing));
EXPECT(equalsObj<BearingRange>(bearingRange)); EXPECT(equalsObj<BearingRange>(bearingRange));
@ -502,7 +502,7 @@ TEST (Serialization, planar_system) {
// xml // xml
EXPECT(equalsXML<PoseKey>(PoseKey(2))); EXPECT(equalsXML<PoseKey>(PoseKey(2)));
EXPECT(equalsXML<PointKey>(PointKey(3))); EXPECT(equalsXML<PointKey>(PointKey(3)));
EXPECT(equalsXML<Values>(values)); EXPECT(equalsXML<planarSLAM::Values>(values));
EXPECT(equalsXML<Prior>(prior)); EXPECT(equalsXML<Prior>(prior));
EXPECT(equalsXML<Bearing>(bearing)); EXPECT(equalsXML<Bearing>(bearing));
EXPECT(equalsXML<BearingRange>(bearingRange)); EXPECT(equalsXML<BearingRange>(bearingRange));
@ -524,7 +524,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::visualSLAM::StereoFactor, "gtsam::visualSLAM::
/* ************************************************************************* */ /* ************************************************************************* */
TEST (Serialization, visual_system) { TEST (Serialization, visual_system) {
using namespace visualSLAM; using namespace visualSLAM;
Values values; visualSLAM::Values values;
PoseKey x1(1), x2(2); PoseKey x1(1), x2(2);
PointKey l1(1), l2(2); PointKey l1(1), l2(2);
Pose3 pose1 = pose3, pose2 = pose3.inverse(); Pose3 pose1 = pose3, pose2 = pose3.inverse();

44
tests/testTupleValues.cpp
View File

@ -39,9 +39,9 @@ static const double tol = 1e-5;
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef LieValues<PoseKey> PoseValues; typedef Values<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues; typedef Values<PointKey> PointValues;
typedef TupleValues2<PoseValues, PointValues> Values; typedef TupleValues2<PoseValues, PointValues> TestValues;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TupleValues, constructors ) TEST( TupleValues, constructors )
@ -49,14 +49,14 @@ TEST( TupleValues, constructors )
Pose2 x1(1,2,3), x2(6,7,8); Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10); Point2 l1(4,5), l2(9,10);
Values::Values1 cfg1; TestValues::Values1 cfg1;
cfg1.insert(PoseKey(1), x1); cfg1.insert(PoseKey(1), x1);
cfg1.insert(PoseKey(2), x2); cfg1.insert(PoseKey(2), x2);
Values::Values2 cfg2; TestValues::Values2 cfg2;
cfg2.insert(PointKey(1), l1); cfg2.insert(PointKey(1), l1);
cfg2.insert(PointKey(2), l2); cfg2.insert(PointKey(2), l2);
Values actual(cfg1, cfg2), expected; TestValues actual(cfg1, cfg2), expected;
expected.insert(PoseKey(1), x1); expected.insert(PoseKey(1), x1);
expected.insert(PoseKey(2), x2); expected.insert(PoseKey(2), x2);
expected.insert(PointKey(1), l1); expected.insert(PointKey(1), l1);
@ -71,13 +71,13 @@ TEST( TupleValues, insert_equals1 )
Pose2 x1(1,2,3), x2(6,7,8); Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10); Point2 l1(4,5), l2(9,10);
Values expected; TestValues expected;
expected.insert(PoseKey(1), x1); expected.insert(PoseKey(1), x1);
expected.insert(PoseKey(2), x2); expected.insert(PoseKey(2), x2);
expected.insert(PointKey(1), l1); expected.insert(PointKey(1), l1);
expected.insert(PointKey(2), l2); expected.insert(PointKey(2), l2);
Values actual; TestValues actual;
actual.insert(PoseKey(1), x1); actual.insert(PoseKey(1), x1);
actual.insert(PoseKey(2), x2); actual.insert(PoseKey(2), x2);
actual.insert(PointKey(1), l1); actual.insert(PointKey(1), l1);
@ -92,13 +92,13 @@ TEST( TupleValues, insert_equals2 )
Pose2 x1(1,2,3), x2(6,7,8); Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10); Point2 l1(4,5), l2(9,10);
Values values1; TestValues values1;
values1.insert(PoseKey(1), x1); values1.insert(PoseKey(1), x1);
values1.insert(PoseKey(2), x2); values1.insert(PoseKey(2), x2);
values1.insert(PointKey(1), l1); values1.insert(PointKey(1), l1);
values1.insert(PointKey(2), l2); values1.insert(PointKey(2), l2);
Values values2; TestValues values2;
values2.insert(PoseKey(1), x1); values2.insert(PoseKey(1), x1);
values2.insert(PoseKey(2), x2); values2.insert(PoseKey(2), x2);
values2.insert(PointKey(1), l1); values2.insert(PointKey(1), l1);
@ -116,7 +116,7 @@ TEST( TupleValues, insert_duplicate )
Pose2 x1(1,2,3), x2(6,7,8); Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10); Point2 l1(4,5), l2(9,10);
Values values1; TestValues values1;
values1.insert(1, x1); // 3 values1.insert(1, x1); // 3
values1.insert(2, x2); // 6 values1.insert(2, x2); // 6
values1.insert(1, l1); // 8 values1.insert(1, l1); // 8
@ -134,7 +134,7 @@ TEST( TupleValues, size_dim )
Pose2 x1(1,2,3), x2(6,7,8); Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10); Point2 l1(4,5), l2(9,10);
Values values1; TestValues values1;
values1.insert(PoseKey(1), x1); values1.insert(PoseKey(1), x1);
values1.insert(PoseKey(2), x2); values1.insert(PoseKey(2), x2);
values1.insert(PointKey(1), l1); values1.insert(PointKey(1), l1);
@ -150,7 +150,7 @@ TEST(TupleValues, at)
Pose2 x1(1,2,3), x2(6,7,8); Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10); Point2 l1(4,5), l2(9,10);
Values values1; TestValues values1;
values1.insert(PoseKey(1), x1); values1.insert(PoseKey(1), x1);
values1.insert(PoseKey(2), x2); values1.insert(PoseKey(2), x2);
values1.insert(PointKey(1), l1); values1.insert(PointKey(1), l1);
@ -171,7 +171,7 @@ TEST(TupleValues, zero_expmap_logmap)
Pose2 x1(1,2,3), x2(6,7,8); Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10); Point2 l1(4,5), l2(9,10);
Values values1; TestValues values1;
values1.insert(PoseKey(1), x1); values1.insert(PoseKey(1), x1);
values1.insert(PoseKey(2), x2); values1.insert(PoseKey(2), x2);
values1.insert(PointKey(1), l1); values1.insert(PointKey(1), l1);
@ -192,13 +192,13 @@ TEST(TupleValues, zero_expmap_logmap)
delta[o["l1"]] = Vector_(2, 1.0, 1.1); delta[o["l1"]] = Vector_(2, 1.0, 1.1);
delta[o["l2"]] = Vector_(2, 1.3, 1.4); delta[o["l2"]] = Vector_(2, 1.3, 1.4);
Values expected; TestValues expected;
expected.insert(PoseKey(1), x1.retract(Vector_(3, 1.0, 1.1, 1.2))); expected.insert(PoseKey(1), x1.retract(Vector_(3, 1.0, 1.1, 1.2)));
expected.insert(PoseKey(2), x2.retract(Vector_(3, 1.3, 1.4, 1.5))); expected.insert(PoseKey(2), x2.retract(Vector_(3, 1.3, 1.4, 1.5)));
expected.insert(PointKey(1), Point2(5.0, 6.1)); expected.insert(PointKey(1), Point2(5.0, 6.1));
expected.insert(PointKey(2), Point2(10.3, 11.4)); expected.insert(PointKey(2), Point2(10.3, 11.4));
Values actual = values1.retract(delta, o); TestValues actual = values1.retract(delta, o);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
// Check log // Check log
@ -216,12 +216,12 @@ typedef TypedSymbol<Point3, 'b'> Point3Key;
typedef TypedSymbol<Point3, 'c'> Point3Key2; typedef TypedSymbol<Point3, 'c'> Point3Key2;
// some values types // some values types
typedef LieValues<PoseKey> PoseValues; typedef Values<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues; typedef Values<PointKey> PointValues;
typedef LieValues<LamKey> LamValues; typedef Values<LamKey> LamValues;
typedef LieValues<Pose3Key> Pose3Values; typedef Values<Pose3Key> Pose3Values;
typedef LieValues<Point3Key> Point3Values; typedef Values<Point3Key> Point3Values;
typedef LieValues<Point3Key2> Point3Values2; typedef Values<Point3Key2> Point3Values2;
// some TupleValues types // some TupleValues types
typedef TupleValues<PoseValues, TupleValuesEnd<PointValues> > ValuesA; typedef TupleValues<PoseValues, TupleValuesEnd<PointValues> > ValuesA;