normals_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 <vector>
35 #include <string>
36 
37 #include <octomap/octomap.h>
38 #include <octomap/OcTree.h>
39 
40 using namespace std;
41 using namespace octomap;
42 
43 
44 void print_query_info(point3d query, OcTreeNode* node) {
45  if (node != NULL) {
46  cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << endl;
47  }
48  else
49  cout << "occupancy probability at " << query << ":\t is unknown" << endl;
50 }
51 
52 int main(int argc, char** argv) {
53 
54  cout << endl;
55  cout << "generating example map" << endl;
56 
57  OcTree tree (0.1); // create empty tree with resolution 0.1
58 
59 
60  // insert some measurements of occupied cells
61 
62  for (int x=-20; x<20; x++) {
63  for (int y=-20; y<20; y++) {
64  for (int z=-20; z<20; z++) {
65  point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
66  tree.updateNode(endpoint, true); // integrate 'occupied' measurement
67  }
68  }
69  }
70 
71  // insert some measurements of free cells
72 
73  for (int x=-30; x<30; x++) {
74  for (int y=-30; y<30; y++) {
75  for (int z=-30; z<30; z++) {
76  point3d endpoint ((float) x*0.02f-1.0f, (float) y*0.02f-1.0f, (float) z*0.02f-1.0f);
77  tree.updateNode(endpoint, false); // integrate 'free' measurement
78  }
79  }
80  }
81 
82  cout << endl;
83  cout << "performing some queries around the desired voxel:" << endl;
84 
85  point3d query;
86  OcTreeNode* result = NULL;
87 
88  for(float z = -0.6f; z < -0.21f; z += 0.1f){
89  for(float y = -0.6f; y < -0.21f; y += 0.1f){
90  for(float x = -0.6f; x < -0.21f; x += 0.1f){
91  query = point3d(x, y, z);
92  result = tree.search(query);
93  print_query_info(query, result);
94  }
95  }
96  }
97 
98  query = point3d(-0.5f, -0.4f, -0.4f);
99  result = tree.search(query);
100 
101  vector<point3d> normals;
102  if (tree.getNormals(query, normals)){
103 
104  cout << endl;
105  string s_norm = (normals.size() > 1) ? " normals " : " normal ";
106  cout << "MC algorithm gives " << normals.size() << s_norm << "in voxel at " << query << endl;
107  for(unsigned i = 0; i < normals.size(); ++i)
108  cout << "\t" << normals[i].x() << "; " << normals[i].y() << "; " << normals[i].z() << endl;
109  } else{
110  cout << "query point unknown (no normals)\n";
111  }
112 }
double getOccupancy() const
Definition: OcTreeNode.h:65
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
void print_query_info(point3d query, OcTreeNode *node)
NODE * search(double x, double y, double z, unsigned int depth=0) const
float & y()
Definition: Vector3.h:136
bool getNormals(const point3d &point, std::vector< point3d > &normals, bool unknownStatus=true) const
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