diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2bd985048..6130a7b06 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -657,7 +657,6 @@ std::pair, // Combine and sort variable blocks in elimination order JacobianFactor::shared_ptr jointFactor; try { - cout << "JacobianFactor make_shared" << endl; jointFactor = boost::make_shared(factors, keys); } catch (std::invalid_argument&) { throw InvalidDenseElimination( @@ -665,8 +664,6 @@ std::pair, "involved in the provided factors."); } - jointFactor->print("JointFactor0:"); - // Do dense elimination if (jointFactor->model_) jointFactor->model_ = jointFactor->model_->QR(jointFactor->Ab_.matrix()); @@ -676,13 +673,9 @@ std::pair, // Zero below the diagonal jointFactor->Ab_.matrix().triangularView().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 989d04c66..1a0f170f4 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 = true; + bool verbose = false; if (verbose) cout << "\nStarting Constrained::QR" << endl; // get size(A) and maxRank @@ -428,34 +428,29 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { Vector pseudo(m); // allocate storage for pseudo-inverse Vector invsigmas = reciprocal(sigmas_); - gtsam::print(invsigmas, "invsigmas: "); Vector weights = emul(invsigmas,invsigmas); // calculate weights once // Denote weights of inequality constraints with the negative sign // TODO: might be slow! for (size_t s = 0; s QPSolver::computeStepSize(const GaussianFactorGraph& workingGraph, const VectorValues& xk, const VectorValues& p) const { - static bool debug = true; + static bool debug = false; double minAlpha = 1.0; int closestFactorIx = -1, closestSigmaIx = -1; @@ -228,9 +228,7 @@ boost::tuple QPSolver::computeStepSize(const GaussianFactorGra Vector aj = jacobian->getA(xj).row(s); ajTp += aj.dot(pj); } - if (debug) { - cout << "s, ajTp: " << s << " " << ajTp << endl; - } + if (debug) cout << "s, ajTp: " << s << " " << ajTp << endl; // Check if aj'*p >0. Don't care if it's not. if (ajTp<=0) continue; @@ -242,15 +240,11 @@ boost::tuple QPSolver::computeStepSize(const GaussianFactorGra Vector aj = jacobian->getA(xj).row(s); ajTx += aj.dot(xkj); } - if (debug) { - cout << "b[s], ajTx: " << b[s] << " " << ajTx << " " << ajTp << endl; - } + if (debug) cout << "b[s], ajTx: " << b[s] << " " << ajTx << " " << ajTp << endl; // alpha = (bj - aj'*xk) / (aj'*p) double alpha = (b[s] - ajTx)/ajTp; - if (debug) { - cout << "alpha: " << alpha << endl; - } + if (debug) cout << "alpha: " << alpha << endl; // We want the minimum of all those max alphas if (alpha < minAlpha) { @@ -266,7 +260,7 @@ boost::tuple QPSolver::computeStepSize(const GaussianFactorGra /* ************************************************************************* */ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& currentSolution) const { - static bool debug = true; + static bool debug = false; // Obtain the solution from the current working graph VectorValues newSolution = workingGraph.optimize(); if (debug) newSolution.print("New solution:"); @@ -294,9 +288,7 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& c int factorIx, sigmaIx; VectorValues p = newSolution - currentSolution; boost::tie(alpha, factorIx, sigmaIx) = computeStepSize(workingGraph, currentSolution, p); - if (debug) { - cout << "alpha, factorIx, sigmaIx: " << alpha << " " << factorIx << " " << sigmaIx << endl; - } + if (debug) cout << "alpha, factorIx, sigmaIx: " << alpha << " " << factorIx << " " << sigmaIx << endl; // also add to the working set the one that complains the most updateWorkingSetInplace(workingGraph, factorIx, sigmaIx, 0.0); // step!