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

Moved NoiseFormatAUTO check out of switch case

release/4.3a0
Siddharth Choudhary 2014-10-22 19:00:39 -04:00
parent 59db3b72aa
commit de175ea128
1 changed files with 19 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
gtsam/slam/dataset.cpp
View File

@ -102,26 +102,28 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
double v1, v2, v3, v4, v5, v6;
is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
if (noiseFormat == 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
noiseFormat = NoiseFormatGRAPH;
}
else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0)
{
// NoiseFormatCOV
noiseFormat = NoiseFormatCOV;
}
else
{
throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format.");
}
}
// 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 ]
@ -153,7 +155,6 @@ 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