|
| 1 | +#include <iostream> |
| 2 | +#include "opencv2/opencv.hpp" |
| 3 | + |
| 4 | +#ifdef _WIN32 |
| 5 | +#include <windows.h> |
| 6 | +#endif // _WIN32 |
| 7 | + |
| 8 | +#ifdef __linux__ |
| 9 | +#include <boost/filesystem.hpp> |
| 10 | +#endif // __linux__ |
| 11 | + |
| 12 | +using namespace cv; |
| 13 | +using namespace std; |
| 14 | + |
| 15 | +class IMUData { |
| 16 | +public: |
| 17 | +IMUData(const double &gx, const double &gy, const double &gz, |
| 18 | +const double &ax, const double &ay, const double &az, |
| 19 | +const double &t) : _g(gx, gy, gz), _a(ax, ay, az), _t(t) {} |
| 20 | + |
| 21 | +string toString() |
| 22 | +{ |
| 23 | +string ret; |
| 24 | + |
| 25 | +ret += to_string(long long(_t * 1e6) * 1000) + ','; |
| 26 | +ret += to_string(_g[0]) + ','; |
| 27 | +ret += to_string(_g[1]) + ','; |
| 28 | +ret += to_string(_g[2]) + ','; |
| 29 | +ret += to_string(_a[0]) + ','; |
| 30 | +ret += to_string(_a[1]) + ','; |
| 31 | +ret += to_string(_a[2]); |
| 32 | + |
| 33 | +return ret; |
| 34 | +} |
| 35 | + |
| 36 | +string toStringM() |
| 37 | +{ |
| 38 | +string ret; |
| 39 | + |
| 40 | +ret += to_string(long long(_t * 1e6) * 1000) + ','; |
| 41 | +ret += to_string(_g[0]) + ','; |
| 42 | +ret += to_string(_g[1]) + ','; |
| 43 | +ret += to_string(_g[2]) + ','; |
| 44 | +ret += to_string(-_a[0]) + ','; |
| 45 | +ret += to_string(-_a[1]) + ','; |
| 46 | +ret += to_string(-_a[2]); |
| 47 | + |
| 48 | +return ret; |
| 49 | +} |
| 50 | + |
| 51 | +// Raw data of imu |
| 52 | +Vec3d _g; //gyr data |
| 53 | +Vec3d _a; //acc data |
| 54 | +double _t; //timestamp |
| 55 | +}; |
| 56 | + |
| 57 | +void LoadFrameNames(const string &strFramesFile, vector<long long> &vFrames); |
| 58 | +bool LoadImus2(const string &strAccelGyroFile, vector<IMUData> &vImus); |
| 59 | + |
| 60 | +const char* keys = |
| 61 | +"{ help h | | Print help message. }" |
| 62 | +"{ videoName | D:/Datasets/ARCNA/2019-02-08T14-26-03/Frames.m4v | Video File Name. }"; |
| 63 | + |
| 64 | +int main(int argc, char* argv[]) |
| 65 | +{ |
| 66 | +CommandLineParser parser(argc, argv, keys); |
| 67 | +string videoName = parser.get<String>("videoName"); |
| 68 | + |
| 69 | +VideoCapture cap(videoName); |
| 70 | + |
| 71 | +if (!cap.isOpened()) |
| 72 | +{ |
| 73 | +cerr << "Error when reading video"; |
| 74 | +return -1; |
| 75 | +} |
| 76 | + |
| 77 | +string workDir = videoName; |
| 78 | +workDir.resize(workDir.size() - 10); |
| 79 | + |
| 80 | +#ifdef _WIN32 |
| 81 | +CreateDirectoryA((workDir + "cam0").c_str(), NULL); |
| 82 | +#endif // _WIN32 |
| 83 | + |
| 84 | +#ifdef __linux__ |
| 85 | +boost::filesystem::create_directories((workDir + "cam0").c_str()); |
| 86 | +#endif // __linux__ |
| 87 | + |
| 88 | +string framesFileName = videoName; |
| 89 | +auto cb = framesFileName.end(); |
| 90 | +*(--cb) = 't'; |
| 91 | +*(--cb) = 'x'; |
| 92 | +*(--cb) = 't'; |
| 93 | + |
| 94 | +vector<long long> vFrames; |
| 95 | +LoadFrameNames(framesFileName, vFrames); |
| 96 | +int nImages = vFrames.size(); |
| 97 | + |
| 98 | +cout << "Image converting: [0 - 100%]" << endl; |
| 99 | +cout << "[ ]" << endl; |
| 100 | +cout << " "; |
| 101 | + |
| 102 | +Mat frame; |
| 103 | +for (int i = 0, n = 1; i < nImages; ++i) |
| 104 | +{ |
| 105 | +if (!cap.read(frame)) |
| 106 | +break; |
| 107 | + |
| 108 | +string imgName = workDir + "cam0/" + to_string(vFrames[i]) + ".jpg"; |
| 109 | +imwrite(imgName, frame); |
| 110 | + |
| 111 | +if (i + 1 >= (nImages / 100.) * n) |
| 112 | +{ |
| 113 | +cout << "."; |
| 114 | +++n; |
| 115 | +} |
| 116 | +} |
| 117 | + |
| 118 | +cout << " " << endl; |
| 119 | + |
| 120 | +//----------------------------------------------------------------------------- |
| 121 | +vector<IMUData> vImus; |
| 122 | +string accGyroFile = workDir + "accel-gyro.txt"; |
| 123 | +if(LoadImus2(accGyroFile, vImus)) |
| 124 | +{ |
| 125 | +int nImus = vImus.size(); |
| 126 | + |
| 127 | +string newImuName = workDir; |
| 128 | +newImuName += "imu0.csv"; |
| 129 | +ofstream fOutImu; |
| 130 | +fOutImu.open(newImuName.c_str(), std::ofstream::out | std::ofstream::trunc); |
| 131 | +for (int ni = 0; ni < nImus; ni++) |
| 132 | +{ |
| 133 | +fOutImu << vImus[ni].toString() << "\n"; |
| 134 | +} |
| 135 | +fOutImu.close(); |
| 136 | +} |
| 137 | +//----------------------------------------------------------------------------- |
| 138 | + |
| 139 | +return 0; |
| 140 | +} |
| 141 | +void LoadFrameNames(const string &strFramesFile, vector<long long> &vFrames) |
| 142 | +{ |
| 143 | +ifstream fFrames; |
| 144 | +fFrames.open(strFramesFile.c_str()); |
| 145 | + |
| 146 | +vFrames.reserve(10000); |
| 147 | + |
| 148 | +while (!fFrames.eof()) |
| 149 | +{ |
| 150 | +string s; |
| 151 | +getline(fFrames, s); |
| 152 | +if (!s.empty()) |
| 153 | +{ |
| 154 | +stringstream ss; |
| 155 | +ss << s; |
| 156 | + |
| 157 | +string s2; |
| 158 | +getline(ss, s2, ','); |
| 159 | + |
| 160 | +double t = std::stod(s2); |
| 161 | +long long it = (long long)(t * 1e6); |
| 162 | +it = it * 1000; |
| 163 | +vFrames.push_back(it); |
| 164 | +} |
| 165 | +} |
| 166 | + |
| 167 | +fFrames.close(); |
| 168 | +} |
| 169 | + |
| 170 | +bool LoadImus2(const string &strAccelGyroFile, vector<IMUData> &vImus) |
| 171 | +{ |
| 172 | +ifstream fImus(strAccelGyroFile.c_str()); |
| 173 | +if (!fImus) |
| 174 | +return false; |
| 175 | +vImus.reserve(40000); |
| 176 | +while (!fImus.eof()) |
| 177 | +{ |
| 178 | +string s; |
| 179 | +getline(fImus, s); |
| 180 | +if (!s.empty()) |
| 181 | +{ |
| 182 | +char c = s.at(0); |
| 183 | +if (c<'0' || c>'9') |
| 184 | +continue; |
| 185 | + |
| 186 | +stringstream ss; |
| 187 | +ss << s; |
| 188 | +double tmpd; |
| 189 | +int cnt = 0; |
| 190 | +double data[10]; // timestamp, wx,wy,wz, ax,ay,az |
| 191 | +while (ss >> tmpd) |
| 192 | +{ |
| 193 | +data[cnt] = tmpd; |
| 194 | +cnt++; |
| 195 | +if (cnt == 7) |
| 196 | +break; |
| 197 | +if (ss.peek() == ',' || ss.peek() == ' ') |
| 198 | +ss.ignore(); |
| 199 | +} |
| 200 | +//data[0] *= 1e-9; |
| 201 | +IMUData imudata(data[1], data[2], data[3], |
| 202 | +data[4], data[5], data[6], data[0]); |
| 203 | +vImus.push_back(imudata); |
| 204 | +} |
| 205 | +} |
| 206 | +return true; |
| 207 | +} |
0 commit comments