Move some Eigen methods to cpp
							parent
							
								
									8c83e432b9
								
							
						
					
					
						commit
						9bca36fd2c
					
				|  | @ -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); | ||||
|  |  | |||
|  | @ -63,12 +63,7 @@ namespace gtsam { | |||
| 
 | ||||
|   public: | ||||
|     /// Construct from an empty matrix (asserts that the matrix is empty)
 | ||||
|     SymmetricBlockMatrix() : | ||||
|       blockStart_(0) | ||||
|     { | ||||
|       variableColOffsets_.push_back(0); | ||||
|       assertInvariants(); | ||||
|     } | ||||
|     SymmetricBlockMatrix(); | ||||
| 
 | ||||
|     /// 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() { | ||||
|       full().triangularView<Eigen::Upper>() *= -1.0; | ||||
|     } | ||||
|     void negate(); | ||||
| 
 | ||||
|     /// Invert the entire active matrix in place.
 | ||||
|     void invertInPlace() { | ||||
|       const auto identity = Matrix::Identity(rows(), rows()); | ||||
|       full().triangularView<Eigen::Upper>() = | ||||
|           selfadjointView() | ||||
|               .llt() | ||||
|               .solve(identity) | ||||
|               .triangularView<Eigen::Upper>(); | ||||
|     } | ||||
|     void invertInPlace(); | ||||
| 
 | ||||
|     /// @}
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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()); | ||||
| } | ||||
| 
 | ||||
| /*
 | ||||
|  |  | |||
|  | @ -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) { | ||||
|  |  | |||
|  | @ -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 { | ||||
|     return Pose3(interpolate<Rot3>(R_, T.R_, t), | ||||
|                  interpolate<Point3>(t_, T.t_, t)); | ||||
|   } | ||||
|   Pose3 interpolateRt(const Pose3& T, double t) const; | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Lie Group
 | ||||
|  |  | |||
|  | @ -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:
 | ||||
|  |  | |||
|  | @ -67,21 +67,14 @@ public: | |||
|   } | ||||
| 
 | ||||
|   /// Construct from point
 | ||||
|   explicit Unit3(const Vector3& p) : | ||||
|       p_(p.normalized()) { | ||||
|   } | ||||
|   explicit Unit3(const Vector3& p); | ||||
| 
 | ||||
|   /// Construct from x,y,z
 | ||||
|   Unit3(double x, double y, double z) : | ||||
|       p_(x, y, z) { | ||||
|     p_.normalize(); | ||||
|   } | ||||
|   Unit3(double x, double y, double z); | ||||
| 
 | ||||
|   /// 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) { | ||||
|     p_.normalize(); | ||||
|   } | ||||
|   explicit Unit3(const Point2& p, double f); | ||||
| 
 | ||||
|   /// Copy constructor
 | ||||
|   Unit3(const Unit3& u) { | ||||
|  |  | |||
|  | @ -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
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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 { | ||||
|     return weight(error).cwiseSqrt(); | ||||
|   } | ||||
|   Vector sqrtWeight(const Vector &error) const; | ||||
| 
 | ||||
|   /** reweight block matrices and a vector according to their weight
 | ||||
|    * implementation */ | ||||
|  |  | |||
|  | @ -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)); | ||||
|  |  | |||
|  | @ -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) { | ||||
|         return Variances(precisions.array().inverse(), smart); | ||||
|       } | ||||
|       static shared_ptr Precisions(const Vector& precisions, bool smart = true); | ||||
| 
 | ||||
|       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) { | ||||
|         return MixedSigmas(Vector::Constant(sigmas.size(), 1000.0), sigmas); | ||||
|       } | ||||
|       static shared_ptr MixedSigmas(const Vector& 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) { | ||||
|         return MixedSigmas(Vector::Constant(sigmas.size(), m), sigmas); | ||||
|       } | ||||
|       static shared_ptr MixedSigmas(double m, const Vector& 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) { | ||||
|         return shared_ptr(new Constrained(mu, variances.cwiseSqrt())); | ||||
|       } | ||||
|       static shared_ptr MixedVariances(const Vector& variances) { | ||||
|         return shared_ptr(new Constrained(variances.cwiseSqrt())); | ||||
|       } | ||||
|       static shared_ptr MixedVariances(const Vector& mu, const Vector& variances); | ||||
|       static shared_ptr MixedVariances(const Vector& variances); | ||||
| 
 | ||||
|       /**
 | ||||
|        * 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) { | ||||
|         return MixedVariances(mu, precisions.array().inverse()); | ||||
|       } | ||||
|       static shared_ptr MixedPrecisions(const Vector& precisions) { | ||||
|         return MixedVariances(precisions.array().inverse()); | ||||
|       } | ||||
|       static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions); | ||||
|       static shared_ptr MixedPrecisions(const Vector& precisions); | ||||
| 
 | ||||
|       /**
 | ||||
|        * 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 { | ||||
|         return noise_->unweightedWhiten(v); | ||||
|       } | ||||
|       double weight(const Vector& v) const override { | ||||
|         return robust_->weight(v.norm()); | ||||
|       } | ||||
|       Vector unweightedWhiten(const Vector& v) const override; | ||||
|       double weight(const Vector& v) const override; | ||||
| 
 | ||||
|       static shared_ptr Create( | ||||
|         const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise); | ||||
|  |  | |||
|  | @ -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) { | ||||
|  |  | |||
|  | @ -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 { | ||||
|     x *= alpha; | ||||
|   } | ||||
|   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 scal(const double alpha, Vector &x) const; | ||||
|   double dot(const Vector &x, const Vector &y) const; | ||||
|   void axpy(const double alpha, const Vector &x, Vector &y) const; | ||||
| 
 | ||||
|   void getb(Vector &b) const; | ||||
| }; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue