| 1299 | } |
| 1300 | |
| 1301 | bool Tracking::ParseIMUParamFile(cv::FileStorage &fSettings) |
| 1302 | { |
| 1303 | bool b_miss_params = false; |
| 1304 | |
| 1305 | cv::Mat cvTbc; |
| 1306 | cv::FileNode node = fSettings["Tbc"]; |
| 1307 | if(!node.empty()) |
| 1308 | { |
| 1309 | cvTbc = node.mat(); |
| 1310 | if(cvTbc.rows != 4 || cvTbc.cols != 4) |
| 1311 | { |
| 1312 | std::cerr << "*Tbc matrix have to be a 4x4 transformation matrix*" << std::endl; |
| 1313 | b_miss_params = true; |
| 1314 | } |
| 1315 | } |
| 1316 | else |
| 1317 | { |
| 1318 | std::cerr << "*Tbc matrix doesn't exist*" << std::endl; |
| 1319 | b_miss_params = true; |
| 1320 | } |
| 1321 | cout << endl; |
| 1322 | cout << "Left camera to Imu Transform (Tbc): " << endl << cvTbc << endl; |
| 1323 | Eigen::Matrix<float,4,4,Eigen::RowMajor> eigTbc(cvTbc.ptr<float>(0)); |
| 1324 | Sophus::SE3f Tbc(eigTbc); |
| 1325 | |
| 1326 | node = fSettings["InsertKFsWhenLost"]; |
| 1327 | mInsertKFsLost = true; |
| 1328 | if(!node.empty() && node.isInt()) |
| 1329 | { |
| 1330 | mInsertKFsLost = (bool) node.operator int(); |
| 1331 | } |
| 1332 | |
| 1333 | if(!mInsertKFsLost) |
| 1334 | cout << "Do not insert keyframes when lost visual tracking " << endl; |
| 1335 | |
| 1336 | |
| 1337 | |
| 1338 | float Ng, Na, Ngw, Naw; |
| 1339 | |
| 1340 | node = fSettings["IMU.Frequency"]; |
| 1341 | if(!node.empty() && node.isInt()) |
| 1342 | { |
| 1343 | mImuFreq = node.operator int(); |
| 1344 | mImuPer = 0.001; //1.0 / (double) mImuFreq; |
| 1345 | } |
| 1346 | else |
| 1347 | { |
| 1348 | std::cerr << "*IMU.Frequency parameter doesn't exist or is not an integer*" << std::endl; |
| 1349 | b_miss_params = true; |
| 1350 | } |
| 1351 | |
| 1352 | node = fSettings["IMU.NoiseGyro"]; |
| 1353 | if(!node.empty() && node.isReal()) |
| 1354 | { |
| 1355 | Ng = node.real(); |
| 1356 | } |
| 1357 | else |
| 1358 | { |