62 boost::filesystem::directory_iterator lastFile;
63 for(boost::filesystem::directory_iterator it(directory); it != lastFile; it++ )
65 boost::filesystem::path
p = it->path();
66 if(
string(
p.extension().string().c_str()) ==
".oct")
70 if(sscanf(
p.filename().string().c_str(),
"scan%3d", &num))
73 if(firstScan == -1) firstScan = num;
74 if(lastScan == -1) lastScan = num;
76 if(num > lastScan) lastScan = num;
77 if(num < firstScan) firstScan = num;
85 list<Point> allPoints;
89 for(
int i = 0; i < numScans; i++)
92 sprintf(scanfile,
"%sscan%03d.oct", directory.c_str(), firstScan + i);
94 cout <<
timestamp <<
"Reading " << scanfile << endl;
101 boost::filesystem::path frame_path(
102 boost::filesystem::path(directory) /
103 boost::filesystem::path(
"scan" + to_string( firstScan + i, 3 ) +
".frames" ) );
104 string frameFileName =
"/" + frame_path.relative_path().string();
106 ifstream frame_in(frameFileName.c_str());
110 boost::filesystem::path pose_path(
111 boost::filesystem::path(directory) /
112 boost::filesystem::path(
"scan" + to_string( firstScan + i, 3 ) +
".pose" ) );
113 string poseFileName =
"/" + pose_path.relative_path().string();
115 ifstream pose_in(poseFileName.c_str());
119 for(
int i = 0; i < 6; i++) pose_in >> euler[i];
120 Vec position(euler[0], euler[1], euler[2]);
121 Vec angle(euler[3], euler[4], euler[5]);
126 cout <<
timestamp <<
"UOS Reader: Warning: No position information found." << endl;
139 for(
int j = 0; j < points.size(); j++)
141 Vec v(points[j].x, points[j].y, points[j].z);
143 if(v.length() < 10000)
148 allPoints.push_back(points[j]);
157 cout <<
timestamp <<
"Read " << allPoints.size() <<
" points." << endl;
163 floatArr points(
new float[3 * allPoints.size()]);
165 floatArr intensities(
new float[allPoints.size()]);
168 bool found_color =
false;
171 for(list<Point>::iterator it = allPoints.begin(); it != allPoints.end(); it++)
175 points[3 * i ] =
p.x;
176 points[3 * i + 1] =
p.y;
177 points[3 * i + 2] =
p.z;
183 if(
p.rgb[0] != 255 &&
p.rgb[1] != 255 &&
p.rgb[2] != 255)
188 intensities[i] =
p.reflectance;
189 if(intensities[i] < min_r) min_r = intensities[i];
190 if(intensities[i] > max_r) max_r = intensities[i];
212 model->m_pointCloud->setPointArray(points, allPoints.size());
213 model->m_pointCloud->setColorArray(
colors, allPoints.size());
214 model->m_pointCloud->addFloatChannel(intensities,
"intensities", allPoints.size(), 1);
228 while(frameFile.good())
230 for(
int i = 0; i < 16; i++ && frameFile.good()) frameFile >> m[i];