eval_octree_accuracy.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 << " <InputFile.graph>\n";
47  std::cerr << "This tool is part of OctoMap and evaluates the statistical accuracy\n"
48  "of an octree map from scan graph data (point clouds with poses).\n";
49 
50  std::cerr << "OPTIONS:\n"
51  " -res <resolution> (default: 0.1 m)\n"
52  " -m <maxrange> (optional) \n"
53  " -n <max scan no.> (optional) \n"
54  "\n";
55 
56  exit(0);
57 }
58 
59 
60 int main(int argc, char** argv) {
61  // default values:
62  double res = 0.1;
63 
64  if (argc < 2)
65  printUsage(argv[0]);
66 
67  string graphFilename = std::string(argv[1]);
68 
69  double maxrange = -1;
70  int max_scan_no = -1;
71  int skip_scan_eval = 5;
72 
73  int arg = 1;
74  while (++arg < argc) {
75  if (! strcmp(argv[arg], "-i"))
76  graphFilename = std::string(argv[++arg]);
77  else if (! strcmp(argv[arg], "-res"))
78  res = atof(argv[++arg]);
79  else if (! strcmp(argv[arg], "-m"))
80  maxrange = atof(argv[++arg]);
81  else if (! strcmp(argv[arg], "-n"))
82  max_scan_no = atoi(argv[++arg]);
83  else {
84  printUsage(argv[0]);
85  }
86  }
87 
88  cout << "\nReading Graph file\n===========================\n";
89  ScanGraph* graph = new ScanGraph();
90  if (!graph->readBinary(graphFilename))
91  exit(2);
92 
93  size_t num_points_in_graph = 0;
94  if (max_scan_no > 0) {
95  num_points_in_graph = graph->getNumPoints(max_scan_no-1);
96  cout << "\n Data points in graph up to scan " << max_scan_no << ": " << num_points_in_graph << endl;
97  }
98  else {
99  num_points_in_graph = graph->getNumPoints();
100  cout << "\n Data points in graph: " << num_points_in_graph << endl;
101  }
102 
103  cout << "\nCreating tree\n===========================\n";
104  OcTree* tree = new OcTree(res);
105 
106  size_t numScans = graph->size();
107  unsigned int currentScan = 1;
108  for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
109 
110  if (currentScan % skip_scan_eval != 0){
111  if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
112  else cout << "("<<currentScan << "/" << numScans << ") " << flush;
113  tree->insertPointCloud(**scan_it, maxrange);
114  } else
115  cout << "(SKIP) " << flush;
116 
117  if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
118  break;
119 
120  currentScan++;
121  }
122 
123  tree->expand();
124 
125 
126  cout << "\nEvaluating scans\n===========================\n";
127  currentScan = 1;
128  size_t num_points = 0;
129  size_t num_voxels_correct = 0;
130  size_t num_voxels_wrong = 0;
131  size_t num_voxels_unknown = 0;
132 
133 
134  for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
135 
136  if (currentScan % skip_scan_eval == 0){
137  if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
138  else cout << "("<<currentScan << "/" << numScans << ") " << flush;
139 
140 
141  pose6d frame_origin = (*scan_it)->pose;
142  point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans());
143 
144  // transform pointcloud:
145  Pointcloud scan (*(*scan_it)->scan);
146  scan.transform(frame_origin);
147  point3d origin = frame_origin.transform(sensor_origin);
148 
149  KeySet free_cells, occupied_cells;
150  tree->computeUpdate(scan, origin, free_cells, occupied_cells, maxrange);
151 
152  num_points += scan.size();
153 
154  // count free cells
155  for (KeySet::iterator it = free_cells.begin(); it != free_cells.end(); ++it) {
156  OcTreeNode* n = tree->search(*it);
157  if (n){
158  if (tree->isNodeOccupied(n))
159  num_voxels_wrong++;
160  else
161  num_voxels_correct++;
162  } else
163  num_voxels_unknown++;
164  } // count occupied cells
165  for (KeySet::iterator it = occupied_cells.begin(); it != occupied_cells.end(); ++it) {
166  OcTreeNode* n = tree->search(*it);
167  if (n){
168  if (tree->isNodeOccupied(n))
169  num_voxels_correct++;
170  else
171  num_voxels_wrong++;
172  } else
173  num_voxels_unknown++;
174  }
175 
176 
177  }
178 
179  if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
180  break;
181 
182  currentScan++;
183 
184 
185  }
186 
187  cout << "\nFinished evaluating " << num_points <<"/"<< num_points_in_graph << " points.\n"
188  <<"Voxels correct: "<<num_voxels_correct<<" #wrong: " <<num_voxels_wrong << " #unknown: " <<num_voxels_unknown
189  <<". % correct: "<< num_voxels_correct/double(num_voxels_correct+num_voxels_wrong)<<"\n\n";
190 
191 
192  delete graph;
193  delete tree;
194 
195  return 0;
196 }
virtual void expand()
Vector3 transform(const Vector3 &v) const
Transformation of a vector.
Definition: Pose6D.cpp:80
iterator begin()
Definition: ScanGraph.h:175
size_t size() const
Definition: ScanGraph.h:180
std::istream & readBinary(std::ifstream &s)
Definition: ScanGraph.cpp:369
void transform(pose6d transform)
Apply transform to each point.
Definition: Pointcloud.cpp:99
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
size_t size() const
Definition: Pointcloud.h:57
NODE * search(double x, double y, double z, unsigned int depth=0) const
size_t getNumPoints(unsigned int max_id=-1) const
Definition: ScanGraph.cpp:611
void printUsage(char *self)
int main(int argc, char **argv)
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
Definition: OcTreeKey.h:129
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:49
This class represents a three-dimensional vector.
Definition: Vector3.h:50
void computeUpdate(const Pointcloud &scan, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree&#39;s parameter for "occupancy" ...
Pose6D inv() const
Inversion.
Definition: Pose6D.cpp:65


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