From 6ef11bb2975ffe3d3c18c484aeb5ddc0e055fcfa Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 9 Apr 2014 21:14:38 -0400 Subject: [PATCH] removed debug statements --- gtsam_unstable/slam/SmartFactorBase.h | 23 +++++------------------ 1 file changed, 5 insertions(+), 18 deletions(-) diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index 40b8b2b09..d4d3fd489 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -569,8 +569,6 @@ public: for (size_t slot=0; slot < allKeys.size(); slot++) KeySlotMap.insert(std::make_pair(allKeys[slot],slot)); - bool debug= false; - // a single point is observed in numKeys cameras size_t numKeys = this->keys_.size(); // cameras observing current point size_t aug_numKeys = (augmentedHessian.rows() - 1) / D; // all cameras in the group @@ -588,21 +586,15 @@ public: size_t aug_i1 = KeySlotMap[cameraKey_i1]; // information vector - store previous vector - vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); - if(debug) std::cout << "(before) augmentedHessian(" << aug_i1 << "," << aug_numKeys << ")= \n" << - vectorBlock << std::endl; - + // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); // add contribution of current factor - augmentedHessian(aug_i1, aug_numKeys) = vectorBlock + augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal() + Fi1.transpose() * b.segment<2>(2 * i1) // F' * b - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) // main block diagonal - store previous block matrixBlock = augmentedHessian(aug_i1, aug_i1); - if(debug) std::cout << "(before) augmentedHessian(" << aug_i1 << "," << aug_i1 << ")= \n" << - matrixBlock << std::endl; - // add contribution of current factor augmentedHessian(aug_i1, aug_i1) = matrixBlock + Fi1.transpose() @@ -613,18 +605,13 @@ public: const Matrix2D& Fi2 = Fblocks.at(i2).second; Key cameraKey_i2 = this->keys_[i2]; - size_t aug_i2 = KeySlotMap[cameraKey_i2]; + size_t aug_i2 = KeySlotMap[cameraKey_i2]; // (DxD) = (Dx2) * ( (2x2) * (2xD) ) // off diagonal block - store previous block - matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); - - if(debug) std::cout << "(aug_i1= " << aug_i1 << ", aug_i2= " << aug_i2 << ") (i2= " <(2 * i2, 0).transpose() * Fi2); }