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

The precisions are used many many times in HessianFactor::updatATA, so I added them to the NoiseModel. The invsigmas are no longer optional and hence do not cause extra allocation at every invocation. Same with precision: returns const &

release/4.3a0
Frank Dellaert 2014-01-02 18:05:44 -05:00
parent c394f9a601
commit 6f64b20a5b
3 changed files with 23 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/linear/HessianFactor.cpp
View File

@ -458,8 +458,8 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt
} else { } else {
noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_)); noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
if(diagonal) { if(diagonal) {
Vector invsigmas2 = update.model_->invsigmas().cwiseProduct(update.model_->invsigmas()); const Vector& precisions = diagonal->precisions();
updateInform.noalias() = updateA.transpose() * invsigmas2.asDiagonal() * updateA; updateInform.noalias() = updateA.transpose() * precisions.asDiagonal() * updateA;
} else } else
throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal"); throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
} }

23
gtsam/linear/NoiseModel.cpp
View File

@ -159,15 +159,12 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const
// Diagonal // Diagonal
/* ************************************************************************* */ /* ************************************************************************* */
Diagonal::Diagonal() : Diagonal::Diagonal() :
Gaussian(1), sigmas_(ones(1)), invsigmas_(ones(1)) { Gaussian(1), sigmas_(ones(1)), invsigmas_(ones(1)), precisions_(ones(1)) {
} }
Diagonal::Diagonal(const Vector& sigmas, bool initialize_invsigmas): Diagonal::Diagonal(const Vector& sigmas) :
Gaussian(sigmas.size()), sigmas_(sigmas) { Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)), precisions_(
if (initialize_invsigmas) emul(invsigmas_, invsigmas_)) {
invsigmas_ = reciprocal(sigmas);
else
invsigmas_ = boost::none;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -198,18 +195,6 @@ void Diagonal::print(const string& name) const {
gtsam::print(sigmas_, name + "diagonal sigmas"); gtsam::print(sigmas_, name + "diagonal sigmas");
} }
/* ************************************************************************* */
Vector Diagonal::invsigmas() const {
if (invsigmas_) return *invsigmas_;
else return reciprocal(sigmas_);
}
/* ************************************************************************* */
double Diagonal::invsigma(size_t i) const {
if (invsigmas_) return (*invsigmas_)(i);
else return 1.0/sigmas_(i);
}
/* ************************************************************************* */ /* ************************************************************************* */
Vector Diagonal::whiten(const Vector& v) const { Vector Diagonal::whiten(const Vector& v) const {
return emul(v, invsigmas()); return emul(v, invsigmas());

28
gtsam/linear/NoiseModel.h
View File

@ -235,19 +235,19 @@ namespace gtsam {
class GTSAM_EXPORT Diagonal : public Gaussian { class GTSAM_EXPORT Diagonal : public Gaussian {
protected: protected:
/** sigmas and reciprocal */ /**
Vector sigmas_; * Standard deviations (sigmas), their inverse and inverse square (weights/precisions)
* These are all computed at construction: the idea is to use one shared model
private: * where computation is done only once, the common use case in many problems.
*/
boost::optional<Vector> invsigmas_; ///< optional to allow for constraints Vector sigmas_, invsigmas_, precisions_;
protected: protected:
/** protected constructor takes sigmas */ /** protected constructor takes sigmas */
Diagonal(); Diagonal();
/** constructor to allow for disabling initializaion of invsigmas */ /** constructor to allow for disabling initializaion of invsigmas */
Diagonal(const Vector& sigmas, bool initialize_invsigmas=true); Diagonal(const Vector& sigmas);
public: public:
@ -292,8 +292,14 @@ namespace gtsam {
/** /**
* Return sqrt precisions * Return sqrt precisions
*/ */
Vector invsigmas() const; inline const Vector& invsigmas() const { return invsigmas_; }
double invsigma(size_t i) const; inline double invsigma(size_t i) const {return invsigmas_(i);}
/**
* Return precisions
*/
inline const Vector& precisions() const { return precisions_; }
inline double precision(size_t i) const {return precisions_(i);}
/** /**
* Return R itself, but note that Whiten(H) is cheaper than R*H * Return R itself, but note that Whiten(H) is cheaper than R*H
@ -338,12 +344,12 @@ namespace gtsam {
/** protected constructor takes sigmas */ /** protected constructor takes sigmas */
// Keeps only sigmas and calculates invsigmas when necessary // Keeps only sigmas and calculates invsigmas when necessary
Constrained(const Vector& sigmas = zero(1)) : Constrained(const Vector& sigmas = zero(1)) :
Diagonal(sigmas, false), mu_(repeat(sigmas.size(), 1000.0)) {} Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) {}
// Keeps only sigmas and calculates invsigmas when necessary // Keeps only sigmas and calculates invsigmas when necessary
// allows for specifying mu // allows for specifying mu
Constrained(const Vector& mu, const Vector& sigmas) : Constrained(const Vector& mu, const Vector& sigmas) :
Diagonal(sigmas, false), mu_(mu) {} Diagonal(sigmas), mu_(mu) {}
public: public: