MapCollection.h
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 #ifndef OCTOMAP_MAP_COLLECTION_H
35 #define OCTOMAP_MAP_COLLECTION_H
36 
37 
38 #include <vector>
39 #include <octomap/MapNode.h>
40 
41 namespace octomap {
42 
43 
44  template <class MAPNODE>
45  class MapCollection {
46  public:
47  MapCollection();
48  MapCollection(std::string filename);
50 
51  void addNode( MAPNODE* node);
52  MAPNODE* addNode(const Pointcloud& cloud, point3d sensor_origin);
53  bool removeNode(const MAPNODE* n);
54  MAPNODE* queryNode(const point3d& p);
55 
56  bool isOccupied(const point3d& p) const;
57  bool isOccupied(float x, float y, float z) const;
58 
59  double getOccupancy(const point3d& p);
60 
61  bool castRay(const point3d& origin, const point3d& direction, point3d& end,
62  bool ignoreUnknownCells=false, double maxRange=-1.0) const;
63 
64  bool writePointcloud(std::string filename);
65  bool write(std::string filename);
66 
67  // TODO
68  void insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin,
69  double maxrange=-1., bool pruning=true, bool lazy_eval = false);
70  // TODO
71  MAPNODE* queryNode(std::string id);
72 
73  typedef typename std::vector<MAPNODE*>::iterator iterator;
74  typedef typename std::vector<MAPNODE*>::const_iterator const_iterator;
75  iterator begin() { return nodes.begin(); }
76  iterator end() { return nodes.end(); }
77  const_iterator begin() const { return nodes.begin(); }
78  const_iterator end() const { return nodes.end(); }
79  size_t size() const { return nodes.size(); }
80 
81  protected:
82  void clear();
83  bool read(std::string filename);
84 
85  // TODO
86  std::vector<Pointcloud*> segment(const Pointcloud& scan) const;
87  // TODO
88  MAPNODE* associate(const Pointcloud& scan);
89 
90  static void splitPathAndFilename(std::string &filenamefullpath, std::string* path, std::string *filename);
91  static std::string combinePathAndFilename(std::string path, std::string filename);
92  static bool readTagValue(std::string tag, std::ifstream &infile, std::string* value);
93 
94  protected:
95 
96  std::vector<MAPNODE*> nodes;
97  };
98 
99 } // end namespace
100 
101 #include "octomap/MapCollection.hxx"
102 
103 #endif
octomap::MapCollection::associate
MAPNODE * associate(const Pointcloud &scan)
octomap::MapCollection::isOccupied
bool isOccupied(const point3d &p) const
octomap::MapCollection::removeNode
bool removeNode(const MAPNODE *n)
octomap::MapCollection::castRay
bool castRay(const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
octomap::Pointcloud
Definition: Pointcloud.h:47
octomap::MapCollection::splitPathAndFilename
static void splitPathAndFilename(std::string &filenamefullpath, std::string *path, std::string *filename)
octomap::MapCollection::iterator
std::vector< MAPNODE * >::iterator iterator
Definition: MapCollection.h:73
octomap::MapCollection::end
iterator end()
Definition: MapCollection.h:76
octomap::MapCollection::readTagValue
static bool readTagValue(std::string tag, std::ifstream &infile, std::string *value)
octomap::MapCollection::addNode
void addNode(MAPNODE *node)
octomap::MapCollection::MapCollection
MapCollection()
octomap::MapCollection
Definition: MapCollection.h:45
octomath::Vector3
This class represents a three-dimensional vector.
Definition: Vector3.h:50
octomap::MapCollection::~MapCollection
~MapCollection()
octomap::MapCollection::segment
std::vector< Pointcloud * > segment(const Pointcloud &scan) const
octomap::MapCollection::nodes
std::vector< MAPNODE * > nodes
Definition: MapCollection.h:96
octomap::MapCollection::const_iterator
std::vector< MAPNODE * >::const_iterator const_iterator
Definition: MapCollection.h:74
octomap::MapCollection::end
const_iterator end() const
Definition: MapCollection.h:78
octomap::MapCollection::read
bool read(std::string filename)
octomap::MapCollection::clear
void clear()
octomap::MapCollection::combinePathAndFilename
static std::string combinePathAndFilename(std::string path, std::string filename)
octomap::MapCollection::size
size_t size() const
Definition: MapCollection.h:79
octomap::MapCollection::getOccupancy
double getOccupancy(const point3d &p)
MapNode.h
octomap::MapCollection::insertScan
void insertScan(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool pruning=true, bool lazy_eval=false)
octomap::MapCollection::write
bool write(std::string filename)
octomap::MapCollection::begin
const_iterator begin() const
Definition: MapCollection.h:77
octomap::MapCollection::writePointcloud
bool writePointcloud(std::string filename)
octomap::MapCollection::begin
iterator begin()
Definition: MapCollection.h:75
octomap
octomap::MapCollection::queryNode
MAPNODE * queryNode(const point3d &p)


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Tue Dec 12 2023 03:39:40