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

Fourth case: sensible order, but covariance matrix

release/4.3a0
dellaert 2014-05-31 22:42:14 -04:00
parent 9ea155dee2
commit 3e532a5160
2 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/slam/dataset.cpp
View File

@ -104,13 +104,15 @@ 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
Matrix M(3, 3); Matrix M(3, 3);
switch (noiseFormat) { switch (noiseFormat) {
case NoiseFormatG2O: case NoiseFormatG2O:
case NoiseFormatCOV:
// i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ] // i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
if (v1 == 0.0 || v4 == 0.0 || v6 == 0.0) if (v1 == 0.0 || v4 == 0.0 || v6 == 0.0)
throw std::invalid_argument( throw std::runtime_error(
"load2D: looks like this is not G2O file format"); "load2D::readNoiseModel looks like this is not G2O matrix order");
M << v1, v2, v3, v2, v4, v5, v3, v5, v6; M << v1, v2, v3, v2, v4, v5, v3, v5, v6;
break; break;
case NoiseFormatTORO: case NoiseFormatTORO:
@ -120,13 +122,15 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
// i.e., [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ] // i.e., [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ]
if (v1 == 0.0 || v3 == 0.0 || v4 == 0.0) if (v1 == 0.0 || v3 == 0.0 || v4 == 0.0)
throw std::invalid_argument( throw std::invalid_argument(
"load2D: looks like this is not TORO file format"); "load2D::readNoiseModel looks like this is not TORO matrix order");
M << v1, v2, v5, v2, v3, v6, v5, v6, v4; M << v1, v2, v5, v2, v3, v6, v5, v6, v4;
break; break;
default: default:
throw std::invalid_argument("load2D: invalid noise format"); throw std::runtime_error("load2D: invalid noise format");
} }
// Now, create a Gaussian noise model
// The smart flag will try to detect a simpler model, e.g., unit
SharedNoiseModel model; SharedNoiseModel model;
switch (noiseFormat) { switch (noiseFormat) {
case NoiseFormatG2O: case NoiseFormatG2O:
@ -135,7 +139,8 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
model = noiseModel::Gaussian::Information(M, smart); model = noiseModel::Gaussian::Information(M, smart);
break; break;
case NoiseFormatGRAPH: case NoiseFormatGRAPH:
// but default case expects covariance matrix case NoiseFormatCOV:
// These cases expect covariance matrix
model = noiseModel::Gaussian::Covariance(M, smart); model = noiseModel::Gaussian::Covariance(M, smart);
break; break;
default: default:

3
gtsam/slam/dataset.h
View File

@ -56,7 +56,8 @@ GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
enum NoiseFormat { enum NoiseFormat {
NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33 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 NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
NoiseFormatGRAPH ///< default: toro-style order, but covariance matrix ! NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix !
NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33
}; };
enum KernelFunctionType { enum KernelFunctionType {