diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 9cc814245..12147adf2 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -880,7 +880,7 @@ static BinaryMeasurement convert(const BinaryMeasurement &p) { const Matrix6 M = gaussian->covariance(); return BinaryMeasurement( p.key1(), p.key2(), p.measured().rotation(), - noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); + noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0), true)); } template <>