46 std::cerr <<
"USAGE: " <<
self <<
" [options]\n\n";
47 std::cerr <<
"This tool is part of OctoMap and inserts the data of a scan graph\n" 48 "file (point clouds with poses) into an octree.\n" 49 "The output is a compact maximum-likelihood binary octree file \n" 50 "(.bt, bonsai tree) and general octree files (.ot) with the full\n" 54 std::cerr <<
"OPTIONS:\n -i <InputFile.graph> (required)\n" 55 " -o <OutputFile.bt> (required) \n" 56 " -res <resolution> (optional, default: 0.1 m)\n" 57 " -m <maxrange> (optional) \n" 58 " -n <max scan no.> (optional) \n" 59 " -log (enable a detailed log file with statistics) \n" 60 " -g (nodes are already in global coordinates and no transformation is required) \n" 61 " -compressML (enable maximum-likelihood compression (lossy) after every scan)\n" 62 " -simple (simple scan insertion ray by ray instead of optimized) \n" 63 " -discretize (approximate raycasting on discretized coordinates, speeds up insertion) \n" 64 " -clamping <p_min> <p_max> (override default sensor model clamping probabilities between 0..1)\n" 65 " -sensor <p_miss> <p_hit> (override default sensor model hit and miss probabilities between 0..1)" 75 unsigned int& num_thresholded,
76 unsigned int& num_other)
81 for(OcTree::tree_iterator it = tree->
begin_tree(), end=tree->
end_tree(); it!= end; ++it){
90 unsigned int numThresholded, numOther;
96 cout <<
"Tree size: " << tree->
size() <<
" nodes (" << numLeafNodes<<
" leafs). " <<numThresholded <<
" nodes thresholded, "<< numOther <<
" other\n";
97 cout <<
"Memory: " << memUsage <<
" byte (" << memUsage/(1024.*1024.) <<
" MB)" << endl;
98 cout <<
"Full grid: "<< memFullGrid <<
" byte (" << memFullGrid/(1024.*1024.) <<
" MB)" << endl;
101 cout <<
"Size: " << x <<
" x " << y <<
" x " << z <<
" m^3\n";
106 int main(
int argc,
char** argv) {
109 string graphFilename =
"";
110 string treeFilename =
"";
111 double maxrange = -1;
112 int max_scan_no = -1;
113 bool detailedLog =
false;
114 bool simpleUpdate =
false;
115 bool discretize =
false;
116 bool dontTransformNodes =
false;
117 unsigned char compression = 1;
131 while (++arg < argc) {
132 if (! strcmp(argv[arg],
"-i"))
133 graphFilename = std::string(argv[++arg]);
134 else if (!strcmp(argv[arg],
"-o"))
135 treeFilename = std::string(argv[++arg]);
136 else if (! strcmp(argv[arg],
"-res") && argc-arg < 2)
138 else if (! strcmp(argv[arg],
"-res"))
139 res = atof(argv[++arg]);
140 else if (! strcmp(argv[arg],
"-log"))
142 else if (! strcmp(argv[arg],
"-g"))
143 dontTransformNodes =
true;
144 else if (! strcmp(argv[arg],
"-simple"))
146 else if (! strcmp(argv[arg],
"-discretize"))
148 else if (! strcmp(argv[arg],
"-compress"))
149 OCTOMAP_WARNING(
"Argument -compress no longer has an effect, incremental pruning is done during each insertion.\n");
150 else if (! strcmp(argv[arg],
"-compressML"))
152 else if (! strcmp(argv[arg],
"-m"))
153 maxrange = atof(argv[++arg]);
154 else if (! strcmp(argv[arg],
"-n"))
155 max_scan_no = atoi(argv[++arg]);
156 else if (! strcmp(argv[arg],
"-clamping") && (argc-arg < 3))
158 else if (! strcmp(argv[arg],
"-clamping")){
159 clampingMin = atof(argv[++arg]);
160 clampingMax = atof(argv[++arg]);
162 else if (! strcmp(argv[arg],
"-sensor") && (argc-arg < 3))
164 else if (! strcmp(argv[arg],
"-sensor")){
165 probMiss = atof(argv[++arg]);
166 probHit = atof(argv[++arg]);
173 if (graphFilename ==
"" || treeFilename ==
"")
182 if (clampingMin >= clampingMax || clampingMin < 0.0 || clampingMax > 1.0){
183 OCTOMAP_ERROR(
"Error in clamping values: 0.0 <= [%f] < [%f] <= 1.0\n", clampingMin, clampingMax);
187 if (probMiss >= probHit || probMiss < 0.0 || probHit > 1.0){
188 OCTOMAP_ERROR(
"Error in sensor model (hit/miss prob.): 0.0 <= [%f] < [%f] <= 1.0\n", probMiss, probHit);
193 std::string treeFilenameOT = treeFilename +
".ot";
194 std::string treeFilenameMLOT = treeFilename +
"_ml.ot";
196 cout <<
"\nReading Graph file\n===========================\n";
201 size_t num_points_in_graph = 0;
202 if (max_scan_no > 0) {
203 num_points_in_graph = graph->
getNumPoints(max_scan_no-1);
204 cout <<
"\n Data points in graph up to scan " << max_scan_no <<
": " << num_points_in_graph << endl;
208 cout <<
"\n Data points in graph: " << num_points_in_graph << endl;
212 if (!dontTransformNodes) {
215 pose6d frame_origin = (*scan_it)->pose;
218 (*scan_it)->scan->transform(frame_origin);
226 std::ofstream logfile;
228 logfile.open((treeFilename+
".log").c_str());
229 logfile <<
"# Memory of processing " << graphFilename <<
" over time\n";
230 logfile <<
"# Resolution: "<< res <<
"; compression: " << int(compression) <<
"; scan endpoints: "<< num_points_in_graph << std::endl;
231 logfile <<
"# [scan number] [bytes octree] [bytes full 3D grid]\n";
236 cout <<
"\nCreating tree\n===========================\n";
245 gettimeofday(&start, NULL);
246 size_t numScans = graph->
size();
247 size_t currentScan = 1;
249 if (max_scan_no > 0) cout <<
"("<<currentScan <<
"/" << max_scan_no <<
") " << flush;
250 else cout <<
"("<<currentScan <<
"/" << numScans <<
") " << flush;
255 tree->
insertPointCloud((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange,
false, discretize);
257 if (compression == 2){
265 if ((max_scan_no > 0) && (currentScan == (
unsigned int) max_scan_no))
270 gettimeofday(&stop, NULL);
272 double time_to_insert = (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec);
276 if (logfile.is_open())
281 cout <<
"\nDone building tree.\n\n";
282 cout <<
"time to insert scans: " << time_to_insert <<
" sec" << endl;
283 cout <<
"time to insert 100.000 points took: " << time_to_insert/ ((double) num_points_in_graph / 100000) <<
" sec (avg)" << endl << endl;
286 std::cout <<
"Pruned tree (lossless compression)\n" <<
"===========================\n";
289 tree->
write(treeFilenameOT);
291 std::cout <<
"Pruned max-likelihood tree (lossy compression)\n" <<
"===========================\n";
297 cout <<
"\nWriting tree files\n===========================\n";
298 tree->
write(treeFilenameMLOT);
299 std::cout <<
"Full Octree (pruned) written to "<< treeFilenameOT << std::endl;
300 std::cout <<
"Full Octree (max.likelihood, pruned) written to "<< treeFilenameMLOT << std::endl;
302 std::cout <<
"Bonsai tree written to "<< treeFilename << std::endl;
unsigned long long memoryFullGrid() const
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
double getClampingThresMin() const
virtual void getMetricSize(double &x, double &y, double &z)
Size of OcTree (all known space) in meters for x, y and z dimension.
const tree_iterator end_tree() const
double getProbHit() const
std::istream & readBinary(std::ifstream &s)
virtual size_t memoryUsage() const
Vector3 transform(const Vector3 &v) const
Transformation of a vector.
void setClampingThresMax(double thresProb)
sets the maximum threshold for occupancy clamping (sensor model)
void setProbHit(double prob)
sets the probability for a "hit" (will be converted to logodds) - sensor model
double getClampingThresMax() const
virtual void insertPointCloud(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false, bool discretize=false)
std::vector< ScanNode * >::iterator iterator
Pose6D inv() const
Inversion.
void outputStatistics(const OcTree *tree)
void setProbMiss(double prob)
sets the probability for a "miss" (will be converted to logodds) - sensor model
bool isNodeAtThreshold(const OcTreeNode *occupancyNode) const
queries whether a node is at the clamping threshold according to the tree's parameter ...
void printUsage(char *self)
bool writeBinary(const std::string &filename)
virtual void insertPointCloudRays(const Pointcloud &scan, const point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false)
bool write(const std::string &filename) const
Write file header and complete tree to file (serialization)
This class represents a tree-dimensional pose of an object.
#define OCTOMAP_WARNING(...)
This class represents a three-dimensional vector.
virtual void toMaxLikelihood()
tree_iterator begin_tree(unsigned char maxDepth=0) const
int main(int argc, char **argv)
double getProbMiss() const
void calcThresholdedNodes(const OcTree *tree, unsigned int &num_thresholded, unsigned int &num_other)
octomath::Pose6D pose6d
Use our Pose6D (float precision) as pose6d in octomap.
#define OCTOMAP_ERROR(...)
size_t getNumPoints(unsigned int max_id=-1) const
virtual size_t size() const
This class represents a Quaternion.
void setClampingThresMin(double thresProb)
sets the minimum threshold for occupancy clamping (sensor model)