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

fix bug in weightedPseudoInverse dealing with negative weights of ineq constraints

release/4.3a0
thduynguyen 2014-04-15 13:55:04 -04:00
parent cb37b025e9
commit cb02a95f94
3 changed files with 19 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/base/Vector.cpp
View File

@ -323,9 +323,14 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights,
size_t m = weights.size();
static const double inf = std::numeric_limits<double>::infinity();
// Check once for zero entries of a. TODO: really needed ?
// Check once for zero entries of a.
vector<bool> isZero;
for (size_t i = 0; i < m; ++i) isZero.push_back(fabs(a[i]) < 1e-9);
for (size_t i = 0; i < m; ++i) {
isZero.push_back(fabs(a[i]) < 1e-9);
// If there is a valid (a[i]!=0) inequality constraint (weight<0),
// ignore it by also setting isZero flag
if (!isZero[i] && (weights[i]<0)) isZero[i] = true;
}
for (size_t i = 0; i < m; ++i) {
// If there is a valid (a!=0) constraint (sigma==0) return the first one
@ -336,11 +341,6 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights,
pseudo = delta(m, i, 1 / a[i]);
return inf;
}
// If there is a valid (a!=0) inequality constraint (sigma<0), ignore it by returning 0
else if (weights[i] < 0 && !isZero[i]) {
pseudo = zero(m);
return 0;
}
}
// Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma)

8
gtsam/linear/JacobianFactor.cpp
View File

@ -657,6 +657,7 @@ std::pair<boost::shared_ptr<GaussianConditional>,
// Combine and sort variable blocks in elimination order
JacobianFactor::shared_ptr jointFactor;
try {
cout << "JacobianFactor make_shared" << endl;
jointFactor = boost::make_shared<JacobianFactor>(factors, keys);
} catch (std::invalid_argument&) {
throw InvalidDenseElimination(
@ -664,8 +665,9 @@ std::pair<boost::shared_ptr<GaussianConditional>,
"involved in the provided factors.");
}
jointFactor->print("JointFactor0:");
// Do dense elimination
SharedDiagonal noiseModel;
if (jointFactor->model_)
jointFactor->model_ = jointFactor->model_->QR(jointFactor->Ab_.matrix());
else
@ -674,9 +676,13 @@ std::pair<boost::shared_ptr<GaussianConditional>,
// Zero below the diagonal
jointFactor->Ab_.matrix().triangularView<Eigen::StrictlyLower>().setZero();
factors.print("Factors to eliminate: ");
jointFactor->print("JointFactor1:");
// Split elimination result into conditional and remaining factor
GaussianConditional::shared_ptr conditional = jointFactor->splitConditional(
keys.size());
cout << "JacobianFactor split conditoinal ok!" << endl;
return make_pair(conditional, jointFactor);
}

6
gtsam/linear/NoiseModel.cpp
View File

@ -415,7 +415,7 @@ Constrained::shared_ptr Constrained::unit(size_t augmentedDim) const {
// It is Gram-Schmidt orthogonalization rather than Householder
// Previously Diagonal::QR
SharedDiagonal Constrained::QR(Matrix& Ab) const {
bool verbose = false;
bool verbose = true;
if (verbose) cout << "\nStarting Constrained::QR" << endl;
// get size(A) and maxRank
@ -488,10 +488,12 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
}
cout << "Create storage for precisions" << endl;
// Create storage for precisions
Vector precisions(Rd.size());
gttic(constrained_QR_write_back_into_Ab);
cout << "write back result" << endl;
// Write back result in Ab, imperative as we are
// TODO: test that is correct if a column was skipped !!!!
size_t i = 0; // start with first row
@ -509,6 +511,8 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
}
gttoc(constrained_QR_write_back_into_Ab);
cout << "return " << (int) mixed << endl;
gtsam::print(precisions, "precisions:");
// Must include mu, as the defaults might be higher, resulting in non-convergence
return mixed ? Constrained::MixedPrecisions(mu_, precisions) : Diagonal::Precisions(precisions);
}