octomap 1.9.8
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
38namespace octomap {
39
40 template <class MAPNODE>
42 }
43
44 template <class MAPNODE>
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>
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
static void splitPathAndFilename(std::string &filenamefullpath, std::string *path, std::string *filename)
Definition: MapCollection.hxx:284
static bool readTagValue(std::string tag, std::ifstream &infile, std::string *value)
Definition: MapCollection.hxx:315
void addNode(MAPNODE *node)
Definition: MapCollection.hxx:125
MapCollection()
Definition: MapCollection.hxx:41
bool writePointcloud(std::string filename)
Definition: MapCollection.hxx:218
void insertScan(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool pruning=true, bool lazy_eval=false)
Definition: MapCollection.hxx:255
double getOccupancy(const point3d &p)
Definition: MapCollection.hxx:171
MAPNODE * queryNode(const point3d &p)
Definition: MapCollection.hxx:142
bool castRay(const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
Definition: MapCollection.hxx:189
bool isOccupied(const point3d &p) const
Definition: MapCollection.hxx:153
MAPNODE * associate(const Pointcloud &scan)
Definition: MapCollection.hxx:278
bool read(std::string filename)
Definition: MapCollection.hxx:64
~MapCollection()
Definition: MapCollection.hxx:50
std::vector< MAPNODE * >::const_iterator const_iterator
Definition: MapCollection.h:74
bool write(std::string filename)
Definition: MapCollection.hxx:230
std::vector< Pointcloud * > segment(const Pointcloud &scan) const
Definition: MapCollection.hxx:270
void clear()
Definition: MapCollection.hxx:55
bool removeNode(const MAPNODE *n)
Definition: MapCollection.hxx:136
static std::string combinePathAndFilename(std::string path, std::string filename)
Definition: MapCollection.hxx:301
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan.
Definition: Pointcloud.h:47
void push_back(float x, float y, float z)
Definition: Pointcloud.h:61
void writeVrml(std::string filename)
Export the Pointcloud to a VRML file.
Definition: Pointcloud.cpp:233
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:49
float & y()
Definition: Pose6D.h:103
double roll() const
Definition: Pose6D.h:109
float & z()
Definition: Pose6D.h:104
double yaw() const
Definition: Pose6D.h:111
double pitch() const
Definition: Pose6D.h:110
float & x()
Definition: Pose6D.h:102
This class represents a three-dimensional vector.
Definition: Vector3.h:50
float & x()
Definition: Vector3.h:131
double distance(const Vector3 &other) const
Definition: Vector3.h:292
float & y()
Definition: Vector3.h:136
float & z()
Definition: Vector3.h:141
Namespace the OctoMap library and visualization tools.
#define OCTOMAP_ERROR_STR(args)
Definition: octomap_types.h:79