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

17
gtsam/slam/dataset.cpp
View File

@ -102,26 +102,28 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
double v1, v2, v3, v4, v5, v6; double v1, v2, v3, v4, v5, v6;
is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
// Read matrix and check that diagonal entries are non-zero if (noiseFormat == NoiseFormatAUTO)
Matrix M(3, 3); {
switch (noiseFormat) {
case NoiseFormatAUTO:
// Try to guess covariance matrix layout // 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) if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0)
{ {
// NoiseFormatGRAPH // NoiseFormatGRAPH
M << v1, v2, v5, v2, v3, v6, v5, v6, v4; noiseFormat = NoiseFormatGRAPH;
} }
else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0) else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0)
{ {
// NoiseFormatCOV // NoiseFormatCOV
M << v1, v2, v3, v2, v4, v5, v3, v5, v6; noiseFormat = NoiseFormatCOV;
} }
else else
{ {
throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format."); throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format.");
} }
break; }
// Read matrix and check that diagonal entries are non-zero
Matrix M(3, 3);
switch (noiseFormat) {
case NoiseFormatG2O: case NoiseFormatG2O:
case NoiseFormatCOV: case NoiseFormatCOV:
// i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] // 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 // In both cases, what is stored in file is the information matrix
model = noiseModel::Gaussian::Information(M, smart); model = noiseModel::Gaussian::Information(M, smart);
break; break;
case NoiseFormatAUTO:
case NoiseFormatGRAPH: case NoiseFormatGRAPH:
case NoiseFormatCOV: case NoiseFormatCOV:
// These cases expect covariance matrix // These cases expect covariance matrix