Merged in feature/improvementsIncrementalFilter (pull request #355)

Feature/improvementsIncrementalFilter

gtsam/geometry/CalibratedCamera.h

@@ -32,9 +32,17 @@ namespace gtsam {
 class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
     CheiralityException> {
 public:
-  CheiralityException() :
+  CheiralityException()
-      ThreadsafeException<CheiralityException>("Cheirality Exception") {
+    : CheiralityException(std::numeric_limits<Key>::max()) {}
-  }
+
+  CheiralityException(Key j)
+    : ThreadsafeException<CheiralityException>("CheiralityException"),
+      j_(j) {}
+
+  Key nearbyVariable() const {return j_;}
+
+private:
+  Key j_;
 };
 
 /**

gtsam/geometry/OrientedPlane3.cpp

@@ -83,8 +83,14 @@ Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobia
 }
 
 /* ************************************************************************* */
-OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const {
+OrientedPlane3 OrientedPlane3::retract(const Vector3& v,
-  return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2));
+                                       OptionalJacobian<3,3> H) const {
+  Matrix22 H_n;
+  Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr));
+  if (H) {
+    *H << H_n, Vector2::Zero(), 0, 0, 1;
+  }
+  return OrientedPlane3(n_retract, d_ + v(2));
 }
 
 /* ************************************************************************* */

gtsam/geometry/OrientedPlane3.h

@@ -134,7 +134,7 @@ public:
   }
 
   /// The retract function
-  OrientedPlane3 retract(const Vector3& v) const;
+  OrientedPlane3 retract(const Vector3& v, OptionalJacobian<3,3> H = boost::none) const;
 
   /// The local coordinates function
   Vector3 localCoordinates(const OrientedPlane3& s) const;

gtsam/geometry/StereoCamera.h

@@ -25,9 +25,19 @@ namespace gtsam {
 
 class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error {
 public:
-  StereoCheiralityException() :
+  StereoCheiralityException()
-      std::runtime_error("Stereo Cheirality Exception") {
+    : StereoCheiralityException(std::numeric_limits<Key>::max()) {}
+
+  StereoCheiralityException(Key j)
+    : std::runtime_error("Stereo Cheirality Exception"),
+      j_(j) {}
+
+  Key nearbyVariable() const {
+    return j_;
   }
+
+private:
+  Key j_;
 };
 
 /**

gtsam/geometry/Unit3.cpp

@@ -250,19 +250,33 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const {
 }
 
 /* ************************************************************************* */
-Unit3 Unit3::retract(const Vector2& v) const {
+Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<2,2> H) const {
   // Compute the 3D xi_hat vector
   const Vector3 xi_hat = basis() * v;
   const double theta = xi_hat.norm();
+  const double c = std::cos(theta);
 
-  // Treat case of very small v differently
+  // Treat case of very small v differently.
+  Matrix23 H_from_point;
   if (theta < std::numeric_limits<double>::epsilon()) {
-    return Unit3(Vector3(std::cos(theta) * p_ + xi_hat));
+    const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat,
+                                                 H? &H_from_point : nullptr);
+    if (H) { // Jacobian
+      *H = H_from_point *
+          (-p_ * xi_hat.transpose() + Matrix33::Identity()) * basis();
+    }
+    return exp_p_xi_hat;
   }
 
-  const Vector3 exp_p_xi_hat =
+  const double st = std::sin(theta) / theta;
-      std::cos(theta) * p_ + xi_hat * (sin(theta) / theta);
+  const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat * st,
-  return Unit3(exp_p_xi_hat);
+                                               H? &H_from_point : nullptr);
+  if (H) { // Jacobian
+    *H = H_from_point *
+        (p_ * -st * xi_hat.transpose() + st * Matrix33::Identity() +
+        xi_hat * ((c - st) / std::pow(theta, 2)) * xi_hat.transpose()) * basis();
+  }
+  return exp_p_xi_hat;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Unit3.h

@@ -188,7 +188,7 @@ public:
   };
 
   /// The retract function
-  Unit3 retract(const Vector2& v) const;
+  Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const;
 
   /// The local coordinates function
   Vector2 localCoordinates(const Unit3& s) const;

gtsam/geometry/tests/testOrientedPlane3.cpp

@@ -161,6 +161,26 @@ TEST (OrientedPlane3, error2) {
   EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
 }
 
+//*******************************************************************************
+TEST (OrientedPlane3, jacobian_retract) {
+  OrientedPlane3 plane(-1, 0.1, 0.2, 5);
+  Matrix33 H_actual;
+  boost::function<OrientedPlane3(const Vector3&)> f =
+      boost::bind(&OrientedPlane3::retract, plane, _1, boost::none);
+  {
+      Vector3 v (-0.1, 0.2, 0.3);
+      plane.retract(v, H_actual);
+      Matrix H_expected_numerical = numericalDerivative11(f, v);
+      EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-9));
+  }
+  {
+      Vector3 v (0, 0, 0);
+      plane.retract(v, H_actual);
+      Matrix H_expected_numerical = numericalDerivative11(f, v);
+      EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-9));
+  }
+}
+
 /* ************************************************************************* */
 int main() {
   srand(time(NULL));

gtsam/geometry/tests/testUnit3.cpp

@@ -371,6 +371,26 @@ TEST(Unit3, retract) {
   }
 }
 
+//*******************************************************************************
+TEST (Unit3, jacobian_retract) {
+  Matrix22 H;
+  Unit3 p;
+  boost::function<Unit3(const Vector2&)> f =
+      boost::bind(&Unit3::retract, p, _1, boost::none);
+  {
+      Vector2 v (-0.2, 0.1);
+      p.retract(v, H);
+      Matrix H_expected_numerical = numericalDerivative11(f, v);
+      EXPECT(assert_equal(H_expected_numerical, H, 1e-9));
+  }
+  {
+      Vector2 v (0, 0);
+      p.retract(v, H);
+      Matrix H_expected_numerical = numericalDerivative11(f, v);
+      EXPECT(assert_equal(H_expected_numerical, H, 1e-9));
+  }
+}
+
 //*******************************************************************************
 TEST(Unit3, retract_expmap) {
   Unit3 p;

gtsam/linear/tests/testGaussianBayesTree.cpp

@@ -297,6 +297,29 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
   EXPECT(newError < origError);
 }
 
+/* ************************************************************************* */
+TEST(GaussianBayesTree, determinant_and_smallestEigenvalue) {
+
+  // create small factor graph
+  GaussianFactorGraph fg;
+  Key x1 = 2, x2 = 0, l1 = 1;
+  SharedDiagonal unit2 = noiseModel::Unit::Create(2);
+  fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2);
+  fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2);
+  fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2);
+  fg += JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2);
+
+  // create corresponding Bayes tree:
+  boost::shared_ptr<gtsam::GaussianBayesTree> bt = fg.eliminateMultifrontal();
+  Matrix H = fg.hessian().first;
+
+  // test determinant
+  // NOTE: the hessian of the factor graph is H = R'R where R is the matrix encoded by the bayes tree,
+  // for this reason we have to take the sqrt
+  double expectedDeterminant = sqrt(H.determinant()); // determinant computed from full matrix
+  double actualDeterminant = bt->determinant();
+  EXPECT_DOUBLES_EQUAL(expectedDeterminant,actualDeterminant,expectedDeterminant*1e-6);// relative tolerance
+}
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}

gtsam/navigation/PreintegrationParams.h

@@ -56,6 +56,7 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
 
   const Matrix3& getAccelerometerCovariance() const { return accelerometerCovariance; }
   const Matrix3& getIntegrationCovariance()   const { return integrationCovariance; }
+  const Vector3& getGravity()   const { return n_gravity; }
   bool           getUse2ndOrderCoriolis()     const { return use2ndOrderCoriolis; }
 
 protected:

gtsam/slam/ProjectionFactor.h

@@ -152,7 +152,7 @@ namespace gtsam {
           std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
               " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
         if (throwCheirality_)
-          throw e;
+          throw CheiralityException(this->key2());
       }
       return Vector2::Constant(2.0 * K_->fx());
     }

gtsam/slam/StereoFactor.h

@@ -144,7 +144,7 @@ public:
       std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
           " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
       if (throwCheirality_)
-        throw e;
+        throw StereoCheiralityException(this->key2());
     }
     return Vector3::Constant(2.0 * K_->fx());
   }

gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp

@@ -52,7 +52,7 @@ Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const {
 /* ************************************************************************* */
 FixedLagSmoother::Result BatchFixedLagSmoother::update(
     const NonlinearFactorGraph& newFactors, const Values& newTheta,
-    const KeyTimestampMap& timestamps) {
+    const KeyTimestampMap& timestamps, const FastVector<size_t>& factorsToRemove) {
 
   // Update all of the internal variables with the new information
   gttic(augment_system);
@@ -69,6 +69,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(
   insertFactors(newFactors);
   gttoc(augment_system);
 
+  // remove factors in factorToRemove
+  for(const size_t i : factorsToRemove){
+    if(factors_[i])
+      factors_[i].reset();
+  }
+
   // Update the Timestamps associated with the factor keys
   updateKeyTimestampMap(timestamps);
 

gtsam_unstable/nonlinear/BatchFixedLagSmoother.h

@@ -47,8 +47,10 @@ public:
   virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const;
 
   /** Add new factors, updating the solution and relinearizing as needed. */
-  Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
+  Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
-      const KeyTimestampMap& timestamps = KeyTimestampMap());
+                const Values& newTheta = Values(),
+                const KeyTimestampMap& timestamps = KeyTimestampMap(),
+                const FastVector<size_t>& factorsToRemove = FastVector<size_t>());
 
   /** Compute an estimate from the incomplete linear delta computed during the last update.
    * This delta is incomplete because it was not updated below wildfire_threshold.  If only

gtsam_unstable/nonlinear/FixedLagSmoother.cpp

@@ -21,6 +21,15 @@
 
 namespace gtsam {
 
+/* ************************************************************************* */
+void FixedLagSmoother::Result::print() const {
+  std::cout << "Nr iterations: " << iterations << '\n'
+            << "Nr intermediateSteps: " << intermediateSteps << '\n'
+            << "Nr nonlinear variables: " << nonlinearVariables << '\n'
+            << "Nr linear variables: " << linearVariables << '\n'
+            << "error: " << error << std::endl;
+}
+
 /* ************************************************************************* */
 void FixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const {
   std::cout << s;

gtsam_unstable/nonlinear/FixedLagSmoother.h

@@ -59,9 +59,9 @@ public:
     size_t getNonlinearVariables() const { return nonlinearVariables; }
     size_t getLinearVariables() const { return linearVariables; }
     double getError() const { return error; }
+    void print() const;
   };
 
-
   /** default constructor */
   FixedLagSmoother(double smootherLag = 0.0) : smootherLag_(smootherLag) { }
 
@@ -90,8 +90,10 @@ public:
   }
 
   /** Add new factors, updating the solution and relinearizing as needed. */
-  virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
+  virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
-      const KeyTimestampMap& timestamps = KeyTimestampMap()) = 0;
+                        const Values& newTheta = Values(),
+                        const KeyTimestampMap& timestamps = KeyTimestampMap(),
+                        const FastVector<size_t>& factorsToRemove = FastVector<size_t>()) = 0;
 
   /** Compute an estimate from the incomplete linear delta computed during the last update.
    * This delta is incomplete because it was not updated below wildfire_threshold.  If only

gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp

@@ -65,7 +65,7 @@ bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs,
 /* ************************************************************************* */
 FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
     const NonlinearFactorGraph& newFactors, const Values& newTheta,
-    const KeyTimestampMap& timestamps) {
+    const KeyTimestampMap& timestamps, const FastVector<size_t>& factorsToRemove) {
 
   const bool debug = ISDEBUG("IncrementalFixedLagSmoother update");
 
@@ -125,8 +125,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
   KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
 
   // Update iSAM2
-  ISAM2Result isamResult = isam_.update(newFactors, newTheta,
+  isamResult_ = isam_.update(newFactors, newTheta,
-      FactorIndices(), constrainedKeys, boost::none, additionalMarkedKeys);
+      factorsToRemove, constrainedKeys, boost::none, additionalMarkedKeys);
 
   if (debug) {
     PrintSymbolicTree(isam_,

gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h

@@ -59,10 +59,12 @@ public:
    * @param newFactors new factors on old and/or new variables
    * @param newTheta new values for new variables only
    * @param timestamps an (optional) map from keys to real time stamps
+   * @param factorsToRemove an (optional) list of factors to remove.
    */
   Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
-      const Values& newTheta = Values(), //
+                const Values& newTheta = Values(), //
-      const KeyTimestampMap& timestamps = KeyTimestampMap());
+                const KeyTimestampMap& timestamps = KeyTimestampMap(),
+                const FastVector<size_t>& factorsToRemove = FactorIndices());
 
   /** Compute an estimate from the incomplete linear delta computed during the last update.
    * This delta is incomplete because it was not updated below wildfire_threshold.  If only
@@ -108,11 +110,17 @@ public:
     return isam_.marginalCovariance(key);
   }
 
+  /// Get results of latest isam2 update
+  const ISAM2Result& getISAM2Result() const{ return isamResult_; }
+
 protected:
   /** An iSAM2 object used to perform inference. The smoother lag is controlled
    * by what factors are removed each iteration */
   ISAM2 isam_;
 
+  /** Store results of latest isam2 update */
+  ISAM2Result isamResult_;
+
   /** Erase any keys associated with timestamps before the provided time */
   void eraseKeysBefore(double timestamp);
 

gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp

@@ -53,11 +53,11 @@ TEST( BatchFixedLagSmoother, Example )
   // the BatchFixedLagSmoother should be identical (even with the linearized approximations at
   // the end of the smoothing lag)
 
-//  SETDEBUG("BatchFixedLagSmoother update", true);
+  //  SETDEBUG("BatchFixedLagSmoother update", true);
-//  SETDEBUG("BatchFixedLagSmoother reorder", true);
+  //  SETDEBUG("BatchFixedLagSmoother reorder", true);
-//  SETDEBUG("BatchFixedLagSmoother optimize", true);
+  //  SETDEBUG("BatchFixedLagSmoother optimize", true);
-//  SETDEBUG("BatchFixedLagSmoother marginalize", true);
+  //  SETDEBUG("BatchFixedLagSmoother marginalize", true);
-//  SETDEBUG("BatchFixedLagSmoother calculateMarginalFactors", true);
+  //  SETDEBUG("BatchFixedLagSmoother calculateMarginalFactors", true);
 
   // Set up parameters
   SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
@@ -177,6 +177,65 @@ TEST( BatchFixedLagSmoother, Example )
     ++i;
   }
 
+  // add/remove an extra factor
+  {
+    Key key1 = Key(i-1);
+    Key key2 = Key(i);
+
+    NonlinearFactorGraph newFactors;
+    Values newValues;
+    Timestamps newTimestamps;
+
+    // add 2 odometry factors
+    newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
+    newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
+    newValues.insert(key2, Point2(double(i)+0.1, -0.1));
+    newTimestamps[key2] = double(i);
+
+    fullgraph.push_back(newFactors);
+    fullinit.insert(newValues);
+
+    // Update the smoother
+    smoother.update(newFactors, newValues, newTimestamps);
+
+    // Check
+    CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
+
+//    NonlinearFactorGraph smootherGraph = smoother.getFactors();
+//    for(size_t i=0; i<smootherGraph.size(); i++){
+//      if(smootherGraph[i]){
+//      std::cout << "i:" << i << std::endl;
+//      smootherGraph[i]->print();
+//      }
+//    }
+
+    // now remove one of the two and try again
+    // empty values and new factors for fake update in which we only remove factors
+    NonlinearFactorGraph emptyNewFactors;
+    Values emptyNewValues;
+    Timestamps emptyNewTimestamps;
+
+    size_t factorIndex = 6; // any index that does not break connectivity of the graph
+    FastVector<size_t> factorToRemove;
+    factorToRemove.push_back(factorIndex);
+
+    const NonlinearFactorGraph smootherFactorsBeforeRemove = smoother.getFactors();
+
+    // remove factor
+    smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove);
+
+    // check that the factors in the smoother are right
+    NonlinearFactorGraph actual = smoother.getFactors();
+    for(size_t i=0; i< smootherFactorsBeforeRemove.size(); i++){
+      // check that the factors that were not removed are there
+      if(smootherFactorsBeforeRemove[i] && i != factorIndex){
+        EXPECT(smootherFactorsBeforeRemove[i]->equals(*actual[i]));
+      }
+      else{ // while the factors that were not there or were removed are no longer there
+        EXPECT(!actual[i]);
+      }
+    }
+  }
 }
 
 /* ************************************************************************* */

gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp

@@ -71,8 +71,6 @@ TEST( IncrementalFixedLagSmoother, Example )
   Values fullinit;
   NonlinearFactorGraph fullgraph;
 
-
-
   // i keeps track of the time step
   size_t i = 0;
 
@@ -177,6 +175,63 @@ TEST( IncrementalFixedLagSmoother, Example )
     ++i;
   }
 
+  // add/remove an extra factor
+  {
+	  Key key1 = MakeKey(i-1);
+	  Key key2 = MakeKey(i);
+
+	  NonlinearFactorGraph newFactors;
+	  Values newValues;
+	  Timestamps newTimestamps;
+
+	  // add 2 odometry factors
+	  newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
+	  newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
+	  newValues.insert(key2, Point2(double(i)+0.1, -0.1));
+	  newTimestamps[key2] = double(i);
+
+	  fullgraph.push_back(newFactors);
+	  fullinit.insert(newValues);
+
+	  // Update the smoother
+	  smoother.update(newFactors, newValues, newTimestamps);
+
+	  // Check
+	  CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
+
+	  // now remove one of the two and try again
+	  // empty values and new factors for fake update in which we only remove factors
+	  NonlinearFactorGraph emptyNewFactors;
+	  Values emptyNewValues;
+	  Timestamps emptyNewTimestamps;
+
+	  size_t factorIndex = 25; // any index that does not break connectivity of the graph
+	  FastVector<size_t> factorToRemove;
+	  factorToRemove.push_back(factorIndex);
+
+	  const NonlinearFactorGraph smootherFactorsBeforeRemove = smoother.getFactors();
+
+	  // remove factor
+	  smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove);
+
+	  // Note: the following test (checking that the number of factor is reduced by 1)
+	  // fails  since we are not reusing slots, hence also when removing a factor we do not change
+	  // the size of the factor graph
+	  // size_t nrFactorsAfterRemoval = smoother.getFactors().size();
+	  // DOUBLES_EQUAL(nrFactorsBeforeRemoval-1, nrFactorsAfterRemoval, 1e-5);
+
+	  // check that the factors in the smoother are right
+	  NonlinearFactorGraph actual = smoother.getFactors();
+	  for(size_t i=0; i< smootherFactorsBeforeRemove.size(); i++){
+	    // check that the factors that were not removed are there
+	    if(smootherFactorsBeforeRemove[i] && i != factorIndex){
+	      EXPECT(smootherFactorsBeforeRemove[i]->equals(*actual[i]));
+	    }
+	    else{ // while the factors that were not there or were removed are no longer there
+	      EXPECT(!actual[i]);
+	    }
+	  }
+  }
 }
 
 /* ************************************************************************* */