Main MRPT website > C++ reference for MRPT 1.4.0
MapCollection.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef OCTOMAP_MAP_COLLECTION_H
10 #define OCTOMAP_MAP_COLLECTION_H
11 
12 // $Id: MapCollection.h 402 2012-08-06 13:39:42Z ahornung $
13 
14 /**
15  * OctoMap:
16  * A probabilistic, flexible, and compact 3D mapping library for robotic systems.
17  * @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009-2011
18  * @see http://octomap.sourceforge.net/
19  * License: New BSD License
20  */
21 
22 /*
23  * Copyright (c) 2009-2011, K. M. Wurm, A. Hornung, University of Freiburg
24  * All rights reserved.
25  *
26  * Redistribution and use in source and binary forms, with or without
27  * modification, are permitted provided that the following conditions are met:
28  *
29  * * Redistributions of source code must retain the above copyright
30  * notice, this list of conditions and the following disclaimer.
31  * * Redistributions in binary form must reproduce the above copyright
32  * notice, this list of conditions and the following disclaimer in the
33  * documentation and/or other materials provided with the distribution.
34  * * Neither the name of the University of Freiburg nor the names of its
35  * contributors may be used to endorse or promote products derived from
36  * this software without specific prior written permission.
37  *
38  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
39  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
40  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
41  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
42  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
43  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
44  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
45  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
46  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
47  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
48  * POSSIBILITY OF SUCH DAMAGE.
49  */
50 
51 #include <vector>
53 
54 namespace octomap {
55 
56 
57  template <class MAPNODE>
58  class MapCollection {
59  public:
60  MapCollection();
61  MapCollection(std::string filename);
63 
64  void addNode( MAPNODE* node);
65  MAPNODE* addNode(const Pointcloud& cloud, point3d sensor_origin);
66  bool removeNode(const MAPNODE* n);
67  MAPNODE* queryNode(const point3d& p);
68 
69  bool isOccupied(const point3d& p) const;
70  bool isOccupied(float x, float y, float z) const;
71 
72  float getOccupancy(const point3d& p);
73 
74  bool castRay(const point3d& origin, const point3d& direction, point3d& end,
75  bool ignoreUnknownCells=false, double maxRange=-1.0) const;
76 
77  bool writePointcloud(std::string filename);
78  bool write(std::string filename);
79 
80  // TODO
81  void insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin,
82  double maxrange=-1., bool pruning=true, bool lazy_eval = false);
83  // TODO
84  MAPNODE* queryNode(std::string id);
85 
88  iterator begin() { return nodes.begin(); }
89  iterator end() { return nodes.end(); }
90  const_iterator begin() const { return nodes.begin(); }
91  const_iterator end() const { return nodes.end(); }
92  size_t size() const { return nodes.size(); }
93 
94  protected:
95  void clear();
96  bool read(std::string filename);
97 
98  // TODO
99  std::vector<Pointcloud*> segment(const Pointcloud& scan) const;
100  // TODO
101  MAPNODE* associate(const Pointcloud& scan);
102 
103  static void splitPathAndFilename(std::string &filenamefullpath, std::string* path, std::string *filename);
104  static std::string combinePathAndFilename(std::string path, std::string filename);
105  static bool readTagValue(std::string tag, std::ifstream &infile, std::string* value);
106 
107  protected:
108 
109  std::vector<MAPNODE*> nodes;
110  };
111 
112 } // end namespace
113 
114 #include "mrpt/otherlibs/octomap/MapCollection.hxx"
115 
116 #endif
bool castRay(const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
std::vector< MAPNODE * >::const_iterator const_iterator
Definition: MapCollection.h:87
OctoMap: A probabilistic, flexible, and compact 3D mapping library for robotic systems.
const_iterator end() const
Definition: MapCollection.h:91
Scalar * iterator
Definition: eigen_plugins.h:23
std::vector< Pointcloud * > segment(const Pointcloud &scan) const
bool isOccupied(const point3d &p) const
const Scalar * const_iterator
Definition: eigen_plugins.h:24
MAPNODE * queryNode(const point3d &p)
static std::string combinePathAndFilename(std::string path, std::string filename)
bool writePointcloud(std::string filename)
float getOccupancy(const point3d &p)
bool write(std::string filename)
MAPNODE * associate(const Pointcloud &scan)
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan...
Definition: Pointcloud.h:63
void addNode(MAPNODE *node)
size_t size() const
Definition: MapCollection.h:92
std::vector< MAPNODE * > nodes
std::vector< MAPNODE * >::iterator iterator
Definition: MapCollection.h:86
const_iterator begin() const
Definition: MapCollection.h:90
static bool readTagValue(std::string tag, std::ifstream &infile, std::string *value)
static void splitPathAndFilename(std::string &filenamefullpath, std::string *path, std::string *filename)
void insertScan(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool pruning=true, bool lazy_eval=false)
bool removeNode(const MAPNODE *n)
bool read(std::string filename)
This class represents a three-dimensional vector.
Definition: Vector3.h:65



Page generated by Doxygen 1.8.11 for MRPT 1.4.0 SVN: at Mon Aug 15 11:50:21 UTC 2016