diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 13a8a90f0..57f32ab2d 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -276,7 +276,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { Key key_j = nonuniqueKeys.at(j); if(i==j){ augmentedHessianUniqueKeys.updateDiagonalBlock( keyToSlotMap[key_i] , augmentedHessian.diagonalBlock(i)); - }else if(i < j){ + }else{ // (i < j) if( keyToSlotMap[key_i] != keyToSlotMap[key_j] ){ augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i] , keyToSlotMap[key_j], augmentedHessian.aboveDiagonalBlock(i,j)); @@ -286,10 +286,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { augmentedHessian.aboveDiagonalBlock(i,j).transpose()); } } - else{ //TODO: remove else - augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i] , keyToSlotMap[key_j], - augmentedHessian.aboveDiagonalBlock(j,i).transpose()); - } } } augmentedHessianUniqueKeys.updateDiagonalBlock(nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));