diff --git a/.cproject b/.cproject
index 2d1a0a81d..24b5f8921 100644
--- a/.cproject
+++ b/.cproject
@@ -453,14 +453,6 @@
true
true
-
- make
- -j5
- testVelocityConstraint3.run
- true
- true
- true
-
make
-j5
@@ -621,6 +613,54 @@
true
true
+
+ make
+ -j5
+ testPlanarSLAM.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testPose2SLAM.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testPose3SLAM.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testSimulated2D.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testSimulated2DOriented.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testVisualSLAM.run
+ true
+ true
+ true
+
make
-j5
@@ -629,18 +669,18 @@
true
true
-
+
make
-j5
- testGeneralSFMFactor_Cal3Bundler.run
+ testSerializationSLAM.run
true
true
true
-
+
make
-j5
- testSerialization.run
+ testGeneralSFMFactor_Cal3Bundler.run
true
true
true
@@ -677,14 +717,6 @@
true
true
-
- make
- -j5
- testWhiteNoiseFactor.run
- true
- true
- true
-
make
-j5
@@ -805,6 +837,14 @@
true
true
+
+ make
+ -j8
+ testImuFactor.run
+ true
+ true
+ true
+
make
-j5
@@ -885,14 +925,6 @@
true
true
-
- make
- -j5
- testBayesTreeOperations.run
- true
- true
- true
-
make
-j5
@@ -1125,14 +1157,6 @@
true
true
-
- make
- -j5
- testSerializationSLAM.run
- true
- true
- true
-
make
-j5
@@ -1597,14 +1621,6 @@
true
true
-
- make
- -j5
- testRot3Q.run
- true
- true
- true
-
make
-j2
@@ -1930,6 +1946,14 @@
true
true
+
+ make
+ -j4
+ testImuFactor.run
+ true
+ true
+ true
+
make
-j2
diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp
index 1641b6922..c2688a0c2 100644
--- a/examples/RangeISAMExample_plaza2.cpp
+++ b/examples/RangeISAMExample_plaza2.cpp
@@ -48,6 +48,7 @@
using namespace std;
using namespace gtsam;
+namespace NM = gtsam::noiseModel;
// data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
// Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
@@ -91,7 +92,7 @@ vector readTriples() {
}
// main
-int main(int argc, char** argv) {
+int main (int argc, char** argv) {
// load Plaza2 data
list odometry = readOdometry();
@@ -102,14 +103,20 @@ int main(int argc, char** argv) {
// parameters
size_t minK = 150; // minimum number of range measurements to process initially
- size_t incK = 5; // minimum number of range measurements to process after
- double sigmaR = 100; // range standard deviation
- bool batchInitialization = true;
+ size_t incK = 25; // minimum number of range measurements to process after
+ bool groundTruth = false;
+ bool robust = true;
// Set Noise parameters
- const noiseModel::Robust::shared_ptr rangeNoiseModel =
- noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(15),
- noiseModel::Isotropic::Sigma(1, sigmaR));
+ Vector priorSigmas = Vector_(3, 1.0, 1.0, M_PI);
+ Vector odoSigmas = Vector_(3, 0.05, 0.01, 0.2);
+ double sigmaR = 100; // range standard deviation
+ const NM::Base::shared_ptr // all same type
+ priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
+ odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
+ gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
+ tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
+ rangeNoise = robust ? tukey : gaussian;
// Initialize iSAM
ISAM2 isam;
@@ -118,34 +125,40 @@ int main(int argc, char** argv) {
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
M_PI - 2.02108900000000);
NonlinearFactorGraph newFactors;
- newFactors.add(
- PriorFactor(0, pose0,
- noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 1.0, M_PI))));
+ newFactors.add(PriorFactor(0, pose0, priorNoise));
Values initial;
initial.insert(0, pose0);
// initialize points drawn from sigma=1 Gaussian in matlab version
- initial.insert(symbol('L', 1), Point2(3.5784, 2.76944));
- initial.insert(symbol('L', 6), Point2(-1.34989, 3.03492));
- initial.insert(symbol('L', 0), Point2(0.725404, -0.0630549));
- initial.insert(symbol('L', 5), Point2(0.714743, -0.204966));
+ if (groundTruth) { // from TL file
+ initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
+ initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
+ initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
+ initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
+ } else {
+ initial.insert(symbol('L', 1), Point2(3.5784, 2.76944));
+ initial.insert(symbol('L', 6), Point2(-1.34989, 3.03492));
+ initial.insert(symbol('L', 0), Point2(0.725404, -0.0630549));
+ initial.insert(symbol('L', 5), Point2(0.714743, -0.204966));
+ }
+
+ // set some loop variables
+ size_t i = 1; // step counter
+ size_t k = 0; // range measurement counter
+ bool initialized = false;
+ Pose2 lastPose = pose0;
+ size_t countK = 0;
// Loop over odometry
gttic_(iSAM);
- size_t i = 1; // step counter
- size_t k = 0; // range measurement counter
- bool update = false;
- Pose2 lastPose = pose0;
- size_t countK = 0;
BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
+ //--------------------------------- odometry loop -----------------------------------------
double t;
Pose2 odometry;
boost::tie(t, odometry) = timedOdometry;
// add odometry factor
- newFactors.add(
- BetweenFactor(i - 1, i, odometry,
- noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.01, 0.2))));
+ newFactors.add(BetweenFactor(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));
// predict pose and add as initial estimate
Pose2 predictedPose = lastPose.compose(odometry);
@@ -156,22 +169,19 @@ int main(int argc, char** argv) {
while (k < K && t >= boost::get<0>(triples[k])) {
size_t j = boost::get<1>(triples[k]);
double range = boost::get<2>(triples[k]);
- newFactors.add(
- RangeFactor(i, symbol('L', j), range,
- rangeNoiseModel));
+ newFactors.add(RangeFactor(i, symbol('L', j), range,rangeNoise));
k = k + 1;
countK = countK + 1;
- update = true;
}
// Check whether to update iSAM 2
- if (update && (k > minK) && (countK > incK)) {
- if (batchInitialization) { // Do a full optimize for first minK ranges
+ if ((k > minK) && (countK > incK)) {
+ if (!initialized) { // Do a full optimize for first minK ranges
gttic_(batchInitialization);
LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial);
initial = batchOptimizer.optimize();
gttoc_(batchInitialization);
- batchInitialization = false; // only once
+ initialized = true;
}
gttic_(update);
isam.update(newFactors, initial);
@@ -185,7 +195,8 @@ int main(int argc, char** argv) {
countK = 0;
}
i += 1;
- } // odometry loop
+ //--------------------------------- odometry loop -----------------------------------------
+ } // BOOST_FOREACH
gttoc_(iSAM);
// Print timings
diff --git a/matlab/gtsam_examples/RangeISAMExample_plaza.m b/matlab/gtsam_examples/RangeISAMExample_plaza.m
index b24ae13c7..788745a3d 100644
--- a/matlab/gtsam_examples/RangeISAMExample_plaza.m
+++ b/matlab/gtsam_examples/RangeISAMExample_plaza.m
@@ -27,19 +27,20 @@ import gtsam.*
% Time (sec) X_pose (m) Y_pose (m)
% TD
% Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
-if true % switch between data files
+if false % switch between data files
datafile = findExampleDataFile('Plaza1_.mat');
headingOffset=0;
minK=200; % minimum number of range measurements to process initially
+ incK=5; % minimum number of range measurements to process after
else
datafile = findExampleDataFile('Plaza2_.mat');
headingOffset=pi;
minK=150; % needs less for init
+ incK=25; % minimum number of range measurements to process after
end
load(datafile)
M=size(DR,1);
K=size(TD,1);
-incK=5; % minimum number of range measurements to process after
sigmaR = 100; % range standard deviation
sigmaInitial = 1; % draw initial landmark guess from Gaussian
useGroundTruth = false;