diff --git a/.cproject b/.cproject
index 91d9c7689..2439190b6 100644
--- a/.cproject
+++ b/.cproject
@@ -309,6 +309,14 @@
true
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -335,7 +343,6 @@
make
-
tests/testBayesTree.run
true
false
@@ -343,7 +350,6 @@
make
-
testBinaryBayesNet.run
true
false
@@ -391,7 +397,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -399,7 +404,6 @@
make
-
tests/testSymbolicFactor.run
true
false
@@ -407,7 +411,6 @@
make
-
testSymbolicFactorGraph.run
true
false
@@ -423,20 +426,11 @@
make
-
tests/testBayesTree
true
false
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j5
@@ -525,22 +519,6 @@
false
true
-
- make
- -j2
- tests/testPose2.run
- true
- true
- true
-
-
- make
- -j2
- tests/testPose3.run
- true
- true
- true
-
make
-j2
@@ -557,6 +535,22 @@
true
true
+
+ make
+ -j2
+ tests/testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testPose3.run
+ true
+ true
+ true
+
make
-j2
@@ -581,26 +575,26 @@
true
true
-
+
make
- -j2
- all
+ -j5
+ testValues.run
true
true
true
-
+
make
- -j2
- check
+ -j5
+ testOrdering.run
true
true
true
-
+
make
- -j2
- clean
+ -j5
+ testKey.run
true
true
true
@@ -685,26 +679,26 @@
true
true
-
+
make
- -j5
- testValues.run
+ -j2
+ all
true
true
true
-
+
make
- -j5
- testOrdering.run
+ -j2
+ check
true
true
true
-
+
make
- -j5
- testKey.run
+ -j2
+ clean
true
true
true
@@ -959,7 +953,6 @@
make
-
testGraph.run
true
false
@@ -967,7 +960,6 @@
make
-
testJunctionTree.run
true
false
@@ -975,7 +967,6 @@
make
-
testSymbolicBayesNetB.run
true
false
@@ -1111,7 +1102,6 @@
make
-
testErrors.run
true
false
@@ -1575,6 +1565,7 @@
make
+
testSimulated2DOriented.run
true
false
@@ -1614,6 +1605,7 @@
make
+
testSimulated2D.run
true
false
@@ -1621,6 +1613,7 @@
make
+
testSimulated3D.run
true
false
@@ -1836,6 +1829,7 @@
make
+
tests/testGaussianISAM2
true
false
@@ -1857,14 +1851,110 @@
true
true
-
+
make
-j2
- install
+ testRot3.run
true
true
true
+
+ make
+ -j2
+ testRot2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ timeRot3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testCal3_S2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testSimpleCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testHomography2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testCalibratedCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPoint2.run
+ true
+ true
+ true
+
+
+ make
+ -j1
+ install
+ true
+ false
+ true
+
make
-j2
@@ -1875,10 +1965,10 @@
make
- -j5
+ -j1
check
true
- true
+ false
true
@@ -2058,7 +2148,6 @@
cpack
-
-G DEB
true
false
@@ -2066,7 +2155,6 @@
cpack
-
-G RPM
true
false
@@ -2074,7 +2162,6 @@
cpack
-
-G TGZ
true
false
@@ -2082,7 +2169,6 @@
cpack
-
--config CPackSourceConfig.cmake
true
false
@@ -2224,98 +2310,34 @@
true
true
-
+
make
- -j2
- testRot3.run
+ -j5
+ testSpirit.run
true
true
true
-
+
make
- -j2
- testRot2.run
+ -j5
+ testWrap.run
true
true
true
-
+
make
- -j2
- testPose3.run
+ -j5
+ check.wrap
true
true
true
-
+
make
- -j2
- timeRot3.run
- true
- true
- true
-
-
- make
- -j2
- testPose2.run
- true
- true
- true
-
-
- make
- -j2
- testCal3_S2.run
- true
- true
- true
-
-
- make
- -j2
- testSimpleCamera.run
- true
- true
- true
-
-
- make
- -j2
- testHomography2.run
- true
- true
- true
-
-
- make
- -j2
- testCalibratedCamera.run
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- testPoint2.run
+ -j5
+ wrap
true
true
true
@@ -2359,38 +2381,6 @@
false
true
-
- make
- -j5
- testSpirit.run
- true
- true
- true
-
-
- make
- -j5
- testWrap.run
- true
- true
- true
-
-
- make
- -j5
- check.wrap
- true
- true
- true
-
-
- make
- -j5
- wrap
- true
- true
- true
-
diff --git a/examples/Pose2SLAMExample_graph.m b/examples/Pose2SLAMExample_graph.m
new file mode 100644
index 000000000..65271c053
--- /dev/null
+++ b/examples/Pose2SLAMExample_graph.m
@@ -0,0 +1,47 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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 Read graph from file and perform GraphSLAM
+% @author Frank Dellaert
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% Initialize graph, initial estimate, and odometry noise
+import gtsam.*
+model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]);
+maxID = 0;
+addNoise = false;
+smart = true;
+[graph,initial]=load2D('Data/w100-odom.graph',model,maxID,addNoise,smart);
+initial.print(sprintf('Initial estimate:\n'));
+
+%% Add a Gaussian prior on pose x_1
+import gtsam.*
+priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin
+priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]);
+graph.addPosePrior(0, priorMean, priorNoise); % add directly to graph
+
+%% Plot Initial Estimate
+figure(1);clf
+P=initial.poses;
+plot(P(:,1),P(:,2),'g-*'); axis equal
+
+%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
+result = graph.optimize(initial,1);
+P=result.poses;
+hold on; plot(P(:,1),P(:,2),'b-*')
+result.print(sprintf('\nFinal result:\n'));
+
+%% Plot Covariance Ellipses
+marginals = graph.marginals(result);
+P={};
+for i=1:result.size()-1
+ pose_i = result.pose(i);
+ P{i}=marginals.marginalCovariance(i);
+ plotPose2(pose_i,'b',P{i})
+end
+fprintf(1,'%.5f %.5f %.5f\n',P{99})
\ No newline at end of file