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

All unit tests pass with nonlinear factors templated on value instead of key

release/4.3a0
Richard Roberts 2012-02-07 04:02:20 +00:00
parent eaa9e4d739
commit 2f7f535f34
32 changed files with 283 additions and 322 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
examples/CameraResectioning.cpp
View File

@ -26,28 +26,25 @@
using namespace gtsam;
typedef TypedSymbol<Pose3, 'x'> PoseKey;
/**
* Unary factor for the pose.
*/
class ResectioningFactor: public NonlinearFactor1<PoseKey> {
typedef NonlinearFactor1<PoseKey> Base;
class ResectioningFactor: public NonlinearFactor1<Pose3> {
typedef NonlinearFactor1<Pose3> Base;
shared_ptrK K_; // camera's intrinsic parameters
Point3 P_; // 3D point on the calibration rig
Point2 p_; // 2D measurement of the 3D point
public:
ResectioningFactor(const SharedNoiseModel& model, const PoseKey& key,
ResectioningFactor(const SharedNoiseModel& model, const Symbol& key,
const shared_ptrK& calib, const Point2& p, const Point3& P) :
Base(model, key), K_(calib), P_(P), p_(p) {
}
virtual ~ResectioningFactor() {}
virtual Vector evaluateError(const Pose3& X, boost::optional<Matrix&> H =
boost::none) const {
virtual Vector evaluateError(const Pose3& X, boost::optional<Matrix&> H = boost::none) const {
SimpleCamera camera(*K_, X);
Point2 reprojectionError(camera.project(P_, H) - p_);
return reprojectionError.vector();
@ -69,7 +66,7 @@ int main(int argc, char* argv[]) {
/* create keys for variables */
// we have only 1 variable to solve: the camera pose
PoseKey X(1);
Symbol X('x',1);
/* 1. create graph */
NonlinearFactorGraph graph;

29
examples/PlanarSLAMExample_easy.cpp
View File

@ -36,25 +36,22 @@ using namespace planarSLAM;
* - Landmarks are 2 meters away from the robot trajectory
*/
int main(int argc, char** argv) {
// create keys for variables
PoseKey x1(1), x2(2), x3(3);
PointKey l1(1), l2(2);
// create graph container and add factors to it
// create graph container and add factors to it
Graph graph;
/* add prior */
// 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
graph.addPrior(x1, prior_measurement, prior_model); // add directly to graph
graph.addPrior(1, prior_measurement, prior_model); // add directly to graph
/* add odometry */
// general noisemodel for odometry
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)
graph.addOdometry(x1, x2, odom_measurement, odom_model);
graph.addOdometry(x2, x3, odom_measurement, odom_model);
graph.addOdometry(1, 2, odom_measurement, odom_model);
graph.addOdometry(2, 3, odom_measurement, odom_model);
/* add measurements */
// general noisemodel for measurements
@ -69,24 +66,24 @@ int main(int argc, char** argv) {
range32 = 2.0;
// create bearing/range factors and add them
graph.addBearingRange(x1, l1, bearing11, range11, meas_model);
graph.addBearingRange(x2, l1, bearing21, range21, meas_model);
graph.addBearingRange(x3, l2, bearing32, range32, meas_model);
graph.addBearingRange(1, 1, bearing11, range11, meas_model);
graph.addBearingRange(2, 1, bearing21, range21, meas_model);
graph.addBearingRange(3, 2, bearing32, range32, meas_model);
graph.print("full graph");
// initialize to noisy points
planarSLAM::Values initialEstimate;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insert(l1, Point2(1.8, 2.1));
initialEstimate.insert(l2, Point2(4.1, 1.8));
initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insertPoint(1, Point2(1.8, 2.1));
initialEstimate.insertPoint(2, Point2(4.1, 1.8));
initialEstimate.print("initial estimate");
// optimize using Levenberg-Marquardt optimization with an ordering from colamd
planarSLAM::Values result = optimize<Graph>(graph, initialEstimate);
planarSLAM::Values result = optimize(graph, initialEstimate);
result.print("final result");
return 0;

25
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -40,11 +40,8 @@
#include <gtsam/linear/GaussianMultifrontalSolver.h>
// 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::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
typedef gtsam::NonlinearOptimizer<gtsam::NonlinearFactorGraph,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential; // optimization engine for this domain
typedef gtsam::NonlinearOptimizer<gtsam::NonlinearFactorGraph,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal; // optimization engine for this domain
using namespace std;
using namespace gtsam;
@ -60,17 +57,17 @@ using namespace gtsam;
*/
int main(int argc, char** argv) {
// create keys for variables
PoseKey x1(1), x2(2), x3(3);
PointKey l1(1), l2(2);
Symbol x1('x',1), x2('x',2), x3('x',3);
Symbol l1('l',1), l2('l',2);
// create graph container and add factors to it
Graph::shared_ptr graph(new Graph);
NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
/* add prior */
// 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<PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor
PriorFactor<Pose2> posePrior(x1, prior_measurement, prior_model); // create the factor
graph->add(posePrior); // add the factor to the graph
/* add odometry */
@ -78,8 +75,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<PoseKey> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<PoseKey> odom23(x2, x3, odom_measurement, odom_model);
BetweenFactor<Pose2> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<Pose2> odom23(x2, x3, odom_measurement, odom_model);
graph->add(odom12); // add both to graph
graph->add(odom23);
@ -96,9 +93,9 @@ int main(int argc, char** argv) {
range32 = 2.0;
// create bearing/range factors
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);
BearingRangeFactor<Pose2, Point2> meas11(x1, l1, bearing11, range11, meas_model);
BearingRangeFactor<Pose2, Point2> meas21(x2, l1, bearing21, range21, meas_model);
BearingRangeFactor<Pose2, Point2> meas32(x3, l2, bearing32, range32, meas_model);
// add the factors
graph->add(meas11);

19
examples/Pose2SLAMExample_advanced.cpp
View File

@ -33,9 +33,6 @@ using namespace boost;
using namespace pose2SLAM;
int main(int argc, char** argv) {
// create keys for robot positions
PoseKey x1(1), x2(2), x3(3);
/* 1. create graph container and add factors to it */
shared_ptr<Graph> graph(new Graph);
@ -43,7 +40,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
graph->addPrior(x1, prior_measurement, prior_model); // add directly to graph
graph->addPrior(1, prior_measurement, prior_model); // add directly to graph
/* 2.b add odometry */
// general noisemodel for odometry
@ -51,16 +48,16 @@ int main(int argc, char** argv) {
/* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph->addOdometry(x1, x2, odom_measurement, odom_model);
graph->addOdometry(x2, x3, odom_measurement, odom_model);
graph->addOdometry(1, 2, odom_measurement, odom_model);
graph->addOdometry(2, 3, odom_measurement, odom_model);
graph->print("full graph");
/* 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);
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));
initial->insertPose(1, Pose2(0.5, 0.0, 0.2));
initial->insertPose(2, Pose2(2.3, 0.1,-0.2));
initial->insertPose(3, Pose2(4.1, 0.1, 0.1));
initial->print("initial estimate");
/* 4.2.1 Alternatively, you can go through the process step by step
@ -76,8 +73,8 @@ int main(int argc, char** argv) {
result.print("final result");
/* Get covariances */
Matrix covariance1 = optimizer_result.marginalCovariance(x1);
Matrix covariance2 = optimizer_result.marginalCovariance(x2);
Matrix covariance1 = optimizer_result.marginalCovariance(PoseKey(1));
Matrix covariance2 = optimizer_result.marginalCovariance(PoseKey(1));
print(covariance1, "Covariance1");
print(covariance2, "Covariance2");

14
examples/Pose2SLAMExample_easy.cpp
View File

@ -31,8 +31,6 @@ using namespace gtsam;
using namespace pose2SLAM;
int main(int argc, char** argv) {
// create keys for robot positions
PoseKey x1(1), x2(2), x3(3);
/* 1. create graph container and add factors to it */
Graph graph ;
@ -41,7 +39,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
graph.addPrior(x1, prior_measurement, prior_model); // add directly to graph
graph.addPrior(1, prior_measurement, prior_model); // add directly to graph
/* 2.b add odometry */
// general noisemodel for odometry
@ -49,16 +47,16 @@ int main(int argc, char** argv) {
/* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph.addOdometry(x1, x2, odom_measurement, odom_model);
graph.addOdometry(x2, x3, odom_measurement, odom_model);
graph.addOdometry(1, 2, odom_measurement, odom_model);
graph.addOdometry(2, 3, odom_measurement, odom_model);
graph.print("full graph");
/* 3. Create the data structure to hold the initial estinmate to the solution
* initialize to noisy points */
pose2SLAM::Values initial;
initial.insert(x1, Pose2(0.5, 0.0, 0.2));
initial.insert(x2, Pose2(2.3, 0.1,-0.2));
initial.insert(x3, Pose2(4.1, 0.1, 0.1));
initial.insertPose(1, Pose2(0.5, 0.0, 0.2));
initial.insertPose(2, Pose2(2.3, 0.1,-0.2));
initial.insertPose(3, Pose2(4.1, 0.1, 0.1));
initial.print("initial estimate");
/* 4 Single Step Optimization

35
examples/SimpleRotation.cpp
View File

@ -35,25 +35,6 @@
using namespace std;
using namespace gtsam;
/**
* Step 1: Setup basic types for optimization of a single variable type
* This can be considered to be specifying the domain of the problem we wish
* to solve. In this case, we will create a very simple domain that operates
* on variables of a specific type, in this case, Rot2.
*
* To create a domain:
* - variable types need to have a key defined to act as a label in graphs
* - a "RotValues" structure needs to be defined to store the system state
* - a graph container acting on a given RotValues
*
* In a typical scenario, these typedefs could be placed in a header
* file and reused between projects. Also, RotValues can be combined to
* form a "TupleValues" to enable multiple variable types, such as 2D points
* and 2D poses.
*/
typedef TypedSymbol<Rot2, 'x'> Key; /// Variable labels for a specific type
typedef NonlinearFactorGraph Graph; /// Graph container for constraints - needs to know type of variables
const double degree = M_PI / 180;
int main() {
@ -64,7 +45,7 @@ int main() {
*/
/**
* Step 2: create a factor on to express a unary constraint
* Step 1: create a factor on to express a unary constraint
* The "prior" in this case is the measurement from a sensor,
* with a model of the noise on the measurement.
*
@ -80,11 +61,11 @@ int main() {
Rot2 prior = Rot2::fromAngle(30 * degree);
prior.print("goal angle");
SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree);
Key key(1);
PriorFactor<Key> factor(key, prior, model);
Symbol key('x',1);
PriorFactor<Rot2> factor(key, prior, model);
/**
* Step 3: create a graph container and add the factor to it
* Step 2: create a graph container and add the factor to it
* Before optimizing, all factors need to be added to a Graph container,
* which provides the necessary top-level functionality for defining a
* system of constraints.
@ -92,12 +73,12 @@ int main() {
* In this case, there is only one factor, but in a practical scenario,
* many more factors would be added.
*/
Graph graph;
NonlinearFactorGraph graph;
graph.add(factor);
graph.print("full graph");
/**
* Step 4: create an initial estimate
* Step 3: create an initial estimate
* An initial estimate of the solution for the system is necessary to
* start optimization. This system state is the "RotValues" structure,
* which is similar in structure to a STL map, in that it maps
@ -117,14 +98,14 @@ int main() {
initial.print("initial estimate");
/**
* Step 5: optimize
* Step 4: optimize
* After formulating the problem with a graph of constraints
* and an initial estimate, executing optimization is as simple
* as calling a general optimization function with the graph and
* initial estimate. This will yield a new RotValues structure
* with the final state of the optimization.
*/
Values result = optimize<Graph>(graph, initial);
Values result = optimize(graph, initial);
result.print("final result");
return 0;

21
examples/easyPoint2KalmanFilter.cpp
View File

@ -30,7 +30,6 @@ using namespace std;
using namespace gtsam;
// Define Types for Linear System Test
typedef TypedSymbol<Point2, 'x'> LinearKey;
typedef Point2 LinearMeasurement;
int main() {
@ -40,7 +39,7 @@ int main() {
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
// Create an ExtendedKalmanFilter object
ExtendedKalmanFilter<LinearKey> ekf(x_initial, P_initial);
ExtendedKalmanFilter<Point2> ekf(x_initial, P_initial);
@ -61,11 +60,11 @@ int main() {
// This simple motion can be modeled with a BetweenFactor
// Create Keys
LinearKey x0(0), x1(1);
Symbol x0('x',0), x1('x',1);
// Predict delta based on controls
Point2 difference(1,0);
// Create Factor
BetweenFactor<LinearKey> factor1(x0, x1, difference, Q);
BetweenFactor<Point2> factor1(x0, x1, difference, Q);
// Predict the new value with the EKF class
Point2 x1_predict = ekf.predict(factor1);
@ -86,7 +85,7 @@ int main() {
// This simple measurement can be modeled with a PriorFactor
Point2 z1(1.0, 0.0);
PriorFactor<LinearKey> factor2(x1, z1, R);
PriorFactor<Point2> factor2(x1, z1, R);
// Update the Kalman Filter with the measurement
Point2 x1_update = ekf.update(factor2);
@ -96,15 +95,15 @@ int main() {
// Do the same thing two more times...
// Predict
LinearKey x2(2);
Symbol x2('x',2);
difference = Point2(1,0);
BetweenFactor<LinearKey> factor3(x1, x2, difference, Q);
BetweenFactor<Point2> factor3(x1, x2, difference, Q);
Point2 x2_predict = ekf.predict(factor1);
x2_predict.print("X2 Predict");
// Update
Point2 z2(2.0, 0.0);
PriorFactor<LinearKey> factor4(x2, z2, R);
PriorFactor<Point2> factor4(x2, z2, R);
Point2 x2_update = ekf.update(factor4);
x2_update.print("X2 Update");
@ -112,15 +111,15 @@ int main() {
// Do the same thing one more time...
// Predict
LinearKey x3(3);
Symbol x3('x',3);
difference = Point2(1,0);
BetweenFactor<LinearKey> factor5(x2, x3, difference, Q);
BetweenFactor<Point2> factor5(x2, x3, difference, Q);
Point2 x3_predict = ekf.predict(factor5);
x3_predict.print("X3 Predict");
// Update
Point2 z3(3.0, 0.0);
PriorFactor<LinearKey> factor6(x3, z3, R);
PriorFactor<Point2> factor6(x3, z3, R);
Point2 x3_update = ekf.update(factor6);
x3_update.print("X3 Update");

36
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -33,8 +33,6 @@
using namespace std;
using namespace gtsam;
typedef TypedSymbol<Point2, 'x'> Key; /// Variable labels for a specific type
int main() {
// [code below basically does SRIF with LDL]
@ -55,14 +53,14 @@ int main() {
// i.e., we should get 0,0 0,1 0,2 if there is no noise
// Create new state variable
Key x0(0);
Symbol x0('x',0);
ordering->insert(x0, 0);
// Initialize state x0 (2D point) at origin by adding a prior factor, i.e., Bayes net P(x0)
// 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<Key> factor1(x0, x_initial, P_initial);
PriorFactor<Point2> 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));
@ -88,12 +86,12 @@ int main() {
// so, difference = x_{t+1} - x_{t} = F*x_{t} + B*u_{t} - I*x_{t}
// = (F - I)*x_{t} + B*u_{t}
// = B*u_{t} (for our example)
Key x1(1);
Symbol x1('x',1);
ordering->insert(x1, 1);
Point2 difference(1,0);
SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
BetweenFactor<Key> factor2(x0, x1, difference, Q);
BetweenFactor<Point2> 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));
@ -116,7 +114,7 @@ int main() {
// Extract the current estimate of x1,P1 from the Bayes Network
VectorValues result = optimize(*linearBayesNet);
Point2 x1_predict = linearizationPoints[x1].retract(result[ordering->at(x1)]);
Point2 x1_predict = linearizationPoints.at<Point2>(x1).retract(result[ordering->at(x1)]);
x1_predict.print("X1 Predict");
// Update the new linearization point to the new estimate
@ -171,7 +169,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<Key> factor4(x1, z1, R1);
PriorFactor<Point2> factor4(x1, z1, R1);
// Linearize the factor and add it to the linear factor graph
linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering));
@ -188,7 +186,7 @@ int main() {
// Extract the current estimate of x1 from the Bayes Network
result = optimize(*linearBayesNet);
Point2 x1_update = linearizationPoints[x1].retract(result[ordering->at(x1)]);
Point2 x1_update = linearizationPoints.at<Point2>(x1).retract(result[ordering->at(x1)]);
x1_update.print("X1 Update");
// Update the linearization point to the new estimate
@ -213,7 +211,7 @@ int main() {
linearFactorGraph->add(0, tmpPrior1.getA(tmpPrior1.begin()), tmpPrior1.getb() - tmpPrior1.getA(tmpPrior1.begin()) * result[ordering->at(x1)], tmpPrior1.get_model());
// Create a key for the new state
Key x2(2);
Symbol x2('x',2);
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
@ -223,7 +221,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<Key> factor6(x1, x2, difference, Q);
BetweenFactor<Point2> factor6(x1, x2, difference, Q);
// Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x2, x1_update);
@ -235,7 +233,7 @@ int main() {
// Extract the current estimate of x2 from the Bayes Network
result = optimize(*linearBayesNet);
Point2 x2_predict = linearizationPoints[x2].retract(result[ordering->at(x2)]);
Point2 x2_predict = linearizationPoints.at<Point2>(x2).retract(result[ordering->at(x2)]);
x2_predict.print("X2 Predict");
// Update the linearization point to the new estimate
@ -261,7 +259,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<Key> factor8(x2, z2, R2);
PriorFactor<Point2> factor8(x2, z2, R2);
// Linearize the factor and add it to the linear factor graph
linearFactorGraph->push_back(factor8.linearize(linearizationPoints, *ordering));
@ -279,7 +277,7 @@ int main() {
// Extract the current estimate of x2 from the Bayes Network
result = optimize(*linearBayesNet);
Point2 x2_update = linearizationPoints[x2].retract(result[ordering->at(x2)]);
Point2 x2_update = linearizationPoints.at<Point2>(x2).retract(result[ordering->at(x2)]);
x2_update.print("X2 Update");
// Update the linearization point to the new estimate
@ -302,7 +300,7 @@ int main() {
linearFactorGraph->add(0, tmpPrior3.getA(tmpPrior3.begin()), tmpPrior3.getb() - tmpPrior3.getA(tmpPrior3.begin()) * result[ordering->at(x2)], tmpPrior3.get_model());
// Create a key for the new state
Key x3(3);
Symbol x3('x',3);
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
@ -312,7 +310,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<Key> factor10(x2, x3, difference, Q);
BetweenFactor<Point2> factor10(x2, x3, difference, Q);
// Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x3, x2_update);
@ -324,7 +322,7 @@ int main() {
// Extract the current estimate of x3 from the Bayes Network
result = optimize(*linearBayesNet);
Point2 x3_predict = linearizationPoints[x3].retract(result[ordering->at(x3)]);
Point2 x3_predict = linearizationPoints.at<Point2>(x3).retract(result[ordering->at(x3)]);
x3_predict.print("X3 Predict");
// Update the linearization point to the new estimate
@ -350,7 +348,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<Key> factor12(x3, z3, R3);
PriorFactor<Point2> factor12(x3, z3, R3);
// Linearize the factor and add it to the linear factor graph
linearFactorGraph->push_back(factor12.linearize(linearizationPoints, *ordering));
@ -368,7 +366,7 @@ int main() {
// Extract the current estimate of x2 from the Bayes Network
result = optimize(*linearBayesNet);
Point2 x3_update = linearizationPoints[x3].retract(result[ordering->at(x3)]);
Point2 x3_update = linearizationPoints.at<Point2>(x3).retract(result[ordering->at(x3)]);
x3_update.print("X3 Update");
// Update the linearization point to the new estimate

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

@ -153,7 +153,7 @@ public:
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));
config_->insert(key_to, config_->at<POSE>(key_from).compose(relativePose));
}
};

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

@ -27,10 +27,10 @@
namespace gtsam {
/* ************************************************************************* */
template<class KEY>
typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::solve_(
template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::solve_(
const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering,
const Values& linearizationPoints, const KEY& lastKey,
const Values& linearizationPoints, const Symbol& lastKey,
JacobianFactor::shared_ptr& newPrior) const {
// Extract the Index of the provided last key
@ -43,7 +43,7 @@ namespace gtsam {
// Extract the current estimate of x1,P1 from the Bayes Network
VectorValues result = optimize(*linearBayesNet);
T x = linearizationPoints[lastKey].retract(result[lastIndex]);
T x = linearizationPoints.at<T>(lastKey).retract(result[lastIndex]);
// Create a Jacobian Factor from the root node of the produced Bayes Net.
// This will act as a prior for the next iteration.
@ -66,8 +66,8 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class KEY>
ExtendedKalmanFilter<KEY>::ExtendedKalmanFilter(T x_initial,
template<class VALUE>
ExtendedKalmanFilter<VALUE>::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 KEY>
typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::predict(
template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::predict(
const MotionFactor& motionFactor) {
// TODO: This implementation largely ignores the actual factor symbols.
@ -91,8 +91,8 @@ namespace gtsam {
// different keys will still compute as if a common key-set was used
// Create Keys
KEY x0 = motionFactor.key1();
KEY x1 = motionFactor.key2();
Symbol x0 = motionFactor.key1();
Symbol x1 = motionFactor.key2();
// Create an elimination ordering
Ordering ordering;
@ -122,8 +122,8 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class KEY>
typename ExtendedKalmanFilter<KEY>::T ExtendedKalmanFilter<KEY>::update(
template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::update(
const MeasurementFactor& measurementFactor) {
// TODO: This implementation largely ignores the actual factor symbols.
@ -131,7 +131,7 @@ namespace gtsam {
// different keys will still compute as if a common key-set was used
// Create Keys
KEY x0 = measurementFactor.key();
Symbol x0 = measurementFactor.key();
// Create an elimination ordering
Ordering ordering;

12
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

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

2
gtsam/nonlinear/NonlinearEquality.h
View File

@ -219,7 +219,7 @@ namespace gtsam {
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearEquality1("
<< (std::string) this->key_ << "),"<< "\n";
<< (std::string) this->key() << "),"<< "\n";
this->noiseModel_->print();
value_.print("Value");
}

16
gtsam/slam/BoundingConstraint.h
View File

@ -95,18 +95,18 @@ private:
* Binary scalar inequality constraint, with a similar value() function
* to implement for specific systems
*/
template<class KEY1, class KEY2>
struct BoundingConstraint2: public NonlinearFactor2<KEY1, KEY2> {
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
template<class VALUE1, class VALUE2>
struct BoundingConstraint2: public NonlinearFactor2<VALUE1, VALUE2> {
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef NonlinearFactor2<KEY1, KEY2> Base;
typedef boost::shared_ptr<BoundingConstraint2<KEY1, KEY2> > shared_ptr;
typedef NonlinearFactor2<VALUE1, VALUE2> Base;
typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
BoundingConstraint2(const KEY1& key1, const KEY2& key2, double threshold,
BoundingConstraint2(const Symbol& key1, const Symbol& key2, double threshold,
bool isGreaterThan, double mu = 1000.0)
: Base(noiseModel::Constrained::All(1, mu), key1, key2),
threshold_(threshold), isGreaterThan_(isGreaterThan) {}
@ -127,7 +127,7 @@ struct BoundingConstraint2: public NonlinearFactor2<KEY1, KEY2> {
/** active when constraint *NOT* met */
bool active(const Values& c) const {
// note: still active at equality to avoid zigzagging
double x = value(c[this->key1_], c[this->key2_]);
double x = value(c[this->key1()], c[this->key2()]);
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
}

2
gtsam/slam/GeneralSFMFactor.h
View File

@ -53,7 +53,7 @@ namespace gtsam {
* @param j is the index of the landmark
*/
GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Symbol& landmarkKey) :
Base(model, cameraKey, cameraKey), measured_(measured) {}
Base(model, cameraKey, landmarkKey), measured_(measured) {}
GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor
GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2

2
gtsam/slam/planarSLAM.cpp
View File

@ -37,7 +37,7 @@ namespace planarSLAM {
}
void Graph::addOdometry(Index i, Index j, const Pose2& odometry, const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(PoseKey(i), PointKey(j), odometry, model));
sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), odometry, model));
push_back(factor);
}

2
gtsam/slam/planarSLAM.h
View File

@ -97,7 +97,7 @@ namespace planarSLAM {
void addPoseConstraint(Index poseKey, const Pose2& pose);
/// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
void addOdometry(Index poseKey, Index pointKey, const Pose2& odometry, const SharedNoiseModel& model);
void addOdometry(Index poseKey1, Index poseKey2, 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(Index poseKey, Index pointKey, const Rot2& bearing, const SharedNoiseModel& model);

2
gtsam/slam/simulated2DConstraints.h
View File

@ -147,7 +147,7 @@ namespace simulated2D {
}
};
typedef MaxDistanceConstraint<PoseKey> PoseMaxDistConstraint; ///< Simulated2D domain example factor
typedef MaxDistanceConstraint<Point2> PoseMaxDistConstraint; ///< Simulated2D domain example factor
/**
* Binary inequality constraint forcing a minimum range

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

@ -44,7 +44,7 @@ public:
}
void addPoint3Constraint(int j, const Point3& p) {
boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(Symbol('x',j), p));
boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(Symbol('l',j), p));
push_back(factor);
}
@ -89,8 +89,7 @@ TEST( GeneralSFMFactor, equals )
TEST( GeneralSFMFactor, error ) {
Point2 z(3.,0.);
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor(new Projection(z, sigma, "x1", "l1"));
boost::shared_ptr<Projection> factor(new Projection(z, sigma, "x1", "l1"));
// For the following configuration, the factor predicts 320,240
Values values;
Rot3 R;
@ -167,13 +166,13 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(CameraKey((int)i), X[i]) ;
values->insert(Symbol('x',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(PointKey(i), pt) ;
values->insert(Symbol('l',i), pt) ;
}
graph->addCameraConstraint(0, X[0]);
@ -206,7 +205,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(CameraKey((int)i), X[i]) ;
values->insert(Symbol('x',i), X[i]) ;
// add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) {
@ -214,10 +213,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(PointKey(i), pt) ;
values->insert(Symbol('l',i), pt) ;
}
else {
values->insert(PointKey(i), L[i]) ;
values->insert(Symbol('l',i), L[i]) ;
}
}
@ -252,14 +251,14 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(CameraKey((int)i), X[i]) ;
values->insert(Symbol('x',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(PointKey(i), pt) ;
values->insert(Symbol('l',i), pt) ;
}
for ( size_t i = 0 ; i < X.size() ; ++i )
@ -301,7 +300,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
focal_noise = 1,
skew_noise = 1e-5;
if ( i == 0 ) {
values->insert(CameraKey((int)i), X[i]) ;
values->insert(Symbol('x',i), X[i]) ;
}
else {
@ -312,12 +311,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
skew_noise, // s
trans_noise, trans_noise // ux, uy
) ;
values->insert(CameraKey((int)i), X[i].retract(delta)) ;
values->insert(Symbol('x',i), X[i].retract(delta)) ;
}
}
for ( size_t i = 0 ; i < L.size() ; ++i ) {
values->insert(PointKey(i), L[i]) ;
values->insert(Symbol('l',i), L[i]) ;
}
// fix X0 and all landmarks, allow only the X[1] to move
@ -357,14 +356,14 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(CameraKey((int)i), X[i]) ;
values->insert(Symbol('x',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(PointKey(i), pt) ;
values->insert(Symbol('l',i), pt) ;
}
graph->addCameraConstraint(0, X[0]);

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

@ -207,7 +207,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(CameraKey((int)i), X[i]) ;
values->insert(Symbol('x',i), X[i]) ;
// add noise only to the first landmark
for ( size_t i = 0 ; i < L.size() ; ++i ) {
@ -215,10 +215,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(PointKey(i), pt) ;
values->insert(Symbol('l',i), pt) ;
}
else {
values->insert(PointKey(i), L[i]) ;
values->insert(Symbol('l',i), L[i]) ;
}
}
@ -253,14 +253,14 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(CameraKey((int)i), X[i]) ;
values->insert(Symbol('x',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(PointKey(i), pt) ;
values->insert(Symbol('l',i), pt) ;
}
for ( size_t i = 0 ; i < X.size() ; ++i )
@ -300,7 +300,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
rot_noise = 1e-5, trans_noise = 1e-3,
focal_noise = 1, distort_noise = 1e-3;
if ( i == 0 ) {
values->insert(CameraKey((int)i), X[i]) ;
values->insert(Symbol('x',i), X[i]) ;
}
else {
@ -309,12 +309,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
trans_noise, trans_noise, trans_noise, // translation
focal_noise, distort_noise, distort_noise // f, k1, k2
) ;
values->insert(CameraKey((int)i), X[i].retract(delta)) ;
values->insert(Symbol('x',i), X[i].retract(delta)) ;
}
}
for ( size_t i = 0 ; i < L.size() ; ++i ) {
values->insert(PointKey(i), L[i]) ;
values->insert(Symbol('l',i), L[i]) ;
}
// fix X0 and all landmarks, allow only the X[1] to move
@ -353,14 +353,14 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(CameraKey((int)i), X[i]) ;
values->insert(Symbol('x',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(PointKey(i), pt) ;
values->insert(Symbol('l',i), pt) ;
}
graph->addCameraConstraint(0, X[0]);

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

@ -131,12 +131,12 @@ TEST( Pose3Factor, error )
// Create factor
SharedNoiseModel I6(noiseModel::Unit::Create(6));
Pose3Factor factor(1,2, z, I6);
Pose3Factor factor(PoseKey(1), PoseKey(2), z, I6);
// Create config
Values x;
x.insert(Key(1),t1);
x.insert(Key(2),t2);
x.insert(PoseKey(1),t1);
x.insert(PoseKey(2),t2);
// Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2))
Vector actual = factor.unwhitenedError(x);
@ -146,10 +146,10 @@ TEST( Pose3Factor, error )
/* ************************************************************************* */
TEST(Pose3Graph, partial_prior_xy) {
typedef PartialPriorFactor<pose3SLAM::Key> Partial;
typedef PartialPriorFactor<Pose3> Partial;
// XY prior - full mask interface
pose3SLAM::Key key(1);
Symbol key = PoseKey(1);
Vector exp_xy = Vector_(2, 3.0, 4.0);
SharedDiagonal model = noiseModel::Unit::Create(2);
Pose3 init(Rot3(), Point3(1.0,-2.0, 3.0)), expected(Rot3(), Point3(3.0, 4.0, 3.0));
@ -169,7 +169,7 @@ TEST(Pose3Graph, partial_prior_xy) {
values.insert(key, init);
Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
EXPECT(assert_equal(expected, actual[key], tol));
EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
}
@ -185,10 +185,10 @@ TEST( Values, pose3Circle )
{
// expected is 4 poses tangent to circle with radius 1m
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)));
expected.insert(PoseKey(0), Pose3(R1, Point3( 1, 0, 0)));
expected.insert(PoseKey(1), Pose3(R2, Point3( 0, 1, 0)));
expected.insert(PoseKey(2), Pose3(R3, Point3(-1, 0, 0)));
expected.insert(PoseKey(3), Pose3(R4, Point3( 0,-1, 0)));
Values actual = pose3SLAM::circle(4,1.0);
CHECK(assert_equal(expected,actual));
@ -198,10 +198,10 @@ TEST( Values, pose3Circle )
TEST( Values, expmap )
{
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)));
expected.insert(PoseKey(0), Pose3(R1, Point3( 1.0, 0.1, 0)));
expected.insert(PoseKey(1), Pose3(R2, Point3(-0.1, 1.0, 0)));
expected.insert(PoseKey(2), Pose3(R3, Point3(-1.0,-0.1, 0)));
expected.insert(PoseKey(3), Pose3(R4, Point3( 0.1,-1.0, 0)));
Ordering ordering(*expected.orderingArbitrary());
VectorValues delta(expected.dims(ordering));

24
gtsam/slam/visualSLAM.h
View File

@ -85,8 +85,8 @@ namespace visualSLAM {
* @param K shared pointer to calibration object
*/
void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
const Symbol& poseKey, const Symbol& pointKey, const shared_ptrK& K) {
boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, poseKey, pointKey, K));
Index poseKey, Index pointKey, const shared_ptrK& K) {
boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, PoseKey(poseKey), PointKey(pointKey), K));
push_back(factor);
}
@ -95,8 +95,8 @@ namespace visualSLAM {
* @param key variable key of the camera pose
* @param p to which pose to constrain it to
*/
void addPoseConstraint(const Symbol& key, const Pose3& p = Pose3()) {
boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(key, p));
void addPoseConstraint(Index poseKey, const Pose3& p = Pose3()) {
boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(PoseKey(poseKey), p));
push_back(factor);
}
@ -105,8 +105,8 @@ namespace visualSLAM {
* @param key variable key of the landmark
* @param p point around which soft prior is defined
*/
void addPointConstraint(const Symbol& key, const Point3& p = Point3()) {
boost::shared_ptr<PointConstraint> factor(new PointConstraint(key, p));
void addPointConstraint(Index pointKey, const Point3& p = Point3()) {
boost::shared_ptr<PointConstraint> factor(new PointConstraint(PointKey(pointKey), p));
push_back(factor);
}
@ -116,8 +116,8 @@ namespace visualSLAM {
* @param p around which soft prior is defined
* @param model uncertainty model of this prior
*/
void addPosePrior(const Symbol& key, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) {
boost::shared_ptr<PosePrior> factor(new PosePrior(key, p, model));
void addPosePrior(Index poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) {
boost::shared_ptr<PosePrior> factor(new PosePrior(PoseKey(poseKey), p, model));
push_back(factor);
}
@ -127,8 +127,8 @@ namespace visualSLAM {
* @param p to which point to constrain it to
* @param model uncertainty model of this prior
*/
void addPointPrior(const Symbol& key, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) {
boost::shared_ptr<PointPrior> factor(new PointPrior(key, p, model));
void addPointPrior(Index pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) {
boost::shared_ptr<PointPrior> factor(new PointPrior(PointKey(pointKey), p, model));
push_back(factor);
}
@ -139,8 +139,8 @@ namespace visualSLAM {
* @param range approximate range to landmark
* @param model uncertainty model of this prior
*/
void addRangeFactor(const Symbol& poseKey, const Symbol& pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) {
push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(poseKey, pointKey, range, model)));
void addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) {
push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(PoseKey(poseKey), PointKey(pointKey), range, model)));
}
}; // Graph

15
tests/testBoundingConstraint.cpp
View File

@ -17,6 +17,8 @@
#include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include <gtsam/slam/simulated2DConstraints.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
@ -37,7 +39,7 @@ typedef boost::shared_ptr<Values> shared_values;
typedef NonlinearOptimizer<Graph> Optimizer;
// some simple inequality constraints
simulated2D::PoseKey key(1);
Symbol key(simulated2D::PoseKey(1));
double mu = 10.0;
// greater than
iq2D::PoseXInequality constraint1(key, 1.0, true, mu);
@ -151,7 +153,7 @@ TEST( testBoundingConstraint, unary_simple_optimization1) {
Point2 start_pt(0.0, 1.0);
shared_graph graph(new Graph());
simulated2D::PoseKey x1(1);
Symbol x1("x1");
graph->add(iq2D::PoseXInequality(x1, 1.0, true));
graph->add(iq2D::PoseYInequality(x1, 2.0, true));
graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
@ -173,7 +175,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
Point2 start_pt(2.0, 3.0);
shared_graph graph(new Graph());
simulated2D::PoseKey x1(1);
Symbol x1("x1");
graph->add(iq2D::PoseXInequality(x1, 1.0, false));
graph->add(iq2D::PoseYInequality(x1, 2.0, false));
graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
@ -189,7 +191,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
/* ************************************************************************* */
TEST( testBoundingConstraint, MaxDistance_basics) {
simulated2D::PoseKey key1(1), key2(2);
Symbol key1("x1"), key2("x2");
Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu);
EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol);
@ -231,7 +233,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
simulated2D::PoseKey x1(1), x2(2);
Symbol x1("x1"), x2("x2");
Graph graph;
graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1));
@ -254,8 +256,7 @@ TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
/* ************************************************************************* */
TEST( testBoundingConstraint, avoid_demo) {
simulated2D::PoseKey x1(1), x2(2), x3(3);
simulated2D::PointKey l1(1);
Symbol x1("x1"), x2("x2"), x3("x3"), l1("l1");
double radius = 1.0;
Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
Point2 odo(2.0, 0.0);

73
tests/testExtendedKalmanFilter.cpp
View File

@ -16,6 +16,8 @@
#include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
@ -24,9 +26,6 @@
using namespace gtsam;
// Define Types for System Test
typedef TypedSymbol<Point2, 'x'> TestKey;
/* ************************************************************************* */
TEST( ExtendedKalmanFilter, linear ) {
@ -35,10 +34,10 @@ TEST( ExtendedKalmanFilter, linear ) {
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
// Create an ExtendedKalmanFilter object
ExtendedKalmanFilter<TestKey> ekf(x_initial, P_initial);
ExtendedKalmanFilter<Point2> ekf(x_initial, P_initial);
// Create the TestKeys for our example
TestKey x0(0), x1(1), x2(2), x3(3);
Symbol x0("x0"), x1("x1"), x2("x2"), x3("x3");
// Create the controls and measurement properties for our example
double dt = 1.0;
@ -57,30 +56,30 @@ TEST( ExtendedKalmanFilter, linear ) {
// Run iteration 1
// Create motion factor
BetweenFactor<TestKey> factor1(x0, x1, difference, Q);
BetweenFactor<Point2> factor1(x0, x1, difference, Q);
Point2 predict1 = ekf.predict(factor1);
EXPECT(assert_equal(expected1,predict1));
// Create the measurement factor
PriorFactor<TestKey> factor2(x1, z1, R);
PriorFactor<Point2> factor2(x1, z1, R);
Point2 update1 = ekf.update(factor2);
EXPECT(assert_equal(expected1,update1));
// Run iteration 2
BetweenFactor<TestKey> factor3(x1, x2, difference, Q);
BetweenFactor<Point2> factor3(x1, x2, difference, Q);
Point2 predict2 = ekf.predict(factor3);
EXPECT(assert_equal(expected2,predict2));
PriorFactor<TestKey> factor4(x2, z2, R);
PriorFactor<Point2> factor4(x2, z2, R);
Point2 update2 = ekf.update(factor4);
EXPECT(assert_equal(expected2,update2));
// Run iteration 3
BetweenFactor<TestKey> factor5(x2, x3, difference, Q);
BetweenFactor<Point2> factor5(x2, x3, difference, Q);
Point2 predict3 = ekf.predict(factor5);
EXPECT(assert_equal(expected3,predict3));
PriorFactor<TestKey> factor6(x3, z3, R);
PriorFactor<Point2> factor6(x3, z3, R);
Point2 update3 = ekf.update(factor6);
EXPECT(assert_equal(expected3,update3));
@ -89,12 +88,12 @@ TEST( ExtendedKalmanFilter, linear ) {
// Create Motion Model Factor
class NonlinearMotionModel : public NonlinearFactor2<TestKey,TestKey> {
class NonlinearMotionModel : public NonlinearFactor2<Point2,Point2> {
public:
typedef TestKey::Value T;
typedef Point2 T;
private:
typedef NonlinearFactor2<TestKey,TestKey> Base;
typedef NonlinearFactor2<Point2, Point2> Base;
typedef NonlinearMotionModel This;
protected:
@ -104,7 +103,7 @@ protected:
public:
NonlinearMotionModel(){}
NonlinearMotionModel(const TestKey& TestKey1, const TestKey& TestKey2) :
NonlinearMotionModel(const Symbol& TestKey1, const Symbol& TestKey2) :
Base(noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)), TestKey1, TestKey2), Q_(2,2) {
// Initialize motion model parameters:
@ -156,14 +155,14 @@ public:
/* print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearMotionModel\n";
std::cout << " TestKey1: " << (std::string) key1_ << std::endl;
std::cout << " TestKey2: " << (std::string) key2_ << std::endl;
std::cout << " TestKey1: " << (std::string) key1() << std::endl;
std::cout << " TestKey2: " << (std::string) key2() << std::endl;
}
/** Check if two factors are equal. Note type is IndexFactor and needs cast. */
virtual bool equals(const NonlinearFactor2<TestKey,TestKey>& f, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*> (&f);
return (e != NULL) && (key1_ == e->key1_) && (key2_ == e->key2_);
return (e != NULL) && (key1() == e->key1()) && (key2() == e->key2());
}
/**
@ -182,7 +181,7 @@ public:
/** Vector of errors, whitened ! */
Vector whitenedError(const Values& c) const {
return QInvSqrt(c[key1_])*unwhitenedError(c);
return QInvSqrt(c[key1()])*unwhitenedError(c);
}
/**
@ -191,11 +190,11 @@ public:
* Hence b = z - h(x1,x2) = - error_vector(x)
*/
boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
const X1& x1 = c[key1_];
const X2& x2 = c[key2_];
const X1& x1 = c[key1()];
const X2& x2 = c[key2()];
Matrix A1, A2;
Vector b = -evaluateError(x1, x2, A1, A2);
const Index var1 = ordering[key1_], var2 = ordering[key2_];
const Index var1 = ordering[key1()], var2 = ordering[key2()];
SharedDiagonal constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL) {
@ -236,13 +235,13 @@ public:
};
// Create Measurement Model Factor
class NonlinearMeasurementModel : public NonlinearFactor1<TestKey> {
class NonlinearMeasurementModel : public NonlinearFactor1<Point2> {
public:
typedef TestKey::Value T;
typedef Point2 T;
private:
typedef NonlinearFactor1<TestKey> Base;
typedef NonlinearFactor1<Point2> Base;
typedef NonlinearMeasurementModel This;
protected:
@ -253,7 +252,7 @@ protected:
public:
NonlinearMeasurementModel(){}
NonlinearMeasurementModel(const TestKey& TestKey, Vector z) :
NonlinearMeasurementModel(const Symbol& TestKey, Vector z) :
Base(noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)), TestKey), z_(z), R_(1,1) {
// Initialize nonlinear measurement model parameters:
@ -294,13 +293,13 @@ public:
/* print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearMeasurementModel\n";
std::cout << " TestKey: " << (std::string) key_ << std::endl;
std::cout << " TestKey: " << (std::string) key() << std::endl;
}
/** Check if two factors are equal. Note type is IndexFactor and needs cast. */
virtual bool equals(const NonlinearFactor1<TestKey>& f, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*> (&f);
return (e != NULL) && (key_ == e->key_);
return (e != NULL) && Base::equals(f);
}
/**
@ -319,7 +318,7 @@ public:
/** Vector of errors, whitened ! */
Vector whitenedError(const Values& c) const {
return RInvSqrt(c[key_])*unwhitenedError(c);
return RInvSqrt(c[key()])*unwhitenedError(c);
}
/**
@ -328,10 +327,10 @@ public:
* Hence b = z - h(x1) = - error_vector(x)
*/
boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
const X& x1 = c[key_];
const X& x1 = c[key()];
Matrix A1;
Vector b = -evaluateError(x1, A1);
const Index var1 = ordering[key_];
const Index var1 = ordering[key()];
SharedDiagonal constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL) {
@ -345,7 +344,7 @@ public:
}
/** vector of errors */
Vector evaluateError(const TestKey::Value& p, boost::optional<Matrix&> H1 = boost::none) const {
Vector evaluateError(const T& p, boost::optional<Matrix&> H1 = boost::none) const {
// error = z - h(p)
// H = d error / d p = -d h/ d p = -H
Vector z_hat = h(p);
@ -406,17 +405,17 @@ TEST( ExtendedKalmanFilter, nonlinear ) {
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
// Create an ExtendedKalmanFilter object
ExtendedKalmanFilter<TestKey> ekf(x_initial, P_initial);
ExtendedKalmanFilter<Point2> ekf(x_initial, P_initial);
// Enter Predict-Update Loop
Point2 x_predict, x_update;
for(unsigned int i = 0; i < 10; ++i){
// Create motion factor
NonlinearMotionModel motionFactor(TestKey(i-1), TestKey(i));
NonlinearMotionModel motionFactor(Symbol('x',i-1), Symbol('x',i));
x_predict = ekf.predict(motionFactor);
// Create a measurement factor
NonlinearMeasurementModel measurementFactor(TestKey(i), Vector_(1, z[i]));
NonlinearMeasurementModel measurementFactor(Symbol('x',i), Vector_(1, z[i]));
x_update = ekf.update(measurementFactor);
EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6));

20
tests/testGaussianISAM2.cpp
View File

@ -202,8 +202,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
// SETDEBUG("ISAM2 recalculate", true);
// Pose and landmark key types from planarSLAM
typedef planarSLAM::PoseKey PoseKey;
typedef planarSLAM::PointKey PointKey;
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
@ -335,8 +335,8 @@ TEST(ISAM2, slamlike_solution_dogleg)
// SETDEBUG("ISAM2 recalculate", true);
// Pose and landmark key types from planarSLAM
typedef planarSLAM::PoseKey PoseKey;
typedef planarSLAM::PointKey PointKey;
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
@ -463,8 +463,8 @@ TEST(ISAM2, slamlike_solution_dogleg)
TEST(ISAM2, clone) {
// Pose and landmark key types from planarSLAM
typedef planarSLAM::PoseKey PoseKey;
typedef planarSLAM::PointKey PointKey;
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
@ -636,8 +636,8 @@ TEST(ISAM2, removeFactors)
// then removes the 2nd-to-last landmark measurement
// Pose and landmark key types from planarSLAM
typedef planarSLAM::PoseKey PoseKey;
typedef planarSLAM::PointKey PointKey;
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
@ -775,8 +775,8 @@ TEST(ISAM2, constrained_ordering)
// SETDEBUG("ISAM2 recalculate", true);
// Pose and landmark key types from planarSLAM
typedef planarSLAM::PoseKey PoseKey;
typedef planarSLAM::PointKey PointKey;
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
// Set up parameters
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));

6
tests/testGaussianJunctionTree.cpp
View File

@ -125,8 +125,8 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2)
/* ************************************************************************* */
TEST(GaussianJunctionTree, slamlike) {
typedef planarSLAM::PoseKey PoseKey;
typedef planarSLAM::PointKey PointKey;
using planarSLAM::PoseKey;
using planarSLAM::PointKey;
Values init;
planarSLAM::Graph newfactors;
@ -195,7 +195,7 @@ TEST(GaussianJunctionTree, simpleMarginal) {
// Create a simple graph
pose2SLAM::Graph fg;
fg.addPrior(pose2SLAM::PoseKey(0), Pose2(), sharedSigma(3, 10.0));
fg.addPrior(0, Pose2(), sharedSigma(3, 10.0));
fg.addOdometry(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0)));
Values init;

12
tests/testGraph.cpp
View File

@ -86,16 +86,16 @@ TEST( Graph, composePoses )
graph.addOdometry(2,3, p23, cov);
graph.addOdometry(4,3, p43, cov);
PredecessorMap<pose2SLAM::PoseKey> tree;
tree.insert(pose2SLAM::PoseKey(1),2);
tree.insert(pose2SLAM::PoseKey(2),2);
tree.insert(pose2SLAM::PoseKey(3),2);
tree.insert(pose2SLAM::PoseKey(4),3);
PredecessorMap<Symbol> tree;
tree.insert(pose2SLAM::PoseKey(1),pose2SLAM::PoseKey(2));
tree.insert(pose2SLAM::PoseKey(2),pose2SLAM::PoseKey(2));
tree.insert(pose2SLAM::PoseKey(3),pose2SLAM::PoseKey(2));
tree.insert(pose2SLAM::PoseKey(4),pose2SLAM::PoseKey(3));
Pose2 rootPose = p2;
boost::shared_ptr<Values> actual = composePoses<pose2SLAM::Graph, pose2SLAM::Odometry,
Pose2, pose2SLAM::PoseKey> (graph, tree, rootPose);
Pose2, Symbol> (graph, tree, rootPose);
Values expected;
expected.insert(pose2SLAM::PoseKey(1), p1);

12
tests/testInference.cpp
View File

@ -54,12 +54,12 @@ TEST( Inference, marginals2)
SharedDiagonal poseModel(sharedSigma(3, 0.1));
SharedDiagonal pointModel(sharedSigma(3, 0.1));
fg.addPrior(planarSLAM::PoseKey(0), Pose2(), poseModel);
fg.addOdometry(planarSLAM::PoseKey(0), planarSLAM::PoseKey(1), Pose2(1.0,0.0,0.0), poseModel);
fg.addOdometry(planarSLAM::PoseKey(1), planarSLAM::PoseKey(2), Pose2(1.0,0.0,0.0), poseModel);
fg.addBearingRange(planarSLAM::PoseKey(0), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(planarSLAM::PoseKey(1), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(planarSLAM::PoseKey(2), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel);
fg.addPrior(0, Pose2(), poseModel);
fg.addOdometry(0, 1, Pose2(1.0,0.0,0.0), poseModel);
fg.addOdometry(1, 2, Pose2(1.0,0.0,0.0), poseModel);
fg.addBearingRange(0, 0, Rot2(), 1.0, pointModel);
fg.addBearingRange(1, 0, Rot2(), 1.0, pointModel);
fg.addBearingRange(2, 0, Rot2(), 1.0, pointModel);
Values init;
init.insert(planarSLAM::PoseKey(0), Pose2(0.0,0.0,0.0));

49
tests/testNonlinearEquality.cpp
View File

@ -31,15 +31,14 @@ namespace eq2D = simulated2D::equality_constraints;
static const double tol = 1e-5;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef PriorFactor<PoseKey> PosePrior;
typedef NonlinearEquality<PoseKey> PoseNLE;
typedef PriorFactor<Pose2> PosePrior;
typedef NonlinearEquality<Pose2> PoseNLE;
typedef boost::shared_ptr<PoseNLE> shared_poseNLE;
typedef NonlinearFactorGraph PoseGraph;
typedef NonlinearOptimizer<PoseGraph> PoseOptimizer;
PoseKey key(1);
Symbol key('x',1);
/* ************************************************************************* */
TEST ( NonlinearEquality, linearization ) {
@ -60,7 +59,7 @@ TEST ( NonlinearEquality, linearization ) {
/* ********************************************************************** */
TEST ( NonlinearEquality, linearization_pose ) {
PoseKey key(1);
Symbol key('x',1);
Pose2 value;
Values config;
config.insert(key, value);
@ -89,7 +88,7 @@ TEST ( NonlinearEquality, linearization_fail ) {
/* ********************************************************************** */
TEST ( NonlinearEquality, linearization_fail_pose ) {
PoseKey key(1);
Symbol key('x',1);
Pose2 value(2.0, 1.0, 2.0),
wrong(2.0, 3.0, 4.0);
Values bad_linearize;
@ -105,7 +104,7 @@ TEST ( NonlinearEquality, linearization_fail_pose ) {
/* ********************************************************************** */
TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
PoseKey key(1);
Symbol key('x',1);
Pose2 value,
wrong(2.0, 3.0, 4.0);
Values bad_linearize;
@ -155,7 +154,7 @@ TEST ( NonlinearEquality, equals ) {
/* ************************************************************************* */
TEST ( NonlinearEquality, allow_error_pose ) {
PoseKey key1(1);
Symbol key1('x',1);
Pose2 feasible1(1.0, 2.0, 3.0);
double error_gain = 500.0;
PoseNLE nle(key1, feasible1, error_gain);
@ -183,7 +182,7 @@ TEST ( NonlinearEquality, allow_error_pose ) {
/* ************************************************************************* */
TEST ( NonlinearEquality, allow_error_optimize ) {
PoseKey key1(1);
Symbol key1('x',1);
Pose2 feasible1(1.0, 2.0, 3.0);
double error_gain = 500.0;
PoseNLE nle(key1, feasible1, error_gain);
@ -200,7 +199,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
// optimize
boost::shared_ptr<Ordering> ord(new Ordering());
ord->push_back(key1);
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-5, 1e-5);
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-5, 1e-5);
PoseOptimizer optimizer(graph, init, ord, params);
PoseOptimizer result = optimizer.levenbergMarquardt();
@ -214,7 +213,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
// create a hard constraint
PoseKey key1(1);
Symbol key1('x',1);
Pose2 feasible1(1.0, 2.0, 3.0);
// initialize away from the ideal
@ -236,7 +235,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
// optimize
boost::shared_ptr<Ordering> ord(new Ordering());
ord->push_back(key1);
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-5, 1e-5);
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-5, 1e-5);
PoseOptimizer optimizer(graph, init, ord, params);
PoseOptimizer result = optimizer.levenbergMarquardt();
@ -258,7 +257,7 @@ typedef NonlinearOptimizer<Graph> Optimizer;
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, unary_basics ) {
Point2 pt(1.0, 2.0);
simulated2D::PoseKey key(1);
Symbol key1('x',1);
double mu = 1000.0;
eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
@ -281,7 +280,7 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) {
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, unary_linearization ) {
Point2 pt(1.0, 2.0);
simulated2D::PoseKey key(1);
Symbol key1('x',1);
double mu = 1000.0;
Ordering ordering;
ordering += key;
@ -306,7 +305,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
// create a single-node graph with a soft and hard constraint to
// ensure that the hard constraint overrides the soft constraint
Point2 truth_pt(1.0, 2.0);
simulated2D::PoseKey key(1);
Symbol key('x',1);
double mu = 10.0;
eq2D::UnaryEqualityConstraint::shared_ptr constraint(
new eq2D::UnaryEqualityConstraint(truth_pt, key, mu));
@ -337,7 +336,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, odo_basics ) {
Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0);
simulated2D::PoseKey key1(1), key2(2);
Symbol key1('x',1), key2('x',2);
double mu = 1000.0;
eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
@ -363,7 +362,7 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) {
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, odo_linearization ) {
Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0);
simulated2D::PoseKey key1(1), key2(2);
Symbol key1('x',1), key2('x',2);
double mu = 1000.0;
Ordering ordering;
ordering += key1, key2;
@ -396,7 +395,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
// a hard prior on one variable, and a conflicting soft prior
// on the other variable - the constraints should override the soft constraint
Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0);
simulated2D::PoseKey key1(1), key2(2);
Symbol key1('x',1), key2('x',2);
// hard prior on x1
eq2D::UnaryEqualityConstraint::shared_ptr constraint1(
@ -438,8 +437,8 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
shared_graph graph(new Graph());
simulated2D::PoseKey x1(1), x2(2);
simulated2D::PointKey l1(1), l2(2);
Symbol x1('x',1), x2('x',2);
Symbol l1('l',1), l2('l',2);
Point2 pt_x1(1.0, 1.0),
pt_x2(5.0, 6.0);
graph->add(eq2D::UnaryEqualityConstraint(pt_x1, x1));
@ -476,8 +475,8 @@ TEST (testNonlinearEqualityConstraint, map_warp ) {
shared_graph graph(new Graph());
// keys
simulated2D::PoseKey x1(1), x2(2);
simulated2D::PointKey l1(1), l2(2);
Symbol x1('x',1), x2('x',2);
Symbol l1('l',1), l2('l',2);
// constant constraint on x1
Point2 pose1(1.0, 1.0);
@ -526,7 +525,7 @@ typedef visualSLAM::Graph VGraph;
typedef NonlinearOptimizer<VGraph> VOptimizer;
// factors for visual slam
typedef NonlinearEquality2<visualSLAM::PointKey> Point3Equality;
typedef NonlinearEquality2<Point3> Point3Equality;
/* ********************************************************************* */
TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
@ -543,8 +542,8 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away
// keys
visualSLAM::PoseKey x1(1), x2(2);
visualSLAM::PointKey l1(1), l2(2);
Symbol x1('x',1), x2('x',2);
Symbol l1('l',1), l2('l',2);
// create graph
VGraph::shared_graph graph(new VGraph());

15
tests/testNonlinearFactor.cpp
View File

@ -40,6 +40,9 @@ using namespace std;
using namespace gtsam;
using namespace example;
using simulated2D::PoseKey;
using simulated2D::PointKey;
typedef boost::shared_ptr<NonlinearFactor > shared_nlf;
/* ************************************************************************* */
@ -49,11 +52,11 @@ TEST( NonlinearFactor, equals )
// create two nonlinear2 factors
Point2 z3(0.,-1.);
simulated2D::Measurement f0(z3, sigma, 1,1);
simulated2D::Measurement f0(z3, sigma, PoseKey(1),PointKey(1));
// measurement between x2 and l1
Point2 z4(-1.5, -1.);
simulated2D::Measurement f1(z4, sigma, 2,1);
simulated2D::Measurement f1(z4, sigma, PoseKey(2),PointKey(1));
CHECK(assert_equal(f0,f0));
CHECK(f0.equals(f0));
@ -199,7 +202,7 @@ TEST( NonlinearFactor, linearize_constraint1 )
SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas);
Point2 mu(1., -1.);
Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, 1));
Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, PoseKey(1)));
Values config;
config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
@ -219,7 +222,7 @@ TEST( NonlinearFactor, linearize_constraint2 )
SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas);
Point2 z3(1.,-1.);
simulated2D::Measurement f0(z3, constraint, 1,1);
simulated2D::Measurement f0(z3, constraint, PoseKey(1),PointKey(1));
Values config;
config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
@ -283,7 +286,7 @@ TEST(NonlinearFactor, NonlinearFactor4) {
class TestFactor5 : public NonlinearFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> {
public:
typedef NonlinearFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base;
TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {}
TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4", "x5") {}
virtual Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
@ -333,7 +336,7 @@ TEST(NonlinearFactor, NonlinearFactor5) {
class TestFactor6 : public NonlinearFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> {
public:
typedef NonlinearFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base;
TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {}
TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4", "x5", "x6") {}
virtual Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,

17
tests/testNonlinearISAM.cpp
View File

@ -25,15 +25,14 @@ TEST(testNonlinearISAM, markov_chain ) {
Sampler sampler(model, 42u);
// create initial graph
PoseKey key(0);
Pose2 cur_pose; // start at origin
Graph start_factors;
start_factors.addPoseConstraint(key, cur_pose);
start_factors.addPoseConstraint(0, cur_pose);
planarSLAM::Values init;
planarSLAM::Values expected;
init.insert(key, cur_pose);
expected.insert(key, cur_pose);
init.insertPose(0, cur_pose);
expected.insertPose(0, cur_pose);
isam.update(start_factors, init);
// loop for a period of time to verify memory usage
@ -41,8 +40,7 @@ TEST(testNonlinearISAM, markov_chain ) {
Pose2 z(1.0, 2.0, 0.1);
for (size_t i=1; i<=nrPoses; ++i) {
Graph new_factors;
PoseKey key1(i-1), key2(i);
new_factors.addOdometry(key1, key2, z, model);
new_factors.addOdometry(i-1, i, z, model);
planarSLAM::Values new_init;
// perform a check on changing orderings
@ -61,16 +59,15 @@ TEST(testNonlinearISAM, markov_chain ) {
}
cur_pose = cur_pose.compose(z);
new_init.insert(key2, cur_pose.retract(sampler.sample()));
expected.insert(key2, cur_pose);
new_init.insertPose(i, cur_pose.retract(sampler.sample()));
expected.insertPose(i, cur_pose);
isam.update(new_factors, new_init);
}
// verify values - all but the last one should be very close
planarSLAM::Values actual = isam.estimate();
for (size_t i=0; i<nrPoses; ++i) {
PoseKey cur_key(i);
EXPECT(assert_equal(expected[cur_key], actual[cur_key], tol));
EXPECT(assert_equal(expected.pose(i), actual.pose(i), tol));
}
}

13
tests/testRot3Optimization.cpp
View File

@ -29,9 +29,8 @@
using namespace gtsam;
typedef TypedSymbol<Rot3, 'r'> Key;
typedef PriorFactor<Key> Prior;
typedef BetweenFactor<Key> Between;
typedef PriorFactor<Rot3> Prior;
typedef BetweenFactor<Rot3> Between;
typedef NonlinearFactorGraph Graph;
/* ************************************************************************* */
@ -41,11 +40,11 @@ TEST(Rot3, optimize) {
Values truth;
Values initial;
Graph fg;
fg.add(Prior(0, Rot3(), sharedSigma(3, 0.01)));
fg.add(Prior(Symbol('r',0), Rot3(), sharedSigma(3, 0.01)));
for(int j=0; j<6; ++j) {
truth.insert(Key(j), Rot3::Rz(M_PI/3.0 * double(j)));
initial.insert(Key(j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
fg.add(Between(Key(j), Key((j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j)));
initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
fg.add(Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
}
NonlinearOptimizationParameters params;

14
tests/testSerialization.cpp
View File

@ -525,8 +525,8 @@ TEST (Serialization, planar_system) {
graph.add(constraint);
// text
EXPECT(equalsObj<PoseKey>(PoseKey(2)));
EXPECT(equalsObj<PointKey>(PointKey(3)));
EXPECT(equalsObj<Symbol>(PoseKey(2)));
EXPECT(equalsObj<Symbol>(PointKey(3)));
EXPECT(equalsObj<planarSLAM::Values>(values));
EXPECT(equalsObj<Prior>(prior));
EXPECT(equalsObj<Bearing>(bearing));
@ -537,8 +537,8 @@ TEST (Serialization, planar_system) {
EXPECT(equalsObj<Graph>(graph));
// xml
EXPECT(equalsXML<PoseKey>(PoseKey(2)));
EXPECT(equalsXML<PointKey>(PointKey(3)));
EXPECT(equalsXML<Symbol>(PoseKey(2)));
EXPECT(equalsXML<Symbol>(PointKey(3)));
EXPECT(equalsXML<planarSLAM::Values>(values));
EXPECT(equalsXML<Prior>(prior));
EXPECT(equalsXML<Bearing>(bearing));
@ -562,8 +562,8 @@ BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoF
TEST (Serialization, visual_system) {
using namespace visualSLAM;
Values values;
PoseKey x1(1), x2(2);
PointKey l1(1), l2(2);
Symbol x1('x',1), x2('x',2);
Symbol l1('l',1), l2('l',2);
Pose3 pose1 = pose3, pose2 = pose3.inverse();
Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0);
values.insert(x1, pose1);
@ -574,7 +574,7 @@ TEST (Serialization, visual_system) {
boost::shared_ptr<Cal3_S2> K(new Cal3_S2(cal1));
Graph graph;
graph.addMeasurement(Point2(1.0, 2.0), model2, x1, l1, K);
graph.addMeasurement(Point2(1.0, 2.0), model2, 1, 1, K);
graph.addPointConstraint(1, pt1);
graph.addPointPrior(1, pt2, model3);
graph.addPoseConstraint(1, pose1);