From cb02a95f94f67460cb957bae6aad8892ae316299 Mon Sep 17 00:00:00 2001
From: thduynguyen <thduynguyen@gmail.com>
Date: Tue, 15 Apr 2014 13:55:04 -0400
Subject: [PATCH] fix bug in weightedPseudoInverse dealing with negative
 weights of ineq constraints

---
 gtsam/base/Vector.cpp           | 14 +++++++-------
 gtsam/linear/JacobianFactor.cpp |  8 +++++++-
 gtsam/linear/NoiseModel.cpp     |  6 +++++-
 3 files changed, 19 insertions(+), 9 deletions(-)

diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp
index c2d72b85e..755a175db 100644
--- a/gtsam/base/Vector.cpp
+++ b/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)
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 7d7281a4d..2bd985048 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/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);
 }
diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index f98a92ff5..989d04c66 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/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);
 }