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

removed debug bloks

release/4.3a0
Luca 2014-04-09 21:04:52 -04:00
parent dc7b5d58c0
commit 4bdefc3f70
1 changed files with 0 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam_unstable/slam/SmartFactorBase.h
View File

@ -597,10 +597,6 @@ public:
+ Fi1.transpose() * b.segment<2>(2 * i1) // F' * b + Fi1.transpose() * b.segment<2>(2 * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal();
if(debug) std::cout << "(after) augmentedHessian(" << aug_i1 << "," << aug_numKeys << ")= \n" <<
vectorBlock << std::endl;
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
// main block diagonal - store previous block // main block diagonal - store previous block
matrixBlock = augmentedHessian(aug_i1, aug_i1); matrixBlock = augmentedHessian(aug_i1, aug_i1);
@ -612,10 +608,6 @@ public:
+ Fi1.transpose() + Fi1.transpose()
* (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
matrixBlock = augmentedHessian(aug_i1, aug_i1);
if(debug) std::cout << "(after) augmentedHessian(" << aug_i1 << "," << aug_i1 << ")= \n" <<
matrixBlock << std::endl;
// upper triangular part of the hessian // upper triangular part of the hessian
for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
const Matrix2D& Fi2 = Fblocks.at(i2).second; const Matrix2D& Fi2 = Fblocks.at(i2).second;
@ -635,13 +627,6 @@ public:
augmentedHessian(aug_i1, aug_i2) = matrixBlock augmentedHessian(aug_i1, aug_i2) = matrixBlock
- Fi1.transpose() - Fi1.transpose()
* (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
if(debug) std::cout << "(after) augmentedHessian(" << aug_i1 << "," << aug_i2 << ")= \n" <<
augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() << std::endl;
matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal();
if(debug) std::cout << "(after, after) augmentedHessian(" << aug_i1 << "," << aug_i2 << ")= \n" <<
matrixBlock<< std::endl;
} }
} // end of for over cameras } // end of for over cameras