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;
is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
// Read matrix and check that diagonal entries are non-zero
Matrix M(3, 3);
switch (noiseFormat) {
case NoiseFormatG2O:
case NoiseFormatCOV:
// i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
if (v1 == 0.0 || v4 == 0.0 || v6 == 0.0)
throw std::invalid_argument(
"load2D: looks like this is not G2O file format");
throw std::runtime_error(
"load2D::readNoiseModel looks like this is not G2O matrix order");
M << v1, v2, v3, v2, v4, v5, v3, v5, v6;
break;
case NoiseFormatTORO:
@ -120,13 +122,15 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
// i.e., [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ]
if (v1 == 0.0 || v3 == 0.0 || v4 == 0.0)
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;
break;
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;
switch (noiseFormat) {
case NoiseFormatG2O:
@ -135,7 +139,8 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
model = noiseModel::Gaussian::Information(M, smart);
break;
case NoiseFormatGRAPH:
// but default case expects covariance matrix
case NoiseFormatCOV:
// These cases expect covariance matrix
model = noiseModel::Gaussian::Covariance(M, smart);
break;
default:

3
gtsam/slam/dataset.h
View File

@ -56,7 +56,8 @@ GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
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 !
NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix !
NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33
};
enum KernelFunctionType {