15 std::cerr <<
"\nUSAGE: " <<
self <<
" inputfile.bt [max_depth] (optional)\n\n";
22 const float& center_offset,
26 if (pos & 1) child_center(0) = parent_center(0) + center_offset;
27 else child_center(0) = parent_center(0) - center_offset;
30 if (pos & 2) child_center(1) = parent_center(1) + center_offset;
31 else child_center(1) = parent_center(1) - center_offset;
33 if (pos & 4) child_center(2) = parent_center(2) + center_offset;
34 else child_center(2) = parent_center(2) - center_offset;
39 unsigned int max_depth,
42 OcTree* tree,
bool occupied)
44 if ((depth <= max_depth) && (node != NULL) ) {
47 float center_offset = float(tree_center(0) / pow( 2., (
double) depth+1));
50 for (
unsigned int i=0; i<8; i++) {
61 double voxelSize = tree->
getResolution() * pow(2.,
double(16 - depth));
62 voxels.push_back(std::make_pair(parent_center - tree_center, voxelSize));
72 unsigned int max_depth,
77 if ((depth <= max_depth) && (node != NULL) ) {
80 double center_offset = tree_center(0) / pow(2., (
double) depth + 1);
83 for (
unsigned int i = 0; i < 8; i++) {
91 double voxelSize = resolution * pow(2.,
double(16 - depth));
92 voxels.push_back(std::make_pair(parent_center - tree_center, voxelSize));
98 void compareResults(
const std::list<OcTreeVolume>& list_iterator,
const std::list<OcTreeVolume>& list_depr){
99 EXPECT_EQ(list_iterator.size(), list_depr.size());
100 list<OcTreeVolume>::const_iterator list_it = list_iterator.begin();
101 list<OcTreeVolume>::const_iterator list_depr_it = list_depr.begin();
103 for (; list_it != list_iterator.end(); ++list_it, ++list_depr_it){
104 EXPECT_NEAR(list_it->first.x(), list_depr_it->first.x(), 0.005);
105 EXPECT_NEAR(list_it->first.y(), list_depr_it->first.y(), 0.005);
106 EXPECT_NEAR(list_it->first.z(), list_depr_it->first.z(), 0.005);
108 std::cout <<
"Resulting lists (size "<< list_iterator.size() <<
") identical\n";
114 return ( lhs.second < rhs.second
115 || (lhs.second == rhs.second &&
116 lhs.first.x() < rhs.first.x()
117 && lhs.first.y() < rhs.first.y()
118 && lhs.first.z() < rhs.first.z()));
122 double timediff(
const timeval& start,
const timeval& stop){
123 return (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec);
130 double temp_x,temp_y,temp_z;
140 OcTree::leaf_bbx_iterator it_bbx = tree->
begin_leafs_bbx(bbxMinKey,bbxMaxKey);
147 OcTree::leaf_iterator end = tree->
end_leafs();
151 for( ; it!= end && it_bbx != end_bbx; ++it, ++it_bbx){
164 typedef unordered_ns::unordered_map<OcTreeKey, double, OcTreeKey::KeyHash> KeyVolumeMap;
166 KeyVolumeMap bbxVoxels;
182 for (
unsigned i = 0; i < 3; ++i){
187 EXPECT_TRUE(currentKey[i] >= bbxMinKey[i] && currentKey[i] <= bbxMaxKey[i]);
190 bbxVoxels.insert(std::pair<OcTreeKey,double>(currentKey, it.getSize()));
193 std::cout <<
"Bounding box traversed ("<< count <<
" leaf nodes)\n\n";
197 for(OcTree::leaf_iterator it = tree->
begin(), end=tree->
end(); it!= end; ++it) {
199 if ( key[0] >= bbxMinKey[0] && key[0] <= bbxMaxKey[0]
200 && key[1] >= bbxMinKey[1] && key[1] <= bbxMaxKey[1]
201 && key[2] >= bbxMinKey[2] && key[2] <= bbxMaxKey[2])
203 KeyVolumeMap::iterator bbxIt = bbxVoxels.find(key);
212 int main(
int argc,
char** argv) {
217 string btFilename =
"";
218 unsigned char maxDepth = 16;
224 const unsigned char tree_depth(16);
225 const unsigned int tree_max_val(32768);
226 double time_it, time_depr;
228 if (argc <= 1|| argc >3 || strcmp(argv[1],
"-h") == 0){
232 btFilename = std::string(argv[1]);
234 maxDepth = (
unsigned char)atoi(argv[2]);
236 maxDepth = std::min((
unsigned char)16,maxDepth);
239 maxDepth = tree_depth;
246 size_t iteratedNodes = 0;
247 OcTree::tree_iterator t_it = emptyTree.
begin_tree(maxDepth);
248 OcTree::tree_iterator t_end = emptyTree.
end_tree();
250 for( ; t_it != t_end; ++t_it){
255 for(OcTree::leaf_iterator l_it = emptyTree.
begin_leafs(maxDepth), l_end=emptyTree.
end_leafs(); l_it!= l_end; ++l_it){
264 cout <<
"\nReading OcTree file\n===========================\n";
266 if (tree->
size()<= 1){
267 std::cout <<
"Error reading file, exiting!\n";
272 std::list<OcTreeVolume> list_depr;
273 std::list<OcTreeVolume> list_iterator;
278 gettimeofday(&start, NULL);
280 gettimeofday(&stop, NULL);
283 gettimeofday(&start, NULL);
284 size_t num_leafs_it = 0;
285 for(OcTree::leaf_iterator it = tree->
begin(), end=tree->
end(); it!= end; ++it) {
288 gettimeofday(&stop, NULL);
290 std::cout <<
"Number of leafs: " << num_leafs_it <<
" / " << num_leafs_recurs <<
", times: " 291 <<time_it <<
" / " << time_depr <<
"\n========================\n\n";
298 tree_center(0) = tree_center(1) = tree_center(2)
301 gettimeofday(&start, NULL);
303 gettimeofday(&stop, NULL);
306 gettimeofday(&start, NULL);
307 for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it){
308 if(tree->isNodeOccupied(*it))
311 list_iterator.push_back(
OcTreeVolume(it.getCoordinate(), it.getSize()));
315 gettimeofday(&stop, NULL);
318 std::cout <<
"Occupied lists traversed, times: " 319 <<time_it <<
" / " << time_depr <<
"\n";
321 std::cout <<
"========================\n\n";
327 list_iterator.clear();
329 gettimeofday(&start, NULL);
330 for(OcTree::leaf_iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it) {
331 if(!tree->isNodeOccupied(*it))
332 list_iterator.push_back(
OcTreeVolume(it.getCoordinate(), it.getSize()));
334 gettimeofday(&stop, NULL);
337 gettimeofday(&start, NULL);
338 getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree,
false);
339 gettimeofday(&stop, NULL);
342 std::cout <<
"Free lists traversed, times: " 343 <<time_it <<
" / " << time_depr <<
"\n";
345 std::cout <<
"========================\n\n";
352 list_iterator.clear();
355 gettimeofday(&start, NULL);
356 getVoxelsRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree->getResolution());
357 gettimeofday(&stop, NULL);
360 gettimeofday(&start, NULL);
361 for(OcTree::tree_iterator it = tree->begin_tree(maxDepth), end=tree->end_tree();
365 list_iterator.push_back(
OcTreeVolume(it.getCoordinate(), it.getSize()));
367 gettimeofday(&stop, NULL);
373 std::cout <<
"All inner lists traversed, times: " 374 <<time_it <<
" / " << time_depr <<
"\n";
376 std::cout <<
"========================\n\n";
381 gettimeofday(&start, NULL);
383 for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end();
390 gettimeofday(&stop, NULL);
393 std::cout <<
"Time to traverse all leafs at max depth " <<(
unsigned int)maxDepth <<
" ("<<count<<
" nodes): "<< time_it <<
" s\n\n";
409 for(OcTree::leaf_iterator it = simpleTree.
begin_leafs(maxDepth), end=simpleTree.
end_leafs(); it!= end; ++it) {
410 std::cout << it.getDepth() <<
" " <<
" "<<it.getCoordinate()<< std::endl;
414 std::cout <<
"Tests successful\n";
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
bool OcTreeVolumeSortPredicate(const OcTreeVolume &lhs, const OcTreeVolume &rhs)
double timediff(const timeval &start, const timeval &stop)
iterator begin(unsigned char maxDepth=0) const
const tree_iterator end_tree() const
OcTreeNode * getChild(unsigned int i)
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
void getVoxelsRecurs(std::list< OcTreeVolume > &voxels, unsigned int max_depth, OcTreeNode *node, unsigned int depth, const point3d &parent_center, const point3d &tree_center, double resolution)
mimics old deprecated behavior to compare against
const iterator end() const
const leaf_iterator end_leafs() const
leaf_bbx_iterator begin_leafs_bbx(const OcTreeKey &min, const OcTreeKey &max, unsigned char maxDepth=0) const
NODE * search(double x, double y, double z, unsigned int depth=0) const
std::pair< point3d, double > OcTreeVolume
A voxel defined by its center point3d and its side length.
bool childExists(unsigned int i) const
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
#define EXPECT_NEAR(a, b, prec)
void compareResults(const std::list< OcTreeVolume > &list_iterator, const std::list< OcTreeVolume > &list_depr)
compare two lists of octree nodes on equality
int main(int argc, char **argv)
#define EXPECT_FALSE(args)
virtual void getMetricMin(double &x, double &y, double &z)
minimum value of the bounding box of all known space in x, y, z
void printUsage(char *self)
This class represents a three-dimensional vector.
tree_iterator begin_tree(unsigned char maxDepth=0) const
virtual void getMetricMax(double &x, double &y, double &z)
maximum value of the bounding box of all known space in x, y, z
double getResolution() const
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
leaf_iterator begin_leafs(unsigned char maxDepth=0) const
void boundingBoxTest(OcTree *tree)
void computeChildCenter(const unsigned int &pos, const float ¢er_offset, const point3d &parent_center, point3d &child_center)
void getLeafNodesRecurs(std::list< OcTreeVolume > &voxels, unsigned int max_depth, OcTreeNode *node, unsigned int depth, const point3d &parent_center, const point3d &tree_center, OcTree *tree, bool occupied)
mimics old deprecated behavior to compare against
const leaf_bbx_iterator end_leafs_bbx() const
#define EXPECT_TRUE(args)
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree's parameter for "occupancy" ...
virtual size_t size() const