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 = tree->
getResolution() * 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){
262 for(OcTree::leaf_bbx_iterator l_it = emptyTree.
begin_leafs_bbx(ptMinBBX, ptMaxBBX, maxDepth), l_end=emptyTree.
end_leafs_bbx(); l_it!= l_end; ++l_it){
269 for(OcTree::leaf_bbx_iterator l_it = emptyTree.
begin_leafs_bbx(minBBX, maxBBX, maxDepth), l_end=emptyTree.
end_leafs_bbx(); l_it!= l_end; ++l_it){
279 cout <<
"\nReading OcTree file\n===========================\n";
281 if (tree->
size()<= 1){
282 std::cout <<
"Error reading file, exiting!\n";
287 std::list<OcTreeVolume> list_depr;
288 std::list<OcTreeVolume> list_iterator;
293 gettimeofday(&start, NULL);
295 gettimeofday(&stop, NULL);
298 gettimeofday(&start, NULL);
299 size_t num_leafs_it = 0;
300 for(OcTree::leaf_iterator it = tree->
begin(), end=tree->
end(); it!= end; ++it) {
303 gettimeofday(&stop, NULL);
305 std::cout <<
"Number of leafs: " << num_leafs_it <<
" / " << num_leafs_recurs <<
", times: " 306 <<time_it <<
" / " << time_depr <<
"\n========================\n\n";
313 tree_center(0) = tree_center(1) = tree_center(2)
316 gettimeofday(&start, NULL);
318 gettimeofday(&stop, NULL);
321 gettimeofday(&start, NULL);
322 for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it){
323 if(tree->isNodeOccupied(*it))
326 list_iterator.push_back(
OcTreeVolume(it.getCoordinate(), it.getSize()));
330 gettimeofday(&stop, NULL);
333 std::cout <<
"Occupied lists traversed, times: " 334 <<time_it <<
" / " << time_depr <<
"\n";
336 std::cout <<
"========================\n\n";
342 list_iterator.clear();
344 gettimeofday(&start, NULL);
345 for(OcTree::leaf_iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it) {
346 if(!tree->isNodeOccupied(*it))
347 list_iterator.push_back(
OcTreeVolume(it.getCoordinate(), it.getSize()));
349 gettimeofday(&stop, NULL);
352 gettimeofday(&start, NULL);
353 getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree,
false);
354 gettimeofday(&stop, NULL);
357 std::cout <<
"Free lists traversed, times: " 358 <<time_it <<
" / " << time_depr <<
"\n";
360 std::cout <<
"========================\n\n";
367 list_iterator.clear();
370 gettimeofday(&start, NULL);
371 getVoxelsRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree);
372 gettimeofday(&stop, NULL);
375 gettimeofday(&start, NULL);
376 for(OcTree::tree_iterator it = tree->begin_tree(maxDepth), end=tree->end_tree();
380 list_iterator.push_back(
OcTreeVolume(it.getCoordinate(), it.getSize()));
382 gettimeofday(&stop, NULL);
388 std::cout <<
"All inner lists traversed, times: " 389 <<time_it <<
" / " << time_depr <<
"\n";
391 std::cout <<
"========================\n\n";
396 gettimeofday(&start, NULL);
398 for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end();
405 gettimeofday(&stop, NULL);
408 std::cout <<
"Time to traverse all leafs at max depth " <<(
unsigned int)maxDepth <<
" ("<<count<<
" nodes): "<< time_it <<
" s\n\n";
424 for(OcTree::leaf_iterator it = simpleTree.
begin_leafs(maxDepth), end=simpleTree.
end_leafs(); it!= end; ++it) {
425 std::cout << it.getDepth() <<
" " <<
" "<<it.getCoordinate()<< std::endl;
429 std::cout <<
"Tests successful\n";
tree_iterator begin_tree(unsigned char maxDepth=0) const
const tree_iterator end_tree() const
bool OcTreeVolumeSortPredicate(const OcTreeVolume &lhs, const OcTreeVolume &rhs)
double timediff(const timeval &start, const timeval &stop)
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
const iterator end() const
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
NODE * getNodeChild(NODE *node, unsigned int childIdx) const
std::pair< point3d, double > OcTreeVolume
A voxel defined by its center point3d and its side length.
const leaf_bbx_iterator end_leafs_bbx() 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
void getVoxelsRecurs(std::list< OcTreeVolume > &voxels, unsigned int max_depth, OcTreeNode *node, unsigned int depth, const point3d &parent_center, const point3d &tree_center, OcTree *tree)
mimics old deprecated behavior to compare against
int main(int argc, char **argv)
leaf_iterator begin_leafs(unsigned char maxDepth=0) const
#define EXPECT_FALSE(args)
NODE * search(double x, double y, double z, unsigned int depth=0) const
iterator begin(unsigned char maxDepth=0) const
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
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)
double getResolution() const
leaf_bbx_iterator begin_leafs_bbx(const OcTreeKey &min, const OcTreeKey &max, unsigned char maxDepth=0) const
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
This class represents a three-dimensional vector.
virtual void getMetricMax(double &x, double &y, double &z)
maximum value of the bounding box of all known space in x, y, z
const leaf_iterator end_leafs() const
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
virtual size_t size() const
void boundingBoxTest(OcTree *tree)
void computeChildCenter(const unsigned int &pos, const float ¢er_offset, const point3d &parent_center, point3d &child_center)
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree's parameter for "occupancy" ...
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
#define EXPECT_TRUE(args)
bool nodeChildExists(const NODE *node, unsigned int childIdx) const
bool nodeHasChildren(const NODE *node) const