Merged from branch 'trunk'

gtsam.h

@@ -2047,6 +2047,7 @@ virtual class ISAM2  : gtsam::ISAM2BayesTree {
 
   gtsam::Values getLinearizationPoint() const;
   gtsam::Values calculateEstimate() const;
+  Matrix marginalCovariance(size_t key) const;
   gtsam::Values calculateBestEstimate() const;
   gtsam::VectorValues getDelta() const;
   gtsam::NonlinearFactorGraph getFactorsUnsafe() const;

gtsam/nonlinear/ISAM2.cpp

@@ -1018,6 +1018,13 @@ Values ISAM2::calculateEstimate() const {
   return ret;
 }
 
+/* ************************************************************************* */
+Matrix ISAM2::marginalCovariance(Index key) const {
+  return marginalFactor(ordering_[key],
+    params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky)
+    ->information().inverse();
+}
+
 /* ************************************************************************* */
 Values ISAM2::calculateBestEstimate() const {
   VectorValues delta(theta_.dims(ordering_));

gtsam/nonlinear/ISAM2.h

@@ -585,6 +585,9 @@ public:
   template<class VALUE>
   VALUE calculateEstimate(Key key) const;
 
+  /** Return marginal on any variable as a covariance matrix */
+  GTSAM_EXPORT Matrix marginalCovariance(Index key) const;
+
   /// @name Public members for non-typical usage
   /// @{
 

gtsam_unstable/examples/SmartRangeExample_plaza1.cpp

@@ -0,0 +1,265 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 SmartRangeExample_plaza2.cpp
+ * @brief A 2D Range SLAM example
+ * @date June 20, 2013
+ * @author FRank Dellaert
+ */
+
+// Both relative poses and recovered trajectory poses will be stored as Pose2 objects
+#include <gtsam/geometry/Pose2.h>
+
+// Each variable in the system (poses and landmarks) must be identified with a unique key.
+// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
+// Here we will use Symbols
+#include <gtsam/nonlinear/Symbol.h>
+
+// We want to use iSAM2 to solve the range-SLAM problem incrementally
+#include <gtsam/nonlinear/ISAM2.h>
+
+// iSAM2 requires as input a set set of new factors to be added stored in a factor graph,
+// and initial guesses for any new variables used in the added factors
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+
+// We will use a non-liear solver to batch-inituialize from the first 150 frames
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+
+// In GTSAM, measurement functions are represented as 'factors'. Several common factors
+// have been provided with the library for solving robotics SLAM problems.
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/RangeFactor.h>
+#include <gtsam_unstable/slam/SmartRangeFactor.h>
+
+// Standard headers, added last, so we know headers above work on their own
+#include <boost/foreach.hpp>
+#include <fstream>
+#include <iostream>
+
+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)
+
+// load the odometry
+// DR: Odometry Input (delta distance traveled and delta heading change)
+//    Time (sec)  Delta Dist. Trav. (m) Delta Heading (rad)
+typedef pair<double, Pose2> TimedOdometry;
+list<TimedOdometry> readOdometry() {
+  list<TimedOdometry> odometryList;
+  ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt");
+  if (!is)
+    throw runtime_error(
+        "/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt file not found");
+
+  while (is) {
+    double t, distance_traveled, delta_heading;
+    is >> t >> distance_traveled >> delta_heading;
+    odometryList.push_back(
+        TimedOdometry(t, Pose2(distance_traveled, 0, delta_heading)));
+  }
+  is.clear(); /* clears the end-of-file and error flags */
+  return odometryList;
+}
+
+// load the ranges from TD
+//    Time (sec)  Sender / Antenna ID Receiver Node ID  Range (m)
+typedef boost::tuple<double, size_t, double> RangeTriple;
+vector<RangeTriple> readTriples() {
+  vector<RangeTriple> triples;
+  ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt");
+  if (!is)
+    throw runtime_error(
+        "/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt file not found");
+
+  while (is) {
+    double t, sender, receiver, range;
+    is >> t >> sender >> receiver >> range;
+    triples.push_back(RangeTriple(t, receiver, range));
+  }
+  is.clear(); /* clears the end-of-file and error flags */
+  return triples;
+}
+
+// main
+int main(int argc, char** argv) {
+
+  // load Plaza1 data
+  list<TimedOdometry> odometry = readOdometry();
+//  size_t M = odometry.size();
+
+  vector<RangeTriple> triples = readTriples();
+  size_t K = triples.size();
+
+  // parameters
+  size_t start = 220, end=3000;
+  size_t minK = 100; // first batch of smart factors
+  size_t incK = 50; // minimum number of range measurements to process after
+  bool robust = true;
+  bool smart = true;
+
+  // Set Noise parameters
+  Vector priorSigmas = Vector3(1, 1, M_PI);
+  Vector odoSigmas = Vector3(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;
+
+  // Add prior on first pose
+  Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
+      M_PI - 2.02108900000000);
+  NonlinearFactorGraph newFactors;
+  newFactors.add(PriorFactor<Pose2>(0, pose0, priorNoise));
+
+  ofstream os2(
+      "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
+  ofstream os3(
+      "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultSR.txt");
+
+  //  initialize points (Gaussian)
+  Values initial;
+  if (!smart) {
+    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));
+  }
+  Values landmarkEstimates = initial; // copy landmarks
+  initial.insert(0, pose0);
+
+  //  initialize smart range factors
+  size_t ids[] = { 1, 6, 0, 5 };
+  typedef boost::shared_ptr<SmartRangeFactor> SmartPtr;
+  map<size_t, SmartPtr> smartFactors;
+  if (smart) {
+    BOOST_FOREACH(size_t jj,ids) {
+      smartFactors[jj] = SmartPtr(new SmartRangeFactor(sigmaR));
+      newFactors.add(smartFactors[jj]);
+    }
+  }
+
+  // set some loop variables
+  size_t i = 1; // step counter
+  size_t k = 0; // range measurement counter
+  Pose2 lastPose = pose0;
+  size_t countK = 0, totalCount=0;
+
+  // Loop over odometry
+  gttic_(iSAM);
+  BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
+    //--------------------------------- odometry loop -----------------------------------------
+    double t;
+    Pose2 odometry;
+    boost::tie(t, odometry) = timedOdometry;
+
+    // add odometry factor
+    newFactors.add(
+        BetweenFactor<Pose2>(i - 1, i, odometry,
+            NM::Diagonal::Sigmas(odoSigmas)));
+
+    // predict pose and add as initial estimate
+    Pose2 predictedPose = lastPose.compose(odometry);
+    lastPose = predictedPose;
+    initial.insert(i, predictedPose);
+    landmarkEstimates.insert(i, predictedPose);
+
+    // Check if there are range factors to be added
+    while (k < K && t >= boost::get<0>(triples[k])) {
+      size_t j = boost::get<1>(triples[k]);
+      double range = boost::get<2>(triples[k]);
+      if (i > start) {
+        if (smart && totalCount < minK) {
+          smartFactors[j]->addRange(i, range);
+          printf("adding range %g for %d on %d",range,(int)j,(int)i);cout << endl;
+        }
+        else {
+          RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range,
+              rangeNoise);
+          // Throw out obvious outliers based on current landmark estimates
+          Vector error = factor.unwhitenedError(landmarkEstimates);
+          if (k <= 200 || fabs(error[0]) < 5)
+            newFactors.add(factor);
+        }
+        totalCount += 1;
+      }
+      k = k + 1;
+      countK = countK + 1;
+    }
+
+    // Check whether to update iSAM 2
+    if (k >= minK && countK >= incK) {
+      gttic_(update);
+      isam.update(newFactors, initial);
+      gttoc_(update);
+      gttic_(calculateEstimate);
+      Values result = isam.calculateEstimate();
+      gttoc_(calculateEstimate);
+      lastPose = result.at<Pose2>(i);
+      bool hasLandmarks = result.exists(symbol('L', ids[0]));
+      if (hasLandmarks) {
+        // update landmark estimates
+        landmarkEstimates = Values();
+        BOOST_FOREACH(size_t jj,ids)
+          landmarkEstimates.insert(symbol('L', jj), result.at(symbol('L', jj)));
+      }
+      newFactors = NonlinearFactorGraph();
+      initial = Values();
+      if (smart && !hasLandmarks) {
+        cout << "initialize from smart landmarks" << endl;
+        BOOST_FOREACH(size_t jj,ids) {
+          Point2 landmark = smartFactors[jj]->triangulate(result);
+          initial.insert(symbol('L', jj), landmark);
+          landmarkEstimates.insert(symbol('L', jj), landmark);
+        }
+      }
+      countK = 0;
+      BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& it, result.filter<Point2>())
+        os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
+            << endl;
+      if (smart) {
+        BOOST_FOREACH(size_t jj,ids) {
+          Point2 landmark = smartFactors[jj]->triangulate(result);
+          os3 << jj << "\t" << landmark.x() << "\t" << landmark.y() << "\t1"
+              << endl;
+        }
+      }
+    }
+    i += 1;
+    if (i>end) break;
+    //--------------------------------- odometry loop -----------------------------------------
+  } // BOOST_FOREACH
+  gttoc_(iSAM);
+
+  // Print timings
+  tictoc_print_();
+
+  // Write result to file
+  Values result = isam.calculateEstimate();
+  ofstream os(
+      "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
+  BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, result.filter<Pose2>())
+    os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
+        << it.value.theta() << endl;
+  exit(0);
+}
+

gtsam_unstable/examples/SmartRangeExample_plaza2.cpp

@@ -61,7 +61,8 @@ list<TimedOdometry> readOdometry() {
   list<TimedOdometry> odometryList;
   ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt");
   if (!is)
-    throw runtime_error("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt file not found");
+    throw runtime_error(
+        "/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt file not found");
 
   while (is) {
     double t, distance_traveled, delta_heading;
@@ -80,7 +81,8 @@ vector<RangeTriple> readTriples() {
   vector<RangeTriple> triples;
   ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt");
   if (!is)
-    throw runtime_error("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt file not found");
+    throw runtime_error(
+        "/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt file not found");
 
   while (is) {
     double t, sender, receiver, range;
@@ -92,7 +94,7 @@ vector<RangeTriple> readTriples() {
 }
 
 // main
-int main (int argc, char** argv) {
+int main(int argc, char** argv) {
 
   // load Plaza1 data
   list<TimedOdometry> odometry = readOdometry();
@@ -106,7 +108,7 @@ int main (int argc, char** argv) {
   bool robust = false;
 
   // Set Noise parameters
-  Vector priorSigmas = Vector3(1,1,M_PI);
+  Vector priorSigmas = Vector3(0.01, 0.01, 0.01);
   Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
   double sigmaR = 100; // range standard deviation
   const NM::Base::shared_ptr // all same type
@@ -120,8 +122,7 @@ int main (int argc, char** argv) {
   ISAM2 isam;
 
   // Add prior on first pose
-  Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
+  Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, -2.02108900000000);
-      M_PI - 2.02108900000000);
   NonlinearFactorGraph newFactors;
   newFactors.add(PriorFactor<Pose2>(0, pose0, priorNoise));
 
@@ -149,7 +150,9 @@ int main (int argc, char** argv) {
     boost::tie(t, odometry) = timedOdometry;
 
     // add odometry factor
-    newFactors.add(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));
+    newFactors.add(
+        BetweenFactor<Pose2>(i - 1, i, odometry,
+            NM::Diagonal::Sigmas(odoSigmas)));
 
     // predict pose and add as initial estimate
     Pose2 predictedPose = lastPose.compose(odometry);
@@ -161,10 +164,10 @@ 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]);
-      RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range,rangeNoise);
+      RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range, rangeNoise);
       // Throw out obvious outliers based on current landmark estimates
-      Vector error=factor.unwhitenedError(landmarkEstimates);
+      Vector error = factor.unwhitenedError(landmarkEstimates);
-      if (k<=200 || fabs(error[0])<5)
+      if (k <= 200 || fabs(error[0]) < 5)
         newFactors.add(factor);
       k = k + 1;
       countK = countK + 1;
@@ -199,12 +202,16 @@ int main (int argc, char** argv) {
 
   // Write result to file
   Values result = isam.calculateEstimate();
-  ofstream os2("/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
+  ofstream os2(
+      "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
   BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& it, result.filter<Point2>())
-    os2 << it.key << "\t" <<  it.value.x() << "\t" <<  it.value.y() << "\t1" << endl;
+    os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
-  ofstream os("/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
+        << endl;
+  ofstream os(
+      "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
   BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, result.filter<Pose2>())
-    os << it.key << "\t" <<  it.value.x() << "\t" <<  it.value.y() << "\t" <<  it.value.theta() << endl;
+    os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
+        << it.value.theta() << endl;
   exit(0);
 }
 

gtsam_unstable/examples/plotRangeResults.p

@@ -2,5 +2,5 @@
 reset
 #set terminal pdf
 set title "Smart range SLAM result"
-set size ratio 1
+set size ratio -1
-plot "rangeResult.txt" using 2:3 with lines, "rangeResultLM.txt" using 2:3:4 with circles
+plot "rangeResult.txt" using 2:3 with lines, "rangeResultLM.txt" using 2:3:4 with circles, "rangeResultSR.txt" using 2:3:4 with circles

gtsam_unstable/slam/SmartRangeFactor.h

@@ -69,6 +69,8 @@ public:
   /** print */
   virtual void print(const std::string& s = "",
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    std::cout << s << "SmartRangeFactor with " << size() << " measurements\n";
+    NoiseModelFactor::print(s);
   }
 
   /** Check if two factors are equal */
@@ -82,7 +84,16 @@ public:
    * Triangulate a point from at least three pose-range pairs
    * Checks for best pair that includes first point
    */
-  static Point2 triangulate(const std::list<Circle2>& circles) {
+  Point2 triangulate(const Values& x) const {
+    gttic_(triangulate);
+    // create n circles corresponding to measured range around each pose
+    std::list<Circle2> circles;
+    size_t n = size();
+    for (size_t j = 0; j < n; j++) {
+      const Pose2& pose = x.at<Pose2>(keys_[j]);
+      circles.push_back(Circle2(pose.translation(), measurements_[j]));
+    }
+
     Circle2 circle1 = circles.front();
     boost::optional<Point2> best_fh;
     boost::optional<Circle2> best_circle;
@@ -116,6 +127,7 @@ public:
       error2 += it.center.dist(p2);
     }
     return (error1 < error2) ? p1 : p2;
+    gttoc_(triangulate);
   }
 
   /**
@@ -133,16 +145,9 @@ public:
     } else {
       Vector error = zero(1);
 
-      // create n circles corresponding to measured range around each pose
-      std::list<Circle2> circles;
-      for (size_t j = 0; j < n; j++) {
-        const Pose2& pose = x.at<Pose2>(keys_[j]);
-        circles.push_back(Circle2(pose.translation(), measurements_[j]));
-      }
-
       // triangulate to get the optimized point
       // TODO: Should we have a (better?) variant that does this in relative coordinates ?
-      Point2 optimizedPoint = triangulate(circles);
+      Point2 optimizedPoint = triangulate(x);
 
       // TODO: triangulation should be followed by an optimization given poses
       // now evaluate the errors between predicted and measured range

matlab/gtsam_examples/RangeISAMExample_plaza.m

@@ -27,7 +27,7 @@ import gtsam.*
 %    Time (sec)	X_pose (m)	Y_pose (m)
 % TD
 %    Time (sec)	Sender / Antenna ID	Receiver Node ID	Range (m)
-if false % switch between data files
+if true % switch between data files
   datafile = findExampleDataFile('Plaza1_.mat');
   headingOffset=0;
   minK=200; % minimum number of range measurements to process initially
@@ -65,7 +65,7 @@ isam = ISAM2;
 %% Add prior on first pose
 pose0 = Pose2(GT(1,2),GT(1,3),headingOffset+GT(1,4));
 newFactors = NonlinearFactorGraph;
-if ~addRange | ~useGroundTruth
+if ~addRange || ~useGroundTruth
   newFactors.add(PriorFactorPose2(0,pose0,noiseModels.prior));
 end
 initial = Values;

matlab/gtsam_examples/RangeSLAMExample_plaza.m

@@ -27,7 +27,7 @@ import gtsam.*
 %    Time (sec)	X_pose (m)	Y_pose (m)
 % TD
 %    Time (sec)	Sender / Antenna ID	Receiver Node ID	Range (m)
-datafile = findExampleDataFile('Plaza1_.mat');
+datafile = findExampleDataFile('Plaza2_.mat');
 load(datafile)
 M=size(DR,1);
 K=size(TD,1);
@@ -42,7 +42,7 @@ base = noiseModel.mEstimator.Tukey(5);
 noiseModels.range = noiseModel.Robust(base,noiseModel.Isotropic.Sigma(1, sigmaR));
 
 %% Add prior on first pose
-pose0 = Pose2(GT(1,2),GT(1,3),GT(1,4));
+pose0 = Pose2(GT(1,2),GT(1,3),pi+GT(1,4));
 graph = NonlinearFactorGraph;
 graph.add(PriorFactorPose2(0,pose0,noiseModels.prior));
 initial = Values;
@@ -84,7 +84,7 @@ for i=1:M
 end
 toc
 
-%% GRaph was built, optimize !
+%% Graph was built, optimize !
 tic
 batchOptimizer = LevenbergMarquardtOptimizer(graph, initial);
 result = batchOptimizer.optimize();

tests/testGaussianISAM2.cpp

@@ -18,6 +18,7 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/BearingRangeFactor.h>
@@ -1027,6 +1028,18 @@ TEST_UNSAFE(ISAM2, marginalizeLeaves5)
   EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys));
 }
 
+/* ************************************************************************* */
+TEST(ISAM2, marginalCovariance)
+{
+  // Create isam2
+  ISAM2 isam = createSlamlikeISAM2();
+
+  // Check marginal
+  Matrix expected = Marginals(isam.getFactorsUnsafe(), isam.getLinearizationPoint()).marginalCovariance(5);
+  Matrix actual = isam.marginalCovariance(5);
+  EXPECT(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

wrap/Class.cpp

@@ -478,6 +478,7 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi
   proxyFile.oss << "    function sobj = saveobj(obj)\n";
   proxyFile.oss << "      % SAVEOBJ Saves the object to a matlab-readable format\n";
   proxyFile.oss << "      sobj = obj.string_serialize();\n";
+  proxyFile.oss << "    end\n";
 }
 
 /* ************************************************************************* */

wrap/tests/expected/Point3.m

@@ -66,6 +66,7 @@ classdef Point3 < handle
     function sobj = saveobj(obj)
       % SAVEOBJ Saves the object to a matlab-readable format
       sobj = obj.string_serialize();
+    end
   end
 
   methods(Static = true)