octomap  1.9.7
MapCollection.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 #include <stdio.h>
35 #include <sstream>
36 #include <fstream>
37 
38 namespace octomap {
39 
40  template <class MAPNODE>
42  }
43 
44  template <class MAPNODE>
45  MapCollection<MAPNODE>::MapCollection(std::string filename) {
46  this->read(filename);
47  }
48 
49  template <class MAPNODE>
51  this->clear();
52  }
53 
54  template <class MAPNODE>
56  // FIXME: memory leak, else we run into double frees in, e.g., the viewer...
57 
58  // for(typename std::vector<MAPNODE*>::iterator it= nodes.begin(); it != nodes.end(); ++it)
59  // delete *it;
60  nodes.clear();
61  }
62 
63  template <class MAPNODE>
64  bool MapCollection<MAPNODE>::read(std::string filenamefullpath) {
65 
66  std::string path;
67  std::string filename;
68  splitPathAndFilename(filenamefullpath, &path, &filename);
69 
70  std::ifstream infile;
71  infile.open(filenamefullpath.c_str(), std::ifstream::in);
72  if(!infile.is_open()){
73  OCTOMAP_ERROR_STR("Could not open "<< filenamefullpath << ". MapCollection not loaded.");
74  return false;
75  }
76 
77  bool ok = true;
78  while(ok){
79  std::string nodeID;
80  ok = readTagValue("MAPNODEID", infile, &nodeID);
81  if(!ok){
82  //do not throw error, you could be at the end of the file
83  break;
84  }
85 
86  std::string mapNodeFilename;
87  ok = readTagValue("MAPNODEFILENAME", infile, &mapNodeFilename);
88  if(!ok){
89  OCTOMAP_ERROR_STR("Could not read MAPNODEFILENAME.");
90  break;
91  }
92 
93  std::string poseStr;
94  ok = readTagValue("MAPNODEPOSE", infile, &poseStr);
95  std::istringstream poseStream(poseStr);
96  float x,y,z;
97  poseStream >> x >> y >> z;
98  double roll,pitch,yaw;
99  poseStream >> roll >> pitch >> yaw;
100  ok = ok && !poseStream.fail();
101  if(!ok){
102  OCTOMAP_ERROR_STR("Could not read MAPNODEPOSE.");
103  break;
104  }
105  octomap::pose6d origin(x, y, z, roll, pitch, yaw);
106 
107  MAPNODE* node = new MAPNODE(combinePathAndFilename(path,mapNodeFilename), origin);
108  node->setId(nodeID);
109 
110  if(!ok){
111  for(unsigned int i=0; i<nodes.size(); i++){
112  delete nodes[i];
113  }
114  infile.close();
115  return false;
116  } else {
117  nodes.push_back(node);
118  }
119  }
120  infile.close();
121  return true;
122  }
123 
124  template <class MAPNODE>
125  void MapCollection<MAPNODE>::addNode( MAPNODE* node){
126  nodes.push_back(node);
127  }
128 
129  template <class MAPNODE>
130  MAPNODE* MapCollection<MAPNODE>::addNode(const Pointcloud& cloud, point3d sensor_origin) {
131  // TODO...
132  return 0;
133  }
134 
135  template <class MAPNODE>
136  bool MapCollection<MAPNODE>::removeNode(const MAPNODE* n) {
137  // TODO...
138  return false;
139  }
140 
141  template <class MAPNODE>
143  for (const_iterator it = this->begin(); it != this->end(); ++it) {
144  point3d ptrans = (*it)->getOrigin().inv().transform(p);
145  typename MAPNODE::TreeType::NodeType* n = (*it)->getMap()->search(ptrans);
146  if (!n) continue;
147  if ((*it)->getMap()->isNodeOccupied(n)) return (*it);
148  }
149  return 0;
150  }
151 
152  template <class MAPNODE>
154  for (const_iterator it = this->begin(); it != this->end(); ++it) {
155  point3d ptrans = (*it)->getOrigin().inv().transform(p);
156  typename MAPNODE::TreeType::NodeType* n = (*it)->getMap()->search(ptrans);
157  if (!n) continue;
158  if ((*it)->getMap()->isNodeOccupied(n)) return true;
159  }
160  return false;
161  }
162 
163  template <class MAPNODE>
164  bool MapCollection<MAPNODE>::isOccupied(float x, float y, float z) const {
165  point3d q(x,y,z);
166  return this->isOccupied(q);
167  }
168 
169 
170  template <class MAPNODE>
172  double max_occ_val = 0;
173  bool is_unknown = true;
174  for (const_iterator it = this->begin(); it != this->end(); ++it) {
175  point3d ptrans = (*it)->getOrigin().inv().transform(p);
176  typename MAPNODE::TreeType::NodeType* n = (*it)->getMap()->search(ptrans);
177  if (n) {
178  double occ = n->getOccupancy();
179  if (occ > max_occ_val) max_occ_val = occ;
180  is_unknown = false;
181  }
182  }
183  if (is_unknown) return 0.5;
184  return max_occ_val;
185  }
186 
187 
188  template <class MAPNODE>
189  bool MapCollection<MAPNODE>::castRay(const point3d& origin, const point3d& direction, point3d& end,
190  bool ignoreUnknownCells, double maxRange) const {
191  bool hit_obstacle = false;
192  double min_dist = 1e6;
193  // SPEEDUP: use openMP to do raycasting in parallel
194  // SPEEDUP: use bounding boxes to determine submaps
195  for (const_iterator it = this->begin(); it != this->end(); ++it) {
196  point3d origin_trans = (*it)->getOrigin().inv().transform(origin);
197  point3d direction_trans = (*it)->getOrigin().inv().rot().rotate(direction);
198  printf("ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f in node %s\n",
199  origin_trans.x(), origin_trans.y(), origin_trans.z(),
200  direction_trans.x(), direction_trans.y(), direction_trans.z(),
201  (*it)->getId().c_str());
202  point3d temp_endpoint;
203  if ((*it)->getMap()->castRay(origin_trans, direction_trans, temp_endpoint, ignoreUnknownCells, maxRange)) {
204  printf("hit obstacle in node %s\n", (*it)->getId().c_str());
205  double current_dist = origin_trans.distance(temp_endpoint);
206  if (current_dist < min_dist) {
207  min_dist = current_dist;
208  end = (*it)->getOrigin().transform(temp_endpoint);
209  }
210  hit_obstacle = true;
211  } // end if hit obst
212  } // end for
213  return hit_obstacle;
214  }
215 
216 
217  template <class MAPNODE>
218  bool MapCollection<MAPNODE>::writePointcloud(std::string filename) {
219  Pointcloud pc;
220  for(typename std::vector<MAPNODE* >::iterator it = nodes.begin(); it != nodes.end(); ++it){
221  Pointcloud tmp = (*it)->generatePointcloud();
222  pc.push_back(tmp);
223  }
224  pc.writeVrml(filename);
225  return true;
226  }
227 
228 
229  template <class MAPNODE>
230  bool MapCollection<MAPNODE>::write(std::string filename) {
231  bool ok = true;
232 
233  std::ofstream outfile(filename.c_str());
234  outfile << "#This file was generated by the write-method of MapCollection\n";
235 
236  for(typename std::vector<MAPNODE* >::iterator it = nodes.begin(); it != nodes.end(); ++it){
237  std::string id = (*it)->getId();
238  pose6d origin = (*it)->getOrigin();
239  std::string nodemapFilename = "nodemap_";
240  nodemapFilename.append(id);
241  nodemapFilename.append(".bt");
242 
243  outfile << "MAPNODEID " << id << "\n";
244  outfile << "MAPNODEFILENAME "<< nodemapFilename << "\n";
245  outfile << "MAPNODEPOSE " << origin.x() << " " << origin.y() << " " << origin.z() << " "
246  << origin.roll() << " " << origin.pitch() << " " << origin.yaw() << std::endl;
247  ok = ok && (*it)->writeMap(nodemapFilename);
248  }
249  outfile.close();
250  return ok;
251  }
252 
253  // TODO
254  template <class MAPNODE>
255  void MapCollection<MAPNODE>::insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin,
256  double maxrange, bool pruning, bool lazy_eval) {
257  fprintf(stderr, "ERROR: MapCollection::insertScan is not implemented yet.\n");
258  }
259 
260  template <class MAPNODE>
261  MAPNODE* MapCollection<MAPNODE>::queryNode(std::string id) {
262  for (const_iterator it = this->begin(); it != this->end(); ++it) {
263  if ((*it)->getId() == id) return *(it);
264  }
265  return 0;
266  }
267 
268  // TODO
269  template <class MAPNODE>
270  std::vector<Pointcloud*> MapCollection<MAPNODE>::segment(const Pointcloud& scan) const {
271  std::vector<Pointcloud*> result;
272  fprintf(stderr, "ERROR: MapCollection::segment is not implemented yet.\n");
273  return result;
274  }
275 
276  // TODO
277  template <class MAPNODE>
279  fprintf(stderr, "ERROR: MapCollection::associate is not implemented yet.\n");
280  return 0;
281  }
282 
283  template <class MAPNODE>
284  void MapCollection<MAPNODE>::splitPathAndFilename(std::string &filenamefullpath,
285  std::string* path, std::string *filename) {
286 #ifdef WIN32
287  std::string::size_type lastSlash = filenamefullpath.find_last_of('\\');
288 #else
289  std::string::size_type lastSlash = filenamefullpath.find_last_of('/');
290 #endif
291  if (lastSlash != std::string::npos){
292  *filename = filenamefullpath.substr(lastSlash + 1);
293  *path = filenamefullpath.substr(0, lastSlash);
294  } else {
295  *filename = filenamefullpath;
296  *path = "";
297  }
298  }
299 
300  template <class MAPNODE>
301  std::string MapCollection<MAPNODE>::combinePathAndFilename(std::string path, std::string filename) {
302  std::string result = path;
303  if(path != ""){
304 #ifdef WIN32
305  result.append("\\");
306 #else
307  result.append("/");
308 #endif
309  }
310  result.append(filename);
311  return result;
312  }
313 
314  template <class MAPNODE>
315  bool MapCollection<MAPNODE>::readTagValue(std::string /*tag*/, std::ifstream& infile, std::string* value) {
316  std::string line;
317  while( getline(infile, line) ){
318  if(line.length() != 0 && line[0] != '#')
319  break;
320  }
321  *value = "";
322  std::string::size_type firstSpace = line.find(' ');
323  if(firstSpace != std::string::npos && firstSpace != line.size()-1){
324  *value = line.substr(firstSpace + 1);
325  return true;
326  }
327  else return false;
328  }
329 
330 } // namespace
octomath::Pose6D::x
float & x()
Definition: Pose6D.h:102
octomap::MapCollection::associate
MAPNODE * associate(const Pointcloud &scan)
Definition: MapCollection.hxx:278
octomath::Pose6D::yaw
double yaw() const
Definition: Pose6D.h:111
octomap::MapCollection::isOccupied
bool isOccupied(const point3d &p) const
Definition: MapCollection.hxx:153
octomap::MapCollection::removeNode
bool removeNode(const MAPNODE *n)
Definition: MapCollection.hxx:136
octomap::MapCollection::castRay
bool castRay(const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
Definition: MapCollection.hxx:189
octomap::Pointcloud
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan.
Definition: Pointcloud.h:47
octomath::Pose6D::z
float & z()
Definition: Pose6D.h:104
octomap::Pointcloud::push_back
void push_back(float x, float y, float z)
Definition: Pointcloud.h:61
octomap::MapCollection::segment
std::vector< Pointcloud * > segment(const Pointcloud &scan) const
Definition: MapCollection.hxx:270
octomap::MapCollection::addNode
void addNode(MAPNODE *node)
Definition: MapCollection.hxx:125
octomap::MapCollection::MapCollection
MapCollection()
Definition: MapCollection.hxx:41
octomath::Pose6D::y
float & y()
Definition: Pose6D.h:103
octomath::Vector3
This class represents a three-dimensional vector.
Definition: Vector3.h:50
octomap::MapCollection::~MapCollection
~MapCollection()
Definition: MapCollection.hxx:50
octomath::Pose6D::pitch
double pitch() const
Definition: Pose6D.h:110
octomap::MapCollection::splitPathAndFilename
static void splitPathAndFilename(std::string &filenamefullpath, std::string *path, std::string *filename)
Definition: MapCollection.hxx:284
octomap::MapCollection::const_iterator
std::vector< MAPNODE * >::const_iterator const_iterator
Definition: MapCollection.h:74
octomap::Pointcloud::writeVrml
void writeVrml(std::string filename)
Export the Pointcloud to a VRML file.
Definition: Pointcloud.cpp:233
octomap::MapCollection::read
bool read(std::string filename)
Definition: MapCollection.hxx:64
octomap::MapCollection::clear
void clear()
Definition: MapCollection.hxx:55
octomap::MapCollection::getOccupancy
double getOccupancy(const point3d &p)
Definition: MapCollection.hxx:171
octomap::MapCollection::readTagValue
static bool readTagValue(std::string tag, std::ifstream &infile, std::string *value)
Definition: MapCollection.hxx:315
octomap::MapCollection::combinePathAndFilename
static std::string combinePathAndFilename(std::string path, std::string filename)
Definition: MapCollection.hxx:301
OCTOMAP_ERROR_STR
#define OCTOMAP_ERROR_STR(args)
Definition: octomap_types.h:79
octomap::MapCollection::queryNode
MAPNODE * queryNode(const point3d &p)
Definition: MapCollection.hxx:142
octomap::MapCollection::insertScan
void insertScan(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool pruning=true, bool lazy_eval=false)
Definition: MapCollection.hxx:255
octomap::MapCollection::write
bool write(std::string filename)
Definition: MapCollection.hxx:230
octomath::Vector3::y
float & y()
Definition: Vector3.h:136
octomap::MapCollection::writePointcloud
bool writePointcloud(std::string filename)
Definition: MapCollection.hxx:218
octomath::Pose6D::roll
double roll() const
Definition: Pose6D.h:109
octomath::Vector3::distance
double distance(const Vector3 &other) const
Definition: Vector3.h:292
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
octomath::Vector3::z
float & z()
Definition: Vector3.h:141
octomath::Vector3::x
float & x()
Definition: Vector3.h:131