From 9bca36fd2c5d359be673755ca8e4168c1b0c7e51 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Jan 2023 09:05:04 -0800 Subject: [PATCH] Move some Eigen methods to cpp --- gtsam/base/SymmetricBlockMatrix.cpp | 18 +++++++++++ gtsam/base/SymmetricBlockMatrix.h | 20 ++---------- gtsam/geometry/Point2.h | 2 +- gtsam/geometry/Pose3.cpp | 6 ++++ gtsam/geometry/Pose3.h | 5 +-- gtsam/geometry/Unit3.cpp | 8 +++++ gtsam/geometry/Unit3.h | 13 ++------ gtsam/linear/LossFunctions.cpp | 4 +++ gtsam/linear/LossFunctions.h | 4 +-- gtsam/linear/NoiseModel.cpp | 49 ++++++++++++++++++++++++++--- gtsam/linear/NoiseModel.h | 40 +++++++---------------- gtsam/linear/PCGSolver.cpp | 11 +++++++ gtsam/linear/PCGSolver.h | 12 ++----- 13 files changed, 115 insertions(+), 77 deletions(-) diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index 7f41da137..d37bbf7d1 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/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() *= -1.0; +} + +/* ************************************************************************* */ +void SymmetricBlockMatrix::invertInPlace() { + const auto identity = Matrix::Identity(rows(), rows()); + full().triangularView() = + selfadjointView().llt().solve(identity).triangularView(); +} + /* ************************************************************************* */ void SymmetricBlockMatrix::choleskyPartial(DenseIndex nFrontals) { gttic(VerticalBlockMatrix_choleskyPartial); diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 302a1ec34..e8afe6e38 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -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 @@ -265,19 +260,10 @@ namespace gtsam { } /// Negate the entire active matrix. - void negate() { - full().triangularView() *= -1.0; - } + void negate(); /// Invert the entire active matrix in place. - void invertInPlace() { - const auto identity = Matrix::Identity(rows(), rows()); - full().triangularView() = - selfadjointView() - .llt() - .solve(identity) - .triangularView(); - } + void invertInPlace(); /// @} diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index d8b6daca8..21b44ada5 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -45,7 +45,7 @@ typedef std::vector > Point2Vector; /// multiply with scalar inline Point2 operator*(double s, const Point2& p) { - return p * s; + return Point2(s * p.x(), s * p.y()); } /* diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2652fc073..6a8c61560 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -158,6 +158,12 @@ bool Pose3::equals(const Pose3& pose, double tol) const { return R_.equals(pose.R_, tol) && traits::Equals(t_, pose.t_, tol); } +/* ************************************************************************* */ +Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { + return Pose3(interpolate(R_, T.R_, t), + interpolate(t_, T.t_, t)); +} + /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 80741e1c3..678df8376 100644 --- a/gtsam/geometry/Pose3.h +++ b/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 { - return Pose3(interpolate(R_, T.R_, t), - interpolate(t_, T.t_, t)); - } + Pose3 interpolateRt(const Pose3& T, double t) const; /// @} /// @name Lie Group diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 6f70d840e..17169a5f5 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/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: diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index ebd5c63c9..2989159a3 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -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) { diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 7307c4a68..64ded7cc3 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/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 diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index d9cfc1f3c..ee05cb987 100644 --- a/gtsam/linear/LossFunctions.h +++ b/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 { - return weight(error).cwiseSqrt(); - } + Vector sqrtWeight(const Vector &error) const; /** reweight block matrices and a vector according to their weight * implementation */ diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 7108a7660..8c6b5fd55 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/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 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)); diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 7bcf808e5..447121848 100644 --- a/gtsam/linear/NoiseModel.h +++ b/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) { - 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); diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index a7af7d8d8..141412159 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/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 & dimensions) { diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h index 198b77ec8..cb90ae520 100644 --- a/gtsam/linear/PCGSolver.h +++ b/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 { - 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; };