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


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Feb 28 2022 22:58:06