graph2tree.cpp
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * http://octomap.github.com/
4  *
5  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
6  * All rights reserved.
7  * License: New BSD
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above copyright
15  * notice, this list of conditions and the following disclaimer in the
16  * documentation and/or other materials provided with the distribution.
17  * * Neither the name of the University of Freiburg nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 #include <string.h>
35 #include <stdlib.h>
36 #include <iostream>
37 #include <fstream>
38 
39 #include <octomap/octomap.h>
40 #include <octomap/octomap_timing.h>
41 
42 using namespace std;
43 using namespace octomap;
44 
45 void printUsage(char* self){
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"
51  "information.\n\n";
52 
53 
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)"
66  "\n";
67 
68 
69 
70 
71  exit(0);
72 }
73 
74 void calcThresholdedNodes(const OcTree* tree,
75  unsigned int& num_thresholded,
76  unsigned int& num_other)
77 {
78  num_thresholded = 0;
79  num_other = 0;
80 
81  for(OcTree::tree_iterator it = tree->begin_tree(), end=tree->end_tree(); it!= end; ++it){
82  if (tree->isNodeAtThreshold(*it))
83  num_thresholded++;
84  else
85  num_other++;
86  }
87 }
88 
89 void outputStatistics(const OcTree* tree){
90  unsigned int numThresholded, numOther;
91  calcThresholdedNodes(tree, numThresholded, numOther);
92  size_t memUsage = tree->memoryUsage();
93  unsigned long long memFullGrid = tree->memoryFullGrid();
94  size_t numLeafNodes = tree->getNumLeafNodes();
95 
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;
99  double x, y, z;
100  tree->getMetricSize(x, y, z);
101  cout << "Size: " << x << " x " << y << " x " << z << " m^3\n";
102  cout << endl;
103 }
104 
105 
106 int main(int argc, char** argv) {
107  // default values:
108  double res = 0.1;
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;
118 
119  // get default sensor model values:
120  OcTree emptyTree(0.1);
121  double clampingMin = emptyTree.getClampingThresMin();
122  double clampingMax = emptyTree.getClampingThresMax();
123  double probMiss = emptyTree.getProbMiss();
124  double probHit = emptyTree.getProbHit();
125 
126 
127  timeval start;
128  timeval stop;
129 
130  int arg = 0;
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)
137  printUsage(argv[0]);
138  else if (! strcmp(argv[arg], "-res"))
139  res = atof(argv[++arg]);
140  else if (! strcmp(argv[arg], "-log"))
141  detailedLog = true;
142  else if (! strcmp(argv[arg], "-g"))
143  dontTransformNodes = true;
144  else if (! strcmp(argv[arg], "-simple"))
145  simpleUpdate = true;
146  else if (! strcmp(argv[arg], "-discretize"))
147  discretize = true;
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"))
151  compression = 2;
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))
157  printUsage(argv[0]);
158  else if (! strcmp(argv[arg], "-clamping")){
159  clampingMin = atof(argv[++arg]);
160  clampingMax = atof(argv[++arg]);
161  }
162  else if (! strcmp(argv[arg], "-sensor") && (argc-arg < 3))
163  printUsage(argv[0]);
164  else if (! strcmp(argv[arg], "-sensor")){
165  probMiss = atof(argv[++arg]);
166  probHit = atof(argv[++arg]);
167  }
168  else {
169  printUsage(argv[0]);
170  }
171  }
172 
173  if (graphFilename == "" || treeFilename == "")
174  printUsage(argv[0]);
175 
176  // verify input:
177  if (res <= 0.0){
178  OCTOMAP_ERROR("Resolution must be positive");
179  exit(1);
180  }
181 
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);
184  exit(1);
185  }
186 
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);
189  exit(1);
190  }
191 
192 
193  std::string treeFilenameOT = treeFilename + ".ot";
194  std::string treeFilenameMLOT = treeFilename + "_ml.ot";
195 
196  cout << "\nReading Graph file\n===========================\n";
197  ScanGraph* graph = new ScanGraph();
198  if (!graph->readBinary(graphFilename))
199  exit(2);
200 
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;
205  }
206  else {
207  num_points_in_graph = graph->getNumPoints();
208  cout << "\n Data points in graph: " << num_points_in_graph << endl;
209  }
210 
211  // transform pointclouds first, so we can directly operate on them later
212  if (!dontTransformNodes) {
213  for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
214 
215  pose6d frame_origin = (*scan_it)->pose;
216  point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans());
217 
218  (*scan_it)->scan->transform(frame_origin);
219  point3d transformed_sensor_origin = frame_origin.transform(sensor_origin);
220  (*scan_it)->pose = pose6d(transformed_sensor_origin, octomath::Quaternion());
221 
222  }
223  }
224 
225 
226  std::ofstream logfile;
227  if (detailedLog){
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";
232  }
233 
234 
235 
236  cout << "\nCreating tree\n===========================\n";
237  OcTree* tree = new OcTree(res);
238 
239  tree->setClampingThresMin(clampingMin);
240  tree->setClampingThresMax(clampingMax);
241  tree->setProbHit(probHit);
242  tree->setProbMiss(probMiss);
243 
244 
245  gettimeofday(&start, NULL); // start timer
246  size_t numScans = graph->size();
247  size_t currentScan = 1;
248  for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
249  if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
250  else cout << "("<<currentScan << "/" << numScans << ") " << flush;
251 
252  if (simpleUpdate)
253  tree->insertPointCloudRays((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange);
254  else
255  tree->insertPointCloud((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange, false, discretize);
256 
257  if (compression == 2){
258  tree->toMaxLikelihood();
259  tree->prune();
260  }
261 
262  if (detailedLog)
263  logfile << currentScan << " " << tree->memoryUsage() << " " << tree->memoryFullGrid() << "\n";
264 
265  if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
266  break;
267 
268  currentScan++;
269  }
270  gettimeofday(&stop, NULL); // stop timer
271 
272  double time_to_insert = (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec);
273 
274  // get rid of graph in mem before doing anything fancy with tree (=> memory)
275  delete graph;
276  if (logfile.is_open())
277  logfile.close();
278 
279 
280 
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;
284 
285 
286  std::cout << "Pruned tree (lossless compression)\n" << "===========================\n";
287  outputStatistics(tree);
288 
289  tree->write(treeFilenameOT);
290 
291  std::cout << "Pruned max-likelihood tree (lossy compression)\n" << "===========================\n";
292  tree->toMaxLikelihood();
293  tree->prune();
294  outputStatistics(tree);
295 
296 
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;
301  tree->writeBinary(treeFilename);
302  std::cout << "Bonsai tree written to "<< treeFilename << std::endl;
303  cout << endl;
304 
305  delete tree;
306 
307  return 0;
308 }
tree_iterator begin_tree(unsigned char maxDepth=0) const
const tree_iterator end_tree() const
Vector3 transform(const Vector3 &v) const
Transformation of a vector.
Definition: Pose6D.cpp:80
iterator begin()
Definition: ScanGraph.h:175
bool isNodeAtThreshold(const OcTreeNode *occupancyNode) const
queries whether a node is at the clamping threshold according to the tree&#39;s parameter ...
size_t size() const
Definition: ScanGraph.h:180
virtual void getMetricSize(double &x, double &y, double &z)
Size of OcTree (all known space) in meters for x, y and z dimension.
std::istream & readBinary(std::ifstream &s)
Definition: ScanGraph.cpp:369
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
unsigned long long memoryFullGrid() 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
Definition: ScanGraph.h:173
iterator end()
Definition: ScanGraph.h:176
void outputStatistics(const OcTree *tree)
Definition: graph2tree.cpp:89
void setProbMiss(double prob)
sets the probability for a "miss" (will be converted to logodds) - sensor model
virtual size_t memoryUsage() const
void printUsage(char *self)
Definition: graph2tree.cpp:45
size_t getNumPoints(unsigned int max_id=-1) const
Definition: ScanGraph.cpp:611
bool writeBinary(const std::string &filename)
virtual void insertPointCloudRays(const Pointcloud &scan, const point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false)
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:49
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
#define OCTOMAP_WARNING(...)
Definition: octomap_types.h:76
This class represents a three-dimensional vector.
Definition: Vector3.h:50
virtual void toMaxLikelihood()
int main(int argc, char **argv)
Definition: graph2tree.cpp:106
void calcThresholdedNodes(const OcTree *tree, unsigned int &num_thresholded, unsigned int &num_other)
Definition: graph2tree.cpp:74
virtual size_t size() const
octomath::Pose6D pose6d
Use our Pose6D (float precision) as pose6d in octomap.
Definition: octomap_types.h:51
bool write(const std::string &filename) const
Write file header and complete tree to file (serialization)
#define OCTOMAP_ERROR(...)
Definition: octomap_types.h:78
virtual void prune()
Pose6D inv() const
Inversion.
Definition: Pose6D.cpp:65
This class represents a Quaternion.
Definition: Quaternion.h:56
void setClampingThresMin(double thresProb)
sets the minimum threshold for occupancy clamping (sensor model)


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Feb 28 2022 22:58:06