fix bug in weightedPseudoInverse dealing with negative weights of ineq constraints

gtsam/base/Vector.cpp

@@ -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)

gtsam/linear/JacobianFactor.cpp

@@ -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);
 }

gtsam/linear/NoiseModel.cpp

@@ -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);
 }