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/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/LieValues.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
@ -28,7 +28,7 @@
using namespace gtsam;
typedef TypedSymbol<Pose3, 'x'> PoseKey;
typedef LieValues<PoseKey> PoseValues;
typedef Values<PoseKey> PoseValues;
/**
* 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");
// initialize to noisy points
Values initialEstimate;
planarSLAM::Values initialEstimate;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
@ -86,7 +86,7 @@ int main(int argc, char** argv) {
initialEstimate.print("initial estimate");
// optimize using Levenberg-Marquardt optimization with an ordering from colamd
Values result = optimize<Graph, Values>(graph, initialEstimate);
planarSLAM::Values result = optimize<Graph, planarSLAM::Values>(graph, initialEstimate);
result.print("final result");
return 0;

26
examples/PlanarSLAMSelfContained_advanced.cpp
View File

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

4
examples/Pose2SLAMExample_advanced.cpp
View File

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

4
examples/Pose2SLAMExample_easy.cpp
View File

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

10
examples/Pose2SLAMwSPCG_advanced.cpp
View File

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

3
examples/Pose2SLAMwSPCG_easy.cpp
View File

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

28
examples/SimpleRotation.cpp
View File

@ -23,12 +23,12 @@
#include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/NoiseModel.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/NonlinearOptimization-inl.h>
/*
* TODO: make factors independent of Values
* TODO: make factors independent of RotValues
* TODO: make toplevel documentation
* TODO: Clean up nonlinear optimization API
*/
@ -44,17 +44,17 @@ using namespace gtsam;
*
* To create a domain:
* - 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 graph container acting on a given Values
* - a "RotValues" structure needs to be defined to store the system state
* - a graph container acting on a given RotValues
*
* 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
* and 2D poses.
*/
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 NonlinearFactorGraph<Values> Graph; /// Graph container for constraints - needs to know type of variables
typedef Values<Key> RotValues; /// Class to store values - acts as a state for the system
typedef NonlinearFactorGraph<RotValues> Graph; /// Graph container for constraints - needs to know type of variables
const double degree = M_PI / 180;
@ -71,7 +71,7 @@ int main() {
* with a model of the noise on the measurement.
*
* 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.
*
* In general, creating a factor requires:
@ -83,7 +83,7 @@ int main() {
prior.print("goal angle");
SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree);
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
@ -101,7 +101,7 @@ int main() {
/**
* Step 4: create an initial estimate
* 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
* 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
* 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
* are multiple types of variables.
*/
Values initial;
RotValues initial;
initial.insert(key, Rot2::fromAngle(20 * degree));
initial.print("initial estimate");
@ -123,10 +123,10 @@ int main() {
* After formulating the problem with a graph of constraints
* and an initial estimate, executing optimization is as simple
* 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.
*/
Values result = optimize<Graph, Values>(graph, initial);
RotValues result = optimize<Graph, RotValues>(graph, initial);
result.print("final result");
return 0;

4
examples/easyPoint2KalmanFilter.cpp
View File

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

20
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -22,7 +22,7 @@
#include <gtsam/slam/PriorFactor.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/Key.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
@ -35,7 +35,7 @@ using namespace std;
using namespace gtsam;
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() {
@ -48,7 +48,7 @@ int main() {
Ordering::shared_ptr ordering(new Ordering);
// Create a structure to hold the linearization points
Values linearizationPoints;
KalmanValues linearizationPoints;
// Ground truth example
// 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
Point2 x_initial(0,0);
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
linearizationPoints.insert(x0, x_initial);
linearFactorGraph->push_back(factor1.linearize(linearizationPoints, *ordering));
@ -95,7 +95,7 @@ int main() {
Point2 difference(1,0);
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
linearizationPoints.insert(x1, x_initial);
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.
Point2 z1(1.0, 0.0);
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
linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering));
@ -225,7 +225,7 @@ int main() {
// Create a nonlinear factor describing the motion model
difference = Point2(1,0);
Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
BetweenFactor<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
linearizationPoints.insert(x2, x1_update);
@ -263,7 +263,7 @@ int main() {
// And update using z2 ...
Point2 z2(2.0, 0.0);
SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
PriorFactor<Values, Key> factor8(x2, z2, R2);
PriorFactor<KalmanValues, Key> factor8(x2, z2, R2);
// Linearize the factor and add it to the linear factor graph
linearFactorGraph->push_back(factor8.linearize(linearizationPoints, *ordering));
@ -314,7 +314,7 @@ int main() {
// Create a nonlinear factor describing the motion model
difference = Point2(1,0);
Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
BetweenFactor<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
linearizationPoints.insert(x3, x2_update);
@ -352,7 +352,7 @@ int main() {
// And update using z3 ...
Point2 z3(3.0, 0.0);
SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
PriorFactor<Values, Key> factor12(x3, z3, R3);
PriorFactor<KalmanValues, Key> factor12(x3, z3, R3);
// Linearize the factor and add it to the linear factor graph
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.
*/
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) {
// 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
initialValues = shared_ptr<Values> (new Values());
initialValues = shared_ptr<visualSLAM::Values> (new visualSLAM::Values());
initialValues->insert(pose_id, pose);
for (size_t i = 0; i < measurements.size(); i++) {
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
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,
// create a graph of new factors and update ISAM
for (size_t i = 0; i < g_measurements.size(); i++) {
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);
isam.update(*newFactors, *initialValues);
Values currentEstimate = isam.estimate();
visualSLAM::Values currentEstimate = isam.estimate();
cout << "****************************************************" << endl;
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.
* 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.
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
@ -136,7 +136,7 @@ int main(int argc, char* argv[]) {
boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
// Create an initial Values structure using groundtruth values as the initial estimates
boost::shared_ptr<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;
initialEstimates->print("INITIAL ESTIMATES: ");
@ -148,7 +148,7 @@ int main(int argc, char* argv[]) {
// Optimize the graph
cout << "*******************************************************" << endl;
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
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
* 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.
*
* \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
* 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.
*
* \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
* either BayesNet<GaussianConditional> (or GaussianBayesNet) or BayesTree<GaussianConditional>.
* @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.
* @param initialDelta The initial trust region radius.
* @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
* templated.
*
* @tparam VALUES The LieValues or TupleValues\Emph{N} that contains the
* @tparam VALUES The Values or TupleValues\Emph{N} that contains the
* variables.
*/
template <class VALUES>

4
gtsam/nonlinear/Makefile.am
View File

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

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

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

6
gtsam/nonlinear/TupleValues.h
View File

@ -16,7 +16,7 @@
* @author Alex Cunningham
*/
#include <gtsam/nonlinear/LieValues.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/VectorValues.h>
#pragma once
@ -24,11 +24,11 @@
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
* do this with recursive templates (instead of blind pointer casting) to
* reduce run-time overhead and keep static type checking. The interface
* mimics that of a single LieValues.
* mimics that of a single Values.
*
* This uses a recursive structure of values pairs to form a lisp-like
* list, with a special case (TupleValuesEnd) that contains only one values

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
*/
@ -28,9 +28,9 @@
#include <gtsam/base/Lie-inl.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;
@ -38,8 +38,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
void LieValues<J>::print(const string &s) const {
cout << "LieValues " << s << ", size " << values_.size() << "\n";
void Values<J>::print(const string &s) const {
cout << "Values " << s << ", size " << values_.size() << "\n";
BOOST_FOREACH(const KeyValuePair& v, values_) {
gtsam::print(v.second, (string)(v.first));
}
@ -47,7 +47,7 @@ namespace gtsam {
/* ************************************************************************* */
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;
BOOST_FOREACH(const KeyValuePair& v, values_) {
if (!expected.exists(v.first)) return false;
@ -59,15 +59,15 @@ namespace gtsam {
/* ************************************************************************* */
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);
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;
}
/* ************************************************************************* */
template<class J>
size_t LieValues<J>::dim() const {
size_t Values<J>::dim() const {
size_t n = 0;
BOOST_FOREACH(const KeyValuePair& value, values_)
n += value.second.dim();
@ -76,26 +76,26 @@ namespace gtsam {
/* ************************************************************************* */
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));
}
/* ************************************************************************* */
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));
}
/* ************************************************************************* */
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_)
insert(v.first, v.second);
}
/* ************************************************************************* */
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::optional<typename J::Value> t = cfg.exists_(v.first);
if (t) values_[v.first] = *t;
@ -104,13 +104,13 @@ namespace gtsam {
/* ************************************************************************* */
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;
}
/* ************************************************************************* */
template<class J>
std::list<J> LieValues<J>::keys() const {
std::list<J> Values<J>::keys() const {
std::list<J> ret;
BOOST_FOREACH(const KeyValuePair& v, values_)
ret.push_back(v.first);
@ -119,16 +119,16 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
void LieValues<J>::erase(const J& j) {
void Values<J>::erase(const J& j) {
size_t dim; // unused
erase(j, dim);
}
/* ************************************************************************* */
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);
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();
values_.erase(it);
}
@ -136,8 +136,8 @@ namespace gtsam {
/* ************************************************************************* */
// todo: insert for every element is inefficient
template<class J>
LieValues<J> LieValues<J>::retract(const VectorValues& delta, const Ordering& ordering) const {
LieValues<J> newValues;
Values<J> Values<J>::retract(const VectorValues& delta, const Ordering& ordering) const {
Values<J> newValues;
typedef pair<J,typename J::Value> KeyValue;
BOOST_FOREACH(const KeyValue& value, this->values_) {
const J& j = value.first;
@ -153,7 +153,7 @@ namespace gtsam {
/* ************************************************************************* */
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);
this->apply(dimCollector);
return dimCollector.dimensions;
@ -161,7 +161,7 @@ namespace gtsam {
/* ************************************************************************* */
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
_ValuesKeyOrderer keyOrderer(firstVar);
this->apply(keyOrderer);
@ -171,12 +171,12 @@ namespace gtsam {
// /* ************************************************************************* */
// // todo: insert for every element is inefficient
// 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()) {
// 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.");
// }
// LieValues<J> newValues;
// Values<J> newValues;
// int delta_offset = 0;
// typedef pair<J,typename J::Value> KeyValue;
// BOOST_FOREACH(const KeyValue& value, this->values_) {
@ -193,7 +193,7 @@ namespace gtsam {
// todo: insert for every element is inefficient
// todo: currently only logmaps elements in both configs
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));
localCoordinates(cp, ordering, delta);
return delta;
@ -201,7 +201,7 @@ namespace gtsam {
/* ************************************************************************* */
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;
BOOST_FOREACH(const KeyValue& value, cp) {
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
*
* @brief A templated config for Lie-group elements
*
* Detailed story:
* A values structure is a map from keys to values. It is used to specify the value of a bunch
* of variables in a factor graph. A 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
* 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)
*/
template<class J>
class LieValues {
class Values {
public:
@ -72,18 +72,18 @@ namespace gtsam {
public:
LieValues() {}
LieValues(const LieValues& config) :
Values() {}
Values(const Values& config) :
values_(config.values_) {}
template<class J_ALT>
LieValues(const LieValues<J_ALT>& other) {} // do nothing when initializing with wrong type
virtual ~LieValues() {}
Values(const Values<J_ALT>& other) {} // do nothing when initializing with wrong type
virtual ~Values() {}
/** print */
void print(const std::string &s="") const;
/** 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 */
const Value& at(const J& j) const;
@ -120,13 +120,13 @@ namespace gtsam {
size_t dim() const;
/** 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) */
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) */
void localCoordinates(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const;
void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const;
// imperative methods:
@ -134,10 +134,10 @@ namespace gtsam {
void insert(const J& j, const Value& val);
/** 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 */
void update(const LieValues& cfg);
void update(const Values& cfg);
/** single element change of existing element */
void update(const J& j, const Value& val);
@ -157,7 +157,7 @@ namespace gtsam {
std::list<J> keys() const;
/** Replace all keys and variables */
LieValues& operator=(const LieValues& rhs) {
Values& operator=(const Values& rhs) {
values_ = rhs.values_;
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
*/
@ -22,32 +22,32 @@ using namespace boost::assign;
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
using namespace gtsam;
using namespace std;
static double inf = std::numeric_limits<double>::infinity();
typedef TypedSymbol<LieVector, 'v'> VecKey;
typedef LieValues<VecKey> Values;
typedef Values<VecKey> TestValues;
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);
expected.insert(key1,v);
Values actual;
TestValues actual;
actual.insert(key1,v);
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 v2 = Vector_(3, 5.0, 6.0, 8.0);
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 v2 = Vector_(3, inf, inf, inf);
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 v2 = Vector_(3, 8.0, 9.0, 1.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 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(key2, Vector_(3, 5.0, 6.0, 7.0));
// 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(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[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(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(key2, Vector_(3, 5.0, 6.0, 7.0));
//
@ -151,7 +151,7 @@ TEST(LieValues, expmap_a)
// VectorValues increment(config0.dims(ordering));
// 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(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(key2, Vector_(3, 5.0, 6.0, 7.0));
//
@ -169,7 +169,7 @@ TEST(LieValues, expmap_a)
// 1.0, 1.1, 1.2,
// 1.3, 1.4, 1.5);
//
// Values expected;
// TestValues expected;
// expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
// 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(key2, Vector_(3, 5.0, 6.0, 7.0));
//config0.print("config0");
CHECK(equal(config0, config0));
CHECK(config0.equals(config0));
LieValues<string,Pose2> poseconfig;
TestValues<string,Pose2> poseconfig;
poseconfig.insert("p1", Pose2(1,2,3));
poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
//poseconfig.print("poseconfig");
@ -195,10 +195,10 @@ TEST(LieValues, expmap_a)
}*/
/* ************************************************************************* */
/*TEST(LieValues, extract_keys)
/*TEST(TestValues, extract_keys)
{
typedef TypedSymbol<Pose2, 'x'> PoseKey;
LieValues<PoseKey, Pose2> config;
TestValues<PoseKey, Pose2> config;
config.insert(PoseKey(1), 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(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(key2, Vector_(1, 2.));
Values superset;
TestValues superset;
superset.insert(key1, Vector_(1, -1.));
superset.insert(key2, Vector_(1, -2.));
superset.insert(key3, Vector_(1, -3.));
config0.update(superset);
Values expected;
TestValues expected;
expected.insert(key1, Vector_(1, -1.));
expected.insert(key2, Vector_(1, -2.));
CHECK(assert_equal(expected,config0));
}
/* ************************************************************************* */
TEST(LieValues, dummy_initialization)
TEST(TestValues, dummy_initialization)
{
typedef TypedSymbol<LieVector, 'z'> Key;
typedef LieValues<Key> Values1;
typedef Values<Key> Values1;
Values1 init1;
init1.insert(Key(1), Vector_(2, 1.0, 2.0));
init1.insert(Key(2), Vector_(2, 4.0, 3.0));
Values init2;
TestValues init2;
init2.insert(key1, Vector_(2, 1.0, 2.0));
init2.insert(key2, Vector_(2, 4.0, 3.0));
Values1 actual1(init2);
Values actual2(init1);
TestValues actual2(init1);
EXPECT(actual1.empty());
EXPECT(actual2.empty());

2
gtsam/slam/PartialPriorFactor.h
View File

@ -33,7 +33,7 @@ namespace gtsam {
*
* It takes two template parameters:
* 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
* 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
* It takes two template parameters:
* 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
* 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/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
namespace gtsam {
INSTANTIATE_VALUES(simulated3D::PointKey)
INSTANTIATE_VALUES(simulated3D::PoseKey)
using namespace simulated3D;
INSTANTIATE_LIE_VALUES(PointKey)
INSTANTIATE_LIE_VALUES(PoseKey)
INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues)
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, 'l'> PointKey;
typedef LieValues<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
typedef TupleValues2<PoseValues, PointValues> Values;
/**

9
gtsam/slam/planarSLAM.cpp
View File

@ -24,11 +24,10 @@
// Use planarSLAM namespace for specific SLAM instance
namespace gtsam {
using namespace planarSLAM;
INSTANTIATE_LIE_VALUES(PointKey)
INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values)
INSTANTIATE_VALUES(planarSLAM::PointKey)
INSTANTIATE_TUPLE_VALUES2(planarSLAM::PoseValues, planarSLAM::PointValues)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(planarSLAM::Values)
INSTANTIATE_NONLINEAR_OPTIMIZER(planarSLAM::Graph, planarSLAM::Values)
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 TypedSymbol<Point2, 'l'> PointKey;
/// Typedef for LieValues structure with PoseKey type
typedef LieValues<PoseKey> PoseValues;
/// Typedef for Values structure with PoseKey type
typedef Values<PoseKey> PoseValues;
/// Typedef for LieValues structure with PointKey type
typedef LieValues<PointKey> PointValues;
/// Typedef for Values structure with PointKey type
typedef Values<PointKey> PointValues;
/// Values class, inherited from TupleValues2, using PoseKeys and PointKeys
struct Values: public TupleValues2<PoseValues, PointValues> {

11
gtsam/slam/pose2SLAM.cpp
View File

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

6
gtsam/slam/pose2SLAM.h
View File

@ -19,7 +19,7 @@
#include <gtsam/base/LieVector.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LieValues.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h>
@ -37,8 +37,8 @@ namespace gtsam {
/// Keys with Pose2 and symbol 'x'
typedef TypedSymbol<Pose2, 'x'> Key;
/// LieValues with type 'Key'
typedef LieValues<Key> Values;
/// Values with type 'Key'
typedef Values<Key> Values;
/**
* 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/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
// Use pose3SLAM namespace for specific SLAM instance
namespace gtsam {
using namespace pose3SLAM;
INSTANTIATE_LIE_VALUES(Key)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values)
INSTANTIATE_VALUES(pose3SLAM::Key)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(pose3SLAM::Values)
INSTANTIATE_NONLINEAR_OPTIMIZER(pose3SLAM::Graph, pose3SLAM::Values)
namespace pose3SLAM {

6
gtsam/slam/pose3SLAM.h
View File

@ -18,7 +18,7 @@
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/LieValues.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h>
@ -33,8 +33,8 @@ namespace gtsam {
/// Creates a Key with data Pose3 and symbol 'x'
typedef TypedSymbol<Pose3, 'x'> Key;
/// Creates a LieValues structure with type 'Key'
typedef LieValues<Key> Values;
/// Creates a Values structure with type 'Key'
typedef Values<Key> Values;
/**
* 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/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
namespace gtsam {
INSTANTIATE_VALUES(simulated2D::PoseKey)
using namespace simulated2D;
INSTANTIATE_LIE_VALUES(PoseKey)
INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues)
namespace simulated2D {

4
gtsam/slam/simulated2D.h
View File

@ -31,8 +31,8 @@ namespace gtsam {
// Simulated2D robots have no orientation, just a position
typedef TypedSymbol<Point2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef LieValues<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
/**
* 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
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef LieValues<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
typedef TupleValues2<PoseValues, PointValues> Values;
//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 TypedSymbol<GeneralCamera, 'x'> CameraKey;
typedef TypedSymbol<Point3, 'l'> PointKey;
typedef LieValues<CameraKey> CameraConfig;
typedef LieValues<PointKey> PointConfig;
typedef TupleValues2<CameraConfig, PointConfig> Values;
typedef GeneralSFMFactor<Values, CameraKey, PointKey> Projection;
typedef NonlinearEquality<Values, CameraKey> CameraConstraint;
typedef NonlinearEquality<Values, PointKey> Point3Constraint;
typedef Values<CameraKey> CameraConfig;
typedef Values<PointKey> PointConfig;
typedef TupleValues2<CameraConfig, PointConfig> VisualValues;
typedef GeneralSFMFactor<VisualValues, CameraKey, PointKey> Projection;
typedef NonlinearEquality<VisualValues, CameraKey> CameraConstraint;
typedef NonlinearEquality<VisualValues, PointKey> Point3Constraint;
class Graph: public NonlinearFactorGraph<Values> {
class Graph: public NonlinearFactorGraph<VisualValues> {
public:
void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
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;
}
typedef NonlinearOptimizer<Graph,Values> Optimizer;
typedef NonlinearOptimizer<Graph,VisualValues> Optimizer;
const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
@ -90,7 +90,7 @@ TEST( GeneralSFMFactor, equals )
boost::shared_ptr<Projection>
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>
factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
// For the following configuration, the factor predicts 320,240
Values values;
VisualValues values;
Rot3 R;
Point3 t1(0,0,-6);
Pose3 x1(R,t1);
values.insert(1, GeneralCamera(x1));
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 ;
/* ************************************************************************* */
vector<Point3> genPoint3() {
const double z = 5;
vector<Point3> L ;
@ -174,7 +174,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// add initial
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 )
values->insert((int)i, X[i]) ;
@ -189,20 +189,13 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// Create an ordering of the variables
shared_ptr<Ordering> ordering = getOrdering(X,L);
//graph->print("graph") ; values->print("values") ;
NonlinearOptimizationParameters::sharedThis params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_defaultK::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl;
//optimizer2.values()->print("optimized") ;
CHECK(optimizer2.error() < 0.5 * 1e-5 * nMeasurements);
EXPECT(optimizer2.error() < 0.5 * 1e-5 * nMeasurements);
}
/* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
vector<Point3> L = genPoint3();
@ -220,7 +213,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
// add initial
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 )
values->insert((int)i, X[i]) ;
@ -244,13 +237,11 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
NonlinearOptimizationParameters::sharedThis params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_varK_SingleMeasurementError::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl;
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
vector<Point3> L = genPoint3();
@ -268,7 +259,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
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 )
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));
Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_varK_FixCameras::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl;
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
vector<Point3> L = genPoint3();
@ -314,7 +302,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
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 ) {
const double
rot_noise = 1e-5,
@ -351,14 +339,10 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
shared_ptr<Ordering> ordering = getOrdering(X,L);
NonlinearOptimizationParameters::sharedThis params (
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);
//cout << "optimize_varK_FixLandmarks::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl;
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -379,7 +363,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
// add initial
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 )
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));
Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_varK_BA::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl;
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
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 TypedSymbol<GeneralCamera, 'x'> CameraKey;
typedef TypedSymbol<Point3, 'l'> PointKey;
typedef LieValues<CameraKey> CameraConfig;
typedef LieValues<PointKey> PointConfig;
typedef TupleValues2<CameraConfig, PointConfig> Values;
typedef GeneralSFMFactor<Values, CameraKey, PointKey> Projection;
typedef NonlinearEquality<Values, CameraKey> CameraConstraint;
typedef NonlinearEquality<Values, PointKey> Point3Constraint;
typedef Values<CameraKey> CameraConfig;
typedef Values<PointKey> PointConfig;
typedef TupleValues2<CameraConfig, PointConfig> VisualValues;
typedef GeneralSFMFactor<VisualValues, CameraKey, PointKey> Projection;
typedef NonlinearEquality<VisualValues, CameraKey> CameraConstraint;
typedef NonlinearEquality<VisualValues, PointKey> Point3Constraint;
class Graph: public NonlinearFactorGraph<Values> {
/* ************************************************************************* */
class Graph: public NonlinearFactorGraph<VisualValues> {
public:
void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) {
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;
}
typedef NonlinearOptimizer<Graph,Values> Optimizer;
typedef NonlinearOptimizer<Graph,VisualValues> Optimizer;
const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
@ -89,7 +90,7 @@ TEST( GeneralSFMFactor, equals )
boost::shared_ptr<Projection>
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>
factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
// For the following configuration, the factor predicts 320,240
Values values;
VisualValues values;
Rot3 R;
Point3 t1(0,0,-6);
Pose3 x1(R,t1);
values.insert(1, GeneralCamera(x1));
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 ;
/* ************************************************************************* */
vector<Point3> genPoint3() {
const double z = 5;
vector<Point3> L ;
@ -172,7 +174,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// add initial
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 )
values->insert((int)i, X[i]) ;
@ -187,20 +189,13 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// Create an ordering of the variables
shared_ptr<Ordering> ordering = getOrdering(X,L);
//graph->print("graph") ; values->print("values") ;
NonlinearOptimizationParameters::sharedThis params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_defaultK::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl;
//optimizer2.values()->print("optimized") ;
CHECK(optimizer2.error() < 0.5 * 1e-5 * nMeasurements);
EXPECT(optimizer2.error() < 0.5 * 1e-5 * nMeasurements);
}
/* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
vector<Point3> L = genPoint3();
@ -218,7 +213,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
// add initial
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 )
values->insert((int)i, X[i]) ;
@ -242,13 +237,11 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
NonlinearOptimizationParameters::sharedThis params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_varK_SingleMeasurementError::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl;
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
vector<Point3> L = genPoint3();
@ -266,7 +259,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
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 )
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));
Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_varK_FixCameras::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl;
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
vector<Point3> L = genPoint3();
@ -312,7 +302,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
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 ) {
const double
rot_noise = 1e-5, trans_noise = 1e-3,
@ -345,14 +335,10 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
shared_ptr<Ordering> ordering = getOrdering(X,L);
NonlinearOptimizationParameters::sharedThis params (
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);
//cout << "optimize_varK_FixLandmarks::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl;
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -373,7 +359,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
// add initial
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 )
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));
Optimizer optimizer(graph, values, ordering, params);
//cout << "optimize_varK_BA::" << endl ;
//cout << "before optimization, error is " << optimizer.error() << endl;
Optimizer optimizer2 = optimizer.levenbergMarquardt();
//cout << "after optimization, error is " << optimizer2.error() << endl;
CHECK(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
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;
using namespace pose3SLAM;
/* ************************************************************************* */
// test optimization with 6 poses arranged in a hexagon and a loop closure
TEST(Pose3Graph, optimizeCircle) {
@ -79,8 +77,8 @@ TEST(Pose3Graph, optimizeCircle) {
shared_ptr<Ordering> ordering(new Ordering);
*ordering += "x0","x1","x2","x3","x4","x5";
NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15);
Optimizer optimizer0(fg, initial, ordering, params);
Optimizer optimizer = optimizer0.levenbergMarquardt();
pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params);
pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
Pose3Values actual = *optimizer.values();
@ -93,12 +91,12 @@ TEST(Pose3Graph, optimizeCircle) {
/* ************************************************************************* */
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)
// height prior - single element interface
Key key(1);
pose3SLAM::Key key(1);
double exp_height = 5.0;
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));
@ -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);
EXPECT(assert_equal(expA, actA));
Graph graph;
pose3SLAM::Graph graph;
// graph.add(height); // FAIL - on compile, can't initialize a reference?
graph.push_back(boost::shared_ptr<Partial>(new Partial(height)));
Values values;
pose3SLAM::Values values;
values.insert(key, init);
// linearization
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_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
}
@ -148,10 +146,10 @@ TEST( Pose3Factor, error )
/* ************************************************************************* */
TEST(Pose3Graph, partial_prior_xy) {
typedef PartialPriorFactor<Values, Key> Partial;
typedef PartialPriorFactor<pose3SLAM::Values, pose3SLAM::Key> Partial;
// XY prior - full mask interface
Key key(1);
pose3SLAM::Key key(1);
Vector exp_xy = Vector_(2, 3.0, 4.0);
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));
@ -164,13 +162,13 @@ TEST(Pose3Graph, partial_prior_xy) {
0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
EXPECT(assert_equal(expA, actA));
Graph graph;
pose3SLAM::Graph graph;
graph.push_back(Partial::shared_ptr(new Partial(priorXY)));
Values values;
pose3SLAM::Values values;
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_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 gtsam;
using namespace gtsam::visualSLAM;
// make cube
static Point3
@ -48,11 +47,11 @@ TEST( ProjectionFactor, error )
// Create the factor with a measurement that is 3 pixels off in x
Point2 z(323.,240.);
int cameraFrameNumber=1, landmarkNumber=1;
boost::shared_ptr<ProjectionFactor>
factor(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
boost::shared_ptr<visualSLAM::ProjectionFactor>
factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
// 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);
Point3 l1; config.insert(1, l1);
// Point should project to Point2(320.,240.)
@ -73,7 +72,7 @@ TEST( ProjectionFactor, error )
CHECK(assert_equal(expected,*actual,1e-3));
// linearize graph
Graph graph;
visualSLAM::Graph graph;
graph.push_back(factor);
FactorGraph<GaussianFactor> expected_lfg;
expected_lfg.push_back(actual);
@ -81,13 +80,13 @@ TEST( ProjectionFactor, error )
CHECK(assert_equal(expected_lfg,*actual_lfg));
// 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 l2(1,2,3); expected_config.insert(1, l2);
VectorValues delta(expected_config.dims(ordering));
delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
delta[ordering["l1"]] = Vector_(3, 1.,2.,3.);
Values actual_config = config.retract(delta, ordering);
visualSLAM::Values actual_config = config.retract(delta, ordering);
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
Vector z = Vector_(2,323.,240.);
int cameraFrameNumber=1, landmarkNumber=1;
boost::shared_ptr<ProjectionFactor>
factor1(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
boost::shared_ptr<visualSLAM::ProjectionFactor>
factor1(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
boost::shared_ptr<ProjectionFactor>
factor2(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
boost::shared_ptr<visualSLAM::ProjectionFactor>
factor2(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
CHECK(assert_equal(*factor1, *factor2));
}

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

@ -23,27 +23,26 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/slam/simulated2D.h>
using namespace gtsam;
using namespace std;
using namespace simulated2D;
using namespace gtsam;
/* ************************************************************************* */
TEST( simulated2D, Simulated2DValues )
{
Values actual;
simulated2D::Values actual;
actual.insertPose(1,Point2(1,1));
actual.insertPoint(2,Point2(2,2));
CHECK(assert_equal(actual,actual,1e-9));
EXPECT(assert_equal(actual,actual,1e-9));
}
/* ************************************************************************* */
TEST( simulated2D, Dprior )
{
Point2 x(1,-9);
Matrix numerical = numericalDerivative11(prior,x);
Matrix numerical = numericalDerivative11(simulated2D::prior,x);
Matrix computed;
prior(x,computed);
CHECK(assert_equal(numerical,computed,1e-9));
simulated2D::prior(x,computed);
EXPECT(assert_equal(numerical,computed,1e-9));
}
/* ************************************************************************* */
@ -51,11 +50,11 @@ TEST( simulated2D, Dprior )
{
Point2 x1(1,-9),x2(-5,6);
Matrix H1,H2;
odo(x1,x2,H1,H2);
Matrix A1 = numericalDerivative21(odo,x1,x2);
CHECK(assert_equal(A1,H1,1e-9));
Matrix A2 = numericalDerivative22(odo,x1,x2);
CHECK(assert_equal(A2,H2,1e-9));
simulated2D::odo(x1,x2,H1,H2);
Matrix A1 = numericalDerivative21(simulated2D::odo,x1,x2);
EXPECT(assert_equal(A1,H1,1e-9));
Matrix A2 = numericalDerivative22(simulated2D::odo,x1,x2);
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/simulated2DOriented.h>
using namespace gtsam;
using namespace std;
using namespace simulated2DOriented;
/* ************************************************************************* */
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));
}
using namespace gtsam;
/* ************************************************************************* */
TEST( simulated2DOriented, Dprior )
{
Pose2 x(1,-9, 0.1);
Matrix numerical = numericalDerivative11(prior,x);
Matrix numerical = numericalDerivative11(simulated2DOriented::prior,x);
Matrix computed;
prior(x,computed);
simulated2DOriented::prior(x,computed);
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);
Matrix H1,H2;
odo(x1,x2,H1,H2);
Matrix A1 = numericalDerivative21(odo,x1,x2);
simulated2DOriented::odo(x1,x2,H1,H2);
Matrix A1 = numericalDerivative21(simulated2DOriented::odo,x1,x2);
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));
}
@ -64,11 +54,11 @@ TEST( simulated2DOriented, constructor )
{
Pose2 measurement(0.2, 0.3, 0.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;
config.insert(PoseKey(1), Pose2(1., 0., 0.2));
config.insert(PoseKey(2), Pose2(2., 0., 0.1));
simulated2DOriented::Values config;
config.insert(simulated2DOriented::PoseKey(1), Pose2(1., 0., 0.2));
config.insert(simulated2DOriented::PoseKey(2), Pose2(2., 0., 0.1));
boost::shared_ptr<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary());
}

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

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

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

@ -29,9 +29,7 @@
using namespace std;
using namespace gtsam;
using namespace gtsam::visualSLAM;
using namespace boost;
typedef NonlinearOptimizer<Graph, Values> Optimizer;
Pose3 camera1(Matrix_(3,3,
1., 0., 0.,
@ -52,16 +50,16 @@ TEST( StereoFactor, singlePoint)
{
//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<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);
// 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
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
Point3 l1(0, 0, 0);
@ -69,7 +67,7 @@ TEST( StereoFactor, singlePoint)
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 relativeThreshold = 1e-5;

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

@ -32,7 +32,6 @@ using namespace boost;
using namespace std;
using namespace gtsam;
using namespace gtsam::visualSLAM;
using namespace boost;
static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
@ -58,7 +57,7 @@ Pose3 camera2(Matrix_(3,3,
Point3(0,0,5.00));
/* ************************************************************************* */
Graph testGraph() {
visualSLAM::Graph testGraph() {
Point2 z11(-100, 100);
Point2 z12(-100,-100);
Point2 z13( 100,-100);
@ -69,7 +68,7 @@ Graph testGraph() {
Point2 z24( 125, 125);
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(z12, sigma, 1, 2, sK);
g.addMeasurement(z13, sigma, 1, 3, sK);
@ -85,14 +84,14 @@ Graph testGraph() {
TEST( Graph, optimizeLM)
{
// build a graph
shared_ptr<Graph> graph(new Graph(testGraph()));
shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph(testGraph()));
// add 3 landmark constraints
graph->addPointConstraint(1, landmark1);
graph->addPointConstraint(2, landmark2);
graph->addPointConstraint(3, landmark3);
// 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(2, camera2);
initialEstimate->insert(1, landmark1);
@ -106,12 +105,12 @@ TEST( Graph, optimizeLM)
// Create an optimizer and check its error
// 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);
// Iterate once, and the config should not have changed because we started
// with the ground truth
Optimizer afterOneIteration = optimizer.iterate();
visualSLAM::Optimizer afterOneIteration = optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
@ -123,13 +122,13 @@ TEST( Graph, optimizeLM)
TEST( Graph, optimizeLM2)
{
// build a graph
shared_ptr<Graph> graph(new Graph(testGraph()));
shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph(testGraph()));
// add 2 camera constraints
graph->addPoseConstraint(1, camera1);
graph->addPoseConstraint(2, camera2);
// 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(2, camera2);
initialEstimate->insert(1, landmark1);
@ -143,12 +142,12 @@ TEST( Graph, optimizeLM2)
// Create an optimizer and check its error
// 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);
// Iterate once, and the config should not have changed because we started
// with the ground truth
Optimizer afterOneIteration = optimizer.iterate();
visualSLAM::Optimizer afterOneIteration = optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
@ -160,13 +159,13 @@ TEST( Graph, optimizeLM2)
TEST( Graph, CHECK_ORDERING)
{
// build a graph
shared_ptr<Graph> graph(new Graph(testGraph()));
shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph(testGraph()));
// add 2 camera constraints
graph->addPoseConstraint(1, camera1);
graph->addPoseConstraint(2, camera2);
// 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(2, camera2);
initialEstimate->insert(1, landmark1);
@ -178,12 +177,12 @@ TEST( Graph, CHECK_ORDERING)
// Create an optimizer and check its error
// 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);
// Iterate once, and the config should not have changed because we started
// with the ground truth
Optimizer afterOneIteration = optimizer.iterate();
visualSLAM::Optimizer afterOneIteration = optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// check if correct
@ -194,23 +193,23 @@ TEST( Graph, CHECK_ORDERING)
TEST( Values, update_with_large_delta) {
// this test ensures that if the update for delta is larger than
// the size of the config, it only updates existing variables
Values init;
visualSLAM::Values init;
init.insert(1, Pose3());
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, Point3(1.1, 2.1, 3.1));
Ordering largeOrdering;
Values largeValues = init;
visualSLAM::Values largeValues = init;
largeValues.insert(2, Pose3());
largeOrdering += "x1","l1","x2";
VectorValues delta(largeValues.dims(largeOrdering));
delta[largeOrdering["x1"]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1);
delta[largeOrdering["l1"]] = Vector_(3, 0.1, 0.1, 0.1);
delta[largeOrdering["x2"]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1);
Values actual = init.retract(delta, largeOrdering);
visualSLAM::Values actual = init.retract(delta, largeOrdering);
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<Point3,'l'> PointKey; ///< The key type used for points
typedef LieValues<PoseKey> PoseValues; ///< Values used for poses
typedef LieValues<PointKey> PointValues; ///< Values used for points
typedef Values<PoseKey> PoseValues; ///< Values used for poses
typedef Values<PointKey> PointValues; ///< Values used for points
typedef TupleValues2<PoseValues, PointValues> Values; ///< 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/nonlinear/ExtendedKalmanFilter-inl.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/geometry/Point2.h>
using namespace gtsam;
// Define Types for System Test
typedef TypedSymbol<Point2, 'x'> TestKey;
typedef LieValues<TestKey> TestValues;
typedef Values<TestKey> TestValues;
/* ************************************************************************* */
TEST( ExtendedKalmanFilter, linear ) {

4
tests/testGaussianJunctionTree.cpp
View File

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

28
tests/testInference.cpp
View File

@ -50,29 +50,27 @@ TEST( Inference, marginals )
/* ************************************************************************* */
TEST( Inference, marginals2)
{
using namespace gtsam::planarSLAM;
Graph fg;
planarSLAM::Graph fg;
SharedDiagonal poseModel(sharedSigma(3, 0.1));
SharedDiagonal pointModel(sharedSigma(3, 0.1));
fg.addPrior(PoseKey(0), Pose2(), poseModel);
fg.addOdometry(PoseKey(0), 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.addBearingRange(PoseKey(0), PointKey(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(PoseKey(1), PointKey(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(PoseKey(2), PointKey(0), Rot2(), 1.0, pointModel);
fg.addPrior(planarSLAM::PoseKey(0), Pose2(), poseModel);
fg.addOdometry(planarSLAM::PoseKey(0), planarSLAM::PoseKey(1), 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(planarSLAM::PoseKey(0), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(planarSLAM::PoseKey(1), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(planarSLAM::PoseKey(2), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel);
Values init;
init.insert(PoseKey(0), Pose2(0.0,0.0,0.0));
init.insert(PoseKey(1), Pose2(1.0,0.0,0.0));
init.insert(PoseKey(2), Pose2(2.0,0.0,0.0));
init.insert(PointKey(0), Point2(1.0,1.0));
planarSLAM::Values init;
init.insert(planarSLAM::PoseKey(0), Pose2(0.0,0.0,0.0));
init.insert(planarSLAM::PoseKey(1), Pose2(1.0,0.0,0.0));
init.insert(planarSLAM::PoseKey(2), Pose2(2.0,0.0,0.0));
init.insert(planarSLAM::PointKey(0), Point2(1.0,1.0));
Ordering ordering(*fg.orderingCOLAMD(init));
FactorGraph<GaussianFactor>::shared_ptr gfg(fg.linearize(init, ordering));
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/NonlinearOptimizer-inl.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
using namespace std;
using namespace gtsam;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef LieValues<PoseKey> PoseValues;
typedef Values<PoseKey> PoseValues;
typedef PriorFactor<PoseValues, PoseKey> PosePrior;
typedef NonlinearEquality<PoseValues, PoseKey> 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/linear/GaussianFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
using namespace std;
using namespace gtsam;
@ -83,14 +83,14 @@ TEST( NonlinearFactor, NonlinearFactor )
Graph fg = createNonlinearFactorGraph();
// 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
Graph::sharedFactor factor = fg[0];
// calculate the error_vector from the factor "f1"
// error_vector = [0.1 0.1]
Vector actual_e = boost::dynamic_pointer_cast<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));
// error = 0.5 * [1 1] * [1;1] = 1
@ -104,7 +104,7 @@ TEST( NonlinearFactor, NonlinearFactor )
/* ************************************************************************* */
TEST( NonlinearFactor, linearize_f1 )
{
Values c = createNoisyValues();
example::Values c = createNoisyValues();
// Grab a non-linear factor
Graph nfg = createNonlinearFactorGraph();
@ -126,7 +126,7 @@ TEST( NonlinearFactor, linearize_f1 )
/* ************************************************************************* */
TEST( NonlinearFactor, linearize_f2 )
{
Values c = createNoisyValues();
example::Values c = createNoisyValues();
// Grab a non-linear factor
Graph nfg = createNonlinearFactorGraph();
@ -149,7 +149,7 @@ TEST( NonlinearFactor, linearize_f3 )
Graph::sharedFactor nlf = nfg[2];
// We linearize at noisy config from SmallExample
Values c = createNoisyValues();
example::Values c = createNoisyValues();
GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary());
GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary());
@ -166,7 +166,7 @@ TEST( NonlinearFactor, linearize_f4 )
Graph::sharedFactor nlf = nfg[3];
// We linearize at noisy config from SmallExample
Values c = createNoisyValues();
example::Values c = createNoisyValues();
GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary());
GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary());
@ -182,7 +182,7 @@ TEST( NonlinearFactor, size )
Graph fg = createNonlinearFactorGraph();
// create a values structure for the non linear factor graph
Values cfg = createNoisyValues();
example::Values cfg = createNoisyValues();
// get some factors from the graph
Graph::sharedFactor factor1 = fg[0], factor2 = fg[1],
@ -202,7 +202,7 @@ TEST( NonlinearFactor, linearize_constraint1 )
Point2 mu(1., -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));
GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary());
@ -222,7 +222,7 @@ TEST( NonlinearFactor, linearize_constraint2 )
Point2 z3(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::PointKey(1), Point2(5.0, 4.0));
GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary());
@ -236,7 +236,7 @@ TEST( NonlinearFactor, linearize_constraint2 )
/* ************************************************************************* */
typedef TypedSymbol<LieVector, 'x'> TestKey;
typedef LieValues<TestKey> TestValues;
typedef gtsam::Values<TestKey> TestValues;
/* ************************************************************************* */
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 )
{
Graph fg = createNonlinearFactorGraph();
Values c1 = createValues();
example::Values c1 = createValues();
double actual1 = fg.error(c1);
DOUBLES_EQUAL( 0.0, actual1, 1e-9 );
Values c2 = createNoisyValues();
example::Values c2 = createNoisyValues();
double actual2 = fg.error(c2);
DOUBLES_EQUAL( 5.625, actual2, 1e-9 );
}
@ -85,7 +85,7 @@ TEST( Graph, GET_ORDERING)
TEST( Graph, probPrime )
{
Graph fg = createNonlinearFactorGraph();
Values cfg = createValues();
example::Values cfg = createValues();
// evaluate the probability of the factor graph
double actual = fg.probPrime(cfg);
@ -97,7 +97,7 @@ TEST( Graph, probPrime )
TEST( Graph, linearize )
{
Graph fg = createNonlinearFactorGraph();
Values initial = createNoisyValues();
example::Values initial = createNoisyValues();
boost::shared_ptr<FactorGraph<GaussianFactor> > linearized = fg.linearize(initial, *initial.orderingArbitrary());
FactorGraph<GaussianFactor> expected = createGaussianFactorGraph(*initial.orderingArbitrary());
CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations

10
tests/testNonlinearISAM.cpp
View File

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

6
tests/testPose2SLAMwSPCG.cpp
View File

@ -40,7 +40,7 @@ TEST(testPose2SLAMwSPCG, example1) {
graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ;
graph.addPrior(x1, Pose2(0,0,0), sigma) ;
Values initial;
pose2SLAM::Values initial;
initial.insert(x1, Pose2( 0, 0, 0));
initial.insert(x2, Pose2( 0, 2.1, 0.01));
initial.insert(x3, Pose2( 0, 3.9,-0.01));
@ -51,7 +51,7 @@ TEST(testPose2SLAMwSPCG, example1) {
initial.insert(x8, Pose2(3.9, 2.1, 0.01));
initial.insert(x9, Pose2(4.1, 3.9,-0.01));
Values expected;
pose2SLAM::Values expected;
expected.insert(x1, Pose2(0.0, 0.0, 0.0));
expected.insert(x2, Pose2(0.0, 2.0, 0.0));
expected.insert(x3, Pose2(0.0, 4.0, 0.0));
@ -62,7 +62,7 @@ TEST(testPose2SLAMwSPCG, example1) {
expected.insert(x8, Pose2(4.0, 2.0, 0.0));
expected.insert(x9, Pose2(4.0, 4.0, 0.0));
Values actual = optimizeSPCG(graph, initial);
pose2SLAM::Values actual = optimizeSPCG(graph, initial);
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) {
using namespace example;
Graph nfg = createNonlinearFactorGraph();
Values c1 = createValues();
example::Values c1 = createValues();
EXPECT(equalsObj(nfg));
EXPECT(equalsXML(nfg));
@ -464,7 +464,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::planarSLAM::Constraint, "gtsam::planarSLAM::Cons
/* ************************************************************************* */
TEST (Serialization, planar_system) {
using namespace planarSLAM;
Values values;
planarSLAM::Values values;
values.insert(PointKey(3), Point2(1.0, 2.0));
values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3));
@ -490,7 +490,7 @@ TEST (Serialization, planar_system) {
// text
EXPECT(equalsObj<PoseKey>(PoseKey(2)));
EXPECT(equalsObj<PointKey>(PointKey(3)));
EXPECT(equalsObj<Values>(values));
EXPECT(equalsObj<planarSLAM::Values>(values));
EXPECT(equalsObj<Prior>(prior));
EXPECT(equalsObj<Bearing>(bearing));
EXPECT(equalsObj<BearingRange>(bearingRange));
@ -502,7 +502,7 @@ TEST (Serialization, planar_system) {
// xml
EXPECT(equalsXML<PoseKey>(PoseKey(2)));
EXPECT(equalsXML<PointKey>(PointKey(3)));
EXPECT(equalsXML<Values>(values));
EXPECT(equalsXML<planarSLAM::Values>(values));
EXPECT(equalsXML<Prior>(prior));
EXPECT(equalsXML<Bearing>(bearing));
EXPECT(equalsXML<BearingRange>(bearingRange));
@ -524,7 +524,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::visualSLAM::StereoFactor, "gtsam::visualSLAM::
/* ************************************************************************* */
TEST (Serialization, visual_system) {
using namespace visualSLAM;
Values values;
visualSLAM::Values values;
PoseKey x1(1), x2(2);
PointKey l1(1), l2(2);
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<Point2, 'l'> PointKey;
typedef LieValues<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues;
typedef TupleValues2<PoseValues, PointValues> Values;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
typedef TupleValues2<PoseValues, PointValues> TestValues;
/* ************************************************************************* */
TEST( TupleValues, constructors )
@ -49,14 +49,14 @@ TEST( TupleValues, constructors )
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
Values::Values1 cfg1;
TestValues::Values1 cfg1;
cfg1.insert(PoseKey(1), x1);
cfg1.insert(PoseKey(2), x2);
Values::Values2 cfg2;
TestValues::Values2 cfg2;
cfg2.insert(PointKey(1), l1);
cfg2.insert(PointKey(2), l2);
Values actual(cfg1, cfg2), expected;
TestValues actual(cfg1, cfg2), expected;
expected.insert(PoseKey(1), x1);
expected.insert(PoseKey(2), x2);
expected.insert(PointKey(1), l1);
@ -71,13 +71,13 @@ TEST( TupleValues, insert_equals1 )
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
Values expected;
TestValues expected;
expected.insert(PoseKey(1), x1);
expected.insert(PoseKey(2), x2);
expected.insert(PointKey(1), l1);
expected.insert(PointKey(2), l2);
Values actual;
TestValues actual;
actual.insert(PoseKey(1), x1);
actual.insert(PoseKey(2), x2);
actual.insert(PointKey(1), l1);
@ -92,13 +92,13 @@ TEST( TupleValues, insert_equals2 )
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
Values values1;
TestValues values1;
values1.insert(PoseKey(1), x1);
values1.insert(PoseKey(2), x2);
values1.insert(PointKey(1), l1);
values1.insert(PointKey(2), l2);
Values values2;
TestValues values2;
values2.insert(PoseKey(1), x1);
values2.insert(PoseKey(2), x2);
values2.insert(PointKey(1), l1);
@ -116,7 +116,7 @@ TEST( TupleValues, insert_duplicate )
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
Values values1;
TestValues values1;
values1.insert(1, x1); // 3
values1.insert(2, x2); // 6
values1.insert(1, l1); // 8
@ -134,7 +134,7 @@ TEST( TupleValues, size_dim )
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
Values values1;
TestValues values1;
values1.insert(PoseKey(1), x1);
values1.insert(PoseKey(2), x2);
values1.insert(PointKey(1), l1);
@ -150,7 +150,7 @@ TEST(TupleValues, at)
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
Values values1;
TestValues values1;
values1.insert(PoseKey(1), x1);
values1.insert(PoseKey(2), x2);
values1.insert(PointKey(1), l1);
@ -171,7 +171,7 @@ TEST(TupleValues, zero_expmap_logmap)
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
Values values1;
TestValues values1;
values1.insert(PoseKey(1), x1);
values1.insert(PoseKey(2), x2);
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["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(2), x2.retract(Vector_(3, 1.3, 1.4, 1.5)));
expected.insert(PointKey(1), Point2(5.0, 6.1));
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 log
@ -216,12 +216,12 @@ typedef TypedSymbol<Point3, 'b'> Point3Key;
typedef TypedSymbol<Point3, 'c'> Point3Key2;
// some values types
typedef LieValues<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues;
typedef LieValues<LamKey> LamValues;
typedef LieValues<Pose3Key> Pose3Values;
typedef LieValues<Point3Key> Point3Values;
typedef LieValues<Point3Key2> Point3Values2;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
typedef Values<LamKey> LamValues;
typedef Values<Pose3Key> Pose3Values;
typedef Values<Point3Key> Point3Values;
typedef Values<Point3Key2> Point3Values2;
// some TupleValues types
typedef TupleValues<PoseValues, TupleValuesEnd<PointValues> > ValuesA;