47 using std::stringstream;
49 #include <boost/filesystem.hpp>
64 boost::filesystem::path directory(dir);
65 if(is_directory(directory))
74 boost::filesystem::directory_iterator lastFile;
77 for(boost::filesystem::directory_iterator it(directory); it != lastFile; it++ )
79 boost::filesystem::path
p = it->path();
80 if(
p.extension().string() ==
".3d")
84 if(sscanf(
p.filename().string().c_str(),
"scan%3d", &num))
87 if(firstScan == -1) firstScan = num;
88 if(lastScan == -1) lastScan = num;
90 if(num > lastScan) lastScan = num;
91 if(num < firstScan) firstScan = num;
107 if(
m_lastScan >= -1 && m_lastScan <= lastScan && m_lastScan >= firstScan)
115 cout <<
timestamp <<
"Reading " << n3dFiles <<
" scans in UOS format "
116 <<
"(From " << firstScan <<
" to " << lastScan <<
")." << endl;
123 for(boost::filesystem::directory_iterator it(directory); it != lastFile; it++ )
125 boost::filesystem::path
p = it->path();
129 if(sscanf(
p.filename().string().c_str(),
"%d", &num))
131 if(firstScan == -1) firstScan = num;
132 if(lastScan == -1) lastScan = num;
135 if(num > lastScan) lastScan = num;
136 if(num < firstScan) firstScan = num;
151 if(
m_lastScan >= -1 && m_lastScan <= lastScan && m_lastScan >= firstScan)
159 cout <<
timestamp <<
"Reading " << nDirs <<
" scans in old UOS format "
160 <<
"(From " << firstScan <<
" to " << lastScan <<
")." << endl;
172 cout <<
timestamp <<
"UOSReader: " << dir <<
" is not a directory." << endl;
186 cout <<
timestamp <<
"UOSReader: " << dir <<
" unable to open " << target <<
" for writing." << endl;
203 list<Vec > allPoints;
204 list<BaseVector<int> > allColors;
206 size_t point_counter = 0;
208 vector<indexPair> sub_clouds;
211 size_t numPointsTotal = 0;
212 for(
int fileCounter =
first; fileCounter <= last; fileCounter++)
215 boost::filesystem::path scan_path(
216 boost::filesystem::path(dir) /
217 boost::filesystem::path(
"scan" +
to_string( fileCounter, 3 ) +
".3d" ) );
218 string scanFileName =
"/" + scan_path.relative_path().string();
225 size_t skipPoints = 1;
234 cout <<
timestamp <<
"Reduction mode. Writing every " << skipPoints <<
"th point." << endl;
237 for(
int fileCounter =
first; fileCounter <= last; fileCounter++)
243 ifstream scan_in, pose_in, frame_in;
246 boost::filesystem::path scan_path(
247 boost::filesystem::path(dir) /
248 boost::filesystem::path(
"scan" +
to_string( fileCounter, 3 ) +
".3d" ) );
249 string scanFileName = scan_path.string();
255 bool has_color = (num_attributes == 3) || (num_attributes == 4);
256 bool has_intensity = (num_attributes == 1) || (num_attributes == 4);
260 cout <<
timestamp <<
"Reading color information." << endl;
265 cout <<
timestamp <<
"Reading intensity information." << endl;
269 scan_in.open(scanFileName.c_str());
273 cout <<
timestamp <<
"UOS Reader: Unable to read scan " << scanFileName << endl;
281 list<Vec > tmp_points;
285 boost::filesystem::path frame_path(
286 boost::filesystem::path(dir) /
287 boost::filesystem::path(
"scan" +
to_string( fileCounter, 3 ) +
".frames" ) );
288 string frameFileName = frame_path.string();
290 frame_in.open(frameFileName.c_str());
294 boost::filesystem::path pose_path(
295 boost::filesystem::path(dir) /
296 boost::filesystem::path(
"scan" +
to_string( fileCounter, 3 ) +
".pose" ) );
297 string poseFileName = pose_path.string();
299 pose_in.open(poseFileName.c_str());
303 for(
int i = 0; i < 6; i++) pose_in >> euler[i];
305 euler[3] *= 0.017453293;
306 euler[4] *= 0.017453293;
307 euler[5] *= 0.017453293;
309 Vec position(euler[0], euler[1], euler[2]);
310 Vec angle(euler[3], euler[4], euler[5]);
316 cout <<
timestamp <<
"UOS Reader: Warning: No position information found." << endl;
332 cout <<
timestamp <<
"Processing " << scanFileName <<
" @ "
333 << euler[0] <<
" " << euler[1] <<
" " << euler[2] <<
" "
334 << euler[3] <<
" " << euler[4] <<
" " << euler[5] << endl;
338 scan_in.getline(dummy, 1024);
345 while(scan_in.good())
347 float x, y, z, rem, dummy;
352 if(has_intensity && !has_color)
354 scan_in >> x >> y >> z >> rem;
356 else if(has_intensity && has_color)
358 scan_in >> x >> y >> z >> rem >> r >> g >> b;
361 else if(has_color && !has_intensity)
363 scan_in >> x >> y >> z >> r >> g >> b;
368 scan_in >> x >> y >> z;
369 for(
int n_dummys = 0; n_dummys < num_attributes; n_dummys++) scan_in >> dummy;
378 tmp_points.push_back(point);
384 if(point_counter % skipPoints == 0)
387 m_outputFile << point[0] <<
" " << point[1] <<
" " << point[2] <<
" ";
414 if(allPoints.size() > 0)
416 firstIndex = allPoints.size();
424 list<Vec >::iterator it, it1;
425 for(it = tmp_points.begin(); it != tmp_points.end(); it++)
429 allPoints.push_back(v);
434 if(allPoints.size() > 0)
436 lastIndex = allPoints.size() - 1;
444 sub_clouds.push_back(make_pair(firstIndex, lastIndex));
451 if ( allPoints.size() )
453 cout <<
timestamp <<
"UOS Reader: Read " << allPoints.size() <<
" points." << endl;
462 numPoints = allPoints.size();
463 points =
floatArr(
new float[3 * allPoints.size()] );
464 list<Vec >::iterator p_it;
466 for( p_it = allPoints.begin(); p_it != allPoints.end(); p_it++ )
470 points[i + 1] = v[1];
471 points[i + 2] = v[2];
476 if ( allColors.size() )
478 pointColors =
ucharArr(
new unsigned char[ 3 * numPoints ] );
480 list<BaseVector<int>>::iterator c_it;
481 for(c_it = allColors.begin(); c_it != allColors.end(); c_it++)
484 pointColors[i ] = (
unsigned char) v[0];
485 pointColors[i + 1] = (
unsigned char) v[1];
486 pointColors[i + 2] = (
unsigned char) v[2];
494 model->m_pointCloud->setPointArray( points, numPoints );
496 if (allColors.size())
498 model->m_pointCloud->setColorArray(pointColors, numPoints);
502 if (sub_clouds.size())
505 for(
size_t i = 0; i < sub_clouds.size(); i++)
507 sub_clouds_array[i*2 + 0] = sub_clouds[i].first;
508 sub_clouds_array[i*2 + 1] = sub_clouds[i].second;
511 model->m_pointCloud->addIndexChannel(sub_clouds_array,
"sub_clouds", numPoints, 2);
522 list<Vec > allPoints;
523 for(
int fileCounter =
first; fileCounter <= last; fileCounter++)
526 ifstream scan_in, pose_in, frame_in;
533 boost::filesystem::path
p(
534 boost::filesystem::path(dir) /
535 boost::filesystem::path(
to_string( fileCounter, 3 ) ) /
536 boost::filesystem::path(
"position.dat" ) );
540 poseFileName =
"/" +
p.relative_path().string();
543 pose_in.open(poseFileName.c_str());
546 if (!pose_in.good())
continue;
547 cout <<
timestamp <<
"Processing Scan " << dir <<
"/" <<
to_string(fileCounter, 3) << endl;
550 for (
unsigned int i = 0; i < 6; pose_in >> euler[i++]);
553 for (
unsigned int i = 0; i < 3; i++) euler[i] = euler[i] * 0.1;
556 for (
unsigned int i = 3; i <= 5; i++) {
559 euler[i] =
rad(euler[i]);
563 for (
int i = 1; ; i++) {
566 boost::filesystem::path sfile(
567 boost::filesystem::path(dir) /
568 boost::filesystem::path(
to_string( fileCounter, 3 ) ) /
569 boost::filesystem::path(
"scan" +
to_string(i) +
".dat" ) );
570 scanFileName =
"/" + sfile.relative_path().string();
572 scan_in.open(scanFileName.c_str());
573 if (!scan_in.good()) {
580 int Nr = 0, intensity_flag = 0;
582 double current_angle;
586 scan_in.getline(firstLine, 80);
589 cNr[0] = firstLine[2];
590 cNr[1] = firstLine[3];
591 cNr[2] = firstLine[4];
596 if (firstLine[16] !=
'i') {
599 cAngle[0] = firstLine[35];
600 cAngle[1] = firstLine[36];
601 cAngle[2] = firstLine[37];
602 cAngle[3] = firstLine[38];
603 cAngle[4] = firstLine[39];
604 cAngle[5] = firstLine[40];
605 cAngle[6] = firstLine[41];
607 current_angle = atof(cAngle);
608 cout << current_angle << endl;
612 cAngle[0] = firstLine[54];
613 cAngle[1] = firstLine[55];
614 cAngle[2] = firstLine[56];
615 cAngle[3] = firstLine[57];
616 cAngle[4] = firstLine[58];
617 cAngle[5] = firstLine[59];
618 cAngle[6] = firstLine[60];
620 current_angle = atof(cAngle);
623 double cos_currentAngle = cos(
rad(current_angle));
624 double sin_currentAngle = sin(
rad(current_angle));
626 for (
int j = 0; j < Nr; j++) {
627 if (!intensity_flag) {
628 scan_in >> X >> Z >> D >> I;
637 p[1] = Z * sin_currentAngle;
638 p[2] = Z * cos_currentAngle;
650 boost::filesystem::path framePath(
651 boost::filesystem::path(dir) /
652 boost::filesystem::path(
"scan" +
to_string( fileCounter, 3 ) +
".frames" ) );
653 string frameFileName =
"/" + framePath.relative_path().string();
656 frame_in.open(frameFileName.c_str());
665 Vec position(euler[0], euler[1], euler[2]);
666 Vec angle(euler[3], euler[4], euler[5]);
671 list<Vec >::iterator it;
672 for(it = ptss.begin(); it != ptss.end(); it++)
676 allPoints.push_back(v);
684 if(allPoints.size() > 0)
686 cout <<
timestamp <<
"UOS Reader: Read " << allPoints.size() <<
" points." << endl;
687 n = allPoints.size();
688 floatArr points(
new float[3 * allPoints.size()] );
689 list<Vec >::iterator p_it;
691 for( p_it = allPoints.begin(); p_it != allPoints.end(); p_it++ )
695 points[t_index ] = v[0];
696 points[t_index + 1] = v[1];
697 points[t_index + 2] = v[2];
704 model->m_pointCloud->setPointArray( points, n );
711 while(frameFile.good())
713 for(
int i = 0; i < 16; i++ && frameFile.good()) frameFile >> m[i];