12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Move some Eigen methods to cpp

release/4.3a0
Frank Dellaert 2023-01-22 09:05:04 -08:00
parent 8c83e432b9
commit 9bca36fd2c
13 changed files with 115 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/base/SymmetricBlockMatrix.cpp
View File

@ -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);

20
gtsam/base/SymmetricBlockMatrix.h
View File

@ -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();
/// @}

2
gtsam/geometry/Point2.h
View File

@ -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());
}
/*

6
gtsam/geometry/Pose3.cpp
View File

@ -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) {

5
gtsam/geometry/Pose3.h
View File

@ -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

8
gtsam/geometry/Unit3.cpp
View File

@ -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:

13
gtsam/geometry/Unit3.h
View File

@ -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) {

4
gtsam/linear/LossFunctions.cpp
View File

@ -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

4
gtsam/linear/LossFunctions.h
View File

@ -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 */

49
gtsam/linear/NoiseModel.cpp
View File

@ -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));

40
gtsam/linear/NoiseModel.h
View File

@ -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);

11
gtsam/linear/PCGSolver.cpp
View File

@ -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) {

12
gtsam/linear/PCGSolver.h
View File

@ -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;
};