3 #include <boost/format.hpp>
10 using namespace boost::filesystem;
15 ScanDirectoryParser::~ScanDirectoryParser()
24 ScanDirectoryParser::ScanDirectoryParser(
const std::string& directory) noexcept
28 if(!exists(directory))
30 std::cout <<
timestamp <<
"Directory " << directory <<
" does not exist." << std::endl;
34 m_directory = directory;
38 m_pointExtension =
".txt";
39 m_poseExtension =
".dat";
40 m_pointPrefix =
"scan";
41 m_posePrefix =
"scan";
47 void ScanDirectoryParser::setPointCloudPrefix(
const std::string& prefix)
49 m_pointPrefix = prefix;
51 void ScanDirectoryParser::setPointCloudExtension(
const std::string& extension)
53 cout << extension << endl;
54 m_pointExtension = extension;
56 void ScanDirectoryParser::setPosePrefix(
const std::string& prefix)
58 m_posePrefix = prefix;
60 void ScanDirectoryParser::setPoseExtension(
const std::string& extension)
62 m_poseExtension = extension;
65 void ScanDirectoryParser::setStart(
int s)
69 void ScanDirectoryParser::setEnd(
int e)
75 size_t ScanDirectoryParser::examinePLY(
const std::string&
filename)
80 size_t ScanDirectoryParser::examineASCII(
const std::string&
filename)
86 PointBufferPtr ScanDirectoryParser::octreeSubSample(
const double& voxelSize,
const size_t& minPoints)
92 std::cout <<
timestamp <<
"Reading " << i->m_filename << std::endl;
93 ModelPtr model = ModelFactory::readModel(i->m_filename);
99 std::cout <<
timestamp <<
"Building octree with voxel size " << voxelSize <<
" from " << i->m_filename << std::endl;
104 std::cout <<
timestamp <<
"Transforming reduced point cloud" << std::endl;
105 out_model->m_pointCloud = reduced;
106 transformPointCloud<double>(out_model, i->m_pose);
109 std::stringstream name_stream;
110 Path p(i->m_filename);
111 name_stream <<
p.stem().string() <<
"_reduced" <<
".ply";
112 std::cout <<
timestamp <<
"Saving data to " << name_stream.str() << std::endl;
113 ModelFactory::saveModel(out_model, name_stream.str());
115 std::cout <<
timestamp <<
"Points written: " << reduced->numPoints() << std::endl;
127 size_t actual_points = 0;
129 for(
auto i : m_scans)
131 ModelPtr model = ModelFactory::readModel(i->m_filename);
142 float total_ratio = (float)i->m_numPoints / m_numPoints;
143 float target_ratio = total_ratio * tz;
146 target_size = (
int)(target_ratio + 0.5);
147 std::cout <<
timestamp <<
"Sampling " << target_size <<
" points from " << i->m_filename << std::endl;
154 std::cout <<
timestamp <<
"Using orignal points from " << i->m_filename << std::endl;
156 target_size = buffer->numPoints();
160 std::cout <<
timestamp <<
"Transforming point cloud" << std::endl;
161 out_model->m_pointCloud = reduced;
162 transformPointCloud<double>(out_model, i->m_pose);
165 std::stringstream name_stream;
166 Path p(i->m_filename);
167 name_stream <<
p.stem().string() <<
"_reduced" <<
".ply";
168 std::cout <<
timestamp <<
"Saving data to " << name_stream.str() << std::endl;
169 ModelFactory::saveModel(out_model, name_stream.str());
171 actual_points += target_size;
172 std::cout <<
timestamp <<
"Points written: " << actual_points <<
" / " << tz << std::endl;
176 return out_model->m_pointCloud;
179 void ScanDirectoryParser::parseDirectory()
181 std::cout <<
timestamp <<
"Parsing directory" << m_directory << std::endl;
182 std::cout <<
timestamp <<
"Point prefix and extension: " << m_pointPrefix <<
" " << m_pointExtension << std::endl;
183 std::cout <<
timestamp <<
"Pose prefix and extension: " << m_posePrefix <<
" " << m_poseExtension << std::endl;
186 for(
size_t i = m_start; i <= m_end; i++)
189 std::stringstream point_ss;
190 point_ss << m_pointPrefix << boost::format(
"%03d") % i << m_pointExtension;
191 std::string pointFileName = point_ss.str();
195 std::stringstream pose_ss;
196 pose_ss << m_posePrefix << boost::format(
"%03d") % i << m_poseExtension;
197 std::string poseFileName = pose_ss.str();
202 if(exists(pointPath))
204 std::cout <<
timestamp <<
"Counting points in file " << pointPath << std::endl;
205 if(pointPath.extension() ==
".3d" || pointPath.extension() ==
".txt" || pointPath.extension() ==
".pts")
207 n = examineASCII(pointPath.string());
209 else if(pointPath.extension() ==
".ply")
211 n = examinePLY(pointPath.string());
217 std::cout <<
timestamp <<
"Point cloud file " << pointPath <<
" does not exist." << std::endl;
225 matrix = getTransformationFromFile<double>(posePath.string());
226 std::cout <<
timestamp <<
"Found transformation: " << posePath <<
" @ " << std::endl << matrix << std::endl;
230 std::cout <<
timestamp <<
"Scan pose file " << posePath <<
"does not exist. Will not transfrom." << std::endl;
236 info->
m_filename = std::string(pointPath.string());
240 m_scans.push_back(info);
242 std::cout <<
timestamp <<
"Finished parsing. Directory contains " << m_scans.size() <<
" scans with " << m_numPoints <<
" points." << std::endl;