ScanGraph.cpp
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * http://octomap.github.com/
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 <iomanip>
35 #include <fstream>
36 #include <sstream>
37 #include <stdlib.h>
38 
39 #include <octomap/math/Pose6D.h>
40 #include <octomap/ScanGraph.h>
41 
42 namespace octomap {
43 
44 
46  if (scan != NULL){
47  delete scan;
48  scan = NULL;
49  }
50  }
51 
52  std::ostream& ScanNode::writeBinary(std::ostream &s) const {
53 
54  // file structure: pointcloud | pose | id
55 
56  scan->writeBinary(s);
57  pose.writeBinary(s);
58  s.write((char*)&id, sizeof(id));
59 
60  return s;
61  }
62 
63  std::istream& ScanNode::readBinary(std::istream &s) {
64 
65  this->scan = new Pointcloud();
66  this->scan->readBinary(s);
67 
68  this->pose.readBinary(s);
69 
70  s.read((char*)&this->id, sizeof(this->id));
71 
72  return s;
73  }
74 
75 
76  std::ostream& ScanNode::writePoseASCII(std::ostream &s) const {
77  s << " " << this->id; // export pose for human editor
78  s << " ";
79  this->pose.trans().write(s);
80  s << " ";
81  this->pose.rot().toEuler().write(s);
82  s << std::endl;
83  return s;
84  }
85 
86  std::istream& ScanNode::readPoseASCII(std::istream &s) {
87  unsigned int read_id;
88  s >> read_id;
89  if (read_id != this->id)
90  OCTOMAP_ERROR("ERROR while reading ScanNode pose from ASCII. id %d does not match real id %d.\n", read_id, this->id);
91 
92  this->pose.trans().read(s);
93 
94  // read rotation from euler angles
95  point3d rot;
96  rot.read(s);
97  this->pose.rot() = octomath::Quaternion(rot);
98  return s;
99  }
100 
101 
102  std::ostream& ScanEdge::writeBinary(std::ostream &s) const {
103 
104  // file structure: first_id | second_id | constraint | weight
105 
106  s.write((char*)&first->id, sizeof(first->id));
107  s.write((char*)&second->id, sizeof(second->id));
108  constraint.writeBinary(s);
109  s.write((char*)&weight, sizeof(weight));
110  return s;
111  }
112 
113  std::istream& ScanEdge::readBinary(std::istream &s, ScanGraph& graph) {
114  unsigned int first_id, second_id;
115  s.read((char*)&first_id, sizeof(first_id));
116  s.read((char*)&second_id, sizeof(second_id));
117 
118  this->first = graph.getNodeByID(first_id);
119  if (this->first == NULL) OCTOMAP_ERROR("ERROR while reading ScanEdge. first node not found.\n");
120  this->second = graph.getNodeByID(second_id);
121  if (this->second == NULL) OCTOMAP_ERROR("ERROR while reading ScanEdge. second node not found.\n");
122 
123  this->constraint.readBinary(s);
124  s.read((char*)&weight, sizeof(weight));
125 
126  return s;
127  }
128 
129 
130  std::ostream& ScanEdge::writeASCII(std::ostream &s) const {
131 
132  // file structure: first_id | second_id | constraint | weight
133 
134  s << " " << first->id << " " << second->id;
135  s << " ";
136  constraint.write(s);
137  s << " " << weight;
138  s << std::endl;
139  return s;
140  }
141 
142  std::istream& ScanEdge::readASCII(std::istream &s, ScanGraph& graph) {
143 
144  unsigned int first_id, second_id;
145  s >> first_id;
146  s >> second_id;
147 
148  this->first = graph.getNodeByID(first_id);
149  if (this->first == NULL) OCTOMAP_ERROR("ERROR while reading ScanEdge. first node %d not found.\n", first_id);
150  this->second = graph.getNodeByID(second_id);
151  if (this->second == NULL) OCTOMAP_ERROR("ERROR while reading ScanEdge. second node %d not found.\n", second_id);
152 
153  this->constraint.read(s);
154  s >> weight;
155  return s;
156  }
157 
158 
160  this->clear();
161  }
162 
164  for (unsigned int i=0; i<nodes.size(); i++) {
165  delete nodes[i];
166  }
167  nodes.clear();
168  for (unsigned int i=0; i<edges.size(); i++) {
169  delete edges[i];
170  }
171  edges.clear();
172  }
173 
174 
176  if (scan != 0) {
177  nodes.push_back(new ScanNode(scan, pose, (unsigned int) nodes.size()));
178  return nodes.back();
179  }
180  else {
181  OCTOMAP_ERROR("scan is invalid.\n");
182  return NULL;
183  }
184  }
185 
186 
187  ScanEdge* ScanGraph::addEdge(ScanNode* first, ScanNode* second, pose6d constraint) {
188 
189  if ((first != 0) && (second != 0)) {
190  edges.push_back(new ScanEdge(first, second, constraint));
191  // OCTOMAP_DEBUG("ScanGraph::AddEdge %d --> %d\n", first->id, second->id);
192  return edges.back();
193  }
194  else {
195  OCTOMAP_ERROR("addEdge:: one or both nodes invalid.\n");
196  return NULL;
197  }
198  }
199 
200 
201  ScanEdge* ScanGraph::addEdge(unsigned int first_id, unsigned int second_id) {
202 
203  if ( this->edgeExists(first_id, second_id)) {
204  OCTOMAP_ERROR("addEdge:: Edge exists!\n");
205  return NULL;
206  }
207 
208  ScanNode* first = getNodeByID(first_id);
209  ScanNode* second = getNodeByID(second_id);
210 
211  if ((first != 0) && (second != 0)) {
212  pose6d constr = first->pose.inv() * second->pose;
213  return this->addEdge(first, second, constr);
214  }
215  else {
216  OCTOMAP_ERROR("addEdge:: one or both scans invalid.\n");
217  return NULL;
218  }
219  }
220 
221 
223  if (nodes.size() >= 2) {
224  ScanNode* first = nodes[nodes.size()-2];
225  ScanNode* second = nodes[nodes.size()-1];
226  pose6d c = (first->pose).inv() * second->pose;
227  this->addEdge(first, second, c);
228  }
229  }
230 
231 
232  void ScanGraph::exportDot(std::string filename) {
233  std::ofstream outfile (filename.c_str());
234  outfile << "graph ScanGraph" << std::endl;
235  outfile << "{" << std::endl;
236  for (unsigned int i=0; i<edges.size(); i++) {
237  outfile << (edges[i]->first)->id
238  << " -- "
239  << (edges[i]->second)->id
240  << " [label="
241  << std::fixed << std::setprecision(2) << edges[i]->constraint.transLength()
242  << "]" << std::endl;
243  }
244  outfile << "}" << std::endl;
245  outfile.close();
246  }
247 
248  ScanNode* ScanGraph::getNodeByID(unsigned int id) {
249  for (unsigned int i = 0; i < nodes.size(); i++) {
250  if (nodes[i]->id == id) return nodes[i];
251  }
252  return NULL;
253  }
254 
255  bool ScanGraph::edgeExists(unsigned int first_id, unsigned int second_id) {
256 
257  for (unsigned int i=0; i<edges.size(); i++) {
258  if (
259  (((edges[i]->first)->id == first_id) && ((edges[i]->second)->id == second_id))
260  ||
261  (((edges[i]->first)->id == second_id) && ((edges[i]->second)->id == first_id))) {
262  return true;
263  }
264  }
265  return false;
266  }
267 
268  std::vector<unsigned int> ScanGraph::getNeighborIDs(unsigned int id) {
269  std::vector<unsigned int> res;
270  ScanNode* node = getNodeByID(id);
271  if (node) {
272  // check all nodes
273  for (unsigned int i = 0; i < nodes.size(); i++) {
274  if (node->id == nodes[i]->id) continue;
275  if (edgeExists(id, nodes[i]->id)) {
276  res.push_back(nodes[i]->id);
277  }
278  }
279  }
280  return res;
281  }
282 
283  std::vector<ScanEdge*> ScanGraph::getOutEdges(ScanNode* node) {
284  std::vector<ScanEdge*> res;
285  if (node) {
286  for (std::vector<ScanEdge*>::iterator it = edges.begin(); it != edges.end(); it++) {
287  if ((*it)->first == node) {
288  res.push_back(*it);
289  }
290  }
291  }
292  return res;
293  }
294 
295  std::vector<ScanEdge*> ScanGraph::getInEdges(ScanNode* node) {
296  std::vector<ScanEdge*> res;
297  if (node) {
298  for (std::vector<ScanEdge*>::iterator it = edges.begin(); it != edges.end(); it++) {
299  if ((*it)->second == node) {
300  res.push_back(*it);
301  }
302  }
303  }
304  return res;
305  }
306 
308  for(ScanGraph::iterator it=this->begin(); it != this->end(); it++) {
309  ((*it)->scan)->transformAbsolute((*it)->pose);
310  }
311  }
312 
313  bool ScanGraph::writeBinary(const std::string& filename) const {
314  std::ofstream binary_outfile( filename.c_str(), std::ios_base::binary);
315  if (!binary_outfile.is_open()){
316  OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing written.");
317  return false;
318  }
319  writeBinary(binary_outfile);
320  binary_outfile.close();
321  return true;
322  }
323 
324  std::ostream& ScanGraph::writeBinary(std::ostream &s) const {
325 
326  // file structure: n | node_1 | ... | node_n | m | edge_1 | ... | edge_m
327 
328  // write nodes ---------------------------------
329  // note: size is always an unsigned int!
330  unsigned int graph_size = (unsigned int) this->size();
331  if (graph_size) OCTOMAP_DEBUG("writing %u nodes to binary file...\n", graph_size);
332  s.write((char*)&graph_size, sizeof(graph_size));
333 
334  for (ScanGraph::const_iterator it = this->begin(); it != this->end(); it++) {
335  (*it)->writeBinary(s);
336  }
337 
338  if (graph_size) OCTOMAP_DEBUG("done.\n");
339 
340  // write edges ---------------------------------
341  unsigned int num_edges = (unsigned int) this->edges.size();
342  if (num_edges) OCTOMAP_DEBUG("writing %u edges to binary file...\n", num_edges);
343  s.write((char*)&num_edges, sizeof(num_edges));
344 
345  for (ScanGraph::const_edge_iterator it = this->edges_begin(); it != this->edges_end(); it++) {
346  (*it)->writeBinary(s);
347  }
348 
349  if (num_edges) OCTOMAP_DEBUG(" done.\n");
350 
351  return s;
352  }
353 
354  bool ScanGraph::readBinary(const std::string& filename) {
355  std::ifstream binary_infile(filename.c_str(), std::ios_base::binary);
356  if (!binary_infile.is_open()){
357  OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing read.");
358  return false;
359  }
360  readBinary(binary_infile);
361  binary_infile.close();
362  return true;
363  }
364 
365  std::istream& ScanGraph::readBinary(std::ifstream &s) {
366  if (!s.is_open()){
367  OCTOMAP_ERROR_STR("Could not read from input filestream in ScanGraph::readBinary");
368  return s;
369  } else if (!s.good()){
370  OCTOMAP_WARNING_STR("Input filestream not \"good\" in ScanGraph::readBinary");
371  }
372  this->clear();
373 
374  // read nodes ---------------------------------
375  unsigned int graph_size = 0;
376  s.read((char*)&graph_size, sizeof(graph_size));
377  if (graph_size) OCTOMAP_DEBUG("reading %d nodes from binary file...\n", graph_size);
378 
379  if (graph_size > 0) {
380  this->nodes.reserve(graph_size);
381 
382  for (unsigned int i=0; i<graph_size; i++) {
383 
384  ScanNode* node = new ScanNode();
385  node->readBinary(s);
386  if (!s.fail()) {
387  this->nodes.push_back(node);
388  }
389  else {
390  OCTOMAP_ERROR("ScanGraph::readBinary: ERROR.\n" );
391  break;
392  }
393  }
394  }
395  if (graph_size) OCTOMAP_DEBUG("done.\n");
396 
397  // read edges ---------------------------------
398  unsigned int num_edges = 0;
399  s.read((char*)&num_edges, sizeof(num_edges));
400  if (num_edges) OCTOMAP_DEBUG("reading %d edges from binary file...\n", num_edges);
401 
402  if (num_edges > 0) {
403  this->edges.reserve(num_edges);
404 
405  for (unsigned int i=0; i<num_edges; i++) {
406 
407  ScanEdge* edge = new ScanEdge();
408  edge->readBinary(s, *this);
409  if (!s.fail()) {
410  this->edges.push_back(edge);
411  }
412  else {
413  OCTOMAP_ERROR("ScanGraph::readBinary: ERROR.\n" );
414  break;
415  }
416  }
417  }
418  if (num_edges) OCTOMAP_DEBUG("done.\n");
419  return s;
420  }
421 
422  void ScanGraph::readPlainASCII(const std::string& filename){
423  std::ifstream infile(filename.c_str());
424  if (!infile.is_open()){
425  OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing read.");
426  return;
427  }
428  readPlainASCII(infile);
429  infile.close();
430  }
431 
432  std::istream& ScanGraph::readPlainASCII(std::istream& s){
433  std::string currentLine;
434  ScanNode* currentNode = NULL;
435  while (true){
436  getline(s, currentLine);
437  if (s.good() && !s.eof()){
438  std::stringstream ss;
439  ss << currentLine;
440  // skip empty and comment lines:
441  if (currentLine.size() == 0
442  || (currentLine.compare(0,1, "#") == 0)
443  || (currentLine.compare(0,1, " ") == 0)){
444 
445  continue;
446  } else if(currentLine.compare(0,4,"NODE")==0){
447  if (currentNode){
448  this->nodes.push_back(currentNode);
449  this->connectPrevious();
450  OCTOMAP_DEBUG_STR("ScanNode "<< currentNode->pose << " done, size: "<< currentNode->scan->size());
451  }
452 
453  currentNode = new ScanNode();
454  currentNode->scan = new Pointcloud();
455 
456  float x, y, z, roll, pitch, yaw;
457  std::string tmp;
458  ss >> tmp >> x >> y >> z >> roll >> pitch >> yaw;
459  pose6d pose(x, y, z, roll, pitch, yaw);
460  //std::cout << "Pose "<< pose << " found.\n";
461  currentNode->pose = pose;
462  } else{
463  if (currentNode == NULL){
464  // TODO: allow "simple" pc files by setting initial Scan Pose to (0,0,0)
465  OCTOMAP_ERROR_STR("Error parsing log file, no Scan to add point to!");
466  break;
467  }
468  float x, y, z;
469  ss >> x >> y >> z;
470 
471  //std::cout << "Point "<< x << "," <<y <<"," <<z << " found.\n";
472  currentNode->scan->push_back(x,y,z);
473  }
474  } else{
475  if (currentNode){
476  this->nodes.push_back(currentNode);
477  this->connectPrevious();
478  OCTOMAP_DEBUG_STR("Final ScanNode "<< currentNode->pose << " done, size: "<< currentNode->scan->size());
479  }
480  break;
481  }
482  }
483 
484  return s;
485  }
486 
487  std::ostream& ScanGraph::writeEdgesASCII(std::ostream &s) const {
488 
489  // file structure: n | edge_1 | ... | edge_n
490 
491  OCTOMAP_DEBUG_STR("Writing " << this->edges.size() << " edges to ASCII file...");
492 
493  s << " " << this->edges.size();
494  s << std::endl;
495 
496  for (ScanGraph::const_edge_iterator it = this->edges_begin(); it != this->edges_end(); it++) {
497  (*it)->writeASCII(s);
498  }
499  s << std::endl;
500  OCTOMAP_DEBUG_STR("Done.");
501 
502  return s;
503  }
504 
505 
506  std::istream& ScanGraph::readEdgesASCII(std::istream &s) {
507 
508  unsigned int num_edges = 0;
509  s >> num_edges;
510  OCTOMAP_DEBUG("Reading %d edges from ASCII file...\n", num_edges);
511 
512  if (num_edges > 0) {
513 
514  for (unsigned int i=0; i<this->edges.size(); i++) delete edges[i];
515  this->edges.clear();
516 
517  this->edges.reserve(num_edges);
518 
519  for (unsigned int i=0; i<num_edges; i++) {
520 
521  ScanEdge* edge = new ScanEdge();
522  edge->readASCII(s, *this);
523  if (!s.fail()) {
524  this->edges.push_back(edge);
525  }
526  else {
527  OCTOMAP_ERROR("ScanGraph::readBinary: ERROR.\n" );
528  break;
529  }
530  }
531  }
532 
533  OCTOMAP_DEBUG("done.\n");
534 
535  return s;
536  }
537 
538 
539  std::ostream& ScanGraph::writeNodePosesASCII(std::ostream &s) const {
540 
541  OCTOMAP_DEBUG("Writing %lu node poses to ASCII file...\n", (unsigned long) this->size());
542 
543  for (ScanGraph::const_iterator it = this->begin(); it != this->end(); it++) {
544  (*it)->writePoseASCII(s);
545  }
546  s << std::endl;
547  OCTOMAP_DEBUG("done.\n");
548 
549  return s;
550  }
551 
552  std::istream& ScanGraph::readNodePosesASCII(std::istream &s) {
553 
554  for (ScanGraph::const_iterator it = this->begin(); it != this->end(); it++) {
555  (*it)->readPoseASCII(s);
556  }
557 
558  for (ScanGraph::edge_iterator it = this->edges_begin(); it != this->edges_end(); it++) {
559  ScanNode* first = (*it)->first;
560  ScanNode* second = (*it)->second;
561  (*it)->constraint = (first->pose).inv() * second->pose;
562  }
563 
564  // constraints and nodes are inconsistent, rewire graph
565 // for (unsigned int i=0; i<this->edges.size(); i++) delete edges[i];
566 // this->edges.clear();
567 
568 
569 // ScanGraph::iterator first_it = this->begin();
570 // ScanGraph::iterator second_it = first_it+1;
571 
572 // for ( ; second_it != this->end(); first_it++, second_it++) {
573 // ScanNode* first = (*first_it);
574 // ScanNode* second = (*second_it);
575 // octomath::Pose6D c = (first->pose).inv() * second->pose;
576 // this->addEdge(first, second, c);
577 // }
578 
579 
580  return s;
581  }
582 
583 
584  void ScanGraph::cropEachScan(point3d lowerBound, point3d upperBound) {
585 
586  for (ScanGraph::iterator it = this->begin(); it != this->end(); it++) {
587  ((*it)->scan)->crop(lowerBound, upperBound);
588  }
589  }
590 
591 
592  void ScanGraph::crop(point3d lowerBound, point3d upperBound) {
593 
594 
595  // for all node in graph...
596  for (ScanGraph::iterator it = this->begin(); it != this->end(); it++) {
597  pose6d scan_pose = (*it)->pose;
598  Pointcloud* pc = new Pointcloud((*it)->scan);
599  pc->transformAbsolute(scan_pose);
600  pc->crop(lowerBound, upperBound);
601  pc->transform(scan_pose.inv());
602  delete (*it)->scan;
603  (*it)->scan = pc;
604  }
605  }
606 
607  size_t ScanGraph::getNumPoints(unsigned int max_id) const {
608  size_t retval = 0;
609 
610  for (ScanGraph::const_iterator it = this->begin(); it != this->end(); it++) {
611  retval += (*it)->scan->size();
612  if ((max_id > 0) && ((*it)->id == max_id)) break;
613  }
614  return retval;
615  }
616 
617 
618 } // end namespace
619 
620 
621 
std::ostream & writeBinary(std::ostream &s) const
Binary output operator.
Definition: Pose6D.cpp:141
std::istream & readPoseASCII(std::istream &s)
Definition: ScanGraph.cpp:86
std::ostream & write(std::ostream &s) const
Definition: Vector3.cpp:78
ScanNode * addNode(Pointcloud *scan, pose6d pose)
Definition: ScanGraph.cpp:175
std::istream & readBinary(std::istream &s, ScanGraph &graph)
Definition: ScanGraph.cpp:113
std::istream & readASCII(std::istream &s, ScanGraph &graph)
Definition: ScanGraph.cpp:142
std::istream & readBinary(std::ifstream &s)
Definition: ScanGraph.cpp:365
Pointcloud * scan
Definition: ScanGraph.h:73
void exportDot(std::string filename)
Definition: ScanGraph.cpp:232
std::istream & readNodePosesASCII(std::istream &s)
Definition: ScanGraph.cpp:552
#define OCTOMAP_DEBUG(...)
Definition: octomap_types.h:71
std::istream & read(std::istream &s)
Definition: Vector3.cpp:69
std::vector< ScanNode * >::const_iterator const_iterator
Definition: ScanGraph.h:174
Vector3 toEuler() const
Conversion to Euler angles.
Definition: Quaternion.cpp:124
void transform(pose6d transform)
Apply transform to each point.
Definition: Pointcloud.cpp:97
ScanNode * getNodeByID(unsigned int id)
will return NULL if node was not found
Definition: ScanGraph.cpp:248
pose6d pose
6D pose from which the scan was performed
Definition: ScanGraph.h:74
std::istream & readBinary(std::istream &s)
Definition: ScanGraph.cpp:63
std::istream & readBinary(std::istream &s)
Binary input operator.
Definition: Pose6D.cpp:134
void push_back(float x, float y, float z)
Definition: Pointcloud.h:61
std::vector< ScanEdge * > getOutEdges(ScanNode *node)
Definition: ScanGraph.cpp:283
unsigned int id
Definition: ScanGraph.h:75
std::ostream & writeBinary(std::ostream &s) const
Definition: ScanGraph.cpp:324
void clear()
Clears all nodes and edges, and will delete the corresponding objects.
Definition: ScanGraph.cpp:163
std::vector< ScanNode * >::iterator iterator
Definition: ScanGraph.h:173
ScanEdge * addEdge(ScanNode *first, ScanNode *second, pose6d constraint)
Definition: ScanGraph.cpp:187
Pose6D inv() const
Inversion.
Definition: Pose6D.cpp:65
std::ostream & writeBinary(std::ostream &s) const
Definition: Pointcloud.cpp:311
std::ostream & writeBinary(std::ostream &s) const
Definition: ScanGraph.cpp:52
std::ostream & writeEdgesASCII(std::ostream &s) const
Definition: ScanGraph.cpp:487
void transformScans()
Transform every scan according to its pose.
Definition: ScanGraph.cpp:307
std::vector< unsigned int > getNeighborIDs(unsigned int id)
Definition: ScanGraph.cpp:268
void crop(point3d lowerBound, point3d upperBound)
Cut graph (all containing Pointclouds) to given BBX in global coords.
Definition: ScanGraph.cpp:592
std::istream & readBinary(std::istream &s)
Definition: Pointcloud.cpp:285
std::ostream & writeBinary(std::ostream &s) const
Definition: ScanGraph.cpp:102
std::vector< ScanEdge * >::iterator edge_iterator
Definition: ScanGraph.h:183
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:49
void connectPrevious()
Connect previously added ScanNode to the one before that.
Definition: ScanGraph.cpp:222
Vector3 & trans()
Translational component.
Definition: Pose6D.h:80
std::ostream & writeNodePosesASCII(std::ostream &s) const
Definition: ScanGraph.cpp:539
This class represents a three-dimensional vector.
Definition: Vector3.h:50
#define OCTOMAP_WARNING_STR(args)
Definition: octomap_types.h:76
std::istream & readPlainASCII(std::istream &s)
Definition: ScanGraph.cpp:432
#define OCTOMAP_ERROR_STR(args)
Definition: octomap_types.h:78
std::vector< ScanEdge * >::const_iterator const_edge_iterator
Definition: ScanGraph.h:184
#define OCTOMAP_DEBUG_STR(args)
Definition: octomap_types.h:72
size_t size() const
Definition: Pointcloud.h:57
void transformAbsolute(pose6d transform)
Apply transform to each point, undo previous transforms.
Definition: Pointcloud.cpp:108
std::istream & readEdgesASCII(std::istream &s)
Definition: ScanGraph.cpp:506
Quaternion & rot()
Rotational component.
Definition: Pose6D.h:86
void cropEachScan(point3d lowerBound, point3d upperBound)
Cut Pointclouds to given BBX in local coords.
Definition: ScanGraph.cpp:584
void crop(point3d lowerBound, point3d upperBound)
Crop Pointcloud to given bounding box.
Definition: Pointcloud.cpp:157
#define OCTOMAP_ERROR(...)
Definition: octomap_types.h:77
std::vector< ScanEdge * > getInEdges(ScanNode *node)
Definition: ScanGraph.cpp:295
size_t getNumPoints(unsigned int max_id=-1) const
Definition: ScanGraph.cpp:607
std::ostream & writePoseASCII(std::ostream &s) const
Definition: ScanGraph.cpp:76
This class represents a Quaternion.
Definition: Quaternion.h:56
bool edgeExists(unsigned int first_id, unsigned int second_id)
Definition: ScanGraph.cpp:255
std::ostream & writeASCII(std::ostream &s) const
Definition: ScanGraph.cpp:130


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Wed Jun 5 2019 19:26:27