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

Changed default NoiseFormat to NoiseFormatAUTO which tries to guess the noise format.

release/4.3a0
Siddharth Choudhary 2014-10-22 17:49:18 -04:00
parent 10435794ed
commit 59db3b72aa
2 changed files with 22 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/slam/dataset.cpp
View File

@ -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

7
gtsam/slam/dataset.h
View File

@ -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<std::string, SharedNoiseModel> 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