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