diff --git a/.cproject b/.cproject
index b6034e167..c47c94ecf 100644
--- a/.cproject
+++ b/.cproject
@@ -286,39 +286,7 @@
-
- make
- -j2
- tests/testDSFVector.run
- true
- true
- true
-
-
- make
- -j2
- tests/testTestableAssertions.run
- true
- true
- true
-
-
- make
- -j2
- tests/testVector.run
- true
- true
- true
-
-
- make
- -j2
- tests/testMatrix.run
- true
- true
- true
-
-
+
make
-j2
check
@@ -326,23 +294,15 @@
true
true
-
+
make
-j2
- tests/testNumericalDerivative.run
+ tests/testSPQRUtil.run
true
true
true
-
- make
- -j2
- tests/testBlockMatrices.run
- true
- true
- true
-
-
+
make
-j2
clean
@@ -350,110 +310,6 @@
true
true
-
- make
- -j2
- tests/testCholesky.run
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- testBTree.run
- true
- true
- true
-
-
- make
- -j2
- testDSF.run
- true
- true
- true
-
-
- make
- -j2
- testDSFVector.run
- true
- true
- true
-
-
- make
- -j2
- testMatrix.run
- true
- true
- true
-
-
- make
- -j2
- testSPQRUtil.run
- true
- true
- true
-
-
- make
- -j2
- testVector.run
- true
- true
- true
-
-
- make
- -j2
- timeMatrix.run
- true
- true
- true
-
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- tests/testLieConfig.run
- true
- true
- true
-
make
-j2
@@ -480,6 +336,7 @@
make
+
tests/testBayesTree.run
true
false
@@ -487,6 +344,7 @@
make
+
testBinaryBayesNet.run
true
false
@@ -534,6 +392,7 @@
make
+
testSymbolicBayesNet.run
true
false
@@ -541,6 +400,7 @@
make
+
tests/testSymbolicFactor.run
true
false
@@ -548,6 +408,7 @@
make
+
testSymbolicFactorGraph.run
true
false
@@ -563,15 +424,120 @@
make
+
tests/testBayesTree
true
false
true
-
+
make
-j2
- vSFMexample.run
+ testGaussianFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j5
+ all
+ true
+ false
+ true
+
+
+ make
+ -j5
+ check
+ true
+ false
+ true
+
+
+ make
+ -j2
+ tests/testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testPose3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
true
true
true
@@ -616,7 +582,7 @@
true
true
-
+
make
-j2
check
@@ -624,22 +590,6 @@
true
true
-
- make
- -j2
- tests/testSPQRUtil.run
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
make
-j2
@@ -672,7 +622,7 @@
true
true
-
+
make
-j2
check
@@ -680,58 +630,18 @@
true
true
-
+
make
-j2
- all
+ vSFMexample.run
true
true
true
-
+
make
-j2
- clean
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
-
- tests/testGaussianISAM2
- true
- false
- true
-
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- clean
+ testVSLAMGraph
true
true
true
@@ -888,6 +798,890 @@
true
true
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testGaussianConditional.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ timeGaussianFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ timeVectorConfig.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testVectorBTree.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testVectorMap.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testNoiseModel.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testBayesNetPreconditioner.run
+ true
+ true
+ true
+
+
+ make
+
+ testErrors.run
+ true
+ false
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testGaussianJunctionTree.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testGaussianFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testGaussianConditional.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/timeSLAMlike.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testBTree.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testDSF.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testDSFVector.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testMatrix.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testSPQRUtil.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testVector.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ timeMatrix.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testClusterTree.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testJunctionTree.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testEliminationTree.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testSymbolicFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testVariableSlots.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testConditional.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testSymbolicFactorGraph.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testNonlinearConstraint.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testLieConfig.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testConstraintOptimizer.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ install
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testStereoCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testRot3M.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testPose3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testPoint3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testCalibratedCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/timeRot3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/timePose3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/timeStereoCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testPoint2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPlanarSLAM.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2Config.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2Factor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2Prior.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2SLAM.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose3Config.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose3SLAM.run
+ true
+ true
+ true
+
+
+ make
+ testSimulated2DOriented.run
+ true
+ false
+ true
+
+
+ make
+ -j2
+ testVSLAMConfig.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testVSLAMFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testVSLAMGraph.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose3Factor.run
+ true
+ true
+ true
+
+
+ make
+ testSimulated2D.run
+ true
+ false
+ true
+
+
+ make
+ testSimulated3D.run
+ true
+ false
+ true
+
+
+ make
+ -j2
+ tests/testGaussianISAM2
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testDSFVector.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testTestableAssertions.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testVector.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testMatrix.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testNumericalDerivative.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testBlockMatrices.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testCholesky.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testVectorValues.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testNoiseModel.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testGaussianFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testHessianFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testGaussianConditional.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testGaussianFactorGraph.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testGaussianJunctionTree.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testKalmanFilter.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testGaussianDensity.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ SimpleRotation.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ CameraResectioning
+ true
+ true
+ true
+
+
+ make
+ -j2
+ PlanarSLAMExample_easy.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ easyPoint2KalmanFilter.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ elaboratePoint2KalmanFilter.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ PlanarSLAMSelfContained_advanced.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ Pose2SLAMExample_advanced.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ Pose2SLAMExample_easy.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ Pose2SLAMwSPCG_easy.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ tests/testGaussianISAM2
+ true
+ false
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testLieConfig.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ check
+ true
+ true
+ true
+
make
-j2
@@ -984,126 +1778,6 @@
true
true
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- tests/testVectorValues.run
- true
- true
- true
-
-
- make
- -j2
- tests/testNoiseModel.run
- true
- true
- true
-
-
- make
- -j2
- tests/testGaussianFactor.run
- true
- true
- true
-
-
- make
- -j2
- tests/testHessianFactor.run
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- tests/testGaussianConditional.run
- true
- true
- true
-
-
- make
- -j2
- tests/testGaussianFactorGraph.run
- true
- true
- true
-
-
- make
- -j2
- tests/testGaussianJunctionTree.run
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- tests/testKalmanFilter.run
- true
- true
- true
-
-
- make
- -j2
- tests/testGaussianDensity.run
- true
- true
- true
-
-
- make
- -j2
- install
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j2
@@ -1152,150 +1826,6 @@
true
true
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- tests/testGaussianJunctionTree.run
- true
- true
- true
-
-
- make
- -j2
- tests/testGaussianFactor.run
- true
- true
- true
-
-
- make
- -j2
- tests/testGaussianConditional.run
- true
- true
- true
-
-
- make
- -j2
- tests/timeSLAMlike.run
- true
- true
- true
-
-
- make
- -j2
- tests/testPose2.run
- true
- true
- true
-
-
- make
- -j2
- tests/testPose3.run
- true
- true
- true
-
-
- make
- -j2
- tests/testStereoCamera.run
- true
- true
- true
-
-
- make
- -j2
- tests/testRot3M.run
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- tests/testPose2.run
- true
- true
- true
-
-
- make
- -j2
- tests/testPose3.run
- true
- true
- true
-
-
- make
- -j2
- tests/testPoint3.run
- true
- true
- true
-
-
- make
- -j2
- tests/testCalibratedCamera.run
- true
- true
- true
-
-
- make
- -j2
- tests/timeRot3.run
- true
- true
- true
-
-
- make
- -j2
- tests/timePose3.run
- true
- true
- true
-
-
- make
- -j2
- tests/timeStereoCamera.run
- true
- true
- true
-
-
- make
- -j2
- tests/testPoint2.run
- true
- true
- true
-
make
-j1
@@ -1459,7 +1989,6 @@
cmake
..
-
true
false
true
@@ -1664,525 +2193,6 @@
true
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
-
- make
- -j2
- testVSLAMGraph
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- testClusterTree.run
- true
- true
- true
-
-
- make
- -j2
- testJunctionTree.run
- true
- true
- true
-
-
- make
- -j2
- tests/testEliminationTree.run
- true
- true
- true
-
-
- make
- -j2
- tests/testSymbolicFactor.run
- true
- true
- true
-
-
- make
- -j2
- tests/testVariableSlots.run
- true
- true
- true
-
-
- make
- -j2
- tests/testConditional.run
- true
- true
- true
-
-
- make
- -j2
- tests/testSymbolicFactorGraph.run
- true
- true
- true
-
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- testNonlinearConstraint.run
- true
- true
- true
-
-
- make
- -j2
- testLieConfig.run
- true
- true
- true
-
-
- make
- -j2
- testConstraintOptimizer.run
- true
- true
- true
-
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- testGaussianConditional.run
- true
- true
- true
-
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
-
- make
- -j2
- timeGaussianFactor.run
- true
- true
- true
-
-
- make
- -j2
- timeVectorConfig.run
- true
- true
- true
-
-
- make
- -j2
- testVectorBTree.run
- true
- true
- true
-
-
- make
- -j2
- testVectorMap.run
- true
- true
- true
-
-
- make
- -j2
- testNoiseModel.run
- true
- true
- true
-
-
- make
- -j2
- testBayesNetPreconditioner.run
- true
- true
- true
-
-
- make
- testErrors.run
- true
- false
- true
-
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- clean all
- true
- true
- true
-
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j5
- all
- true
- false
- true
-
-
- make
- -j5
- check
- true
- false
- true
-
-
- make
- -j2
- SimpleRotation.run
- true
- true
- true
-
-
- make
- -j2
- CameraResectioning
- true
- true
- true
-
-
- make
- -j2
- PlanarSLAMExample_easy.run
- true
- true
- true
-
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- easyPoint2KalmanFilter.run
- true
- true
- true
-
-
- make
- -j2
- elaboratePoint2KalmanFilter.run
- true
- true
- true
-
-
- make
- -j2
- PlanarSLAMSelfContained_advanced.run
- true
- true
- true
-
-
- make
- -j2
- Pose2SLAMExample_advanced.run
- true
- true
- true
-
-
- make
- -j2
- Pose2SLAMExample_easy.run
- true
- true
- true
-
-
- make
- -j2
- Pose2SLAMwSPCG_easy.run
- true
- true
- true
-
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- testPlanarSLAM.run
- true
- true
- true
-
-
- make
- -j2
- testPose2Config.run
- true
- true
- true
-
-
- make
- -j2
- testPose2Factor.run
- true
- true
- true
-
-
- make
- -j2
- testPose2Prior.run
- true
- true
- true
-
-
- make
- -j2
- testPose2SLAM.run
- true
- true
- true
-
-
- make
- -j2
- testPose3Config.run
- true
- true
- true
-
-
- make
- -j2
- testPose3SLAM.run
- true
- true
- true
-
-
- make
-
- testSimulated2DOriented.run
- true
- false
- true
-
-
- make
- -j2
- testVSLAMConfig.run
- true
- true
- true
-
-
- make
- -j2
- testVSLAMFactor.run
- true
- true
- true
-
-
- make
- -j2
- testVSLAMGraph.run
- true
- true
- true
-
-
- make
- -j2
- testPose3Factor.run
- true
- true
- true
-
-
- make
-
- testSimulated2D.run
- true
- false
- true
-
-
- make
-
- testSimulated3D.run
- true
- false
- true
-
-
- make
- -j2
- tests/testGaussianISAM2
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- all
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp
index 1bfd6c785..f00509c3b 100644
--- a/examples/CameraResectioning.cpp
+++ b/examples/CameraResectioning.cpp
@@ -29,8 +29,8 @@ using namespace gtsam;
/**
* Unary factor for the pose.
*/
-class ResectioningFactor: public NonlinearFactor1 {
- typedef NonlinearFactor1 Base;
+class ResectioningFactor: public NoiseModelFactor1 {
+ typedef NoiseModelFactor1 Base;
shared_ptrK K_; // camera's intrinsic parameters
Point3 P_; // 3D point on the calibration rig
diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h
index a1130dfd9..bcc6c8957 100644
--- a/gtsam/base/Testable.h
+++ b/gtsam/base/Testable.h
@@ -23,7 +23,7 @@
* void print(const std::string& name) const = 0;
*
* equality up to tolerance
- * tricky to implement, see NonlinearFactor1 for an example
+ * tricky to implement, see NoiseModelFactor1 for an example
* equals is not supposed to print out *anything*, just return true|false
* bool equals(const Derived& expected, double tol) const = 0;
*
diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h
index 2fa5f691d..0b5fca660 100644
--- a/gtsam/inference/Conditional.h
+++ b/gtsam/inference/Conditional.h
@@ -46,7 +46,7 @@ private:
/** Create keys by adding key in front */
template
static std::vector MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
- std::vector keys((lastParent - firstParent) + 1);
+ std::vector keys((lastParent - firstParent) + 1);
std::copy(firstParent, lastParent, keys.begin() + 1);
keys[0] = key;
return keys;
@@ -62,16 +62,16 @@ protected:
public:
- typedef KEY Key;
- typedef Conditional This;
- typedef Factor Base;
+ typedef KEY KeyType;
+ typedef Conditional This;
+ typedef Factor Base;
/**
* Typedef to the factor type that produces this conditional and that this
* conditional can be converted to using a factor constructor. Derived
* classes must redefine this.
*/
- typedef gtsam::Factor FactorType;
+ typedef gtsam::Factor FactorType;
/** A shared_ptr to this class. Derived classes must redefine this. */
typedef boost::shared_ptr shared_ptr;
@@ -95,23 +95,23 @@ public:
Conditional() : nrFrontals_(0) { assertInvariants(); }
/** No parents */
- Conditional(Key key) : FactorType(key), nrFrontals_(1) { assertInvariants(); }
+ Conditional(KeyType key) : FactorType(key), nrFrontals_(1) { assertInvariants(); }
/** Single parent */
- Conditional(Key key, Key parent) : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); }
+ Conditional(KeyType key, KeyType parent) : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); }
/** Two parents */
- Conditional(Key key, Key parent1, Key parent2) : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); }
+ Conditional(KeyType key, KeyType parent1, KeyType parent2) : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); }
/** Three parents */
- Conditional(Key key, Key parent1, Key parent2, Key parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); }
+ Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); }
/// @}
/// @name Advanced Constructors
/// @{
/** Constructor from a frontal variable and a vector of parents */
- Conditional(Key key, const std::vector& parents) :
+ Conditional(KeyType key, const std::vector& parents) :
FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(1) {
assertInvariants();
}
@@ -145,8 +145,8 @@ public:
size_t nrParents() const { return FactorType::size() - nrFrontals_; }
/** Special accessor when there is only one frontal variable. */
- Key firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); }
- Key lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); }
+ KeyType firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); }
+ KeyType lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); }
/** return a view of the frontal keys */
Frontals frontals() const {
@@ -198,9 +198,9 @@ private:
template
void Conditional::print(const std::string& s) const {
std::cout << s << " P(";
- BOOST_FOREACH(Key key, frontals()) std::cout << " " << key;
+ BOOST_FOREACH(KeyType key, frontals()) std::cout << " " << key;
if (nrParents()>0) std::cout << " |";
- BOOST_FOREACH(Key parent, parents()) std::cout << " " << parent;
+ BOOST_FOREACH(KeyType parent, parents()) std::cout << " " << parent;
std::cout << ")" << std::endl;
}
diff --git a/gtsam/inference/Factor-inl.h b/gtsam/inference/Factor-inl.h
index 09336186e..d05cc1de3 100644
--- a/gtsam/inference/Factor-inl.h
+++ b/gtsam/inference/Factor-inl.h
@@ -45,8 +45,8 @@ namespace gtsam {
void Factor::assertInvariants() const {
#ifndef NDEBUG
// Check that keys are all unique
- std::multiset < Key > nonunique(keys_.begin(), keys_.end());
- std::set < Key > unique(keys_.begin(), keys_.end());
+ std::multiset < KeyType > nonunique(keys_.begin(), keys_.end());
+ std::set < KeyType > unique(keys_.begin(), keys_.end());
bool correct = (nonunique.size() == unique.size())
&& std::equal(nonunique.begin(), nonunique.end(), unique.begin());
if (!correct) throw std::logic_error(
diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h
index 5a0b1e333..962bf44d1 100644
--- a/gtsam/inference/Factor.h
+++ b/gtsam/inference/Factor.h
@@ -55,28 +55,28 @@ class Factor {
public:
- typedef KEY Key; ///< The KEY template parameter
- typedef Factor This; ///< This class
+ typedef KEY KeyType; ///< The KEY template parameter
+ typedef Factor This; ///< This class
/**
* Typedef to the conditional type obtained by eliminating this factor,
* derived classes must redefine this.
*/
- typedef Conditional ConditionalType;
+ typedef Conditional ConditionalType;
/// A shared_ptr to this class, derived classes must redefine this.
typedef boost::shared_ptr shared_ptr;
/// Iterator over keys
- typedef typename std::vector::iterator iterator;
+ typedef typename std::vector::iterator iterator;
/// Const iterator over keys
- typedef typename std::vector::const_iterator const_iterator;
+ typedef typename std::vector::const_iterator const_iterator;
protected:
/// The keys involved in this factor
- std::vector keys_;
+ std::vector keys_;
friend class JacobianFactor;
friend class HessianFactor;
@@ -102,19 +102,19 @@ public:
Factor() {}
/** Construct unary factor */
- Factor(Key key) : keys_(1) {
+ Factor(KeyType key) : keys_(1) {
keys_[0] = key; assertInvariants(); }
/** Construct binary factor */
- Factor(Key key1, Key key2) : keys_(2) {
+ Factor(KeyType key1, KeyType key2) : keys_(2) {
keys_[0] = key1; keys_[1] = key2; assertInvariants(); }
/** Construct ternary factor */
- Factor(Key key1, Key key2, Key key3) : keys_(3) {
+ Factor(KeyType key1, KeyType key2, KeyType key3) : keys_(3) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); }
/** Construct 4-way factor */
- Factor(Key key1, Key key2, Key key3, Key key4) : keys_(4) {
+ Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4) : keys_(4) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); }
/// @}
@@ -122,13 +122,13 @@ public:
/// @{
/** Construct n-way factor */
- Factor(const std::set& keys) {
- BOOST_FOREACH(const Key& key, keys) keys_.push_back(key);
+ Factor(const std::set& keys) {
+ BOOST_FOREACH(const KeyType& key, keys) keys_.push_back(key);
assertInvariants();
}
/** Construct n-way factor */
- Factor(const std::vector& keys) : keys_(keys) {
+ Factor(const std::vector& keys) : keys_(keys) {
assertInvariants();
}
@@ -157,16 +157,16 @@ public:
/// @{
/// First key
- Key front() const { return keys_.front(); }
+ KeyType front() const { return keys_.front(); }
/// Last key
- Key back() const { return keys_.back(); }
+ KeyType back() const { return keys_.back(); }
/// find
- const_iterator find(Key key) const { return std::find(begin(), end(), key); }
+ const_iterator find(KeyType key) const { return std::find(begin(), end(), key); }
///TODO: comment
- const std::vector& keys() const { return keys_; }
+ const std::vector& keys() const { return keys_; }
/** iterators */
const_iterator begin() const { return keys_.begin(); } ///TODO: comment
@@ -194,7 +194,7 @@ public:
/**
* @return keys involved in this factor
*/
- std::vector& keys() { return keys_; }
+ std::vector& keys() { return keys_; }
/** mutable iterators */
iterator begin() { return keys_.begin(); } ///TODO: comment
diff --git a/gtsam/inference/IndexConditional.cpp b/gtsam/inference/IndexConditional.cpp
index eca7a220d..58d5f93d1 100644
--- a/gtsam/inference/IndexConditional.cpp
+++ b/gtsam/inference/IndexConditional.cpp
@@ -43,11 +43,11 @@ namespace gtsam {
/* ************************************************************************* */
bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
#ifndef NDEBUG
- BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); }
+ BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); }
#endif
bool parentChanged = false;
- BOOST_FOREACH(Key& parent, parents()) {
- Key newParent = inversePermutation[parent];
+ BOOST_FOREACH(KeyType& parent, parents()) {
+ KeyType newParent = inversePermutation[parent];
if(parent != newParent) {
parentChanged = true;
parent = newParent;
@@ -61,8 +61,8 @@ namespace gtsam {
void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) {
// The permutation may not move the separators into the frontals
#ifndef NDEBUG
- BOOST_FOREACH(const Key frontal, this->frontals()) {
- BOOST_FOREACH(const Key separator, this->parents()) {
+ BOOST_FOREACH(const KeyType frontal, this->frontals()) {
+ BOOST_FOREACH(const KeyType separator, this->parents()) {
assert(inversePermutation[frontal] < inversePermutation[separator]);
}
}
diff --git a/gtsam/inference/tests/testJunctionTree.cpp b/gtsam/inference/tests/testJunctionTree.cpp
index 7374bbad8..16309abda 100644
--- a/gtsam/inference/tests/testJunctionTree.cpp
+++ b/gtsam/inference/tests/testJunctionTree.cpp
@@ -24,10 +24,6 @@ using namespace boost::assign;
#include
#include
-// Magically casts strings like "x3" to a Symbol('x',3) key, see Symbol.h
-#define GTSAM_MAGIC_KEY
-
-#include
#include
#include
#include
diff --git a/gtsam/inference/tests/testSerializationInference.cpp b/gtsam/inference/tests/testSerializationInference.cpp
index 2db7014f1..d4c8ea62a 100644
--- a/gtsam/inference/tests/testSerializationInference.cpp
+++ b/gtsam/inference/tests/testSerializationInference.cpp
@@ -16,12 +16,9 @@
* @date Feb 7, 2012
*/
-#define GTSAM_MAGIC_KEY
-
#include
#include
#include
-#include
#include
#include
@@ -32,13 +29,12 @@ 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"]);
+ sfg.push_factor(0);
+ sfg.push_factor(0,1);
+ sfg.push_factor(0,2);
+ sfg.push_factor(2,1);
EXPECT(equalsObj(sfg));
EXPECT(equalsXML(sfg));
@@ -46,11 +42,9 @@ TEST (Serialization, symbolic_graph) {
/* ************************************************************************* */
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"]));
+ IndexConditional::shared_ptr x2(new IndexConditional(1, 2, 0));
+ IndexConditional::shared_ptr l1(new IndexConditional(2, 0));
+ IndexConditional::shared_ptr x1(new IndexConditional(0));
SymbolicBayesNet sbn;
sbn.push_back(x2);
diff --git a/gtsam/inference/tests/timeSymbolMaps.cpp b/gtsam/inference/tests/timeSymbolMaps.cpp
index 61ce1b30c..1912f34d3 100644
--- a/gtsam/inference/tests/timeSymbolMaps.cpp
+++ b/gtsam/inference/tests/timeSymbolMaps.cpp
@@ -37,11 +37,11 @@ private:
Map values_;
public:
- typedef pair value_type;
+ typedef pair value_type;
SymbolMapExp() {}
- T& at(const Symbol& key) {
+ T& at(Key key) {
typename Map::iterator it = values_.find(key.chr());
if(it != values_.end())
return it->second.at(key.index());
@@ -49,7 +49,7 @@ public:
throw invalid_argument("Key " + (string)key + " not present");
}
- void set(const Symbol& key, const T& value) {
+ void set(Key key, const T& value) {
Vec& vec(values_[key.chr()]);
//vec.reserve(10000);
if(key.index() >= vec.size()) {
@@ -62,13 +62,13 @@ public:
};
template
-class SymbolMapBinary : public std::map {
+class SymbolMapBinary : public std::map {
private:
- typedef std::map Base;
+ typedef std::map Base;
public:
- SymbolMapBinary() : std::map() {}
+ SymbolMapBinary() : std::map() {}
- T& at(const Symbol& key) {
+ T& at(Key key) {
typename Base::iterator it = Base::find(key);
if (it == Base::end())
throw(std::invalid_argument("SymbolMap::[] invalid key: " + (std::string)key));
@@ -76,8 +76,8 @@ public:
}
};
-struct SymbolHash : public std::unary_function {
- std::size_t operator()(Symbol const& x) const {
+struct SymbolHash : public std::unary_function {
+ std::size_t operator()(Key const& x) const {
std::size_t seed = 0;
boost::hash_combine(seed, x.chr());
boost::hash_combine(seed, x.index());
@@ -86,9 +86,9 @@ struct SymbolHash : public std::unary_function {
};
template
-class SymbolMapHash : public boost::unordered_map {
+class SymbolMapHash : public boost::unordered_map {
public:
- SymbolMapHash() : boost::unordered_map(60000) {}
+ SymbolMapHash() : boost::unordered_map(60000) {}
};
struct Value {
@@ -107,7 +107,7 @@ int main(int argc, char *argv[]) {
// pre-allocate
cout << "Generating test data ..." << endl;
- vector > values;
+ vector > values;
for(size_t i=0; i::initialize(const GRAPH& G, const Values&
// make the ordering
list keys = predecessorMap2Keys(tree_);
- ordering_ = boost::make_shared(list(keys.begin(), keys.end()));
+ ordering_ = boost::make_shared(list(keys.begin(), keys.end()));
// build factor pairs
pairs_.clear();
typedef pair EG ;
BOOST_FOREACH( const EG &eg, tree_ ) {
- Symbol key1 = Symbol(eg.first),
- key2 = Symbol(eg.second) ;
+ Key key1 = Key(eg.first),
+ key2 = Key(eg.second) ;
pairs_.insert(pair((*ordering_)[key1], (*ordering_)[key2])) ;
}
}
diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
index 7cd67f6d2..06a24d473 100644
--- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
+++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
@@ -30,7 +30,7 @@ namespace gtsam {
template
typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_(
const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering,
- const Values& linearizationPoints, const Symbol& lastKey,
+ const Values& linearizationPoints, Key lastKey,
JacobianFactor::shared_ptr& newPrior) const {
// Extract the Index of the provided last key
@@ -91,8 +91,8 @@ namespace gtsam {
// different keys will still compute as if a common key-set was used
// Create Keys
- Symbol x0 = motionFactor.key1();
- Symbol x1 = motionFactor.key2();
+ Key x0 = motionFactor.key1();
+ Key x1 = motionFactor.key2();
// Create an elimination ordering
Ordering ordering;
@@ -131,7 +131,7 @@ namespace gtsam {
// different keys will still compute as if a common key-set was used
// Create Keys
- Symbol x0 = measurementFactor.key();
+ Key x0 = measurementFactor.key();
// Create an elimination ordering
Ordering ordering;
diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h
index bdfaa7651..13a612f84 100644
--- a/gtsam/nonlinear/ExtendedKalmanFilter.h
+++ b/gtsam/nonlinear/ExtendedKalmanFilter.h
@@ -46,8 +46,8 @@ namespace gtsam {
typedef boost::shared_ptr > shared_ptr;
typedef VALUE T;
- typedef NonlinearFactor2 MotionFactor;
- typedef NonlinearFactor1 MeasurementFactor;
+ typedef NoiseModelFactor2 MotionFactor;
+ typedef NoiseModelFactor1 MeasurementFactor;
protected:
T x_; // linearization point
@@ -55,7 +55,7 @@ namespace gtsam {
T solve_(const GaussianFactorGraph& linearFactorGraph,
const Ordering& ordering, const Values& linearizationPoints,
- const Symbol& x, JacobianFactor::shared_ptr& newPrior) const;
+ Key x, JacobianFactor::shared_ptr& newPrior) const;
public:
diff --git a/gtsam/nonlinear/GaussianISAM2-inl.h b/gtsam/nonlinear/GaussianISAM2-inl.h
index d0d37d886..de3992d28 100644
--- a/gtsam/nonlinear/GaussianISAM2-inl.h
+++ b/gtsam/nonlinear/GaussianISAM2-inl.h
@@ -121,7 +121,7 @@ namespace gtsam {
///* ************************************************************************* */
//boost::shared_ptr optimize2(const GaussianISAM2::sharedClique& root) {
// boost::shared_ptr delta(new VectorValues());
- // set changed;
+ // set changed;
// // starting from the root, call optimize on each conditional
// optimize2(root, delta);
// return delta;
diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h
index eae39bf52..c2483488c 100644
--- a/gtsam/nonlinear/ISAM2-impl-inl.h
+++ b/gtsam/nonlinear/ISAM2-impl-inl.h
@@ -44,8 +44,10 @@ struct ISAM2::Impl {
* @param delta Current linear delta to be augmented with zeros
* @param ordering Current ordering to be augmented with new variables
* @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
+ * @param keyFormatter Formatter for printing nonlinear keys during debugging
*/
- static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes);
+ static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, Ordering& ordering,
+ typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
* Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol
@@ -61,10 +63,12 @@ struct ISAM2::Impl {
* Any variables in the VectorValues delta whose vector magnitude is greater than
* or equal to relinearizeThreshold are returned.
* @param delta The linear delta to check against the threshold
+ * @param keyFormatter Formatter for printing nonlinear keys during debugging
* @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold
*/
- static FastSet CheckRelinearization(const Permuted& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
+ static FastSet CheckRelinearization(const Permuted& delta, const Ordering& ordering,
+ const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
* Recursively search this clique and its children for marked keys appearing
@@ -94,10 +98,12 @@ struct ISAM2::Impl {
* expmapped deltas will be set to an invalid value (infinity) to catch bugs
* where we might expmap something twice, or expmap it but then not
* recalculate its delta.
+ * @param keyFormatter Formatter for printing nonlinear keys during debugging
*/
static void ExpmapMasked(Values& values, const Permuted& delta,
const Ordering& ordering, const std::vector& mask,
- boost::optional&> invalidateIfDebug = boost::optional&>());
+ boost::optional&> invalidateIfDebug = boost::optional&>(),
+ const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
* Reorder and eliminate factors. These factors form a subset of the full
@@ -120,7 +126,7 @@ struct ISAM2::Impl {
/* ************************************************************************* */
template
void ISAM2::Impl::AddVariables(
- const Values& newTheta, Values& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes) {
+ const Values& newTheta, Values& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
theta.insert(newTheta);
@@ -139,7 +145,7 @@ void ISAM2::Impl::AddVariables(
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
delta.permutation()[nextVar] = nextVar;
ordering.insert(key_value.first, nextVar);
- if(debug) cout << "Adding variable " << (string)key_value.first << " with order " << nextVar << endl;
+ if(debug) cout << "Adding variable " << keyFormatter(key_value.first) << " with order " << nextVar << endl;
++ nextVar;
}
assert(delta.permutation().size() == delta.container().size());
@@ -155,7 +161,7 @@ template
FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
FastSet indices;
BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) {
- BOOST_FOREACH(const Symbol& key, factor->keys()) {
+ BOOST_FOREACH(Key key, factor->keys()) {
indices.insert(ordering[key]);
}
}
@@ -164,7 +170,8 @@ FastSet ISAM2::Impl::IndicesFromFactors(const Ordering
/* ************************************************************************* */
template
-FastSet ISAM2::Impl::CheckRelinearization(const Permuted& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
+FastSet ISAM2::Impl::CheckRelinearization(const Permuted& delta, const Ordering& ordering,
+ const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
FastSet relinKeys;
if(relinearizeThreshold.type() == typeid(double)) {
@@ -178,7 +185,7 @@ FastSet ISAM2::Impl::CheckRelinearization(const Permut
} else if(relinearizeThreshold.type() == typeid(FastMap)) {
const FastMap& thresholds = boost::get >(relinearizeThreshold);
BOOST_FOREACH(const Ordering::value_type& key_index, ordering) {
- const Vector& threshold = thresholds.find(key_index.first.chr())->second;
+ const Vector& threshold = thresholds.find(Symbol(key_index.first).chr())->second;
Index j = key_index.second;
if(threshold.rows() != delta[j].rows())
throw std::invalid_argument("Relinearization threshold vector dimensionality passed into iSAM2 parameters does not match actual variable dimensionality");
@@ -213,8 +220,8 @@ void ISAM2::Impl::FindAll(typename ISAM2Clique::
/* ************************************************************************* */
template
-void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted& delta,
- const Ordering& ordering, const vector& mask, boost::optional&> invalidateIfDebug) {
+void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted& delta, const Ordering& ordering,
+ const vector& mask, boost::optional&> invalidateIfDebug, const KeyFormatter& keyFormatter) {
// If debugging, invalidate if requested, otherwise do not invalidate.
// Invalidating means setting expmapped entries to Inf, to trigger assertions
// if we try to re-use them.
@@ -231,9 +238,9 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const Permute
const Index var = key_index->second;
if(ISDEBUG("ISAM2 update verbose")) {
if(mask[var])
- cout << "expmap " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
+ cout << "expmap " << keyFormatter(key_value->first) << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
else
- cout << " " << (string)key_value->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
+ cout << " " << keyFormatter(key_value->first) << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
}
assert(delta[var].size() == (int)key_value->second.dim());
assert(delta[var].unaryExpr(&isfinite).all());
diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h
index 5c87d1dda..a054eb179 100644
--- a/gtsam/nonlinear/ISAM2-inl.h
+++ b/gtsam/nonlinear/ISAM2-inl.h
@@ -105,7 +105,7 @@ ISAM2::relinearizeAffectedFactors(const FastList& aff
tic(3,"check candidates");
BOOST_FOREACH(size_t idx, candidates) {
bool inside = true;
- BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) {
+ BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) {
Index var = ordering_[key];
if (affectedKeysSet.find(var) == affectedKeysSet.end()) {
inside = false;
@@ -398,7 +398,7 @@ boost::shared_ptr > ISAM2::recalculate(
template
ISAM2Result ISAM2::update(
const GRAPH& newFactors, const Values& newTheta, const FastVector& removeFactorIndices,
- const boost::optional >& constrainedKeys, bool force_relinearize) {
+ const boost::optional >& constrainedKeys, bool force_relinearize) {
static const bool debug = ISDEBUG("ISAM2 update");
static const bool verbose = ISDEBUG("ISAM2 update verbose");
@@ -519,7 +519,7 @@ ISAM2Result ISAM2::update(
boost::optional > constrainedIndices;
if(constrainedKeys) {
constrainedIndices.reset(FastSet());
- BOOST_FOREACH(const Symbol& key, *constrainedKeys) {
+ BOOST_FOREACH(Key key, *constrainedKeys) {
constrainedIndices->insert(ordering_[key]);
}
}
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 00d07a040..4c8df2997 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -106,16 +106,19 @@ struct ISAM2Params {
bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from update()
+ KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
+
/** Specify parameters as constructor arguments */
ISAM2Params(
- OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params public variables, ISAM2Params::optimizationParams
- RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params public variables, ISAM2Params::relinearizeThreshold
- int _relinearizeSkip = 10, ///< see ISAM2Params public variables, ISAM2Params::relinearizeSkip
- bool _enableRelinearization = true, ///< see ISAM2Params public variables, ISAM2Params::enableRelinearization
- bool _evaluateNonlinearError = false ///< see ISAM2Params public variables, ISAM2Params::evaluateNonlinearError
+ OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams
+ RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params::relinearizeThreshold
+ int _relinearizeSkip = 10, ///< see ISAM2Params::relinearizeSkip
+ bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization
+ bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError
+ const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter
) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold),
relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization),
- evaluateNonlinearError(_evaluateNonlinearError) {}
+ evaluateNonlinearError(_evaluateNonlinearError), keyFormatter(_keyFormatter) {}
};
/**
@@ -369,7 +372,7 @@ public:
*/
ISAM2Result update(const GRAPH& newFactors = GRAPH(), const Values& newTheta = Values(),
const FastVector& removeFactorIndices = FastVector(),
- const boost::optional >& constrainedKeys = boost::none,
+ const boost::optional >& constrainedKeys = boost::none,
bool force_relinearize = false);
/** Access the current linearization point */
diff --git a/gtsam/nonlinear/Key.cpp b/gtsam/nonlinear/Key.cpp
new file mode 100644
index 000000000..821285558
--- /dev/null
+++ b/gtsam/nonlinear/Key.cpp
@@ -0,0 +1,34 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+ * @brief
+ * @author Richard Roberts
+ * @date Feb 20, 2012
+ */
+
+#include
+
+#include
+#include
+
+namespace gtsam {
+
+std::string _defaultKeyFormatter(Key key) {
+ const Symbol asSymbol(key);
+ if(asSymbol.chr() > 0)
+ return (std::string)asSymbol;
+ else
+ return boost::lexical_cast(key);
+ }
+
+}
diff --git a/gtsam/nonlinear/Key.h b/gtsam/nonlinear/Key.h
new file mode 100644
index 000000000..65a65c6d4
--- /dev/null
+++ b/gtsam/nonlinear/Key.h
@@ -0,0 +1,40 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+ * @brief
+ * @author Richard Roberts
+ * @date Feb 20, 2012
+ */
+#pragma once
+
+#include
+#include
+
+namespace gtsam {
+
+ /// Integer nonlinear key type
+ typedef size_t Key;
+
+ /// Typedef for a function to format a key, i.e. to convert it to a string
+ typedef boost::function KeyFormatter;
+
+ // Helper function for DefaultKeyFormatter
+ std::string _defaultKeyFormatter(Key key);
+
+ /// The default KeyFormatter, which is used if no KeyFormatter is passed to
+ /// a nonlinear 'print' function. Automatically detects plain integer keys
+ /// and Symbol keys.
+ static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
+
+}
+
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index 1a1fce839..a1c685668 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -48,7 +48,7 @@ namespace gtsam {
* \nosubgrouping
*/
template
- class NonlinearEquality: public NonlinearFactor1 {
+ class NonlinearEquality: public NoiseModelFactor1 {
public:
typedef VALUE T;
@@ -68,7 +68,7 @@ namespace gtsam {
typedef NonlinearEquality This;
// typedef to base class
- typedef NonlinearFactor1 Base;
+ typedef NoiseModelFactor1 Base;
public:
@@ -89,7 +89,7 @@ namespace gtsam {
/**
* Constructor - forces exact evaluation
*/
- NonlinearEquality(const Symbol& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare) :
+ NonlinearEquality(Key j, const T& feasible, bool (*_compare)(const T&, const T&) = compare) :
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(false), error_gain_(0.0),
compare_(_compare) {
@@ -98,7 +98,7 @@ namespace gtsam {
/**
* Constructor - allows inexact evaluation
*/
- NonlinearEquality(const Symbol& j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare) :
+ NonlinearEquality(Key j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare) :
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(true), error_gain_(error_gain),
compare_(_compare) {
@@ -108,8 +108,8 @@ namespace gtsam {
/// @name Testable
/// @{
- virtual 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 KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+ std::cout << "Constraint: " << s << " on [" << keyFormatter(this->key()) << "]\n";
gtsam::print(feasible_,"Feasible Point");
std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
}
@@ -147,7 +147,7 @@ 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 " + DefaultKeyFormatter(this->key()) + "!");
return repeat(nj, std::numeric_limits::infinity()); // set error to infinity if not equal
}
}
@@ -169,7 +169,7 @@ namespace gtsam {
friend class boost::serialization::access;
template
void serialize(ARCHIVE & ar, const unsigned int version) {
- ar & boost::serialization::make_nvp("NonlinearFactor1",
+ ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object(*this));
ar & BOOST_SERIALIZATION_NVP(feasible_);
ar & BOOST_SERIALIZATION_NVP(allow_error_);
@@ -183,13 +183,13 @@ namespace gtsam {
* Simple unary equality constraint - fixes a value for a variable
*/
template
- class NonlinearEquality1 : public NonlinearFactor1 {
+ class NonlinearEquality1 : public NoiseModelFactor1 {
public:
typedef VALUE X;
protected:
- typedef NonlinearFactor1 Base;
+ typedef NoiseModelFactor1 Base;
/** default constructor to allow for serialization */
NonlinearEquality1() {}
@@ -204,7 +204,7 @@ namespace gtsam {
typedef boost::shared_ptr > shared_ptr;
///TODO: comment
- NonlinearEquality1(const X& value, const Symbol& key1, double mu = 1000.0)
+ NonlinearEquality1(const X& value, Key key1, double mu = 1000.0)
: Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {}
virtual ~NonlinearEquality1() {}
@@ -217,9 +217,9 @@ namespace gtsam {
}
/** Print */
- virtual void print(const std::string& s = "") const {
+ virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << ": NonlinearEquality1("
- << (std::string) this->key() << "),"<< "\n";
+ << keyFormatter(this->key()) << "),"<< "\n";
this->noiseModel_->print();
value_.print("Value");
}
@@ -230,7 +230,7 @@ namespace gtsam {
friend class boost::serialization::access;
template
void serialize(ARCHIVE & ar, const unsigned int version) {
- ar & boost::serialization::make_nvp("NonlinearFactor1",
+ ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object(*this));
ar & BOOST_SERIALIZATION_NVP(value_);
}
@@ -242,12 +242,12 @@ namespace gtsam {
* be the same.
*/
template
- class NonlinearEquality2 : public NonlinearFactor2 {
+ class NonlinearEquality2 : public NoiseModelFactor2 {
public:
typedef VALUE X;
protected:
- typedef NonlinearFactor2 Base;
+ typedef NoiseModelFactor2 Base;
GTSAM_CONCEPT_MANIFOLD_TYPE(X);
@@ -259,7 +259,7 @@ namespace gtsam {
typedef boost::shared_ptr > shared_ptr;
///TODO: comment
- NonlinearEquality2(const Symbol& key1, const Symbol& key2, double mu = 1000.0)
+ NonlinearEquality2(Key key1, Key key2, double mu = 1000.0)
: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
virtual ~NonlinearEquality2() {}
@@ -279,7 +279,7 @@ namespace gtsam {
friend class boost::serialization::access;
template
void serialize(ARCHIVE & ar, const unsigned int version) {
- ar & boost::serialization::make_nvp("NonlinearFactor2",
+ ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object(*this));
}
}; // \NonlinearEquality2
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 5ebe662e5..e260f24db 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -24,6 +24,7 @@
#include
#include
+#include
#include
#include
@@ -32,6 +33,7 @@
#include
#include
+#include
namespace gtsam {
@@ -44,12 +46,12 @@ namespace gtsam {
* which are objects in non-linear manifolds (Lie groups).
* \nosubgrouping
*/
-class NonlinearFactor: public Factor {
+class NonlinearFactor: public Factor {
protected:
// Some handy typedefs
- typedef Factor Base;
+ typedef Factor Base;
typedef NonlinearFactor This;
public:
@@ -77,8 +79,10 @@ public:
/// @{
/** print */
- virtual void print(const std::string& s = "") const {
- std::cout << s << ": NonlinearFactor\n";
+ virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+ std::cout << s << "keys = { ";
+ BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; }
+ std::cout << "}" << endl;
}
/** Check if two factors are equal */
@@ -184,11 +188,8 @@ protected:
public:
/** Print */
- virtual void print(const std::string& s = "") const {
- std::cout << s << ": NoiseModelFactor\n";
- std::cout << " ";
- BOOST_FOREACH(const Symbol& key, this->keys()) { std::cout << (std::string)key << " "; }
- std::cout << "\n";
+ virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+ Base::print(s, keyFormatter);
this->noiseModel_->print(" noise model: ");
}
@@ -291,7 +292,7 @@ private:
/** A convenient base class for creating your own NoiseModelFactor with 1
* variable. To derive from this class, implement evaluateError(). */
template
-class NonlinearFactor1: public NoiseModelFactor {
+class NoiseModelFactor1: public NoiseModelFactor {
public:
@@ -301,23 +302,23 @@ public:
protected:
typedef NoiseModelFactor Base;
- typedef NonlinearFactor1 This;
+ typedef NoiseModelFactor1 This;
public:
/** Default constructor for I/O only */
- NonlinearFactor1() {}
+ NoiseModelFactor1() {}
- virtual ~NonlinearFactor1() {}
+ virtual ~NoiseModelFactor1() {}
- inline const Symbol& key() const { return keys_[0]; }
+ inline Key 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 Symbol& key1) :
+ NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) :
Base(noiseModel) {
keys_.resize(1);
keys_[0] = key1;
@@ -338,12 +339,6 @@ public:
}
}
- /** Print */
- virtual void print(const std::string& s = "") const {
- std::cout << s << ": NonlinearFactor1(" << (std::string) keys_[0] << ")\n";
- this->noiseModel_->print(" noise model: ");
- }
-
/**
* Override this method to finish implementing a unary factor.
* If the optional Matrix reference argument is specified, it should compute
@@ -361,14 +356,14 @@ private:
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object(*this));
}
-};// \class NonlinearFactor1
+};// \class NoiseModelFactor1
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 2
* variables. To derive from this class, implement evaluateError(). */
template
-class NonlinearFactor2: public NoiseModelFactor {
+class NoiseModelFactor2: public NoiseModelFactor {
public:
@@ -379,32 +374,32 @@ public:
protected:
typedef NoiseModelFactor Base;
- typedef NonlinearFactor2 This;
+ typedef NoiseModelFactor2 This;
public:
/**
* Default Constructor for I/O
*/
- NonlinearFactor2() {}
+ NoiseModelFactor2() {}
/**
* Constructor
* @param j1 key of the first variable
* @param j2 key of the second variable
*/
- NonlinearFactor2(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2) :
+ NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) :
Base(noiseModel) {
keys_.resize(2);
keys_[0] = j1;
keys_[1] = j2;
}
- virtual ~NonlinearFactor2() {}
+ virtual ~NoiseModelFactor2() {}
/** methods to retrieve both keys */
- inline const Symbol& key1() const { return keys_[0]; }
- inline const Symbol& key2() const { return keys_[1]; }
+ inline Key key1() const { return keys_[0]; }
+ inline Key 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. */
@@ -422,14 +417,6 @@ public:
}
}
- /** Print */
- virtual void print(const std::string& s = "") const {
- std::cout << s << ": NonlinearFactor2("
- << (std::string) keys_[0] << ","
- << (std::string) keys_[1] << ")\n";
- this->noiseModel_->print(" noise model: ");
- }
-
/**
* Override this method to finish implementing a binary factor.
* If any of the optional Matrix reference arguments are specified, it should compute
@@ -448,13 +435,13 @@ private:
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object(*this));
}
-}; // \class NonlinearFactor2
+}; // \class NoiseModelFactor2
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 3
* variables. To derive from this class, implement evaluateError(). */
template
-class NonlinearFactor3: public NoiseModelFactor {
+class NoiseModelFactor3: public NoiseModelFactor {
public:
@@ -466,14 +453,14 @@ public:
protected:
typedef NoiseModelFactor Base;
- typedef NonlinearFactor3 This;
+ typedef NoiseModelFactor3 This;
public:
/**
* Default Constructor for I/O
*/
- NonlinearFactor3() {}
+ NoiseModelFactor3() {}
/**
* Constructor
@@ -481,7 +468,7 @@ public:
* @param j2 key of the second variable
* @param j3 key of the third variable
*/
- NonlinearFactor3(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3) :
+ NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) :
Base(noiseModel) {
keys_.resize(3);
keys_[0] = j1;
@@ -489,12 +476,12 @@ public:
keys_[2] = j3;
}
- virtual ~NonlinearFactor3() {}
+ virtual ~NoiseModelFactor3() {}
/** methods to retrieve keys */
- 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 Key key1() const { return keys_[0]; }
+ inline Key key2() const { return keys_[1]; }
+ inline Key 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. */
@@ -509,16 +496,6 @@ public:
}
}
- /** Print */
- virtual void print(const std::string& s = "") const {
- std::cout << s << ": NonlinearFactor3("
- << (std::string) this->keys_[0] << ","
- << (std::string) this->keys_[1] << ","
- << (std::string) this->keys_[2] << ")\n";
- this->noiseModel_->print(" noise model: ");
- }
-
-
/**
* Override this method to finish implementing a trinary factor.
* If any of the optional Matrix reference arguments are specified, it should compute
@@ -539,13 +516,13 @@ private:
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object(*this));
}
-}; // \class NonlinearFactor3
+}; // \class NoiseModelFactor3
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 4
* variables. To derive from this class, implement evaluateError(). */
template
-class NonlinearFactor4: public NoiseModelFactor {
+class NoiseModelFactor4: public NoiseModelFactor {
public:
@@ -558,14 +535,14 @@ public:
protected:
typedef NoiseModelFactor Base;
- typedef NonlinearFactor4 This;
+ typedef NoiseModelFactor4 This;
public:
/**
* Default Constructor for I/O
*/
- NonlinearFactor4() {}
+ NoiseModelFactor4() {}
/**
* Constructor
@@ -574,7 +551,7 @@ public:
* @param j3 key of the third variable
* @param j4 key of the fourth variable
*/
- NonlinearFactor4(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4) :
+ NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) :
Base(noiseModel) {
keys_.resize(4);
keys_[0] = j1;
@@ -583,13 +560,13 @@ public:
keys_[3] = j4;
}
- virtual ~NonlinearFactor4() {}
+ virtual ~NoiseModelFactor4() {}
/** methods to retrieve keys */
- 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 Key key1() const { return keys_[0]; }
+ inline Key key2() const { return keys_[1]; }
+ inline Key key3() const { return keys_[2]; }
+ inline Key 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. */
@@ -604,16 +581,6 @@ public:
}
}
- /** Print */
- virtual void print(const std::string& s = "") const {
- std::cout << s << ": NonlinearFactor4("
- << (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: ");
- }
-
/**
* Override this method to finish implementing a 4-way factor.
* If any of the optional Matrix reference arguments are specified, it should compute
@@ -635,13 +602,13 @@ private:
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object(*this));
}
-}; // \class NonlinearFactor4
+}; // \class NoiseModelFactor4
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 5
* variables. To derive from this class, implement evaluateError(). */
template
-class NonlinearFactor5: public NoiseModelFactor {
+class NoiseModelFactor5: public NoiseModelFactor {
public:
@@ -655,14 +622,14 @@ public:
protected:
typedef NoiseModelFactor Base;
- typedef NonlinearFactor5 This;
+ typedef NoiseModelFactor5 This;
public:
/**
* Default Constructor for I/O
*/
- NonlinearFactor5() {}
+ NoiseModelFactor5() {}
/**
* Constructor
@@ -672,7 +639,7 @@ public:
* @param j4 key of the fourth variable
* @param j5 key of the fifth variable
*/
- NonlinearFactor5(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4, const Symbol& j5) :
+ NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) :
Base(noiseModel) {
keys_.resize(5);
keys_[0] = j1;
@@ -682,14 +649,14 @@ public:
keys_[4] = j5;
}
- virtual ~NonlinearFactor5() {}
+ virtual ~NoiseModelFactor5() {}
/** methods to retrieve keys */
- 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 Key key1() const { return keys_[0]; }
+ inline Key key2() const { return keys_[1]; }
+ inline Key key3() const { return keys_[2]; }
+ inline Key key4() const { return keys_[3]; }
+ inline Key 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. */
@@ -704,17 +671,6 @@ public:
}
}
- /** Print */
- virtual void print(const std::string& s = "") const {
- std::cout << s << ": NonlinearFactor5("
- << (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: ");
- }
-
/**
* Override this method to finish implementing a 5-way factor.
* If any of the optional Matrix reference arguments are specified, it should compute
@@ -737,13 +693,13 @@ private:
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object(*this));
}
-}; // \class NonlinearFactor5
+}; // \class NoiseModelFactor5
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 6
* variables. To derive from this class, implement evaluateError(). */
template
-class NonlinearFactor6: public NoiseModelFactor {
+class NoiseModelFactor6: public NoiseModelFactor {
public:
@@ -758,14 +714,14 @@ public:
protected:
typedef NoiseModelFactor Base;
- typedef NonlinearFactor6 This;
+ typedef NoiseModelFactor6 This;
public:
/**
* Default Constructor for I/O
*/
- NonlinearFactor6() {}
+ NoiseModelFactor6() {}
/**
* Constructor
@@ -776,7 +732,7 @@ public:
* @param j5 key of the fifth variable
* @param j6 key of the fifth variable
*/
- NonlinearFactor6(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4, const Symbol& j5, const Symbol& j6) :
+ NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) :
Base(noiseModel) {
keys_.resize(6);
keys_[0] = j1;
@@ -787,15 +743,15 @@ public:
keys_[5] = j6;
}
- virtual ~NonlinearFactor6() {}
+ virtual ~NoiseModelFactor6() {}
/** methods to retrieve keys */
- 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]; }
+ inline Key key1() const { return keys_[0]; }
+ inline Key key2() const { return keys_[1]; }
+ inline Key key3() const { return keys_[2]; }
+ inline Key key4() const { return keys_[3]; }
+ inline Key key5() const { return keys_[4]; }
+ inline Key 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. */
@@ -810,18 +766,6 @@ public:
}
}
- /** Print */
- virtual void print(const std::string& s = "") const {
- std::cout << s << ": NonlinearFactor6("
- << (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: ");
- }
-
/**
* Override this method to finish implementing a 6-way factor.
* If any of the optional Matrix reference arguments are specified, it should compute
@@ -845,7 +789,7 @@ private:
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object(*this));
}
-}; // \class NonlinearFactor6
+}; // \class NoiseModelFactor6
/* ************************************************************************* */
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
index 11f8e84d9..647afa2d5 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@@ -33,8 +33,13 @@ namespace gtsam {
}
/* ************************************************************************* */
- void NonlinearFactorGraph::print(const std::string& str) const {
- Base::print(str);
+ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const {
+ cout << str << "size: " << size() << endl;
+ for (size_t i = 0; i < factors_.size(); i++) {
+ stringstream ss;
+ ss << "factor " << i << ": ";
+ if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter);
+ }
}
/* ************************************************************************* */
@@ -49,8 +54,8 @@ namespace gtsam {
}
/* ************************************************************************* */
- std::set NonlinearFactorGraph::keys() const {
- std::set keys;
+ std::set NonlinearFactorGraph::keys() const {
+ std::set keys;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor)
keys.insert(factor->begin(), factor->end());
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index fa87baedf..3caeec680 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -44,10 +44,10 @@ namespace gtsam {
typedef boost::shared_ptr sharedFactor;
/** print just calls base class */
- void print(const std::string& str = "NonlinearFactorGraph: ") const;
+ void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** return keys in some random order */
- std::set keys() const;
+ std::set keys() const;
/** unnormalized error */
double error(const Values& c) const;
diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h
index 03d2832ce..e054ecacb 100644
--- a/gtsam/nonlinear/NonlinearISAM-inl.h
+++ b/gtsam/nonlinear/NonlinearISAM-inl.h
@@ -50,7 +50,7 @@ void NonlinearISAM::update(const Factors& newFactors,
// Augment ordering
// FIXME: should just loop over new values
BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors)
- BOOST_FOREACH(const Symbol& key, factor->keys())
+ BOOST_FOREACH(Key key, factor->keys())
ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present
boost::shared_ptr linearizedNewFactors(
@@ -99,7 +99,7 @@ Values NonlinearISAM::estimate() const {
/* ************************************************************************* */
template
-Matrix NonlinearISAM::marginalCovariance(const Symbol& key) const {
+Matrix NonlinearISAM::marginalCovariance(Key key) const {
return isam_.marginalCovariance(ordering_[key]);
}
diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h
index f74cff5ea..16547770b 100644
--- a/gtsam/nonlinear/NonlinearISAM.h
+++ b/gtsam/nonlinear/NonlinearISAM.h
@@ -70,7 +70,7 @@ public:
Values estimate() const;
/** find the marginal covariance for a single variable */
- Matrix marginalCovariance(const Symbol& key) const;
+ Matrix marginalCovariance(Key key) const;
// access
@@ -104,7 +104,7 @@ public:
void reorder_relinearize();
/** manually add a key to the end of the ordering */
- void addKey(const Symbol& key) { ordering_.push_back(key); }
+ void addKey(Key key) { ordering_.push_back(key); }
/** replace the current ordering */
void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; }
diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h
index 5a2caed64..4e41649e6 100644
--- a/gtsam/nonlinear/NonlinearOptimizer.h
+++ b/gtsam/nonlinear/NonlinearOptimizer.h
@@ -242,7 +242,7 @@ public:
/**
* Return mean and covariance on a single variable
*/
- Matrix marginalCovariance(Symbol j) const {
+ Matrix marginalCovariance(Key j) const {
return createSolver()->marginalCovariance((*ordering_)[j]);
}
diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp
index d8b5eac7e..10c3370cf 100644
--- a/gtsam/nonlinear/Ordering.cpp
+++ b/gtsam/nonlinear/Ordering.cpp
@@ -26,9 +26,9 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
-Ordering::Ordering(const std::list & L):nVars_(0) {
+Ordering::Ordering(const std::list & L):nVars_(0) {
int i = 0;
- BOOST_FOREACH( const Symbol& s, L )
+ BOOST_FOREACH( Key s, L )
insert(s, i++) ;
}
@@ -40,12 +40,12 @@ void Ordering::permuteWithInverse(const Permutation& inversePermutation) {
}
/* ************************************************************************* */
-void Ordering::print(const string& str) const {
+void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const {
cout << str << " ";
BOOST_FOREACH(const Ordering::value_type& key_order, *this) {
if(key_order != *begin())
cout << ", ";
- cout << (string)key_order.first << ":" << key_order.second;
+ cout << keyFormatter(key_order.first) << ":" << key_order.second;
}
cout << endl;
}
@@ -70,7 +70,7 @@ Ordering::value_type Ordering::pop_back() {
}
/* ************************************************************************* */
-Index Ordering::pop_back(const Symbol& key) {
+Index Ordering::pop_back(Key key) {
Map::iterator item = order_.find(key);
if(item == order_.end()) {
throw invalid_argument("Attempting to remove a key from an ordering that does not contain that key");
diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h
index 47d048f2b..fcb3d5bfb 100644
--- a/gtsam/nonlinear/Ordering.h
+++ b/gtsam/nonlinear/Ordering.h
@@ -17,10 +17,10 @@
#pragma once
-#include