Merge pull request #256 from borglab/fix/todos

Various improvements

gtsam/geometry/Cal3DS2.h

@@ -76,10 +76,10 @@ public:
   Vector localCoordinates(const Cal3DS2& T2) const ;
 
   /// Return dimensions of calibration manifold object
-  virtual size_t dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
+  virtual size_t dim() const { return dimension ; }
 
   /// Return dimensions of calibration manifold object
-  static size_t Dim() { return 9; }  //TODO: make a final dimension variable
+  static size_t Dim() { return dimension; }
 
   /// @}
   /// @name Clone

gtsam/geometry/Cal3Unified.h

@@ -118,10 +118,10 @@ public:
   Vector10 localCoordinates(const Cal3Unified& T2) const ;
 
   /// Return dimensions of calibration manifold object
-  virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
+  virtual size_t dim() const { return dimension ; }
 
   /// Return dimensions of calibration manifold object
-  static size_t Dim() { return 10; }  //TODO: make a final dimension variable
+  static size_t Dim() { return dimension; }
 
   /// Return all parameters as a vector
   Vector10 vector() const ;

gtsam/geometry/Cal3_S2.h

@@ -193,14 +193,10 @@ public:
   /// @{
 
   /// return DOF, dimensionality of tangent space
-  inline size_t dim() const {
-    return 5;
-  }
+  inline size_t dim() const { return dimension; }
 
   /// return DOF, dimensionality of tangent space
-  static size_t Dim() {
-    return 5;
-  }
+  static size_t Dim() { return dimension; }
 
   /// Given 5-dim tangent vector, create new calibration
   inline Cal3_S2 retract(const Vector& d) const {

gtsam/geometry/Cal3_S2Stereo.h

@@ -111,14 +111,10 @@ namespace gtsam {
     /// @{
 
     /// return DOF, dimensionality of tangent space
-    inline size_t dim() const {
-      return 6;
-    }
+    inline size_t dim() const { return dimension; }
 
     /// return DOF, dimensionality of tangent space
-    static size_t Dim() {
-      return 6;
-    }
+    static size_t Dim() { return dimension; }
 
     /// Given 6-dim tangent vector, create new calibration
     inline Cal3_S2Stereo retract(const Vector& d) const {

gtsam/geometry/CalibratedCamera.h

@@ -326,12 +326,12 @@ public:
 
   /// @deprecated
   inline size_t dim() const {
-    return 6;
+    return dimension;
   }
 
   /// @deprecated
   inline static size_t Dim() {
-    return 6;
+    return dimension;
   }
 
   /// @}

gtsam/geometry/SOn-inl.h

@@ -20,6 +20,10 @@
 
 #include <gtsam/base/Matrix.h>
 
+#include <iostream>
+
+using namespace std;
+
 namespace gtsam {
 
 // Implementation for N>5 just uses dynamic version
@@ -109,4 +113,9 @@ typename SO<N>::VectorN2 SO<N>::vec(
   return X;
 }
 
+template <int N>
+void SO<N>::print(const std::string& s) const {
+    cout << s << matrix_ << endl;
+}
+
 }  // namespace gtsam

gtsam/geometry/SOn.cpp

@@ -13,6 +13,7 @@
  * @file    SOn.cpp
  * @brief   Definitions of dynamic specializations of SO(n)
  * @author  Frank Dellaert
+ * @author  Varun Agrawal
  * @date    March 2019
  */
 

gtsam/geometry/SOn.h

@@ -24,7 +24,6 @@
 
 #include <Eigen/Core>
 
-#include <iostream> // TODO(frank): how to avoid?
 #include <random>
 #include <string>
 #include <type_traits>
@@ -148,9 +147,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   /// @name Testable
   /// @{
 
-  void print(const std::string& s) const {
-    std::cout << s << matrix_ << std::endl;
-  }
+  void print(const std::string& s = std::string()) const;
 
   bool equals(const SO& other, double tol) const {
     return equal_with_abs_tol(matrix_, other.matrix_, tol);

gtsam/geometry/triangulation.h

@@ -123,7 +123,17 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
   return std::make_pair(graph, values);
 }
 
-/// PinholeCamera specific version // TODO: (chris) why does this exist?
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+/// DEPRECATED: PinholeCamera specific version
+template<class CALIBRATION>
+Point3 triangulateNonlinear(
+    const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
+    const Point2Vector& measurements, const Point3& initialEstimate) {
+  return triangulateNonlinear<PinholeCamera<CALIBRATION> > //
+  (cameras, measurements, initialEstimate);
+}
+
+/// DEPRECATED: PinholeCamera specific version
 template<class CALIBRATION>
 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
     const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
@@ -132,6 +142,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
   return triangulationGraph<PinholeCamera<CALIBRATION> > //
   (cameras, measurements, landmarkKey, initialEstimate);
 }
+#endif
 
 /**
  * Optimize for triangulation
@@ -186,15 +197,6 @@ Point3 triangulateNonlinear(
   return optimize(graph, values, Symbol('p', 0));
 }
 
-/// PinholeCamera specific version  // TODO: (chris) why does this exist?
-template<class CALIBRATION>
-Point3 triangulateNonlinear(
-    const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
-    const Point2Vector& measurements, const Point3& initialEstimate) {
-  return triangulateNonlinear<PinholeCamera<CALIBRATION> > //
-  (cameras, measurements, initialEstimate);
-}
-
 /**
  * Create a 3*4 camera projection matrix from calibration and pose.
  * Functor for partial application on calibration

gtsam/linear/NoiseModel.cpp

@@ -135,14 +135,23 @@ bool Gaussian::equals(const Base& expected, double tol) const {
   const Gaussian* p = dynamic_cast<const Gaussian*> (&expected);
   if (p == nullptr) return false;
   if (typeid(*this) != typeid(*p)) return false;
-  //if (!sqrt_information_) return true; // ALEX todo;
   return equal_with_abs_tol(R(), p->R(), sqrt(tol));
 }
 
+/* ************************************************************************* */
+Matrix Gaussian::covariance() const {
+  // Uses a fast version of `covariance = information().inverse();`
+  const Matrix& R = this->R();
+  Matrix I = Matrix::Identity(R.rows(), R.cols());
+  // Fast inverse of upper-triangular matrix R using forward-substitution
+  Matrix Rinv = R.triangularView<Eigen::Upper>().solve(I);
+  // (R' * R)^{-1} = R^{-1} * R^{-1}'
+  return Rinv * Rinv.transpose();
+}
+
 /* ************************************************************************* */
 Vector Gaussian::sigmas() const {
-  // TODO(frank): can this be done faster?
-  return Vector((thisR().transpose() * thisR()).inverse().diagonal()).cwiseSqrt();
+  return Vector(covariance().diagonal()).cwiseSqrt();
 }
 
 /* ************************************************************************* */

gtsam/linear/NoiseModel.h

@@ -270,7 +270,7 @@ namespace gtsam {
       virtual Matrix information() const { return R().transpose() * R(); }
 
       /// Compute covariance matrix
-      virtual Matrix covariance() const { return information().inverse(); }
+      virtual Matrix covariance() const;
 
     private:
       /** Serialization function */