void GroundTruthOdometry::loadTrajectory(const std::string & filename) { std::ifstream file; std::string line; file.open(filename.c_str()); while (!file.eof()) { unsigned long long int utime; float x, y, z, qx, qy, qz, qw; std::getline(file, line); int n = sscanf(line.c_str(), "%llu,%f,%f,%f,%f,%f,%f,%f", &utime, &x, &y, &z, &qx, &qy, &qz, &qw); if(file.eof()) break; assert(n == 8); Eigen::Quaternionf q(qw, qx, qy, qz); Eigen::Vector3f t(x, y, z); Eigen::Isometry3f T; T.setIdentity(); T.pretranslate(t).rotate(q); camera_trajectory[utime] = T; } }