diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index ba8d478ad..a41047ae4 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -174,7 +174,7 @@ public: (*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b; (*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose()); (*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b; - (*info)(slotB, slotB)(0,0) = b.transpose() * b; + (*info)(slotB, slotB)(0,0) += b.transpose() * b; } } };