intersection_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 int main(int argc, char** argv) {
44 
45  cout << "generating example map" << endl;
46 
47  OcTree tree (0.1); // create empty tree with resolution 0.1
48 
49  // insert some measurements of free cells
50 
51  for (float x = -2; x <= 0; x += 0.02f) {
52  for (float y = -2; y <= 0; y += 0.02f) {
53  for (float z = -2; z <= 0; z += 0.02f) {
54  point3d endpoint(x, y, z);
55  tree.updateNode(endpoint, false); // integrate 'free' measurement
56  }
57  }
58  }
59 
60  // insert some measurements of occupied cells (twice as much)
61  for (float x = -1; x <= 0; x += 0.01f) {
62  for (float y = -1; y <= 0; y += 0.01f) {
63  for (float z = -1; z <= 0; z += 0.01f) {
64  point3d endpoint(x, y, z);
65  tree.updateNode(endpoint, true); // integrate 'occupied' measurement
66  }
67  }
68  }
69 
70  point3d origin(-1.5, -1.5, -0.5);
71  point3d direction;
72  point3d ray_end;
73 
74 
75  for(float z = 0; z <= 0.25; z += 0.125){
76  direction = point3d(1, 1, z);
77  cout << endl;
78  cout << "casting ray from " << origin << " in the " << direction << " direction"<< endl;
79  bool success = tree.castRay(origin, direction, ray_end);
80 
81  if(success){
82  cout << "ray hit cell with center " << ray_end << endl;
83 
84  point3d intersection;
85  success = tree.getRayIntersection(origin, direction, ray_end, intersection);
86  if(success)
87  cout << "entrance point is " << intersection << endl;
88  }
89  }
90 
91  return 0;
92 }
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
virtual bool castRay(const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
This class represents a three-dimensional vector.
Definition: Vector3.h:50
int main(int argc, char **argv)
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:48
virtual bool getRayIntersection(const point3d &origin, const point3d &direction, const point3d &center, point3d &intersection, double delta=0.0) const


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Wed Jun 5 2019 19:26:27