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

(2.0_prep branch) Merged in virtual_values branch

release/4.3a0
Richard Roberts 2012-02-03 17:18:32 +00:00
parent 8052fa8e42 4b2b9d8158
commit 820b33bd55
117 changed files with 1997 additions and 2582 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
examples/CameraResectioning.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
@ -28,13 +27,12 @@
using namespace gtsam;
typedef TypedSymbol<Pose3, 'x'> PoseKey;
typedef Values<PoseKey> PoseValues;
/**
* Unary factor for the pose.
*/
class ResectioningFactor: public NonlinearFactor1<PoseValues, PoseKey> {
typedef NonlinearFactor1<PoseValues, PoseKey> Base;
class ResectioningFactor: public NonlinearFactor1<PoseKey> {
typedef NonlinearFactor1<PoseKey> Base;
shared_ptrK K_; // camera's intrinsic parameters
Point3 P_; // 3D point on the calibration rig
@ -46,6 +44,8 @@ public:
Base(model, key), K_(calib), P_(P), p_(p) {
}
virtual ~ResectioningFactor() {}
virtual Vector evaluateError(const Pose3& X, boost::optional<Matrix&> H =
boost::none) const {
SimpleCamera camera(*K_, X);
@ -72,7 +72,7 @@ int main(int argc, char* argv[]) {
PoseKey X(1);
/* 1. create graph */
NonlinearFactorGraph<PoseValues> graph;
NonlinearFactorGraph graph;
/* 2. add factors to the graph */
// add measurement factors
@ -92,14 +92,13 @@ int main(int argc, char* argv[]) {
graph.push_back(factor);
/* 3. Create an initial estimate for the camera pose */
PoseValues initial;
Values initial;
initial.insert(X, Pose3(Rot3(1.,0.,0.,
0.,-1.,0.,
0.,0.,-1.), Point3(0.,0.,2.0)));
/* 4. Optimize the graph using Levenberg-Marquardt*/
PoseValues result = optimize<NonlinearFactorGraph<PoseValues> , PoseValues> (
graph, initial);
Values result = optimize<NonlinearFactorGraph> (graph, initial);
result.print("Final result: ");
return 0;

4
examples/Makefile.am
View File

@ -16,8 +16,8 @@ noinst_PROGRAMS += PlanarSLAMExample_easy # Solves SLAM example from tutorial
noinst_PROGRAMS += PlanarSLAMSelfContained_advanced # Solves SLAM example from tutorial with all typedefs in the script
noinst_PROGRAMS += Pose2SLAMExample_easy # Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface
noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface
noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver
noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver
#noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver
#noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver
noinst_PROGRAMS += elaboratePoint2KalmanFilter # simple linear Kalman filter on a moving 2D point, but done using factor graphs
noinst_PROGRAMS += easyPoint2KalmanFilter # uses the cool generic templated Kalman filter class to do the same
noinst_PROGRAMS += CameraResectioning

4
examples/PlanarSLAMExample_easy.cpp
View File

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

24
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -34,7 +34,6 @@
#include <gtsam/slam/BearingRangeFactor.h>
// implementations for structures - needed if self-contained, and these should be included last
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
@ -43,12 +42,9 @@
// 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::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
typedef gtsam::NonlinearFactorGraph Graph; // graph structure
typedef gtsam::NonlinearOptimizer<Graph,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential; // optimization engine for this domain
typedef gtsam::NonlinearOptimizer<Graph,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal; // optimization engine for this domain
using namespace std;
using namespace gtsam;
@ -74,7 +70,7 @@ int main(int argc, char** argv) {
// gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
PriorFactor<PlanarValues, PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor
PriorFactor<PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor
graph->add(posePrior); // add the factor to the graph
/* add odometry */
@ -82,8 +78,8 @@ int main(int argc, char** argv) {
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
// create between factors to represent odometry
BetweenFactor<PlanarValues,PoseKey> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<PlanarValues,PoseKey> odom23(x2, x3, odom_measurement, odom_model);
BetweenFactor<PoseKey> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<PoseKey> odom23(x2, x3, odom_measurement, odom_model);
graph->add(odom12); // add both to graph
graph->add(odom23);
@ -100,9 +96,9 @@ int main(int argc, char** argv) {
range32 = 2.0;
// create bearing/range factors
BearingRangeFactor<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);
BearingRangeFactor<PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model);
BearingRangeFactor<PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
BearingRangeFactor<PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model);
// add the factors
graph->add(meas11);
@ -112,7 +108,7 @@ int main(int argc, char** argv) {
graph->print("Full Graph");
// initialize to noisy points
boost::shared_ptr<PlanarValues> initial(new PlanarValues);
boost::shared_ptr<Values> initial(new Values);
initial->insert(x1, Pose2(0.5, 0.0, 0.2));
initial->insert(x2, Pose2(2.3, 0.1,-0.2));
initial->insert(x3, Pose2(4.1, 0.1, 0.1));

4
examples/Pose2SLAMExample_advanced.cpp
View File

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

4
examples/Pose2SLAMExample_easy.cpp
View File

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

10
examples/Pose2SLAMwSPCG_advanced.cpp
View File

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

2
examples/Pose2SLAMwSPCG_easy.cpp
View File

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

10
examples/SimpleRotation.cpp
View File

@ -23,7 +23,6 @@
#include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
@ -53,8 +52,7 @@ using namespace gtsam;
* and 2D poses.
*/
typedef TypedSymbol<Rot2, 'x'> Key; /// Variable labels for a specific type
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
typedef NonlinearFactorGraph Graph; /// Graph container for constraints - needs to know type of variables
const double degree = M_PI / 180;
@ -83,7 +81,7 @@ int main() {
prior.print("goal angle");
SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree);
Key key(1);
PriorFactor<RotValues, Key> factor(key, prior, model);
PriorFactor<Key> factor(key, prior, model);
/**
* Step 3: create a graph container and add the factor to it
@ -114,7 +112,7 @@ int main() {
* on the type of key used to find the appropriate value map if there
* are multiple types of variables.
*/
RotValues initial;
Values initial;
initial.insert(key, Rot2::fromAngle(20 * degree));
initial.print("initial estimate");
@ -126,7 +124,7 @@ int main() {
* initial estimate. This will yield a new RotValues structure
* with the final state of the optimization.
*/
RotValues result = optimize<Graph, RotValues>(graph, initial);
Values result = optimize<Graph>(graph, initial);
result.print("final result");
return 0;

16
examples/easyPoint2KalmanFilter.cpp
View File

@ -24,7 +24,6 @@
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Point2.h>
using namespace std;
@ -32,7 +31,6 @@ using namespace gtsam;
// Define Types for Linear System Test
typedef TypedSymbol<Point2, 'x'> LinearKey;
typedef Values<LinearKey> LinearValues;
typedef Point2 LinearMeasurement;
int main() {
@ -42,7 +40,7 @@ int main() {
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
// Create an ExtendedKalmanFilter object
ExtendedKalmanFilter<LinearValues,LinearKey> ekf(x_initial, P_initial);
ExtendedKalmanFilter<LinearKey> ekf(x_initial, P_initial);
@ -67,7 +65,7 @@ int main() {
// Predict delta based on controls
Point2 difference(1,0);
// Create Factor
BetweenFactor<LinearValues,LinearKey> factor1(x0, x1, difference, Q);
BetweenFactor<LinearKey> factor1(x0, x1, difference, Q);
// Predict the new value with the EKF class
Point2 x1_predict = ekf.predict(factor1);
@ -88,7 +86,7 @@ int main() {
// This simple measurement can be modeled with a PriorFactor
Point2 z1(1.0, 0.0);
PriorFactor<LinearValues,LinearKey> factor2(x1, z1, R);
PriorFactor<LinearKey> factor2(x1, z1, R);
// Update the Kalman Filter with the measurement
Point2 x1_update = ekf.update(factor2);
@ -100,13 +98,13 @@ int main() {
// Predict
LinearKey x2(2);
difference = Point2(1,0);
BetweenFactor<LinearValues,LinearKey> factor3(x1, x2, difference, Q);
BetweenFactor<LinearKey> factor3(x1, x2, difference, Q);
Point2 x2_predict = ekf.predict(factor1);
x2_predict.print("X2 Predict");
// Update
Point2 z2(2.0, 0.0);
PriorFactor<LinearValues,LinearKey> factor4(x2, z2, R);
PriorFactor<LinearKey> factor4(x2, z2, R);
Point2 x2_update = ekf.update(factor4);
x2_update.print("X2 Update");
@ -116,13 +114,13 @@ int main() {
// Predict
LinearKey x3(3);
difference = Point2(1,0);
BetweenFactor<LinearValues,LinearKey> factor5(x2, x3, difference, Q);
BetweenFactor<LinearKey> factor5(x2, x3, difference, Q);
Point2 x3_predict = ekf.predict(factor5);
x3_predict.print("X3 Predict");
// Update
Point2 z3(3.0, 0.0);
PriorFactor<LinearValues,LinearKey> factor6(x3, z3, R);
PriorFactor<LinearKey> factor6(x3, z3, R);
Point2 x3_update = ekf.update(factor6);
x3_update.print("X3 Update");

18
examples/elaboratePoint2KalmanFilter.cpp
View File

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

14
examples/vSLAMexample/vISAMexample.cpp
View File

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

10
examples/vSLAMexample/vSFMexample.cpp
View File

@ -102,17 +102,17 @@ 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.
*/
visualSLAM::Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
visualSLAM::Values initValues;
Values initValues;
// Initialize landmarks 3D positions.
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
initValues.insert(lmit->first, lmit->second);
initValues.insert(PointKey(lmit->first), lmit->second);
// Initialize camera poses.
for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
initValues.insert(poseit->first, poseit->second);
initValues.insert(PoseKey(poseit->first), poseit->second);
return initValues;
}
@ -135,7 +135,7 @@ int main(int argc, char* argv[]) {
boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
// Create an initial Values structure using groundtruth values as the initial estimates
boost::shared_ptr<visualSLAM::Values> initialEstimates(new visualSLAM::Values(initialize(g_landmarks, g_poses)));
boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses)));
cout << "*******************************************************" << endl;
initialEstimates->print("INITIAL ESTIMATES: ");

8
gtsam.h
View File

@ -427,14 +427,14 @@ class Graph {
void addRange(int poseKey, int pointKey, double range, const gtsam::SharedNoiseModel& noiseModel);
void addBearingRange(int poseKey, int pointKey, const gtsam::Rot2& bearing, double range,
const gtsam::SharedNoiseModel& noiseModel);
planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
Values optimize(const Values& initialEstimate);
};
class Odometry {
Odometry(int key1, int key2, const gtsam::Pose2& measured,
const gtsam::SharedNoiseModel& model);
void print(string s) const;
gtsam::GaussianFactor* linearize(const planarSLAM::Values& center, const gtsam::Ordering& ordering) const;
gtsam::GaussianFactor* linearize(const Values& center, const Ordering& ordering) const;
};
class Optimizer {
@ -637,8 +637,8 @@ class Graph {
// GaussianFactor* linearize(const gtsam::Pose2Values& config) const;
//};
//
//class gtsam::Pose2Graph{
// Pose2Graph();
//class gtsam::pose2SLAM::Graph{
// pose2SLAM::Graph();
// void print(string s) const;
// GaussianFactorGraph* linearize_(const gtsam::Pose2Values& config) const;
// void push_back(Pose2Factor* factor);

95
gtsam/base/DerivedValue.h Normal file
View File

@ -0,0 +1,95 @@
/*
* DerivedValue.h
*
* Created on: Jan 26, 2012
* Author: thduynguyen
*/
#pragma once
#include <boost/pool/singleton_pool.hpp>
#include <gtsam/base/Value.h>
namespace gtsam {
template<class DERIVED>
class DerivedValue : public Value {
private:
/// Fake Tag struct for singleton pool allocator. In fact, it is never used!
struct PoolTag { };
protected:
DerivedValue() {}
public:
virtual ~DerivedValue() {}
/**
* Create a duplicate object returned as a pointer to the generic Value interface.
* For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator
*/
virtual Value* clone_() const {
void *place = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
DERIVED* ptr = new(place) DERIVED(static_cast<const DERIVED&>(*this));
return ptr;
}
/**
* Destroy and deallocate this object, only if it was originally allocated using clone_().
*/
virtual void deallocate_() const {
this->~Value();
boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this);
}
/// equals implementing generic Value interface
virtual bool equals_(const Value& p, double tol = 1e-9) const {
// Cast the base class Value pointer to a derived class pointer
const DERIVED& derivedValue2 = dynamic_cast<const DERIVED&>(p);
// Return the result of calling equals on the derived class
return (static_cast<const DERIVED*>(this))->equals(derivedValue2, tol);
}
/// Generic Value interface version of retract
virtual Value* retract_(const Vector& delta) const {
// Call retract on the derived class
const DERIVED retractResult = (static_cast<const DERIVED*>(this))->retract(delta);
// Create a Value pointer copy of the result
void* resultAsValuePlace = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
Value* resultAsValue = new(resultAsValuePlace) DERIVED(retractResult);
// Return the pointer to the Value base class
return resultAsValue;
}
/// Generic Value interface version of localCoordinates
virtual Vector localCoordinates_(const Value& value2) const {
// Cast the base class Value pointer to a derived class pointer
const DERIVED& derivedValue2 = dynamic_cast<const DERIVED&>(value2);
// Return the result of calling localCoordinates on the derived class
return (static_cast<const DERIVED*>(this))->localCoordinates(derivedValue2);
}
/// Assignment operator
virtual Value& operator=(const Value& rhs) {
// Cast the base class Value pointer to a derived class pointer
const DERIVED& derivedRhs = dynamic_cast<const DERIVED&>(rhs);
// Do the assignment and return the result
return (static_cast<DERIVED*>(this))->operator=(derivedRhs);
}
protected:
/// Assignment operator, protected because only the Value or DERIVED
/// assignment operators should be used.
DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
// Nothing to do, do not call base class assignment operator
return *this;
}
};
} /* namespace gtsam */

3
gtsam/base/LieVector.h
View File

@ -19,13 +19,14 @@
#include <gtsam/base/Lie.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/DerivedValue.h>
namespace gtsam {
/**
* LieVector is a wrapper around vector to allow it to be a Lie type
*/
struct LieVector : public Vector {
struct LieVector : public Vector, public DerivedValue<LieVector> {
/** default constructor - should be unnecessary */
LieVector() {}

2
gtsam/base/Makefile.am
View File

@ -26,7 +26,7 @@ sources += timing.cpp debug.cpp
check_PROGRAMS += tests/testDebug tests/testTestableAssertions
# Manifolds and Lie Groups
headers += Manifold.h Group.h
headers += DerivedValue.h Value.h Manifold.h Group.h
headers += Lie.h Lie-inl.h lieProxies.h LieScalar.h
sources += LieVector.cpp
check_PROGRAMS += tests/testLieVector tests/testLieScalar

147
gtsam/base/Value.h Normal file
View File

@ -0,0 +1,147 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Value.h
* @brief The interface class for any variable that can be optimized or used in a factor.
* @author Richard Roberts
* @date Jan 14, 2012
*/
#pragma once
#include <memory>
#include <gtsam/base/Vector.h>
namespace gtsam {
/**
* This is the interface class for any value that may be used as a variable
* assignment in a factor graph, and which you must derive to create new
* variable types to use with gtsam. Examples of built-in classes
* implementing this are mainly in geometry, including Rot3, Pose2, etc.
*
* This interface specifies pure virtual retract_(), localCoordinates_() and
* equals_() functions that work with pointers and references to this interface
* class, i.e. the base class. These functions allow containers, such as
* Values can operate generically on Value objects, retracting or computing
* local coordinates for many Value objects of different types.
*
* When you implement retract_(), localCoordinates_(), and equals_(), we
* suggest first implementing versions of these functions that work directly
* with derived objects, then using the provided helper functions to
* implement the generic Value versions. This makes your implementation
* easier, and also improves performance in situations where the derived type
* is in fact known, such as in most implementations of \c evaluateError() in
* classes derived from NonlinearFactor.
*
* Using the above practice, here is an example of implementing a typical
* class derived from Value:
* \code
class Rot3 : public Value {
public:
// Constructor, there is never a need to call the Value base class constructor.
Rot3() { ... }
// Print for unit tests and debugging (virtual, implements Value::print())
virtual void print(const std::string& str = "") const;
// Equals working directly with Rot3 objects (non-virtual, non-overriding!)
bool equals(const Rot3& other, double tol = 1e-9) const;
// Tangent space dimensionality (virtual, implements Value::dim())
virtual size_t dim() const {
return 3;
}
// retract working directly with Rot3 objects (non-virtual, non-overriding!)
Rot3 retract(const Vector& delta) const {
// Math to implement a 3D rotation retraction e.g. exponential map
return Rot3(result);
}
// localCoordinates working directly with Rot3 objects (non-virtual, non-overriding!)
Vector localCoordinates(const Rot3& r2) const {
// Math to implement 3D rotation localCoordinates, e.g. logarithm map
return Vector(result);
}
// Equals implementing the generic Value interface (virtual, implements Value::equals_())
virtual bool equals_(const Value& other, double tol = 1e-9) const {
// Call our provided helper function to call your Rot3-specific
// equals with appropriate casting.
return CallDerivedEquals(this, other, tol);
}
// retract implementing the generic Value interface (virtual, implements Value::retract_())
virtual std::auto_ptr<Value> retract_(const Vector& delta) const {
// Call our provided helper function to call your Rot3-specific
// retract and do the appropriate casting and allocation.
return CallDerivedRetract(this, delta);
}
// localCoordinates implementing the generic Value interface (virtual, implements Value::localCoordinates_())
virtual Vector localCoordinates_(const Value& value) const {
// Call our provided helper function to call your Rot3-specific
// localCoordinates and do the appropriate casting.
return CallDerivedLocalCoordinates(this, value);
}
};
\endcode
*/
class Value {
public:
/** Allocate and construct a clone of this value */
virtual Value* clone_() const = 0;
/** Deallocate a raw pointer of this value */
virtual void deallocate_() const = 0;
/** Compare this Value with another for equality. */
virtual bool equals_(const Value& other, double tol = 1e-9) const = 0;
/** Print this value, for debugging and unit tests */
virtual void print(const std::string& str = "") const = 0;
/** Return the dimensionality of the tangent space of this value. This is
* the dimensionality of \c delta passed into retract() and of the vector
* returned by localCoordinates().
* @return The dimensionality of the tangent space
*/
virtual size_t dim() const = 0;
/** Increment the value, by mapping from the vector delta in the tangent
* space of the current value back to the manifold to produce a new,
* incremented value.
* @param delta The delta vector in the tangent space of this value, by
* which to increment this value.
*/
virtual Value* retract_(const Vector& delta) const = 0;
/** Compute the coordinates in the tangent space of this value that
* retract() would map to \c value.
* @param value The value whose coordinates should be determined in the
* tangent space of the value on which this function is called.
* @return The coordinates of \c value in the tangent space of \c this.
*/
virtual Vector localCoordinates_(const Value& value) const = 0;
/** Assignment operator */
virtual Value& operator=(const Value& rhs) = 0;
/** Virutal destructor */
virtual ~Value() {}
};
} /* namespace gtsam */

5
gtsam/geometry/Cal3Bundler.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -27,7 +28,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Cal3Bundler {
class Cal3Bundler : public DerivedValue<Cal3Bundler> {
private:
double f_, k1_, k2_ ;
@ -94,7 +95,7 @@ public:
Vector localCoordinates(const Cal3Bundler& T2) const ;
///TODO: comment
int dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
virtual size_t dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
///TODO: comment
static size_t Dim() { return 3; } //TODO: make a final dimension variable

5
gtsam/geometry/Cal3DS2.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -27,7 +28,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Cal3DS2 {
class Cal3DS2 : public DerivedValue<Cal3DS2> {
private:
@ -97,7 +98,7 @@ public:
Vector localCoordinates(const Cal3DS2& T2) const ;
///TODO: comment
int dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
virtual size_t dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
///TODO: comment
static size_t Dim() { return 9; } //TODO: make a final dimension variable

3
gtsam/geometry/Cal3_S2.h
View File

@ -21,6 +21,7 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -30,7 +31,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Cal3_S2 {
class Cal3_S2 : public DerivedValue<Cal3_S2> {
private:
double fx_, fy_, s_, u0_, v0_;

10
gtsam/geometry/CalibratedCamera.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
@ -35,7 +36,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class CalibratedCamera {
class CalibratedCamera : public DerivedValue<CalibratedCamera> {
private:
Pose3 pose_; // 6DOF pose
@ -61,9 +62,10 @@ namespace gtsam {
/// @name Testable
/// @{
void print(const std::string& s="") const {
pose_.print();
}
virtual void print(const std::string& s = "") const {
pose_.print(s);
}
/// check equality to another camera
bool equals (const CalibratedCamera &camera, double tol = 1e-9) const {
return pose_.equals(camera.pose(), tol) ;

3
gtsam/geometry/PinholeCamera.h
View File

@ -20,6 +20,7 @@
#include <cmath>
#include <boost/optional.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
@ -35,7 +36,7 @@ namespace gtsam {
* \nosubgrouping
*/
template <typename Calibration>
class PinholeCamera {
class PinholeCamera : public DerivedValue<PinholeCamera<Calibration> > {
private:
Pose3 pose_;
Calibration k_;

4
gtsam/geometry/Point2.h
View File

@ -18,6 +18,8 @@
#pragma once
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Lie.h>
@ -30,7 +32,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Point2 {
class Point2 : public DerivedValue<Point2> {
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 2;

3
gtsam/geometry/Point3.h
View File

@ -24,6 +24,7 @@
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h>
namespace gtsam {
@ -33,7 +34,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Point3 {
class Point3 : public DerivedValue<Point3> {
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 3;

3
gtsam/geometry/Pose2.h
View File

@ -22,6 +22,7 @@
#include <boost/optional.hpp>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Rot2.h>
@ -32,7 +33,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Pose2 {
class Pose2 : public DerivedValue<Pose2> {
public:
static const size_t dimension = 3;

3
gtsam/geometry/Pose3.h
View File

@ -22,6 +22,7 @@
#define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER
#endif
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
@ -34,7 +35,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Pose3 {
class Pose3 : public DerivedValue<Pose3> {
public:
static const size_t dimension = 6;

4
gtsam/geometry/Rot2.h
View File

@ -19,6 +19,8 @@
#pragma once
#include <boost/optional.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -29,7 +31,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Rot2 {
class Rot2 : public DerivedValue<Rot2> {
public:
/** get the dimension by the type */

6
gtsam/geometry/Rot3.h
View File

@ -32,6 +32,7 @@
#endif
#endif
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/3rdparty/Eigen/Eigen/Geometry>
@ -49,7 +50,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Rot3 {
class Rot3 : public DerivedValue<Rot3> {
public:
static const size_t dimension = 3;
@ -92,6 +93,9 @@ namespace gtsam {
*/
Rot3(const Quaternion& q);
/** Virtual destructor */
virtual ~Rot3() {}
/* Static member function to generate some well known rotations */
/// Rotation around X axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.

2
gtsam/geometry/StereoCamera.h
View File

@ -18,6 +18,8 @@
#pragma once
#include <boost/tuple/tuple.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/StereoPoint2.h>

3
gtsam/geometry/StereoPoint2.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -27,7 +28,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class StereoPoint2 {
class StereoPoint2 : public DerivedValue<StereoPoint2> {
public:
static const size_t dimension = 3;
private:

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

@ -139,19 +139,19 @@ predecessorMap2Graph(const PredecessorMap<KEY>& p_map) {
}
/* ************************************************************************* */
template <class V,class POSE, class VALUES>
template <class V, class POSE, class KEY>
class compose_key_visitor : public boost::default_bfs_visitor {
private:
boost::shared_ptr<VALUES> config_;
boost::shared_ptr<Values> config_;
public:
compose_key_visitor(boost::shared_ptr<VALUES> config_in) {config_ = config_in;}
compose_key_visitor(boost::shared_ptr<Values> config_in) {config_ = config_in;}
template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
typename VALUES::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
typename VALUES::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
KEY key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
POSE relativePose = boost::get(boost::edge_weight, g, edge);
config_->insert(key_to, (*config_)[key_from].compose(relativePose));
}
@ -159,23 +159,23 @@ public:
};
/* ************************************************************************* */
template<class G, class Factor, class POSE, class VALUES>
boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<typename VALUES::Key>& tree,
template<class G, class Factor, class POSE, class KEY>
boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>& tree,
const POSE& rootPose) {
//TODO: change edge_weight_t to edge_pose_t
typedef typename boost::adjacency_list<
boost::vecS, boost::vecS, boost::directedS,
boost::property<boost::vertex_name_t, typename VALUES::Key>,
boost::property<boost::vertex_name_t, KEY>,
boost::property<boost::edge_weight_t, POSE> > PoseGraph;
typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge;
PoseGraph g;
PoseVertex root;
map<typename VALUES::Key, PoseVertex> key2vertex;
map<KEY, PoseVertex> key2vertex;
boost::tie(g, root, key2vertex) =
predecessorMap2Graph<PoseGraph, PoseVertex, typename VALUES::Key>(tree);
predecessorMap2Graph<PoseGraph, PoseVertex, KEY>(tree);
// attach the relative poses to the edges
PoseEdge edge12, edge21;
@ -189,8 +189,8 @@ boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<type
boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
if (!factor) continue;
typename VALUES::Key key1 = factor->key1();
typename VALUES::Key key2 = factor->key2();
KEY key1 = factor->key1();
KEY key2 = factor->key2();
PoseVertex v1 = key2vertex.find(key1)->second;
PoseVertex v2 = key2vertex.find(key2)->second;
@ -207,10 +207,10 @@ boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<type
}
// compose poses
boost::shared_ptr<VALUES> config(new VALUES);
typename VALUES::Key rootKey = boost::get(boost::vertex_name, g, root);
boost::shared_ptr<Values> config(new Values);
KEY rootKey = boost::get(boost::vertex_name, g, root);
config->insert(rootKey, rootPose);
compose_key_visitor<PoseVertex, POSE, VALUES> vis(config);
compose_key_visitor<PoseVertex, POSE, KEY> vis(config);
boost::breadth_first_search(g, root, boost::visitor(vis));
return config;

7
gtsam/inference/graph.h
View File

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

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

@ -21,8 +21,8 @@ using namespace std;
namespace gtsam {
template<class GRAPH, class LINEAR, class VALUES>
void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
template<class GRAPH, class LINEAR, class KEY>
void SubgraphSolver<GRAPH,LINEAR,KEY>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>();
GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared<GaussianFactorGraph>();
@ -47,8 +47,8 @@ void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::
Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(), Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),Rc1,xbar);
}
template<class GRAPH, class LINEAR, class VALUES>
VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() {
template<class GRAPH, class LINEAR, class KEY>
VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,KEY>::optimize() const {
// preconditioned conjugate gradient
VectorValues zeros = pc_->zero();
@ -60,18 +60,18 @@ VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() {
return xbar;
}
template<class GRAPH, class LINEAR, class VALUES>
void SubgraphSolver<GRAPH,LINEAR,VALUES>::initialize(const GRAPH& G, const VALUES& theta0) {
template<class GRAPH, class LINEAR, class KEY>
void SubgraphSolver<GRAPH,LINEAR,KEY>::initialize(const GRAPH& G, const Values& theta0) {
// generate spanning tree
PredecessorMap<Key> tree_ = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
PredecessorMap<KEY> tree_ = gtsam::findMinimumSpanningTree<GRAPH, KEY, Constraint>(G);
// make the ordering
list<Key> keys = predecessorMap2Keys(tree_);
list<KEY> keys = predecessorMap2Keys(tree_);
ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end()));
// build factor pairs
pairs_.clear();
typedef pair<Key,Key> EG ;
typedef pair<KEY,KEY> EG ;
BOOST_FOREACH( const EG &eg, tree_ ) {
Symbol key1 = Symbol(eg.first),
key2 = Symbol(eg.second) ;

13
gtsam/linear/SubgraphSolver.h
View File

@ -16,6 +16,7 @@
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/nonlinear/Values.h>
namespace gtsam {
@ -31,11 +32,11 @@ bool split(const std::map<Index, Index> &M,
* linearize: G * T -> L
* solve : L -> VectorValues
*/
template<class GRAPH, class LINEAR, class VALUES>
template<class GRAPH, class LINEAR, class KEY>
class SubgraphSolver : public IterativeSolver {
private:
typedef typename VALUES::Key Key;
// typedef typename VALUES::Key Key;
typedef typename GRAPH::Pose Pose;
typedef typename GRAPH::Constraint Constraint;
@ -43,7 +44,7 @@ private:
typedef boost::shared_ptr<Ordering> shared_ordering ;
typedef boost::shared_ptr<GRAPH> shared_graph ;
typedef boost::shared_ptr<LINEAR> shared_linear ;
typedef boost::shared_ptr<VALUES> shared_values ;
typedef boost::shared_ptr<Values> shared_values ;
typedef boost::shared_ptr<SubgraphPreconditioner> shared_preconditioner ;
typedef std::map<Index,Index> mapPairIndex ;
@ -61,7 +62,7 @@ private:
public:
SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
SubgraphSolver(const GRAPH& G, const Values& theta0, const Parameters &parameters = Parameters(), bool useQR = false):
IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); }
SubgraphSolver(const LINEAR& GFG) {
@ -85,11 +86,11 @@ public:
IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc), useQR_(useQR) {}
void replaceFactors(const typename LINEAR::shared_ptr &graph);
VectorValues::shared_ptr optimize() ;
VectorValues::shared_ptr optimize() const;
shared_ordering ordering() const { return ordering_; }
protected:
void initialize(const GRAPH& G, const VALUES& theta0);
void initialize(const GRAPH& G, const Values& theta0);
private:
SubgraphSolver():IterativeSolver(){}

2
gtsam/linear/VectorValues.h
View File

@ -175,7 +175,7 @@ namespace gtsam {
/// @name Advanced Constructors
/// @{
/** Construct from a container of variable dimensions (in variable order). */
/** Construct from a container of variable dimensions (in variable order), without initializing any values. */
template<class CONTAINER>
VectorValues(const CONTAINER& dimensions) { append(dimensions); }

2
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -7,7 +7,7 @@
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianISAM.h> // To get optimize(BayesTree<GaussianConditional>)
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
//#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {

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

@ -27,10 +27,10 @@
namespace gtsam {
/* ************************************************************************* */
template<class VALUES, class KEY>
typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::solve_(
template<class KEY>
typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::solve_(
const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering,
const VALUES& linearizationPoints, const KEY& lastKey,
const Values& linearizationPoints, const KEY& lastKey,
JacobianFactor::shared_ptr& newPrior) const {
// Extract the Index of the provided last key
@ -66,8 +66,8 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUES, class KEY>
ExtendedKalmanFilter<VALUES, KEY>::ExtendedKalmanFilter(T x_initial,
template<class KEY>
ExtendedKalmanFilter<KEY>::ExtendedKalmanFilter(T x_initial,
noiseModel::Gaussian::shared_ptr P_initial) {
// Set the initial linearization point to the provided mean
@ -82,8 +82,8 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUES, class KEY>
typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::predict(
template<class KEY>
typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::predict(
const MotionFactor& motionFactor) {
// TODO: This implementation largely ignores the actual factor symbols.
@ -100,7 +100,7 @@ namespace gtsam {
ordering.insert(x1, 1);
// Create a set of linearization points
VALUES linearizationPoints;
Values linearizationPoints;
linearizationPoints.insert(x0, x_);
linearizationPoints.insert(x1, x_); // TODO should this really be x_ ?
@ -122,8 +122,8 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUES, class KEY>
typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::update(
template<class KEY>
typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::update(
const MeasurementFactor& measurementFactor) {
// TODO: This implementation largely ignores the actual factor symbols.
@ -138,7 +138,7 @@ namespace gtsam {
ordering.insert(x0, 0);
// Create a set of linearization points
VALUES linearizationPoints;
Values linearizationPoints;
linearizationPoints.insert(x0, x_);
// Create a Gaussian Factor Graph

10
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -40,21 +40,21 @@ namespace gtsam {
* \nosubgrouping
*/
template<class VALUES, class KEY>
template<class KEY>
class ExtendedKalmanFilter {
public:
typedef boost::shared_ptr<ExtendedKalmanFilter<VALUES, KEY> > shared_ptr;
typedef boost::shared_ptr<ExtendedKalmanFilter<KEY> > shared_ptr;
typedef typename KEY::Value T;
typedef NonlinearFactor2<VALUES, KEY, KEY> MotionFactor;
typedef NonlinearFactor1<VALUES, KEY> MeasurementFactor;
typedef NonlinearFactor2<KEY, KEY> MotionFactor;
typedef NonlinearFactor1<KEY> MeasurementFactor;
protected:
T x_; // linearization point
JacobianFactor::shared_ptr priorFactor_; // density
T solve_(const GaussianFactorGraph& linearFactorGraph,
const Ordering& ordering, const VALUES& linearizationPoints,
const Ordering& ordering, const Values& linearizationPoints,
const KEY& x, JacobianFactor::shared_ptr& newPrior) const;
public:

10
gtsam/nonlinear/GaussianISAM2.h
View File

@ -36,19 +36,19 @@ namespace gtsam {
* variables.
* @tparam GRAPH The NonlinearFactorGraph structure to store factors. Defaults to standard NonlinearFactorGraph<VALUES>
*/
template <class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES, GRAPH> {
typedef ISAM2<GaussianConditional, VALUES, GRAPH> Base;
template <class GRAPH = NonlinearFactorGraph>
class GaussianISAM2 : public ISAM2<GaussianConditional, GRAPH> {
typedef ISAM2<GaussianConditional, GRAPH> Base;
public:
/// @name Standard Constructors
/// @{
/** Create an empty ISAM2 instance */
GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES, GRAPH>(params) {}
GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, GRAPH>(params) {}
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
GaussianISAM2() : ISAM2<GaussianConditional, VALUES, GRAPH>() {}
GaussianISAM2() : ISAM2<GaussianConditional, GRAPH>() {}
/// @}
/// @name Advanced Interface

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

@ -19,10 +19,10 @@ namespace gtsam {
using namespace std;
template<class CONDITIONAL, class VALUES, class GRAPH>
struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
template<class CONDITIONAL, class GRAPH>
struct ISAM2<CONDITIONAL, GRAPH>::Impl {
typedef ISAM2<CONDITIONAL, VALUES, GRAPH> ISAM2Type;
typedef ISAM2<CONDITIONAL, GRAPH> ISAM2Type;
struct PartialSolveResult {
typename ISAM2Type::sharedClique bayesTree;
@ -45,7 +45,7 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
* @param ordering Current ordering to be augmented with new variables
* @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
*/
static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
/**
* Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol
@ -95,7 +95,7 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
* where we might expmap something twice, or expmap it but then not
* recalculate its delta.
*/
static void ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta,
static void ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
const Ordering& ordering, const std::vector<bool>& mask,
boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>());
@ -118,25 +118,9 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
};
/* ************************************************************************* */
struct _VariableAdder {
Ordering& ordering;
Permuted<VectorValues>& vconfig;
Index nextVar;
_VariableAdder(Ordering& _ordering, Permuted<VectorValues>& _vconfig, Index _nextVar) : ordering(_ordering), vconfig(_vconfig), nextVar(_nextVar){}
template<typename I>
void operator()(I xIt) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
vconfig.permutation()[nextVar] = nextVar;
ordering.insert(xIt->first, nextVar);
if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << nextVar << endl;
++ nextVar;
}
};
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) {
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
theta.insert(newTheta);
@ -151,8 +135,13 @@ void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
delta.permutation().resize(originalnVars + newTheta.size());
{
_VariableAdder vadder(ordering, delta, originalnVars);
newTheta.apply(vadder);
Index nextVar = originalnVars;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
delta.permutation()[nextVar] = nextVar;
ordering.insert(key_value.first, nextVar);
if(debug) cout << "Adding variable " << (string)key_value.first << " with order " << nextVar << endl;
++ nextVar;
}
assert(delta.permutation().size() == delta.container().size());
assert(ordering.nVars() == delta.size());
assert(ordering.size() == delta.size());
@ -162,10 +151,10 @@ void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
template<class CONDITIONAL, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
FastSet<Index> indices;
BOOST_FOREACH(const typename NonlinearFactor<VALUES>::shared_ptr& factor, factors) {
BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) {
BOOST_FOREACH(const Symbol& key, factor->keys()) {
indices.insert(ordering[key]);
}
@ -174,8 +163,8 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::IndicesFromFactors(const O
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
template<class CONDITIONAL, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
FastSet<Index> relinKeys;
if(relinearizeThreshold.type() == typeid(double)) {
@ -202,8 +191,8 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(const
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
static const bool debug = false;
// does the separator contain any of the variables?
bool found = false;
@ -223,41 +212,8 @@ void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITI
}
/* ************************************************************************* */
// Internal class used in ExpmapMasked()
// This debug version sets delta entries that are applied to "Inf". The
// idea is that if a delta is applied, the variable is being relinearized,
// so the same delta should not be re-applied because it will be recalc-
// ulated. This is a debug check to prevent against a mix-up of indices
// or not keeping track of recalculated variables.
struct _SelectiveExpmapAndClear {
const Permuted<VectorValues>& delta;
const Ordering& ordering;
const vector<bool>& mask;
const boost::optional<Permuted<VectorValues>&> invalidate;
_SelectiveExpmapAndClear(const Permuted<VectorValues>& _delta, const Ordering& _ordering, const vector<bool>& _mask, boost::optional<Permuted<VectorValues>&> _invalidate) :
delta(_delta), ordering(_ordering), mask(_mask), invalidate(_invalidate) {}
template<typename I>
void operator()(I it_x) {
Index var = ordering[it_x->first];
if(ISDEBUG("ISAM2 update verbose")) {
if(mask[var])
cout << "expmap " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
else
cout << " " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
}
assert(delta[var].size() == (int)it_x->second.dim());
assert(delta[var].unaryExpr(&isfinite<double>).all());
if(mask[var]) {
it_x->second = it_x->second.retract(delta[var]);
if(invalidate)
(*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)
}
}
};
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
void ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta,
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
const Ordering& ordering, const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug) {
// If debugging, invalidate if requested, otherwise do not invalidate.
// Invalidating means setting expmapped entries to Inf, to trigger assertions
@ -266,14 +222,35 @@ void ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::ExpmapMasked(VALUES& values, const
invalidateIfDebug = boost::optional<Permuted<VectorValues>&>();
#endif
_SelectiveExpmapAndClear selectiveExpmapper(delta, ordering, mask, invalidateIfDebug);
values.apply(selectiveExpmapper);
assert(values.size() == ordering.size());
Values::iterator key_value;
Ordering::const_iterator key_index;
for(key_value = values.begin(), key_index = ordering.begin();
key_value != values.end() && key_index != ordering.end(); ++key_value, ++key_index) {
assert(key_value->first == key_index->first);
const Index var = key_index->second;
if(ISDEBUG("ISAM2 update verbose")) {
if(mask[var])
cout << "expmap " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
else
cout << " " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
}
assert(delta[var].size() == (int)key_value->second.dim());
assert(delta[var].unaryExpr(&isfinite<double>).all());
if(mask[var]) {
Value* retracted = key_value->second.retract_(delta[var]);
key_value->second = *retracted;
retracted->deallocate_();
if(invalidateIfDebug)
(*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)
}
}
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
typename ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolveResult
ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
template<class CONDITIONAL, class GRAPH>
typename ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolveResult
ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
static const bool debug = ISDEBUG("ISAM2 recalculate");

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

@ -39,8 +39,8 @@ static const bool disableReordering = false;
static const double batchThreshold = 0.65;
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2(const ISAM2Params& params):
template<class CONDITIONAL, class GRAPH>
ISAM2<CONDITIONAL, GRAPH>::ISAM2(const ISAM2Params& params):
delta_(Permutation(), deltaUnpermuted_), params_(params) {
// See note in gtsam/base/boost_variant_with_workaround.h
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
@ -48,8 +48,8 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2(const ISAM2Params& params):
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2():
template<class CONDITIONAL, class GRAPH>
ISAM2<CONDITIONAL, GRAPH>::ISAM2():
delta_(Permutation(), deltaUnpermuted_) {
// See note in gtsam/base/boost_variant_with_workaround.h
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
@ -57,14 +57,14 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2():
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
FastList<size_t> ISAM2<CONDITIONAL, VALUES, GRAPH>::getAffectedFactors(const FastList<Index>& keys) const {
template<class CONDITIONAL, class GRAPH>
FastList<size_t> ISAM2<CONDITIONAL, GRAPH>::getAffectedFactors(const FastList<Index>& keys) const {
static const bool debug = false;
if(debug) cout << "Getting affected factors for ";
if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } }
if(debug) cout << endl;
FactorGraph<NonlinearFactor<VALUES> > allAffected;
FactorGraph<NonlinearFactor > allAffected;
FastList<size_t> indices;
BOOST_FOREACH(const Index key, keys) {
// const list<size_t> l = nonlinearFactors_.factors(key);
@ -86,9 +86,9 @@ FastList<size_t> ISAM2<CONDITIONAL, VALUES, GRAPH>::getAffectedFactors(const Fas
/* ************************************************************************* */
// retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors)
template<class CONDITIONAL, class VALUES, class GRAPH>
template<class CONDITIONAL, class GRAPH>
FactorGraph<GaussianFactor>::shared_ptr
ISAM2<CONDITIONAL, VALUES, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
ISAM2<CONDITIONAL, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
tic(1,"getAffectedFactors");
FastList<size_t> candidates = getAffectedFactors(affectedKeys);
@ -125,9 +125,9 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::relinearizeAffectedFactors(const FastList<Ind
/* ************************************************************************* */
// find intermediate (linearized) factors from cache that are passed into the affected area
template<class CONDITIONAL, class VALUES, class GRAPH>
FactorGraph<typename ISAM2<CONDITIONAL, VALUES, GRAPH>::CacheFactor>
ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
template<class CONDITIONAL, class GRAPH>
FactorGraph<typename ISAM2<CONDITIONAL, GRAPH>::CacheFactor>
ISAM2<CONDITIONAL, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
static const bool debug = false;
@ -151,8 +151,8 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
return cachedBoundary;
}
template<class CONDITIONAL, class VALUES, class GRAPH>
boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculate(
template<class CONDITIONAL, class GRAPH>
boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
const boost::optional<FastSet<Index> >& constrainKeys, ISAM2Result& result) {
@ -395,8 +395,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculat
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
template<class CONDITIONAL, class GRAPH>
ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
const boost::optional<FastSet<Symbol> >& constrainedKeys, bool force_relinearize) {
@ -579,36 +579,36 @@ ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate() const {
template<class CONDITIONAL, class GRAPH>
Values ISAM2<CONDITIONAL, GRAPH>::calculateEstimate() const {
// We use ExpmapMasked here instead of regular expmap because the former
// handles Permuted<VectorValues>
VALUES ret(theta_);
Values ret(theta_);
vector<bool> mask(ordering_.nVars(), true);
Impl::ExpmapMasked(ret, delta_, ordering_, mask);
return ret;
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
template<class CONDITIONAL, class GRAPH>
template<class KEY>
typename KEY::Value ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate(const KEY& key) const {
typename KEY::Value ISAM2<CONDITIONAL, GRAPH>::calculateEstimate(const KEY& key) const {
const Index index = getOrdering()[key];
const SubVector delta = getDelta()[index];
return getLinearizationPoint()[key].retract(delta);
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateBestEstimate() const {
template<class CONDITIONAL, class GRAPH>
Values ISAM2<CONDITIONAL, GRAPH>::calculateBestEstimate() const {
VectorValues delta(theta_.dims(ordering_));
optimize2(this->root(), delta);
return theta_.retract(delta, ordering_);
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam) {
template<class CONDITIONAL, class GRAPH>
VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam) {
VectorValues delta = *allocateVectorValues(isam);
optimize2(isam.root(), delta);
return delta;

20
gtsam/nonlinear/ISAM2.h
View File

@ -266,13 +266,13 @@ private:
* estimate of all variables.
*
*/
template<class CONDITIONAL, class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
template<class CONDITIONAL, class GRAPH = NonlinearFactorGraph>
class ISAM2: public BayesTree<CONDITIONAL, ISAM2Clique<CONDITIONAL> > {
protected:
/** The current linearization point */
VALUES theta_;
Values theta_;
/** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */
VariableIndex variableIndex_;
@ -314,8 +314,8 @@ private:
public:
typedef BayesTree<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class
typedef ISAM2<CONDITIONAL, VALUES> This; ///< This class
typedef VALUES Values;
typedef ISAM2<CONDITIONAL> This; ///< This class
typedef Values Values;
typedef GRAPH Graph;
/** Create an empty ISAM2 instance */
@ -368,19 +368,19 @@ public:
* (Params::relinearizeSkip).
* @return An ISAM2Result struct containing information about the update
*/
ISAM2Result update(const GRAPH& newFactors = GRAPH(), const VALUES& newTheta = VALUES(),
ISAM2Result update(const GRAPH& newFactors = GRAPH(), const Values& newTheta = Values(),
const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
const boost::optional<FastSet<Symbol> >& constrainedKeys = boost::none,
bool force_relinearize = false);
/** Access the current linearization point */
const VALUES& getLinearizationPoint() const {return theta_;}
const Values& getLinearizationPoint() const {return theta_;}
/** Compute an estimate from the incomplete linear delta computed during the last update.
* This delta is incomplete because it was not updated below wildfire_threshold. If only
* a single variable is needed, it is faster to call calculateEstimate(const KEY&).
*/
VALUES calculateEstimate() const;
Values calculateEstimate() const;
/** Compute an estimate for a single variable using its incomplete linear delta computed
* during the last update. This is faster than calling the no-argument version of
@ -399,7 +399,7 @@ public:
/** Compute an estimate using a complete delta computed by a full back-substitution.
*/
VALUES calculateBestEstimate() const;
Values calculateBestEstimate() const;
/** Access the current delta, computed during the last call to update */
const Permuted<VectorValues>& getDelta() const { return delta_; }
@ -435,8 +435,8 @@ private:
}; // ISAM2
/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
template<class CONDITIONAL, class VALUES, class GRAPH>
VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam);
template<class CONDITIONAL, class GRAPH>
VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam);
} /// namespace gtsam

13
gtsam/nonlinear/Key.h
View File

@ -31,6 +31,9 @@
namespace gtsam {
// Forward declarations
class Symbol;
/**
* TypedSymbol key class is templated on
* 1) the type T it is supposed to retrieve, for extra type checking
@ -76,6 +79,7 @@ public:
std::string latex() const {
return (boost::format("%c_{%d}") % C % j_).str();
}
Symbol symbol() const;
// logic:
@ -298,6 +302,9 @@ public:
std::string label_s = (boost::format("%1%") % label_).str();
return (boost::format("%c%s_{%d}") % C % label_s % this->j_).str();
}
Symbol symbol() const {
return Symbol(*this);
}
// Needed for conversion to LabeledSymbol
size_t convertLabel() const {
@ -354,5 +361,11 @@ private:
}
};
/* ************************************************************************* */
template<class T, char C>
Symbol TypedSymbol<T,C>::symbol() const {
return Symbol(*this);
}
} // namespace gtsam

9
gtsam/nonlinear/Makefile.am
View File

@ -16,15 +16,16 @@ check_PROGRAMS =
#----------------------------------------------------------------------------------------------------
# Lie Groups
headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h
headers += Values-inl.h
sources += Values.cpp
check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering
# Nonlinear nonlinear
headers += Key.h
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
headers += NonlinearFactorGraph.h
headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h
headers += NonlinearFactor.h
sources += NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp NonlinearOptimizationParameters.cpp
sources += NonlinearFactorGraph.cpp NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp NonlinearOptimizationParameters.cpp
headers += DoglegOptimizer.h DoglegOptimizer-inl.h
# Nonlinear iSAM(2)
@ -62,7 +63,7 @@ AM_CXXFLAGS =
#----------------------------------------------------------------------------------------------------
TESTS = $(check_PROGRAMS)
AM_LDFLAGS += $(boost_serialization)
LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../base/libbase.la ../3rdparty/libccolamd.la
LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../geometry/libgeometry.la ../base/libbase.la ../3rdparty/libccolamd.la
LDADD += ../../CppUnitLite/libCppUnitLite.a
AM_DEFAULT_SOURCE_EXT = .cpp

28
gtsam/nonlinear/NonlinearEquality.h
View File

@ -47,8 +47,8 @@ namespace gtsam {
*
* \nosubgrouping
*/
template<class VALUES, class KEY>
class NonlinearEquality: public NonlinearFactor1<VALUES, KEY> {
template<class KEY>
class NonlinearEquality: public NonlinearFactor1<KEY> {
public:
typedef typename KEY::Value T;
@ -71,7 +71,7 @@ namespace gtsam {
*/
bool (*compare_)(const T& a, const T& b);
typedef NonlinearFactor1<VALUES, KEY> Base;
typedef NonlinearFactor1<KEY> Base;
/** default constructor - only for serialization */
NonlinearEquality() {}
@ -110,7 +110,7 @@ namespace gtsam {
}
/** Check if two factors are equal */
bool equals(const NonlinearEquality<VALUES,KEY>& f, double tol = 1e-9) const {
bool equals(const NonlinearEquality<KEY>& f, double tol = 1e-9) const {
if (!Base::equals(f)) return false;
return feasible_.equals(f.feasible_, tol) &&
fabs(error_gain_ - f.error_gain_) < tol;
@ -121,7 +121,7 @@ namespace gtsam {
/// @{
/** actual error function calculation */
virtual double error(const VALUES& c) const {
virtual double error(const Values& c) const {
const T& xj = c[this->key_];
Vector e = this->unwhitenedError(c);
if (allow_error_ || !compare_(xj, feasible_)) {
@ -148,7 +148,7 @@ namespace gtsam {
}
// Linearize is over-written, because base linearization tries to whiten
virtual GaussianFactor::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const {
virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const {
const T& xj = x[this->key_];
Matrix A;
Vector b = evaluateError(xj, A);
@ -177,14 +177,14 @@ namespace gtsam {
/**
* Simple unary equality constraint - fixes a value for a variable
*/
template<class VALUES, class KEY>
class NonlinearEquality1 : public NonlinearFactor1<VALUES, KEY> {
template<class KEY>
class NonlinearEquality1 : public NonlinearFactor1<KEY> {
public:
typedef typename KEY::Value X;
protected:
typedef NonlinearFactor1<VALUES, KEY> Base;
typedef NonlinearFactor1<KEY> Base;
/** default constructor to allow for serialization */
NonlinearEquality1() {}
@ -196,7 +196,7 @@ namespace gtsam {
public:
typedef boost::shared_ptr<NonlinearEquality1<VALUES, KEY> > shared_ptr;
typedef boost::shared_ptr<NonlinearEquality1<KEY> > shared_ptr;
///TODO: comment
NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0)
@ -236,13 +236,13 @@ namespace gtsam {
* Simple binary equality constraint - this constraint forces two factors to
* be the same.
*/
template<class VALUES, class KEY>
class NonlinearEquality2 : public NonlinearFactor2<VALUES, KEY, KEY> {
template<class KEY>
class NonlinearEquality2 : public NonlinearFactor2<KEY, KEY> {
public:
typedef typename KEY::Value X;
protected:
typedef NonlinearFactor2<VALUES, KEY, KEY> Base;
typedef NonlinearFactor2<KEY, KEY> Base;
GTSAM_CONCEPT_MANIFOLD_TYPE(X);
@ -251,7 +251,7 @@ namespace gtsam {
public:
typedef boost::shared_ptr<NonlinearEquality2<VALUES, KEY> > shared_ptr;
typedef boost::shared_ptr<NonlinearEquality2<KEY> > shared_ptr;
///TODO: comment
NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0)

91
gtsam/nonlinear/NonlinearFactor.h
View File

@ -31,6 +31,7 @@
#include <gtsam/linear/SharedNoiseModel.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
@ -57,18 +58,17 @@ inline void __fill_from_tuple<boost::tuples::null_type>(std::vector<Symbol>& vec
* which are objects in non-linear manifolds (Lie groups).
* \nosubgrouping
*/
template<class VALUES>
class NonlinearFactor: public Factor<Symbol> {
protected:
// Some handy typedefs
typedef Factor<Symbol> Base;
typedef NonlinearFactor<VALUES> This;
typedef NonlinearFactor This;
public:
typedef boost::shared_ptr<NonlinearFactor<VALUES> > shared_ptr;
typedef boost::shared_ptr<NonlinearFactor > shared_ptr;
/// @name Standard Constructors
/// @{
@ -120,7 +120,7 @@ public:
* This is typically equal to log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian.
* You can override this for systems with unusual noise models.
*/
virtual double error(const VALUES& c) const = 0;
virtual double error(const Values& c) const = 0;
/** get the dimension of the factor (number of rows on linearization) */
virtual size_t dim() const = 0;
@ -135,11 +135,11 @@ public:
* when the constraint is *NOT* fulfilled.
* @return true if the constraint is active
*/
virtual bool active(const VALUES& c) const { return true; }
virtual bool active(const Values& c) const { return true; }
/** linearize to a GaussianFactor */
virtual boost::shared_ptr<GaussianFactor>
linearize(const VALUES& c, const Ordering& ordering) const = 0;
linearize(const Values& c, const Ordering& ordering) const = 0;
/**
* Create a symbolic factor using the given ordering to determine the
@ -165,20 +165,19 @@ public:
* The noise model is typically Gaussian, but robust and constrained error models are also supported.
*/
template<class VALUES>
class NoiseModelFactor: public NonlinearFactor<VALUES> {
class NoiseModelFactor: public NonlinearFactor {
protected:
// handy typedefs
typedef NonlinearFactor<VALUES> Base;
typedef NoiseModelFactor<VALUES> This;
typedef NonlinearFactor Base;
typedef NoiseModelFactor This;
SharedNoiseModel noiseModel_; /** Noise model */
public:
typedef boost::shared_ptr<NoiseModelFactor<VALUES> > shared_ptr;
typedef boost::shared_ptr<NoiseModelFactor > shared_ptr;
/** Default constructor for I/O only */
NoiseModelFactor() {
@ -225,7 +224,7 @@ public:
}
/** Check if two factors are equal */
virtual bool equals(const NoiseModelFactor<VALUES>& f, double tol = 1e-9) const {
virtual bool equals(const NoiseModelFactor& f, double tol = 1e-9) const {
return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, tol);
}
@ -245,13 +244,13 @@ public:
* If any of the optional Matrix reference arguments are specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2, X3...).
*/
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
/**
* Vector of errors, whitened
* This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian
*/
Vector whitenedError(const VALUES& c) const {
Vector whitenedError(const Values& c) const {
return noiseModel_->whiten(unwhitenedError(c));
}
@ -261,7 +260,7 @@ public:
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
*/
virtual double error(const VALUES& c) const {
virtual double error(const Values& c) const {
if (this->active(c))
return 0.5 * noiseModel_->distance(unwhitenedError(c));
else
@ -273,7 +272,7 @@ public:
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/
boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
boost::shared_ptr<GaussianFactor> linearize(const Values& x, const Ordering& ordering) const {
// Only linearize if the factor is active
if (!this->active(x))
return boost::shared_ptr<JacobianFactor>();
@ -321,8 +320,8 @@ private:
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 1
* variable. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY>
class NonlinearFactor1: public NoiseModelFactor<VALUES> {
template<class KEY>
class NonlinearFactor1: public NoiseModelFactor {
public:
@ -334,8 +333,8 @@ protected:
// The value of the key. Not const to allow serialization
KEY key_;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor1<VALUES, KEY> This;
typedef NoiseModelFactor Base;
typedef NonlinearFactor1<KEY> This;
public:
@ -357,7 +356,7 @@ public:
/** Calls the 1-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
const X& x1 = x[key_];
if(H) {
@ -400,8 +399,8 @@ private:
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 2
* variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2>
class NonlinearFactor2: public NoiseModelFactor<VALUES> {
template<class KEY1, class KEY2>
class NonlinearFactor2: public NoiseModelFactor {
public:
@ -415,8 +414,8 @@ protected:
KEY1 key1_;
KEY2 key2_;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor2<VALUES, KEY1, KEY2> This;
typedef NoiseModelFactor Base;
typedef NonlinearFactor2<KEY1, KEY2> This;
public:
@ -441,7 +440,7 @@ public:
/** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
const X1& x1 = x[key1_];
const X2& x2 = x[key2_];
@ -488,8 +487,8 @@ private:
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 3
* variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3>
class NonlinearFactor3: public NoiseModelFactor<VALUES> {
template<class KEY1, class KEY2, class KEY3>
class NonlinearFactor3: public NoiseModelFactor {
public:
@ -505,8 +504,8 @@ protected:
KEY2 key2_;
KEY3 key3_;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor3<VALUES, KEY1, KEY2, KEY3> This;
typedef NoiseModelFactor Base;
typedef NonlinearFactor3<KEY1, KEY2, KEY3> This;
public:
@ -533,7 +532,7 @@ public:
/** Calls the 3-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]);
@ -582,8 +581,8 @@ private:
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 4
* variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4>
class NonlinearFactor4: public NoiseModelFactor<VALUES> {
template<class KEY1, class KEY2, class KEY3, class KEY4>
class NonlinearFactor4: public NoiseModelFactor {
public:
@ -601,8 +600,8 @@ protected:
KEY3 key3_;
KEY4 key4_;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor4<VALUES, KEY1, KEY2, KEY3, KEY4> This;
typedef NoiseModelFactor Base;
typedef NonlinearFactor4<KEY1, KEY2, KEY3, KEY4> This;
public:
@ -631,7 +630,7 @@ public:
/** Calls the 4-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
@ -682,8 +681,8 @@ private:
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 5
* variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4, class KEY5>
class NonlinearFactor5: public NoiseModelFactor<VALUES> {
template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5>
class NonlinearFactor5: public NoiseModelFactor {
public:
@ -703,8 +702,8 @@ protected:
KEY4 key4_;
KEY5 key5_;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor5<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5> This;
typedef NoiseModelFactor Base;
typedef NonlinearFactor5<KEY1, KEY2, KEY3, KEY4, KEY5> This;
public:
@ -735,7 +734,7 @@ public:
/** Calls the 5-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
@ -789,8 +788,8 @@ private:
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 6
* variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4, class KEY5, class KEY6>
class NonlinearFactor6: public NoiseModelFactor<VALUES> {
template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5, class KEY6>
class NonlinearFactor6: public NoiseModelFactor {
public:
@ -812,8 +811,8 @@ protected:
KEY5 key5_;
KEY6 key6_;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor6<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> This;
typedef NoiseModelFactor Base;
typedef NonlinearFactor6<KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> This;
public:
@ -846,7 +845,7 @@ public:
/** Calls the 6-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);

41
gtsam/nonlinear/NonlinearFactorGraph-inl.h → gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -10,39 +10,35 @@
* -------------------------------------------------------------------------- */
/**
* @file NonlinearFactorGraph-inl.h
* @file NonlinearFactorGraph.cpp
* @brief Factor Graph Consisting of non-linear factors
* @author Frank Dellaert
* @author Carlos Nieto
* @author Christian Potthast
*/
#pragma once
#include <cmath>
#include <boost/foreach.hpp>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/inference.h>
#include <boost/foreach.hpp>
#include <cmath>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class VALUES>
double NonlinearFactorGraph<VALUES>::probPrime(const VALUES& c) const {
double NonlinearFactorGraph::probPrime(const Values& c) const {
return exp(-0.5 * error(c));
}
/* ************************************************************************* */
template<class VALUES>
void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
void NonlinearFactorGraph::print(const std::string& str) const {
Base::print(str);
}
/* ************************************************************************* */
template<class VALUES>
double NonlinearFactorGraph<VALUES>::error(const VALUES& c) const {
double NonlinearFactorGraph::error(const Values& c) const {
double total_error = 0.;
// iterate over all the factors_ to accumulate the log probabilities
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
@ -53,8 +49,7 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUES>
std::set<Symbol> NonlinearFactorGraph<VALUES>::keys() const {
std::set<Symbol> NonlinearFactorGraph::keys() const {
std::set<Symbol> keys;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor)
@ -64,9 +59,8 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUES>
Ordering::shared_ptr NonlinearFactorGraph<VALUES>::orderingCOLAMD(
const VALUES& config) const {
Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
const Values& config) const {
// Create symbolic graph and initial (iterator) ordering
SymbolicFactorGraph::shared_ptr symbolic;
@ -91,8 +85,7 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUES>
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::symbolic(const Ordering& ordering) const {
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const {
// Generate the symbolic factor graph
SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
symbolicfg->reserve(this->size());
@ -108,21 +101,19 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUES>
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph<
VALUES>::symbolic(const VALUES& config) const {
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
const Values& config) const {
// Generate an initial key ordering in iterator order
Ordering::shared_ptr ordering(config.orderingArbitrary());
return make_pair(symbolic(*ordering), ordering);
}
/* ************************************************************************* */
template<class VALUES>
typename GaussianFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::linearize(
const VALUES& config, const Ordering& ordering) const {
GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
const Values& config, const Ordering& ordering) const {
// create an empty linear FG
typename GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
linearFG->reserve(this->size());
// linearize all factors

49
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -28,42 +28,37 @@
namespace gtsam {
/**
* A non-linear factor graph is templated on a values structure, but the factor type
* is fixed as a NonlinearFactor. The values structures are typically (in SAM) more general
* A non-linear factor graph is a graph of non-Gaussian, i.e. non-linear factors,
* which derive from NonlinearFactor. The values structures are typically (in SAM) more general
* than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds.
* Linearizing the non-linear factor graph creates a linear factor graph on the
* tangent vector space at the linearization point. Because the tangent space is a true
* vector space, the config type will be an VectorValues in that linearized factor graph.
* \nosubgrouping
*/
template<class VALUES>
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<VALUES> > {
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
public:
typedef VALUES Values;
typedef FactorGraph<NonlinearFactor<VALUES> > Base;
typedef boost::shared_ptr<NonlinearFactorGraph<VALUES> > shared_ptr;
typedef boost::shared_ptr<NonlinearFactor<VALUES> > sharedFactor;
/// @name Testable
/// @{
typedef FactorGraph<NonlinearFactor> Base;
typedef boost::shared_ptr<NonlinearFactorGraph> shared_ptr;
typedef boost::shared_ptr<NonlinearFactor> sharedFactor;
/** print just calls base class */
void print(const std::string& str = "NonlinearFactorGraph: ") const;
/// @}
/// @name Standard Interface
/// @{
/** return keys in some random order */
std::set<Symbol> keys() const;
/** unnormalized error */
double error(const VALUES& c) const;
double error(const Values& c) const;
/** Unnormalized probability. O(n) */
double probPrime(const VALUES& c) const;
double probPrime(const Values& c) const;
template<class F>
void add(const F& factor) {
this->push_back(boost::shared_ptr<F>(new F(factor)));
}
/**
* Create a symbolic factor graph using an existing ordering
@ -77,31 +72,20 @@ namespace gtsam {
* ordering is found.
*/
std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
symbolic(const VALUES& config) const;
symbolic(const Values& config) const;
/**
* Compute a fill-reducing ordering using COLAMD. This returns the
* ordering and a VariableIndex, which can later be re-used to save
* computation.
*/
Ordering::shared_ptr orderingCOLAMD(const VALUES& config) const;
Ordering::shared_ptr orderingCOLAMD(const Values& config) const;
/**
* linearize a nonlinear factor graph
*/
boost::shared_ptr<GaussianFactorGraph >
linearize(const VALUES& config, const Ordering& ordering) const;
/// @}
/// @name Advanced Interface
/// @{
template<class F>
void add(const F& factor) {
this->push_back(boost::shared_ptr<F>(new F(factor)));
}
/// @}
linearize(const Values& config, const Ordering& ordering) const;
private:
@ -116,4 +100,3 @@ namespace gtsam {
} // namespace
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>

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

@ -29,8 +29,8 @@ using namespace std;
using namespace gtsam;
/* ************************************************************************* */
template<class VALUES, class GRAPH>
void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors,
template<class GRAPH>
void NonlinearISAM<GRAPH>::update(const Factors& newFactors,
const Values& initialValues) {
if(newFactors.size() > 0) {
@ -62,8 +62,8 @@ void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors,
}
/* ************************************************************************* */
template<class VALUES, class GRAPH>
void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() {
template<class GRAPH>
void NonlinearISAM<GRAPH>::reorder_relinearize() {
// cout << "Reordering, relinearizing..." << endl;
@ -89,8 +89,8 @@ void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() {
}
/* ************************************************************************* */
template<class VALUES, class GRAPH>
VALUES NonlinearISAM<VALUES,GRAPH>::estimate() const {
template<class GRAPH>
Values NonlinearISAM<GRAPH>::estimate() const {
if(isam_.size() > 0)
return linPoint_.retract(optimize(isam_), ordering_);
else
@ -98,14 +98,14 @@ VALUES NonlinearISAM<VALUES,GRAPH>::estimate() const {
}
/* ************************************************************************* */
template<class VALUES, class GRAPH>
Matrix NonlinearISAM<VALUES,GRAPH>::marginalCovariance(const Symbol& key) const {
template<class GRAPH>
Matrix NonlinearISAM<GRAPH>::marginalCovariance(const Symbol& key) const {
return isam_.marginalCovariance(ordering_[key]);
}
/* ************************************************************************* */
template<class VALUES, class GRAPH>
void NonlinearISAM<VALUES,GRAPH>::print(const std::string& s) const {
template<class GRAPH>
void NonlinearISAM<GRAPH>::print(const std::string& s) const {
cout << "ISAM - " << s << ":" << endl;
cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl;
isam_.print("GaussianISAM");

3
gtsam/nonlinear/NonlinearISAM.h
View File

@ -24,11 +24,10 @@ namespace gtsam {
/**
* Wrapper class to manage ISAM in a nonlinear context
*/
template<class VALUES, class GRAPH = gtsam::NonlinearFactorGraph<VALUES> >
template<class GRAPH = gtsam::NonlinearFactorGraph >
class NonlinearISAM {
public:
typedef VALUES Values;
typedef GRAPH Factors;
protected:

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

@ -34,17 +34,17 @@ namespace gtsam {
/**
* The Elimination solver
*/
template<class G, class T>
T optimizeSequential(const G& graph, const T& initialEstimate,
template<class G>
Values optimizeSequential(const G& graph, const Values& initialEstimate,
const NonlinearOptimizationParameters& parameters, bool useLM) {
// Use a variable ordering from COLAMD
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
// Create an nonlinear Optimizer that uses a Sequential Solver
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
typedef NonlinearOptimizer<G, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
Optimizer optimizer(boost::make_shared<const G>(graph),
boost::make_shared<const T>(initialEstimate), ordering,
boost::make_shared<const Values>(initialEstimate), ordering,
boost::make_shared<NonlinearOptimizationParameters>(parameters));
// Now optimize using either LM or GN methods.
@ -57,17 +57,17 @@ namespace gtsam {
/**
* The multifrontal solver
*/
template<class G, class T>
T optimizeMultiFrontal(const G& graph, const T& initialEstimate,
template<class G>
Values optimizeMultiFrontal(const G& graph, const Values& initialEstimate,
const NonlinearOptimizationParameters& parameters, bool useLM) {
// Use a variable ordering from COLAMD
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
// Create an nonlinear Optimizer that uses a Multifrontal Solver
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
typedef NonlinearOptimizer<G, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
Optimizer optimizer(boost::make_shared<const G>(graph),
boost::make_shared<const T>(initialEstimate), ordering,
boost::make_shared<const Values>(initialEstimate), ordering,
boost::make_shared<NonlinearOptimizationParameters>(parameters));
// now optimize using either LM or GN methods
@ -81,20 +81,20 @@ namespace gtsam {
/**
* The sparse preconditioned conjugate gradient solver
*/
template<class G, class T>
T optimizeSPCG(const G& graph, const T& initialEstimate,
template<class G>
Values optimizeSPCG(const G& graph, const Values& initialEstimate,
const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
bool useLM = true) {
// initial optimization state is the same in both cases tested
typedef SubgraphSolver<G,GaussianFactorGraph,T> Solver;
typedef SubgraphSolver<G,GaussianFactorGraph,Values> Solver;
typedef boost::shared_ptr<Solver> shared_Solver;
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, Solver> SPCGOptimizer;
typedef NonlinearOptimizer<G, GaussianFactorGraph, Solver> SPCGOptimizer;
shared_Solver solver = boost::make_shared<Solver>(
graph, initialEstimate, IterativeOptimizationParameters());
SPCGOptimizer optimizer(
boost::make_shared<const G>(graph),
boost::make_shared<const T>(initialEstimate),
boost::make_shared<const Values>(initialEstimate),
solver->ordering(),
solver,
boost::make_shared<NonlinearOptimizationParameters>(parameters));
@ -110,20 +110,20 @@ namespace gtsam {
/**
* optimization that returns the values
*/
template<class G, class T>
T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters,
template<class G>
Values optimize(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters,
const LinearSolver& solver,
const NonlinearOptimizationMethod& nonlinear_method) {
switch (solver) {
case SEQUENTIAL:
return optimizeSequential<G,T>(graph, initialEstimate, parameters,
return optimizeSequential<G>(graph, initialEstimate, parameters,
nonlinear_method == LM);
case MULTIFRONTAL:
return optimizeMultiFrontal<G,T>(graph, initialEstimate, parameters,
return optimizeMultiFrontal<G>(graph, initialEstimate, parameters,
nonlinear_method == LM);
#if ENABLE_SPCG
case SPCG:
// return optimizeSPCG<G,T>(graph, initialEstimate, parameters,
// return optimizeSPCG<G,Values>(graph, initialEstimate, parameters,
// nonlinear_method == LM);
throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint");
#endif

4
gtsam/nonlinear/NonlinearOptimization.h
View File

@ -44,8 +44,8 @@ namespace gtsam {
/**
* optimization that returns the values
*/
template<class G, class T>
T optimize(const G& graph, const T& initialEstimate,
template<class G>
Values optimize(const G& graph, const Values& initialEstimate,
const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
const LinearSolver& solver = MULTIFRONTAL,
const NonlinearOptimizationMethod& nonlinear_method = LM);

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

@ -30,8 +30,8 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W>::NonlinearOptimizer(shared_graph graph,
shared_values values, shared_ordering ordering, shared_parameters parameters) :
graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering),
parameters_(parameters), iterations_(0),
@ -47,8 +47,8 @@ namespace gtsam {
/* ************************************************************************* */
// FIXME: remove this constructor
template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W>::NonlinearOptimizer(shared_graph graph,
shared_values values, shared_ordering ordering,
shared_solver spcg_solver, shared_parameters parameters) :
graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering),
@ -66,8 +66,8 @@ namespace gtsam {
/* ************************************************************************* */
// One iteration of Gauss Newton
/* ************************************************************************* */
template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterate() const {
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterate() const {
Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
@ -86,7 +86,7 @@ namespace gtsam {
if (verbosity >= Parameters::DELTA) delta.print("delta");
// take old values and update it
shared_values newValues(new C(values_->retract(delta, *ordering_)));
shared_values newValues(new Values(values_->retract(delta, *ordering_)));
// maybe show output
if (verbosity >= Parameters::VALUES) newValues->print("newValues");
@ -99,8 +99,8 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::gaussNewton() const {
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::gaussNewton() const {
static W writer(error_);
if (error_ < parameters_->sumError_ ) {
@ -130,8 +130,8 @@ namespace gtsam {
// TODO: in theory we can't infinitely recurse, but maybe we should put a max.
// Reminder: the parameters are Graph type $G$, Values class type $T$,
// linear system class $L$, the non linear solver type $S$, and the writer type $W$
template<class G, class T, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::try_lambda(const L& linearSystem) {
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::try_lambda(const L& linearSystem) {
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
const Parameters::LambdaMode lambdaMode = parameters_->lambdaMode_ ;
@ -178,7 +178,7 @@ namespace gtsam {
if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
// update values
shared_values newValues(new T(values_->retract(delta, *ordering_)));
shared_values newValues(new Values(values_->retract(delta, *ordering_)));
// create new optimization state with more adventurous lambda
double error = graph_->error(*newValues);
@ -228,8 +228,8 @@ namespace gtsam {
// One iteration of Levenberg Marquardt
// Reminder: the parameters are Graph type $G$, Values class type $T$,
// linear system class $L$, the non linear solver type $S$, and the writer type $W$
template<class G, class T, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateLM() {
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterateLM() {
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
const double lambda = parameters_->lambda_ ;
@ -253,8 +253,8 @@ namespace gtsam {
/* ************************************************************************* */
// Reminder: the parameters are Graph type $G$, Values class type $T$,
// linear system class $L$, the non linear solver type $S$, and the writer type $W$
template<class G, class T, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::levenbergMarquardt() {
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::levenbergMarquardt() {
// Initialize
bool converged = false;
@ -299,20 +299,20 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class G, class T, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateDogLeg() {
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterateDogLeg() {
S solver(*graph_->linearize(*values_, *ordering_), parameters_->useQR_);
DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(
parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(),
*graph_, *values_, *ordering_, error_, parameters_->verbosity_ > Parameters::ERROR);
shared_values newValues(new T(values_->retract(result.dx_d, *ordering_)));
shared_values newValues(new Values(values_->retract(result.dx_d, *ordering_)));
return newValuesErrorLambda_(newValues, result.f_error, result.Delta);
}
/* ************************************************************************* */
template<class G, class T, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::dogLeg() {
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::dogLeg() {
static W writer(error_);
// check if we're already close enough

44
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -37,20 +37,20 @@ public:
* and then one of the optimization routines is called. These iterate
* until convergence. All methods are functional and return a new state.
*
* The class is parameterized by the Graph type $G$, Values class type $T$,
* The class is parameterized by the Graph type $G$, Values class type $Values$,
* linear system class $L$, the non linear solver type $S$, and the writer type $W$
*
* The values class type $T$ is in order to be able to optimize over non-vector values structures.
* The values class type $Values$ is in order to be able to optimize over non-vector values structures.
*
* A nonlinear system solver $S$
* Concept NonLinearSolver<G,T,L> implements
* linearize: G * T -> L
* solve : L -> T
* Concept NonLinearSolver<G,Values,L> implements
* linearize: G * Values -> L
* solve : L -> Values
*
* The writer $W$ generates output to disk or the screen.
*
* For example, in a 2D case, $G$ can be Pose2Graph, $T$ can be Pose2Values,
* $L$ can be GaussianFactorGraph and $S$ can be Factorization<Pose2Graph, Pose2Values>.
* For example, in a 2D case, $G$ can be pose2SLAM::Graph, $Values$ can be Pose2Values,
* $L$ can be GaussianFactorGraph and $S$ can be Factorization<pose2SLAM::Graph, Pose2Values>.
* The solver class has two main functions: linearize and optimize. The first one
* linearizes the nonlinear cost function around the current estimate, and the second
* one optimizes the linearized system using various methods.
@ -58,12 +58,12 @@ public:
* To use the optimizer in code, include <gtsam/NonlinearOptimizer-inl.h> in your cpp file
* \nosubgrouping
*/
template<class G, class T, class L = GaussianFactorGraph, class GS = GaussianMultifrontalSolver, class W = NullOptimizerWriter>
template<class G, class L = GaussianFactorGraph, class GS = GaussianMultifrontalSolver, class W = NullOptimizerWriter>
class NonlinearOptimizer {
public:
// For performance reasons in recursion, we store values in a shared_ptr
typedef boost::shared_ptr<const T> shared_values; ///Prevent memory leaks in Values
typedef boost::shared_ptr<const Values> shared_values; ///Prevent memory leaks in Values
typedef boost::shared_ptr<const G> shared_graph; /// Prevent memory leaks in Graph
typedef boost::shared_ptr<L> shared_linear; /// Not sure
typedef boost::shared_ptr<const Ordering> shared_ordering; ///ordering parameters
@ -74,7 +74,7 @@ public:
private:
typedef NonlinearOptimizer<G, T, L, GS> This;
typedef NonlinearOptimizer<G, L, GS> This;
typedef boost::shared_ptr<const std::vector<size_t> > shared_dimensions;
/// keep a reference to const version of the graph
@ -182,7 +182,7 @@ public:
/**
* Copy constructor
*/
NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) :
NonlinearOptimizer(const NonlinearOptimizer<G, L, GS> &optimizer) :
graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_),
ordering_(optimizer.ordering_), parameters_(optimizer.parameters_),
iterations_(optimizer.iterations_), dimensions_(optimizer.dimensions_), structure_(optimizer.structure_) {}
@ -248,7 +248,7 @@ public:
/**
* linearize and optimize
* This returns an VectorValues, i.e., vectors in tangent space of T
* This returns an VectorValues, i.e., vectors in tangent space of Values
*/
VectorValues linearizeAndOptimizeForDelta() const {
return *createSolver()->optimize();
@ -340,19 +340,19 @@ public:
* Static interface to LM optimization (no shared_ptr arguments) - see above
*/
static shared_values optimizeLM(const G& graph,
const T& values,
const Values& values,
const Parameters parameters = Parameters()) {
return optimizeLM(boost::make_shared<const G>(graph),
boost::make_shared<const T>(values),
boost::make_shared<const Values>(values),
boost::make_shared<Parameters>(parameters));
}
///TODO: comment
static shared_values optimizeLM(const G& graph,
const T& values,
const Values& values,
Parameters::verbosityLevel verbosity) {
return optimizeLM(boost::make_shared<const G>(graph),
boost::make_shared<const T>(values),
boost::make_shared<const Values>(values),
verbosity);
}
@ -392,19 +392,19 @@ public:
* Static interface to Dogleg optimization (no shared_ptr arguments) - see above
*/
static shared_values optimizeDogLeg(const G& graph,
const T& values,
const Values& values,
const Parameters parameters = Parameters()) {
return optimizeDogLeg(boost::make_shared<const G>(graph),
boost::make_shared<const T>(values),
boost::make_shared<const Values>(values),
boost::make_shared<Parameters>(parameters));
}
///TODO: comment
static shared_values optimizeDogLeg(const G& graph,
const T& values,
const Values& values,
Parameters::verbosityLevel verbosity) {
return optimizeDogLeg(boost::make_shared<const G>(graph),
boost::make_shared<const T>(values),
boost::make_shared<const Values>(values),
verbosity);
}
@ -431,9 +431,9 @@ public:
/**
* Static interface to GN optimization (no shared_ptr arguments) - see above
*/
static shared_values optimizeGN(const G& graph, const T& values, const Parameters parameters = Parameters()) {
static shared_values optimizeGN(const G& graph, const Values& values, const Parameters parameters = Parameters()) {
return optimizeGN(boost::make_shared<const G>(graph),
boost::make_shared<const T>(values),
boost::make_shared<const Values>(values),
boost::make_shared<Parameters>(parameters));
}
};

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

@ -1,165 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file TupleValues-inl.h
* @author Richard Roberts
* @author Manohar Paluri
* @author Alex Cunningham
*/
#pragma once
namespace gtsam {
/* ************************************************************************* */
/** TupleValuesN Implementations */
/* ************************************************************************* */
/* ************************************************************************* */
/** TupleValues 1 */
template<class VALUES1>
TupleValues1<VALUES1>::TupleValues1(const TupleValues1<VALUES1>& values) :
TupleValuesEnd<VALUES1> (values) {}
template<class VALUES1>
TupleValues1<VALUES1>::TupleValues1(const VALUES1& cfg1) :
TupleValuesEnd<VALUES1> (cfg1) {}
template<class VALUES1>
TupleValues1<VALUES1>::TupleValues1(const TupleValuesEnd<VALUES1>& values) :
TupleValuesEnd<VALUES1>(values) {}
/* ************************************************************************* */
/** TupleValues 2 */
template<class VALUES1, class VALUES2>
TupleValues2<VALUES1, VALUES2>::TupleValues2(const TupleValues2<VALUES1, VALUES2>& values) :
TupleValues<VALUES1, TupleValuesEnd<VALUES2> >(values) {}
template<class VALUES1, class VALUES2>
TupleValues2<VALUES1, VALUES2>::TupleValues2(const VALUES1& cfg1, const VALUES2& cfg2) :
TupleValues<VALUES1, TupleValuesEnd<VALUES2> >(
cfg1, TupleValuesEnd<VALUES2>(cfg2)) {}
template<class VALUES1, class VALUES2>
TupleValues2<VALUES1, VALUES2>::TupleValues2(const TupleValues<VALUES1, TupleValuesEnd<VALUES2> >& values) :
TupleValues<VALUES1, TupleValuesEnd<VALUES2> >(values) {}
/* ************************************************************************* */
/** TupleValues 3 */
template<class VALUES1, class VALUES2, class VALUES3>
TupleValues3<VALUES1, VALUES2, VALUES3>::TupleValues3(
const TupleValues3<VALUES1, VALUES2, VALUES3>& values) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >(values) {}
template<class VALUES1, class VALUES2, class VALUES3>
TupleValues3<VALUES1, VALUES2, VALUES3>::TupleValues3(
const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >(
cfg1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> >(
cfg2, TupleValuesEnd<VALUES3>(cfg3))) {}
template<class VALUES1, class VALUES2, class VALUES3>
TupleValues3<VALUES1, VALUES2, VALUES3>::TupleValues3(
const TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >& values) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValuesEnd<VALUES3> > >(values) {}
/* ************************************************************************* */
/** TupleValues 4 */
template<class VALUES1, class VALUES2, class VALUES3, class VALUES4>
TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>::TupleValues4(
const TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>& values) :
TupleValues<VALUES1, TupleValues<VALUES2,
TupleValues<VALUES3, TupleValuesEnd<VALUES4> > > >(values) {}
template<class VALUES1, class VALUES2, class VALUES3, class VALUES4>
TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>::TupleValues4(
const VALUES1& cfg1, const VALUES2& cfg2,
const VALUES3& cfg3,const VALUES4& cfg4) :
TupleValues<VALUES1, TupleValues<VALUES2,
TupleValues<VALUES3, TupleValuesEnd<VALUES4> > > >(
cfg1, TupleValues<VALUES2, TupleValues<VALUES3, TupleValuesEnd<VALUES4> > >(
cfg2, TupleValues<VALUES3, TupleValuesEnd<VALUES4> >(
cfg3, TupleValuesEnd<VALUES4>(cfg4)))) {}
template<class VALUES1, class VALUES2, class VALUES3, class VALUES4>
TupleValues4<VALUES1, VALUES2, VALUES3, VALUES4>::TupleValues4(
const TupleValues<VALUES1, TupleValues<VALUES2,
TupleValues<VALUES3, TupleValuesEnd<VALUES4> > > >& values) :
TupleValues<VALUES1, TupleValues<VALUES2,TupleValues<VALUES3,
TupleValuesEnd<VALUES4> > > >(values) {}
/* ************************************************************************* */
/** TupleValues 5 */
template<class VALUES1, class VALUES2, class VALUES3, class VALUES4, class VALUES5>
TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>::TupleValues5(
const TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>& values) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > > >(values) {}
template<class VALUES1, class VALUES2, class VALUES3, class VALUES4, class VALUES5>
TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>::TupleValues5(
const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3,
const VALUES4& cfg4, const VALUES5& cfg5) :
TupleValues<VALUES1, TupleValues<VALUES2,
TupleValues<VALUES3, TupleValues<VALUES4,
TupleValuesEnd<VALUES5> > > > >(
cfg1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > >(
cfg2, TupleValues<VALUES3, TupleValues<VALUES4, TupleValuesEnd<VALUES5> > >(
cfg3, TupleValues<VALUES4, TupleValuesEnd<VALUES5> >(
cfg4, TupleValuesEnd<VALUES5>(cfg5))))) {}
template<class VALUES1, class VALUES2, class VALUES3, class VALUES4, class VALUES5>
TupleValues5<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5>::TupleValues5(
const TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > > >& values) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValuesEnd<VALUES5> > > > >(values) {}
/* ************************************************************************* */
/** TupleValues 6 */
template<class VALUES1, class VALUES2, class VALUES3,
class VALUES4, class VALUES5, class VALUES6>
TupleValues6<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5, VALUES6>::TupleValues6(
const TupleValues6<VALUES1, VALUES2, VALUES3,
VALUES4, VALUES5, VALUES6>& values) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValues<VALUES5,
TupleValuesEnd<VALUES6> > > > > >(values) {}
template<class VALUES1, class VALUES2, class VALUES3,
class VALUES4, class VALUES5, class VALUES6>
TupleValues6<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5, VALUES6>::TupleValues6(
const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3,
const VALUES4& cfg4, const VALUES5& cfg5, const VALUES6& cfg6) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValues<VALUES5, TupleValuesEnd<VALUES6> > > > > >(
cfg1, TupleValues<VALUES2, TupleValues<VALUES3, TupleValues<VALUES4,
TupleValues<VALUES5, TupleValuesEnd<VALUES6> > > > >(
cfg2, TupleValues<VALUES3, TupleValues<VALUES4, TupleValues<VALUES5,
TupleValuesEnd<VALUES6> > > >(
cfg3, TupleValues<VALUES4, TupleValues<VALUES5,
TupleValuesEnd<VALUES6> > >(
cfg4, TupleValues<VALUES5, TupleValuesEnd<VALUES6> >(
cfg5, TupleValuesEnd<VALUES6>(cfg6)))))) {}
template<class VALUES1, class VALUES2, class VALUES3,
class VALUES4, class VALUES5, class VALUES6>
TupleValues6<VALUES1, VALUES2, VALUES3, VALUES4, VALUES5, VALUES6>::TupleValues6(
const TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValues<VALUES5,
TupleValuesEnd<VALUES6> > > > > >& values) :
TupleValues<VALUES1, TupleValues<VALUES2, TupleValues<VALUES3,
TupleValues<VALUES4, TupleValues<VALUES5,
TupleValuesEnd<VALUES6> > > > > >(values) {}
}

550
gtsam/nonlinear/TupleValues.h
View File

@ -1,550 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file TupleValues.h
* @author Richard Roberts
* @author Manohar Paluri
* @author Alex Cunningham
*/
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/VectorValues.h>
#pragma once
namespace gtsam {
/**
* TupleValues are a structure to manage heterogenous Values, so as to
* enable different types of variables/keys to be used simultaneously. We
* do this with recursive templates (instead of blind pointer casting) to
* reduce run-time overhead and keep static type checking. The interface
* mimics that of a single Values.
*
* This uses a recursive structure of values pairs to form a lisp-like
* list, with a special case (TupleValuesEnd) that contains only one values
* at the end. Because this recursion is done at compile time, there is no
* runtime performance hit to using this structure.
*
* For an easy interface, there are TupleValuesN classes, which wrap
* the recursive TupleValues together as a single class, so you can have
* mixed-type classes from 2-6 types. Note that a TupleValues2 is equivalent
* to the previously used PairValues.
*
* Design and extension note:
* To implement a recursively templated data structure, note that most operations
* have two versions: one with templates and one without. The templated one allows
* for the arguments to be passed to the next values, while the specialized one
* operates on the "first" values. TupleValuesEnd contains only the specialized version.
* \nosubgrouping
*/
template<class VALUES1, class VALUES2>
class TupleValues {
protected:
// Data for internal valuess
VALUES1 first_; /// Arbitrary values
VALUES2 second_; /// A TupleValues or TupleValuesEnd, which wraps an arbitrary values
/** concept checks */
GTSAM_CONCEPT_TESTABLE_TYPE(VALUES1)
GTSAM_CONCEPT_TESTABLE_TYPE(VALUES2)
public:
// typedefs for values subtypes
typedef typename VALUES1::Key Key1;
typedef typename VALUES1::Value Value1;
/// @name Standard Constructors
/// @{
/** default constructor */
TupleValues() {}
/** Copy constructor */
TupleValues(const TupleValues<VALUES1, VALUES2>& values) :
first_(values.first_), second_(values.second_) {}
/** Construct from valuess */
TupleValues(const VALUES1& cfg1, const VALUES2& cfg2) :
first_(cfg1), second_(cfg2) {}
/// @}
/// @name Testable
/// @{
/** Print */
void print(const std::string& s = "") const {
first_.print(s);
second_.print();
}
/** Equality with tolerance for keys and values */
bool equals(const TupleValues<VALUES1, VALUES2>& c, double tol=1e-9) const {
return first_.equals(c.first_, tol) && second_.equals(c.second_, tol);
}
/// @}
/// @name Advanced Interface
/// @{
/**
* Insert a key/value pair to the values.
* Note: if the key is already in the values, the values will not be changed.
* Use update() to allow for changing existing values.
* @param key is the key - can be an int (second version) if the can can be initialized from an int
* @param value is the value to insert
*/
template<class KEY, class VALUE>
void insert(const KEY& key, const VALUE& value) {second_.insert(key, value);}
void insert(int key, const Value1& value) {first_.insert(Key1(key), value);} ///<TODO: comment
void insert(const Key1& key, const Value1& value) {first_.insert(key, value);} ///<TODO: comment
/**
* Insert a complete values at a time.
* Note: if the key is already in the values, the values will not be changed.
* Use update() to allow for changing existing values.
* @param values is a full values to add
*/
template<class CFG1, class CFG2>
void insert(const TupleValues<CFG1, CFG2>& values) { second_.insert(values); }
void insert(const TupleValues<VALUES1, VALUES2>& values) { ///<TODO: comment
first_.insert(values.first_);
second_.insert(values.second_);
}
/**
* Update function for whole valuess - this will change existing values
* @param values is a values to add
*/
template<class CFG1, class CFG2>
void update(const TupleValues<CFG1, CFG2>& values) { second_.update(values); }
void update(const TupleValues<VALUES1, VALUES2>& values) { ///<TODO: comment
first_.update(values.first_);
second_.update(values.second_);
}
/**
* Update function for single key/value pairs - will change existing values
* @param key is the variable identifier
* @param value is the variable value to update
*/
template<class KEY, class VALUE>
void update(const KEY& key, const VALUE& value) { second_.update(key, value); }
void update(const Key1& key, const Value1& value) { first_.update(key, value); } ///<TODO: comment
/**
* Insert a subvalues
* @param values is the values to insert
*/
template<class CFG>
void insertSub(const CFG& values) { second_.insertSub(values); }
void insertSub(const VALUES1& values) { first_.insert(values); } ///<TODO: comment
/** erase an element by key */
template<class KEY>
void erase(const KEY& j) { second_.erase(j); }
void erase(const Key1& j) { first_.erase(j); } ///<TODO: comment
/** clears the values */
void clear() { first_.clear(); second_.clear(); }
/// @}
/// @name Standard Interface
/// @{
/** determine whether an element exists */
template<class KEY>
bool exists(const KEY& j) const { return second_.exists(j); }
bool exists(const Key1& j) const { return first_.exists(j); } ///<TODO: comment
/** a variant of exists */
template<class KEY>
boost::optional<typename KEY::Value> exists_(const KEY& j) const { return second_.exists_(j); }
boost::optional<Value1> exists_(const Key1& j) const { return first_.exists_(j); } ///<TODO: comment
/** access operator */
template<class KEY>
const typename KEY::Value & operator[](const KEY& j) const { return second_[j]; }
const Value1& operator[](const Key1& j) const { return first_[j]; } ///<TODO: comment
/** at access function */
template<class KEY>
const typename KEY::Value & at(const KEY& j) const { return second_.at(j); }
const Value1& at(const Key1& j) const { return first_.at(j); } ///<TODO: comment
/** direct values access */
const VALUES1& values() const { return first_; }
const VALUES2& rest() const { return second_; } ///<TODO: comment
/** zero: create VectorValues of appropriate structure */
VectorValues zero(const Ordering& ordering) const {
return VectorValues::Zero(this->dims(ordering));
}
/** @return number of key/value pairs stored */
size_t size() const { return first_.size() + second_.size(); }
/** @return true if values is empty */
bool empty() const { return first_.empty() && second_.empty(); }
/**
* Generate a default ordering, simply in key sort order. To instead
* create a fill-reducing ordering, use
* NonlinearFactorGraph::orderingCOLAMD(). Alternatively, you may permute
* this ordering yourself (as orderingCOLAMD() does internally).
*/
Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const {
// Generate an initial key ordering in iterator order
_ValuesKeyOrderer keyOrderer(firstVar);
this->apply(keyOrderer);
return keyOrderer.ordering;
}
/**
* Apply a class with an application operator() to a const_iterator over
* every <key,value> pair. The operator must be able to handle such an
* iterator for every type in the Values, (i.e. through templating).
*/
template<typename A>
void apply(A& operation) {
first_.apply(operation);
second_.apply(operation);
}
/**
* TODO: comment
*/
template<typename A>
void apply(A& operation) const {
first_.apply(operation);
second_.apply(operation);
}
/// @}
/// @name Manifold
/// @{
/** @return The dimensionality of the tangent space */
size_t dim() const { return first_.dim() + second_.dim(); }
/** Create an array of variable dimensions using the given ordering */
std::vector<size_t> dims(const Ordering& ordering) const {
_ValuesDimensionCollector dimCollector(ordering);
this->apply(dimCollector);
return dimCollector.dimensions;
}
/** Expmap */
TupleValues<VALUES1, VALUES2> retract(const VectorValues& delta, const Ordering& ordering) const {
return TupleValues(first_.retract(delta, ordering), second_.retract(delta, ordering));
}
/** logmap each element */
VectorValues localCoordinates(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering));
localCoordinates(cp, ordering, delta);
return delta;
}
/** logmap each element */
void localCoordinates(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering, VectorValues& delta) const {
first_.localCoordinates(cp.first_, ordering, delta);
second_.localCoordinates(cp.second_, ordering, delta);
}
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(first_);
ar & BOOST_SERIALIZATION_NVP(second_);
}
};
/**
* End of a recursive TupleValues - contains only one values
*
* Do not use this class directly - it should only be used as a part
* of a recursive structure
*/
template<class VALUES>
class TupleValuesEnd {
protected:
// Data for internal valuess
VALUES first_;
public:
// typedefs
typedef typename VALUES::Key Key1;
typedef typename VALUES::Value Value1;
TupleValuesEnd() {}
TupleValuesEnd(const TupleValuesEnd<VALUES>& values) :
first_(values.first_) {}
TupleValuesEnd(const VALUES& cfg) :
first_(cfg) {}
void print(const std::string& s = "") const {
first_.print();
}
bool equals(const TupleValuesEnd<VALUES>& c, double tol=1e-9) const {
return first_.equals(c.first_, tol);
}
void insert(const Key1& key, const Value1& value) {first_.insert(key, value); }
void insert(int key, const Value1& value) {first_.insert(Key1(key), value);}
void insert(const TupleValuesEnd<VALUES>& values) {first_.insert(values.first_); }
void update(const TupleValuesEnd<VALUES>& values) {first_.update(values.first_); }
void update(const Key1& key, const Value1& value) { first_.update(key, value); }
void insertSub(const VALUES& values) {first_.insert(values); }
const Value1& operator[](const Key1& j) const { return first_[j]; }
const VALUES& values() const { return first_; }
void erase(const Key1& j) { first_.erase(j); }
void clear() { first_.clear(); }
bool empty() const { return first_.empty(); }
bool exists(const Key1& j) const { return first_.exists(j); }
boost::optional<Value1> exists_(const Key1& j) const { return first_.exists_(j); }
const Value1& at(const Key1& j) const { return first_.at(j); }
VectorValues zero(const Ordering& ordering) const {
return VectorValues::Zero(this->dims(ordering));
}
size_t size() const { return first_.size(); }
size_t dim() const { return first_.dim(); }
TupleValuesEnd<VALUES> retract(const VectorValues& delta, const Ordering& ordering) const {
return TupleValuesEnd(first_.retract(delta, ordering));
}
VectorValues localCoordinates(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering));
localCoordinates(cp, ordering, delta);
return delta;
}
void localCoordinates(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering, VectorValues& delta) const {
first_.localCoordinates(cp.first_, ordering, delta);
}
/**
* Apply a class with an application operator() to a const_iterator over
* every <key,value> pair. The operator must be able to handle such an
* iterator for every type in the Values, (i.e. through templating).
*/
template<typename A>
void apply(A& operation) {
first_.apply(operation);
}
template<typename A>
void apply(A& operation) const {
first_.apply(operation);
}
/** Create an array of variable dimensions using the given ordering */
std::vector<size_t> dims(const Ordering& ordering) const {
_ValuesDimensionCollector dimCollector(ordering);
this->apply(dimCollector);
return dimCollector.dimensions;
}
/**
* Generate a default ordering, simply in key sort order. To instead
* create a fill-reducing ordering, use
* NonlinearFactorGraph::orderingCOLAMD(). Alternatively, you may permute
* this ordering yourself (as orderingCOLAMD() does internally).
*/
Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const {
// Generate an initial key ordering in iterator order
_ValuesKeyOrderer keyOrderer(firstVar);
this->apply(keyOrderer);
return keyOrderer.ordering;
}
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(first_);
}
};
/**
* Wrapper classes to act as containers for valuess. Note that these can be cascaded
* recursively, as they are TupleValues, and are primarily a short form of the values
* structure to make use of the TupleValues easier.
*
* The interface is designed to mimic PairValues, but for 2-6 values types.
*/
template<class C1>
class TupleValues1 : public TupleValuesEnd<C1> {
public:
// typedefs
typedef C1 Values1;
typedef TupleValuesEnd<C1> Base;
typedef TupleValues1<C1> This;
TupleValues1() {}
TupleValues1(const This& values);
TupleValues1(const Base& values);
TupleValues1(const Values1& cfg1);
// access functions
inline const Values1& first() const { return this->values(); }
};
template<class C1, class C2>
class TupleValues2 : public TupleValues<C1, TupleValuesEnd<C2> > {
public:
// typedefs
typedef C1 Values1;
typedef C2 Values2;
typedef TupleValues<C1, TupleValuesEnd<C2> > Base;
typedef TupleValues2<C1, C2> This;
TupleValues2() {}
TupleValues2(const This& values);
TupleValues2(const Base& values);
TupleValues2(const Values1& cfg1, const Values2& cfg2);
// access functions
inline const Values1& first() const { return this->values(); }
inline const Values2& second() const { return this->rest().values(); }
};
template<class C1, class C2, class C3>
class TupleValues3 : public TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > > {
public:
// typedefs
typedef C1 Values1;
typedef C2 Values2;
typedef C3 Values3;
typedef TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > > Base;
typedef TupleValues3<C1, C2, C3> This;
TupleValues3() {}
TupleValues3(const Base& values);
TupleValues3(const This& values);
TupleValues3(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3);
// access functions
inline const Values1& first() const { return this->values(); }
inline const Values2& second() const { return this->rest().values(); }
inline const Values3& third() const { return this->rest().rest().values(); }
};
template<class C1, class C2, class C3, class C4>
class TupleValues4 : public TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > {
public:
// typedefs
typedef C1 Values1;
typedef C2 Values2;
typedef C3 Values3;
typedef C4 Values4;
typedef TupleValues<C1, TupleValues<C2,TupleValues<C3, TupleValuesEnd<C4> > > > Base;
typedef TupleValues4<C1, C2, C3, C4> This;
TupleValues4() {}
TupleValues4(const This& values);
TupleValues4(const Base& values);
TupleValues4(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,const Values4& cfg4);
// access functions
inline const Values1& first() const { return this->values(); }
inline const Values2& second() const { return this->rest().values(); }
inline const Values3& third() const { return this->rest().rest().values(); }
inline const Values4& fourth() const { return this->rest().rest().rest().values(); }
};
template<class C1, class C2, class C3, class C4, class C5>
class TupleValues5 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > > {
public:
typedef C1 Values1;
typedef C2 Values2;
typedef C3 Values3;
typedef C4 Values4;
typedef C5 Values5;
typedef TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > > Base;
typedef TupleValues5<C1, C2, C3, C4, C5> This;
TupleValues5() {}
TupleValues5(const This& values);
TupleValues5(const Base& values);
TupleValues5(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,
const Values4& cfg4, const Values5& cfg5);
// access functions
inline const Values1& first() const { return this->values(); }
inline const Values2& second() const { return this->rest().values(); }
inline const Values3& third() const { return this->rest().rest().values(); }
inline const Values4& fourth() const { return this->rest().rest().rest().values(); }
inline const Values5& fifth() const { return this->rest().rest().rest().rest().values(); }
};
template<class C1, class C2, class C3, class C4, class C5, class C6>
class TupleValues6 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > > {
public:
typedef C1 Values1;
typedef C2 Values2;
typedef C3 Values3;
typedef C4 Values4;
typedef C5 Values5;
typedef C6 Values6;
typedef TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > > Base;
typedef TupleValues6<C1, C2, C3, C4, C5, C6> This;
TupleValues6() {}
TupleValues6(const This& values);
TupleValues6(const Base& values);
TupleValues6(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,
const Values4& cfg4, const Values5& cfg5, const Values6& cfg6);
// access functions
inline const Values1& first() const { return this->values(); }
inline const Values2& second() const { return this->rest().values(); }
inline const Values3& third() const { return this->rest().rest().values(); }
inline const Values4& fourth() const { return this->rest().rest().rest().values(); }
inline const Values5& fifth() const { return this->rest().rest().rest().rest().values(); }
inline const Values6& sixth() const { return this->rest().rest().rest().rest().rest().values(); }
};
}
#include <gtsam/nonlinear/TupleValues-inl.h>

253
gtsam/nonlinear/Values-inl.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -10,222 +10,87 @@
* -------------------------------------------------------------------------- */
/**
* @file Values.cpp
* @file Values.h
* @author Richard Roberts
*
* @brief A non-templated config holding any types of Manifold-group elements
*
* Detailed story:
* A values structure is a map from keys to values. It is used to specify the value of a bunch
* of variables in a factor graph. A Values is a values structure which can hold variables that
* are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
* which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
*/
#pragma once
#include <utility>
#include <iostream>
#include <stdexcept>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/Lie-inl.h>
#include <gtsam/nonlinear/Ordering.h>
using namespace std;
#include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition
namespace gtsam {
/* ************************************************************************* */
template<class J>
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));
}
}
/* ************************************************************************* */
class ValueCloneAllocator {
public:
static Value* allocate_clone(const Value& a) { return a.clone_(); }
static void deallocate_clone(const Value* a) { a->deallocate_(); }
private:
ValueCloneAllocator() {}
};
/* ************************************************************************* */
template<class J>
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;
if(!gtsam::equal(v.second, expected[v.first], tol))
return false;
}
return true;
template<typename ValueType>
const ValueType& Values::at(const Symbol& j) const {
// Find the item
KeyValueMap::const_iterator item = values_.find(j);
// Throw exception if it does not exist
if(item == values_.end())
throw ValuesKeyDoesNotExist("retrieve", j);
// Check the type and throw exception if incorrect
if(typeid(*item->second) != typeid(ValueType))
throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
// We have already checked the type, so do a "blind" static_cast, not dynamic_cast
return static_cast<const ValueType&>(*item->second);
}
/* ************************************************************************* */
template<class J>
const typename J::Value& Values<J>::at(const J& j) const {
const_iterator it = values_.find(j);
if (it == values_.end()) throw KeyDoesNotExist<J>("retrieve", j);
else return it->second;
template<typename TypedKey>
const typename TypedKey::Value& Values::at(const TypedKey& j) const {
// Convert to Symbol
const Symbol symbol(j.symbol());
// Call at with the Value type from the key
return at<typename TypedKey::Value>(symbol);
}
/* ************************************************************************* */
template<class J>
size_t Values<J>::dim() const {
size_t n = 0;
BOOST_FOREACH(const KeyValuePair& value, values_)
n += value.second.dim();
return n;
}
template<typename ValueType>
boost::optional<const ValueType&> Values::exists(const Symbol& j) const {
// Find the item
KeyValueMap::const_iterator item = values_.find(j);
/* ************************************************************************* */
template<class J>
VectorValues Values<J>::zero(const Ordering& ordering) const {
return VectorValues::Zero(this->dims(ordering));
}
if(item != values_.end()) {
// Check the type and throw exception if incorrect
if(typeid(*item->second) != typeid(ValueType))
throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
/* ************************************************************************* */
template<class J>
void Values<J>::insert(const J& name, const typename J::Value& val) {
if(!values_.insert(make_pair(name, val)).second)
throw KeyAlreadyExists<J>(name, val);
}
/* ************************************************************************* */
template<class J>
void Values<J>::insert(const Values<J>& cfg) {
BOOST_FOREACH(const KeyValuePair& v, cfg.values_)
insert(v.first, v.second);
}
/* ************************************************************************* */
template<class J>
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;
}
}
/* ************************************************************************* */
template<class J>
void Values<J>::update(const J& j, const typename J::Value& val) {
values_[j] = val;
}
/* ************************************************************************* */
template<class J>
std::list<J> Values<J>::keys() const {
std::list<J> ret;
BOOST_FOREACH(const KeyValuePair& v, values_)
ret.push_back(v.first);
return ret;
}
/* ************************************************************************* */
template<class J>
void Values<J>::erase(const J& j) {
size_t dim; // unused
erase(j, dim);
}
/* ************************************************************************* */
template<class J>
void Values<J>::erase(const J& j, size_t& dim) {
iterator it = values_.find(j);
if (it == values_.end())
throw KeyDoesNotExist<J>("erase", j);
dim = it->second.dim();
values_.erase(it);
}
/* ************************************************************************* */
// todo: insert for every element is inefficient
template<class J>
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;
const typename J::Value& pj = value.second;
Index index;
if(ordering.tryAt(j,index)) {
newValues.insert(j, pj.retract(delta[index]));
} else
newValues.insert(j, pj);
}
return newValues;
}
/* ************************************************************************* */
template<class J>
std::vector<size_t> Values<J>::dims(const Ordering& ordering) const {
_ValuesDimensionCollector dimCollector(ordering);
this->apply(dimCollector);
return dimCollector.dimensions;
}
/* ************************************************************************* */
template<class J>
Ordering::shared_ptr Values<J>::orderingArbitrary(Index firstVar) const {
// Generate an initial key ordering in iterator order
_ValuesKeyOrderer keyOrderer(firstVar);
this->apply(keyOrderer);
return keyOrderer.ordering;
}
// /* ************************************************************************* */
// // todo: insert for every element is inefficient
// template<class J>
// Values<J> Values<J>::retract(const Vector& delta) const {
// if(delta.size() != dim()) {
// cout << "Values::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl;
// throw invalid_argument("Delta vector length does not match config dimensionality.");
// }
// Values<J> newValues;
// int delta_offset = 0;
// typedef pair<J,typename J::Value> KeyValue;
// BOOST_FOREACH(const KeyValue& value, this->values_) {
// const J& j = value.first;
// const typename J::Value& pj = value.second;
// int cur_dim = pj.dim();
// newValues.insert(j,pj.retract(sub(delta, delta_offset, delta_offset+cur_dim)));
// delta_offset += cur_dim;
// }
// return newValues;
// }
/* ************************************************************************* */
// todo: insert for every element is inefficient
// todo: currently only logmaps elements in both configs
template<class J>
inline VectorValues Values<J>::localCoordinates(const Values<J>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering));
localCoordinates(cp, ordering, delta);
return delta;
}
/* ************************************************************************* */
template<class J>
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));
delta[ordering[value.first]] = this->at(value.first).localCoordinates(value.second);
// We have already checked the type, so do a "blind" static_cast, not dynamic_cast
return static_cast<const ValueType&>(*item->second);
} else {
return boost::none;
}
}
/* ************************************************************************* */
template<class J>
const char* KeyDoesNotExist<J>::what() const throw() {
if(message_.empty())
message_ =
"Attempting to " + std::string(operation_) + " the key \"" +
(std::string)key_ + "\", which does not exist in the Values.";
return message_.c_str();
}
template<class TypedKey>
boost::optional<const typename TypedKey::Value&> Values::exists(const TypedKey& j) const {
// Convert to Symbol
const Symbol symbol(j.symbol());
/* ************************************************************************* */
template<class J>
const char* KeyAlreadyExists<J>::what() const throw() {
if(message_.empty())
message_ =
"Attempting to add a key-value pair with key \"" + (std::string)key_ + "\", key already exists.";
return message_.c_str();
// Call exists with the Value type from the key
return exists<typename TypedKey::Value>(symbol);
}
}

213
gtsam/nonlinear/Values.cpp Normal file
View File

@ -0,0 +1,213 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Values.h
* @author Richard Roberts
*
* @brief A non-templated config holding any types of Manifold-group elements
*
* Detailed story:
* A values structure is a map from keys to values. It is used to specify the value of a bunch
* of variables in a factor graph. A Values is a values structure which can hold variables that
* are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
* which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
*/
#include <gtsam/nonlinear/Values.h>
#include <list>
#include <boost/foreach.hpp>
#include <boost/bind.hpp>
#include <boost/iterator/transform_iterator.hpp>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
Values::Values(const Values& other) {
this->insert(other);
}
/* ************************************************************************* */
void Values::print(const string& str) const {
cout << str << "Values with " << size() << " values:\n" << endl;
for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
cout << " " << (string)key_value->first << ": ";
key_value->second.print("");
}
}
/* ************************************************************************* */
bool Values::equals(const Values& other, double tol) const {
if(this->size() != other.size())
return false;
for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) {
if(typeid(it1->second) != typeid(it2->second))
return false;
if(it1->first != it2->first)
return false;
if(!it1->second.equals_(it2->second, tol))
return false;
}
return true; // We return false earlier if we find anything that does not match
}
/* ************************************************************************* */
bool Values::exists(const Symbol& j) const {
return values_.find(j) != values_.end();
}
/* ************************************************************************* */
VectorValues Values::zeroVectors(const Ordering& ordering) const {
return VectorValues::Zero(this->dims(ordering));
}
/* ************************************************************************* */
Values Values::retract(const VectorValues& delta, const Ordering& ordering) const {
Values result;
for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
const SubVector& singleDelta = delta[ordering[key_value->first]]; // Delta for this value
Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
Value* retractedValue(key_value->second.retract_(singleDelta)); // Retract
result.values_.insert(key, retractedValue); // Add retracted result directly to result values
}
return result;
}
/* ************************************************************************* */
VectorValues Values::localCoordinates(const Values& cp, const Ordering& ordering) const {
VectorValues result(this->dims(ordering));
localCoordinates(cp, ordering, result);
return result;
}
/* ************************************************************************* */
void Values::localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& result) const {
if(this->size() != cp.size())
throw DynamicValuesMismatched();
for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) {
if(it1->first != it2->first)
throw DynamicValuesMismatched(); // If keys do not match
// Will throw a dynamic_cast exception if types do not match
result.insert(ordering[it1->first], it1->second.localCoordinates_(it2->second));
}
}
/* ************************************************************************* */
void Values::insert(const Symbol& j, const Value& val) {
Symbol key = j; // Non-const duplicate to deal with non-const insert argument
std::pair<iterator,bool> insertResult = values_.insert(key, val.clone_());
if(!insertResult.second)
throw ValuesKeyAlreadyExists(j);
}
/* ************************************************************************* */
void Values::insert(const Values& values) {
for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
insert(key, key_value->second);
}
}
/* ************************************************************************* */
void Values::update(const Symbol& j, const Value& val) {
// Find the value to update
KeyValueMap::iterator item = values_.find(j);
if(item == values_.end())
throw ValuesKeyDoesNotExist("update", j);
// Cast to the derived type
if(typeid(*item->second) != typeid(val))
throw ValuesIncorrectType(j, typeid(*item->second), typeid(val));
values_.replace(item, val.clone_());
}
/* ************************************************************************* */
void Values::update(const Values& values) {
for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
this->update(key_value->first, key_value->second);
}
}
/* ************************************************************************* */
void Values::erase(const Symbol& j) {
KeyValueMap::iterator item = values_.find(j);
if(item == values_.end())
throw ValuesKeyDoesNotExist("erase", j);
values_.erase(item);
}
/* ************************************************************************* */
FastList<Symbol> Values::keys() const {
FastList<Symbol> result;
for(const_iterator key_value = begin(); key_value != end(); ++key_value)
result.push_back(key_value->first);
return result;
}
/* ************************************************************************* */
Values& Values::operator=(const Values& rhs) {
this->clear();
this->insert(rhs);
return *this;
}
/* ************************************************************************* */
vector<size_t> Values::dims(const Ordering& ordering) const {
vector<size_t> result(values_.size());
BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) {
result[ordering[key_value.first]] = key_value.second.dim();
}
return result;
}
/* ************************************************************************* */
Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const {
Ordering::shared_ptr ordering(new Ordering);
for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
ordering->insert(key_value->first, firstVar++);
}
return ordering;
}
/* ************************************************************************* */
const char* ValuesKeyAlreadyExists::what() const throw() {
if(message_.empty())
message_ =
"Attempting to add a key-value pair with key \"" + (std::string)key_ + "\", key already exists.";
return message_.c_str();
}
/* ************************************************************************* */
const char* ValuesKeyDoesNotExist::what() const throw() {
if(message_.empty())
message_ =
"Attempting to " + std::string(operation_) + " the key \"" +
(std::string)key_ + "\", which does not exist in the Values.";
return message_.c_str();
}
/* ************************************************************************* */
const char* ValuesIncorrectType::what() const throw() {
if(message_.empty())
message_ =
"Attempting to retrieve value with key \"" + (std::string)key_ + "\", type stored in Values is " +
std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name());
return message_.c_str();
}
}

489
gtsam/nonlinear/Values.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -13,115 +13,159 @@
* @file Values.h
* @author Richard Roberts
*
* @brief A templated config for Manifold-group elements
* @brief A non-templated config holding any types of Manifold-group elements
*
* Detailed story:
* A values structure is a map from keys to values. It is used to specify the value of a bunch
* of variables in a factor graph. A Values is a values structure which can hold variables that
* are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
* which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
* \nosubgrouping
*/
#pragma once
#include <set>
#include <string>
#include <utility>
#include <gtsam/base/Testable.h>
#include <boost/pool/pool_alloc.hpp>
#include <boost/ptr_container/ptr_map.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <gtsam/base/Value.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Ordering.h>
namespace boost { template<class T> class optional; }
namespace gtsam { class VectorValues; class Ordering; }
namespace gtsam {
// Forward declarations
template<class J> class KeyDoesNotExist;
template<class J> class KeyAlreadyExists;
class ValueCloneAllocator;
/**
* Manifold type values structure
* Takes two template types
* J: a key type to look up values in the values structure (need to be sortable)
*
* Key concept:
* The key will be assumed to be sortable, and must have a
* typedef called "Value" with the type of the value the key
* labels (example: Pose2, Point2, etc)
*/
template<class J>
/**
* A non-templated config holding any types of Manifold-group elements. A
* values structure is a map from keys to values. It is used to specify the
* value of a bunch of variables in a factor graph. A Values is a values
* structure which can hold variables that are elements on manifolds, not just
* vectors. It then, as a whole, implements a aggregate type which is also a
* manifold element, and hence supports operations dim, retract, and
* localCoordinates.
*/
class Values {
public:
/**
* Typedefs
*/
typedef J Key;
typedef typename J::Value Value;
typedef std::map<Key, Value, std::less<Key>, boost::fast_pool_allocator<std::pair<const Key, Value> > > KeyValueMap;
// typedef FastMap<J,Value> KeyValueMap;
typedef typename KeyValueMap::value_type KeyValuePair;
typedef typename KeyValueMap::iterator iterator;
typedef typename KeyValueMap::const_iterator const_iterator;
private:
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(Value)
GTSAM_CONCEPT_MANIFOLD_TYPE(Value)
// Internally we store a boost ptr_map, with a ValueCloneAllocator (defined
// below) to clone and deallocate the Value objects, and a boost
// fast_pool_allocator to allocate map nodes. In this way, all memory is
// allocated in a boost memory pool.
typedef boost::ptr_map<
Symbol,
Value,
std::less<Symbol>,
ValueCloneAllocator,
boost::fast_pool_allocator<std::pair<const Symbol, void*> > > KeyValueMap;
// The member to store the values, see just above
KeyValueMap values_;
// Types obtained by iterating
typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair;
typedef KeyValueMap::iterator::value_type KeyValuePtrPair;
public:
/// @name Standard Constructors
/// @{
/// A shared_ptr to this class
typedef boost::shared_ptr<Values> shared_ptr;
///TODO comment
/// A pair of const references to the key and value, the dereferenced type of the const_iterator and const_reverse_iterator
typedef std::pair<const Symbol&, const Value&> ConstKeyValuePair;
/// A pair of references to the key and value, the dereferenced type of the iterator and reverse_iterator
typedef std::pair<const Symbol&, Value&> KeyValuePair;
/// Mutable forward iterator, with value type KeyValuePair
typedef boost::transform_iterator<
boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::iterator> iterator;
/// Const forward iterator, with value type ConstKeyValuePair
typedef boost::transform_iterator<
boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_iterator> const_iterator;
/// Mutable reverse iterator, with value type KeyValuePair
typedef boost::transform_iterator<
boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::reverse_iterator> reverse_iterator;
/// Const reverse iterator, with value type ConstKeyValuePair
typedef boost::transform_iterator<
boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
/** Default constructor creates an empty Values class */
Values() {}
///TODO: comment
Values(const Values& config) :
values_(config.values_) {}
/** Copy constructor duplicates all keys and values */
Values(const Values& other);
///TODO: comment
template<class J_ALT>
Values(const Values<J_ALT>& other) {} // do nothing when initializing with wrong type
virtual ~Values() {}
/// @}
/// @name Testable
/// @{
/** print */
void print(const std::string &s="") const;
/** print method for testing and debugging */
void print(const std::string& str = "") const;
/** Test whether configs are identical in keys and values */
bool equals(const Values& expected, double tol=1e-9) const;
/** Test whether the sets of keys and values are identical */
bool equals(const Values& other, double tol=1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/** Retrieve a variable by j, throws KeyDoesNotExist<J> if not found */
const Value& at(const J& j) const;
/** Retrieve a variable by key \c j. The type of the value associated with
* this key is supplied as a template argument to this function.
* @param j Retrieve the value associated with this key
* @tparam Value The type of the value stored with this key, this method
* throws DynamicValuesIncorrectType if this requested type is not correct.
* @return A const reference to the stored value
*/
template<typename ValueType>
const ValueType& at(const Symbol& j) const;
/** operator[] syntax for get, throws KeyDoesNotExist<J> if not found */
const Value& operator[](const J& j) const {
/** Retrieve a variable using a special key (typically TypedSymbol), which
* contains the type of the value associated with the key, and which must
* be conversion constructible to a Symbol, e.g.
* <tt>Symbol(const TypedKey&)</tt>. Throws DynamicValuesKeyDoesNotExist
* the key is not found, and DynamicValuesIncorrectType if the value type
* associated with the requested key does not match the stored value type.
*/
template<class TypedKey>
const typename TypedKey::Value& at(const TypedKey& j) const;
/** operator[] syntax for at(const TypedKey& j) */
template<class TypedKey>
const typename TypedKey::Value& operator[](const TypedKey& j) const {
return at(j); }
/** Check if a variable exists */
bool exists(const J& i) const { return values_.find(i)!=values_.end(); }
/** Check if a value exists with key \c j. See exists<>(const Symbol& j)
* and exists(const TypedKey& j) for versions that return the value if it
* exists. */
bool exists(const Symbol& j) const;
/** Check if a variable exists and return it if so */
boost::optional<Value> exists_(const J& i) const {
const_iterator it = values_.find(i);
if (it==values_.end()) return boost::none; else return it->second;
}
/** Check if a value with key \c j exists, returns the value with type
* \c Value if the key does exist, or boost::none if it does not exist.
* Throws DynamicValuesIncorrectType if the value type associated with the
* requested key does not match the stored value type. */
template<typename ValueType>
boost::optional<const ValueType&> exists(const Symbol& j) const;
/** Check if a value with key \c j exists, returns the value with type
* \c Value if the key does exist, or boost::none if it does not exist.
* Uses a special key (typically TypedSymbol), which contains the type of
* the value associated with the key, and which must be conversion
* constructible to a Symbol, e.g. <tt>Symbol(const TypedKey&)</tt>. Throws
* DynamicValuesIncorrectType if the value type associated with the
* requested key does not match the stored value type.
*/
template<class TypedKey>
boost::optional<const typename TypedKey::Value&> exists(const TypedKey& j) const;
/** The number of variables in this config */
size_t size() const { return values_.size(); }
@ -130,10 +174,64 @@ namespace gtsam {
bool empty() const { return values_.empty(); }
/** Get a zero VectorValues of the correct structure */
VectorValues zero(const Ordering& ordering) const;
VectorValues zeroVectors(const Ordering& ordering) const;
const_iterator begin() const { return values_.begin(); } ///<TODO: comment
const_iterator end() const { return values_.end(); } ///<TODO: comment
private:
static std::pair<const Symbol&, const Value&> make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) {
return std::make_pair<const Symbol&, const Value&>(key_value.first, *key_value.second); }
static std::pair<const Symbol&, Value&> make_deref_pair(const KeyValueMap::iterator::value_type& key_value) {
return std::make_pair<const Symbol&, Value&>(key_value.first, *key_value.second); }
public:
const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); }
const_iterator end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); }
iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); }
iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); }
const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); }
const_reverse_iterator rend() const { return boost::make_transform_iterator(values_.rend(), &make_const_deref_pair); }
reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); }
reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); }
/// @name Manifold Operations
/// @{
/** Add a delta config to current config and returns a new config */
Values retract(const VectorValues& delta, const Ordering& ordering) const;
/** Get a delta config about a linearization point c0 (*this) */
VectorValues localCoordinates(const Values& cp, const Ordering& ordering) const;
/** Get a delta config about a linearization point c0 (*this) */
void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const;
///@}
/** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
void insert(const Symbol& j, const Value& val);
/** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
void insert(const Values& values);
/** single element change of existing element */
void update(const Symbol& j, const Value& val);
/** update the current available values without adding new ones */
void update(const Values& values);
/** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
void erase(const Symbol& j);
/**
* Returns a set of keys in the config
* Note: by construction, the list is ordered
*/
FastList<Symbol> keys() const;
/** Replace all keys and variables */
Values& operator=(const Values& rhs);
/** Remove all variables from the config */
void clear() { values_.clear(); }
/** Create an array of variable dimensions using the given ordering */
std::vector<size_t> dims(const Ordering& ordering) const;
@ -146,174 +244,97 @@ namespace gtsam {
*/
Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const;
/// @}
/// @name Manifold Operations
/// @{
/** The dimensionality of the tangent space */
size_t dim() const;
/** Add a delta config to current config and returns a new config */
Values retract(const VectorValues& delta, const Ordering& ordering) const;
/** Get a delta config about a linearization point c0 (*this) */
VectorValues localCoordinates(const Values& cp, const Ordering& ordering) const;
/** Get a delta config about a linearization point c0 (*this) */
void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const;
/// @}
/// @name Advanced Interface
/// @{
// imperative methods:
iterator begin() { return values_.begin(); } ///<TODO: comment
iterator end() { return values_.end(); } ///<TODO: comment
/** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
void insert(const J& j, const Value& val);
/** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
void insert(const Values& cfg);
/** update the current available values without adding new ones */
void update(const Values& cfg);
/** single element change of existing element */
void update(const J& j, const Value& val);
/** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
void erase(const J& j);
/** Remove a variable from the config while returning the dimensionality of
* the removed element (normally not needed by user code), throws
* KeyDoesNotExist<J> if j is not present.
*/
void erase(const J& j, size_t& dim);
/**
* Returns a set of keys in the config
* Note: by construction, the list is ordered
*/
std::list<J> keys() const;
/** Replace all keys and variables */
Values& operator=(const Values& rhs) {
values_ = rhs.values_;
return (*this);
}
/** Remove all variables from the config */
void clear() {
values_.clear();
}
/**
* Apply a class with an application operator() to a const_iterator over
* every <key,value> pair. The operator must be able to handle such an
* iterator for every type in the Values, (i.e. through templating).
*/
template<typename A>
void apply(A& operation) {
for(iterator it = begin(); it != end(); ++it)
operation(it);
}
template<typename A>
void apply(A& operation) const {
for(const_iterator it = begin(); it != end(); ++it)
operation(it);
}
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(values_);
}
};
struct _ValuesDimensionCollector {
const Ordering& ordering;
std::vector<size_t> dimensions;
_ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {}
template<typename I> void operator()(const I& key_value) {
Index var;
if(ordering.tryAt(key_value->first, var)) {
assert(var < dimensions.size());
dimensions[var] = key_value->second.dim();
}
}
};
/* ************************************************************************* */
struct _ValuesKeyOrderer {
Index var;
Ordering::shared_ptr ordering;
_ValuesKeyOrderer(Index firstVar) : var(firstVar), ordering(new Ordering) {}
template<typename I> void operator()(const I& key_value) {
ordering->insert(key_value->first, var);
++ var;
class ValuesKeyAlreadyExists : public std::exception {
protected:
const Symbol key_; ///< The key that already existed
private:
mutable std::string message_;
public:
/// Construct with the key-value pair attemped to be added
ValuesKeyAlreadyExists(const Symbol& key) throw() :
key_(key) {}
virtual ~ValuesKeyAlreadyExists() throw() {}
/// The duplicate key that was attemped to be added
const Symbol& key() const throw() { return key_; }
/// The message to be displayed to the user
virtual const char* what() const throw();
};
/* ************************************************************************* */
class ValuesKeyDoesNotExist : public std::exception {
protected:
const char* operation_; ///< The operation that attempted to access the key
const Symbol key_; ///< The key that does not exist
private:
mutable std::string message_;
public:
/// Construct with the key that does not exist in the values
ValuesKeyDoesNotExist(const char* operation, const Symbol& key) throw() :
operation_(operation), key_(key) {}
virtual ~ValuesKeyDoesNotExist() throw() {}
/// The key that was attempted to be accessed that does not exist
const Symbol& key() const throw() { return key_; }
/// The message to be displayed to the user
virtual const char* what() const throw();
};
/* ************************************************************************* */
class ValuesIncorrectType : public std::exception {
protected:
const Symbol key_; ///< The key requested
const std::type_info& storedTypeId_;
const std::type_info& requestedTypeId_;
private:
mutable std::string message_;
public:
/// Construct with the key that does not exist in the values
ValuesIncorrectType(const Symbol& key,
const std::type_info& storedTypeId, const std::type_info& requestedTypeId) throw() :
key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
virtual ~ValuesIncorrectType() throw() {}
/// The key that was attempted to be accessed that does not exist
const Symbol& key() const throw() { return key_; }
/// The typeid of the value stores in the Values
const std::type_info& storedTypeId() const { return storedTypeId_; }
/// The requested typeid
const std::type_info& requestedTypeId() const { return requestedTypeId_; }
/// The message to be displayed to the user
virtual const char* what() const throw();
};
/* ************************************************************************* */
class DynamicValuesMismatched : public std::exception {
public:
DynamicValuesMismatched() throw() {}
virtual ~DynamicValuesMismatched() throw() {}
virtual const char* what() const throw() {
return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values";
}
};
/* ************************************************************************* */
template<class J>
class KeyAlreadyExists : public std::exception {
protected:
const J key_; ///< The key that already existed
const typename J::Value value_; ///< The value attempted to be inserted
private:
mutable std::string message_;
public:
/// Construct with the key-value pair attemped to be added
KeyAlreadyExists(const J& key, const typename J::Value& value) throw() :
key_(key), value_(value) {}
virtual ~KeyAlreadyExists() throw() {}
/// The duplicate key that was attemped to be added
const J& key() const throw() { return key_; }
/// The value that was attempted to be added
const typename J::Value& value() const throw() { return value_; }
/// The message to be displayed to the user
virtual const char* what() const throw();
};
/* ************************************************************************* */
template<class J>
class KeyDoesNotExist : public std::exception {
protected:
const char* operation_; ///< The operation that attempted to access the key
const J key_; ///< The key that does not exist
private:
mutable std::string message_;
public:
/// Construct with the key that does not exist in the values
KeyDoesNotExist(const char* operation, const J& key) throw() :
operation_(operation), key_(key) {}
virtual ~KeyDoesNotExist() throw() {}
/// The key that was attempted to be accessed that does not exist
const J& key() const throw() { return key_; }
/// The message to be displayed to the user
virtual const char* what() const throw();
};
} // \namespace gtsam
}
#include <gtsam/nonlinear/Values-inl.h>

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

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file testValues.cpp
* @file testDynamicValues.cpp
* @author Richard Roberts
*/
@ -22,6 +22,7 @@ using namespace boost::assign;
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/Values.h>
using namespace gtsam;
@ -29,27 +30,26 @@ using namespace std;
static double inf = std::numeric_limits<double>::infinity();
typedef TypedSymbol<LieVector, 'v'> VecKey;
typedef Values<VecKey> TestValues;
VecKey key1(1), key2(2), key3(3), key4(4);
/* ************************************************************************* */
TEST( TestValues, equals1 )
TEST( Values, equals1 )
{
TestValues expected;
Vector v = Vector_(3, 5.0, 6.0, 7.0);
Values expected;
LieVector v(3, 5.0, 6.0, 7.0);
expected.insert(key1,v);
TestValues actual;
Values actual;
actual.insert(key1,v);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( TestValues, equals2 )
TEST( Values, equals2 )
{
TestValues cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 5.0, 6.0, 8.0);
Values cfg1, cfg2;
LieVector v1(3, 5.0, 6.0, 7.0);
LieVector v2(3, 5.0, 6.0, 8.0);
cfg1.insert(key1, v1);
cfg2.insert(key1, v2);
CHECK(!cfg1.equals(cfg2));
@ -57,11 +57,11 @@ TEST( TestValues, equals2 )
}
/* ************************************************************************* */
TEST( TestValues, equals_nan )
TEST( Values, equals_nan )
{
TestValues cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, inf, inf, inf);
Values cfg1, cfg2;
LieVector v1(3, 5.0, 6.0, 7.0);
LieVector v2(3, inf, inf, inf);
cfg1.insert(key1, v1);
cfg2.insert(key1, v2);
CHECK(!cfg1.equals(cfg2));
@ -69,13 +69,13 @@ TEST( TestValues, equals_nan )
}
/* ************************************************************************* */
TEST( TestValues, insert_good )
TEST( Values, insert_good )
{
TestValues cfg1, cfg2, expected;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
Vector v4 = Vector_(3, 8.0, 3.0, 7.0);
Values cfg1, cfg2, expected;
LieVector v1(3, 5.0, 6.0, 7.0);
LieVector v2(3, 8.0, 9.0, 1.0);
LieVector v3(3, 2.0, 4.0, 3.0);
LieVector v4(3, 8.0, 3.0, 7.0);
cfg1.insert(key1, v1);
cfg1.insert(key2, v2);
cfg2.insert(key3, v4);
@ -86,31 +86,31 @@ TEST( TestValues, insert_good )
expected.insert(key2, v2);
expected.insert(key3, v4);
CHECK(assert_equal(cfg1, expected));
CHECK(assert_equal(expected, cfg1));
}
/* ************************************************************************* */
TEST( TestValues, insert_bad )
TEST( Values, insert_bad )
{
TestValues cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
Vector v4 = Vector_(3, 8.0, 3.0, 7.0);
Values cfg1, cfg2;
LieVector v1(3, 5.0, 6.0, 7.0);
LieVector v2(3, 8.0, 9.0, 1.0);
LieVector v3(3, 2.0, 4.0, 3.0);
LieVector v4(3, 8.0, 3.0, 7.0);
cfg1.insert(key1, v1);
cfg1.insert(key2, v2);
cfg2.insert(key2, v3);
cfg2.insert(key3, v4);
CHECK_EXCEPTION(cfg1.insert(cfg2), const KeyAlreadyExists<VecKey>);
CHECK_EXCEPTION(cfg1.insert(cfg2), ValuesKeyAlreadyExists);
}
/* ************************************************************************* */
TEST( TestValues, update_element )
TEST( Values, update_element )
{
TestValues cfg;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
Values cfg;
LieVector v1(3, 5.0, 6.0, 7.0);
LieVector v2(3, 8.0, 9.0, 1.0);
cfg.insert(key1, v1);
CHECK(cfg.size() == 1);
@ -122,12 +122,12 @@ TEST( TestValues, update_element )
}
///* ************************************************************************* */
//TEST(TestValues, dim_zero)
//TEST(Values, dim_zero)
//{
// TestValues config0;
// config0.insert(key1, Vector_(2, 2.0, 3.0));
// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
// LONGS_EQUAL(5,config0.dim());
// Values config0;
// config0.insert(key1, LieVector(2, 2.0, 3.0));
// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
// LONGS_EQUAL(5, config0.dim());
//
// VectorValues expected;
// expected.insert(key1, zero(2));
@ -136,151 +136,130 @@ TEST( TestValues, update_element )
//}
/* ************************************************************************* */
TEST(TestValues, expmap_a)
TEST(Values, expmap_a)
{
TestValues config0;
config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
Values config0;
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
Ordering ordering(*config0.orderingArbitrary());
VectorValues increment(config0.dims(ordering));
increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2);
increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
TestValues expected;
expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
Values expected;
expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
CHECK(assert_equal(expected, config0.retract(increment, ordering)));
}
///* ************************************************************************* */
//TEST(TestValues, expmap_b)
//{
// TestValues config0;
// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
//
// Ordering ordering(*config0.orderingArbitrary());
// VectorValues increment(config0.dims(ordering));
// increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
//
// TestValues expected;
// expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
//
// CHECK(assert_equal(expected, config0.retract(increment, ordering)));
//}
/* ************************************************************************* */
TEST(Values, expmap_b)
{
Values config0;
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
///* ************************************************************************* */
//TEST(TestValues, expmap_c)
Ordering ordering(*config0.orderingArbitrary());
VectorValues increment(VectorValues::Zero(config0.dims(ordering)));
increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5);
Values expected;
expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
CHECK(assert_equal(expected, config0.retract(increment, ordering)));
}
/* ************************************************************************* */
//TEST(Values, expmap_c)
//{
// TestValues config0;
// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
// Values config0;
// config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
//
// Vector increment = Vector_(6,
// Vector increment = LieVector(6,
// 1.0, 1.1, 1.2,
// 1.3, 1.4, 1.5);
//
// TestValues expected;
// expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
// Values expected;
// expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
// expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
//
// CHECK(assert_equal(expected, config0.retract(increment)));
//}
/* ************************************************************************* */
/*TEST(TestValues, expmap_d)
TEST(Values, expmap_d)
{
TestValues config0;
config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
Values config0;
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
//config0.print("config0");
CHECK(equal(config0, config0));
CHECK(config0.equals(config0));
TestValues<string,Pose2> poseconfig;
poseconfig.insert("p1", Pose2(1,2,3));
poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
//poseconfig.print("poseconfig");
typedef TypedSymbol<Pose2, 'p'> PoseKey;
Values poseconfig;
poseconfig.insert(PoseKey(1), Pose2(1,2,3));
poseconfig.insert(PoseKey(2), Pose2(0.3, 0.4, 0.5));
CHECK(equal(config0, config0));
CHECK(config0.equals(config0));
}*/
}
/* ************************************************************************* */
/*TEST(TestValues, extract_keys)
TEST(Values, extract_keys)
{
typedef TypedSymbol<Pose2, 'x'> PoseKey;
TestValues<PoseKey, Pose2> config;
Values config;
config.insert(PoseKey(1), Pose2());
config.insert(PoseKey(2), Pose2());
config.insert(PoseKey(4), Pose2());
config.insert(PoseKey(5), Pose2());
list<PoseKey> expected, actual;
FastList<Symbol> expected, actual;
expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5);
actual = config.keys();
CHECK(actual.size() == expected.size());
list<PoseKey>::const_iterator itAct = actual.begin(), itExp = expected.begin();
FastList<Symbol>::const_iterator itAct = actual.begin(), itExp = expected.begin();
for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
CHECK(assert_equal(*itExp, *itAct));
}
}*/
}
/* ************************************************************************* */
TEST(TestValues, exists_)
TEST(Values, exists_)
{
TestValues config0;
config0.insert(key1, Vector_(1, 1.));
config0.insert(key2, Vector_(1, 2.));
Values config0;
config0.insert(key1, LieVector(Vector_(1, 1.)));
config0.insert(key2, LieVector(Vector_(1, 2.)));
boost::optional<LieVector> v = config0.exists_(key1);
boost::optional<const LieVector&> v = config0.exists(key1);
CHECK(assert_equal(Vector_(1, 1.),*v));
}
/* ************************************************************************* */
TEST(TestValues, update)
TEST(Values, update)
{
TestValues config0;
config0.insert(key1, Vector_(1, 1.));
config0.insert(key2, Vector_(1, 2.));
Values config0;
config0.insert(key1, LieVector(1, 1.));
config0.insert(key2, LieVector(1, 2.));
TestValues superset;
superset.insert(key1, Vector_(1, -1.));
superset.insert(key2, Vector_(1, -2.));
superset.insert(key3, Vector_(1, -3.));
Values superset;
superset.insert(key1, LieVector(1, -1.));
superset.insert(key2, LieVector(1, -2.));
config0.update(superset);
TestValues expected;
expected.insert(key1, Vector_(1, -1.));
expected.insert(key2, Vector_(1, -2.));
Values expected;
expected.insert(key1, LieVector(1, -1.));
expected.insert(key2, LieVector(1, -2.));
CHECK(assert_equal(expected,config0));
}
/* ************************************************************************* */
TEST(TestValues, dummy_initialization)
{
typedef TypedSymbol<LieVector, 'z'> Key;
typedef Values<Key> Values1;
Values1 init1;
init1.insert(Key(1), Vector_(2, 1.0, 2.0));
init1.insert(Key(2), Vector_(2, 4.0, 3.0));
TestValues init2;
init2.insert(key1, Vector_(2, 1.0, 2.0));
init2.insert(key2, Vector_(2, 4.0, 3.0));
Values1 actual1(init2);
TestValues actual2(init1);
EXPECT(actual1.empty());
EXPECT(actual2.empty());
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

21
gtsam/slam/AntiFactor.h
View File

@ -27,27 +27,26 @@ namespace gtsam {
* linearized Hessian matrices of the original factor, but with the opposite sign. This effectively
* cancels out any affects of the original factor during optimization."
*/
template<class VALUES>
class AntiFactor: public NonlinearFactor<VALUES> {
class AntiFactor: public NonlinearFactor {
private:
typedef AntiFactor<VALUES> This;
typedef NonlinearFactor<VALUES> Base;
typedef typename NonlinearFactor<VALUES>::shared_ptr sharedFactor;
typedef AntiFactor This;
typedef NonlinearFactor Base;
typedef NonlinearFactor::shared_ptr sharedFactor;
sharedFactor factor_;
public:
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<AntiFactor> shared_ptr;
typedef boost::shared_ptr<AntiFactor> shared_ptr;
/** default constructor - only use for serialization */
AntiFactor() {}
/** constructor - Creates the equivalent AntiFactor from an existing factor */
AntiFactor(typename NonlinearFactor<VALUES>::shared_ptr factor) : Base(factor->begin(), factor->end()), factor_(factor) {}
AntiFactor(NonlinearFactor::shared_ptr factor) : Base(factor->begin(), factor->end()), factor_(factor) {}
virtual ~AntiFactor() {}
@ -60,7 +59,7 @@ namespace gtsam {
}
/** equals */
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol);
}
@ -72,7 +71,7 @@ namespace gtsam {
* For the AntiFactor, this will have the same magnitude of the original factor,
* but the opposite sign.
*/
double error(const VALUES& c) const { return -factor_->error(c); }
double error(const Values& c) const { return -factor_->error(c); }
/** get the dimension of the factor (same as the original factor) */
size_t dim() const { return factor_->dim(); }
@ -81,7 +80,7 @@ namespace gtsam {
* Checks whether this factor should be used based on a set of values.
* The AntiFactor will have the same 'active' profile as the original factor.
*/
bool active(const VALUES& c) const { return factor_->active(c); }
bool active(const Values& c) const { return factor_->active(c); }
/**
* Linearize to a GaussianFactor. The AntiFactor always returns a Hessian Factor
@ -90,7 +89,7 @@ namespace gtsam {
* effectively cancels the effect of the original factor on the factor graph.
*/
boost::shared_ptr<GaussianFactor>
linearize(const VALUES& c, const Ordering& ordering) const {
linearize(const Values& c, const Ordering& ordering) const {
// Generate the linearized factor from the contained nonlinear factor
GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c, ordering);

10
gtsam/slam/BearingFactor.h
View File

@ -25,16 +25,16 @@ namespace gtsam {
/**
* Binary factor for a bearing measurement
*/
template<class VALUES, class POSEKEY, class POINTKEY>
class BearingFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
template<class POSEKEY, class POINTKEY>
class BearingFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
private:
typedef typename POSEKEY::Value Pose;
typedef typename POSEKEY::Value::Rotation Rot;
typedef typename POINTKEY::Value Point;
typedef BearingFactor<VALUES, POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
typedef BearingFactor<POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
Rot z_; /** measurement */
@ -68,7 +68,7 @@ namespace gtsam {
}
/** equals */
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && this->z_.equals(e->z_, tol);
}

10
gtsam/slam/BearingRangeFactor.h
View File

@ -27,16 +27,16 @@ namespace gtsam {
/**
* Binary factor for a bearing measurement
*/
template<class VALUES, class POSEKEY, class POINTKEY>
class BearingRangeFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
template<class POSEKEY, class POINTKEY>
class BearingRangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
private:
typedef typename POSEKEY::Value Pose;
typedef typename POSEKEY::Value::Rotation Rot;
typedef typename POINTKEY::Value Point;
typedef BearingRangeFactor<VALUES, POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
typedef BearingRangeFactor<POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
// the measurement
Rot bearing_;
@ -68,7 +68,7 @@ namespace gtsam {
}
/** equals */
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) &&
fabs(this->range_ - e->range_) < tol &&

20
gtsam/slam/BetweenFactor.h
View File

@ -29,13 +29,13 @@ namespace gtsam {
* KEY1::Value is the Lie Group type
* T is the measurement type, by default the same
*/
template<class VALUES, class KEY1, class T = typename KEY1::Value>
class BetweenFactor: public NonlinearFactor2<VALUES, KEY1, KEY1> {
template<class KEY1, class T = typename KEY1::Value>
class BetweenFactor: public NonlinearFactor2<KEY1, KEY1> {
private:
typedef BetweenFactor<VALUES, KEY1, T> This;
typedef NonlinearFactor2<VALUES, KEY1, KEY1> Base;
typedef BetweenFactor<KEY1, T> This;
typedef NonlinearFactor2<KEY1, KEY1> Base;
T measured_; /** The measurement */
@ -71,7 +71,7 @@ namespace gtsam {
}
/** equals */
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
}
@ -114,15 +114,15 @@ namespace gtsam {
* This constraint requires the underlying type to a Lie type
*
*/
template<class VALUES, class KEY>
class BetweenConstraint : public BetweenFactor<VALUES, KEY> {
template<class KEY>
class BetweenConstraint : public BetweenFactor<KEY> {
public:
typedef boost::shared_ptr<BetweenConstraint<VALUES, KEY> > shared_ptr;
typedef boost::shared_ptr<BetweenConstraint<KEY> > shared_ptr;
/** Syntactic sugar for constrained version */
BetweenConstraint(const typename KEY::Value& measured,
const KEY& key1, const KEY& key2, double mu = 1000.0)
: BetweenFactor<VALUES, KEY>(key1, key2, measured,
: BetweenFactor<KEY>(key1, key2, measured,
noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {}
private:
@ -132,7 +132,7 @@ namespace gtsam {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("BetweenFactor",
boost::serialization::base_object<BetweenFactor<VALUES, KEY> >(*this));
boost::serialization::base_object<BetweenFactor<KEY> >(*this));
}
}; // \class BetweenConstraint

20
gtsam/slam/BoundingConstraint.h
View File

@ -28,11 +28,11 @@ namespace gtsam {
* will need to have its value function implemented to return
* a scalar for comparison.
*/
template<class VALUES, class KEY>
struct BoundingConstraint1: public NonlinearFactor1<VALUES, KEY> {
template<class KEY>
struct BoundingConstraint1: public NonlinearFactor1<KEY> {
typedef typename KEY::Value X;
typedef NonlinearFactor1<VALUES, KEY> Base;
typedef boost::shared_ptr<BoundingConstraint1<VALUES, KEY> > shared_ptr;
typedef NonlinearFactor1<KEY> Base;
typedef boost::shared_ptr<BoundingConstraint1<KEY> > shared_ptr;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
@ -57,7 +57,7 @@ struct BoundingConstraint1: public NonlinearFactor1<VALUES, KEY> {
boost::none) const = 0;
/** active when constraint *NOT* met */
bool active(const VALUES& c) const {
bool active(const Values& c) const {
// note: still active at equality to avoid zigzagging
double x = value(c[this->key_]);
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
@ -95,13 +95,13 @@ private:
* Binary scalar inequality constraint, with a similar value() function
* to implement for specific systems
*/
template<class VALUES, class KEY1, class KEY2>
struct BoundingConstraint2: public NonlinearFactor2<VALUES, KEY1, KEY2> {
template<class KEY1, class KEY2>
struct BoundingConstraint2: public NonlinearFactor2<KEY1, KEY2> {
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base;
typedef boost::shared_ptr<BoundingConstraint2<VALUES, KEY1, KEY2> > shared_ptr;
typedef NonlinearFactor2<KEY1, KEY2> Base;
typedef boost::shared_ptr<BoundingConstraint2<KEY1, KEY2> > shared_ptr;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
@ -125,7 +125,7 @@ struct BoundingConstraint2: public NonlinearFactor2<VALUES, KEY1, KEY2> {
boost::optional<Matrix&> H2 = boost::none) const = 0;
/** active when constraint *NOT* met */
bool active(const VALUES& c) const {
bool active(const Values& c) const {
// note: still active at equality to avoid zigzagging
double x = value(c[this->key1_], c[this->key2_]);
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;

12
gtsam/slam/GeneralSFMFactor.h
View File

@ -29,21 +29,21 @@ namespace gtsam {
/**
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is unknown here compared to GenericProjectionFactor
*/
template <class VALUES, class CamK, class LmK>
template <class CamK, class LmK>
class GeneralSFMFactor:
public NonlinearFactor2<VALUES, CamK, LmK> {
public NonlinearFactor2<CamK, LmK> {
protected:
Point2 z_; ///< the 2D measurement
public:
typedef typename CamK::Value Cam; ///< typedef for camera type
typedef GeneralSFMFactor<VALUES, CamK, LmK> Self ; ///< typedef for this object
typedef NonlinearFactor2<VALUES, CamK, LmK> Base; ///< typedef for the base class
typedef GeneralSFMFactor<CamK, LmK> Self ; ///< typedef for this object
typedef NonlinearFactor2<CamK, LmK> Base; ///< typedef for the base class
typedef Point2 Measurement; ///< typedef for the measurement
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GeneralSFMFactor<VALUES, LmK, CamK> > shared_ptr;
typedef boost::shared_ptr<GeneralSFMFactor<LmK, CamK> > shared_ptr;
/**
* Constructor
@ -72,7 +72,7 @@ namespace gtsam {
/**
* equals
*/
bool equals(const GeneralSFMFactor<VALUES, CamK, LmK> &p, double tol = 1e-9) const {
bool equals(const GeneralSFMFactor<CamK, LmK> &p, double tol = 1e-9) const {
return Base::equals(p, tol) && this->z_.equals(p.z_, tol) ;
}

10
gtsam/slam/PartialPriorFactor.h
View File

@ -38,16 +38,16 @@ namespace gtsam {
* For practical use, it would be good to subclass this factor and have the class type
* construct the mask.
*/
template<class VALUES, class KEY>
class PartialPriorFactor: public NonlinearFactor1<VALUES, KEY> {
template<class KEY>
class PartialPriorFactor: public NonlinearFactor1<KEY> {
public:
typedef typename KEY::Value T;
protected:
typedef NonlinearFactor1<VALUES, KEY> Base;
typedef PartialPriorFactor<VALUES, KEY> This;
typedef NonlinearFactor1<KEY> Base;
typedef PartialPriorFactor<KEY> This;
Vector prior_; ///< measurement on logmap parameters, in compressed form
std::vector<size_t> mask_; ///< indices of values to constrain in compressed prior vector
@ -95,7 +95,7 @@ namespace gtsam {
}
/** equals */
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) &&
gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) &&

11
gtsam/slam/PriorFactor.h
View File

@ -28,15 +28,15 @@ namespace gtsam {
* The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
* a simple type like int will not work
*/
template<class VALUES, class KEY>
class PriorFactor: public NonlinearFactor1<VALUES, KEY> {
template<class KEY>
class PriorFactor: public NonlinearFactor1<KEY> {
public:
typedef typename KEY::Value T;
private:
typedef NonlinearFactor1<VALUES, KEY> Base;
typedef NonlinearFactor1<KEY> Base;
T prior_; /** The measurement */
@ -69,9 +69,8 @@ namespace gtsam {
}
/** equals */
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
const PriorFactor<VALUES, KEY> *e = dynamic_cast<const PriorFactor<
VALUES, KEY>*> (&expected);
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const PriorFactor<KEY> *e = dynamic_cast<const PriorFactor<KEY>*> (&expected);
return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol);
}

10
gtsam/slam/ProjectionFactor.h
View File

@ -29,8 +29,8 @@ namespace gtsam {
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
* i.e. the main building block for visual SLAM.
*/
template<class VALUES, class LMK, class POSK>
class GenericProjectionFactor: public NonlinearFactor2<VALUES, POSK, LMK> {
template<class LMK, class POSK>
class GenericProjectionFactor: public NonlinearFactor2<POSK, LMK> {
protected:
// Keep a copy of measurement and calibration for I/O
@ -40,10 +40,10 @@ namespace gtsam {
public:
/// shorthand for base class type
typedef NonlinearFactor2<VALUES, POSK, LMK> Base;
typedef NonlinearFactor2<POSK, LMK> Base;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GenericProjectionFactor<VALUES, LMK, POSK> > shared_ptr;
typedef boost::shared_ptr<GenericProjectionFactor<LMK, POSK> > shared_ptr;
/// Default constructor
GenericProjectionFactor() :
@ -73,7 +73,7 @@ namespace gtsam {
}
/// equals
bool equals(const GenericProjectionFactor<VALUES, LMK, POSK>& p
bool equals(const GenericProjectionFactor<LMK, POSK>& p
, double tol = 1e-9) const {
return Base::equals(p, tol) && this->z_.equals(p.z_, tol)
&& this->K_->equals(*p.K_, tol);

10
gtsam/slam/RangeFactor.h
View File

@ -25,14 +25,14 @@ namespace gtsam {
/**
* Binary factor for a range measurement
*/
template<class VALUES, class POSEKEY, class POINTKEY>
class RangeFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
template<class POSEKEY, class POINTKEY>
class RangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
private:
double z_; /** measurement */
typedef RangeFactor<VALUES, POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
typedef RangeFactor<POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
typedef typename POSEKEY::Value Pose;
typedef typename POINTKEY::Value Point;
@ -64,7 +64,7 @@ namespace gtsam {
}
/** equals specialized to this factor */
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && fabs(this->z_ - e->z_) < tol;
}

6
gtsam/slam/StereoFactor.h
View File

@ -22,8 +22,8 @@
namespace gtsam {
template<class VALUES, class KEY1, class KEY2>
class GenericStereoFactor: public NonlinearFactor2<VALUES, KEY1, KEY2> {
template<class KEY1, class KEY2>
class GenericStereoFactor: public NonlinearFactor2<KEY1, KEY2> {
private:
// Keep a copy of measurement and calibration for I/O
@ -33,7 +33,7 @@ private:
public:
// shorthand for base class type
typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base; ///< typedef for base class
typedef NonlinearFactor2<KEY1, KEY2> Base; ///< typedef for base class
typedef boost::shared_ptr<GenericStereoFactor> shared_ptr; ///< typedef for shared pointer to this object
typedef typename KEY1::Value CamPose; ///< typedef for Pose Lie Value type

28
gtsam/slam/dataset.cpp
View File

@ -29,7 +29,6 @@ using namespace gtsam;
#define LINESIZE 81920
typedef boost::shared_ptr<pose2SLAM::Graph> sharedPose2Graph;
typedef boost::shared_ptr<pose2SLAM::Values> sharedPose2Values;
typedef pose2SLAM::Odometry Pose2Factor; ///< Typedef for Constraint class for backwards compatibility
namespace gtsam {
@ -66,13 +65,13 @@ pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset, c
/* ************************************************************************* */
pair<sharedPose2Graph, sharedPose2Values> load2D(
pair<sharedPose2Graph, Values::shared_ptr> load2D(
pair<string, boost::optional<SharedDiagonal> > dataset,
int maxID, bool addNoise, bool smart) {
return load2D(dataset.first, dataset.second, maxID, addNoise, smart);
}
pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
pair<sharedPose2Graph, Values::shared_ptr> load2D(const string& filename,
boost::optional<SharedDiagonal> model, int maxID, bool addNoise, bool smart) {
cout << "Will try to read " << filename << endl;
ifstream is(filename.c_str());
@ -81,7 +80,7 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
exit(-1);
}
sharedPose2Values poses(new pose2SLAM::Values);
Values::shared_ptr poses(new Values);
sharedPose2Graph graph(new pose2SLAM::Graph);
string tag;
@ -96,7 +95,7 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
is >> id >> x >> y >> yaw;
// optional filter
if (maxID && id >= maxID) continue;
poses->insert(id, Pose2(x, y, yaw));
poses->insert(pose2SLAM::PoseKey(id), Pose2(x, y, yaw));
}
is.ignore(LINESIZE, '\n');
}
@ -133,8 +132,8 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
l1Xl2 = l1Xl2.retract((*model)->sample());
// Insert vertices if pure odometry file
if (!poses->exists(id1)) poses->insert(id1, Pose2());
if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * l1Xl2);
if (!poses->exists(pose2SLAM::PoseKey(id1))) poses->insert(pose2SLAM::PoseKey(id1), Pose2());
if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at(pose2SLAM::PoseKey(id1)) * l1Xl2);
pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model));
graph->push_back(factor);
@ -149,26 +148,25 @@ pair<sharedPose2Graph, sharedPose2Values> load2D(const string& filename,
}
/* ************************************************************************* */
void save2D(const pose2SLAM::Graph& graph, const pose2SLAM::Values& config, const SharedDiagonal model,
void save2D(const pose2SLAM::Graph& graph, const Values& config, const SharedDiagonal model,
const string& filename) {
typedef pose2SLAM::Values::Key Key;
fstream stream(filename.c_str(), fstream::out);
// save poses
pose2SLAM::Values::Key key;
Pose2 pose;
BOOST_FOREACH(boost::tie(key, pose), config)
stream << "VERTEX2 " << key.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) {
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.second);
stream << "VERTEX2 " << key_value.first.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl;
}
// save edges
Matrix R = model->R();
Matrix RR = trans(R)*R;//prod(trans(R),R);
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor<pose2SLAM::Values> > factor_, graph) {
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
boost::shared_ptr<Pose2Factor> factor = boost::dynamic_pointer_cast<Pose2Factor>(factor_);
if (!factor) continue;
pose = factor->measured().inverse();
Pose2 pose = factor->measured().inverse();
stream << "EDGE2 " << factor->key2().index() << " " << factor->key1().index()
<< " " << pose.x() << " " << pose.y() << " " << pose.theta()
<< " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2)

6
gtsam/slam/dataset.h
View File

@ -40,16 +40,16 @@ std::pair<std::string, boost::optional<gtsam::SharedDiagonal> >
* @param maxID, if non-zero cut out vertices >= maxID
* @param smart: try to reduce complexity of covariance to cheapest model
*/
std::pair<boost::shared_ptr<pose2SLAM::Graph>, boost::shared_ptr<pose2SLAM::Values> > load2D(
std::pair<boost::shared_ptr<pose2SLAM::Graph>, boost::shared_ptr<Values> > load2D(
std::pair<std::string, boost::optional<SharedDiagonal> > dataset,
int maxID = 0, bool addNoise=false, bool smart=true);
std::pair<boost::shared_ptr<pose2SLAM::Graph>, boost::shared_ptr<pose2SLAM::Values> > load2D(
std::pair<boost::shared_ptr<pose2SLAM::Graph>, boost::shared_ptr<Values> > load2D(
const std::string& filename,
boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(),
int maxID = 0, bool addNoise=false, bool smart=true);
/** save 2d graph */
void save2D(const pose2SLAM::Graph& graph, const pose2SLAM::Values& config, const gtsam::SharedDiagonal model,
void save2D(const pose2SLAM::Graph& graph, const Values& config, const gtsam::SharedDiagonal model,
const std::string& filename);
/**

4
gtsam/slam/planarSLAM.cpp
View File

@ -23,8 +23,8 @@
// Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM {
Graph::Graph(const NonlinearFactorGraph<Values>& graph) :
NonlinearFactorGraph<Values>(graph) {}
Graph::Graph(const NonlinearFactorGraph& graph) :
NonlinearFactorGraph(graph) {}
void Graph::addPrior(const PoseKey& i, const Pose2& p,
const SharedNoiseModel& model) {

131
gtsam/slam/planarSLAM.h
View File

@ -22,7 +22,6 @@
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/BearingFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
@ -39,107 +38,63 @@ namespace planarSLAM {
/// Typedef for a PointKey with Point2 data and 'l' symbol
typedef TypedSymbol<Point2, 'l'> PointKey;
/// Typedef for Values structure with PoseKey type
typedef Values<PoseKey> PoseValues;
/// 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> {
/// Default constructor
Values() {}
/// Copy constructor
Values(const TupleValues2<PoseValues, PointValues>& values) :
TupleValues2<PoseValues, PointValues>(values) {
}
/// Copy constructor
Values(const TupleValues2<PoseValues, PointValues>::Base& values) :
TupleValues2<PoseValues, PointValues>(values) {
}
/// From sub-values
Values(const PoseValues& poses, const PointValues& points) :
TupleValues2<PoseValues, PointValues>(poses, points) {
}
// Convenience for MATLAB wrapper, which does not allow for identically named methods
/// get a pose
Pose2 pose(int key) const { return (*this)[PoseKey(key)]; }
/// get a point
Point2 point(int key) const { return (*this)[PointKey(key)]; }
/// insert a pose
void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); }
/// insert a point
void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
};
/**
* List of typedefs for factors
*/
/// A hard constraint for PoseKeys to enforce particular values
typedef NonlinearEquality<PoseKey> Constraint;
/// A prior factor to bias the value of a PoseKey
typedef PriorFactor<PoseKey> Prior;
/// A factor between two PoseKeys set with a Pose2
typedef BetweenFactor<PoseKey> Odometry;
/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
typedef BearingFactor<PoseKey, PointKey> Bearing;
/// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
typedef RangeFactor<PoseKey, PointKey> Range;
/// A factor between a PoseKey and a PointKey to express difference in rotation and location
typedef BearingRangeFactor<PoseKey, PointKey> BearingRange;
/// A hard constraint for PoseKeys to enforce particular values
typedef NonlinearEquality<Values, PoseKey> Constraint;
/// A prior factor to bias the value of a PoseKey
typedef PriorFactor<Values, PoseKey> Prior;
/// A factor between two PoseKeys set with a Pose2
typedef BetweenFactor<Values, PoseKey> Odometry;
/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
typedef BearingFactor<Values, PoseKey, PointKey> Bearing;
/// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
typedef RangeFactor<Values, PoseKey, PointKey> Range;
/// A factor between a PoseKey and a PointKey to express difference in rotation and location
typedef BearingRangeFactor<Values, PoseKey, PointKey> BearingRange;
/// Creates a NonlinearFactorGraph with the Values type
struct Graph: public NonlinearFactorGraph {
/// Creates a NonlinearFactorGraph with the Values type
struct Graph: public NonlinearFactorGraph<Values> {
/// Default constructor for a NonlinearFactorGraph
Graph(){}
/// Default constructor for a NonlinearFactorGraph
Graph(){}
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph& graph);
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph<Values>& graph);
/// Biases the value of PoseKey key with Pose2 p given a noise model
void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel);
/// Biases the value of PoseKey key with Pose2 p given a noise model
void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel);
/// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose);
/// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose);
/// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry,
const SharedNoiseModel& model);
/// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry,
const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation
void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing,
const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation
void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing,
const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location
void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range,
const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location
void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range,
const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location
void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey,
const Rot2& bearing, double range, const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location
void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey,
const Rot2& bearing, double range, const SharedNoiseModel& model);
/// Optimize
Values optimize(const Values& initialEstimate) {
typedef NonlinearOptimizer<Graph> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA);
}
};
/// Optimize
Values optimize(const Values& initialEstimate) {
typedef NonlinearOptimizer<Graph, Values> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA);
}
};
/// Optimizer
typedef NonlinearOptimizer<Graph, Values> Optimizer;
/// Optimizer
typedef NonlinearOptimizer<Graph> Optimizer;
} // planarSLAM

4
gtsam/slam/pose2SLAM.cpp
View File

@ -21,7 +21,7 @@
// Use pose2SLAM namespace for specific SLAM instance
template class gtsam::NonlinearOptimizer<pose2SLAM::Graph, pose2SLAM::Values, gtsam::GaussianFactorGraph, gtsam::GaussianSequentialSolver>;
template class gtsam::NonlinearOptimizer<pose2SLAM::Graph, gtsam::GaussianFactorGraph, gtsam::GaussianSequentialSolver>;
namespace pose2SLAM {
@ -30,7 +30,7 @@ namespace pose2SLAM {
Values x;
double theta = 0, dtheta = 2 * M_PI / n;
for (size_t i = 0; i < n; i++, theta += dtheta)
x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
x.insert(PoseKey(i), Pose2(cos(theta), sin(theta), M_PI_2 + theta));
return x;
}

37
gtsam/slam/pose2SLAM.h
View File

@ -18,10 +18,10 @@
#pragma once
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
@ -36,25 +36,6 @@ namespace pose2SLAM {
/// Keys with Pose2 and symbol 'x'
typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Values class, inherited from Values, using PoseKeys
struct Values: public gtsam::Values<PoseKey> {
/// Default constructor
Values() {}
/// Copy constructor
Values(const gtsam::Values<PoseKey>& values) :
gtsam::Values<PoseKey>(values) {
}
// Convenience for MATLAB wrapper, which does not allow for identically named methods
/// get a pose
Pose2 pose(int key) const { return (*this)[PoseKey(key)]; }
/// insert a pose
void insertPose(int key, const Pose2& pose) { insert(PoseKey(key), pose); }
};
/**
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
@ -70,20 +51,20 @@ namespace pose2SLAM {
*/
/// A hard constraint to enforce a specific value for a pose
typedef NonlinearEquality<Values, PoseKey> HardConstraint;
typedef NonlinearEquality<PoseKey> HardConstraint;
/// A prior factor on a pose with Pose2 data type.
typedef PriorFactor<Values, PoseKey> Prior;
typedef PriorFactor<PoseKey> Prior;
/// A factor to add an odometry measurement between two poses.
typedef BetweenFactor<Values, PoseKey> Odometry;
typedef BetweenFactor<PoseKey> Odometry;
/// Graph
struct Graph: public NonlinearFactorGraph<Values> {
struct Graph: public NonlinearFactorGraph {
/// Default constructor for a NonlinearFactorGraph
Graph(){}
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph<Values>& graph);
Graph(const NonlinearFactorGraph& graph);
/// Adds a Pose2 prior with a noise model to one of the keys in the nonlinear factor graph
void addPrior(const PoseKey& i, const Pose2& p, const SharedNoiseModel& model);
@ -97,18 +78,18 @@ namespace pose2SLAM {
/// Optimize
Values optimize(const Values& initialEstimate) {
typedef NonlinearOptimizer<Graph, Values> Optimizer;
typedef NonlinearOptimizer<Graph> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA);
}
};
/// The sequential optimizer
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph,
typedef NonlinearOptimizer<Graph, GaussianFactorGraph,
GaussianSequentialSolver> OptimizerSequential;
/// The multifrontal optimizer
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph,
typedef NonlinearOptimizer<Graph, GaussianFactorGraph,
GaussianMultifrontalSolver> Optimizer;
} // pose2SLAM

2
gtsam/slam/pose3SLAM.cpp
View File

@ -39,7 +39,7 @@ namespace gtsam {
Point3 gti(radius*cos(theta), radius*sin(theta), 0);
Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down !
Pose3 gTi(gR0 * _0Ri, gti);
x.insert(i, gTi);
x.insert(Key(i), gTi);
}
return x;
}

14
gtsam/slam/pose3SLAM.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h>
@ -33,8 +32,6 @@ namespace gtsam {
/// Creates a Key with data Pose3 and symbol 'x'
typedef TypedSymbol<Pose3, 'x'> Key;
/// 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)
@ -45,14 +42,14 @@ namespace gtsam {
Values circle(size_t n, double R);
/// A prior factor on Key with Pose3 data type.
typedef PriorFactor<Values, Key> Prior;
typedef PriorFactor<Key> Prior;
/// A factor to put constraints between two factors.
typedef BetweenFactor<Values, Key> Constraint;
typedef BetweenFactor<Key> Constraint;
/// A hard constraint would enforce that the given key would have the input value in the results.
typedef NonlinearEquality<Values, Key> HardConstraint;
typedef NonlinearEquality<Key> HardConstraint;
/// Graph
struct Graph: public NonlinearFactorGraph<Values> {
struct Graph: public NonlinearFactorGraph {
/// Adds a factor between keys of the same type
void addPrior(const Key& i, const Pose3& p,
@ -67,14 +64,13 @@ namespace gtsam {
};
/// Optimizer
typedef NonlinearOptimizer<Graph, Values> Optimizer;
typedef NonlinearOptimizer<Graph> Optimizer;
} // pose3SLAM
/**
* Backwards compatibility
*/
typedef pose3SLAM::Values Pose3Values; ///< Typedef for Values class for backwards compatibility
typedef pose3SLAM::Prior Pose3Prior; ///< Typedef for Prior class for backwards compatibility
typedef pose3SLAM::Constraint Pose3Factor; ///< Typedef for Constraint class for backwards compatibility
typedef pose3SLAM::Graph Pose3Graph; ///< Typedef for Graph class for backwards compatibility

89
gtsam/slam/simulated2D.h
View File

@ -19,7 +19,6 @@
#pragma once
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -32,56 +31,6 @@ namespace simulated2D {
// Simulated2D robots have no orientation, just a position
typedef TypedSymbol<Point2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
/**
* Custom Values class that holds poses and points
*/
class Values: public TupleValues2<PoseValues, PointValues> {
public:
typedef TupleValues2<PoseValues, PointValues> Base; ///< base class
typedef boost::shared_ptr<Point2> sharedPoint; ///< shortcut to shared Point type
/// Constructor
Values() {
}
/// Copy constructor
Values(const Base& base) :
Base(base) {
}
/// Insert a pose
void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
insert(i, p);
}
/// Insert a point
void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
insert(j, p);
}
/// Number of poses
int nrPoses() const {
return this->first_.size();
}
/// Number of points
int nrPoints() const {
return this->second_.size();
}
/// Return pose i
Point2 pose(const simulated2D::PoseKey& i) const {
return (*this)[i];
}
/// Return point j
Point2 point(const simulated2D::PointKey& j) const {
return (*this)[j];
}
};
/// Prior on a single pose
inline Point2 prior(const Point2& x) {
@ -112,18 +61,18 @@ namespace simulated2D {
/**
* Unary factor encoding a soft prior on a vector
*/
template<class VALUES = Values, class KEY = PoseKey>
class GenericPrior: public NonlinearFactor1<VALUES, KEY> {
template<class KEY = PoseKey>
class GenericPrior: public NonlinearFactor1<KEY> {
public:
typedef NonlinearFactor1<VALUES, KEY> Base; ///< base class
typedef boost::shared_ptr<GenericPrior<VALUES, KEY> > shared_ptr;
typedef NonlinearFactor1<KEY> Base; ///< base class
typedef boost::shared_ptr<GenericPrior<KEY> > shared_ptr;
typedef typename KEY::Value Pose; ///< shortcut to Pose type
Pose z_; ///< prior mean
/// Create generic prior
GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) :
NonlinearFactor1<VALUES, KEY>(model, key), z_(z) {
NonlinearFactor1<KEY>(model, key), z_(z) {
}
/// Return error and optional derivative
@ -150,11 +99,11 @@ namespace simulated2D {
/**
* Binary factor simulating "odometry" between two Vectors
*/
template<class VALUES = Values, class KEY = PoseKey>
class GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> {
template<class KEY = PoseKey>
class GenericOdometry: public NonlinearFactor2<KEY, KEY> {
public:
typedef NonlinearFactor2<VALUES, KEY, KEY> Base; ///< base class
typedef boost::shared_ptr<GenericOdometry<VALUES, KEY> > shared_ptr;
typedef NonlinearFactor2<KEY, KEY> Base; ///< base class
typedef boost::shared_ptr<GenericOdometry<KEY> > shared_ptr;
typedef typename KEY::Value Pose; ///< shortcut to Pose type
Pose z_; ///< odometry measurement
@ -162,7 +111,7 @@ namespace simulated2D {
/// Create odometry
GenericOdometry(const Pose& z, const SharedNoiseModel& model,
const KEY& i1, const KEY& i2) :
NonlinearFactor2<VALUES, KEY, KEY>(model, i1, i2), z_(z) {
NonlinearFactor2<KEY, KEY>(model, i1, i2), z_(z) {
}
/// Evaluate error and optionally return derivatives
@ -190,11 +139,11 @@ namespace simulated2D {
/**
* Binary factor simulating "measurement" between two Vectors
*/
template<class VALUES = Values, class XKEY = PoseKey, class LKEY = PointKey>
class GenericMeasurement: public NonlinearFactor2<VALUES, XKEY, LKEY> {
template<class XKEY = PoseKey, class LKEY = PointKey>
class GenericMeasurement: public NonlinearFactor2<XKEY, LKEY> {
public:
typedef NonlinearFactor2<VALUES, XKEY, LKEY> Base; ///< base class
typedef boost::shared_ptr<GenericMeasurement<VALUES, XKEY, LKEY> > shared_ptr;
typedef NonlinearFactor2<XKEY, LKEY> Base; ///< base class
typedef boost::shared_ptr<GenericMeasurement<XKEY, LKEY> > shared_ptr;
typedef typename XKEY::Value Pose; ///< shortcut to Pose type
typedef typename LKEY::Value Point; ///< shortcut to Point type
@ -203,7 +152,7 @@ namespace simulated2D {
/// Create measurement factor
GenericMeasurement(const Point& z, const SharedNoiseModel& model,
const XKEY& i, const LKEY& j) :
NonlinearFactor2<VALUES, XKEY, LKEY>(model, i, j), z_(z) {
NonlinearFactor2<XKEY, LKEY>(model, i, j), z_(z) {
}
/// Evaluate error and optionally return derivatives
@ -229,13 +178,13 @@ namespace simulated2D {
};
/** Typedefs for regular use */
typedef GenericPrior<Values, PoseKey> Prior;
typedef GenericOdometry<Values, PoseKey> Odometry;
typedef GenericMeasurement<Values, PoseKey, PointKey> Measurement;
typedef GenericPrior<PoseKey> Prior;
typedef GenericOdometry<PoseKey> Odometry;
typedef GenericMeasurement<PoseKey, PointKey> Measurement;
// Specialization of a graph for this example domain
// TODO: add functions to add factor types
class Graph : public NonlinearFactorGraph<Values> {
class Graph : public NonlinearFactorGraph {
public:
Graph() {}
};

44
gtsam/slam/simulated2DConstraints.h
View File

@ -33,13 +33,13 @@ namespace simulated2D {
namespace equality_constraints {
/** Typedefs for regular use */
typedef NonlinearEquality1<Values, PoseKey> UnaryEqualityConstraint;
typedef NonlinearEquality1<Values, PointKey> UnaryEqualityPointConstraint;
typedef BetweenConstraint<Values, PoseKey> OdoEqualityConstraint;
typedef NonlinearEquality1<PoseKey> UnaryEqualityConstraint;
typedef NonlinearEquality1<PointKey> UnaryEqualityPointConstraint;
typedef BetweenConstraint<PoseKey> OdoEqualityConstraint;
/** Equality between variables */
typedef NonlinearEquality2<Values, PoseKey> PoseEqualityConstraint;
typedef NonlinearEquality2<Values, PointKey> PointEqualityConstraint;
typedef NonlinearEquality2<PoseKey> PoseEqualityConstraint;
typedef NonlinearEquality2<PointKey> PointEqualityConstraint;
} // \namespace equality_constraints
@ -51,10 +51,10 @@ namespace simulated2D {
* @tparam KEY is the key type for the variable constrained
* @tparam IDX is an index in tangent space to constrain, must be less than KEY::VALUE::Dim()
*/
template<class VALUES, class KEY, unsigned int IDX>
struct ScalarCoordConstraint1: public BoundingConstraint1<VALUES, KEY> {
typedef BoundingConstraint1<VALUES, KEY> Base; ///< Base class convenience typedef
typedef boost::shared_ptr<ScalarCoordConstraint1<VALUES, KEY, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
template<class KEY, unsigned int IDX>
struct ScalarCoordConstraint1: public BoundingConstraint1<KEY> {
typedef BoundingConstraint1<KEY> Base; ///< Base class convenience typedef
typedef boost::shared_ptr<ScalarCoordConstraint1<KEY, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
typedef typename KEY::Value Point; ///< Constrained variable type
virtual ~ScalarCoordConstraint1() {}
@ -68,7 +68,7 @@ namespace simulated2D {
*/
ScalarCoordConstraint1(const KEY& key, double c,
bool isGreaterThan, double mu = 1000.0) :
Base(key, c, isGreaterThan, mu) {
Base(key, c, isGreaterThan, mu) {
}
/**
@ -94,8 +94,8 @@ namespace simulated2D {
};
/** typedefs for use with simulated2D systems */
typedef ScalarCoordConstraint1<Values, PoseKey, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
typedef ScalarCoordConstraint1<Values, PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
typedef ScalarCoordConstraint1<PoseKey, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
typedef ScalarCoordConstraint1<PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
/**
* Trait for distance constraints to provide distance
@ -114,9 +114,9 @@ namespace simulated2D {
* @tparam VALUES is the variable set for the graph
* @tparam KEY is the type of the keys for the variables constrained
*/
template<class VALUES, class KEY>
struct MaxDistanceConstraint : public BoundingConstraint2<VALUES, KEY, KEY> {
typedef BoundingConstraint2<VALUES, KEY, KEY> Base; ///< Base class for factor
template<class KEY>
struct MaxDistanceConstraint : public BoundingConstraint2<KEY, KEY> {
typedef BoundingConstraint2<KEY, KEY> Base; ///< Base class for factor
typedef typename KEY::Value Point; ///< Type of variable constrained
virtual ~MaxDistanceConstraint() {}
@ -129,7 +129,7 @@ namespace simulated2D {
* @param mu is the gain for the penalty function
*/
MaxDistanceConstraint(const KEY& key1, const KEY& key2, double range_bound, double mu = 1000.0)
: Base(key1, key2, range_bound, false, mu) {}
: Base(key1, key2, range_bound, false, mu) {}
/**
* computes the range with derivatives
@ -148,7 +148,7 @@ namespace simulated2D {
}
};
typedef MaxDistanceConstraint<Values, PoseKey> PoseMaxDistConstraint; ///< Simulated2D domain example factor
typedef MaxDistanceConstraint<PoseKey> PoseMaxDistConstraint; ///< Simulated2D domain example factor
/**
* Binary inequality constraint forcing a minimum range
@ -157,9 +157,9 @@ namespace simulated2D {
* @tparam XKEY is the type of the pose key constrained
* @tparam PKEY is the type of the point key constrained
*/
template<class VALUES, class XKEY, class PKEY>
struct MinDistanceConstraint : public BoundingConstraint2<VALUES, XKEY, PKEY> {
typedef BoundingConstraint2<VALUES, XKEY, PKEY> Base; ///< Base class for factor
template<class XKEY, class PKEY>
struct MinDistanceConstraint : public BoundingConstraint2<XKEY, PKEY> {
typedef BoundingConstraint2<XKEY, PKEY> Base; ///< Base class for factor
typedef typename XKEY::Value Pose; ///< Type of pose variable constrained
typedef typename PKEY::Value Point; ///< Type of point variable constrained
@ -174,7 +174,7 @@ namespace simulated2D {
*/
MinDistanceConstraint(const XKEY& key1, const PKEY& key2,
double range_bound, double mu = 1000.0)
: Base(key1, key2, range_bound, true, mu) {}
: Base(key1, key2, range_bound, true, mu) {}
/**
* computes the range with derivatives
@ -193,7 +193,7 @@ namespace simulated2D {
}
};
typedef MinDistanceConstraint<Values, PoseKey, PointKey> LandmarkAvoid; ///< Simulated2D domain example factor
typedef MinDistanceConstraint<PoseKey, PointKey> LandmarkAvoid; ///< Simulated2D domain example factor
} // \namespace inequality_constraints

40
gtsam/slam/simulated2DOriented.h
View File

@ -19,7 +19,6 @@
#pragma once
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -31,29 +30,6 @@ namespace simulated2DOriented {
// The types that take an oriented pose2 rather than point2
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
/// Specialized Values structure with syntactic sugar for
/// compatibility with matlab
class Values: public TupleValues2<PoseValues, PointValues> {
public:
Values() {}
void insertPose(const PoseKey& i, const Pose2& p) {
insert(i, p);
}
void insertPoint(const PointKey& j, const Point2& p) {
insert(j, p);
}
int nrPoses() const { return this->first_.size(); }
int nrPoints() const { return this->second_.size(); }
Pose2 pose(const PoseKey& i) const { return (*this)[i]; }
Point2 point(const PointKey& j) const { return (*this)[j]; }
};
//TODO:: point prior is not implemented right now
@ -75,15 +51,15 @@ namespace simulated2DOriented {
boost::none, boost::optional<Matrix&> H2 = boost::none);
/// Unary factor encoding a soft prior on a vector
template<class VALUES = Values, class Key = PoseKey>
struct GenericPosePrior: public NonlinearFactor1<VALUES, Key> {
template<class Key = PoseKey>
struct GenericPosePrior: public NonlinearFactor1<Key> {
Pose2 z_; ///< measurement
/// Create generic pose prior
GenericPosePrior(const Pose2& z, const SharedNoiseModel& model,
const Key& key) :
NonlinearFactor1<VALUES, Key>(model, key), z_(z) {
NonlinearFactor1<Key>(model, key), z_(z) {
}
/// Evaluate error and optionally derivative
@ -97,8 +73,8 @@ namespace simulated2DOriented {
/**
* Binary factor simulating "odometry" between two Vectors
*/
template<class VALUES = Values, class KEY = PoseKey>
struct GenericOdometry: public NonlinearFactor2<VALUES, KEY, KEY> {
template<class KEY = PoseKey>
struct GenericOdometry: public NonlinearFactor2<KEY, KEY> {
Pose2 z_; ///< Between measurement for odometry factor
/**
@ -106,7 +82,7 @@ namespace simulated2DOriented {
*/
GenericOdometry(const Pose2& z, const SharedNoiseModel& model,
const KEY& i1, const KEY& i2) :
NonlinearFactor2<VALUES, KEY, KEY>(model, i1, i2), z_(z) {
NonlinearFactor2<KEY, KEY>(model, i1, i2), z_(z) {
}
/// Evaluate error and optionally derivative
@ -118,10 +94,10 @@ namespace simulated2DOriented {
};
typedef GenericOdometry<Values, PoseKey> Odometry;
typedef GenericOdometry<PoseKey> Odometry;
/// Graph specialization for syntactic sugar use with matlab
class Graph : public NonlinearFactorGraph<Values> {
class Graph : public NonlinearFactorGraph {
public:
Graph() {}
// TODO: add functions to add factors

14
gtsam/slam/simulated3D.h
View File

@ -24,7 +24,6 @@
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/TupleValues.h>
// \namespace
@ -40,10 +39,6 @@ namespace simulated3D {
typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
typedef TupleValues2<PoseValues, PointValues> Values;
/**
* Prior on a single pose
*/
@ -66,7 +61,7 @@ Point3 mea(const Point3& x, const Point3& l,
/**
* A prior factor on a single linear robot pose
*/
struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
struct PointPrior3D: public NonlinearFactor1<PoseKey> {
Point3 z_; ///< The prior pose value for the variable attached to this factor
@ -78,7 +73,7 @@ struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
*/
PointPrior3D(const Point3& z,
const SharedNoiseModel& model, const PoseKey& j) :
NonlinearFactor1<Values, PoseKey> (model, j), z_(z) {
NonlinearFactor1<PoseKey> (model, j), z_(z) {
}
/**
@ -97,8 +92,7 @@ struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
/**
* Models a linear 3D measurement between 3D points
*/
struct Simulated3DMeasurement: public NonlinearFactor2<Values,
PoseKey, PointKey> {
struct Simulated3DMeasurement: public NonlinearFactor2<PoseKey, PointKey> {
Point3 z_; ///< Linear displacement between a pose and landmark
@ -111,7 +105,7 @@ PoseKey, PointKey> {
*/
Simulated3DMeasurement(const Point3& z,
const SharedNoiseModel& model, PoseKey& j1, PointKey j2) :
NonlinearFactor2<Values, PoseKey, PointKey> (
NonlinearFactor2<PoseKey, PointKey> (
model, j1, j2), z_(z) {
}

9
gtsam/slam/smallExample.cpp
View File

@ -39,7 +39,7 @@ using namespace std;
namespace gtsam {
namespace example {
typedef boost::shared_ptr<NonlinearFactor<Values> > shared;
typedef boost::shared_ptr<NonlinearFactor > shared;
static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
@ -195,14 +195,13 @@ namespace example {
0.0, cos(v.y()));
}
struct UnaryFactor: public gtsam::NonlinearFactor1<Values,
simulated2D::PoseKey> {
struct UnaryFactor: public gtsam::NonlinearFactor1<simulated2D::PoseKey> {
Point2 z_;
UnaryFactor(const Point2& z, const SharedNoiseModel& model,
const simulated2D::PoseKey& key) :
gtsam::NonlinearFactor1<Values, simulated2D::PoseKey>(model, key), z_(z) {
gtsam::NonlinearFactor1<simulated2D::PoseKey>(model, key), z_(z) {
}
Vector evaluateError(const Point2& x, boost::optional<Matrix&> A =
@ -423,7 +422,7 @@ namespace example {
boost::tuple<FactorGraph<GaussianFactor>, Ordering, VectorValues> planarGraph(size_t N) {
// create empty graph
NonlinearFactorGraph<Values> nlfg;
NonlinearFactorGraph nlfg;
// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sharedSigma(2,1e-3), key(1,1)));

3
gtsam/slam/smallExample.h
View File

@ -28,8 +28,7 @@
namespace gtsam {
namespace example {
typedef simulated2D::Values Values;
typedef NonlinearFactorGraph<Values> Graph;
typedef NonlinearFactorGraph Graph;
/**
* Create small example for non-linear factor graph

18
gtsam/slam/tests/testAntiFactor.cpp
View File

@ -39,9 +39,9 @@ TEST( AntiFactor, NegativeHessian)
SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim()));
// Create a configuration corresponding to the ground truth
boost::shared_ptr<pose3SLAM::Values> values(new pose3SLAM::Values());
values->insert(1, pose1);
values->insert(2, pose2);
boost::shared_ptr<Values> values(new Values());
values->insert(pose3SLAM::Key(1), pose1);
values->insert(pose3SLAM::Key(2), pose2);
// Define an elimination ordering
Ordering::shared_ptr ordering(new Ordering());
@ -50,7 +50,7 @@ TEST( AntiFactor, NegativeHessian)
// Create a "standard" factor
BetweenFactor<pose3SLAM::Values,pose3SLAM::Key>::shared_ptr originalFactor(new BetweenFactor<pose3SLAM::Values,pose3SLAM::Key>(1, 2, z, sigma));
BetweenFactor<pose3SLAM::Key>::shared_ptr originalFactor(new BetweenFactor<pose3SLAM::Key>(1, 2, z, sigma));
// Linearize it into a Jacobian Factor
GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering);
@ -59,7 +59,7 @@ TEST( AntiFactor, NegativeHessian)
HessianFactor::shared_ptr originalHessian = HessianFactor::shared_ptr(new HessianFactor(*originalJacobian));
// Create the AntiFactor version of the original nonlinear factor
AntiFactor<pose3SLAM::Values>::shared_ptr antiFactor(new AntiFactor<pose3SLAM::Values>(originalFactor));
AntiFactor::shared_ptr antiFactor(new AntiFactor(originalFactor));
// Linearize the AntiFactor into a Hessian Factor
GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(*values, *ordering);
@ -100,9 +100,9 @@ TEST( AntiFactor, EquivalentBayesNet)
graph->addConstraint(1, 2, pose1.between(pose2), sigma);
// Create a configuration corresponding to the ground truth
boost::shared_ptr<pose3SLAM::Values> values(new pose3SLAM::Values());
values->insert(1, pose1);
values->insert(2, pose2);
boost::shared_ptr<Values> values(new Values());
values->insert(pose3SLAM::Key(1), pose1);
values->insert(pose3SLAM::Key(2), pose2);
// Define an elimination ordering
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
@ -119,7 +119,7 @@ TEST( AntiFactor, EquivalentBayesNet)
graph->push_back(f1);
// Add the corresponding AntiFactor between Pose1 and Pose2
AntiFactor<pose3SLAM::Values>::shared_ptr f2(new AntiFactor<pose3SLAM::Values>(f1));
AntiFactor::shared_ptr f2(new AntiFactor(f1));
graph->push_back(f2);
// Again, Eliminate into a BayesNet

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

@ -21,7 +21,6 @@ using namespace boost;
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/slam/GeneralSFMFactor.h>
@ -31,14 +30,11 @@ using namespace gtsam;
typedef PinholeCamera<Cal3_S2> GeneralCamera;
typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
typedef TypedSymbol<Point3, 'l'> PointKey;
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;
typedef GeneralSFMFactor<CameraKey, PointKey> Projection;
typedef NonlinearEquality<CameraKey> CameraConstraint;
typedef NonlinearEquality<PointKey> Point3Constraint;
class Graph: public NonlinearFactorGraph<VisualValues> {
class Graph: public NonlinearFactorGraph {
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));
@ -71,7 +67,7 @@ double getGaussian()
return sqrt(-2.0f * (double)log(S) / S) * V1;
}
typedef NonlinearOptimizer<Graph,VisualValues> Optimizer;
typedef NonlinearOptimizer<Graph> Optimizer;
const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
@ -99,12 +95,12 @@ TEST( GeneralSFMFactor, error ) {
boost::shared_ptr<Projection>
factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
// For the following configuration, the factor predicts 320,240
VisualValues values;
Values values;
Rot3 R;
Point3 t1(0,0,-6);
Pose3 x1(R,t1);
values.insert(1, GeneralCamera(x1));
Point3 l1; values.insert(1, l1);
values.insert(CameraKey(1), GeneralCamera(x1));
Point3 l1; values.insert(PointKey(1), l1);
EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
}
@ -172,15 +168,15 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
graph->addCameraConstraint(0, X[0]);
@ -211,9 +207,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
// add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) {
@ -221,10 +217,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
else {
values->insert(i, L[i]) ;
values->insert(PointKey(i), L[i]) ;
}
}
@ -257,16 +253,16 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
//Point3 pt(L[i].x(), L[i].y(), L[i].z());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
for ( size_t i = 0 ; i < X.size() ; ++i )
@ -300,7 +296,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i ) {
const double
rot_noise = 1e-5,
@ -308,7 +304,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
focal_noise = 1,
skew_noise = 1e-5;
if ( i == 0 ) {
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
}
else {
@ -319,12 +315,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
skew_noise, // s
trans_noise, trans_noise // ux, uy
) ;
values->insert((int)i, X[i].retract(delta)) ;
values->insert(CameraKey((int)i), X[i].retract(delta)) ;
}
}
for ( size_t i = 0 ; i < L.size() ; ++i ) {
values->insert(i, L[i]) ;
values->insert(PointKey(i), L[i]) ;
}
// fix X0 and all landmarks, allow only the X[1] to move
@ -362,16 +358,16 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
// add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
graph->addCameraConstraint(0, X[0]);

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

@ -21,7 +21,6 @@ using namespace boost;
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/slam/GeneralSFMFactor.h>
@ -31,15 +30,12 @@ using namespace gtsam;
typedef PinholeCamera<Cal3Bundler> GeneralCamera;
typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
typedef TypedSymbol<Point3, 'l'> PointKey;
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;
typedef GeneralSFMFactor<CameraKey, PointKey> Projection;
typedef NonlinearEquality<CameraKey> CameraConstraint;
typedef NonlinearEquality<PointKey> Point3Constraint;
/* ************************************************************************* */
class Graph: public NonlinearFactorGraph<VisualValues> {
class Graph: public NonlinearFactorGraph {
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 +68,7 @@ double getGaussian()
return sqrt(-2.0f * (double)log(S) / S) * V1;
}
typedef NonlinearOptimizer<Graph,VisualValues> Optimizer;
typedef NonlinearOptimizer<Graph> Optimizer;
const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
@ -100,12 +96,12 @@ TEST( GeneralSFMFactor, error ) {
boost::shared_ptr<Projection>
factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
// For the following configuration, the factor predicts 320,240
VisualValues values;
Values values;
Rot3 R;
Point3 t1(0,0,-6);
Pose3 x1(R,t1);
values.insert(1, GeneralCamera(x1));
Point3 l1; values.insert(1, l1);
values.insert(CameraKey(1), GeneralCamera(x1));
Point3 l1; values.insert(PointKey(1), l1);
EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
}
@ -173,15 +169,15 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
graph->addCameraConstraint(0, X[0]);
@ -212,9 +208,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
// add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) {
@ -222,10 +218,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
else {
values->insert(i, L[i]) ;
values->insert(PointKey(i), L[i]) ;
}
}
@ -258,16 +254,16 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
//Point3 pt(L[i].x(), L[i].y(), L[i].z());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
for ( size_t i = 0 ; i < X.size() ; ++i )
@ -301,13 +297,13 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
const size_t nMeasurements = L.size()*X.size();
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i ) {
const double
rot_noise = 1e-5, trans_noise = 1e-3,
focal_noise = 1, distort_noise = 1e-3;
if ( i == 0 ) {
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
}
else {
@ -316,12 +312,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
trans_noise, trans_noise, trans_noise, // translation
focal_noise, distort_noise, distort_noise // f, k1, k2
) ;
values->insert((int)i, X[i].retract(delta)) ;
values->insert(CameraKey((int)i), X[i].retract(delta)) ;
}
}
for ( size_t i = 0 ; i < L.size() ; ++i ) {
values->insert(i, L[i]) ;
values->insert(PointKey(i), L[i]) ;
}
// fix X0 and all landmarks, allow only the X[1] to move
@ -358,16 +354,16 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
// add initial
const double noise = baseline*0.1;
boost::shared_ptr<VisualValues> values(new VisualValues);
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert((int)i, X[i]) ;
values->insert(CameraKey((int)i), X[i]) ;
// add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian());
values->insert(i, pt) ;
values->insert(PointKey(i), pt) ;
}
graph->addCameraConstraint(0, X[0]);

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

@ -23,6 +23,7 @@
using namespace std;
using namespace gtsam;
using namespace planarSLAM;
// some shared test values
static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
@ -50,9 +51,9 @@ TEST( planarSLAM, BearingFactor )
planarSLAM::Bearing factor(2, 3, z, sigma);
// create config
planarSLAM::Values c;
c.insert(2, x2);
c.insert(3, l3);
Values c;
c.insert(PoseKey(2), x2);
c.insert(PointKey(3), l3);
// Check error
Vector actual = factor.unwhitenedError(c);
@ -78,9 +79,9 @@ TEST( planarSLAM, RangeFactor )
planarSLAM::Range factor(2, 3, z, sigma);
// create config
planarSLAM::Values c;
c.insert(2, x2);
c.insert(3, l3);
Values c;
c.insert(PoseKey(2), x2);
c.insert(PointKey(3), l3);
// Check error
Vector actual = factor.unwhitenedError(c);
@ -105,9 +106,9 @@ TEST( planarSLAM, BearingRangeFactor )
planarSLAM::BearingRange factor(2, 3, r, b, sigma2);
// create config
planarSLAM::Values c;
c.insert(2, x2);
c.insert(3, l3);
Values c;
c.insert(PoseKey(2), x2);
c.insert(PointKey(3), l3);
// Check error
Vector actual = factor.unwhitenedError(c);
@ -138,10 +139,10 @@ TEST( planarSLAM, PoseConstraint_equals )
TEST( planarSLAM, constructor )
{
// create config
planarSLAM::Values c;
c.insert(2, x2);
c.insert(3, x3);
c.insert(3, l3);
Values c;
c.insert(PoseKey(2), x2);
c.insert(PoseKey(3), x3);
c.insert(PointKey(3), l3);
// create graph
planarSLAM::Graph G;
@ -165,8 +166,8 @@ TEST( planarSLAM, constructor )
Vector expected2 = Vector_(1, -0.1);
Vector expected3 = Vector_(1, 0.22);
// Get NoiseModelFactors
FactorGraph<NoiseModelFactor<planarSLAM::Values> > GNM =
*G.dynamicCastFactors<FactorGraph<NoiseModelFactor<planarSLAM::Values> > >();
FactorGraph<NoiseModelFactor > GNM =
*G.dynamicCastFactors<FactorGraph<NoiseModelFactor > >();
EXPECT(assert_equal(expected0, GNM[0]->unwhitenedError(c)));
EXPECT(assert_equal(expected1, GNM[1]->unwhitenedError(c)));
EXPECT(assert_equal(expected2, GNM[2]->unwhitenedError(c)));

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

@ -118,8 +118,8 @@ TEST( Pose2SLAM, linearization )
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2)
pose2SLAM::Values config;
config.insert(1,p1);
config.insert(2,p2);
config.insert(pose2SLAM::PoseKey(1),p1);
config.insert(pose2SLAM::PoseKey(2),p2);
// Linearize
Ordering ordering(*config.orderingArbitrary());
boost::shared_ptr<FactorGraph<GaussianFactor> > lfg_linearized = graph.linearize(config, ordering);
@ -153,23 +153,23 @@ TEST(Pose2Graph, optimize) {
fg->addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance);
// Create initial config
boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
initial->insert(0, Pose2(0,0,0));
initial->insert(1, Pose2(0,0,0));
boost::shared_ptr<Values> initial(new Values());
initial->insert(pose2SLAM::PoseKey(0), Pose2(0,0,0));
initial->insert(pose2SLAM::PoseKey(1), Pose2(0,0,0));
// Choose an ordering and optimize
shared_ptr<Ordering> ordering(new Ordering);
*ordering += "x0","x1";
typedef NonlinearOptimizer<pose2SLAM::Graph, pose2SLAM::Values> Optimizer;
typedef NonlinearOptimizer<pose2SLAM::Graph> Optimizer;
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15);
Optimizer optimizer0(fg, initial, ordering, params);
Optimizer optimizer = optimizer0.levenbergMarquardt();
// Check with expected config
pose2SLAM::Values expected;
expected.insert(0, Pose2(0,0,0));
expected.insert(1, Pose2(1,2,M_PI_2));
Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0));
expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI_2));
CHECK(assert_equal(expected, *optimizer.values()));
}
@ -178,8 +178,8 @@ TEST(Pose2Graph, optimize) {
TEST(Pose2Graph, optimizeThreePoses) {
// Create a hexagon of poses
pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0);
Pose2 p0 = hexagon[0], p1 = hexagon[1];
Values hexagon = pose2SLAM::circle(3,1.0);
Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)];
// create a Pose graph with one equality constraint and one measurement
shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
@ -190,10 +190,10 @@ TEST(Pose2Graph, optimizeThreePoses) {
fg->addOdometry(2, 0, delta, covariance);
// Create initial config
boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
initial->insert(0, p0);
initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1)));
boost::shared_ptr<Values> initial(new Values());
initial->insert(pose2SLAM::PoseKey(0), p0);
initial->insert(pose2SLAM::PoseKey(1), hexagon[pose2SLAM::PoseKey(1)].retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(pose2SLAM::PoseKey(2), hexagon[pose2SLAM::PoseKey(2)].retract(Vector_(3, 0.1,-0.1, 0.1)));
// Choose an ordering
shared_ptr<Ordering> ordering(new Ordering);
@ -204,7 +204,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
pose2SLAM::Values actual = *optimizer.values();
Values actual = *optimizer.values();
// Check with ground truth
CHECK(assert_equal(hexagon, actual));
@ -215,8 +215,8 @@ TEST(Pose2Graph, optimizeThreePoses) {
TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
// Create a hexagon of poses
pose2SLAM::Values hexagon = pose2SLAM::circle(6,1.0);
Pose2 p0 = hexagon[0], p1 = hexagon[1];
Values hexagon = pose2SLAM::circle(6,1.0);
Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)];
// create a Pose graph with one equality constraint and one measurement
shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
@ -230,13 +230,13 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
fg->addOdometry(5, 0, delta, covariance);
// Create initial config
boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
initial->insert(0, p0);
initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(3, hexagon[3].retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(4, hexagon[4].retract(Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(5, hexagon[5].retract(Vector_(3,-0.1, 0.1,-0.1)));
boost::shared_ptr<Values> initial(new Values());
initial->insert(pose2SLAM::PoseKey(0), p0);
initial->insert(pose2SLAM::PoseKey(1), hexagon[pose2SLAM::PoseKey(1)].retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(pose2SLAM::PoseKey(2), hexagon[pose2SLAM::PoseKey(2)].retract(Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(pose2SLAM::PoseKey(3), hexagon[pose2SLAM::PoseKey(3)].retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(pose2SLAM::PoseKey(4), hexagon[pose2SLAM::PoseKey(4)].retract(Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(pose2SLAM::PoseKey(5), hexagon[pose2SLAM::PoseKey(5)].retract(Vector_(3,-0.1, 0.1,-0.1)));
// Choose an ordering
shared_ptr<Ordering> ordering(new Ordering);
@ -247,13 +247,13 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
pose2SLAM::Values actual = *optimizer.values();
Values actual = *optimizer.values();
// Check with ground truth
CHECK(assert_equal(hexagon, actual));
// Check loop closure
CHECK(assert_equal(delta,actual[5].between(actual[0])));
CHECK(assert_equal(delta,actual[pose2SLAM::PoseKey(5)].between(actual[pose2SLAM::PoseKey(0)])));
// Pose2SLAMOptimizer myOptimizer("3");
@ -281,7 +281,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
//
// myOptimizer.update(x);
//
// pose2SLAM::Values expected;
// Values expected;
// expected.insert(0, Pose2(0.,0.,0.));
// expected.insert(1, Pose2(1.,0.,0.));
// expected.insert(2, Pose2(2.,0.,0.));
@ -315,8 +315,8 @@ TEST(Pose2Graph, optimize2) {
// G.addConstraint(1, 3, Pose2(0.,0.,0.), I3);
// G.addConstraint(2, 3, Pose2(0.,0.,0.), I3);
//
// PredecessorMap<pose2SLAM::Key> tree =
// G.findMinimumSpanningTree<pose2SLAM::Key, Pose2Factor>();
// PredecessorMap<pose2SLAM::pose2SLAM::PoseKey> tree =
// G.findMinimumSpanningTree<pose2SLAM::pose2SLAM::PoseKey, Pose2Factor>();
// CHECK(tree[1] == 1);
// CHECK(tree[2] == 1);
// CHECK(tree[3] == 1);
@ -329,12 +329,12 @@ TEST(Pose2Graph, optimize2) {
// G.addConstraint(1, 3, Pose2(0.,0.,0.), I3);
// G.addConstraint(2, 3, Pose2(0.,0.,0.), I3);
//
// PredecessorMap<pose2SLAM::Key> tree;
// PredecessorMap<pose2SLAM::pose2SLAM::PoseKey> tree;
// tree.insert(1,2);
// tree.insert(2,2);
// tree.insert(3,2);
//
// G.split<pose2SLAM::Key, Pose2Factor>(tree, T, C);
// G.split<pose2SLAM::pose2SLAM::PoseKey, Pose2Factor>(tree, T, C);
// LONGS_EQUAL(2, T.size());
// LONGS_EQUAL(1, C.size());
//}
@ -345,13 +345,13 @@ using namespace pose2SLAM;
TEST(Pose2Values, pose2Circle )
{
// expected is 4 poses tangent to circle with radius 1m
pose2SLAM::Values expected;
expected.insert(0, Pose2( 1, 0, M_PI_2));
expected.insert(1, Pose2( 0, 1, - M_PI ));
expected.insert(2, Pose2(-1, 0, - M_PI_2));
expected.insert(3, Pose2( 0, -1, 0 ));
Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2( 1, 0, M_PI_2));
expected.insert(pose2SLAM::PoseKey(1), Pose2( 0, 1, - M_PI ));
expected.insert(pose2SLAM::PoseKey(2), Pose2(-1, 0, - M_PI_2));
expected.insert(pose2SLAM::PoseKey(3), Pose2( 0, -1, 0 ));
pose2SLAM::Values actual = pose2SLAM::circle(4,1.0);
Values actual = pose2SLAM::circle(4,1.0);
CHECK(assert_equal(expected,actual));
}
@ -359,21 +359,21 @@ TEST(Pose2Values, pose2Circle )
TEST(Pose2SLAM, expmap )
{
// expected is circle shifted to right
pose2SLAM::Values expected;
expected.insert(0, Pose2( 1.1, 0, M_PI_2));
expected.insert(1, Pose2( 0.1, 1, - M_PI ));
expected.insert(2, Pose2(-0.9, 0, - M_PI_2));
expected.insert(3, Pose2( 0.1, -1, 0 ));
Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2( 1.1, 0, M_PI_2));
expected.insert(pose2SLAM::PoseKey(1), Pose2( 0.1, 1, - M_PI ));
expected.insert(pose2SLAM::PoseKey(2), Pose2(-0.9, 0, - M_PI_2));
expected.insert(pose2SLAM::PoseKey(3), Pose2( 0.1, -1, 0 ));
// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
pose2SLAM::Values circle(pose2SLAM::circle(4,1.0));
Values circle(pose2SLAM::circle(4,1.0));
Ordering ordering(*circle.orderingArbitrary());
VectorValues delta(circle.dims(ordering));
delta[ordering[PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0);
delta[ordering[PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0);
delta[ordering[PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0);
delta[ordering[PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0);
pose2SLAM::Values actual = circle.retract(delta, ordering);
delta[ordering[pose2SLAM::PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0);
delta[ordering[pose2SLAM::PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0);
delta[ordering[pose2SLAM::PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0);
delta[ordering[pose2SLAM::PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0);
Values actual = circle.retract(delta, ordering);
CHECK(assert_equal(expected,actual));
}
@ -386,8 +386,8 @@ TEST( Pose2Prior, error )
{
// Choose a linearization point
Pose2 p1(1, 0, 0); // robot at (1,0)
pose2SLAM::Values x0;
x0.insert(1, p1);
Values x0;
x0.insert(pose2SLAM::PoseKey(1), p1);
// Create factor
pose2SLAM::Prior factor(1, p1, sigmas);
@ -408,7 +408,7 @@ TEST( Pose2Prior, error )
VectorValues addition(VectorValues::Zero(x0.dims(ordering)));
addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0);
VectorValues plus = delta + addition;
pose2SLAM::Values x1 = x0.retract(plus, ordering);
Values x1 = x0.retract(plus, ordering);
Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@ -430,8 +430,8 @@ LieVector hprior(const Pose2& p1) {
TEST( Pose2Prior, linearize )
{
// Choose a linearization point at ground truth
pose2SLAM::Values x0;
x0.insert(1,priorVal);
Values x0;
x0.insert(pose2SLAM::PoseKey(1),priorVal);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
@ -450,9 +450,9 @@ TEST( Pose2Factor, error )
// Choose a linearization point
Pose2 p1; // robot at origin
Pose2 p2(1, 0, 0); // robot at (1,0)
pose2SLAM::Values x0;
x0.insert(1, p1);
x0.insert(2, p2);
Values x0;
x0.insert(pose2SLAM::PoseKey(1), p1);
x0.insert(pose2SLAM::PoseKey(2), p2);
// Create factor
Pose2 z = p1.between(p2);
@ -474,7 +474,7 @@ TEST( Pose2Factor, error )
// Check error after increasing p2
VectorValues plus = delta;
plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0);
pose2SLAM::Values x1 = x0.retract(plus, ordering);
Values x1 = x0.retract(plus, ordering);
Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@ -491,9 +491,9 @@ TEST( Pose2Factor, rhs )
// Choose a linearization point
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4)
pose2SLAM::Values x0;
x0.insert(1,p1);
x0.insert(2,p2);
Values x0;
x0.insert(pose2SLAM::PoseKey(1),p1);
x0.insert(pose2SLAM::PoseKey(2),p2);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
@ -521,9 +521,9 @@ TEST( Pose2Factor, linearize )
// Choose a linearization point at ground truth
Pose2 p1(1,2,M_PI_2);
Pose2 p2(-1,4,M_PI);
pose2SLAM::Values x0;
x0.insert(1,p1);
x0.insert(2,p2);
Values x0;
x0.insert(pose2SLAM::PoseKey(1),p1);
x0.insert(pose2SLAM::PoseKey(2),p2);
// expected linearization
Matrix expectedH1 = covariance->Whiten(Matrix_(3,3,

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

@ -36,6 +36,7 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
using namespace pose3SLAM;
// common measurement covariance
static Matrix covariance = eye(6);
@ -48,8 +49,8 @@ TEST(Pose3Graph, optimizeCircle) {
// Create a hexagon of poses
double radius = 10;
Pose3Values hexagon = pose3SLAM::circle(6,radius);
Pose3 gT0 = hexagon[0], gT1 = hexagon[1];
Values hexagon = pose3SLAM::circle(6,radius);
Pose3 gT0 = hexagon[Key(0)], gT1 = hexagon[Key(1)];
// create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose3Graph> fg(new Pose3Graph);
@ -65,13 +66,13 @@ TEST(Pose3Graph, optimizeCircle) {
fg->addConstraint(5,0, _0T1, covariance);
// Create initial config
boost::shared_ptr<Pose3Values> initial(new Pose3Values());
initial->insert(0, gT0);
initial->insert(1, hexagon[1].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(2, hexagon[2].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(3, hexagon[3].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(4, hexagon[4].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(5, hexagon[5].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
boost::shared_ptr<Values> initial(new Values());
initial->insert(Key(0), gT0);
initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(Key(3), hexagon[Key(3)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(Key(4), hexagon[Key(4)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(Key(5), hexagon[Key(5)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
// Choose an ordering and optimize
shared_ptr<Ordering> ordering(new Ordering);
@ -80,18 +81,18 @@ TEST(Pose3Graph, optimizeCircle) {
pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params);
pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
Pose3Values actual = *optimizer.values();
Values actual = *optimizer.values();
// Check with ground truth
CHECK(assert_equal(hexagon, actual,1e-4));
// Check loop closure
CHECK(assert_equal(_0T1,actual[5].between(actual[0]),1e-5));
CHECK(assert_equal(_0T1,actual[Key(5)].between(actual[Key(0)]),1e-5));
}
/* ************************************************************************* */
TEST(Pose3Graph, partial_prior_height) {
typedef PartialPriorFactor<pose3SLAM::Values, pose3SLAM::Key> Partial;
typedef PartialPriorFactor<pose3SLAM::Key> Partial;
// reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3)
@ -109,13 +110,13 @@ TEST(Pose3Graph, partial_prior_height) {
pose3SLAM::Graph graph;
graph.add(height);
pose3SLAM::Values values;
Values values;
values.insert(key, init);
// linearization
EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
EXPECT(assert_equal(expected, actual[key], tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
}
@ -133,9 +134,9 @@ TEST( Pose3Factor, error )
Pose3Factor factor(1,2, z, I6);
// Create config
Pose3Values x;
x.insert(1,t1);
x.insert(2,t2);
Values x;
x.insert(Key(1),t1);
x.insert(Key(2),t2);
// Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2))
Vector actual = factor.unwhitenedError(x);
@ -145,7 +146,7 @@ TEST( Pose3Factor, error )
/* ************************************************************************* */
TEST(Pose3Graph, partial_prior_xy) {
typedef PartialPriorFactor<pose3SLAM::Values, pose3SLAM::Key> Partial;
typedef PartialPriorFactor<pose3SLAM::Key> Partial;
// XY prior - full mask interface
pose3SLAM::Key key(1);
@ -164,10 +165,10 @@ TEST(Pose3Graph, partial_prior_xy) {
pose3SLAM::Graph graph;
graph.add(priorXY);
pose3SLAM::Values values;
Values values;
values.insert(key, init);
pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
EXPECT(assert_equal(expected, actual[key], tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
}
@ -180,27 +181,27 @@ Rot3 R3(Point3( 0,-1, 0), Point3(-1, 0, 0), Point3(0, 0, -1));
Rot3 R4(Point3( 1, 0, 0), Point3( 0,-1, 0), Point3(0, 0, -1));
/* ************************************************************************* */
TEST( Pose3Values, pose3Circle )
TEST( Values, pose3Circle )
{
// expected is 4 poses tangent to circle with radius 1m
Pose3Values expected;
expected.insert(0, Pose3(R1, Point3( 1, 0, 0)));
expected.insert(1, Pose3(R2, Point3( 0, 1, 0)));
expected.insert(2, Pose3(R3, Point3(-1, 0, 0)));
expected.insert(3, Pose3(R4, Point3( 0,-1, 0)));
Values expected;
expected.insert(Key(0), Pose3(R1, Point3( 1, 0, 0)));
expected.insert(Key(1), Pose3(R2, Point3( 0, 1, 0)));
expected.insert(Key(2), Pose3(R3, Point3(-1, 0, 0)));
expected.insert(Key(3), Pose3(R4, Point3( 0,-1, 0)));
Pose3Values actual = pose3SLAM::circle(4,1.0);
Values actual = pose3SLAM::circle(4,1.0);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( Pose3Values, expmap )
TEST( Values, expmap )
{
Pose3Values expected;
expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0)));
expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0)));
expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0)));
expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0)));
Values expected;
expected.insert(Key(0), Pose3(R1, Point3( 1.0, 0.1, 0)));
expected.insert(Key(1), Pose3(R2, Point3(-0.1, 1.0, 0)));
expected.insert(Key(2), Pose3(R3, Point3(-1.0,-0.1, 0)));
expected.insert(Key(3), Pose3(R4, Point3( 0.1,-1.0, 0)));
Ordering ordering(*expected.orderingArbitrary());
VectorValues delta(expected.dims(ordering));
@ -209,7 +210,7 @@ TEST( Pose3Values, expmap )
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0);
Pose3Values actual = pose3SLAM::circle(4,1.0).retract(delta, ordering);
Values actual = pose3SLAM::circle(4,1.0).retract(delta, ordering);
CHECK(assert_equal(expected,actual));
}

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

@ -25,6 +25,7 @@
using namespace std;
using namespace gtsam;
using namespace visualSLAM;
// make cube
static Point3
@ -51,9 +52,9 @@ TEST( ProjectionFactor, error )
factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
// For the following values structure, the factor predicts 320,240
visualSLAM::Values config;
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1);
Point3 l1; config.insert(1, l1);
Values config;
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(PoseKey(1), x1);
Point3 l1; config.insert(PointKey(1), l1);
// Point should project to Point2(320.,240.)
CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(config)));
@ -80,13 +81,13 @@ TEST( ProjectionFactor, error )
CHECK(assert_equal(expected_lfg,*actual_lfg));
// expmap on a config
visualSLAM::Values expected_config;
Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2);
Point3 l2(1,2,3); expected_config.insert(1, l2);
Values expected_config;
Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(PoseKey(1), x2);
Point3 l2(1,2,3); expected_config.insert(PointKey(1), l2);
VectorValues delta(expected_config.dims(ordering));
delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.);
delta[ordering["l1"]] = Vector_(3, 1.,2.,3.);
visualSLAM::Values actual_config = config.retract(delta, ordering);
Values actual_config = config.retract(delta, ordering);
CHECK(assert_equal(expected_config,actual_config,1e-9));
}

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

@ -30,9 +30,9 @@ using namespace simulated2D;
/* ************************************************************************* */
TEST( simulated2D, Simulated2DValues )
{
simulated2D::Values actual;
actual.insertPose(1,Point2(1,1));
actual.insertPoint(2,Point2(2,2));
Values actual;
actual.insert(simulated2D::PoseKey(1),Point2(1,1));
actual.insert(simulated2D::PointKey(2),Point2(2,2));
EXPECT(assert_equal(actual,actual,1e-9));
}

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

@ -57,7 +57,7 @@ TEST( simulated2DOriented, constructor )
SharedDiagonal model(Vector_(3, 1., 1., 1.));
simulated2DOriented::Odometry factor(measurement, model, simulated2DOriented::PoseKey(1), simulated2DOriented::PoseKey(2));
simulated2DOriented::Values config;
Values config;
config.insert(simulated2DOriented::PoseKey(1), Pose2(1., 0., 0.2));
config.insert(simulated2DOriented::PoseKey(2), Pose2(2., 0., 0.1));
boost::shared_ptr<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary());

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

@ -29,7 +29,7 @@ using namespace simulated3D;
/* ************************************************************************* */
TEST( simulated3D, Values )
{
simulated3D::Values actual;
Values actual;
actual.insert(simulated3D::PointKey(1),Point3(1,1,1));
actual.insert(simulated3D::PoseKey(2),Point3(2,2,2));
EXPECT(assert_equal(actual,actual,1e-9));

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

@ -29,6 +29,7 @@
using namespace std;
using namespace gtsam;
using namespace boost;
using namespace visualSLAM;
Pose3 camera1(Matrix_(3,3,
1., 0., 0.,
@ -58,15 +59,15 @@ TEST( StereoFactor, singlePoint)
graph->add(visualSLAM::StereoFactor(z14,sigma, 1, 1, K));
// Create a configuration corresponding to the ground truth
boost::shared_ptr<visualSLAM::Values> values(new visualSLAM::Values());
values->insert(1, camera1); // add camera at z=6.25m looking towards origin
boost::shared_ptr<Values> values(new Values());
values->insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin
Point3 l1(0, 0, 0);
values->insert(1, l1); // add point at origin;
values->insert(PointKey(1), l1); // add point at origin;
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
typedef gtsam::NonlinearOptimizer<visualSLAM::Graph,visualSLAM::Values,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> Optimizer; // optimization engine for this domain
typedef gtsam::NonlinearOptimizer<visualSLAM::Graph, gtsam::GaussianFactorGraph, gtsam::GaussianMultifrontalSolver> Optimizer; // optimization engine for this domain
double absoluteThreshold = 1e-9;
double relativeThreshold = 1e-5;

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

@ -32,6 +32,7 @@ using namespace boost;
using namespace std;
using namespace gtsam;
using namespace boost;
using namespace visualSLAM;
static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
@ -90,13 +91,13 @@ TEST( Graph, optimizeLM)
graph->addPointConstraint(3, landmark3);
// Create an initial values structure corresponding to the ground truth
boost::shared_ptr<visualSLAM::Values> initialEstimate(new visualSLAM::Values);
initialEstimate->insert(1, camera1);
initialEstimate->insert(2, camera2);
initialEstimate->insert(1, landmark1);
initialEstimate->insert(2, landmark2);
initialEstimate->insert(3, landmark3);
initialEstimate->insert(4, landmark4);
boost::shared_ptr<Values> initialEstimate(new Values);
initialEstimate->insert(PoseKey(1), camera1);
initialEstimate->insert(PoseKey(2), camera2);
initialEstimate->insert(PointKey(1), landmark1);
initialEstimate->insert(PointKey(2), landmark2);
initialEstimate->insert(PointKey(3), landmark3);
initialEstimate->insert(PointKey(4), landmark4);
// Create an ordering of the variables
shared_ptr<Ordering> ordering(new Ordering);
@ -127,13 +128,13 @@ TEST( Graph, optimizeLM2)
graph->addPoseConstraint(2, camera2);
// Create an initial values structure corresponding to the ground truth
boost::shared_ptr<visualSLAM::Values> initialEstimate(new visualSLAM::Values);
initialEstimate->insert(1, camera1);
initialEstimate->insert(2, camera2);
initialEstimate->insert(1, landmark1);
initialEstimate->insert(2, landmark2);
initialEstimate->insert(3, landmark3);
initialEstimate->insert(4, landmark4);
boost::shared_ptr<Values> initialEstimate(new Values);
initialEstimate->insert(PoseKey(1), camera1);
initialEstimate->insert(PoseKey(2), camera2);
initialEstimate->insert(PointKey(1), landmark1);
initialEstimate->insert(PointKey(2), landmark2);
initialEstimate->insert(PointKey(3), landmark3);
initialEstimate->insert(PointKey(4), landmark4);
// Create an ordering of the variables
shared_ptr<Ordering> ordering(new Ordering);
@ -164,13 +165,13 @@ TEST( Graph, CHECK_ORDERING)
graph->addPoseConstraint(2, camera2);
// Create an initial values structure corresponding to the ground truth
boost::shared_ptr<visualSLAM::Values> initialEstimate(new visualSLAM::Values);
initialEstimate->insert(1, camera1);
initialEstimate->insert(2, camera2);
initialEstimate->insert(1, landmark1);
initialEstimate->insert(2, landmark2);
initialEstimate->insert(3, landmark3);
initialEstimate->insert(4, landmark4);
boost::shared_ptr<Values> initialEstimate(new Values);
initialEstimate->insert(PoseKey(1), camera1);
initialEstimate->insert(PoseKey(2), camera2);
initialEstimate->insert(PointKey(1), landmark1);
initialEstimate->insert(PointKey(2), landmark2);
initialEstimate->insert(PointKey(3), landmark3);
initialEstimate->insert(PointKey(4), landmark4);
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initialEstimate);
@ -192,23 +193,23 @@ TEST( Graph, CHECK_ORDERING)
TEST( Values, update_with_large_delta) {
// this test ensures that if the update for delta is larger than
// the size of the config, it only updates existing variables
visualSLAM::Values init;
init.insert(1, Pose3());
init.insert(1, Point3(1.0, 2.0, 3.0));
Values init;
init.insert(PoseKey(1), Pose3());
init.insert(PointKey(1), Point3(1.0, 2.0, 3.0));
visualSLAM::Values expected;
expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
expected.insert(1, Point3(1.1, 2.1, 3.1));
Values expected;
expected.insert(PoseKey(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
expected.insert(PointKey(1), Point3(1.1, 2.1, 3.1));
Ordering largeOrdering;
visualSLAM::Values largeValues = init;
largeValues.insert(2, Pose3());
Values largeValues = init;
largeValues.insert(PoseKey(2), Pose3());
largeOrdering += "x1","l1","x2";
VectorValues delta(largeValues.dims(largeOrdering));
delta[largeOrdering["x1"]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1);
delta[largeOrdering["l1"]] = Vector_(3, 0.1, 0.1, 0.1);
delta[largeOrdering["x2"]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1);
visualSLAM::Values actual = init.retract(delta, largeOrdering);
Values actual = init.retract(delta, largeOrdering);
CHECK(assert_equal(expected,actual));
}

26
gtsam/slam/visualSLAM.h
View File

@ -25,7 +25,6 @@
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/geometry/SimpleCamera.h>
@ -40,25 +39,22 @@ namespace visualSLAM {
*/
typedef TypedSymbol<Pose3,'x'> PoseKey; ///< The key type used for poses
typedef TypedSymbol<Point3,'l'> PointKey; ///< The key type 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
typedef NonlinearEquality<Values, PoseKey> PoseConstraint; ///< put a hard constraint on a pose
typedef NonlinearEquality<Values, PointKey> PointConstraint; ///< put a hard constraint on a point
typedef PriorFactor<Values, PoseKey> PosePrior; ///< put a soft prior on a Pose
typedef PriorFactor<Values, PointKey> PointPrior; ///< put a soft prior on a point
typedef RangeFactor<Values, PoseKey, PointKey> RangeFactor; ///< put a factor on the range from a pose to a point
typedef NonlinearEquality<PoseKey> PoseConstraint; ///< put a hard constraint on a pose
typedef NonlinearEquality<PointKey> PointConstraint; ///< put a hard constraint on a point
typedef PriorFactor<PoseKey> PosePrior; ///< put a soft prior on a Pose
typedef PriorFactor<PointKey> PointPrior; ///< put a soft prior on a point
typedef RangeFactor<PoseKey, PointKey> RangeFactor; ///< put a factor on the range from a pose to a point
/// monocular and stereo camera typedefs for general use
typedef GenericProjectionFactor<Values, PointKey, PoseKey> ProjectionFactor;
typedef GenericStereoFactor<Values, PoseKey, PointKey> StereoFactor;
typedef GenericProjectionFactor<PointKey, PoseKey> ProjectionFactor;
typedef GenericStereoFactor<PoseKey, PointKey> StereoFactor;
/**
* Non-linear factor graph for vanilla visual SLAM
*/
class Graph: public NonlinearFactorGraph<Values> {
class Graph: public NonlinearFactorGraph {
public:
/// shared pointer to this type of graph
@ -70,12 +66,12 @@ namespace visualSLAM {
/// print out graph
void print(const std::string& s = "") const {
NonlinearFactorGraph<Values>::print(s);
NonlinearFactorGraph::print(s);
}
/// check if two graphs are equal
bool equals(const Graph& p, double tol = 1e-9) const {
return NonlinearFactorGraph<Values>::equals(p, tol);
return NonlinearFactorGraph::equals(p, tol);
}
/**
@ -148,6 +144,6 @@ namespace visualSLAM {
}; // Graph
/// typedef for Optimizer. The current default will use the multi-frontal solver
typedef NonlinearOptimizer<Graph, Values> Optimizer;
typedef NonlinearOptimizer<Graph> Optimizer;
} // namespaces

4
tests/Makefile.am
View File

@ -16,10 +16,10 @@ check_PROGRAMS += testGaussianJunctionTree
check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph
check_PROGRAMS += testNonlinearOptimizer testDoglegOptimizer
check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph
check_PROGRAMS += testTupleValues
#check_PROGRAMS += testTupleValues
check_PROGRAMS += testNonlinearISAM
check_PROGRAMS += testBoundingConstraint
check_PROGRAMS += testPose2SLAMwSPCG
#check_PROGRAMS += testPose2SLAMwSPCG
check_PROGRAMS += testGaussianISAM2
check_PROGRAMS += testExtendedKalmanFilter
check_PROGRAMS += testRot3Optimization

Some files were not shown because too many files have changed in this diff Show More