35 #include <unordered_set>
41 boost::filesystem::path& transfromFile,
42 std::vector<float>& pts,
43 std::vector<float>& nrm)
45 std::cout <<
timestamp <<
"Transforming normals " << std::endl;
50 sprintf(frames,
"%s/%s.frames", transfromFile.parent_path().c_str(),
51 transfromFile.stem().c_str());
52 sprintf(pose,
"%s/%s.pose", transfromFile.parent_path().c_str(), transfromFile.stem().c_str());
54 boost::filesystem::path framesPath(frames);
55 boost::filesystem::path posePath(pose);
60 if(boost::filesystem::exists(framesPath))
62 std::cout <<
timestamp <<
"Transforming according to " << framesPath.filename() << std::endl;
63 transform = getTransformationFromFrames<double>(framesPath);
65 else if(boost::filesystem::exists(posePath))
67 std::cout <<
timestamp <<
"Transforming according to " << posePath.filename() << std::endl;
68 transform = getTransformationFromFrames<double>(posePath);
72 std::cout <<
timestamp <<
"Warning: found no transformation for "
73 << transfromFile.filename() << std::endl;
78 size_t n_points = buffer->numPoints();
80 floatArr normals = buffer->getFloatArray(
"normals", n_normals, w_normals);
81 floatArr points = buffer->getPointArray();
85 std::cout <<
timestamp <<
"Warning: width of normals is not 3" << std::endl;
88 if(n_normals != n_points)
90 std::cout <<
timestamp <<
"Warning: point and normal count mismatch" << std::endl;
94 for(
size_t i = 0; i < n_points; i++)
97 float x = points[3 * i];
98 float y = points[3 * i + 1];
99 float z = points[3 * i + 2];
108 pts.push_back(tv[0]);
109 pts.push_back(tv[1]);
110 pts.push_back(tv[2]);
114 float nx = normals[3 * i];
115 float ny = normals[3 * i + 1];
116 float nz = normals[3 * i + 2];
125 nrm.push_back(tn[0]);
126 nrm.push_back(tn[1]);
127 nrm.push_back(tn[2]);
134 std::ifstream in(inFile.c_str());
135 std::cout <<
timestamp <<
"Counting points in "
136 << inFile.filename().string() <<
"..." << std::endl;
143 in.getline(line, 1024);
148 std::cout <<
timestamp <<
"File " << inFile.filename().string()
149 <<
" contains " << n_points <<
" points." << std::endl;
156 std::ofstream o(out.c_str());
159 o << position[0] <<
" " << position[1] <<
" " << position[2] << std::endl;
160 o << angles[0] <<
" " << angles[1] <<
" " << angles[2];
166 size_t n_ip = model->m_pointCloud->numPoints();
167 floatArr arr = model->m_pointCloud->getPointArray();
176 size_t n_ip, n_colors;
179 n_ip = model->m_pointCloud->numPoints();
180 floatArr arr = model->m_pointCloud->getPointArray();
182 ucharArr colors = model->m_pointCloud->getUCharArray(
"colors", n_colors, w_colors);
184 for(
int a = 0; a < n_ip; a++)
186 out << arr[a * 3] <<
" " << arr[a * 3 + 1] <<
" " << arr[a * 3 + 2];
188 if(n_colors && !(nocolor))
190 for (
unsigned i = 0; i < w_colors; i++)
192 out <<
" " << (int)
colors[a * w_colors + i];
204 size_t n_points = model->m_pointCloud->numPoints();
205 floatArr arr = model->m_pointCloud->getPointArray();
208 std::cout <<
timestamp <<
"Point cloud contains " << n_points <<
" points." << std::endl;
218 if(reduction < n_points)
220 return (
int)n_points / reduction;
241 if(targetSize < n_points)
243 return (
int)n_points / targetSize;
265 floatArr normals(
new float[n.size()]);
267 std::cout <<
timestamp <<
"Copying buffers for output." << std::endl;
269 for(
size_t i = 0; i <
p.size(); i++)
275 buffer->setPointArray(points,
p.size() / 3);
276 buffer->setNormalArray(normals, n.size() / 3);
278 model->m_pointCloud = buffer;
280 std::cout <<
timestamp <<
"Saving " << outfile << std::endl;
282 std::cout <<
timestamp <<
"Done." << std::endl;
287 ifstream in(
file.c_str());
290 in >> position.
x >> position.
y >> position.
z;
291 in >> angles.
y >> angles.
y >> angles.
z;
298 size_t n_vertices = 0;
307 if(tag ==
"PLY" || tag ==
"ply")
311 while (in.good() && token !=
"end_header" && token !=
"END_HEADER")
317 if(token ==
"vertex" || token ==
"VERTEX")
323 if(token ==
"point" || token ==
"POINT")
328 if(n_points == 0 && n_vertices == 0)
330 std::cout <<
timestamp <<
"PLY contains neither vertices nor points." << std::endl;
357 size_t width = src.
width();
362 boost::shared_array<T> a(red->
dataPtr());
363 boost::shared_array<T> b(src.
dataPtr());
364 for(
size_t i = 0; i < ids.size(); i++)
366 for(
size_t j = 0; j < red->
width(); j++)
368 a[i * width + j] = b[ids[i] * width + j];
378 std::map<std::string, Channel<T>> channels;
379 src->getAllChannelsOfType(channels);
380 for(
auto i : channels)
382 std::cout <<
timestamp <<
"Subsampling channel " << i.first << std::endl;
384 dst->addChannel<T>(c, i.first);
394 subsample<char>(src, buffer, indices);
395 subsample<unsigned char>(src, buffer, indices);
396 subsample<short>(src, buffer, indices);
397 subsample<int>(src, buffer, indices);
398 subsample<unsigned int>(src, buffer, indices);
399 subsample<float>(src, buffer, indices);
400 subsample<double>(src, buffer, indices);
409 size_t numSrcPts = src->numPoints();
412 std::random_device dev;
413 std::mt19937 rng(dev());
414 std::uniform_int_distribution<std::mt19937::result_type> dist(0, numSrcPts);
420 std::unordered_set<size_t> index_set;
421 while(index_set.size() < n)
423 index_set.insert(dist(rng));
429 vector<size_t> indices;
430 indices.insert(indices.end(), index_set.begin(), index_set.end());
434 subsample<char>(src, buffer, indices);
435 subsample<unsigned char>(src, buffer, indices);
436 subsample<short>(src, buffer, indices);
437 subsample<int>(src, buffer, indices);
438 subsample<unsigned int>(src, buffer, indices);
439 subsample<float>(src, buffer, indices);
440 subsample<double>(src, buffer, indices);
444 std::cout <<
timestamp <<
"Sub-sampling not possible. Number of sampling points is " << std::endl;
445 std::cout <<
timestamp <<
"larger than number in src buffer. (" << n <<
" / " << numSrcPts <<
")" << std::endl;
461 #pragma omp parallel for
462 for(
size_t i = 0; i < n; i++)
464 float x = points[3 * i];
465 float y = points[3 * i + 1];
466 float z = points[3 * i + 2];
468 points[3 * i] = z / 100.0f;
469 points[3 * i + 1] = -x / 100.0f;
470 points[3 * i + 2] = y / 100.0f;
478 boost::filesystem::path directory(dir);
479 if(is_directory(directory))
482 boost::filesystem::directory_iterator lastFile;
483 std::vector<boost::filesystem::path> scan_files;
486 for(boost::filesystem::directory_iterator it(directory); it != lastFile; it++ )
488 boost::filesystem::path
p = it->path();
489 if(
p.extension().string() ==
".3d")
493 if(sscanf(
p.filename().string().c_str(),
"scan%3d", &num))
495 scan_files.push_back(
p);
500 if(scan_files.size() > 0)
502 for(
size_t i = 0; i < scan_files.size(); i++)
506 std::string
filename = (scan_files[i]).stem().string();
507 boost::filesystem::path frame_file(
filename +
".frames");
508 boost::filesystem::path pose_file(
filename +
".pose");
510 boost::filesystem::path frame_path = directory/frame_file;
511 boost::filesystem::path pose_path = directory/pose_file;
513 std::cout <<
"Loading '" <<
filename <<
"'" << std::endl;
516 scan->points = model->m_pointCloud;
518 size_t numPoints = scan->points->numPoints();
519 floatArr pts = scan->points->getPointArray();
521 for (
size_t i = 0; i < numPoints; i++)
524 scan->boundingBox.expand(pt);
527 Transformd pose_estimate = Transformd::Identity();
528 Transformd registration = Transformd::Identity();
530 if(boost::filesystem::exists(frame_path))
532 std::cout <<
timestamp <<
"Loading frame information from " << frame_path << std::endl;
533 registration = getTransformationFromFrames<double>(frame_path);
537 std::cout <<
timestamp <<
"Did not find a frame file for " <<
filename << std::endl;
540 if(boost::filesystem::exists(pose_path))
542 std::cout <<
timestamp <<
"Loading pose estimation from " << pose_path << std::endl;
543 pose_estimate = getTransformationFromPose<double>(pose_path);
547 std::cout <<
timestamp <<
"Did not find a pose file for " <<
filename << std::endl;
551 scan->registration = registration;
552 scan->poseEstimation = pose_estimate;
554 scans.push_back(scan);
559 std::cout <<
timestamp <<
"Error in parseSLAMDirectory(): '"
560 <<
"Directory does not contain any .3d files." << std::endl;
565 std::cout <<
timestamp <<
"Error in parseSLAMDirectory(): '"
566 << dir <<
"' is nor a directory." << std::endl;