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

small improvement on negative weights

release/4.3a0
thduynguyen 2014-04-15 06:11:26 -04:00
parent ba1273ae6b
commit cb37b025e9
1 changed files with 5 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/linear/NoiseModel.cpp
View File

@ -428,15 +428,13 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
Vector pseudo(m); // allocate storage for pseudo-inverse
Vector invsigmas = reciprocal(sigmas_);
// Obtain the signs of each elements.
// We use negative signs to denote inequality constraints
// TODO: might be slow!
Vector signs(sigmas_.size());
for (size_t s = 0; s<sigmas_.size(); ++s) signs[s] = (sigmas_[s]<0)?-1.0:1.0 ;
gtsam::print(invsigmas, "invsigmas: ");
Vector weights = emul(invsigmas,invsigmas); // calculate weights once
// We use negative signs to denote inequality constraints
weights = emul(weights, signs);
// Denote weights of inequality constraints with the negative sign
// TODO: might be slow!
for (size_t s = 0; s<sigmas_.size(); ++s)
weights[s] = (sigmas_[s]<0)?-weights[s]:weights[s];
gtsam::print(weights, "weights: ");
// We loop over all columns, because the columns that can be eliminated