Move some Eigen methods to cpp

gtsam/base/SymmetricBlockMatrix.cpp

@@ -24,6 +24,12 @@
 
 namespace gtsam {
 
+/* ************************************************************************* */
+SymmetricBlockMatrix::SymmetricBlockMatrix() : blockStart_(0) {
+  variableColOffsets_.push_back(0);
+  assertInvariants();
+}
+
 /* ************************************************************************* */
 SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf(
     const SymmetricBlockMatrix& other) {
@@ -61,6 +67,18 @@ Matrix SymmetricBlockMatrix::block(DenseIndex I, DenseIndex J) const {
   }
 }
 
+/* ************************************************************************* */
+void SymmetricBlockMatrix::negate() {
+  full().triangularView<Eigen::Upper>() *= -1.0;
+}
+
+/* ************************************************************************* */
+void SymmetricBlockMatrix::invertInPlace() {
+  const auto identity = Matrix::Identity(rows(), rows());
+  full().triangularView<Eigen::Upper>() =
+      selfadjointView().llt().solve(identity).triangularView<Eigen::Upper>();
+}
+
 /* ************************************************************************* */
 void SymmetricBlockMatrix::choleskyPartial(DenseIndex nFrontals) {
   gttic(VerticalBlockMatrix_choleskyPartial);

gtsam/base/SymmetricBlockMatrix.h

@@ -63,12 +63,7 @@ namespace gtsam {
 
   public:
     /// Construct from an empty matrix (asserts that the matrix is empty)
-    SymmetricBlockMatrix() :
+    SymmetricBlockMatrix();
-      blockStart_(0)
-    {
-      variableColOffsets_.push_back(0);
-      assertInvariants();
-    }
 
     /// Construct from a container of the sizes of each block.
     template<typename CONTAINER>
@@ -265,19 +260,10 @@ namespace gtsam {
     }
 
     /// Negate the entire active matrix.
-    void negate() {
+    void negate();
-      full().triangularView<Eigen::Upper>() *= -1.0;
-    }
 
     /// Invert the entire active matrix in place.
-    void invertInPlace() {
+    void invertInPlace();
-      const auto identity = Matrix::Identity(rows(), rows());
-      full().triangularView<Eigen::Upper>() =
-          selfadjointView()
-              .llt()
-              .solve(identity)
-              .triangularView<Eigen::Upper>();
-    }
 
     /// @}
 

gtsam/geometry/Point2.h

@@ -45,7 +45,7 @@ typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;
 
 /// multiply with scalar
 inline Point2 operator*(double s, const Point2& p) {
-  return p * s;
+  return Point2(s * p.x(), s * p.y());
 }
 
 /*

gtsam/geometry/Pose3.cpp

@@ -158,6 +158,12 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
   return R_.equals(pose.R_, tol) && traits<Point3>::Equals(t_, pose.t_, tol);
 }
 
+/* ************************************************************************* */
+Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
+  return Pose3(interpolate<Rot3>(R_, T.R_, t),
+                interpolate<Point3>(t_, T.t_, t));
+}
+
 /* ************************************************************************* */
 /** Modified from Murray94book version (which assumes w and v normalized?) */
 Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {

gtsam/geometry/Pose3.h

@@ -129,10 +129,7 @@ public:
    * @param T End point of interpolation.
    * @param t A value in [0, 1].
    */
-  Pose3 interpolateRt(const Pose3& T, double t) const {
+  Pose3 interpolateRt(const Pose3& T, double t) const;
-    return Pose3(interpolate<Rot3>(R_, T.R_, t),
-                 interpolate<Point3>(t_, T.t_, t));
-  }
 
   /// @}
   /// @name Lie Group

gtsam/geometry/Unit3.cpp

@@ -32,6 +32,14 @@ using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
+Unit3::Unit3(const Vector3& p) : p_(p.normalized()) {}
+
+Unit3::Unit3(double x, double y, double z) : p_(x, y, z) { p_.normalize(); }
+
+Unit3::Unit3(const Point2& p, double f) : p_(p.x(), p.y(), f) {
+  p_.normalize();
+}
 /* ************************************************************************* */
 Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2, 3> H) {
   // 3*3 Derivative of representation with respect to point is 3*3:

gtsam/geometry/Unit3.h

@@ -67,21 +67,14 @@ public:
   }
 
   /// Construct from point
-  explicit Unit3(const Vector3& p) :
+  explicit Unit3(const Vector3& p);
-      p_(p.normalized()) {
-  }
 
   /// Construct from x,y,z
-  Unit3(double x, double y, double z) :
+  Unit3(double x, double y, double z);
-      p_(x, y, z) {
-    p_.normalize();
-  }
 
   /// Construct from 2D point in plane at focal length f
   /// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point
-  explicit Unit3(const Point2& p, double f) : p_(p.x(), p.y(), f) {
+  explicit Unit3(const Point2& p, double f);
-    p_.normalize();
-  }
 
   /// Copy constructor
   Unit3(const Unit3& u) {

gtsam/linear/LossFunctions.cpp

@@ -40,6 +40,10 @@ Vector Base::weight(const Vector& error) const {
   return w;
 }
 
+Vector Base::sqrtWeight(const Vector &error) const {
+  return weight(error).cwiseSqrt();
+}
+
 // The following three functions re-weight block matrices and a vector
 // according to their weight implementation
 

gtsam/linear/LossFunctions.h

@@ -115,9 +115,7 @@ class GTSAM_EXPORT Base {
   Vector weight(const Vector &error) const;
 
   /** square root version of the weight function */
-  Vector sqrtWeight(const Vector &error) const {
+  Vector sqrtWeight(const Vector &error) const;
-    return weight(error).cwiseSqrt();
-  }
 
   /** reweight block matrices and a vector according to their weight
    * implementation */

gtsam/linear/NoiseModel.cpp

@@ -239,6 +239,8 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const
   whitenInPlace(b);
 }
 
+Matrix Gaussian::information() const { return R().transpose() * R(); }
+
 /* ************************************************************************* */
 // Diagonal
 /* ************************************************************************* */
@@ -284,6 +286,11 @@ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
   full: return Diagonal::shared_ptr(new Diagonal(sigmas));
 }
 
+/* ************************************************************************* */
+Diagonal::shared_ptr Diagonal::Precisions(const Vector& precisions,
+                                          bool smart) {
+  return Variances(precisions.array().inverse(), smart);
+}
 /* ************************************************************************* */
 void Diagonal::print(const string& name) const {
   gtsam::print(sigmas_, name + "diagonal sigmas ");
@@ -294,22 +301,18 @@ Vector Diagonal::whiten(const Vector& v) const {
   return v.cwiseProduct(invsigmas_);
 }
 
-/* ************************************************************************* */
 Vector Diagonal::unwhiten(const Vector& v) const {
   return v.cwiseProduct(sigmas_);
 }
 
-/* ************************************************************************* */
 Matrix Diagonal::Whiten(const Matrix& H) const {
   return vector_scale(invsigmas(), H);
 }
 
-/* ************************************************************************* */
 void Diagonal::WhitenInPlace(Matrix& H) const {
   vector_scale_inplace(invsigmas(), H);
 }
 
-/* ************************************************************************* */
 void Diagonal::WhitenInPlace(Eigen::Block<Matrix> H) const {
   H = invsigmas().asDiagonal() * H;
 }
@@ -377,6 +380,32 @@ Vector Constrained::whiten(const Vector& v) const {
   return c;
 }
 
+/* ************************************************************************* */
+Constrained::shared_ptr Constrained::MixedSigmas(const Vector& sigmas) {
+  return MixedSigmas(Vector::Constant(sigmas.size(), 1000.0), sigmas);
+}
+
+Constrained::shared_ptr Constrained::MixedSigmas(double m,
+                                                 const Vector& sigmas) {
+  return MixedSigmas(Vector::Constant(sigmas.size(), m), sigmas);
+}
+
+Constrained::shared_ptr Constrained::MixedVariances(const Vector& mu,
+                                                    const Vector& variances) {
+  return shared_ptr(new Constrained(mu, variances.cwiseSqrt()));
+}
+Constrained::shared_ptr Constrained::MixedVariances(const Vector& variances) {
+  return shared_ptr(new Constrained(variances.cwiseSqrt()));
+}
+
+Constrained::shared_ptr Constrained::MixedPrecisions(const Vector& mu,
+                                                     const Vector& precisions) {
+  return MixedVariances(mu, precisions.array().inverse());
+}
+Constrained::shared_ptr Constrained::MixedPrecisions(const Vector& precisions) {
+  return MixedVariances(precisions.array().inverse());
+}
+
 /* ************************************************************************* */
 double Constrained::squaredMahalanobisDistance(const Vector& v) const {
   Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
@@ -623,6 +652,11 @@ void Unit::print(const std::string& name) const {
   cout << name << "unit (" << dim_ << ") " << endl;
 }
 
+/* ************************************************************************* */
+double Unit::squaredMahalanobisDistance(const Vector& v) const {
+  return v.dot(v);
+}
+
 /* ************************************************************************* */
 // Robust
 /* ************************************************************************* */
@@ -663,6 +697,13 @@ void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{
   robust_->reweight(A1,A2,A3,b);
 }
 
+Vector Robust::unweightedWhiten(const Vector& v) const {
+  return noise_->unweightedWhiten(v);
+}
+double Robust::weight(const Vector& v) const {
+  return robust_->weight(v.norm());
+}
+
 Robust::shared_ptr Robust::Create(
 const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
   return shared_ptr(new Robust(robust,noise));

gtsam/linear/NoiseModel.h

@@ -255,7 +255,7 @@ namespace gtsam {
       virtual Matrix R() const { return thisR();}
 
       /// Compute information matrix
-      virtual Matrix information() const { return R().transpose() * R(); }
+      virtual Matrix information() const;
 
       /// Compute covariance matrix
       virtual Matrix covariance() const;
@@ -319,9 +319,7 @@ namespace gtsam {
        * A diagonal noise model created by specifying a Vector of precisions, i.e.
        * i.e. the diagonal of the information matrix, i.e., weights
        */
-      static shared_ptr Precisions(const Vector& precisions, bool smart = true) {
+      static shared_ptr Precisions(const Vector& precisions, bool smart = true);
-        return Variances(precisions.array().inverse(), smart);
-      }
 
       void print(const std::string& name) const override;
       Vector sigmas() const override { return sigmas_; }
@@ -426,39 +424,27 @@ namespace gtsam {
        * A diagonal noise model created by specifying a Vector of
        * standard devations, some of which might be zero
        */
-      static shared_ptr MixedSigmas(const Vector& sigmas) {
+      static shared_ptr MixedSigmas(const Vector& sigmas);
-        return MixedSigmas(Vector::Constant(sigmas.size(), 1000.0), sigmas);
-      }
 
       /**
        * A diagonal noise model created by specifying a Vector of
        * standard devations, some of which might be zero
        */
-      static shared_ptr MixedSigmas(double m, const Vector& sigmas) {
+      static shared_ptr MixedSigmas(double m, const Vector& sigmas);
-        return MixedSigmas(Vector::Constant(sigmas.size(), m), sigmas);
-      }
 
       /**
        * A diagonal noise model created by specifying a Vector of
        * standard devations, some of which might be zero
        */
-      static shared_ptr MixedVariances(const Vector& mu, const Vector& variances) {
+      static shared_ptr MixedVariances(const Vector& mu, const Vector& variances);
-        return shared_ptr(new Constrained(mu, variances.cwiseSqrt()));
+      static shared_ptr MixedVariances(const Vector& variances);
-      }
-      static shared_ptr MixedVariances(const Vector& variances) {
-        return shared_ptr(new Constrained(variances.cwiseSqrt()));
-      }
 
       /**
        * A diagonal noise model created by specifying a Vector of
        * precisions, some of which might be inf
        */
-      static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions) {
+      static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions);
-        return MixedVariances(mu, precisions.array().inverse());
+      static shared_ptr MixedPrecisions(const Vector& precisions);
-      }
-      static shared_ptr MixedPrecisions(const Vector& precisions) {
-        return MixedVariances(precisions.array().inverse());
-      }
 
       /**
        * The squaredMahalanobisDistance function for a constrained noisemodel,
@@ -616,7 +602,7 @@ namespace gtsam {
       bool isUnit() const override { return true; }
 
       void print(const std::string& name) const override;
-      double squaredMahalanobisDistance(const Vector& v) const override {return v.dot(v); }
+      double squaredMahalanobisDistance(const Vector& v) const override;
       Vector whiten(const Vector& v) const override { return v; }
       Vector unwhiten(const Vector& v) const override { return v; }
       Matrix Whiten(const Matrix& H) const override { return H; }
@@ -710,12 +696,8 @@ namespace gtsam {
       void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
       void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
 
-      Vector unweightedWhiten(const Vector& v) const override {
+      Vector unweightedWhiten(const Vector& v) const override;
-        return noise_->unweightedWhiten(v);
+      double weight(const Vector& v) const override;
-      }
-      double weight(const Vector& v) const override {
-        return robust_->weight(v.norm());
-      }
 
       static shared_ptr Create(
         const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);

gtsam/linear/PCGSolver.cpp

@@ -136,6 +136,17 @@ void GaussianFactorGraphSystem::rightPrecondition(const Vector &x,
   preconditioner_.transposeSolve(x, y);
 }
 
+/**********************************************************************************/
+void GaussianFactorGraphSystem::scal(const double alpha, Vector &x) const {
+  x *= alpha;
+}
+double GaussianFactorGraphSystem::dot(const Vector &x, const Vector &y) const {
+  return x.dot(y);
+}
+void GaussianFactorGraphSystem::axpy(const double alpha, const Vector &x,
+                                     Vector &y) const {
+  y += alpha * x;
+}
 /**********************************************************************************/
 VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
     const map<Key, size_t> & dimensions) {

gtsam/linear/PCGSolver.h

@@ -102,15 +102,9 @@ public:
   void multiply(const Vector &x, Vector& y) const;
   void leftPrecondition(const Vector &x, Vector &y) const;
   void rightPrecondition(const Vector &x, Vector &y) const;
-  inline void scal(const double alpha, Vector &x) const {
+  void scal(const double alpha, Vector &x) const;
-    x *= alpha;
+  double dot(const Vector &x, const Vector &y) const;
-  }
+  void axpy(const double alpha, const Vector &x, Vector &y) const;
-  inline double dot(const Vector &x, const Vector &y) const {
-    return x.dot(y);
-  }
-  inline void axpy(const double alpha, const Vector &x, Vector &y) const {
-    y += alpha * x;
-  }
 
   void getb(Vector &b) const;
 };