simple_example.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 <octomap/octomap.h>
35 #include <octomap/OcTree.h>
36 
37 using namespace std;
38 using namespace octomap;
39 
40 
41 void print_query_info(point3d query, OcTreeNode* node) {
42  if (node != NULL) {
43  cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << endl;
44  }
45  else
46  cout << "occupancy probability at " << query << ":\t is unknown" << endl;
47 }
48 
49 int main(int argc, char** argv) {
50 
51  cout << endl;
52  cout << "generating example map" << endl;
53 
54  OcTree tree (0.1); // create empty tree with resolution 0.1
55 
56 
57  // insert some measurements of occupied cells
58 
59  for (int x=-20; x<20; x++) {
60  for (int y=-20; y<20; y++) {
61  for (int z=-20; z<20; z++) {
62  point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
63  tree.updateNode(endpoint, true); // integrate 'occupied' measurement
64  }
65  }
66  }
67 
68  // insert some measurements of free cells
69 
70  for (int x=-30; x<30; x++) {
71  for (int y=-30; y<30; y++) {
72  for (int z=-30; z<30; z++) {
73  point3d endpoint ((float) x*0.02f-1.0f, (float) y*0.02f-1.0f, (float) z*0.02f-1.0f);
74  tree.updateNode(endpoint, false); // integrate 'free' measurement
75  }
76  }
77  }
78 
79  cout << endl;
80  cout << "performing some queries:" << endl;
81 
82  point3d query (0., 0., 0.);
83  OcTreeNode* result = tree.search (query);
84  print_query_info(query, result);
85 
86  query = point3d(-1.,-1.,-1.);
87  result = tree.search (query);
88  print_query_info(query, result);
89 
90  query = point3d(1.,1.,1.);
91  result = tree.search (query);
92  print_query_info(query, result);
93 
94 
95  cout << endl;
96  tree.writeBinary("simple_tree.bt");
97  cout << "wrote example file simple_tree.bt" << endl << endl;
98  cout << "now you can use octovis to visualize: octovis simple_tree.bt" << endl;
99  cout << "Hint: hit 'F'-key in viewer to see the freespace" << endl << endl;
100 
101 }
double getOccupancy() const
Definition: OcTreeNode.h:65
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
NODE * search(double x, double y, double z, unsigned int depth=0) const
void print_query_info(point3d query, OcTreeNode *node)
bool writeBinary(const std::string &filename)
This class represents a three-dimensional vector.
Definition: Vector3.h:50
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:49
int main(int argc, char **argv)


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