octree2pointcloud.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 <string.h>
35 #include <stdlib.h>
36 #include <iostream>
37 #include <fstream>
38 
39 #include <octomap/octomap.h>
40 #include <octomap/octomap_timing.h>
41 
42 using namespace std;
43 using namespace octomap;
44 
45 void printUsage(char* self){
46  cerr << "USAGE: " << self << " <InputFile.bt> <OutputFile.pcd>\n";
47  cerr << "This tool creates a point cloud of the occupied cells\n";
48  exit(0);
49 }
50 
51 
52 int main(int argc, char** argv) {
53  if (argc != 3)
54  printUsage(argv[0]);
55 
56  string inputFilename = argv[1];
57  string outputFilename = argv[2];
58 
59  OcTree* tree = new OcTree(0.1);
60  if (!tree->readBinary(inputFilename)){
61  OCTOMAP_ERROR("Could not open file, exiting.\n");
62  exit(1);
63  }
64 
65  unsigned int maxDepth = tree->getTreeDepth();
66  cout << "tree depth is " << maxDepth << endl;
67 
68  // expand collapsed occupied nodes until all occupied leaves are at maximum depth
69  vector<OcTreeNode*> collapsed_occ_nodes;
70  do {
71  collapsed_occ_nodes.clear();
72  for (OcTree::iterator it = tree->begin(); it != tree->end(); ++it)
73  {
74  if(tree->isNodeOccupied(*it) && it.getDepth() < maxDepth)
75  {
76  collapsed_occ_nodes.push_back(&(*it));
77  }
78  }
79  for (vector<OcTreeNode*>::iterator it = collapsed_occ_nodes.begin(); it != collapsed_occ_nodes.end(); ++it)
80  {
81  tree->expandNode(*it);
82  }
83  cout << "expanded " << collapsed_occ_nodes.size() << " nodes" << endl;
84  } while(collapsed_occ_nodes.size() > 0);
85 
86  vector<point3d> pcl;
87  for (OcTree::iterator it = tree->begin(); it != tree->end(); ++it)
88  {
89  if(tree->isNodeOccupied(*it))
90  {
91  pcl.push_back(it.getCoordinate());
92  }
93  }
94 
95  delete tree;
96 
97  ofstream f(outputFilename.c_str(), ofstream::out);
98  f << "# .PCD v0.7" << endl
99  << "VERSION 0.7" << endl
100  << "FIELDS x y z" << endl
101  << "SIZE 4 4 4" << endl
102  << "TYPE F F F" << endl
103  << "COUNT 1 1 1" << endl
104  << "WIDTH " << pcl.size() << endl
105  << "HEIGHT 1" << endl
106  << "VIEWPOINT 0 0 0 0 0 0 1" << endl
107  << "POINTS " << pcl.size() << endl
108  << "DATA ascii" << endl;
109  for (size_t i = 0; i < pcl.size(); i++)
110  f << pcl[i].x() << " " << pcl[i].y() << " " << pcl[i].z() << endl;
111  f.close();
112 
113  return 0;
114 }
iterator begin(unsigned char maxDepth=0) const
const iterator end() const
unsigned int getTreeDepth() const
virtual void expandNode(NODE *node)
void printUsage(char *self)
int main(int argc, char **argv)
#define OCTOMAP_ERROR(...)
Definition: octomap_types.h:77
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree&#39;s parameter for "occupancy" ...
virtual size_t size() const


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