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

Use precisions_ where possible

release/4.3a0
Frank Dellaert 2014-01-04 10:14:59 -05:00
parent aa6aee1157
commit f8fbfaea50
3 changed files with 3 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/linear/HessianFactor.cpp
View File

@ -254,10 +254,10 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : info_(matrix_) {
if(jf.model_->isConstrained())
throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model");
else {
Vector invsigmas = jf.model_->invsigmas().cwiseProduct(jf.model_->invsigmas());
const Vector& precisions = jf.model_->precisions();
info_.copyStructureFrom(jf.Ab_);
BlockInfo::constBlock A = jf.Ab_.full();
matrix_.noalias() = A.transpose() * invsigmas.asDiagonal() * A;
matrix_.noalias() = A.transpose() * precisions.asDiagonal() * A;
}
} else if(dynamic_cast<const HessianFactor*>(&gf)) {
const HessianFactor& hf(static_cast<const HessianFactor&>(gf));

4
gtsam/linear/NoiseModel.cpp
View File

@ -308,8 +308,6 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
list<Triple> Rd;
Vector pseudo(m); // allocate storage for pseudo-inverse
Vector invsigmas = reciprocal(sigmas_);
Vector weights = emul(invsigmas,invsigmas); // calculate weights once
// We loop over all columns, because the columns that can be eliminated
// are not necessarily contiguous. For each one, estimate the corresponding
@ -321,7 +319,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
// Calculate weighted pseudo-inverse and corresponding precision
gttic(constrained_QR_weightedPseudoinverse);
double precision = weightedPseudoinverse(a, weights, pseudo);
double precision = weightedPseudoinverse(a, precisions_, pseudo);
gttoc(constrained_QR_weightedPseudoinverse);
// If precision is zero, no information on this column

2
gtsam/linear/NoiseModel.h
View File

@ -342,11 +342,9 @@ namespace gtsam {
Vector mu_;
/** protected constructor takes sigmas */
// Keeps only sigmas and calculates invsigmas when necessary
Constrained(const Vector& sigmas = zero(1)) :
Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) {}
// Keeps only sigmas and calculates invsigmas when necessary
// allows for specifying mu
Constrained(const Vector& mu, const Vector& sigmas) :
Diagonal(sigmas), mu_(mu) {}