diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 607819cc7..0f4f24bd0 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -105,6 +105,23 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, // Read matrix and check that diagonal entries are non-zero Matrix M(3, 3); switch (noiseFormat) { + case NoiseFormatAUTO: + // Try to guess covariance matrix layout + if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0) + { + // NoiseFormatGRAPH + M << v1, v2, v5, v2, v3, v6, v5, v6, v4; + } + else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0) + { + // NoiseFormatCOV + M << v1, v2, v3, v2, v4, v5, v3, v5, v6; + } + else + { + throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format."); + } + break; case NoiseFormatG2O: case NoiseFormatCOV: // i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] @@ -136,6 +153,7 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart, // In both cases, what is stored in file is the information matrix model = noiseModel::Gaussian::Information(M, smart); break; + case NoiseFormatAUTO: case NoiseFormatGRAPH: case NoiseFormatCOV: // These cases expect covariance matrix diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 09a5baac7..3a0f8edb7 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -57,7 +57,8 @@ enum NoiseFormat { NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33 NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix ! - NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33 + NoiseFormatCOV, ///< Covariance matrix C11, C12, C13, C22, C23, C33 + NoiseFormatAUTO ///< Try to guess covariance matrix layout }; /// Robust kernel type to wrap around quadratic noise model @@ -79,7 +80,7 @@ GTSAM_EXPORT GraphAndValues load2D( std::pair dataset, int maxID = 0, bool addNoise = false, bool smart = true, // - NoiseFormat noiseFormat = NoiseFormatGRAPH, + NoiseFormat noiseFormat = NoiseFormatAUTO, KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /** @@ -95,7 +96,7 @@ GTSAM_EXPORT GraphAndValues load2D( */ GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, SharedNoiseModel model = SharedNoiseModel(), Key maxID = 0, bool addNoise = - false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatGRAPH, // + false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, // KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel