00001
00002 #include <stdio.h>
00003 #include <stdlib.h>
00004 #include <string.h>
00005
00006 #include <octomap/octomap_timing.h>
00007 #include <octomap/octomap.h>
00008 #include <octomap/math/Utils.h>
00009 #include "testing.h"
00010
00011 using namespace std;
00012 using namespace octomap;
00013
00014 void printUsage(char* self){
00015 std::cerr << "\nUSAGE: " << self << " inputfile.bt [max_depth] (optional)\n\n";
00016
00017
00018 exit(1);
00019 }
00020
00021 void computeChildCenter (const unsigned int& pos,
00022 const float& center_offset,
00023 const point3d& parent_center,
00024 point3d& child_center) {
00025
00026 if (pos & 1) child_center(0) = parent_center(0) + center_offset;
00027 else child_center(0) = parent_center(0) - center_offset;
00028
00029
00030 if (pos & 2) child_center(1) = parent_center(1) + center_offset;
00031 else child_center(1) = parent_center(1) - center_offset;
00032
00033 if (pos & 4) child_center(2) = parent_center(2) + center_offset;
00034 else child_center(2) = parent_center(2) - center_offset;
00035 }
00036
00038 void getLeafNodesRecurs(std::list<OcTreeVolume>& voxels,
00039 unsigned int max_depth,
00040 OcTreeNode* node, unsigned int depth,
00041 const point3d& parent_center, const point3d& tree_center,
00042 OcTree* tree, bool occupied)
00043 {
00044 if ((depth <= max_depth) && (node != NULL) ) {
00045 if (node->hasChildren() && (depth != max_depth)) {
00046
00047 double center_offset = tree_center(0) / pow( 2., (double) depth+1);
00048 point3d search_center;
00049
00050 for (unsigned int i=0; i<8; i++) {
00051 if (node->childExists(i)) {
00052
00053 computeChildCenter(i, center_offset, parent_center, search_center);
00054 getLeafNodesRecurs(voxels, max_depth, node->getChild(i), depth+1, search_center, tree_center, tree, occupied);
00055
00056 }
00057 }
00058 }
00059 else {
00060 if (tree->isNodeOccupied(node) == occupied){
00061 double voxelSize = tree->getResolution() * pow(2., double(16 - depth));
00062 voxels.push_back(std::make_pair(parent_center - tree_center, voxelSize));
00063
00064 }
00065 }
00066 }
00067 }
00068
00069
00071 void getVoxelsRecurs(std::list<OcTreeVolume>& voxels,
00072 unsigned int max_depth,
00073 OcTreeNode* node, unsigned int depth,
00074 const point3d& parent_center, const point3d& tree_center,
00075 double resolution){
00076
00077 if ((depth <= max_depth) && (node != NULL) ) {
00078 if (node->hasChildren() && (depth != max_depth)) {
00079
00080 double center_offset = tree_center(0) / pow(2., (double) depth + 1);
00081 point3d search_center;
00082
00083 for (unsigned int i = 0; i < 8; i++) {
00084 if (node->childExists(i)) {
00085 computeChildCenter(i, (float) center_offset, parent_center, search_center);
00086 getVoxelsRecurs(voxels, max_depth, node->getChild(i), depth + 1, search_center, tree_center, resolution);
00087
00088 }
00089 }
00090 }
00091 double voxelSize = resolution * pow(2., double(16 - depth));
00092 voxels.push_back(std::make_pair(parent_center - tree_center, voxelSize));
00093 }
00094 }
00095
00096
00098 void compareResults(const std::list<OcTreeVolume>& list_iterator, const std::list<OcTreeVolume>& list_depr){
00099 EXPECT_EQ(list_iterator.size(), list_depr.size());
00100 list<OcTreeVolume>::const_iterator list_it = list_iterator.begin();
00101 list<OcTreeVolume>::const_iterator list_depr_it = list_depr.begin();
00102
00103 for (; list_it != list_iterator.end(); ++list_it, ++list_depr_it){
00104 EXPECT_NEAR(list_it->first.x(), list_depr_it->first.x(), 0.005);
00105 EXPECT_NEAR(list_it->first.y(), list_depr_it->first.y(), 0.005);
00106 EXPECT_NEAR(list_it->first.z(), list_depr_it->first.z(), 0.005);
00107 }
00108 std::cout << "Resulting lists (size "<< list_iterator.size() << ") identical\n";
00109 }
00110
00111
00112 bool OcTreeVolumeSortPredicate(const OcTreeVolume& lhs, const OcTreeVolume& rhs)
00113 {
00114 return ( lhs.second < rhs.second
00115 || (lhs.second == rhs.second &&
00116 lhs.first.x() < rhs.first.x()
00117 && lhs.first.y() < rhs.first.y()
00118 && lhs.first.z() < rhs.first.z()));
00119 }
00120
00121
00122 double timediff(const timeval& start, const timeval& stop){
00123 return (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec);
00124 }
00125
00126 int main(int argc, char** argv) {
00127
00128
00129
00130
00131 string btFilename = "";
00132 unsigned char maxDepth = 16;
00133
00134
00135
00136 timeval start;
00137 timeval stop;
00138 const unsigned char tree_depth(16);
00139 const unsigned int tree_max_val(32768);
00140 double time_it, time_depr;
00141
00142 if (argc <= 1|| argc >3 || strcmp(argv[1], "-h") == 0){
00143 printUsage(argv[0]);
00144 }
00145
00146 btFilename = std::string(argv[1]);
00147 if (argc > 2){
00148 maxDepth = (unsigned char)atoi(argv[2]);
00149 }
00150 maxDepth = std::min((unsigned char)16,maxDepth);
00151
00152 if (maxDepth== 0)
00153 maxDepth = tree_depth;
00154
00155
00156 OcTree emptyTree(0.2);
00157 EXPECT_EQ(emptyTree.size(), 0);
00158 EXPECT_EQ(emptyTree.calcNumNodes(), 0);
00159
00160 size_t iteratedNodes = 0;
00161 OcTree::tree_iterator t_it = emptyTree.begin_tree(maxDepth);
00162 OcTree::tree_iterator t_end = emptyTree.end_tree();
00163 EXPECT_TRUE (t_it == t_end);
00164 for( ; t_it != t_end; ++t_it){
00165 iteratedNodes++;
00166 }
00167 EXPECT_EQ(iteratedNodes, 0);
00168
00169
00170 for(OcTree::leaf_iterator l_it = emptyTree.begin_leafs(maxDepth), l_end=emptyTree.end_leafs(); l_it!= l_end; ++l_it){
00171 iteratedNodes++;
00172 }
00173 EXPECT_EQ(iteratedNodes, 0);
00174
00175
00176
00177
00178
00179 cout << "\nReading OcTree file\n===========================\n";
00180 OcTree* tree = new OcTree(btFilename);
00181 if (tree->size()<= 1){
00182 std::cout << "Error reading file, exiting!\n";
00183 return 1;
00184 }
00185
00186 size_t count;
00187 std::list<OcTreeVolume> list_depr;
00188 std::list<OcTreeVolume> list_iterator;
00189
00193 gettimeofday(&start, NULL);
00194 size_t num_leafs_recurs = tree->getNumLeafNodes();
00195 gettimeofday(&stop, NULL);
00196 time_depr = timediff(start, stop);
00197
00198 gettimeofday(&start, NULL);
00199 size_t num_leafs_it = 0;
00200 for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) {
00201 num_leafs_it++;
00202 }
00203 gettimeofday(&stop, NULL);
00204 time_it = timediff(start, stop);
00205 std::cout << "Number of leafs: " << num_leafs_it << " / " << num_leafs_recurs << ", times: "
00206 <<time_it << " / " << time_depr << "\n========================\n\n";
00207
00208
00212 point3d tree_center;
00213 tree_center(0) = tree_center(1) = tree_center(2)
00214 = (float) (((double) tree_max_val) * tree->getResolution());
00215
00216 gettimeofday(&start, NULL);
00217 getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, true);
00218 gettimeofday(&stop, NULL);
00219 time_depr = timediff(start, stop);
00220
00221 gettimeofday(&start, NULL);
00222 for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it){
00223 if(tree->isNodeOccupied(*it))
00224 {
00225
00226 list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
00227 }
00228
00229 }
00230 gettimeofday(&stop, NULL);
00231 time_it = timediff(start, stop);
00232
00233 std::cout << "Occupied lists traversed, times: "
00234 <<time_it << " / " << time_depr << "\n";
00235 compareResults(list_iterator, list_depr);
00236 std::cout << "========================\n\n";
00237
00238
00242 list_iterator.clear();
00243 list_depr.clear();
00244 gettimeofday(&start, NULL);
00245 for(OcTree::leaf_iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it) {
00246 if(!tree->isNodeOccupied(*it))
00247 list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
00248 }
00249 gettimeofday(&stop, NULL);
00250 time_it = timediff(start, stop);
00251
00252 gettimeofday(&start, NULL);
00253 getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, false);
00254 gettimeofday(&stop, NULL);
00255 time_depr = timediff(start, stop);
00256
00257 std::cout << "Free lists traversed, times: "
00258 <<time_it << " / " << time_depr << "\n";
00259 compareResults(list_iterator, list_depr);
00260 std::cout << "========================\n\n";
00261
00262
00263
00267 list_iterator.clear();
00268 list_depr.clear();
00269
00270 gettimeofday(&start, NULL);
00271 getVoxelsRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree->getResolution());
00272 gettimeofday(&stop, NULL);
00273 time_depr = timediff(start, stop);
00274
00275 gettimeofday(&start, NULL);
00276 for(OcTree::tree_iterator it = tree->begin_tree(maxDepth), end=tree->end_tree();
00277 it!= end; ++it){
00278
00279
00280 list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize()));
00281 }
00282 gettimeofday(&stop, NULL);
00283 time_it = timediff(start, stop);
00284
00285 list_iterator.sort(OcTreeVolumeSortPredicate);
00286 list_depr.sort(OcTreeVolumeSortPredicate);
00287
00288 std::cout << "All inner lists traversed, times: "
00289 <<time_it << " / " << time_depr << "\n";
00290 compareResults(list_iterator, list_depr);
00291 std::cout << "========================\n\n";
00292
00293
00294
00295
00296 gettimeofday(&start, NULL);
00297 count = 0;
00298 for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end();
00299 it!= end; ++it){
00300
00301
00302 count++;
00303 }
00304
00305 gettimeofday(&stop, NULL);
00306 time_it = timediff(start, stop);
00307
00308 std::cout << "Time to traverse all leafs at max depth " <<(unsigned int)maxDepth <<" ("<<count<<" nodes): "<< time_it << " s\n\n";
00309
00310
00311
00312
00316
00317
00318 OcTreeKey bbxMinKey, bbxMaxKey;
00319 double temp_x,temp_y,temp_z;
00320 tree->getMetricMin(temp_x,temp_y,temp_z);
00321 octomap::point3d bbxMin(temp_x,temp_y,temp_z);
00322
00323 tree->getMetricMax(temp_x,temp_y,temp_z);
00324 octomap::point3d bbxMax(temp_x,temp_y,temp_z);
00325
00326 EXPECT_TRUE(tree->coordToKeyChecked(bbxMin, bbxMinKey));
00327 EXPECT_TRUE(tree->coordToKeyChecked(bbxMax, bbxMaxKey));
00328
00329 OcTree::leaf_bbx_iterator it_bbx = tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey);
00330 EXPECT_TRUE(it_bbx == tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey));
00331 OcTree::leaf_bbx_iterator end_bbx = tree->end_leafs_bbx();
00332 EXPECT_TRUE(end_bbx == tree->end_leafs_bbx());
00333
00334 OcTree::leaf_iterator it = tree->begin_leafs();
00335 EXPECT_TRUE(it == tree->begin_leafs());
00336 OcTree::leaf_iterator end = tree->end_leafs();
00337 EXPECT_TRUE(end == tree->end_leafs());
00338
00339
00340 for( ; it!= end && it_bbx != end_bbx; ++it, ++it_bbx){
00341 EXPECT_TRUE(it == it_bbx);
00342 }
00343 EXPECT_TRUE(it == end && it_bbx == end_bbx);
00344
00345
00346
00347 tree->expand();
00348 bbxMin = point3d(-1, -1, - 1);
00349 bbxMax = point3d(3, 2, 1);
00350 EXPECT_TRUE(tree->coordToKeyChecked(bbxMin, bbxMinKey));
00351 EXPECT_TRUE(tree->coordToKeyChecked(bbxMax, bbxMaxKey));
00352
00353 typedef unordered_ns::unordered_map<OcTreeKey, double, OcTreeKey::KeyHash> KeyVolumeMap;
00354
00355 KeyVolumeMap bbxVoxels;
00356
00357 count = 0;
00358 for(OcTree::leaf_bbx_iterator it = tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey), end=tree->end_leafs_bbx();
00359 it!= end; ++it)
00360 {
00361 count++;
00362 OcTreeKey currentKey = it.getKey();
00363
00364 EXPECT_FALSE(it->hasChildren());
00365
00366
00367 OcTreeNode* node = tree->search(currentKey);
00368 EXPECT_TRUE(node);
00369 EXPECT_EQ(node, &(*it));
00370
00371 for (unsigned i = 0; i < 3; ++i){
00372
00373
00374
00375
00376 EXPECT_TRUE(currentKey[i] >= bbxMinKey[i] && currentKey[i] <= bbxMaxKey[i]);
00377 }
00378
00379 bbxVoxels.insert(std::pair<OcTreeKey,double>(currentKey, it.getSize()));
00380 }
00381 EXPECT_EQ(bbxVoxels.size(), count);
00382 std::cout << "Bounding box traversed ("<< count << " leaf nodes)\n\n";
00383
00384
00385
00386 for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) {
00387 OcTreeKey key = it.getKey();
00388 if ( key[0] >= bbxMinKey[0] && key[0] <= bbxMaxKey[0]
00389 && key[1] >= bbxMinKey[1] && key[1] <= bbxMaxKey[1]
00390 && key[2] >= bbxMinKey[2] && key[2] <= bbxMaxKey[2])
00391 {
00392 KeyVolumeMap::iterator bbxIt = bbxVoxels.find(key);
00393 EXPECT_FALSE(bbxIt == bbxVoxels.end());
00394 EXPECT_TRUE(key == bbxIt->first);
00395 EXPECT_EQ(it.getSize(), bbxIt->second);
00396 }
00397
00398 }
00399
00400
00401 OcTree simpleTree(0.01);
00402 simpleTree.updateNode(point3d(10, 10, 10), 5.0f);
00403 for(OcTree::leaf_iterator it = simpleTree.begin_leafs(maxDepth), end=simpleTree.end_leafs(); it!= end; ++it) {
00404 std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl;
00405 }
00406
00407
00408 std::cout << "Tests successful\n";
00409
00410
00411 return 0;
00412 }
00413