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

Merged sub-branch into 2.0_prep branch

release/4.3a0
Richard Roberts 2012-02-08 22:05:24 +00:00
parent 9f055fc812 669c7c8dc8
commit 0d2a9018e3
97 changed files with 2166 additions and 2242 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.cproject
View File

@ -21,7 +21,7 @@
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="" buildPath="${workspace_loc:/gtsam/build}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder arguments="" buildPath="${workspace_loc:/gtsam/build}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">

2
.project
View File

@ -31,7 +31,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildLocation</key>
<value>${workspace_loc:/gtsam/build-fast}</value>
<value>${workspace_loc:/gtsam/build}</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>

15
examples/CameraResectioning.cpp
View File

@ -16,7 +16,7 @@
* @date Aug 23, 2011
*/
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h>
@ -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;

27
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -19,7 +19,7 @@
#include <iostream>
// for all nonlinear keys
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
// for points and poses
#include <gtsam/geometry/Point2.h>
@ -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

37
examples/SimpleRotation.cpp
View File

@ -22,7 +22,7 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
@ -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");

38
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -23,7 +23,7 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
@ -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
examples/vSLAMexample/vISAMexample.cpp
View File

@ -20,7 +20,7 @@
#include <boost/foreach.hpp>
using namespace boost;
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

2
examples/vSLAMexample/vSFMexample.cpp
View File

@ -19,7 +19,7 @@
#include <boost/shared_ptr.hpp>
using namespace boost;
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

10
gtsam/base/DerivedValue.h
View File

@ -83,6 +83,16 @@ public:
return (static_cast<DERIVED*>(this))->operator=(derivedRhs);
}
/// Conversion to the derived class
operator const DERIVED& () const {
return static_cast<const DERIVED&>(*this);
}
/// Conversion to the derived class
operator DERIVED& () {
return static_cast<DERIVED&>(*this);
}
protected:
/// Assignment operator, protected because only the Value or DERIVED
/// assignment operators should be used.

10
gtsam/base/Makefile.am
View File

@ -36,6 +36,11 @@ headers += BTree.h DSF.h FastMap.h FastSet.h FastList.h FastVector.h
sources += DSFVector.cpp
check_PROGRAMS += tests/testBTree tests/testDSF tests/testDSFVector
## flag disabled to force this test to get updated properly
if ENABLE_SERIALIZATION
check_PROGRAMS += tests/testSerializationBase
endif
# Timing tests
noinst_PROGRAMS = tests/timeMatrix tests/timeVirtual tests/timeVirtual2 tests/timeTest
noinst_PROGRAMS += tests/timeMatrixOps
@ -56,6 +61,11 @@ AM_CPPFLAGS =
AM_CPPFLAGS += $(BOOST_CPPFLAGS) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS)
# link to serialization library for test
if ENABLE_SERIALIZATION
tests_testSerializationBase_LDFLAGS = -lboost_serialization
endif
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------

148
gtsam/base/serializationTestHelpers.h Normal file
View File

@ -0,0 +1,148 @@
/* ----------------------------------------------------------------------------
* 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 serializationTestHelpers.h
* @brief
* @author Richard Roberts
* @date Feb 7, 2012
*/
#pragma once
#include <sstream>
#include <string>
// includes for standard serialization types
#include <boost/serialization/export.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/list.hpp>
#include <boost/serialization/deque.hpp>
#include <boost/serialization/weak_ptr.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
// whether to print the serialized text to stdout
const bool verbose = false;
namespace gtsam { namespace serializationTestHelpers {
/* ************************************************************************* */
// Serialization testing code.
/* ************************************************************************* */
template<class T>
std::string serialize(const T& input) {
std::ostringstream out_archive_stream;
boost::archive::text_oarchive out_archive(out_archive_stream);
out_archive << input;
return out_archive_stream.str();
}
template<class T>
void deserialize(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
boost::archive::text_iarchive in_archive(in_archive_stream);
in_archive >> output;
}
// Templated round-trip serialization
template<class T>
void roundtrip(const T& input, T& output) {
// Serialize
std::string serialized = serialize(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
deserialize(serialized, output);
}
// This version requires equality operator
template<class T>
bool equality(const T& input = T()) {
T output;
roundtrip<T>(input,output);
return input==output;
}
// This version requires equals
template<class T>
bool equalsObj(const T& input = T()) {
T output;
roundtrip<T>(input,output);
return input.equals(output);
}
// De-referenced version for pointers
template<class T>
bool equalsDereferenced(const T& input) {
T output;
roundtrip<T>(input,output);
return input->equals(*output);
}
/* ************************************************************************* */
template<class T>
std::string serializeXML(const T& input) {
std::ostringstream out_archive_stream;
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp("data", input);
return out_archive_stream.str();
}
template<class T>
void deserializeXML(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
boost::archive::xml_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp("data", output);
}
// Templated round-trip serialization using XML
template<class T>
void roundtripXML(const T& input, T& output) {
// Serialize
std::string serialized = serializeXML<T>(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
// De-serialize
deserializeXML(serialized, output);
}
// This version requires equality operator
template<class T>
bool equalityXML(const T& input = T()) {
T output;
roundtripXML<T>(input,output);
return input==output;
}
// This version requires equals
template<class T>
bool equalsXML(const T& input = T()) {
T output;
roundtripXML<T>(input,output);
return input.equals(output);
}
// This version is for pointers
template<class T>
bool equalsDereferencedXML(const T& input = T()) {
T output;
roundtripXML<T>(input,output);
return input->equals(*output);
}
} }

41
gtsam/base/tests/testSerializationBase.cpp Normal file
View File

@ -0,0 +1,41 @@
/* ----------------------------------------------------------------------------
* 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 testSerializationBase.cpp
* @brief
* @author Richard Roberts
* @date Feb 7, 2012
*/
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */
TEST (Serialization, matrix_vector) {
EXPECT(equality<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
EXPECT(equality<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityXML<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityXML<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

10
gtsam/geometry/Makefile.am
View File

@ -32,6 +32,11 @@ headers += Tensor1Expression.h Tensor2Expression.h Tensor3Expression.h Tensor5Ex
sources += projectiveGeometry.cpp tensorInterface.cpp
check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testFundamental
## flag disabled to force this test to get updated properly
if ENABLE_SERIALIZATION
check_PROGRAMS += tests/testSerializationGeometry
endif
# Timing tests
noinst_PROGRAMS = tests/timeRot3 tests/timePose3 tests/timeCalibratedCamera tests/timeStereoCamera
@ -53,6 +58,11 @@ libgeometry_la_SOURCES = $(allsources)
AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS)
# link to serialization library for test
if ENABLE_SERIALIZATION
tests_testSerializationGeometry_LDFLAGS = -lboost_serialization
endif
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------

115
gtsam/geometry/tests/testSerializationGeometry.cpp Normal file
View File

@ -0,0 +1,115 @@
/* ----------------------------------------------------------------------------
* 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 testSerializationGeometry.cpp
* @brief
* @author Richard Roberts
* @date Feb 7, 2012
*/
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */
// Export all classes derived from Value
BOOST_CLASS_EXPORT(gtsam::Cal3_S2)
BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo)
BOOST_CLASS_EXPORT(gtsam::Cal3Bundler)
BOOST_CLASS_EXPORT(gtsam::CalibratedCamera)
BOOST_CLASS_EXPORT(gtsam::Point2)
BOOST_CLASS_EXPORT(gtsam::Point3)
BOOST_CLASS_EXPORT(gtsam::Pose2)
BOOST_CLASS_EXPORT(gtsam::Pose3)
BOOST_CLASS_EXPORT(gtsam::Rot2)
BOOST_CLASS_EXPORT(gtsam::Rot3)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
BOOST_CLASS_EXPORT(gtsam::StereoPoint2)
/* ************************************************************************* */
Point3 pt3(1.0, 2.0, 3.0);
Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
Pose3 pose3(rt3, pt3);
Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
Cal3Bundler cal3(1.0, 2.0, 3.0);
Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
CalibratedCamera cal5(Pose3(rt3, pt3));
PinholeCamera<Cal3_S2> cam1(pose3, cal1);
StereoCamera cam2(pose3, cal4ptr);
StereoPoint2 spt(1.0, 2.0, 3.0);
/* ************************************************************************* */
TEST (Serialization, text_geometry) {
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
EXPECT(equalsObj(pt3));
EXPECT(equalsObj<gtsam::Rot3>(rt3));
EXPECT(equalsObj<gtsam::Pose3>(Pose3(rt3, pt3)));
EXPECT(equalsObj(cal1));
EXPECT(equalsObj(cal2));
EXPECT(equalsObj(cal3));
EXPECT(equalsObj(cal4));
EXPECT(equalsObj(cal5));
EXPECT(equalsObj(cam1));
EXPECT(equalsObj(cam2));
EXPECT(equalsObj(spt));
}
/* ************************************************************************* */
TEST (Serialization, xml_geometry) {
EXPECT(equalsXML<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
EXPECT(equalsXML<gtsam::Point3>(pt3));
EXPECT(equalsXML<gtsam::Rot3>(rt3));
EXPECT(equalsXML<gtsam::Pose3>(Pose3(rt3, pt3)));
EXPECT(equalsXML(cal1));
EXPECT(equalsXML(cal2));
EXPECT(equalsXML(cal3));
EXPECT(equalsXML(cal4));
EXPECT(equalsXML(cal5));
EXPECT(equalsXML(cam1));
EXPECT(equalsXML(cam2));
EXPECT(equalsXML(spt));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

10
gtsam/inference/Makefile.am
View File

@ -45,6 +45,11 @@ check_PROGRAMS += tests/testClusterTree
check_PROGRAMS += tests/testJunctionTree
check_PROGRAMS += tests/testPermutation
## flag disabled to force this test to get updated properly
if ENABLE_SERIALIZATION
check_PROGRAMS += tests/testSerializationInference
endif
#----------------------------------------------------------------------------------------------------
# Create a libtool library that is not installed
# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am
@ -64,6 +69,11 @@ AM_CPPFLAGS = $(ccolamd_inc) $(BOOST_CPPFLAGS) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS)
AM_CXXFLAGS =
# link to serialization library for test
if ENABLE_SERIALIZATION
tests_testSerializationInference_LDFLAGS = -lboost_serialization
endif
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------

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

2
gtsam/inference/tests/testFactorGraph.cpp
View File

@ -25,7 +25,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/inference/SymbolicFactorGraph.h>

2
gtsam/inference/tests/testISAM.cpp
View File

@ -21,7 +21,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/inference/BayesNet.h>

2
gtsam/inference/tests/testJunctionTree.cpp
View File

@ -24,7 +24,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/nonlinear/Ordering.h>

91
gtsam/inference/tests/testSerializationInference.cpp Normal file
View File

@ -0,0 +1,91 @@
/* ----------------------------------------------------------------------------
* 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 testSerializationInference.cpp
* @brief
* @author Richard Roberts
* @date Feb 7, 2012
*/
#define GTSAM_MAGIC_KEY
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */
TEST (Serialization, symbolic_graph) {
Ordering o; o += "x1","l1","x2";
// construct expected symbolic graph
SymbolicFactorGraph sfg;
sfg.push_factor(o["x1"]);
sfg.push_factor(o["x1"],o["x2"]);
sfg.push_factor(o["x1"],o["l1"]);
sfg.push_factor(o["l1"],o["x2"]);
EXPECT(equalsObj(sfg));
EXPECT(equalsXML(sfg));
}
/* ************************************************************************* */
TEST (Serialization, symbolic_bn) {
Ordering o; o += "x2","l1","x1";
IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"]));
IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"]));
IndexConditional::shared_ptr x1(new IndexConditional(o["x1"]));
SymbolicBayesNet sbn;
sbn.push_back(x2);
sbn.push_back(l1);
sbn.push_back(x1);
EXPECT(equalsObj(sbn));
EXPECT(equalsXML(sbn));
}
/* ************************************************************************* */
TEST (Serialization, symbolic_bayes_tree ) {
typedef BayesTree<IndexConditional> SymbolicBayesTree;
static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
IndexConditional::shared_ptr
B(new IndexConditional(_B_)),
L(new IndexConditional(_L_, _B_)),
E(new IndexConditional(_E_, _L_, _B_)),
S(new IndexConditional(_S_, _L_, _B_)),
T(new IndexConditional(_T_, _E_, _L_)),
X(new IndexConditional(_X_, _E_));
// Bayes Tree for Asia example
SymbolicBayesTree bayesTree;
SymbolicBayesTree::insert(bayesTree, B);
SymbolicBayesTree::insert(bayesTree, L);
SymbolicBayesTree::insert(bayesTree, E);
SymbolicBayesTree::insert(bayesTree, S);
SymbolicBayesTree::insert(bayesTree, T);
SymbolicBayesTree::insert(bayesTree, X);
EXPECT(equalsObj(bayesTree));
EXPECT(equalsXML(bayesTree));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

2
gtsam/inference/tests/timeSymbolMaps.cpp
View File

@ -22,7 +22,7 @@
#include <vector>
#include <map>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
using namespace std;
using namespace boost;

10
gtsam/linear/Makefile.am
View File

@ -41,6 +41,11 @@ sources += iterative.cpp SubgraphPreconditioner.cpp SubgraphSolver.cpp
headers += IterativeSolver.h IterativeOptimizationParameters.h
headers += SubgraphSolver-inl.h
## flag disabled to force this test to get updated properly
if ENABLE_SERIALIZATION
check_PROGRAMS += tests/testSerializationLinear
endif
# Timing tests
noinst_PROGRAMS = tests/timeGaussianFactor tests/timeFactorOverhead tests/timeSLAMlike
@ -58,6 +63,11 @@ AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS)
AM_CXXFLAGS =
# link to serialization library for test
if ENABLE_SERIALIZATION
tests_testSerializationLinear_LDFLAGS = -lboost_serialization
endif
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------

161
gtsam/linear/tests/testSerializationLinear.cpp Normal file
View File

@ -0,0 +1,161 @@
/* ----------------------------------------------------------------------------
* 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 testSerializationLinear.cpp
* @brief
* @author Richard Roberts
* @date Feb 7, 2012
*/
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedNoiseModel.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/linear/SharedGaussian.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */
// Noise model components
/* ************************************************************************* */
/* ************************************************************************* */
// Export Noisemodels
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* ************************************************************************* */
// example noise models
noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1));
noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
/* ************************************************************************* */
TEST (Serialization, noiseModels) {
// tests using pointers to the derived class
EXPECT( equalsDereferenced<noiseModel::Diagonal::shared_ptr>(diag3));
EXPECT(equalsDereferencedXML<noiseModel::Diagonal::shared_ptr>(diag3));
EXPECT( equalsDereferenced<noiseModel::Gaussian::shared_ptr>(gaussian3));
EXPECT(equalsDereferencedXML<noiseModel::Gaussian::shared_ptr>(gaussian3));
EXPECT( equalsDereferenced<noiseModel::Isotropic::shared_ptr>(iso3));
EXPECT(equalsDereferencedXML<noiseModel::Isotropic::shared_ptr>(iso3));
EXPECT( equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
EXPECT( equalsDereferenced<noiseModel::Unit::shared_ptr>(unit3));
EXPECT(equalsDereferencedXML<noiseModel::Unit::shared_ptr>(unit3));
}
/* ************************************************************************* */
TEST (Serialization, SharedNoiseModel_noiseModels) {
SharedNoiseModel diag3_sg = diag3;
EXPECT(equalsDereferenced<SharedNoiseModel>(diag3_sg));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3_sg));
EXPECT(equalsDereferenced<SharedNoiseModel>(diag3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3));
EXPECT(equalsDereferenced<SharedNoiseModel>(iso3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(iso3));
EXPECT(equalsDereferenced<SharedNoiseModel>(gaussian3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(gaussian3));
EXPECT(equalsDereferenced<SharedNoiseModel>(unit3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(unit3));
EXPECT(equalsDereferenced<SharedNoiseModel>(constrained3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(constrained3));
}
/* ************************************************************************* */
TEST (Serialization, SharedDiagonal_noiseModels) {
EXPECT(equalsDereferenced<SharedDiagonal>(diag3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(diag3));
EXPECT(equalsDereferenced<SharedDiagonal>(iso3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(iso3));
EXPECT(equalsDereferenced<SharedDiagonal>(unit3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(unit3));
EXPECT(equalsDereferenced<SharedDiagonal>(constrained3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(constrained3));
}
/* ************************************************************************* */
// Linear components
/* ************************************************************************* */
/* Create GUIDs for factors */
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
/* ************************************************************************* */
TEST (Serialization, linear_factors) {
VectorValues values;
values.insert(0, Vector_(1, 1.0));
values.insert(1, Vector_(2, 2.0,3.0));
values.insert(2, Vector_(2, 4.0,5.0));
EXPECT(equalsObj<VectorValues>(values));
EXPECT(equalsXML<VectorValues>(values));
Index i1 = 4, i2 = 7;
Matrix A1 = eye(3), A2 = -1.0 * eye(3);
Vector b = ones(3);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0));
JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
EXPECT(equalsObj(jacobianfactor));
EXPECT(equalsXML(jacobianfactor));
HessianFactor hessianfactor(jacobianfactor);
EXPECT(equalsObj(hessianfactor));
EXPECT(equalsXML(hessianfactor));
}
/* ************************************************************************* */
TEST (Serialization, gaussian_conditional) {
Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.);
Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4);
Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34);
Vector d(2); d << 0.2, 0.5;
GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2));
EXPECT(equalsObj(cg));
EXPECT(equalsXML(cg));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

3
gtsam/linear/tests/timeGaussianFactor.cpp
View File

@ -15,9 +15,6 @@
* @author Alireza Fathi
*/
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
#define GTSAM_MAGIC_KEY
#include <time.h>
/*STL/C++*/

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:

371
gtsam/nonlinear/Key.h
View File

@ -1,371 +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 Key.h
* @date Jan 12, 2010
* @author: Frank Dellaert
* @author: Richard Roberts
*/
#pragma once
#include <list>
#include <iostream>
#include <boost/mpl/char.hpp>
#include <boost/format.hpp>
#include <boost/serialization/nvp.hpp>
#ifdef GTSAM_MAGIC_KEY
#include <boost/lexical_cast.hpp>
#endif
#define ALPHA '\224'
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
* 2) the character constant used for its string representation
*/
template<class T, char C>
class TypedSymbol {
protected:
size_t j_;
public:
// typedefs
typedef T Value;
typedef boost::mpl::char_<C> Chr; // to reconstruct the type: use Chr::value
// Constructors:
TypedSymbol() :
j_(0) {
}
TypedSymbol(size_t j) :
j_(j) {
}
virtual ~TypedSymbol() {}
// Get stuff:
///TODO: comment
size_t index() const {
return j_;
}
static char chr() {
return C;
}
const char* c_str() const {
return ((std::string) (*this)).c_str();
}
operator std::string() const {
return (boost::format("%c%d") % C % j_).str();
}
std::string latex() const {
return (boost::format("%c_{%d}") % C % j_).str();
}
Symbol symbol() const;
// logic:
bool operator<(const TypedSymbol& compare) const {
return j_ < compare.j_;
}
bool operator==(const TypedSymbol& compare) const {
return j_ == compare.j_;
}
bool operator!=(const TypedSymbol& compare) const {
return j_ != compare.j_;
}
int compare(const TypedSymbol& compare) const {
return j_ - compare.j_;
}
// Testable Requirements
virtual void print(const std::string& s = "") const {
std::cout << s << ": " << (std::string) (*this) << std::endl;
}
bool equals(const TypedSymbol& expected, double tol = 0.0) const {
return (*this) == expected;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(j_);
}
};
/** forward declaration to avoid circular dependencies */
template<class T, char C, typename L>
class TypedLabeledSymbol;
/**
* Character and index key used in VectorValues, GaussianFactorGraph,
* GaussianFactor, etc. These keys are generated at runtime from TypedSymbol
* keys when linearizing a nonlinear factor graph. This key is not type
* safe, so cannot be used with any Nonlinear* classes.
*/
class Symbol {
protected:
unsigned char c_;
size_t j_;
public:
/** Default constructor */
Symbol() :
c_(0), j_(0) {
}
/** Copy constructor */
Symbol(const Symbol& key) :
c_(key.c_), j_(key.j_) {
}
/** Constructor */
Symbol(unsigned char c, size_t j) :
c_(c), j_(j) {
}
/** Casting constructor from TypedSymbol */
template<class T, char C>
Symbol(const TypedSymbol<T, C>& symbol) :
c_(C), j_(symbol.index()) {
}
/** Casting constructor from TypedLabeledSymbol */
template<class T, char C, typename L>
Symbol(const TypedLabeledSymbol<T, C, L>& symbol) :
c_(C), j_(symbol.encode()) {
}
/** "Magic" key casting constructor from string */
#ifdef GTSAM_MAGIC_KEY
Symbol(const std::string& str) {
if(str.length() < 1)
throw std::invalid_argument("Cannot parse string key '" + str + "'");
else {
const char *c_str = str.c_str();
c_ = c_str[0];
if(str.length() > 1)
j_ = boost::lexical_cast<size_t>(c_str+1);
else
j_ = 0;
}
}
Symbol(const char *c_str) {
std::string str(c_str);
if(str.length() < 1)
throw std::invalid_argument("Cannot parse string key '" + str + "'");
else {
c_ = c_str[0];
if(str.length() > 1)
j_ = boost::lexical_cast<size_t>(c_str+1);
else
j_ = 0;
}
}
#endif
// Testable Requirements
void print(const std::string& s = "") const {
std::cout << s << ": " << (std::string) (*this) << std::endl;
}
bool equals(const Symbol& expected, double tol = 0.0) const {
return (*this) == expected;
}
/** Retrieve key character */
unsigned char chr() const {
return c_;
}
/** Retrieve key index */
size_t index() const {
return j_;
}
/** Create a string from the key */
operator std::string() const {
return str(boost::format("%c%d") % c_ % j_);
}
/** Comparison for use in maps */
bool operator<(const Symbol& comp) const {
return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_);
}
bool operator==(const Symbol& comp) const {
return comp.c_ == c_ && comp.j_ == j_;
}
bool operator!=(const Symbol& comp) const {
return comp.c_ != c_ || comp.j_ != j_;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(c_);
ar & BOOST_SERIALIZATION_NVP(j_);
}
};
// Conversion utilities
template<class KEY> Symbol key2symbol(KEY key) {
return Symbol(key);
}
template<class KEY> std::list<Symbol> keys2symbols(std::list<KEY> keys) {
std::list<Symbol> symbols;
std::transform(keys.begin(), keys.end(), std::back_inserter(symbols),
key2symbol<KEY> );
return symbols;
}
/**
* TypedLabeledSymbol is a variation of the TypedSymbol that allows
* for a runtime label to be placed on the label, so as to express
* "Pose 5 for robot 3"
* Labels should be kept to base datatypes (int, char, etc) to
* minimize cost of comparisons
*
* The labels will be compared first when comparing Keys, followed by the
* index
*/
template<class T, char C, typename L>
class TypedLabeledSymbol: public TypedSymbol<T, C> {
protected:
// Label
L label_;
public:
typedef TypedSymbol<T, C> Base;
// Constructors:
TypedLabeledSymbol() {
}
TypedLabeledSymbol(size_t j, L label) :
Base(j), label_(label) {
}
/** Constructor that decodes encoded labels */
TypedLabeledSymbol(const Symbol& sym) :
TypedSymbol<T, C> (0) {
size_t shift = (sizeof(size_t) - sizeof(short)) * 8;
this->j_ = (sym.index() << shift) >> shift; // truncate upper bits
label_ = (L) (sym.index() >> shift); // remove lower bits
}
/** Constructor to upgrade an existing typed label with a label */
TypedLabeledSymbol(const Base& key, L label) :
Base(key.index()), label_(label) {
}
// Get stuff:
L label() const {
return label_;
}
const char* c_str() const {
return ((std::string)(*this)).c_str();
}
operator std::string() const {
std::string label_s = (boost::format("%1%") % label_).str();
return (boost::format("%c%s_%d") % C % label_s % this->j_).str();
}
std::string latex() const {
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 {
return label_;
}
/**
* Encoding two numbers into a single size_t for conversion to Symbol
* Stores the label in the upper bytes of the index
*/
size_t encode() const {
short label = (short) label_; //bound size of label to 2 bytes
size_t shift = (sizeof(size_t) - sizeof(short)) * 8;
size_t modifier = ((size_t) label) << shift;
return this->j_ + modifier;
}
// logic:
bool operator<(const TypedLabeledSymbol& compare) const {
if (label_ == compare.label_) // sort by label first
return this->j_ < compare.j_;
else
return label_ < compare.label_;
}
bool operator==(const TypedLabeledSymbol& compare) const {
return this->j_ == compare.j_ && label_ == compare.label_;
}
int compare(const TypedLabeledSymbol& compare) const {
if (label_ == compare.label_) // sort by label first
return this->j_ - compare.j_;
else
return label_ - compare.label_;
}
// Testable Requirements
void print(const std::string& s = "") const {
std::cout << s << ": " << (std::string) (*this) << std::endl;
}
bool equals(const TypedLabeledSymbol& expected, double tol = 0.0) const {
return (*this) == expected;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
typedef TypedSymbol<T, C> Base;
ar & boost::serialization::make_nvp("TypedLabeledSymbol",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(label_);
}
};
/* ************************************************************************* */
template<class T, char C>
Symbol TypedSymbol<T,C>::symbol() const {
return Symbol(*this);
}
} // namespace gtsam

12
gtsam/nonlinear/Makefile.am
View File

@ -21,7 +21,7 @@ sources += Values.cpp
check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering
# Nonlinear nonlinear
headers += Key.h
headers += Symbol.h
headers += NonlinearFactorGraph.h
headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h
headers += NonlinearFactor.h
@ -44,6 +44,11 @@ headers += WhiteNoiseFactor.h
# Kalman Filter
headers += ExtendedKalmanFilter.h ExtendedKalmanFilter-inl.h
## flag disabled to force this test to get updated properly
if ENABLE_SERIALIZATION
check_PROGRAMS += tests/testSerializationNonlinear
endif
#----------------------------------------------------------------------------------------------------
# Create a libtool library that is not installed
# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am
@ -58,6 +63,11 @@ AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS)
AM_CXXFLAGS =
# link to serialization library for test
if ENABLE_SERIALIZATION
tests_testSerializationNonlinear_LDFLAGS = -lboost_serialization
endif
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------

65
gtsam/nonlinear/NonlinearEquality.h
View File

@ -47,11 +47,11 @@ namespace gtsam {
*
* \nosubgrouping
*/
template<class KEY>
class NonlinearEquality: public NonlinearFactor1<KEY> {
template<class VALUE>
class NonlinearEquality: public NonlinearFactor1<VALUE> {
public:
typedef typename KEY::Value T;
typedef VALUE T;
private:
@ -64,6 +64,12 @@ namespace gtsam {
// error gain in allow error case
double error_gain_;
// typedef to this class
typedef NonlinearEquality<VALUE> This;
// typedef to base class
typedef NonlinearFactor1<VALUE> Base;
public:
/**
@ -71,7 +77,6 @@ namespace gtsam {
*/
bool (*compare_)(const T& a, const T& b);
typedef NonlinearFactor1<KEY> Base;
/** default constructor - only for serialization */
NonlinearEquality() {}
@ -84,7 +89,7 @@ namespace gtsam {
/**
* Constructor - forces exact evaluation
*/
NonlinearEquality(const KEY& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare<T>) :
NonlinearEquality(const Symbol& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare<T>) :
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(false), error_gain_(0.0),
compare_(_compare) {
@ -93,7 +98,7 @@ namespace gtsam {
/**
* Constructor - allows inexact evaluation
*/
NonlinearEquality(const KEY& j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare<T>) :
NonlinearEquality(const Symbol& j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare<T>) :
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(true), error_gain_(error_gain),
compare_(_compare) {
@ -103,17 +108,17 @@ namespace gtsam {
/// @name Testable
/// @{
void print(const std::string& s = "") const {
std::cout << "Constraint: " << s << " on [" << (std::string)(this->key_) << "]\n";
virtual void print(const std::string& s = "") const {
std::cout << "Constraint: " << s << " on [" << (std::string)(this->key()) << "]\n";
gtsam::print(feasible_,"Feasible Point");
std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
}
/** Check if two factors are equal */
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;
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&f);
return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) &&
fabs(error_gain_ - e->error_gain_) < tol;
}
/// @}
@ -122,7 +127,7 @@ namespace gtsam {
/** actual error function calculation */
virtual double error(const Values& c) const {
const T& xj = c[this->key_];
const T& xj = c.at<T>(this->key());
Vector e = this->unwhitenedError(c);
if (allow_error_ || !compare_(xj, feasible_)) {
return error_gain_ * dot(e,e);
@ -132,7 +137,7 @@ namespace gtsam {
}
/** error function */
inline Vector evaluateError(const T& xj, boost::optional<Matrix&> H = boost::none) const {
Vector evaluateError(const T& xj, boost::optional<Matrix&> H = boost::none) const {
size_t nj = feasible_.dim();
if (allow_error_) {
if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
@ -142,18 +147,18 @@ namespace gtsam {
return zero(nj); // set error to zero if equal
} else {
if (H) throw std::invalid_argument(
"Linearization point not feasible for " + (std::string)(this->key_) + "!");
"Linearization point not feasible for " + (std::string)(this->key()) + "!");
return repeat(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
}
}
// Linearize is over-written, because base linearization tries to whiten
virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const {
const T& xj = x[this->key_];
const T& xj = x.at<T>(this->key());
Matrix A;
Vector b = evaluateError(xj, A);
SharedDiagonal model = noiseModel::Constrained::All(b.size());
return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key_], A, b, model));
return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key()], A, b, model));
}
/// @}
@ -177,14 +182,14 @@ namespace gtsam {
/**
* Simple unary equality constraint - fixes a value for a variable
*/
template<class KEY>
class NonlinearEquality1 : public NonlinearFactor1<KEY> {
template<class VALUE>
class NonlinearEquality1 : public NonlinearFactor1<VALUE> {
public:
typedef typename KEY::Value X;
typedef VALUE X;
protected:
typedef NonlinearFactor1<KEY> Base;
typedef NonlinearFactor1<VALUE> Base;
/** default constructor to allow for serialization */
NonlinearEquality1() {}
@ -196,10 +201,10 @@ namespace gtsam {
public:
typedef boost::shared_ptr<NonlinearEquality1<KEY> > shared_ptr;
typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
///TODO: comment
NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0)
NonlinearEquality1(const X& value, const Symbol& key1, double mu = 1000.0)
: Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {}
virtual ~NonlinearEquality1() {}
@ -214,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");
}
@ -236,13 +241,13 @@ namespace gtsam {
* Simple binary equality constraint - this constraint forces two factors to
* be the same.
*/
template<class KEY>
class NonlinearEquality2 : public NonlinearFactor2<KEY, KEY> {
template<class VALUE>
class NonlinearEquality2 : public NonlinearFactor2<VALUE, VALUE> {
public:
typedef typename KEY::Value X;
typedef VALUE X;
protected:
typedef NonlinearFactor2<KEY, KEY> Base;
typedef NonlinearFactor2<VALUE, VALUE> Base;
GTSAM_CONCEPT_MANIFOLD_TYPE(X);
@ -251,10 +256,10 @@ namespace gtsam {
public:
typedef boost::shared_ptr<NonlinearEquality2<KEY> > shared_ptr;
typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr;
///TODO: comment
NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0)
NonlinearEquality2(const Symbol& key1, const Symbol& key2, double mu = 1000.0)
: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
virtual ~NonlinearEquality2() {}

316
gtsam/nonlinear/NonlinearFactor.h
View File

@ -68,7 +68,7 @@ protected:
public:
typedef boost::shared_ptr<NonlinearFactor > shared_ptr;
typedef boost::shared_ptr<NonlinearFactor> shared_ptr;
/// @name Standard Constructors
/// @{
@ -77,18 +77,6 @@ public:
NonlinearFactor() {
}
/**
* Constructor
* @param keys A boost::tuple containing the variables involved in this factor,
* example: <tt>NonlinearFactor(make_tuple(symbol1, symbol2, symbol3))</tt>
*/
template<class U1, class U2>
NonlinearFactor(const boost::tuples::cons<U1,U2>& keys) {
this->keys_.resize(boost::tuples::length<boost::tuples::cons<U1,U2> >::value);
// Use helper function to fill key vector, using 'cons' representation of tuple
__fill_from_tuple(this->keys(), 0, keys);
}
/**
* Constructor
* @param keys The variables involved in this factor
@ -107,6 +95,11 @@ public:
std::cout << s << ": NonlinearFactor\n";
}
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
return Base::equals(f);
}
/// @}
/// @name Standard Interface
/// @{
@ -186,16 +179,6 @@ public:
/** Destructor */
virtual ~NoiseModelFactor() {}
/**
* Constructor
* @param keys A boost::tuple containing the variables involved in this factor,
* example: <tt>NoiseModelFactor(noiseModel, make_tuple(symbol1, symbol2, symbol3)</tt>
*/
template<class U1, class U2>
NoiseModelFactor(const SharedNoiseModel& noiseModel, const boost::tuples::cons<U1,U2>& keys)
: Base(keys), noiseModel_(noiseModel) {
}
/**
* Constructor
* @param keys The variables involved in this factor
@ -224,8 +207,9 @@ public:
}
/** Check if two factors are equal */
virtual bool equals(const NoiseModelFactor& f, double tol = 1e-9) const {
return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, tol);
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
return e && Base::equals(f, tol) && noiseModel_->equals(*e->noiseModel_, tol);
}
/** get the dimension of the factor (number of rows on linearization) */
@ -320,21 +304,18 @@ private:
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 1
* variable. To derive from this class, implement evaluateError(). */
template<class KEY>
template<class VALUE>
class NonlinearFactor1: public NoiseModelFactor {
public:
// typedefs for value types pulled from keys
typedef typename KEY::Value X;
typedef VALUE X;
protected:
// The value of the key. Not const to allow serialization
KEY key_;
typedef NoiseModelFactor Base;
typedef NonlinearFactor1<KEY> This;
typedef NonlinearFactor1<VALUE> This;
public:
@ -343,22 +324,24 @@ public:
virtual ~NonlinearFactor1() {}
inline const KEY& key() const { return key_; }
inline const Symbol& key() const { return keys_[0]; }
/**
* Constructor
* @param z measurement
* @param key by which to look up X value in Values
*/
NonlinearFactor1(const SharedNoiseModel& noiseModel, const KEY& key1) :
Base(noiseModel, make_tuple(key1)), key_(key1) {
NonlinearFactor1(const SharedNoiseModel& noiseModel, const Symbol& key1) :
Base(noiseModel) {
keys_.resize(1);
keys_[0] = key1;
}
/** 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 {
if(this->active(x)) {
const X& x1 = x[key_];
const X& x1 = x.at<X>(keys_[0]);
if(H) {
return evaluateError(x1, (*H)[0]);
} else {
@ -371,7 +354,7 @@ public:
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor1(" << (std::string) this->key_ << ")\n";
std::cout << s << ": NonlinearFactor1(" << (std::string) keys_[0] << ")\n";
this->noiseModel_->print(" noise model: ");
}
@ -391,7 +374,6 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key_);
}
};// \class NonlinearFactor1
@ -399,23 +381,19 @@ private:
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 2
* variables. To derive from this class, implement evaluateError(). */
template<class KEY1, class KEY2>
template<class VALUE1, class VALUE2>
class NonlinearFactor2: public NoiseModelFactor {
public:
// typedefs for value types pulled from keys
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef VALUE1 X1;
typedef VALUE2 X2;
protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
typedef NoiseModelFactor Base;
typedef NonlinearFactor2<KEY1, KEY2> This;
typedef NonlinearFactor2<VALUE1, VALUE2> This;
public:
@ -429,21 +407,25 @@ public:
* @param j1 key of the first variable
* @param j2 key of the second variable
*/
NonlinearFactor2(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2) :
Base(noiseModel, make_tuple(j1,j2)), key1_(j1), key2_(j2) {}
NonlinearFactor2(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2) :
Base(noiseModel) {
keys_.resize(2);
keys_[0] = j1;
keys_[1] = j2;
}
virtual ~NonlinearFactor2() {}
/** methods to retrieve both keys */
inline const KEY1& key1() const { return key1_; }
inline const KEY2& key2() const { return key2_; }
inline const Symbol& key1() const { return keys_[0]; }
inline const Symbol& key2() const { return keys_[1]; }
/** 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 {
if(this->active(x)) {
const X1& x1 = x[key1_];
const X2& x2 = x[key2_];
const X1& x1 = x.at<X1>(keys_[0]);
const X2& x2 = x.at<X2>(keys_[1]);
if(H) {
return evaluateError(x1, x2, (*H)[0], (*H)[1]);
} else {
@ -457,8 +439,8 @@ public:
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor2("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ")\n";
<< (std::string) keys_[0] << ","
<< (std::string) keys_[1] << ")\n";
this->noiseModel_->print(" noise model: ");
}
@ -479,33 +461,26 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
}
}; // \class NonlinearFactor2
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 3
* variables. To derive from this class, implement evaluateError(). */
template<class KEY1, class KEY2, class KEY3>
template<class VALUE1, class VALUE2, class VALUE3>
class NonlinearFactor3: public NoiseModelFactor {
public:
// typedefs for value types pulled from keys
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef VALUE3 X3;
protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
typedef NoiseModelFactor Base;
typedef NonlinearFactor3<KEY1, KEY2, KEY3> This;
typedef NonlinearFactor3<VALUE1, VALUE2, VALUE3> This;
public:
@ -520,24 +495,29 @@ public:
* @param j2 key of the second variable
* @param j3 key of the third variable
*/
NonlinearFactor3(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3) :
Base(noiseModel, make_tuple(j1,j2,j3)), key1_(j1), key2_(j2), key3_(j3) {}
NonlinearFactor3(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3) :
Base(noiseModel) {
keys_.resize(3);
keys_[0] = j1;
keys_[1] = j2;
keys_[2] = j3;
}
virtual ~NonlinearFactor3() {}
/** methods to retrieve keys */
inline const KEY1& key1() const { return key1_; }
inline const KEY2& key2() const { return key2_; }
inline const KEY3& key3() const { return key3_; }
inline const Symbol& key1() const { return keys_[0]; }
inline const Symbol& key2() const { return keys_[1]; }
inline const Symbol& key3() const { return keys_[2]; }
/** 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 {
if(this->active(x)) {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
else
return evaluateError(x[key1_], x[key2_], x[key3_]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]));
} else {
return zero(this->dim());
}
@ -546,9 +526,9 @@ public:
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor3("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ","
<< (std::string) this->key3_ << ")\n";
<< (std::string) this->keys_[0] << ","
<< (std::string) this->keys_[1] << ","
<< (std::string) this->keys_[2] << ")\n";
this->noiseModel_->print(" noise model: ");
}
@ -572,36 +552,27 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
}
}; // \class NonlinearFactor3
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 4
* variables. To derive from this class, implement evaluateError(). */
template<class KEY1, class KEY2, class KEY3, class KEY4>
template<class VALUE1, class VALUE2, class VALUE3, class VALUE4>
class NonlinearFactor4: public NoiseModelFactor {
public:
// typedefs for value types pulled from keys
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
typedef typename KEY4::Value X4;
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef VALUE3 X3;
typedef VALUE4 X4;
protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
KEY4 key4_;
typedef NoiseModelFactor Base;
typedef NonlinearFactor4<KEY1, KEY2, KEY3, KEY4> This;
typedef NonlinearFactor4<VALUE1, VALUE2, VALUE3, VALUE4> This;
public:
@ -617,25 +588,31 @@ public:
* @param j3 key of the third variable
* @param j4 key of the fourth variable
*/
NonlinearFactor4(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4) :
Base(noiseModel, make_tuple(j1,j2,j3,j4)), key1_(j1), key2_(j2), key3_(j3), key4_(j4) {}
NonlinearFactor4(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4) :
Base(noiseModel) {
keys_.resize(4);
keys_[0] = j1;
keys_[1] = j2;
keys_[2] = j3;
keys_[3] = j4;
}
virtual ~NonlinearFactor4() {}
/** methods to retrieve keys */
inline const KEY1& key1() const { return key1_; }
inline const KEY2& key2() const { return key2_; }
inline const KEY3& key3() const { return key3_; }
inline const KEY4& key4() const { return key4_; }
inline const Symbol& key1() const { return keys_[0]; }
inline const Symbol& key2() const { return keys_[1]; }
inline const Symbol& key3() const { return keys_[2]; }
inline const Symbol& key4() const { return keys_[3]; }
/** 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 {
if(this->active(x)) {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]));
} else {
return zero(this->dim());
}
@ -644,10 +621,10 @@ public:
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor4("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ","
<< (std::string) this->key3_ << ","
<< (std::string) this->key4_ << ")\n";
<< (std::string) this->keys_[0] << ","
<< (std::string) this->keys_[1] << ","
<< (std::string) this->keys_[2] << ","
<< (std::string) this->keys_[3] << ")\n";
this->noiseModel_->print(" noise model: ");
}
@ -671,39 +648,28 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
ar & BOOST_SERIALIZATION_NVP(key4_);
}
}; // \class NonlinearFactor4
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 5
* variables. To derive from this class, implement evaluateError(). */
template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5>
template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5>
class NonlinearFactor5: public NoiseModelFactor {
public:
// typedefs for value types pulled from keys
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
typedef typename KEY4::Value X4;
typedef typename KEY5::Value X5;
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef VALUE3 X3;
typedef VALUE4 X4;
typedef VALUE5 X5;
protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
KEY4 key4_;
KEY5 key5_;
typedef NoiseModelFactor Base;
typedef NonlinearFactor5<KEY1, KEY2, KEY3, KEY4, KEY5> This;
typedef NonlinearFactor5<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5> This;
public:
@ -720,26 +686,33 @@ public:
* @param j4 key of the fourth variable
* @param j5 key of the fifth variable
*/
NonlinearFactor5(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5) :
Base(noiseModel, make_tuple(j1,j2,j3,j4,j5)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5) {}
NonlinearFactor5(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4, const Symbol& j5) :
Base(noiseModel) {
keys_.resize(5);
keys_[0] = j1;
keys_[1] = j2;
keys_[2] = j3;
keys_[3] = j4;
keys_[4] = j5;
}
virtual ~NonlinearFactor5() {}
/** methods to retrieve keys */
inline const KEY1& key1() const { return key1_; }
inline const KEY2& key2() const { return key2_; }
inline const KEY3& key3() const { return key3_; }
inline const KEY4& key4() const { return key4_; }
inline const KEY5& key5() const { return key5_; }
inline const Symbol& key1() const { return keys_[0]; }
inline const Symbol& key2() const { return keys_[1]; }
inline const Symbol& key3() const { return keys_[2]; }
inline const Symbol& key4() const { return keys_[3]; }
inline const Symbol& key5() const { return keys_[4]; }
/** 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 {
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]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]));
} else {
return zero(this->dim());
}
@ -748,11 +721,11 @@ public:
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor5("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ","
<< (std::string) this->key3_ << ","
<< (std::string) this->key4_ << ","
<< (std::string) this->key5_ << ")\n";
<< (std::string) this->keys_[0] << ","
<< (std::string) this->keys_[1] << ","
<< (std::string) this->keys_[2] << ","
<< (std::string) this->keys_[3] << ","
<< (std::string) this->keys_[4] << ")\n";
this->noiseModel_->print(" noise model: ");
}
@ -777,42 +750,29 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
ar & BOOST_SERIALIZATION_NVP(key4_);
ar & BOOST_SERIALIZATION_NVP(key5_);
}
}; // \class NonlinearFactor5
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 6
* variables. To derive from this class, implement evaluateError(). */
template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5, class KEY6>
template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5, class VALUE6>
class NonlinearFactor6: public NoiseModelFactor {
public:
// typedefs for value types pulled from keys
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
typedef typename KEY4::Value X4;
typedef typename KEY5::Value X5;
typedef typename KEY6::Value X6;
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef VALUE3 X3;
typedef VALUE4 X4;
typedef VALUE5 X5;
typedef VALUE6 X6;
protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
KEY4 key4_;
KEY5 key5_;
KEY6 key6_;
typedef NoiseModelFactor Base;
typedef NonlinearFactor6<KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> This;
typedef NonlinearFactor6<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6> This;
public:
@ -830,27 +790,35 @@ public:
* @param j5 key of the fifth variable
* @param j6 key of the fifth variable
*/
NonlinearFactor6(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5, const KEY6& j6) :
Base(noiseModel, make_tuple(j1,j2,j3,j4,j5,j6)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5), key6_(j6) {}
NonlinearFactor6(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4, const Symbol& j5, const Symbol& j6) :
Base(noiseModel) {
keys_.resize(6);
keys_[0] = j1;
keys_[1] = j2;
keys_[2] = j3;
keys_[3] = j4;
keys_[4] = j5;
keys_[5] = j6;
}
virtual ~NonlinearFactor6() {}
/** methods to retrieve keys */
inline const KEY1& key1() const { return key1_; }
inline const KEY2& key2() const { return key2_; }
inline const KEY3& key3() const { return key3_; }
inline const KEY4& key4() const { return key4_; }
inline const KEY5& key5() const { return key5_; }
inline const KEY6& key6() const { return key6_; }
inline const Symbol& key1() const { return keys_[0]; }
inline const Symbol& key2() const { return keys_[1]; }
inline const Symbol& key3() const { return keys_[2]; }
inline const Symbol& key4() const { return keys_[3]; }
inline const Symbol& key5() const { return keys_[4]; }
inline const Symbol& key6() const { return keys_[5]; }
/** 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 {
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]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]));
} else {
return zero(this->dim());
}
@ -859,12 +827,12 @@ public:
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor6("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ","
<< (std::string) this->key3_ << ","
<< (std::string) this->key4_ << ","
<< (std::string) this->key5_ << ","
<< (std::string) this->key6_ << ")\n";
<< (std::string) this->keys_[0] << ","
<< (std::string) this->keys_[1] << ","
<< (std::string) this->keys_[2] << ","
<< (std::string) this->keys_[3] << ","
<< (std::string) this->keys_[4] << ","
<< (std::string) this->keys_[5] << ")\n";
this->noiseModel_->print(" noise model: ");
}
@ -890,12 +858,6 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
ar & BOOST_SERIALIZATION_NVP(key4_);
ar & BOOST_SERIALIZATION_NVP(key5_);
ar & BOOST_SERIALIZATION_NVP(key6_);
}
}; // \class NonlinearFactor6

2
gtsam/nonlinear/Ordering.h
View File

@ -19,7 +19,7 @@
#include <map>
#include <set>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/inference.h>
#include <boost/foreach.hpp>

136
gtsam/nonlinear/Symbol.h Normal file
View File

@ -0,0 +1,136 @@
/* ----------------------------------------------------------------------------
* 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 Symbol.h
* @date Jan 12, 2010
* @author: Frank Dellaert
* @author: Richard Roberts
*/
#pragma once
#include <list>
#include <iostream>
#include <boost/mpl/char.hpp>
#include <boost/format.hpp>
#include <boost/serialization/nvp.hpp>
#ifdef GTSAM_MAGIC_KEY
#include <boost/lexical_cast.hpp>
#endif
#define ALPHA '\224'
namespace gtsam {
/**
* Character and index key used in VectorValues, GaussianFactorGraph,
* GaussianFactor, etc. These keys are generated at runtime from TypedSymbol
* keys when linearizing a nonlinear factor graph. This key is not type
* safe, so cannot be used with any Nonlinear* classes.
*/
class Symbol {
protected:
unsigned char c_;
size_t j_;
public:
/** Default constructor */
Symbol() :
c_(0), j_(0) {
}
/** Copy constructor */
Symbol(const Symbol& key) :
c_(key.c_), j_(key.j_) {
}
/** Constructor */
Symbol(unsigned char c, size_t j) :
c_(c), j_(j) {
}
/** "Magic" key casting constructor from string */
#ifdef GTSAM_MAGIC_KEY
Symbol(const std::string& str) {
if(str.length() < 1)
throw std::invalid_argument("Cannot parse string key '" + str + "'");
else {
const char *c_str = str.c_str();
c_ = c_str[0];
if(str.length() > 1)
j_ = boost::lexical_cast<size_t>(c_str+1);
else
j_ = 0;
}
}
Symbol(const char *c_str) {
std::string str(c_str);
if(str.length() < 1)
throw std::invalid_argument("Cannot parse string key '" + str + "'");
else {
c_ = c_str[0];
if(str.length() > 1)
j_ = boost::lexical_cast<size_t>(c_str+1);
else
j_ = 0;
}
}
#endif
// Testable Requirements
void print(const std::string& s = "") const {
std::cout << s << ": " << (std::string) (*this) << std::endl;
}
bool equals(const Symbol& expected, double tol = 0.0) const {
return (*this) == expected;
}
/** Retrieve key character */
unsigned char chr() const {
return c_;
}
/** Retrieve key index */
size_t index() const {
return j_;
}
/** Create a string from the key */
operator std::string() const {
return str(boost::format("%c%d") % c_ % j_);
}
/** Comparison for use in maps */
bool operator<(const Symbol& comp) const {
return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_);
}
bool operator==(const Symbol& comp) const {
return comp.c_ == c_ && comp.j_ == j_;
}
bool operator!=(const Symbol& comp) const {
return comp.c_ != c_ || comp.j_ != j_;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(c_);
ar & BOOST_SERIALIZATION_NVP(j_);
}
};
} // namespace gtsam

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

@ -23,7 +23,10 @@
*/
#include <utility>
#include <boost/type_traits/conditional.hpp>
#include <boost/type_traits/is_base_of.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition
namespace gtsam {
@ -37,6 +40,30 @@ namespace gtsam {
ValueCloneAllocator() {}
};
/* ************************************************************************* */
class ValueAutomaticCasting {
const Symbol& key_;
const Value& value_;
public:
ValueAutomaticCasting(const Symbol& key, const Value& value) : key_(key), value_(value) {}
template<class ValueType>
operator const ValueType& () const {
// Check the type and throw exception if incorrect
if(typeid(ValueType) != typeid(value_))
throw ValuesIncorrectType(key_, typeid(ValueType), typeid(value_));
// We have already checked the type, so do a "blind" static_cast, not dynamic_cast
return static_cast<const ValueType&>(value_);
}
};
// /* ************************************************************************* */
// template<class ValueType>
// ValueType& operator=(ValueType& lhs, const ValueAutomaticCasting& rhs) {
// lhs = rhs;
// }
/* ************************************************************************* */
template<typename ValueType>
const ValueType& Values::at(const Symbol& j) const {
@ -55,6 +82,18 @@ namespace gtsam {
return static_cast<const ValueType&>(*item->second);
}
/* ************************************************************************* */
inline ValueAutomaticCasting 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);
return ValueAutomaticCasting(item->first, *item->second);
}
/* ************************************************************************* */
template<typename TypedKey>
const typename TypedKey::Value& Values::at(const TypedKey& j) const {
@ -65,6 +104,11 @@ namespace gtsam {
return at<typename TypedKey::Value>(symbol);
}
/* ************************************************************************* */
inline ValueAutomaticCasting Values::operator[](const Symbol& j) const {
return at(j);
}
/* ************************************************************************* */
template<typename ValueType>
boost::optional<const ValueType&> Values::exists(const Symbol& j) const {

15
gtsam/nonlinear/Values.h
View File

@ -37,13 +37,14 @@
#include <gtsam/base/Value.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
// Forward declarations
class ValueCloneAllocator;
class ValueAutomaticCasting;
/**
* A non-templated config holding any types of Manifold-group elements. A
@ -138,6 +139,15 @@ namespace gtsam {
template<typename ValueType>
const ValueType& at(const Symbol& j) const;
/** Retrieve a variable by key \c j. This non-templated version returns a
* special ValueAutomaticCasting object that may be assigned to the proper
* value.
* @param j Retrieve the value associated with this key
* @return A ValueAutomaticCasting object that may be assignmed to a Value
* of the proper type.
*/
ValueAutomaticCasting at(const Symbol& 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.
@ -153,6 +163,9 @@ namespace gtsam {
const typename TypedKey::Value& operator[](const TypedKey& j) const {
return at(j); }
/** operator[] syntax for at(const Symbol& j) */
ValueAutomaticCasting operator[](const Symbol& j) const;
/** 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. */

97
gtsam/nonlinear/tests/testKey.cpp
View File

@ -19,106 +19,11 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
using namespace std;
using namespace gtsam;
class Pose3;
/* ************************************************************************* */
TEST ( TypedSymbol, basic_operations ) {
typedef TypedSymbol<Pose3, 'x'> Key;
Key key1(0),
key2(0),
key3(1),
key4(2);
CHECK(key1.index()==0);
CHECK(key1 == key2);
CHECK(assert_equal(key1, key2));
CHECK(!(key1 == key3));
CHECK(key1 < key3);
CHECK(key3 < key4);
}
/* ************************************************************************* */
TEST ( TypedLabledSymbol, basic_operations ) {
typedef TypedSymbol<Pose3, 'x'> SimpleKey;
typedef TypedLabeledSymbol<Pose3, 'x', int> RobotKey;
SimpleKey key7(1);
RobotKey key1(0, 1),
key2(0, 1),
key3(1, 1),
key4(2, 1),
key5(0, 2),
key6(1, 2),
key8(1, 3),
key9(key7, 3);
CHECK(key1.label()==1);
CHECK(key1.index()==0);
CHECK(key1 == key2);
CHECK(assert_equal(key1, key2));
CHECK(!(key1 == key3));
CHECK(key1 < key3);
CHECK(key3 < key4);
CHECK(!(key1 == key5));
CHECK(key1 < key5);
CHECK(key5 < key6);
CHECK(assert_equal(key9, key8));
}
/* ************************************************************************* */
TEST ( TypedLabledSymbol, encoding ) {
typedef TypedLabeledSymbol<Pose3, 'x', char> RobotKey;
RobotKey key1(37, 'A');
// Note: calculations done in test due to possible differences between machines
// take the upper two bytes for the label
short label = key1.label();
// find the shift necessary
size_t shift = (sizeof(size_t)-sizeof(short)) * 8;
size_t modifier = label;
modifier = modifier << shift;
size_t index = key1.index() + modifier;
// check index encoding
Symbol act1(key1), exp('x', index);
CHECK(assert_equal(exp, act1));
// check casting
Symbol act2 = (Symbol) key1;
CHECK(assert_equal(exp, act2));
// decode
CHECK(assert_equal(key1, RobotKey(act1)));
}
/* ************************************************************************* */
TEST ( TypedLabledSymbol, template_reconstruction ) {
typedef TypedSymbol<Pose3, 'x'> Key;
typedef TypedLabeledSymbol<Key::Value, Key::Chr::value, char> NewKey;
NewKey k(1, 'A');
}
/* ************************************************************************* */
TEST ( Key, keys2symbols )
{
typedef TypedSymbol<int, 'x'> Key;
list<Symbol> expected;
expected += Key(1), Key(2), Key(3);
list<TypedSymbol<int, 'x'> > typeds;
typeds += 1, 2, 3;
CHECK(expected == keys2symbols(typeds));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

69
gtsam/nonlinear/tests/testSerializationNonlinear.cpp Normal file
View File

@ -0,0 +1,69 @@
/* ----------------------------------------------------------------------------
* 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 testSerializationNonlinear.cpp
* @brief
* @author Richard Roberts
* @date Feb 7, 2012
*/
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */
// Export all classes derived from Value
BOOST_CLASS_EXPORT(gtsam::Cal3_S2)
BOOST_CLASS_EXPORT(gtsam::Cal3Bundler)
BOOST_CLASS_EXPORT(gtsam::Point3)
BOOST_CLASS_EXPORT(gtsam::Pose3)
BOOST_CLASS_EXPORT(gtsam::Rot3)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
/* ************************************************************************* */
typedef PinholeCamera<Cal3_S2> PinholeCal3S2;
typedef PinholeCamera<Cal3DS2> PinholeCal3DS2;
typedef PinholeCamera<Cal3Bundler> PinholeCal3Bundler;
/* ************************************************************************* */
Point3 pt3(1.0, 2.0, 3.0);
Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
Pose3 pose3(rt3, pt3);
Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
Cal3Bundler cal3(1.0, 2.0, 3.0);
TEST (Serialization, TemplatedValues) {
Values values;
values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1));
values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2));
values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3));
values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1));
EXPECT(equalsObj(values));
EXPECT(equalsXML(values));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

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

@ -20,6 +20,8 @@
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#define GTSAM_MAGIC_KEY
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/geometry/Pose2.h>
@ -29,9 +31,7 @@ using namespace gtsam;
using namespace std;
static double inf = std::numeric_limits<double>::infinity();
typedef TypedSymbol<LieVector, 'v'> VecKey;
VecKey key1(1), key2(2), key3(3), key4(4);
Symbol key1("v1"), key2("v2"), key3("v3"), key4("v4");
/* ************************************************************************* */
TEST( Values, equals1 )
@ -114,11 +114,11 @@ TEST( Values, update_element )
cfg.insert(key1, v1);
CHECK(cfg.size() == 1);
CHECK(assert_equal(v1, cfg.at(key1)));
CHECK(assert_equal(v1, cfg.at<LieVector>(key1)));
cfg.update(key1, v2);
CHECK(cfg.size() == 1);
CHECK(assert_equal(v2, cfg.at(key1)));
CHECK(assert_equal(v2, cfg.at<LieVector>(key1)));
}
///* ************************************************************************* */
@ -200,10 +200,9 @@ TEST(Values, expmap_d)
CHECK(equal(config0, config0));
CHECK(config0.equals(config0));
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));
poseconfig.insert("p1", Pose2(1,2,3));
poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
CHECK(equal(config0, config0));
CHECK(config0.equals(config0));
@ -212,16 +211,15 @@ TEST(Values, expmap_d)
/* ************************************************************************* */
TEST(Values, extract_keys)
{
typedef TypedSymbol<Pose2, 'x'> PoseKey;
Values config;
config.insert(PoseKey(1), Pose2());
config.insert(PoseKey(2), Pose2());
config.insert(PoseKey(4), Pose2());
config.insert(PoseKey(5), Pose2());
config.insert("x1", Pose2());
config.insert("x2", Pose2());
config.insert("x4", Pose2());
config.insert("x5", Pose2());
FastList<Symbol> expected, actual;
expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5);
expected += "x1", "x2", "x4", "x5";
actual = config.keys();
CHECK(actual.size() == expected.size());
@ -238,7 +236,7 @@ TEST(Values, exists_)
config0.insert(key1, LieVector(Vector_(1, 1.)));
config0.insert(key2, LieVector(Vector_(1, 2.)));
boost::optional<const LieVector&> v = config0.exists(key1);
boost::optional<const LieVector&> v = config0.exists<LieVector>(key1);
CHECK(assert_equal(Vector_(1, 1.),*v));
}

30
gtsam/slam/BearingFactor.h
View File

@ -25,18 +25,18 @@ namespace gtsam {
/**
* Binary factor for a bearing measurement
*/
template<class POSEKEY, class POINTKEY>
class BearingFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
template<class POSE, class POINT>
class BearingFactor: public NonlinearFactor2<POSE, POINT> {
private:
typedef typename POSEKEY::Value Pose;
typedef typename POSEKEY::Value::Rotation Rot;
typedef typename POINTKEY::Value Point;
typedef POSE Pose;
typedef typename Pose::Rotation Rot;
typedef POINT Point;
typedef BearingFactor<POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
typedef BearingFactor<POSE, POINT> This;
typedef NonlinearFactor2<POSE, POINT> Base;
Rot z_; /** measurement */
Rot measured_; /** measurement */
/** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(Rot)
@ -48,9 +48,9 @@ namespace gtsam {
BearingFactor() {}
/** primary constructor */
BearingFactor(const POSEKEY& i, const POINTKEY& j, const Rot& z,
BearingFactor(const Symbol& poseKey, const Symbol& pointKey, const Rot& measured,
const SharedNoiseModel& model) :
Base(model, i, j), z_(z) {
Base(model, poseKey, pointKey), measured_(measured) {
}
virtual ~BearingFactor() {}
@ -59,18 +59,18 @@ namespace gtsam {
Vector evaluateError(const Pose& pose, const Point& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Rot hx = pose.bearing(point, H1, H2);
return Rot::Logmap(z_.between(hx));
return Rot::Logmap(measured_.between(hx));
}
/** return the measured */
inline const Rot measured() const {
return z_;
const Rot& measured() const {
return measured_;
}
/** equals */
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);
return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
}
private:
@ -81,7 +81,7 @@ namespace gtsam {
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(measured_);
}
}; // BearingFactor

46
gtsam/slam/BearingRangeFactor.h
View File

@ -27,20 +27,20 @@ namespace gtsam {
/**
* Binary factor for a bearing measurement
*/
template<class POSEKEY, class POINTKEY>
class BearingRangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
template<class POSE, class POINT>
class BearingRangeFactor: public NonlinearFactor2<POSE, POINT> {
private:
typedef typename POSEKEY::Value Pose;
typedef typename POSEKEY::Value::Rotation Rot;
typedef typename POINTKEY::Value Point;
typedef POSE Pose;
typedef typename POSE::Rotation Rot;
typedef POINT Point;
typedef BearingRangeFactor<POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
typedef BearingRangeFactor<POSE, POINT> This;
typedef NonlinearFactor2<POSE, POINT> Base;
// the measurement
Rot bearing_;
double range_;
Rot measuredBearing_;
double measuredRange_;
/** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(Rot)
@ -50,9 +50,9 @@ namespace gtsam {
public:
BearingRangeFactor() {} /* Default constructor */
BearingRangeFactor(const POSEKEY& i, const POINTKEY& j, const Rot& bearing, const double range,
BearingRangeFactor(const Symbol& poseKey, const Symbol& pointKey, const Rot& measuredBearing, const double measuredRange,
const SharedNoiseModel& model) :
Base(model, i, j), bearing_(bearing), range_(range) {
Base(model, poseKey, pointKey), measuredBearing_(measuredBearing), measuredRange_(measuredRange) {
}
virtual ~BearingRangeFactor() {}
@ -60,10 +60,10 @@ namespace gtsam {
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": BearingRangeFactor("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ")\n";
bearing_.print(" measured bearing");
std::cout << " measured range: " << range_ << std::endl;
<< (std::string) this->key1() << ","
<< (std::string) this->key2() << ")\n";
measuredBearing_.print(" measured bearing");
std::cout << " measured range: " << measuredRange_ << std::endl;
this->noiseModel_->print(" noise model");
}
@ -71,8 +71,8 @@ namespace gtsam {
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 &&
this->bearing_.equals(e->bearing_, tol);
fabs(this->measuredRange_ - e->measuredRange_) < tol &&
this->measuredBearing_.equals(e->measuredBearing_, tol);
}
/** h(x)-z -> between(z,h(x)) for Rot manifold */
@ -85,10 +85,10 @@ namespace gtsam {
boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>();
Rot y1 = pose.bearing(point, H11_, H12_);
Vector e1 = Rot::Logmap(bearing_.between(y1));
Vector e1 = Rot::Logmap(measuredBearing_.between(y1));
double y2 = pose.range(point, H21_, H22_);
Vector e2 = Vector_(1, y2 - range_);
Vector e2 = Vector_(1, y2 - measuredRange_);
if (H1) *H1 = gtsam::stack(2, &H11, &H21);
if (H2) *H2 = gtsam::stack(2, &H12, &H22);
@ -96,8 +96,8 @@ namespace gtsam {
}
/** return the measured */
inline const std::pair<Rot, double> measured() const {
return std::make_pair(bearing_, range_);
const std::pair<Rot, double> measured() const {
return std::make_pair(measuredBearing_, measuredRange_);
}
private:
@ -108,8 +108,8 @@ namespace gtsam {
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(bearing_);
ar & BOOST_SERIALIZATION_NVP(range_);
ar & BOOST_SERIALIZATION_NVP(measuredBearing_);
ar & BOOST_SERIALIZATION_NVP(measuredRange_);
}
}; // BearingRangeFactor

43
gtsam/slam/BetweenFactor.h
View File

@ -26,18 +26,21 @@ namespace gtsam {
/**
* A class for a measurement predicted by "between(config[key1],config[key2])"
* KEY1::Value is the Lie Group type
* T is the measurement type, by default the same
* @tparam VALUE the Value type
*/
template<class KEY1, class T = typename KEY1::Value>
class BetweenFactor: public NonlinearFactor2<KEY1, KEY1> {
template<class VALUE>
class BetweenFactor: public NonlinearFactor2<VALUE, VALUE> {
public:
typedef VALUE T;
private:
typedef BetweenFactor<KEY1, T> This;
typedef NonlinearFactor2<KEY1, KEY1> Base;
typedef BetweenFactor<VALUE> This;
typedef NonlinearFactor2<VALUE, VALUE> Base;
T measured_; /** The measurement */
VALUE measured_; /** The measurement */
/** concept check by type */
GTSAM_CONCEPT_LIE_TYPE(T)
@ -52,7 +55,7 @@ namespace gtsam {
BetweenFactor() {}
/** Constructor */
BetweenFactor(const KEY1& key1, const KEY1& key2, const T& measured,
BetweenFactor(const Symbol& key1, const Symbol& key2, const VALUE& measured,
const SharedNoiseModel& model) :
Base(model, key1, key2), measured_(measured) {
}
@ -64,8 +67,8 @@ namespace gtsam {
/** print */
virtual void print(const std::string& s) const {
std::cout << s << "BetweenFactor("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ")\n";
<< (std::string) this->key1() << ","
<< (std::string) this->key2() << ")\n";
measured_.print(" measured");
this->noiseModel_->print(" noise model");
}
@ -79,7 +82,7 @@ namespace gtsam {
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const typename KEY1::Value& p1, const typename KEY1::Value& p2,
Vector evaluateError(const T& p1, const T& p2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
T hx = p1.between(p2, H1, H2); // h(x)
@ -88,12 +91,12 @@ namespace gtsam {
}
/** return the measured */
inline const T measured() const {
const VALUE& measured() const {
return measured_;
}
/** number of variables attached to this factor */
inline std::size_t size() const {
std::size_t size() const {
return 2;
}
@ -114,16 +117,14 @@ namespace gtsam {
* This constraint requires the underlying type to a Lie type
*
*/
template<class KEY>
class BetweenConstraint : public BetweenFactor<KEY> {
template<class VALUE>
class BetweenConstraint : public BetweenFactor<VALUE> {
public:
typedef boost::shared_ptr<BetweenConstraint<KEY> > shared_ptr;
typedef boost::shared_ptr<BetweenConstraint<VALUE> > shared_ptr;
/** Syntactic sugar for constrained version */
BetweenConstraint(const typename KEY::Value& measured,
const KEY& key1, const KEY& key2, double mu = 1000.0)
: BetweenFactor<KEY>(key1, key2, measured,
noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {}
BetweenConstraint(const VALUE& measured, const Symbol& key1, const Symbol& key2, double mu = 1000.0) :
BetweenFactor<VALUE>(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {}
private:
@ -132,7 +133,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<KEY> >(*this));
boost::serialization::base_object<BetweenFactor<VALUE> >(*this));
}
}; // \class BetweenConstraint

30
gtsam/slam/BoundingConstraint.h
View File

@ -28,16 +28,16 @@ namespace gtsam {
* will need to have its value function implemented to return
* a scalar for comparison.
*/
template<class KEY>
struct BoundingConstraint1: public NonlinearFactor1<KEY> {
typedef typename KEY::Value X;
typedef NonlinearFactor1<KEY> Base;
typedef boost::shared_ptr<BoundingConstraint1<KEY> > shared_ptr;
template<class VALUE>
struct BoundingConstraint1: public NonlinearFactor1<VALUE> {
typedef VALUE X;
typedef NonlinearFactor1<VALUE> Base;
typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
BoundingConstraint1(const KEY& key, double threshold,
BoundingConstraint1(const Symbol& key, double threshold,
bool isGreaterThan, double mu = 1000.0) :
Base(noiseModel::Constrained::All(1, mu), key),
threshold_(threshold), isGreaterThan_(isGreaterThan) {
@ -59,7 +59,7 @@ struct BoundingConstraint1: public NonlinearFactor1<KEY> {
/** active when constraint *NOT* met */
bool active(const Values& c) const {
// note: still active at equality to avoid zigzagging
double x = value(c[this->key_]);
double x = value(c[this->key()]);
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
}
@ -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_;
}

38
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 CamK, class LmK>
template <class CAMERA, class LANDMARK>
class GeneralSFMFactor:
public NonlinearFactor2<CamK, LmK> {
public NonlinearFactor2<CAMERA, LANDMARK> {
protected:
Point2 z_; ///< the 2D measurement
Point2 measured_; ///< the 2D measurement
public:
typedef typename CamK::Value Cam; ///< typedef for camera type
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
typedef CAMERA Cam; ///< typedef for camera type
typedef GeneralSFMFactor<CAMERA, LANDMARK> This; ///< typedef for this object
typedef NonlinearFactor2<CAMERA, LANDMARK> 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<LmK, CamK> > shared_ptr;
typedef boost::shared_ptr<This> shared_ptr;
/**
* Constructor
@ -52,11 +52,12 @@ namespace gtsam {
* @param i is basically the frame number
* @param j is the index of the landmark
*/
GeneralSFMFactor(const Point2& z, const SharedNoiseModel& model, const CamK& i, const LmK& j) : Base(model, i, j), z_(z) {}
GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Symbol& landmarkKey) :
Base(model, cameraKey, landmarkKey), measured_(measured) {}
GeneralSFMFactor():z_(0.0,0.0) {} ///< default constructor
GeneralSFMFactor(const Point2 & p):z_(p) {} ///< constructor that takes a Point2
GeneralSFMFactor(double x, double y):z_(x,y) {} ///< constructor that takes doubles x,y to make a Point2
GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor
GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2
GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2
virtual ~GeneralSFMFactor() {} ///< destructor
@ -66,14 +67,15 @@ namespace gtsam {
*/
void print(const std::string& s = "SFMFactor") const {
Base::print(s);
z_.print(s + ".z");
measured_.print(s + ".z");
}
/**
* equals
*/
bool equals(const GeneralSFMFactor<CamK, LmK> &p, double tol = 1e-9) const {
return Base::equals(p, tol) && this->z_.equals(p.z_, tol) ;
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ;
}
/** h(x)-z */
@ -83,14 +85,14 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
Vector error = z_.localCoordinates(camera.project2(point,H1,H2));
Vector error = measured_.localCoordinates(camera.project2(point,H1,H2));
// gtsam::print(error, "error");
return error;
}
/** return the measured */
inline const Point2 measured() const {
return z_;
return measured_;
}
private:
@ -98,7 +100,7 @@ namespace gtsam {
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};

10
gtsam/slam/Makefile.am
View File

@ -54,6 +54,11 @@ check_PROGRAMS += tests/testGeneralSFMFactor tests/testGeneralSFMFactor_Cal3Bund
headers += StereoFactor.h
check_PROGRAMS += tests/testStereoFactor
## flag disabled to force this test to get updated properly
if ENABLE_SERIALIZATION
check_PROGRAMS += tests/testSerializationSLAM
endif
#----------------------------------------------------------------------------------------------------
# Create a libtool library that is not installed
# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am
@ -67,6 +72,11 @@ libslam_la_SOURCES = $(sources)
AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS)
# link to serialization library for test
if ENABLE_SERIALIZATION
tests_testSerializationSLAM_LDFLAGS = -lboost_serialization
endif
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------

16
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 KEY>
class PartialPriorFactor: public NonlinearFactor1<KEY> {
template<class VALUE>
class PartialPriorFactor: public NonlinearFactor1<VALUE> {
public:
typedef typename KEY::Value T;
typedef VALUE T;
protected:
typedef NonlinearFactor1<KEY> Base;
typedef PartialPriorFactor<KEY> This;
typedef NonlinearFactor1<VALUE> Base;
typedef PartialPriorFactor<VALUE> This;
Vector prior_; ///< measurement on logmap parameters, in compressed form
std::vector<size_t> mask_; ///< indices of values to constrain in compressed prior vector
@ -60,7 +60,7 @@ namespace gtsam {
* constructor with just minimum requirements for a factor - allows more
* computation in the constructor. This should only be used by subclasses
*/
PartialPriorFactor(const KEY& key, const SharedNoiseModel& model)
PartialPriorFactor(const Symbol& key, const SharedNoiseModel& model)
: Base(model, key) {}
public:
@ -71,14 +71,14 @@ namespace gtsam {
virtual ~PartialPriorFactor() {}
/** Single Element Constructor: acts on a single parameter specified by idx */
PartialPriorFactor(const KEY& key, size_t idx, double prior, const SharedNoiseModel& model) :
PartialPriorFactor(const Symbol& key, size_t idx, double prior, const SharedNoiseModel& model) :
Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) {
assert(model->dim() == 1);
this->fillH();
}
/** Indices Constructor: specify the mask with a set of indices */
PartialPriorFactor(const KEY& key, const std::vector<size_t>& mask, const Vector& prior,
PartialPriorFactor(const Symbol& key, const std::vector<size_t>& mask, const Vector& prior,
const SharedNoiseModel& model) :
Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) {
assert((size_t)prior_.size() == mask.size());

31
gtsam/slam/PriorFactor.h
View File

@ -21,32 +21,30 @@
namespace gtsam {
/**
* A class for a soft prior on any Lie type
* It takes two template parameters:
* Key (typically TypedSymbol) is used to look up T's in a Values
* Values where the T's are stored, typically Values<Key> or a TupleValues<...>
* The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
* a simple type like int will not work
* A class for a soft prior on any Value type
*/
template<class KEY>
class PriorFactor: public NonlinearFactor1<KEY> {
template<class VALUE>
class PriorFactor: public NonlinearFactor1<VALUE> {
public:
typedef typename KEY::Value T;
typedef VALUE T;
private:
typedef NonlinearFactor1<KEY> Base;
typedef NonlinearFactor1<VALUE> Base;
T prior_; /** The measurement */
VALUE prior_; /** The measurement */
/** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(T)
public:
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<PriorFactor> shared_ptr;
/// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<PriorFactor<VALUE> > shared_ptr;
/// Typedef to this class
typedef PriorFactor<VALUE> This;
/** default constructor - only use for serialization */
PriorFactor() {}
@ -54,8 +52,7 @@ namespace gtsam {
virtual ~PriorFactor() {}
/** Constructor */
PriorFactor(const KEY& key, const T& prior,
const SharedNoiseModel& model) :
PriorFactor(const Symbol& key, const VALUE& prior, const SharedNoiseModel& model) :
Base(model, key), prior_(prior) {
}
@ -63,14 +60,14 @@ namespace gtsam {
/** print */
virtual void print(const std::string& s) const {
std::cout << s << "PriorFactor(" << (std::string) this->key_ << ")\n";
std::cout << s << "PriorFactor(" << (std::string) this->key() << ")\n";
prior_.print(" prior");
this->noiseModel_->print(" noise model");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const PriorFactor<KEY> *e = dynamic_cast<const PriorFactor<KEY>*> (&expected);
const This* e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol);
}

44
gtsam/slam/ProjectionFactor.h
View File

@ -29,21 +29,24 @@ 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 LMK, class POSK>
class GenericProjectionFactor: public NonlinearFactor2<POSK, LMK> {
template<class POSE, class LANDMARK>
class GenericProjectionFactor: public NonlinearFactor2<POSE, LANDMARK> {
protected:
// Keep a copy of measurement and calibration for I/O
Point2 z_; ///< 2D measurement
Point2 measured_; ///< 2D measurement
boost::shared_ptr<Cal3_S2> K_; ///< shared pointer to calibration object
public:
/// shorthand for base class type
typedef NonlinearFactor2<POSK, LMK> Base;
typedef NonlinearFactor2<POSE, LANDMARK> Base;
/// shorthand for this class
typedef GenericProjectionFactor<POSE, LANDMARK> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GenericProjectionFactor<LMK, POSK> > shared_ptr;
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
GenericProjectionFactor() :
@ -52,15 +55,16 @@ namespace gtsam {
/**
* Constructor
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param z is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation
* @param j_pose is basically the frame number
* @param j_landmark is the index of the landmark
* @param K shared pointer to the constant calibration
*/
GenericProjectionFactor(const Point2& z, const SharedNoiseModel& model,
POSK j_pose, LMK j_landmark, const shared_ptrK& K) :
Base(model, j_pose, j_landmark), z_(z), K_(K) {
GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
const Symbol poseKey, const Symbol& pointKey, const shared_ptrK& K) :
Base(model, poseKey, pointKey), measured_(measured), K_(K) {
}
/**
@ -69,14 +73,13 @@ namespace gtsam {
*/
void print(const std::string& s = "ProjectionFactor") const {
Base::print(s);
z_.print(s + ".z");
measured_.print(s + ".z");
}
/// equals
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);
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol);
}
/// Evaluate error h(x)-z and optionally derivatives
@ -84,21 +87,20 @@ namespace gtsam {
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
try {
SimpleCamera camera(*K_, pose);
Point2 reprojectionError(camera.project(point, H1, H2) - z_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
return reprojectionError.vector();
}
catch( CheiralityException& e) {
} catch( CheiralityException& e) {
if (H1) *H1 = zeros(2,6);
if (H2) *H2 = zeros(2,3);
cout << e.what() << ": Landmark "<< this->key2_.index() <<
" moved behind camera " << this->key1_.index() << endl;
cout << e.what() << ": Landmark "<< this->key2().index() <<
" moved behind camera " << this->key1().index() << endl;
return ones(2) * 2.0 * K_->fx();
}
}
/** return the measurement */
inline const Point2 measured() const {
return z_;
const Point2& measured() const {
return measured_;
}
private:
@ -108,7 +110,7 @@ namespace gtsam {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
};

33
gtsam/slam/RangeFactor.h
View File

@ -25,17 +25,17 @@ namespace gtsam {
/**
* Binary factor for a range measurement
*/
template<class POSEKEY, class POINTKEY>
class RangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
template<class POSE, class POINT>
class RangeFactor: public NonlinearFactor2<POSE, POINT> {
private:
double z_; /** measurement */
double measured_; /** measurement */
typedef RangeFactor<POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
typedef RangeFactor<POSE, POINT> This;
typedef NonlinearFactor2<POSE, POINT> Base;
typedef typename POSEKEY::Value Pose;
typedef typename POINTKEY::Value Point;
typedef POSE Pose;
typedef POINT Point;
// Concept requirements for this factor
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(Pose, Point)
@ -44,34 +44,33 @@ namespace gtsam {
RangeFactor() {} /* Default constructor */
RangeFactor(const POSEKEY& i, const POINTKEY& j, double z,
RangeFactor(const Symbol& poseKey, const Symbol& pointKey, double measured,
const SharedNoiseModel& model) :
Base(model, i, j), z_(z) {
Base(model, poseKey, pointKey), measured_(measured) {
}
virtual ~RangeFactor() {}
/** h(x)-z */
Vector evaluateError(const typename POSEKEY::Value& pose, const typename POINTKEY::Value& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Vector evaluateError(const Pose& pose, const Point& point, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
double hx = pose.range(point, H1, H2);
return Vector_(1, hx - z_);
return Vector_(1, hx - measured_);
}
/** return the measured */
inline double measured() const {
return z_;
double measured() const {
return measured_;
}
/** equals specialized to this factor */
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;
return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->measured_) < tol;
}
/** print contents */
void print(const std::string& s="") const {
Base::print(s + std::string(" range: ") + boost::lexical_cast<std::string>(z_));
Base::print(s + std::string(" range: ") + boost::lexical_cast<std::string>(measured_));
}
private:
@ -82,7 +81,7 @@ namespace gtsam {
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(measured_);
}
}; // RangeFactor

53
gtsam/slam/StereoFactor.h
View File

@ -22,20 +22,20 @@
namespace gtsam {
template<class KEY1, class KEY2>
class GenericStereoFactor: public NonlinearFactor2<KEY1, KEY2> {
template<class POSE, class LANDMARK>
class GenericStereoFactor: public NonlinearFactor2<POSE, LANDMARK> {
private:
// Keep a copy of measurement and calibration for I/O
StereoPoint2 z_; ///< the measurement
StereoPoint2 measured_; ///< the measurement
boost::shared_ptr<Cal3_S2Stereo> K_; ///< shared pointer to calibration
public:
// shorthand for base class type
typedef NonlinearFactor2<KEY1, KEY2> Base; ///< typedef for base class
typedef NonlinearFactor2<POSE, LANDMARK> 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
typedef POSE CamPose; ///< typedef for Pose Lie Value type
/**
* Default constructor
@ -44,18 +44,17 @@ public:
/**
* Constructor
* @param z is the Stereo Point measurement (u_l, u_r, v). v will be identical for left & right for rectified stereo pair
* @param measured is the Stereo Point measurement (u_l, u_r, v). v will be identical for left & right for rectified stereo pair
* @param model is the noise model in on the measurement
* @param j_pose the pose index
* @param j_landmark the landmark index
* @param poseKey the pose variable key
* @param landmarkKey the landmark variable key
* @param K the constant calibration
*/
GenericStereoFactor(const StereoPoint2& z, const SharedNoiseModel& model, KEY1 j_pose,
KEY2 j_landmark, const shared_ptrKStereo& K) :
Base(model, j_pose, j_landmark), z_(z), K_(K) {
GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey, const shared_ptrKStereo& K) :
Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {
}
~GenericStereoFactor() {} ///< desctructor
~GenericStereoFactor() {} ///< destructor
/**
* print
@ -63,41 +62,27 @@ public:
*/
void print(const std::string& s) const {
Base::print(s);
z_.print(s + ".z");
measured_.print(s + ".z");
}
/**
* equals
*/
bool equals(const GenericStereoFactor& f, double tol) const {
const GenericStereoFactor* p = dynamic_cast<const GenericStereoFactor*> (&f);
if (p == NULL)
goto fail;
//if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail;
if (!z_.equals(p->z_, tol))
goto fail;
return true;
fail: print("actual");
p->print("expected");
return false;
virtual bool equals(const NonlinearFactor& f, double tol) const {
const GenericStereoFactor* p = dynamic_cast<const GenericStereoFactor*> (&f);
return p && Base::equals(f) && measured_.equals(p->measured_, tol);
}
/** h(x)-z */
Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
StereoCamera stereoCam(pose, K_);
return (stereoCam.project(point, H1, H2) - z_).vector();
}
/// get the measurement z
StereoPoint2 z() {
return z_;
return (stereoCam.project(point, H1, H2) - measured_).vector();
}
/** return the measured */
inline const StereoPoint2 measured() const {
return z_;
const StereoPoint2& measured() const {
return measured_;
}
private:
@ -105,7 +90,7 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
};

4
gtsam/slam/dataset.cpp
View File

@ -133,9 +133,9 @@ pair<sharedPose2Graph, Values::shared_ptr> load2D(const string& filename,
// Insert vertices if pure odometry file
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);
if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at<Pose2>(pose2SLAM::PoseKey(id1)) * l1Xl2);
pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model));
pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(pose2SLAM::PoseKey(id1), pose2SLAM::PoseKey(id2), l1Xl2, *model));
graph->push_back(factor);
}
is.ignore(LINESIZE, '\n');

28
gtsam/slam/planarSLAM.cpp
View File

@ -26,38 +26,34 @@ namespace planarSLAM {
Graph::Graph(const NonlinearFactorGraph& graph) :
NonlinearFactorGraph(graph) {}
void Graph::addPrior(const PoseKey& i, const Pose2& p,
const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
void Graph::addPrior(Index i, const Pose2& p, const SharedNoiseModel& model) {
sharedFactor factor(new Prior(PoseKey(i), p, model));
push_back(factor);
}
void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) {
sharedFactor factor(new Constraint(i, p));
void Graph::addPoseConstraint(Index i, const Pose2& p) {
sharedFactor factor(new Constraint(PoseKey(i), p));
push_back(factor);
}
void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(i, j, z, model));
void Graph::addOdometry(Index i, Index j, const Pose2& odometry, const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), odometry, model));
push_back(factor);
}
void Graph::addBearing(const PoseKey& i, const PointKey& j, const Rot2& z,
const SharedNoiseModel& model) {
sharedFactor factor(new Bearing(i, j, z, model));
void Graph::addBearing(Index i, Index j, const Rot2& z, const SharedNoiseModel& model) {
sharedFactor factor(new Bearing(PoseKey(i), PointKey(j), z, model));
push_back(factor);
}
void Graph::addRange(const PoseKey& i, const PointKey& j, double z,
const SharedNoiseModel& model) {
sharedFactor factor(new Range(i, j, z, model));
void Graph::addRange(Index i, Index j, double z, const SharedNoiseModel& model) {
sharedFactor factor(new Range(PoseKey(i), PointKey(j), z, model));
push_back(factor);
}
void Graph::addBearingRange(const PoseKey& i, const PointKey& j, const Rot2& z1,
void Graph::addBearingRange(Index i, Index j, const Rot2& z1,
double z2, const SharedNoiseModel& model) {
sharedFactor factor(new BearingRange(i, j, z1, z2, model));
sharedFactor factor(new BearingRange(PoseKey(i), PointKey(j), z1, z2, model));
push_back(factor);
}

108
gtsam/slam/planarSLAM.h
View File

@ -31,28 +31,29 @@
// Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM {
using namespace gtsam;
using namespace gtsam;
/// Typedef for a PoseKey with Pose2 data and 'x' symbol
typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Typedef for a PointKey with Point2 data and 'l' symbol
typedef TypedSymbol<Point2, 'l'> PointKey;
/**
* 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;
/// Convenience function for constructing a pose key
inline Symbol PointKey(Index j) { return Symbol('l', j); }
/*
* List of typedefs for factors
*/
/// A hard constraint for PoseKeys to enforce particular values
typedef NonlinearEquality<Pose2> Constraint;
/// A prior factor to bias the value of a PoseKey
typedef PriorFactor<Pose2> Prior;
/// A factor between two PoseKeys set with a Pose2
typedef BetweenFactor<Pose2> Odometry;
/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
typedef BearingFactor<Pose2, Point2> Bearing;
/// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
typedef RangeFactor<Pose2, Point2> Range;
/// A factor between a PoseKey and a PointKey to express difference in rotation and location
typedef BearingRangeFactor<Pose2, Point2> BearingRange;
/** Values class, using specific PoseKeys and PointKeys
* Mainly as a convenience for MATLAB wrapper, which does not allow for identically named methods
@ -68,60 +69,55 @@ namespace planarSLAM {
}
/// get a pose
Pose2 pose(int key) const { return (*this)[PoseKey(key)]; }
Pose2 pose(Index key) const { return at<Pose2>(PoseKey(key)); }
/// get a point
Point2 point(int key) const { return (*this)[PointKey(key)]; }
Point2 point(Index key) const { return at<Point2>(PointKey(key)); }
/// insert a pose
void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); }
void insertPose(Index key, const Pose2& pose) { insert(PoseKey(key), pose); }
/// insert a point
void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
void insertPoint(Index key, const Point2& point) { insert(PointKey(key), point); }
};
/// Creates a NonlinearFactorGraph with the Values type
struct Graph: public NonlinearFactorGraph {
/// Creates a NonlinearFactorGraph with the Values type
struct Graph: public NonlinearFactorGraph {
/// 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& graph);
/// Biases the value of PoseKey key with Pose2 p given a noise model
void addPrior(Index poseKey, 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(Index 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(Index poseKey1, Index poseKey2, 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(Index poseKey, Index 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(Index poseKey, Index 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(Index poseKey, Index 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> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA);
}
};
/// Optimizer
typedef NonlinearOptimizer<Graph> Optimizer;
/// Optimizer
typedef NonlinearOptimizer<Graph> Optimizer;
} // planarSLAM

12
gtsam/slam/pose2SLAM.cpp
View File

@ -35,20 +35,20 @@ namespace pose2SLAM {
}
/* ************************************************************************* */
void Graph::addPrior(const PoseKey& i, const Pose2& p,
void Graph::addPrior(Index i, const Pose2& p,
const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
sharedFactor factor(new Prior(PoseKey(i), p, model));
push_back(factor);
}
void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) {
sharedFactor factor(new HardConstraint(i, p));
void Graph::addPoseConstraint(Index i, const Pose2& p) {
sharedFactor factor(new HardConstraint(PoseKey(i), p));
push_back(factor);
}
void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
void Graph::addOdometry(Index i, Index j, const Pose2& z,
const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(i, j, z, model));
sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), z, model));
push_back(factor);
}

22
gtsam/slam/pose2SLAM.h
View File

@ -20,7 +20,7 @@
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -33,8 +33,8 @@ namespace pose2SLAM {
using namespace gtsam;
/// Keys with Pose2 and symbol 'x'
typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Values class, inherited from Values, using PoseKeys, mainly used as a convenience for MATLAB wrapper
struct Values: public gtsam::Values {
@ -50,10 +50,10 @@ namespace pose2SLAM {
// Convenience for MATLAB wrapper, which does not allow for identically named methods
/// get a pose
Pose2 pose(int key) const { return (*this)[PoseKey(key)]; }
Pose2 pose(Index key) const { return at<Pose2>(PoseKey(key)); }
/// insert a pose
void insertPose(int key, const Pose2& pose) { insert(PoseKey(key), pose); }
void insertPose(Index key, const Pose2& pose) { insert(PoseKey(key), pose); }
};
/**
@ -70,11 +70,11 @@ namespace pose2SLAM {
*/
/// A hard constraint to enforce a specific value for a pose
typedef NonlinearEquality<PoseKey> HardConstraint;
typedef NonlinearEquality<Pose2> HardConstraint;
/// A prior factor on a pose with Pose2 data type.
typedef PriorFactor<PoseKey> Prior;
typedef PriorFactor<Pose2> Prior;
/// A factor to add an odometry measurement between two poses.
typedef BetweenFactor<PoseKey> Odometry;
typedef BetweenFactor<Pose2> Odometry;
/// Graph
struct Graph: public NonlinearFactorGraph {
@ -86,13 +86,13 @@ namespace pose2SLAM {
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);
void addPrior(Index i, const Pose2& p, const SharedNoiseModel& model);
/// Creates a hard constraint for key i with the given Pose2 p.
void addPoseConstraint(const PoseKey& i, const Pose2& p);
void addPoseConstraint(Index i, const Pose2& p);
/// Creates a between factor between keys i and j with a noise model with Pose2 z in the graph
void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
void addOdometry(Index i, Index j, const Pose2& z,
const SharedNoiseModel& model);
/// Optimize

16
gtsam/slam/pose3SLAM.cpp
View File

@ -39,26 +39,24 @@ 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(Key(i), gTi);
x.insert(PoseKey(i), gTi);
}
return x;
}
/* ************************************************************************* */
void Graph::addPrior(const Key& i, const Pose3& p,
const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
void Graph::addPrior(Index i, const Pose3& p, const SharedNoiseModel& model) {
sharedFactor factor(new Prior(PoseKey(i), p, model));
push_back(factor);
}
void Graph::addConstraint(const Key& i, const Key& j, const Pose3& z,
const SharedNoiseModel& model) {
sharedFactor factor(new Constraint(i, j, z, model));
void Graph::addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model) {
sharedFactor factor(new Constraint(PoseKey(i), PoseKey(j), z, model));
push_back(factor);
}
void Graph::addHardConstraint(const Key& i, const Pose3& p) {
sharedFactor factor(new HardConstraint(i, p));
void Graph::addHardConstraint(Index i, const Pose3& p) {
sharedFactor factor(new HardConstraint(PoseKey(i), p));
push_back(factor);
}

20
gtsam/slam/pose3SLAM.h
View File

@ -20,7 +20,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
@ -30,8 +30,8 @@ namespace gtsam {
/// Use pose3SLAM namespace for specific SLAM instance
namespace pose3SLAM {
/// Creates a Key with data Pose3 and symbol 'x'
typedef TypedSymbol<Pose3, 'x'> Key;
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/**
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
@ -42,25 +42,23 @@ namespace gtsam {
Values circle(size_t n, double R);
/// A prior factor on Key with Pose3 data type.
typedef PriorFactor<Key> Prior;
typedef PriorFactor<Pose3> Prior;
/// A factor to put constraints between two factors.
typedef BetweenFactor<Key> Constraint;
typedef BetweenFactor<Pose3> Constraint;
/// A hard constraint would enforce that the given key would have the input value in the results.
typedef NonlinearEquality<Key> HardConstraint;
typedef NonlinearEquality<Pose3> HardConstraint;
/// Graph
struct Graph: public NonlinearFactorGraph {
/// Adds a factor between keys of the same type
void addPrior(const Key& i, const Pose3& p,
const SharedNoiseModel& model);
void addPrior(Index i, const Pose3& p, const SharedNoiseModel& model);
/// Creates a between factor between keys i and j with a noise model with Pos3 z in the graph
void addConstraint(const Key& i, const Key& j, const Pose3& z,
const SharedNoiseModel& model);
void addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model);
/// Creates a hard constraint for key i with the given Pose3 p.
void addHardConstraint(const Key& i, const Pose3& p);
void addHardConstraint(Index i, const Pose3& p);
};
/// Optimizer

99
gtsam/slam/simulated2D.h
View File

@ -29,8 +29,12 @@ namespace simulated2D {
using namespace gtsam;
// Simulated2D robots have no orientation, just a position
typedef TypedSymbol<Point2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Convenience function for constructing a landmark key
inline Symbol PointKey(Index j) { return Symbol('l', j); }
/**
* Custom Values class that holds poses and points, mainly used as a convenience for MATLAB wrapper
@ -53,14 +57,14 @@ namespace simulated2D {
}
/// Insert a pose
void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
insert(i, p);
void insertPose(Index j, const Point2& p) {
insert(PoseKey(j), p);
nrPoses_++;
}
/// Insert a point
void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
insert(j, p);
void insertPoint(Index j, const Point2& p) {
insert(PointKey(j), p);
nrPoints_++;
}
@ -75,13 +79,13 @@ namespace simulated2D {
}
/// Return pose i
Point2 pose(const simulated2D::PoseKey& i) const {
return (*this)[i];
Point2 pose(Index j) const {
return at<Point2>(PoseKey(j));
}
/// Return point j
Point2 point(const simulated2D::PointKey& j) const {
return (*this)[j];
Point2 point(Index j) const {
return at<Point2>(PointKey(j));
}
};
@ -115,24 +119,23 @@ namespace simulated2D {
/**
* Unary factor encoding a soft prior on a vector
*/
template<class KEY = PoseKey>
class GenericPrior: public NonlinearFactor1<KEY> {
template<class VALUE = Point2>
class GenericPrior: public NonlinearFactor1<VALUE> {
public:
typedef NonlinearFactor1<KEY> Base; ///< base class
typedef boost::shared_ptr<GenericPrior<KEY> > shared_ptr;
typedef typename KEY::Value Pose; ///< shortcut to Pose type
typedef NonlinearFactor1<VALUE> Base; ///< base class
typedef boost::shared_ptr<GenericPrior<VALUE> > shared_ptr;
typedef VALUE Pose; ///< shortcut to Pose type
Pose z_; ///< prior mean
Pose measured_; ///< prior mean
/// Create generic prior
GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) :
NonlinearFactor1<KEY>(model, key), z_(z) {
GenericPrior(const Pose& z, const SharedNoiseModel& model, const Symbol& key) :
Base(model, key), measured_(z) {
}
/// Return error and optional derivative
Vector evaluateError(const Pose& x, boost::optional<Matrix&> H =
boost::none) const {
return (prior(x, H) - z_).vector();
Vector evaluateError(const Pose& x, boost::optional<Matrix&> H = boost::none) const {
return (prior(x, H) - measured_).vector();
}
private:
@ -146,33 +149,32 @@ namespace simulated2D {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
/**
* Binary factor simulating "odometry" between two Vectors
*/
template<class KEY = PoseKey>
class GenericOdometry: public NonlinearFactor2<KEY, KEY> {
template<class VALUE = Point2>
class GenericOdometry: public NonlinearFactor2<VALUE, VALUE> {
public:
typedef NonlinearFactor2<KEY, KEY> Base; ///< base class
typedef boost::shared_ptr<GenericOdometry<KEY> > shared_ptr;
typedef typename KEY::Value Pose; ///< shortcut to Pose type
typedef NonlinearFactor2<VALUE, VALUE> Base; ///< base class
typedef boost::shared_ptr<GenericOdometry<VALUE> > shared_ptr;
typedef VALUE Pose; ///< shortcut to Pose type
Pose z_; ///< odometry measurement
Pose measured_; ///< odometry measurement
/// Create odometry
GenericOdometry(const Pose& z, const SharedNoiseModel& model,
const KEY& i1, const KEY& i2) :
NonlinearFactor2<KEY, KEY>(model, i1, i2), z_(z) {
GenericOdometry(const Pose& measured, const SharedNoiseModel& model, const Symbol& key1, const Symbol& key2) :
Base(model, key1, key2), measured_(measured) {
}
/// Evaluate error and optionally return derivatives
Vector evaluateError(const Pose& x1, const Pose& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
return (odo(x1, x2, H1, H2) - z_).vector();
return (odo(x1, x2, H1, H2) - measured_).vector();
}
private:
@ -186,34 +188,33 @@ namespace simulated2D {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
/**
* Binary factor simulating "measurement" between two Vectors
*/
template<class XKEY = PoseKey, class LKEY = PointKey>
class GenericMeasurement: public NonlinearFactor2<XKEY, LKEY> {
template<class POSE, class LANDMARK>
class GenericMeasurement: public NonlinearFactor2<POSE, LANDMARK> {
public:
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
typedef NonlinearFactor2<POSE, LANDMARK> Base; ///< base class
typedef boost::shared_ptr<GenericMeasurement<POSE, LANDMARK> > shared_ptr;
typedef POSE Pose; ///< shortcut to Pose type
typedef LANDMARK Landmark; ///< shortcut to Landmark type
Point z_; ///< Measurement
Landmark measured_; ///< Measurement
/// Create measurement factor
GenericMeasurement(const Point& z, const SharedNoiseModel& model,
const XKEY& i, const LKEY& j) :
NonlinearFactor2<XKEY, LKEY>(model, i, j), z_(z) {
GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey) :
Base(model, poseKey, landmarkKey), measured_(measured) {
}
/// Evaluate error and optionally return derivatives
Vector evaluateError(const Pose& x1, const Point& x2,
Vector evaluateError(const Pose& x1, const Landmark& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
return (mea(x1, x2, H1, H2) - z_).vector();
return (mea(x1, x2, H1, H2) - measured_).vector();
}
private:
@ -227,14 +228,14 @@ namespace simulated2D {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
/** Typedefs for regular use */
typedef GenericPrior<PoseKey> Prior;
typedef GenericOdometry<PoseKey> Odometry;
typedef GenericMeasurement<PoseKey, PointKey> Measurement;
typedef GenericPrior<Point2> Prior;
typedef GenericOdometry<Point2> Odometry;
typedef GenericMeasurement<Point2, Point2> Measurement;
// Specialization of a graph for this example domain
// TODO: add functions to add factor types

62
gtsam/slam/simulated2DConstraints.h
View File

@ -33,13 +33,13 @@ namespace simulated2D {
namespace equality_constraints {
/** Typedefs for regular use */
typedef NonlinearEquality1<PoseKey> UnaryEqualityConstraint;
typedef NonlinearEquality1<PointKey> UnaryEqualityPointConstraint;
typedef BetweenConstraint<PoseKey> OdoEqualityConstraint;
typedef NonlinearEquality1<Point2> UnaryEqualityConstraint;
typedef NonlinearEquality1<Point2> UnaryEqualityPointConstraint;
typedef BetweenConstraint<Point2> OdoEqualityConstraint;
/** Equality between variables */
typedef NonlinearEquality2<PoseKey> PoseEqualityConstraint;
typedef NonlinearEquality2<PointKey> PointEqualityConstraint;
typedef NonlinearEquality2<Point2> PoseEqualityConstraint;
typedef NonlinearEquality2<Point2> PointEqualityConstraint;
} // \namespace equality_constraints
@ -47,15 +47,14 @@ namespace simulated2D {
/**
* Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c)
* @tparam VALUES is the values structure for the graph
* @tparam KEY is the key type for the variable constrained
* @tparam VALUE is the value type for the variable constrained, e.g. Pose2, Point3, etc.
* @tparam IDX is an index in tangent space to constrain, must be less than KEY::VALUE::Dim()
*/
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
template<class VALUE, unsigned int IDX>
struct ScalarCoordConstraint1: public BoundingConstraint1<VALUE> {
typedef BoundingConstraint1<VALUE> Base; ///< Base class convenience typedef
typedef boost::shared_ptr<ScalarCoordConstraint1<VALUE, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
typedef VALUE Point; ///< Constrained variable type
virtual ~ScalarCoordConstraint1() {}
@ -66,7 +65,7 @@ namespace simulated2D {
* @param isGreaterThan is a flag to set inequality as greater than or less than
* @param mu is the penalty function gain
*/
ScalarCoordConstraint1(const KEY& key, double c,
ScalarCoordConstraint1(const Symbol& key, double c,
bool isGreaterThan, double mu = 1000.0) :
Base(key, c, isGreaterThan, mu) {
}
@ -94,8 +93,8 @@ namespace simulated2D {
};
/** typedefs for use with simulated2D systems */
typedef ScalarCoordConstraint1<PoseKey, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
typedef ScalarCoordConstraint1<PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
typedef ScalarCoordConstraint1<Point2, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
typedef ScalarCoordConstraint1<Point2, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
/**
* Trait for distance constraints to provide distance
@ -114,10 +113,10 @@ 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 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
template<class VALUE>
struct MaxDistanceConstraint : public BoundingConstraint2<VALUE, VALUE> {
typedef BoundingConstraint2<VALUE, VALUE> Base; ///< Base class for factor
typedef VALUE Point; ///< Type of variable constrained
virtual ~MaxDistanceConstraint() {}
@ -128,8 +127,8 @@ namespace simulated2D {
* @param range_bound is the maximum range allowed between the variables
* @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) {}
MaxDistanceConstraint(const Symbol& key1, const Symbol& key2, double range_bound, double mu = 1000.0) :
Base(key1, key2, range_bound, false, mu) {}
/**
* computes the range with derivatives
@ -148,20 +147,19 @@ 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
* NOTE: this is not a convex function! Be careful with initialization.
* @tparam VALUES is the variable set for the graph
* @tparam XKEY is the type of the pose key constrained
* @tparam PKEY is the type of the point key constrained
* @tparam POSE is the type of the pose value constrained
* @tparam POINT is the type of the point value constrained
*/
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
template<class POSE, class POINT>
struct MinDistanceConstraint : public BoundingConstraint2<POSE, POINT> {
typedef BoundingConstraint2<POSE, POINT> Base; ///< Base class for factor
typedef POSE Pose; ///< Type of pose variable constrained
typedef POINT Point; ///< Type of point variable constrained
virtual ~MinDistanceConstraint() {}
@ -172,7 +170,7 @@ namespace simulated2D {
* @param range_bound is the minimum range allowed between the variables
* @param mu is the gain for the penalty function
*/
MinDistanceConstraint(const XKEY& key1, const PKEY& key2,
MinDistanceConstraint(const Symbol& key1, const Symbol& key2,
double range_bound, double mu = 1000.0)
: Base(key1, key2, range_bound, true, mu) {}
@ -193,7 +191,7 @@ namespace simulated2D {
}
};
typedef MinDistanceConstraint<PoseKey, PointKey> LandmarkAvoid; ///< Simulated2D domain example factor
typedef MinDistanceConstraint<Point2, Point2> LandmarkAvoid; ///< Simulated2D domain example factor
} // \namespace inequality_constraints

53
gtsam/slam/simulated2DOriented.h
View File

@ -27,9 +27,11 @@ namespace simulated2DOriented {
using namespace gtsam;
// The types that take an oriented pose2 rather than point2
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Convenience function for constructing a landmark key
inline Symbol PointKey(Index j) { return Symbol('l', j); }
/// Specialized Values structure with syntactic sugar for
/// compatibility with matlab
@ -38,21 +40,21 @@ namespace simulated2DOriented {
public:
Values() : nrPoses_(0), nrPoints_(0) {}
void insertPose(const PoseKey& i, const Pose2& p) {
insert(i, p);
void insertPose(Index j, const Pose2& p) {
insert(PoseKey(j), p);
nrPoses_++;
}
void insertPoint(const PointKey& j, const Point2& p) {
insert(j, p);
void insertPoint(Index j, const Point2& p) {
insert(PointKey(j), p);
nrPoints_++;
}
int nrPoses() const { return nrPoses_; }
int nrPoints() const { return nrPoints_; }
Pose2 pose(const PoseKey& i) const { return (*this)[i]; }
Point2 point(const PointKey& j) const { return (*this)[j]; }
Pose2 pose(Index j) const { return at<Pose2>(PoseKey(j)); }
Point2 point(Index j) const { return at<Point2>(PointKey(j)); }
};
//TODO:: point prior is not implemented right now
@ -75,21 +77,20 @@ namespace simulated2DOriented {
boost::none, boost::optional<Matrix&> H2 = boost::none);
/// Unary factor encoding a soft prior on a vector
template<class Key = PoseKey>
struct GenericPosePrior: public NonlinearFactor1<Key> {
template<class VALUE = Pose2>
struct GenericPosePrior: public NonlinearFactor1<VALUE> {
Pose2 z_; ///< measurement
Pose2 measured_; ///< measurement
/// Create generic pose prior
GenericPosePrior(const Pose2& z, const SharedNoiseModel& model,
const Key& key) :
NonlinearFactor1<Key>(model, key), z_(z) {
GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, const Symbol& key) :
NonlinearFactor1<VALUE>(model, key), measured_(measured) {
}
/// Evaluate error and optionally derivative
Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
boost::none) const {
return z_.localCoordinates(prior(x, H));
return measured_.localCoordinates(prior(x, H));
}
};
@ -97,31 +98,31 @@ namespace simulated2DOriented {
/**
* Binary factor simulating "odometry" between two Vectors
*/
template<class KEY = PoseKey>
struct GenericOdometry: public NonlinearFactor2<KEY, KEY> {
Pose2 z_; ///< Between measurement for odometry factor
template<class VALUE = Pose2>
struct GenericOdometry: public NonlinearFactor2<VALUE, VALUE> {
Pose2 measured_; ///< Between measurement for odometry factor
/**
* Creates an odometry factor between two poses
*/
GenericOdometry(const Pose2& z, const SharedNoiseModel& model,
const KEY& i1, const KEY& i2) :
NonlinearFactor2<KEY, KEY>(model, i1, i2), z_(z) {
GenericOdometry(const Pose2& measured, const SharedNoiseModel& model,
const Symbol& i1, const Symbol& i2) :
NonlinearFactor2<VALUE, VALUE>(model, i1, i2), measured_(measured) {
}
/// Evaluate error and optionally derivative
Vector evaluateError(const Pose2& x1, const Pose2& x2,
Vector evaluateError(const VALUE& x1, const VALUE& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
return z_.localCoordinates(odo(x1, x2, H1, H2));
return measured_.localCoordinates(odo(x1, x2, H1, H2));
}
};
typedef GenericOdometry<PoseKey> Odometry;
typedef GenericOdometry<Pose2> Odometry;
/// Graph specialization for syntactic sugar use with matlab
class Graph : public NonlinearFactorGraph {
class Graph : public NonlinearFactorGraph {
public:
Graph() {}
// TODO: add functions to add factors

48
gtsam/slam/simulated3D.h
View File

@ -23,7 +23,7 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
// \namespace
@ -36,8 +36,11 @@ namespace simulated3D {
* the simulated2D domain.
*/
typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Convenience function for constructing a pose key
inline Symbol PointKey(Index j) { return Symbol('l', j); }
/**
* Prior on a single pose
@ -61,19 +64,18 @@ Point3 mea(const Point3& x, const Point3& l,
/**
* A prior factor on a single linear robot pose
*/
struct PointPrior3D: public NonlinearFactor1<PoseKey> {
struct PointPrior3D: public NonlinearFactor1<Point3> {
Point3 z_; ///< The prior pose value for the variable attached to this factor
Point3 measured_; ///< The prior pose value for the variable attached to this factor
/**
* Constructor for a prior factor
* @param z is the measured/prior position for the pose
* @param measured is the measured/prior position for the pose
* @param model is the measurement model for the factor (Dimension: 3)
* @param j is the key for the pose
* @param key is the key for the pose
*/
PointPrior3D(const Point3& z,
const SharedNoiseModel& model, const PoseKey& j) :
NonlinearFactor1<PoseKey> (model, j), z_(z) {
PointPrior3D(const Point3& measured, const SharedNoiseModel& model, const Symbol& key) :
NonlinearFactor1<Point3> (model, key), measured_(measured) {
}
/**
@ -85,29 +87,27 @@ struct PointPrior3D: public NonlinearFactor1<PoseKey> {
*/
Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
boost::none) {
return (prior(x, H) - z_).vector();
return (prior(x, H) - measured_).vector();
}
};
/**
* Models a linear 3D measurement between 3D points
*/
struct Simulated3DMeasurement: public NonlinearFactor2<PoseKey, PointKey> {
struct Simulated3DMeasurement: public NonlinearFactor2<Point3, Point3> {
Point3 z_; ///< Linear displacement between a pose and landmark
Point3 measured_; ///< Linear displacement between a pose and landmark
/**
* Creates a measurement factor with a given measurement
* @param z is the measurement, a linear displacement between poses and landmarks
* @param measured is the measurement, a linear displacement between poses and landmarks
* @param model is a measurement model for the factor (Dimension: 3)
* @param j1 is the pose key of the robot
* @param j2 is the point key for the landmark
* @param poseKey is the pose key of the robot
* @param pointKey is the point key for the landmark
*/
Simulated3DMeasurement(const Point3& z,
const SharedNoiseModel& model, PoseKey& j1, PointKey j2) :
NonlinearFactor2<PoseKey, PointKey> (
model, j1, j2), z_(z) {
}
Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model,
const Symbol& poseKey, const Symbol& pointKey) :
NonlinearFactor2<Point3, Point3>(model, poseKey, pointKey), measured_(measured) {}
/**
* Error function with optional derivatives
@ -117,9 +117,9 @@ struct Simulated3DMeasurement: public NonlinearFactor2<PoseKey, PointKey> {
* @param H2 is an optional Jacobian matrix in terms of x2 (Dimension: 3x3)
* @return vector error between measurement and prediction (Dimension: 3)
*/
Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
return (mea(x1, x2, H1, H2) - z_).vector();
Vector evaluateError(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
return (mea(x1, x2, H1, H2) - measured_).vector();
}
};

51
gtsam/slam/smallExample.cpp
View File

@ -24,7 +24,7 @@
using namespace std;
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/base/Matrix.h>
@ -39,7 +39,10 @@ using namespace std;
namespace gtsam {
namespace example {
typedef boost::shared_ptr<NonlinearFactor > shared;
using simulated2D::PoseKey;
using simulated2D::PointKey;
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);
@ -57,22 +60,22 @@ namespace example {
// prior on x1
Point2 mu;
shared f1(new simulated2D::Prior(mu, sigma0_1, 1));
shared f1(new simulated2D::Prior(mu, sigma0_1, PoseKey(1)));
nlfg->push_back(f1);
// odometry between x1 and x2
Point2 z2(1.5, 0);
shared f2(new simulated2D::Odometry(z2, sigma0_1, 1, 2));
shared f2(new simulated2D::Odometry(z2, sigma0_1, PoseKey(1), PoseKey(2)));
nlfg->push_back(f2);
// measurement between x1 and l1
Point2 z3(0, -1);
shared f3(new simulated2D::Measurement(z3, sigma0_2, 1, 1));
shared f3(new simulated2D::Measurement(z3, sigma0_2, PoseKey(1), PointKey(1)));
nlfg->push_back(f3);
// measurement between x2 and l1
Point2 z4(-1.5, -1.);
shared f4(new simulated2D::Measurement(z4, sigma0_2, 2, 1));
shared f4(new simulated2D::Measurement(z4, sigma0_2, PoseKey(2), PointKey(1)));
nlfg->push_back(f4);
return nlfg;
@ -86,9 +89,9 @@ namespace example {
/* ************************************************************************* */
Values createValues() {
Values c;
c.insert(simulated2D::PoseKey(1), Point2(0.0, 0.0));
c.insert(simulated2D::PoseKey(2), Point2(1.5, 0.0));
c.insert(simulated2D::PointKey(1), Point2(0.0, -1.0));
c.insert(PoseKey(1), Point2(0.0, 0.0));
c.insert(PoseKey(2), Point2(1.5, 0.0));
c.insert(PointKey(1), Point2(0.0, -1.0));
return c;
}
@ -104,9 +107,9 @@ namespace example {
/* ************************************************************************* */
boost::shared_ptr<const Values> sharedNoisyValues() {
boost::shared_ptr<Values> c(new Values);
c->insert(simulated2D::PoseKey(1), Point2(0.1, 0.1));
c->insert(simulated2D::PoseKey(2), Point2(1.4, 0.2));
c->insert(simulated2D::PointKey(1), Point2(0.1, -1.1));
c->insert(PoseKey(1), Point2(0.1, 0.1));
c->insert(PoseKey(2), Point2(1.4, 0.2));
c->insert(PointKey(1), Point2(0.1, -1.1));
return c;
}
@ -195,17 +198,15 @@ namespace example {
0.0, cos(v.y()));
}
struct UnaryFactor: public gtsam::NonlinearFactor1<simulated2D::PoseKey> {
struct UnaryFactor: public gtsam::NonlinearFactor1<Point2> {
Point2 z_;
UnaryFactor(const Point2& z, const SharedNoiseModel& model,
const simulated2D::PoseKey& key) :
gtsam::NonlinearFactor1<simulated2D::PoseKey>(model, key), z_(z) {
UnaryFactor(const Point2& z, const SharedNoiseModel& model, const Symbol& key) :
gtsam::NonlinearFactor1<Point2>(model, key), z_(z) {
}
Vector evaluateError(const Point2& x, boost::optional<Matrix&> A =
boost::none) const {
Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const {
if (A) *A = H(x);
return (h(x) - z_).vector();
}
@ -220,7 +221,7 @@ namespace example {
Vector z = Vector_(2, 1.0, 0.0);
double sigma = 0.1;
boost::shared_ptr<smallOptimize::UnaryFactor> factor(
new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), 1));
new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), PoseKey(1)));
fg->push_back(factor);
return fg;
}
@ -238,23 +239,23 @@ namespace example {
// prior on x1
Point2 x1(1.0, 0.0);
shared prior(new simulated2D::Prior(x1, sigma1_0, 1));
shared prior(new simulated2D::Prior(x1, sigma1_0, PoseKey(1)));
nlfg.push_back(prior);
poses.insert(simulated2D::PoseKey(1), x1);
for (int t = 2; t <= T; t++) {
// odometry between x_t and x_{t-1}
Point2 odo(1.0, 0.0);
shared odometry(new simulated2D::Odometry(odo, sigma1_0, t - 1, t));
shared odometry(new simulated2D::Odometry(odo, sigma1_0, PoseKey(t - 1), PoseKey(t)));
nlfg.push_back(odometry);
// measurement on x_t is like perfect GPS
Point2 xt(t, 0);
shared measurement(new simulated2D::Prior(xt, sigma1_0, t));
shared measurement(new simulated2D::Prior(xt, sigma1_0, PoseKey(t)));
nlfg.push_back(measurement);
// initial estimate
poses.insert(simulated2D::PoseKey(t), xt);
poses.insert(PoseKey(t), xt);
}
return make_pair(nlfg, poses);
@ -414,8 +415,8 @@ namespace example {
/* ************************************************************************* */
// Create key for simulated planar graph
simulated2D::PoseKey key(int x, int y) {
return simulated2D::PoseKey(1000*x+y);
Symbol key(int x, int y) {
return PoseKey(1000*x+y);
}
/* ************************************************************************* */

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

@ -27,6 +27,8 @@
using namespace std;
using namespace gtsam;
using pose3SLAM::PoseKey;
/* ************************************************************************* */
TEST( AntiFactor, NegativeHessian)
{
@ -40,17 +42,17 @@ TEST( AntiFactor, NegativeHessian)
// Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values());
values->insert(pose3SLAM::Key(1), pose1);
values->insert(pose3SLAM::Key(2), pose2);
values->insert(PoseKey(1), pose1);
values->insert(PoseKey(2), pose2);
// Define an elimination ordering
Ordering::shared_ptr ordering(new Ordering());
ordering->insert(pose3SLAM::Key(1), 0);
ordering->insert(pose3SLAM::Key(2), 1);
ordering->insert(PoseKey(1), 0);
ordering->insert(PoseKey(2), 1);
// Create a "standard" factor
BetweenFactor<pose3SLAM::Key>::shared_ptr originalFactor(new BetweenFactor<pose3SLAM::Key>(1, 2, z, sigma));
BetweenFactor<Pose3>::shared_ptr originalFactor(new BetweenFactor<Pose3>(PoseKey(1), PoseKey(2), z, sigma));
// Linearize it into a Jacobian Factor
GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering);
@ -101,8 +103,8 @@ TEST( AntiFactor, EquivalentBayesNet)
// Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values());
values->insert(pose3SLAM::Key(1), pose1);
values->insert(pose3SLAM::Key(2), pose2);
values->insert(PoseKey(1), pose1);
values->insert(PoseKey(2), pose2);
// Define an elimination ordering
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
@ -115,7 +117,7 @@ TEST( AntiFactor, EquivalentBayesNet)
VectorValues expectedDeltas = optimize(*expectedBayesNet);
// Add an additional factor between Pose1 and Pose2
pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(1, 2, z, sigma));
pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(PoseKey(1), PoseKey(2), z, sigma));
graph->push_back(f1);
// Add the corresponding AntiFactor between Pose1 and Pose2

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

@ -12,7 +12,7 @@
#include <CppUnitLite/TestHarness.h>
using namespace boost;
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/base/Testable.h>
@ -28,25 +28,23 @@ using namespace std;
using namespace gtsam;
typedef PinholeCamera<Cal3_S2> GeneralCamera;
typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
typedef TypedSymbol<Point3, 'l'> PointKey;
typedef GeneralSFMFactor<CameraKey, PointKey> Projection;
typedef NonlinearEquality<CameraKey> CameraConstraint;
typedef NonlinearEquality<PointKey> Point3Constraint;
typedef GeneralSFMFactor<GeneralCamera, Point3> Projection;
typedef NonlinearEquality<GeneralCamera> CameraConstraint;
typedef NonlinearEquality<Point3> Point3Constraint;
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));
void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, Symbol('x',i), Symbol('l',j)));
}
void addCameraConstraint(int j, const GeneralCamera& p) {
boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(j, p));
boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(Symbol('x',j), p));
push_back(factor);
}
void addPoint3Constraint(int j, const Point3& p) {
boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(j, p));
boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(Symbol('l',j), p));
push_back(factor);
}
@ -76,7 +74,7 @@ TEST( GeneralSFMFactor, equals )
{
// Create two identical factors and make sure they're equal
Vector z = Vector_(2,323.,240.);
const int cameraFrameNumber=1, landmarkNumber=1;
const Symbol cameraFrameNumber="x1", landmarkNumber="l1";
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
@ -90,17 +88,15 @@ TEST( GeneralSFMFactor, equals )
/* ************************************************************************* */
TEST( GeneralSFMFactor, error ) {
Point2 z(3.,0.);
const int cameraFrameNumber=1, landmarkNumber=1;
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
boost::shared_ptr<Projection> factor(new Projection(z, sigma, "x1", "l1"));
// For the following configuration, the factor predicts 320,240
Values values;
Rot3 R;
Point3 t1(0,0,-6);
Pose3 x1(R,t1);
values.insert(CameraKey(1), GeneralCamera(x1));
Point3 l1; values.insert(PointKey(1), l1);
values.insert("x1", GeneralCamera(x1));
Point3 l1; values.insert("l1", l1);
EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
}
@ -170,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]);
@ -209,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 ) {
@ -217,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]) ;
}
}
@ -255,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 )
@ -304,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 {
@ -315,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
@ -360,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]);

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

@ -12,7 +12,7 @@
#include <CppUnitLite/TestHarness.h>
using namespace boost;
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/base/Testable.h>
@ -28,26 +28,24 @@ using namespace std;
using namespace gtsam;
typedef PinholeCamera<Cal3Bundler> GeneralCamera;
typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
typedef TypedSymbol<Point3, 'l'> PointKey;
typedef GeneralSFMFactor<CameraKey, PointKey> Projection;
typedef NonlinearEquality<CameraKey> CameraConstraint;
typedef NonlinearEquality<PointKey> Point3Constraint;
typedef GeneralSFMFactor<GeneralCamera, Point3> Projection;
typedef NonlinearEquality<GeneralCamera> CameraConstraint;
typedef NonlinearEquality<Point3> Point3Constraint;
/* ************************************************************************* */
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));
void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, Symbol('x',i), Symbol('l',j)));
}
void addCameraConstraint(int j, const GeneralCamera& p) {
boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(j, p));
boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(Symbol('x', j), p));
push_back(factor);
}
void addPoint3Constraint(int j, const Point3& p) {
boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(j, p));
boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(Symbol('l', j), p));
push_back(factor);
}
@ -77,7 +75,7 @@ TEST( GeneralSFMFactor, equals )
{
// Create two identical factors and make sure they're equal
Vector z = Vector_(2,323.,240.);
const int cameraFrameNumber=1, landmarkNumber=1;
const Symbol cameraFrameNumber="x1", landmarkNumber="l1";
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
@ -91,17 +89,16 @@ TEST( GeneralSFMFactor, equals )
/* ************************************************************************* */
TEST( GeneralSFMFactor, error ) {
Point2 z(3.,0.);
const int cameraFrameNumber=1, landmarkNumber=1;
const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection>
factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
factor(new Projection(z, sigma, "x1", "l1"));
// For the following configuration, the factor predicts 320,240
Values values;
Rot3 R;
Point3 t1(0,0,-6);
Pose3 x1(R,t1);
values.insert(CameraKey(1), GeneralCamera(x1));
Point3 l1; values.insert(PointKey(1), l1);
values.insert("x1", GeneralCamera(x1));
Point3 l1; values.insert("l1", l1);
EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
}
@ -171,13 +168,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]);
@ -210,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 ) {
@ -218,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]) ;
}
}
@ -256,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 )
@ -303,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 {
@ -312,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
@ -356,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]);

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

@ -37,7 +37,7 @@ SharedNoiseModel
/* ************************************************************************* */
TEST( planarSLAM, PriorFactor_equals )
{
planarSLAM::Prior factor1(2, x1, I3), factor2(2, x2, I3);
planarSLAM::Prior factor1(PoseKey(2), x1, I3), factor2(PoseKey(2), x2, I3);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -48,7 +48,7 @@ TEST( planarSLAM, BearingFactor )
{
// Create factor
Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
planarSLAM::Bearing factor(2, 3, z, sigma);
planarSLAM::Bearing factor(PoseKey(2), PointKey(3), z, sigma);
// create config
planarSLAM::Values c;
@ -64,8 +64,8 @@ TEST( planarSLAM, BearingFactor )
TEST( planarSLAM, BearingFactor_equals )
{
planarSLAM::Bearing
factor1(2, 3, Rot2::fromAngle(0.1), sigma),
factor2(2, 3, Rot2::fromAngle(2.3), sigma);
factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), sigma),
factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(2.3), sigma);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -76,7 +76,7 @@ TEST( planarSLAM, RangeFactor )
{
// Create factor
double z(sqrt(2) - 0.22); // h(x) - z = 0.22
planarSLAM::Range factor(2, 3, z, sigma);
planarSLAM::Range factor(PoseKey(2), PointKey(3), z, sigma);
// create config
planarSLAM::Values c;
@ -91,7 +91,7 @@ TEST( planarSLAM, RangeFactor )
/* ************************************************************************* */
TEST( planarSLAM, RangeFactor_equals )
{
planarSLAM::Range factor1(2, 3, 1.2, sigma), factor2(2, 3, 7.2, sigma);
planarSLAM::Range factor1(PoseKey(2), PointKey(3), 1.2, sigma), factor2(PoseKey(2), PointKey(3), 7.2, sigma);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -103,7 +103,7 @@ TEST( planarSLAM, BearingRangeFactor )
// Create factor
Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
double b(sqrt(2) - 0.22); // h(x) - z = 0.22
planarSLAM::BearingRange factor(2, 3, r, b, sigma2);
planarSLAM::BearingRange factor(PoseKey(2), PointKey(3), r, b, sigma2);
// create config
planarSLAM::Values c;
@ -119,8 +119,8 @@ TEST( planarSLAM, BearingRangeFactor )
TEST( planarSLAM, BearingRangeFactor_equals )
{
planarSLAM::BearingRange
factor1(2, 3, Rot2::fromAngle(0.1), 7.3, sigma2),
factor2(2, 3, Rot2::fromAngle(3), 2.0, sigma2);
factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), 7.3, sigma2),
factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(3), 2.0, sigma2);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -129,7 +129,7 @@ TEST( planarSLAM, BearingRangeFactor_equals )
/* ************************************************************************* */
TEST( planarSLAM, PoseConstraint_equals )
{
planarSLAM::Constraint factor1(2, x2), factor2(2, x3);
planarSLAM::Constraint factor1(PoseKey(2), x2), factor2(PoseKey(2), x3);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));

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

@ -14,7 +14,7 @@
* @author Frank Dellaert, Viorela Ila
**/
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/slam/pose2SLAM.h>
@ -34,6 +34,7 @@ using namespace boost::assign;
using namespace std;
typedef pose2SLAM::Odometry Pose2Factor;
using pose2SLAM::PoseKey;
// common measurement covariance
static double sx=0.5, sy=0.5,st=0.1;
@ -49,7 +50,7 @@ static noiseModel::Gaussian::shared_ptr covariance(
// Test constraint, small displacement
Vector f1(const Pose2& pose1, const Pose2& pose2) {
Pose2 z(2.1130913087, 0.468461064817, 0.436332312999);
Pose2Factor constraint(1, 2, z, covariance);
Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance);
return constraint.evaluateError(pose1, pose2);
}
@ -57,7 +58,7 @@ TEST( Pose2SLAM, constraint1 )
{
// create a factor between unknown poses p1 and p2
Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999);
Pose2Factor constraint(1, 2, pose2, covariance);
Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance);
Matrix H1, H2;
Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
@ -72,7 +73,7 @@ TEST( Pose2SLAM, constraint1 )
// Test constraint, large displacement
Vector f2(const Pose2& pose1, const Pose2& pose2) {
Pose2 z(2,2,M_PI_2);
Pose2Factor constraint(1, 2, z, covariance);
Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance);
return constraint.evaluateError(pose1, pose2);
}
@ -80,7 +81,7 @@ TEST( Pose2SLAM, constraint2 )
{
// create a factor between unknown poses p1 and p2
Pose2 pose1, pose2(2,2,M_PI_2);
Pose2Factor constraint(1, 2, pose2, covariance);
Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance);
Matrix H1, H2;
Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
@ -110,7 +111,7 @@ TEST( Pose2SLAM, linearization )
{
// create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2);
Pose2Factor constraint(1,2,measured, covariance);
Pose2Factor constraint(PoseKey(1), PoseKey(2), measured, covariance);
pose2SLAM::Graph graph;
graph.addOdometry(1,2,measured, covariance);
@ -178,8 +179,8 @@ TEST(Pose2Graph, optimize) {
TEST(Pose2Graph, optimizeThreePoses) {
// Create a hexagon of poses
Values hexagon = pose2SLAM::circle(3,1.0);
Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)];
pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0);
Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1);
// create a Pose graph with one equality constraint and one measurement
shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
@ -190,10 +191,10 @@ TEST(Pose2Graph, optimizeThreePoses) {
fg->addOdometry(2, 0, delta, covariance);
// Create initial config
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)));
boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
initial->insertPose(0, p0);
initial->insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1)));
// Choose an ordering
shared_ptr<Ordering> ordering(new Ordering);
@ -207,7 +208,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
Values actual = *optimizer.values();
// Check with ground truth
CHECK(assert_equal(hexagon, actual));
CHECK(assert_equal((const Values&)hexagon, actual));
}
/* ************************************************************************* */
@ -215,8 +216,8 @@ TEST(Pose2Graph, optimizeThreePoses) {
TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
// Create a hexagon of poses
Values hexagon = pose2SLAM::circle(6,1.0);
Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)];
pose2SLAM::Values hexagon = pose2SLAM::circle(6,1.0);
Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1);
// create a Pose graph with one equality constraint and one measurement
shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
@ -230,13 +231,13 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
fg->addOdometry(5, 0, delta, covariance);
// Create initial config
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)));
boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
initial->insertPose(0, p0);
initial->insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1)));
initial->insertPose(3, hexagon.pose(3).retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insertPose(4, hexagon.pose(4).retract(Vector_(3, 0.1,-0.1, 0.1)));
initial->insertPose(5, hexagon.pose(5).retract(Vector_(3,-0.1, 0.1,-0.1)));
// Choose an ordering
shared_ptr<Ordering> ordering(new Ordering);
@ -250,10 +251,10 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
Values actual = *optimizer.values();
// Check with ground truth
CHECK(assert_equal(hexagon, actual));
CHECK(assert_equal((const Values&)hexagon, actual));
// Check loop closure
CHECK(assert_equal(delta,actual[pose2SLAM::PoseKey(5)].between(actual[pose2SLAM::PoseKey(0)])));
CHECK(assert_equal(delta, actual.at<Pose2>(PoseKey(5)).between(actual.at<Pose2>(PoseKey(0)))));
// Pose2SLAMOptimizer myOptimizer("3");
@ -387,10 +388,10 @@ TEST( Pose2Prior, error )
// Choose a linearization point
Pose2 p1(1, 0, 0); // robot at (1,0)
pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1), p1);
x0.insert(PoseKey(1), p1);
// Create factor
pose2SLAM::Prior factor(1, p1, sigmas);
pose2SLAM::Prior factor(PoseKey(1), p1, sigmas);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
@ -417,7 +418,7 @@ TEST( Pose2Prior, error )
/* ************************************************************************* */
// common Pose2Prior for tests below
static gtsam::Pose2 priorVal(2,2,M_PI_2);
static pose2SLAM::Prior priorFactor(1,priorVal, sigmas);
static pose2SLAM::Prior priorFactor(PoseKey(1), priorVal, sigmas);
/* ************************************************************************* */
// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
@ -431,7 +432,7 @@ TEST( Pose2Prior, linearize )
{
// Choose a linearization point at ground truth
pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1),priorVal);
x0.insertPose(1, priorVal);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
@ -451,12 +452,12 @@ TEST( Pose2Factor, error )
Pose2 p1; // robot at origin
Pose2 p2(1, 0, 0); // robot at (1,0)
pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1), p1);
x0.insert(pose2SLAM::PoseKey(2), p2);
x0.insertPose(1, p1);
x0.insertPose(2, p2);
// Create factor
Pose2 z = p1.between(p2);
Pose2Factor factor(1, 2, z, covariance);
Pose2Factor factor(PoseKey(1), PoseKey(2), z, covariance);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
@ -483,7 +484,7 @@ TEST( Pose2Factor, error )
/* ************************************************************************* */
// common Pose2Factor for tests below
static Pose2 measured(2,2,M_PI_2);
static Pose2Factor factor(1,2,measured, covariance);
static Pose2Factor factor(PoseKey(1),PoseKey(2),measured, covariance);
/* ************************************************************************* */
TEST( Pose2Factor, rhs )

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

@ -28,7 +28,7 @@ using namespace boost::assign;
// TODO: DANGEROUS, create shared pointers
#define GTSAM_MAGIC_GAUSSIAN 6
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/slam/pose3SLAM.h>
@ -50,7 +50,7 @@ TEST(Pose3Graph, optimizeCircle) {
// Create a hexagon of poses
double radius = 10;
Values hexagon = pose3SLAM::circle(6,radius);
Pose3 gT0 = hexagon[Key(0)], gT1 = hexagon[Key(1)];
Pose3 gT0 = hexagon[PoseKey(0)], gT1 = hexagon[PoseKey(1)];
// create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose3Graph> fg(new Pose3Graph);
@ -67,12 +67,12 @@ TEST(Pose3Graph, optimizeCircle) {
// Create initial config
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)));
initial->insert(PoseKey(0), gT0);
initial->insert(PoseKey(1), hexagon.at<Pose3>(PoseKey(1)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(PoseKey(2), hexagon.at<Pose3>(PoseKey(2)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(PoseKey(3), hexagon.at<Pose3>(PoseKey(3)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(PoseKey(4), hexagon.at<Pose3>(PoseKey(4)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(PoseKey(5), hexagon.at<Pose3>(PoseKey(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);
@ -87,17 +87,17 @@ TEST(Pose3Graph, optimizeCircle) {
CHECK(assert_equal(hexagon, actual,1e-4));
// Check loop closure
CHECK(assert_equal(_0T1,actual[Key(5)].between(actual[Key(0)]),1e-5));
CHECK(assert_equal(_0T1, actual.at<Pose3>(PoseKey(5)).between(actual.at<Pose3>(PoseKey(0))),1e-5));
}
/* ************************************************************************* */
TEST(Pose3Graph, partial_prior_height) {
typedef PartialPriorFactor<pose3SLAM::Key> Partial;
typedef PartialPriorFactor<Pose3> Partial;
// reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3)
// height prior - single element interface
pose3SLAM::Key key(1);
Symbol key = PoseKey(1);
double exp_height = 5.0;
SharedDiagonal model = noiseModel::Unit::Create(1);
Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height));
@ -117,7 +117,7 @@ TEST(Pose3Graph, partial_prior_height) {
EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
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);
}
@ -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));

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

@ -18,7 +18,7 @@
#include <CppUnitLite/TestHarness.h>
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/slam/visualSLAM.h>
@ -49,7 +49,7 @@ TEST( ProjectionFactor, error )
Point2 z(323.,240.);
int cameraFrameNumber=1, landmarkNumber=1;
boost::shared_ptr<visualSLAM::ProjectionFactor>
factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
factor(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK));
// For the following values structure, the factor predicts 320,240
Values config;
@ -98,10 +98,10 @@ TEST( ProjectionFactor, equals )
Vector z = Vector_(2,323.,240.);
int cameraFrameNumber=1, landmarkNumber=1;
boost::shared_ptr<visualSLAM::ProjectionFactor>
factor1(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
factor1(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK));
boost::shared_ptr<visualSLAM::ProjectionFactor>
factor2(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
factor2(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK));
CHECK(assert_equal(*factor1, *factor2));
}

209
gtsam/slam/tests/testSerializationSLAM.cpp Normal file
View File

@ -0,0 +1,209 @@
/* ----------------------------------------------------------------------------
* 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 testSerializationSLAM.cpp
* @brief
* @author Richard Roberts
* @date Feb 7, 2012
*/
#define GTSAM_MAGIC_KEY
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/slam/smallExample.h>
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
/* Create GUIDs for factors */
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
/* ************************************************************************* */
// Export Noisemodels
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* ************************************************************************* */
TEST (Serialization, smallExample_linear) {
using namespace example;
Ordering ordering; ordering += "x1","x2","l1";
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
EXPECT(equalsObj(ordering));
EXPECT(equalsXML(ordering));
EXPECT(equalsObj(fg));
EXPECT(equalsXML(fg));
GaussianBayesNet cbn = createSmallGaussianBayesNet();
EXPECT(equalsObj(cbn));
EXPECT(equalsXML(cbn));
}
/* ************************************************************************* */
TEST (Serialization, gaussianISAM) {
using namespace example;
Ordering ordering;
GaussianFactorGraph smoother;
boost::tie(smoother, ordering) = createSmoother(7);
BayesTree<GaussianConditional> bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
GaussianISAM isam(bayesTree);
EXPECT(equalsObj(isam));
EXPECT(equalsXML(isam));
}
/* ************************************************************************* */
/* Create GUIDs for factors in simulated2D */
BOOST_CLASS_EXPORT_GUID(simulated2D::Prior, "gtsam::simulated2D::Prior" )
BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" )
BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement")
BOOST_CLASS_EXPORT(gtsam::Point2)
/* ************************************************************************* */
TEST (Serialization, smallExample) {
using namespace example;
Graph nfg = createNonlinearFactorGraph();
Values c1 = createValues();
EXPECT(equalsObj(nfg));
EXPECT(equalsXML(nfg));
EXPECT(equalsObj(c1));
EXPECT(equalsXML(c1));
}
/* ************************************************************************* */
/* Create GUIDs for factors */
BOOST_CLASS_EXPORT_GUID(planarSLAM::Prior, "gtsam::planarSLAM::Prior");
BOOST_CLASS_EXPORT_GUID(planarSLAM::Bearing, "gtsam::planarSLAM::Bearing");
BOOST_CLASS_EXPORT_GUID(planarSLAM::Range, "gtsam::planarSLAM::Range");
BOOST_CLASS_EXPORT_GUID(planarSLAM::BearingRange,"gtsam::planarSLAM::BearingRange");
BOOST_CLASS_EXPORT_GUID(planarSLAM::Odometry, "gtsam::planarSLAM::Odometry");
BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint, "gtsam::planarSLAM::Constraint");
BOOST_CLASS_EXPORT(gtsam::Pose2)
/* ************************************************************************* */
TEST (Serialization, planar_system) {
using namespace planarSLAM;
planarSLAM::Values values;
values.insert(PointKey(3), Point2(1.0, 2.0));
values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3));
SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3);
SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3);
SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3);
Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1);
Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1);
Range range(PoseKey(2), PointKey(9), 7.0, model1);
BearingRange bearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2);
Odometry odometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3);
Constraint constraint(PoseKey(9), Pose2(2.0,-1.0, 0.2));
Graph graph;
graph.add(prior);
graph.add(bearing);
graph.add(range);
graph.add(bearingRange);
graph.add(odometry);
graph.add(constraint);
// text
EXPECT(equalsObj<Symbol>(PoseKey(2)));
EXPECT(equalsObj<Symbol>(PointKey(3)));
EXPECT(equalsObj<planarSLAM::Values>(values));
EXPECT(equalsObj<Prior>(prior));
EXPECT(equalsObj<Bearing>(bearing));
EXPECT(equalsObj<BearingRange>(bearingRange));
EXPECT(equalsObj<Range>(range));
EXPECT(equalsObj<Odometry>(odometry));
EXPECT(equalsObj<Constraint>(constraint));
EXPECT(equalsObj<Graph>(graph));
// xml
EXPECT(equalsXML<Symbol>(PoseKey(2)));
EXPECT(equalsXML<Symbol>(PointKey(3)));
EXPECT(equalsXML<planarSLAM::Values>(values));
EXPECT(equalsXML<Prior>(prior));
EXPECT(equalsXML<Bearing>(bearing));
EXPECT(equalsXML<BearingRange>(bearingRange));
EXPECT(equalsXML<Range>(range));
EXPECT(equalsXML<Odometry>(odometry));
EXPECT(equalsXML<Constraint>(constraint));
EXPECT(equalsXML<Graph>(graph));
}
/* ************************************************************************* */
/* Create GUIDs for factors */
BOOST_CLASS_EXPORT_GUID(visualSLAM::PoseConstraint, "gtsam::visualSLAM::PoseConstraint");
BOOST_CLASS_EXPORT_GUID(visualSLAM::PointConstraint, "gtsam::visualSLAM::PointConstraint");
BOOST_CLASS_EXPORT_GUID(visualSLAM::PosePrior, "gtsam::visualSLAM::PosePrior");
BOOST_CLASS_EXPORT_GUID(visualSLAM::PointPrior, "gtsam::visualSLAM::PointPrior");
BOOST_CLASS_EXPORT_GUID(visualSLAM::ProjectionFactor,"gtsam::visualSLAM::ProjectionFactor");
BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoFactor");
BOOST_CLASS_EXPORT(gtsam::Pose3)
BOOST_CLASS_EXPORT(gtsam::Point3)
Point3 pt3(1.0, 2.0, 3.0);
Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
Pose3 pose3(rt3, pt3);
Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
/* ************************************************************************* */
TEST (Serialization, visual_system) {
using namespace visualSLAM;
Values values;
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);
values.insert(l1, pt1);
SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3);
SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3);
SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3);
boost::shared_ptr<Cal3_S2> K(new Cal3_S2(cal1));
Graph graph;
graph.addMeasurement(Point2(1.0, 2.0), model2, 1, 1, K);
graph.addPointConstraint(1, pt1);
graph.addPointPrior(1, pt2, model3);
graph.addPoseConstraint(1, pose1);
graph.addPosePrior(1, pose2, model6);
EXPECT(equalsObj(values));
EXPECT(equalsObj(graph));
EXPECT(equalsXML(values));
EXPECT(equalsXML(graph));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

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

@ -52,11 +52,11 @@ TEST( StereoFactor, singlePoint)
boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));
boost::shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph());
graph->add(visualSLAM::PoseConstraint(1,camera1));
graph->add(visualSLAM::PoseConstraint(PoseKey(1),camera1));
StereoPoint2 z14(320,320.0-50, 240);
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
graph->add(visualSLAM::StereoFactor(z14,sigma, 1, 1, K));
graph->add(visualSLAM::StereoFactor(z14,sigma, PoseKey(1), PointKey(1), K));
// Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values());

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

@ -22,7 +22,7 @@
#include <boost/shared_ptr.hpp>
using namespace boost;
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

70
gtsam/slam/visualSLAM.h
View File

@ -23,7 +23,7 @@
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
@ -34,22 +34,24 @@ namespace visualSLAM {
using namespace gtsam;
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Convenience function for constructing a pose key
inline Symbol PointKey(Index j) { return Symbol('l', j); }
/**
* Typedefs that make up the visualSLAM namespace.
*/
typedef TypedSymbol<Pose3,'x'> PoseKey; ///< The key type used for poses
typedef TypedSymbol<Point3,'l'> PointKey; ///< The key type used for points
typedef boost::shared_ptr<Values> shared_values; ///< shared pointer to values data structure
typedef NonlinearEquality<PoseKey> PoseConstraint; ///< put a hard constraint on a pose
typedef NonlinearEquality<PointKey> PointConstraint; ///< put a hard constraint on a point
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
typedef NonlinearEquality<Pose3> PoseConstraint; ///< put a hard constraint on a pose
typedef NonlinearEquality<Point3> PointConstraint; ///< put a hard constraint on a point
typedef PriorFactor<Pose3> PosePrior; ///< put a soft prior on a Pose
typedef PriorFactor<Point3> PointPrior; ///< put a soft prior on a point
typedef RangeFactor<Pose3, Point3> RangeFactor; ///< put a factor on the range from a pose to a point
/// monocular and stereo camera typedefs for general use
typedef GenericProjectionFactor<PointKey, PoseKey> ProjectionFactor;
typedef GenericStereoFactor<PoseKey, PointKey> StereoFactor;
typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;
typedef GenericStereoFactor<Pose3, Point3> StereoFactor;
/**
* Non-linear factor graph for vanilla visual SLAM
@ -76,69 +78,69 @@ namespace visualSLAM {
/**
* Add a projection factor measurement (monocular)
* @param z the measurement
* @param measured the measurement
* @param model the noise model for the measurement
* @param i index of camera
* @param j index of point
* @param poseKey variable key for the camera pose
* @param pointKey variable key for the landmark
* @param K shared pointer to calibration object
*/
void addMeasurement(const Point2& z, const SharedNoiseModel& model,
PoseKey i, PointKey j, const shared_ptrK& K) {
boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(z, model, i, j, K));
void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
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);
}
/**
* Add a constraint on a pose (for now, *must* be satisfied in any Values)
* @param j index of camera
* @param key variable key of the camera pose
* @param p to which pose to constrain it to
*/
void addPoseConstraint(int j, const Pose3& p = Pose3()) {
boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(j, p));
void addPoseConstraint(Index poseKey, const Pose3& p = Pose3()) {
boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(PoseKey(poseKey), p));
push_back(factor);
}
/**
* Add a constraint on a point (for now, *must* be satisfied in any Values)
* @param j index of landmark
* @param key variable key of the landmark
* @param p point around which soft prior is defined
*/
void addPointConstraint(int j, const Point3& p = Point3()) {
boost::shared_ptr<PointConstraint> factor(new PointConstraint(j, p));
void addPointConstraint(Index pointKey, const Point3& p = Point3()) {
boost::shared_ptr<PointConstraint> factor(new PointConstraint(PointKey(pointKey), p));
push_back(factor);
}
/**
* Add a prior on a pose
* @param j index of camera
* @param key variable key of the camera pose
* @param p around which soft prior is defined
* @param model uncertainty model of this prior
*/
void addPosePrior(int j, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) {
boost::shared_ptr<PosePrior> factor(new PosePrior(j, 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);
}
/**
* Add a prior on a landmark
* @param j index of landmark
* @param key variable key of the landmark
* @param p to which point to constrain it to
* @param model uncertainty model of this prior
*/
void addPointPrior(int j, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) {
boost::shared_ptr<PointPrior> factor(new PointPrior(j, 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);
}
/**
* Add a range prior to a landmark
* @param i index of pose
* @param j index of landmark
* @param poseKey variable key of the camera pose
* @param pointKey variable key of the landmark
* @param range approximate range to landmark
* @param model uncertainty model of this prior
*/
void addRangeFactor(PoseKey i, PointKey j, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) {
push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(i, j, 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

10
tests/Makefile.am
View File

@ -24,11 +24,6 @@ check_PROGRAMS += testGaussianISAM2
check_PROGRAMS += testExtendedKalmanFilter
check_PROGRAMS += testRot3Optimization
## flag disabled to force this test to get updated properly
#if ENABLE_SERIALIZATION
check_PROGRAMS += testSerialization
#endif
# Timing tests
noinst_PROGRAMS = timeGaussianFactorGraph timeSequentialOnDataset timeMultifrontalOnDataset
@ -39,11 +34,6 @@ TESTS = $(check_PROGRAMS)
AM_LDFLAGS = $(BOOST_LDFLAGS)
AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir)
# link to serialization library for test
#if ENABLE_SERIALIZATION
AM_LDFLAGS += -lboost_serialization
#endif
LDADD = ../gtsam/libgtsam.la ../CppUnitLite/libCppUnitLite.a
AM_DEFAULT_SOURCE_EXT = .cpp

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

2
tests/testGaussianBayesNet.cpp
View File

@ -25,7 +25,7 @@
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/base/Testable.h>

2
tests/testGaussianFactor.cpp
View File

@ -26,7 +26,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/base/Matrix.h>

2
tests/testGaussianFactorGraph.cpp
View File

@ -28,7 +28,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/base/Matrix.h>

2
tests/testGaussianISAM.cpp
View File

@ -21,7 +21,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/geometry/Rot2.h>

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

8
tests/testGaussianJunctionTree.cpp
View File

@ -25,7 +25,7 @@
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/base/debug.h>
@ -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;

37
tests/testGraph.cpp
View File

@ -23,6 +23,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
// TODO: DANGEROUS, create shared pointers
#define GTSAM_MAGIC_GAUSSIAN 3
@ -37,22 +39,21 @@ using namespace gtsam;
// -> x3 -> x4
// -> x5
TEST ( Ordering, predecessorMap2Keys ) {
typedef TypedSymbol<Pose2,'x'> PoseKey;
PredecessorMap<PoseKey> p_map;
p_map.insert(1,1);
p_map.insert(2,1);
p_map.insert(3,1);
p_map.insert(4,3);
p_map.insert(5,1);
PredecessorMap<Symbol> p_map;
p_map.insert("x1","x1");
p_map.insert("x2","x1");
p_map.insert("x3","x1");
p_map.insert("x4","x3");
p_map.insert("x5","x1");
list<PoseKey> expected;
expected += 4,5,3,2,1;//PoseKey(4), PoseKey(5), PoseKey(3), PoseKey(2), PoseKey(1);
list<Symbol> expected;
expected += "x4","x5","x3","x2","x1";//PoseKey(4), PoseKey(5), PoseKey(3), PoseKey(2), PoseKey(1);
list<PoseKey> actual = predecessorMap2Keys<PoseKey>(p_map);
list<Symbol> actual = predecessorMap2Keys<Symbol>(p_map);
LONGS_EQUAL(expected.size(), actual.size());
list<PoseKey>::const_iterator it1 = expected.begin();
list<PoseKey>::const_iterator it2 = actual.begin();
list<Symbol>::const_iterator it1 = expected.begin();
list<Symbol>::const_iterator it2 = actual.begin();
for(; it1!=expected.end(); it1++, it2++)
CHECK(*it1 == *it2)
}
@ -86,16 +87,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);

14
tests/testInference.cpp
View File

@ -17,7 +17,7 @@
#include <CppUnitLite/TestHarness.h>
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/linear/GaussianSequentialSolver.h>
@ -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());

70
tests/testNonlinearFactor.cpp
View File

@ -25,7 +25,7 @@
// TODO: DANGEROUS, create shared pointers
#define GTSAM_MAGIC_GAUSSIAN 2
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/base/Testable.h>
@ -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));
@ -235,13 +238,10 @@ TEST( NonlinearFactor, linearize_constraint2 )
}
/* ************************************************************************* */
typedef TypedSymbol<LieVector, 'x'> TestKey;
/* ************************************************************************* */
class TestFactor4 : public NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> {
class TestFactor4 : public NonlinearFactor4<LieVector, LieVector, LieVector, LieVector> {
public:
typedef NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> Base;
TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4) {}
typedef NonlinearFactor4<LieVector, LieVector, LieVector, LieVector> Base;
TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4") {}
virtual Vector
evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4,
@ -263,13 +263,13 @@ public:
TEST(NonlinearFactor, NonlinearFactor4) {
TestFactor4 tf;
Values tv;
tv.insert(TestKey(1), LieVector(1, 1.0));
tv.insert(TestKey(2), LieVector(1, 2.0));
tv.insert(TestKey(3), LieVector(1, 3.0));
tv.insert(TestKey(4), LieVector(1, 4.0));
tv.insert("x1", LieVector(1, 1.0));
tv.insert("x2", LieVector(1, 2.0));
tv.insert("x3", LieVector(1, 3.0));
tv.insert("x4", LieVector(1, 4.0));
EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4);
Ordering ordering; ordering += "x1", "x2", "x3", "x4";
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);
@ -283,10 +283,10 @@ TEST(NonlinearFactor, NonlinearFactor4) {
}
/* ************************************************************************* */
class TestFactor5 : public NonlinearFactor5<TestKey, TestKey, TestKey, TestKey, TestKey> {
class TestFactor5 : public NonlinearFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> {
public:
typedef NonlinearFactor5<TestKey, TestKey, TestKey, TestKey, TestKey> Base;
TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {}
typedef NonlinearFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base;
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,
@ -310,14 +310,14 @@ public:
TEST(NonlinearFactor, NonlinearFactor5) {
TestFactor5 tf;
Values tv;
tv.insert(TestKey(1), LieVector(1, 1.0));
tv.insert(TestKey(2), LieVector(1, 2.0));
tv.insert(TestKey(3), LieVector(1, 3.0));
tv.insert(TestKey(4), LieVector(1, 4.0));
tv.insert(TestKey(5), LieVector(1, 5.0));
tv.insert("x1", LieVector(1, 1.0));
tv.insert("x2", LieVector(1, 2.0));
tv.insert("x3", LieVector(1, 3.0));
tv.insert("x4", LieVector(1, 4.0));
tv.insert("x5", LieVector(1, 5.0));
EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5);
Ordering ordering; ordering += "x1", "x2", "x3", "x4", "x5";
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);
@ -333,10 +333,10 @@ TEST(NonlinearFactor, NonlinearFactor5) {
}
/* ************************************************************************* */
class TestFactor6 : public NonlinearFactor6<TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> {
class TestFactor6 : public NonlinearFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> {
public:
typedef NonlinearFactor6<TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> Base;
TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {}
typedef NonlinearFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base;
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,
@ -362,15 +362,15 @@ public:
TEST(NonlinearFactor, NonlinearFactor6) {
TestFactor6 tf;
Values tv;
tv.insert(TestKey(1), LieVector(1, 1.0));
tv.insert(TestKey(2), LieVector(1, 2.0));
tv.insert(TestKey(3), LieVector(1, 3.0));
tv.insert(TestKey(4), LieVector(1, 4.0));
tv.insert(TestKey(5), LieVector(1, 5.0));
tv.insert(TestKey(6), LieVector(1, 6.0));
tv.insert("x1", LieVector(1, 1.0));
tv.insert("x2", LieVector(1, 2.0));
tv.insert("x3", LieVector(1, 3.0));
tv.insert("x4", LieVector(1, 4.0));
tv.insert("x5", LieVector(1, 5.0));
tv.insert("x6", LieVector(1, 6.0));
EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5), TestKey(6);
Ordering ordering; ordering += "x1", "x2", "x3", "x4", "x5", "x6";
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);

2
tests/testNonlinearFactorGraph.cpp
View File

@ -27,7 +27,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/base/Testable.h>

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

2
tests/testNonlinearOptimizer.cpp
View File

@ -26,7 +26,7 @@ using namespace boost::assign;
#include <boost/shared_ptr.hpp>
using namespace boost;
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/base/Matrix.h>

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;

592
tests/testSerialization.cpp
View File

@ -1,592 +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
* -------------------------------------------------------------------------- */
/**
* @brief Unit tests for serialization of library classes
* @author Frank Dellaert
* @author Alex Cunningham
*/
/* ************************************************************************* */
// Serialization testing code.
/* ************************************************************************* */
#include <sstream>
#include <string>
// includes for standard serialization types
#include <boost/serialization/export.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/list.hpp>
#include <boost/serialization/deque.hpp>
#include <boost/serialization/weak_ptr.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include <gtsam/geometry/Point2.h>
// whether to print the serialized text to stdout
const bool verbose = false;
template<class T>
std::string serialize(const T& input) {
std::ostringstream out_archive_stream;
boost::archive::text_oarchive out_archive(out_archive_stream);
out_archive << input;
return out_archive_stream.str();
}
template<class T>
void deserialize(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
boost::archive::text_iarchive in_archive(in_archive_stream);
in_archive >> output;
}
// Templated round-trip serialization
template<class T>
void roundtrip(const T& input, T& output) {
// Serialize
std::string serialized = serialize(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
deserialize(serialized, output);
}
// This version requires equality operator
template<class T>
bool equality(const T& input = T()) {
T output;
roundtrip<T>(input,output);
return input==output;
}
// This version requires equals
template<class T>
bool equalsObj(const T& input = T()) {
T output;
roundtrip<T>(input,output);
return input.equals(output);
}
// De-referenced version for pointers
template<class T>
bool equalsDereferenced(const T& input) {
T output;
roundtrip<T>(input,output);
return input->equals(*output);
}
/* ************************************************************************* */
template<class T>
std::string serializeXML(const T& input) {
std::ostringstream out_archive_stream;
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp("data", input);
return out_archive_stream.str();
}
template<class T>
void deserializeXML(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
boost::archive::xml_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp("data", output);
}
// Templated round-trip serialization using XML
template<class T>
void roundtripXML(const T& input, T& output) {
// Serialize
std::string serialized = serializeXML<T>(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
// De-serialize
deserializeXML(serialized, output);
}
// This version requires equality operator
template<class T>
bool equalityXML(const T& input = T()) {
T output;
roundtripXML<T>(input,output);
return input==output;
}
// This version requires equals
template<class T>
bool equalsXML(const T& input = T()) {
T output;
roundtripXML<T>(input,output);
return input.equals(output);
}
// This version is for pointers
template<class T>
bool equalsDereferencedXML(const T& input = T()) {
T output;
roundtripXML<T>(input,output);
return input->equals(*output);
}
/* ************************************************************************* */
// Actual Tests
/* ************************************************************************* */
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
#define GTSAM_MAGIC_KEY
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/slam/smallExample.h>
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/slam/pose3SLAM.h>
#include <gtsam/slam/visualSLAM.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST (Serialization, matrix_vector) {
EXPECT(equality<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
EXPECT(equality<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityXML<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityXML<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
}
/* ************************************************************************* */
// Export all classes derived from Value
BOOST_CLASS_EXPORT(gtsam::Cal3_S2)
BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo)
BOOST_CLASS_EXPORT(gtsam::Cal3Bundler)
BOOST_CLASS_EXPORT(gtsam::CalibratedCamera)
BOOST_CLASS_EXPORT(gtsam::Point2)
BOOST_CLASS_EXPORT(gtsam::Point3)
BOOST_CLASS_EXPORT(gtsam::Pose2)
BOOST_CLASS_EXPORT(gtsam::Pose3)
BOOST_CLASS_EXPORT(gtsam::Rot2)
BOOST_CLASS_EXPORT(gtsam::Rot3)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
BOOST_CLASS_EXPORT(gtsam::StereoPoint2)
/* ************************************************************************* */
Point3 pt3(1.0, 2.0, 3.0);
Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
Pose3 pose3(rt3, pt3);
Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
Cal3Bundler cal3(1.0, 2.0, 3.0);
Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
CalibratedCamera cal5(Pose3(rt3, pt3));
PinholeCamera<Cal3_S2> cam1(pose3, cal1);
StereoCamera cam2(pose3, cal4ptr);
StereoPoint2 spt(1.0, 2.0, 3.0);
/* ************************************************************************* */
TEST (Serialization, text_geometry) {
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
EXPECT(equalsObj(pt3));
EXPECT(equalsObj<gtsam::Rot3>(rt3));
EXPECT(equalsObj<gtsam::Pose3>(Pose3(rt3, pt3)));
EXPECT(equalsObj(cal1));
EXPECT(equalsObj(cal2));
EXPECT(equalsObj(cal3));
EXPECT(equalsObj(cal4));
EXPECT(equalsObj(cal5));
EXPECT(equalsObj(cam1));
EXPECT(equalsObj(cam2));
EXPECT(equalsObj(spt));
}
/* ************************************************************************* */
TEST (Serialization, xml_geometry) {
EXPECT(equalsXML<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
EXPECT(equalsXML<gtsam::Point3>(pt3));
EXPECT(equalsXML<gtsam::Rot3>(rt3));
EXPECT(equalsXML<gtsam::Pose3>(Pose3(rt3, pt3)));
EXPECT(equalsXML(cal1));
EXPECT(equalsXML(cal2));
EXPECT(equalsXML(cal3));
EXPECT(equalsXML(cal4));
EXPECT(equalsXML(cal5));
EXPECT(equalsXML(cam1));
EXPECT(equalsXML(cam2));
EXPECT(equalsXML(spt));
}
/* ************************************************************************* */
typedef PinholeCamera<Cal3_S2> PinholeCal3S2;
typedef PinholeCamera<Cal3DS2> PinholeCal3DS2;
typedef PinholeCamera<Cal3Bundler> PinholeCal3Bundler;
typedef TypedSymbol<Cal3_S2,'a'> PinholeCal3S2Key;
typedef TypedSymbol<Cal3DS2,'s'> PinholeCal3DS2Key;
typedef TypedSymbol<Cal3Bundler,'d'> PinholeCal3BundlerKey;
TEST (Serialization, TemplatedValues) {
Values values;
values.insert(PinholeCal3S2Key(0), PinholeCal3S2(pose3, cal1));
values.insert(PinholeCal3DS2Key(5), PinholeCal3DS2(pose3, cal2));
values.insert(PinholeCal3BundlerKey(47), PinholeCal3Bundler(pose3, cal3));
values.insert(PinholeCal3S2Key(5), PinholeCal3S2(pose3, cal1));
EXPECT(equalsObj(values));
EXPECT(equalsXML(values));
}
/* ************************************************************************* */
// Export Noisemodels
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* ************************************************************************* */
// example noise models
noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1));
noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
/* ************************************************************************* */
TEST (Serialization, noiseModels) {
// tests using pointers to the derived class
EXPECT( equalsDereferenced<noiseModel::Diagonal::shared_ptr>(diag3));
EXPECT(equalsDereferencedXML<noiseModel::Diagonal::shared_ptr>(diag3));
EXPECT( equalsDereferenced<noiseModel::Gaussian::shared_ptr>(gaussian3));
EXPECT(equalsDereferencedXML<noiseModel::Gaussian::shared_ptr>(gaussian3));
EXPECT( equalsDereferenced<noiseModel::Isotropic::shared_ptr>(iso3));
EXPECT(equalsDereferencedXML<noiseModel::Isotropic::shared_ptr>(iso3));
EXPECT( equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
EXPECT( equalsDereferenced<noiseModel::Unit::shared_ptr>(unit3));
EXPECT(equalsDereferencedXML<noiseModel::Unit::shared_ptr>(unit3));
}
/* ************************************************************************* */
TEST (Serialization, SharedNoiseModel_noiseModels) {
SharedNoiseModel diag3_sg = diag3;
EXPECT(equalsDereferenced<SharedNoiseModel>(diag3_sg));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3_sg));
EXPECT(equalsDereferenced<SharedNoiseModel>(diag3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3));
EXPECT(equalsDereferenced<SharedNoiseModel>(iso3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(iso3));
EXPECT(equalsDereferenced<SharedNoiseModel>(gaussian3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(gaussian3));
EXPECT(equalsDereferenced<SharedNoiseModel>(unit3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(unit3));
EXPECT(equalsDereferenced<SharedNoiseModel>(constrained3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(constrained3));
}
/* ************************************************************************* */
TEST (Serialization, SharedDiagonal_noiseModels) {
EXPECT(equalsDereferenced<SharedDiagonal>(diag3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(diag3));
EXPECT(equalsDereferenced<SharedDiagonal>(iso3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(iso3));
EXPECT(equalsDereferenced<SharedDiagonal>(unit3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(unit3));
EXPECT(equalsDereferenced<SharedDiagonal>(constrained3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(constrained3));
}
/* ************************************************************************* */
// Linear components
/* ************************************************************************* */
/* ************************************************************************* */
TEST (Serialization, linear_factors) {
VectorValues values;
values.insert(0, Vector_(1, 1.0));
values.insert(1, Vector_(2, 2.0,3.0));
values.insert(2, Vector_(2, 4.0,5.0));
EXPECT(equalsObj<VectorValues>(values));
EXPECT(equalsXML<VectorValues>(values));
Index i1 = 4, i2 = 7;
Matrix A1 = eye(3), A2 = -1.0 * eye(3);
Vector b = ones(3);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0));
JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
EXPECT(equalsObj(jacobianfactor));
EXPECT(equalsXML(jacobianfactor));
HessianFactor hessianfactor(jacobianfactor);
EXPECT(equalsObj(hessianfactor));
EXPECT(equalsXML(hessianfactor));
}
/* ************************************************************************* */
TEST (Serialization, gaussian_conditional) {
Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.);
Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4);
Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34);
Vector d(2); d << 0.2, 0.5;
GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2));
EXPECT(equalsObj(cg));
EXPECT(equalsXML(cg));
}
/* Create GUIDs for factors */
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
/* ************************************************************************* */
TEST (Serialization, smallExample_linear) {
using namespace example;
Ordering ordering; ordering += "x1","x2","l1";
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
EXPECT(equalsObj(ordering));
EXPECT(equalsXML(ordering));
EXPECT(equalsObj(fg));
EXPECT(equalsXML(fg));
GaussianBayesNet cbn = createSmallGaussianBayesNet();
EXPECT(equalsObj(cbn));
EXPECT(equalsXML(cbn));
}
/* ************************************************************************* */
TEST (Serialization, symbolic_graph) {
Ordering o; o += "x1","l1","x2";
// construct expected symbolic graph
SymbolicFactorGraph sfg;
sfg.push_factor(o["x1"]);
sfg.push_factor(o["x1"],o["x2"]);
sfg.push_factor(o["x1"],o["l1"]);
sfg.push_factor(o["l1"],o["x2"]);
EXPECT(equalsObj(sfg));
EXPECT(equalsXML(sfg));
}
/* ************************************************************************* */
TEST (Serialization, symbolic_bn) {
Ordering o; o += "x2","l1","x1";
IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"]));
IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"]));
IndexConditional::shared_ptr x1(new IndexConditional(o["x1"]));
SymbolicBayesNet sbn;
sbn.push_back(x2);
sbn.push_back(l1);
sbn.push_back(x1);
EXPECT(equalsObj(sbn));
EXPECT(equalsXML(sbn));
}
/* ************************************************************************* */
TEST (Serialization, symbolic_bayes_tree ) {
typedef BayesTree<IndexConditional> SymbolicBayesTree;
static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
IndexConditional::shared_ptr
B(new IndexConditional(_B_)),
L(new IndexConditional(_L_, _B_)),
E(new IndexConditional(_E_, _L_, _B_)),
S(new IndexConditional(_S_, _L_, _B_)),
T(new IndexConditional(_T_, _E_, _L_)),
X(new IndexConditional(_X_, _E_));
// Bayes Tree for Asia example
SymbolicBayesTree bayesTree;
SymbolicBayesTree::insert(bayesTree, B);
SymbolicBayesTree::insert(bayesTree, L);
SymbolicBayesTree::insert(bayesTree, E);
SymbolicBayesTree::insert(bayesTree, S);
SymbolicBayesTree::insert(bayesTree, T);
SymbolicBayesTree::insert(bayesTree, X);
EXPECT(equalsObj(bayesTree));
EXPECT(equalsXML(bayesTree));
}
/* ************************************************************************* */
TEST (Serialization, gaussianISAM) {
using namespace example;
Ordering ordering;
GaussianFactorGraph smoother;
boost::tie(smoother, ordering) = createSmoother(7);
BayesTree<GaussianConditional> bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
GaussianISAM isam(bayesTree);
EXPECT(equalsObj(isam));
EXPECT(equalsXML(isam));
}
/* ************************************************************************* */
/* Create GUIDs for factors in simulated2D */
BOOST_CLASS_EXPORT_GUID(simulated2D::Prior, "gtsam::simulated2D::Prior" )
BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" )
BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement")
/* ************************************************************************* */
TEST (Serialization, smallExample) {
using namespace example;
Graph nfg = createNonlinearFactorGraph();
Values c1 = createValues();
EXPECT(equalsObj(nfg));
EXPECT(equalsXML(nfg));
EXPECT(equalsObj(c1));
EXPECT(equalsXML(c1));
}
/* ************************************************************************* */
/* Create GUIDs for factors */
BOOST_CLASS_EXPORT_GUID(planarSLAM::Prior, "gtsam::planarSLAM::Prior");
BOOST_CLASS_EXPORT_GUID(planarSLAM::Bearing, "gtsam::planarSLAM::Bearing");
BOOST_CLASS_EXPORT_GUID(planarSLAM::Range, "gtsam::planarSLAM::Range");
BOOST_CLASS_EXPORT_GUID(planarSLAM::BearingRange,"gtsam::planarSLAM::BearingRange");
BOOST_CLASS_EXPORT_GUID(planarSLAM::Odometry, "gtsam::planarSLAM::Odometry");
BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint, "gtsam::planarSLAM::Constraint");
/* ************************************************************************* */
TEST (Serialization, planar_system) {
using namespace planarSLAM;
planarSLAM::Values values;
values.insert(PointKey(3), Point2(1.0, 2.0));
values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3));
SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3);
SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3);
SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3);
Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1);
Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1);
Range range(PoseKey(2), PointKey(9), 7.0, model1);
BearingRange bearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2);
Odometry odometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3);
Constraint constraint(PoseKey(9), Pose2(2.0,-1.0, 0.2));
Graph graph;
graph.add(prior);
graph.add(bearing);
graph.add(range);
graph.add(bearingRange);
graph.add(odometry);
graph.add(constraint);
// text
EXPECT(equalsObj<PoseKey>(PoseKey(2)));
EXPECT(equalsObj<PointKey>(PointKey(3)));
EXPECT(equalsObj<planarSLAM::Values>(values));
EXPECT(equalsObj<Prior>(prior));
EXPECT(equalsObj<Bearing>(bearing));
EXPECT(equalsObj<BearingRange>(bearingRange));
EXPECT(equalsObj<Range>(range));
EXPECT(equalsObj<Odometry>(odometry));
EXPECT(equalsObj<Constraint>(constraint));
EXPECT(equalsObj<Graph>(graph));
// xml
EXPECT(equalsXML<PoseKey>(PoseKey(2)));
EXPECT(equalsXML<PointKey>(PointKey(3)));
EXPECT(equalsXML<planarSLAM::Values>(values));
EXPECT(equalsXML<Prior>(prior));
EXPECT(equalsXML<Bearing>(bearing));
EXPECT(equalsXML<BearingRange>(bearingRange));
EXPECT(equalsXML<Range>(range));
EXPECT(equalsXML<Odometry>(odometry));
EXPECT(equalsXML<Constraint>(constraint));
EXPECT(equalsXML<Graph>(graph));
}
/* ************************************************************************* */
/* Create GUIDs for factors */
BOOST_CLASS_EXPORT_GUID(visualSLAM::PoseConstraint, "gtsam::visualSLAM::PoseConstraint");
BOOST_CLASS_EXPORT_GUID(visualSLAM::PointConstraint, "gtsam::visualSLAM::PointConstraint");
BOOST_CLASS_EXPORT_GUID(visualSLAM::PosePrior, "gtsam::visualSLAM::PosePrior");
BOOST_CLASS_EXPORT_GUID(visualSLAM::PointPrior, "gtsam::visualSLAM::PointPrior");
BOOST_CLASS_EXPORT_GUID(visualSLAM::ProjectionFactor,"gtsam::visualSLAM::ProjectionFactor");
BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoFactor");
/* ************************************************************************* */
TEST (Serialization, visual_system) {
using namespace visualSLAM;
Values values;
PoseKey x1(1), x2(2);
PointKey l1(1), l2(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);
values.insert(l1, pt1);
SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3);
SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3);
SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3);
boost::shared_ptr<Cal3_S2> K(new Cal3_S2(cal1));
Graph graph;
graph.addMeasurement(Point2(1.0, 2.0), model2, x1, l1, K);
graph.addPointConstraint(1, pt1);
graph.addPointPrior(1, pt2, model3);
graph.addPoseConstraint(1, pose1);
graph.addPosePrior(1, pose2, model6);
EXPECT(equalsObj(values));
EXPECT(equalsObj(graph));
EXPECT(equalsXML(values));
EXPECT(equalsXML(graph));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

2
tests/testSymbolicBayesNet.cpp
View File

@ -21,7 +21,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/base/Testable.h>

2
tests/testSymbolicFactorGraph.cpp
View File

@ -20,7 +20,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/slam/smallExample.h>

4
tests/testTupleValues.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/base/TestableAssertions.h>
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <gtsam/geometry/Pose2.h>
@ -29,7 +29,7 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/Vector.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/TupleValues.h>
using namespace gtsam;

2
tests/timeGaussianFactorGraph.cpp
View File

@ -15,7 +15,7 @@
* @author Frank Dellaert
*/
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
#define GTSAM_MAGIC_KEY
#include <time.h>