octomap  1.9.7
MapNode.hxx
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * https://octomap.github.io/
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 namespace octomap {
35 
36  template <class TREETYPE>
37  MapNode<TREETYPE>::MapNode(): node_map(0) {
38  }
39 
40  template <class TREETYPE>
41  MapNode<TREETYPE>::MapNode(TREETYPE* in_node_map, pose6d in_origin) {
42  this->node_map = in_node_map;
43  this->origin = in_origin;
44  }
45 
46  template <class TREETYPE>
47  MapNode<TREETYPE>::MapNode(const Pointcloud& in_cloud, pose6d in_origin): node_map(0) {
48  }
49 
50  template <class TREETYPE>
51  MapNode<TREETYPE>::MapNode(std::string filename, pose6d in_origin): node_map(0){
52  readMap(filename);
53  this->origin = in_origin;
54  id = filename;
55  }
56 
57  template <class TREETYPE>
59  clear();
60  }
61 
62  template <class TREETYPE>
63  void MapNode<TREETYPE>::updateMap(const Pointcloud& cloud, point3d sensor_origin) {
64  }
65 
66  template <class TREETYPE>
68  Pointcloud pc;
69  point3d_list occs;
70  node_map->getOccupied(occs);
71  for(point3d_list::iterator it = occs.begin(); it != occs.end(); ++it){
72  pc.push_back(*it);
73  }
74  return pc;
75  }
76 
77  template <class TREETYPE>
79  if(node_map != 0){
80  delete node_map;
81  node_map = 0;
82  id = "";
83  origin = pose6d(0.0,0.0,0.0,0.0,0.0,0.0);
84  }
85  }
86 
87  template <class TREETYPE>
88  bool MapNode<TREETYPE>::readMap(std::string filename){
89  if(node_map != 0)
90  delete node_map;
91 
92  node_map = new TREETYPE(0.05);
93  return node_map->readBinary(filename);
94  }
95 
96  template <class TREETYPE>
97  bool MapNode<TREETYPE>::writeMap(std::string filename){
98  return node_map->writeBinary(filename);
99  }
100 
101 } // namespace
octomap::MapNode::updateMap
void updateMap(const Pointcloud &cloud, point3d sensor_origin)
Definition: MapNode.hxx:63
octomap::Pointcloud
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan.
Definition: Pointcloud.h:47
octomap::Pointcloud::push_back
void push_back(float x, float y, float z)
Definition: Pointcloud.h:61
octomap::MapNode::writeMap
bool writeMap(std::string filename)
Definition: MapNode.hxx:97
octomap::MapNode::origin
pose6d origin
Definition: MapNode.h:70
octomath::Vector3
This class represents a three-dimensional vector.
Definition: Vector3.h:50
octomap::MapNode::readMap
bool readMap(std::string filename)
Definition: MapNode.hxx:88
octomap::MapNode::MapNode
MapNode()
Definition: MapNode.hxx:37
octomap::MapNode::~MapNode
~MapNode()
Definition: MapNode.hxx:58
octomap::point3d_list
std::list< octomath::Vector3 > point3d_list
Definition: octomap_types.h:54
octomap
Tables used by the Marching Cubes Algorithm The tables are from Paul Bourke's web page http://paulbou...
octomath::Pose6D
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:49
octomap::MapNode::generatePointcloud
Pointcloud generatePointcloud()
Definition: MapNode.hxx:67
octomap::MapNode::clear
void clear()
Definition: MapNode.hxx:78
octomap::pose6d
octomath::Pose6D pose6d
Use our Pose6D (float precision) as pose6d in octomap.
Definition: octomap_types.h:51