int main(int argc, char** argv) { //############################################################## string btFilename = ""; unsigned char maxDepth = 16; // test timing: timeval start; timeval stop; const unsigned char tree_depth(16); const unsigned int tree_max_val(32768); double time_it, time_depr; if (argc <= 1|| argc >3 || strcmp(argv[1], "-h") == 0){ printUsage(argv[0]); } btFilename = std::string(argv[1]); if (argc > 2){ maxDepth = (unsigned char)atoi(argv[2]); } maxDepth = std::min((unsigned char)16,maxDepth); if (maxDepth== 0) maxDepth = tree_depth; // iterate over empty tree: OcTree emptyTree(0.2); EXPECT_EQ(emptyTree.size(), 0); EXPECT_EQ(emptyTree.calcNumNodes(), 0); size_t iteratedNodes = 0; OcTree::tree_iterator t_it = emptyTree.begin_tree(maxDepth); OcTree::tree_iterator t_end = emptyTree.end_tree(); EXPECT_TRUE (t_it == t_end); for( ; t_it != t_end; ++t_it){ iteratedNodes++; } EXPECT_EQ(iteratedNodes, 0); for(OcTree::leaf_iterator l_it = emptyTree.begin_leafs(maxDepth), l_end=emptyTree.end_leafs(); l_it!= l_end; ++l_it){ iteratedNodes++; } EXPECT_EQ(iteratedNodes, 0); cout << "\nReading OcTree file\n===========================\n"; OcTree* tree = new OcTree(btFilename); if (tree->size()<= 1){ std::cout << "Error reading file, exiting!\n"; return 1; } size_t count; std::list<OcTreeVolume> list_depr; std::list<OcTreeVolume> list_iterator; /** * get number of nodes: */ gettimeofday(&start, NULL); // start timer size_t num_leafs_recurs = tree->getNumLeafNodes(); gettimeofday(&stop, NULL); // stop timer time_depr = timediff(start, stop); gettimeofday(&start, NULL); // start timer size_t num_leafs_it = 0; for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) { num_leafs_it++; } gettimeofday(&stop, NULL); // stop timer time_it = timediff(start, stop); std::cout << "Number of leafs: " << num_leafs_it << " / " << num_leafs_recurs << ", times: " <<time_it << " / " << time_depr << "\n========================\n\n"; /** * get all occupied leafs */ point3d tree_center; tree_center(0) = tree_center(1) = tree_center(2) = (float) (((double) tree_max_val) * tree->getResolution()); gettimeofday(&start, NULL); // start timer getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, true); gettimeofday(&stop, NULL); // stop timer time_depr = timediff(start, stop); gettimeofday(&start, NULL); // start timer for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it){ if(tree->isNodeOccupied(*it)) { //count ++; list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize())); } } gettimeofday(&stop, NULL); // stop timer time_it = timediff(start, stop); std::cout << "Occupied lists traversed, times: " <<time_it << " / " << time_depr << "\n"; compareResults(list_iterator, list_depr); std::cout << "========================\n\n"; /** * get all free leafs */ list_iterator.clear(); list_depr.clear(); gettimeofday(&start, NULL); // start timer for(OcTree::leaf_iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it) { if(!tree->isNodeOccupied(*it)) list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize())); } gettimeofday(&stop, NULL); // stop timer time_it = timediff(start, stop); gettimeofday(&start, NULL); // start timer getLeafNodesRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree, false); gettimeofday(&stop, NULL); // stop timer time_depr = timediff(start, stop); std::cout << "Free lists traversed, times: " <<time_it << " / " << time_depr << "\n"; compareResults(list_iterator, list_depr); std::cout << "========================\n\n"; /** * get all volumes */ list_iterator.clear(); list_depr.clear(); gettimeofday(&start, NULL); // start timer getVoxelsRecurs(list_depr,maxDepth,tree->getRoot(), 0, tree_center, tree_center, tree->getResolution()); gettimeofday(&stop, NULL); // stop timer time_depr = timediff(start, stop); gettimeofday(&start, NULL); // start timers for(OcTree::tree_iterator it = tree->begin_tree(maxDepth), end=tree->end_tree(); it!= end; ++it){ //count ++; //std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl; list_iterator.push_back(OcTreeVolume(it.getCoordinate(), it.getSize())); } gettimeofday(&stop, NULL); // stop timer time_it = timediff(start, stop); list_iterator.sort(OcTreeVolumeSortPredicate); list_depr.sort(OcTreeVolumeSortPredicate); std::cout << "All inner lists traversed, times: " <<time_it << " / " << time_depr << "\n"; compareResults(list_iterator, list_depr); std::cout << "========================\n\n"; // traverse all leaf nodes, timing: gettimeofday(&start, NULL); // start timers count = 0; for(OcTree::iterator it = tree->begin(maxDepth), end=tree->end(); it!= end; ++it){ // do something: // std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl; count++; } gettimeofday(&stop, NULL); // stop timer time_it = timediff(start, stop); std::cout << "Time to traverse all leafs at max depth " <<(unsigned int)maxDepth <<" ("<<count<<" nodes): "<< time_it << " s\n\n"; /** * bounding box tests */ //tree->expand(); // test complete tree (should be equal to no bbx) OcTreeKey bbxMinKey, bbxMaxKey; double temp_x,temp_y,temp_z; tree->getMetricMin(temp_x,temp_y,temp_z); octomap::point3d bbxMin(temp_x,temp_y,temp_z); tree->getMetricMax(temp_x,temp_y,temp_z); octomap::point3d bbxMax(temp_x,temp_y,temp_z); EXPECT_TRUE(tree->coordToKeyChecked(bbxMin, bbxMinKey)); EXPECT_TRUE(tree->coordToKeyChecked(bbxMax, bbxMaxKey)); OcTree::leaf_bbx_iterator it_bbx = tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey); EXPECT_TRUE(it_bbx == tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey)); OcTree::leaf_bbx_iterator end_bbx = tree->end_leafs_bbx(); EXPECT_TRUE(end_bbx == tree->end_leafs_bbx()); OcTree::leaf_iterator it = tree->begin_leafs(); EXPECT_TRUE(it == tree->begin_leafs()); OcTree::leaf_iterator end = tree->end_leafs(); EXPECT_TRUE(end == tree->end_leafs()); for( ; it!= end && it_bbx != end_bbx; ++it, ++it_bbx){ EXPECT_TRUE(it == it_bbx); } EXPECT_TRUE(it == end && it_bbx == end_bbx); // now test an actual bounding box: tree->expand(); // (currently only works properly for expanded tree (no multires) bbxMin = point3d(-1, -1, - 1); bbxMax = point3d(3, 2, 1); EXPECT_TRUE(tree->coordToKeyChecked(bbxMin, bbxMinKey)); EXPECT_TRUE(tree->coordToKeyChecked(bbxMax, bbxMaxKey)); typedef unordered_ns::unordered_map<OcTreeKey, double, OcTreeKey::KeyHash> KeyVolumeMap; KeyVolumeMap bbxVoxels; count = 0; for(OcTree::leaf_bbx_iterator it = tree->begin_leafs_bbx(bbxMinKey,bbxMaxKey), end=tree->end_leafs_bbx(); it!= end; ++it) { count++; OcTreeKey currentKey = it.getKey(); // leaf is actually a leaf: EXPECT_FALSE(it->hasChildren()); // leaf exists in tree: OcTreeNode* node = tree->search(currentKey); EXPECT_TRUE(node); EXPECT_EQ(node, &(*it)); // all leafs are actually in the bbx: for (unsigned i = 0; i < 3; ++i){ // if (!(currentKey[i] >= bbxMinKey[i] && currentKey[i] <= bbxMaxKey[i])){ // std::cout << "Key failed: " << i << " " << currentKey[i] << " "<< bbxMinKey[i] << " "<< bbxMaxKey[i] // << "size: "<< it.getSize()<< std::endl; // } EXPECT_TRUE(currentKey[i] >= bbxMinKey[i] && currentKey[i] <= bbxMaxKey[i]); } bbxVoxels.insert(std::pair<OcTreeKey,double>(currentKey, it.getSize())); } EXPECT_EQ(bbxVoxels.size(), count); std::cout << "Bounding box traversed ("<< count << " leaf nodes)\n\n"; // compare with manual BBX check on all leafs: for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) { OcTreeKey key = it.getKey(); if ( key[0] >= bbxMinKey[0] && key[0] <= bbxMaxKey[0] && key[1] >= bbxMinKey[1] && key[1] <= bbxMaxKey[1] && key[2] >= bbxMinKey[2] && key[2] <= bbxMaxKey[2]) { KeyVolumeMap::iterator bbxIt = bbxVoxels.find(key); EXPECT_FALSE(bbxIt == bbxVoxels.end()); EXPECT_TRUE(key == bbxIt->first); EXPECT_EQ(it.getSize(), bbxIt->second); } } // test tree with one node: OcTree simpleTree(0.01); simpleTree.updateNode(point3d(10, 10, 10), 5.0f); for(OcTree::leaf_iterator it = simpleTree.begin_leafs(maxDepth), end=simpleTree.end_leafs(); it!= end; ++it) { std::cout << it.getDepth() << " " << " "<<it.getCoordinate()<< std::endl; } std::cout << "Tests successful\n"; return 0; }
int main(int argc, char** argv) { if (argc != 2){ std::cerr << "Error: you need to specify a test as argument" << std::endl; return 1; // exit 1 means failure } std::string test_name (argv[1]); // ------------------------------------------------------------ if (test_name == "MathVector") { // test constructors Vector3* twos = new Vector3(); Vector3* ones = new Vector3(1,1,1); for (int i=0;i<3;i++) { (*twos)(i) = 2; } // test basic operations Vector3 subtraction = *twos - *ones; Vector3 addition = *twos + *ones; Vector3 multiplication = *twos * 2.; for (int i=0;i<3;i++) { EXPECT_FLOAT_EQ (subtraction(i), 1.); EXPECT_FLOAT_EQ (addition(i), 3.); EXPECT_FLOAT_EQ (multiplication(i), 4.); } // copy constructor Vector3 rotation = *ones; // rotation rotation.rotate_IP (M_PI, 1., 0.1); EXPECT_FLOAT_EQ (rotation.x(), 1.2750367); EXPECT_FLOAT_EQ (rotation.y(), (-1.1329513)); EXPECT_FLOAT_EQ (rotation.z(), 0.30116868); // ------------------------------------------------------------ } else if (test_name == "MathPose") { // constructors Pose6D a (1.0f, 0.1f, 0.1f, 0.0f, 0.1f, (float) M_PI/4. ); Pose6D b; Vector3 trans(1.0f, 0.1f, 0.1f); Quaternion rot(0.0f, 0.1f, (float) M_PI/4.); Pose6D c(trans, rot); // comparator EXPECT_TRUE ( a == c); // toEuler EXPECT_FLOAT_EQ (c.yaw() , M_PI/4.); // transform Vector3 t = c.transform (trans); EXPECT_FLOAT_EQ (t.x() , 1.6399229); EXPECT_FLOAT_EQ (t.y() , 0.8813442); EXPECT_FLOAT_EQ (t.z() , 0.099667005); // inverse transform Pose6D c_inv = c.inv(); Vector3 t2 = c_inv.transform (t); EXPECT_FLOAT_EQ (t2.x() , trans.x()); EXPECT_FLOAT_EQ (t2.y() , trans.y()); EXPECT_FLOAT_EQ (t2.z() , trans.z()); // ------------------------------------------------------------ } else if (test_name == "InsertRay") { double p = 0.5; EXPECT_FLOAT_EQ(p, probability(logodds(p))); p = 0.1; EXPECT_FLOAT_EQ(p, probability(logodds(p))); p = 0.99; EXPECT_FLOAT_EQ(p, probability(logodds(p))); float l = 0; EXPECT_FLOAT_EQ(l, logodds(probability(l))); l = -4; EXPECT_FLOAT_EQ(l, logodds(probability(l))); l = 2; EXPECT_FLOAT_EQ(l, logodds(probability(l))); OcTree tree (0.05); tree.setProbHit(0.7); tree.setProbMiss(0.4); point3d origin (0.01f, 0.01f, 0.02f); point3d point_on_surface (2.01f,0.01f,0.01f); for (int i=0; i<360; i++) { for (int j=0; j<360; j++) { EXPECT_TRUE (tree.insertRay(origin, origin+point_on_surface)); point_on_surface.rotate_IP (0,0,DEG2RAD(1.)); } point_on_surface.rotate_IP (0,DEG2RAD(1.),0); } EXPECT_TRUE (tree.writeBinary("sphere_rays.bt")); EXPECT_EQ ((int) tree.size(), 50615); // ------------------------------------------------------------ // ray casting is now in "test_raycasting.cpp" // ------------------------------------------------------------ // insert scan test // insert graph node test // write graph test } else if (test_name == "InsertScan") { Pointcloud* measurement = new Pointcloud(); point3d origin (0.01f, 0.01f, 0.02f); point3d point_on_surface (2.01f, 0.01f, 0.01f); for (int i=0; i<360; i++) { for (int j=0; j<360; j++) { point3d p = origin+point_on_surface; measurement->push_back(p); point_on_surface.rotate_IP (0,0,DEG2RAD(1.)); } point_on_surface.rotate_IP (0,DEG2RAD(1.),0); } OcTree tree (0.05); tree.insertPointCloud(*measurement, origin); EXPECT_EQ (tree.size(), 53959); ScanGraph* graph = new ScanGraph(); Pose6D node_pose (origin.x(), origin.y(), origin.z(),0.0f,0.0f,0.0f); graph->addNode(measurement, node_pose); EXPECT_TRUE (graph->writeBinary("test.graph")); delete graph; // ------------------------------------------------------------ // graph read file test } else if (test_name == "ReadGraph") { // not really meaningful, see better test in "test_scans.cpp" ScanGraph graph; EXPECT_TRUE (graph.readBinary("test.graph")); // ------------------------------------------------------------ } else if (test_name == "StampedTree") { OcTreeStamped stamped_tree (0.05); // fill tree for (int x=-20; x<20; x++) for (int y=-20; y<20; y++) for (int z=-20; z<20; z++) { point3d p ((float) x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f); stamped_tree.updateNode(p, true); // integrate 'occupied' measurement } // test if update times set point3d query (0.1f, 0.1f, 0.1f); OcTreeNodeStamped* result = stamped_tree.search (query); EXPECT_TRUE (result); unsigned int tree_time = stamped_tree.getLastUpdateTime(); unsigned int node_time = result->getTimestamp(); std::cout << "After 1st update (cube): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp() << std::endl; EXPECT_TRUE (tree_time > 0); #ifdef _WIN32 Sleep(1000); #else sleep(1); #endif stamped_tree.integrateMissNoTime(result); // reduce occupancy, no time update std::cout << "After 2nd update (single miss): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << node_time << std::endl; EXPECT_EQ (node_time, result->getTimestamp()); // node time updated? point3d query2 = point3d (0.1f, 0.1f, 0.3f); stamped_tree.updateNode(query2, true); // integrate 'occupied' measurement OcTreeNodeStamped* result2 = stamped_tree.search (query2); EXPECT_TRUE (result2); result = stamped_tree.search (query); EXPECT_TRUE (result); std::cout << "After 3rd update (single hit at (0.1, 0.1, 0.3): Tree time " << stamped_tree.getLastUpdateTime() << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp() << "; node(0.1, 0.1, 0.3) time " << result2->getTimestamp() << std::endl; EXPECT_TRUE (result->getTimestamp() < result2->getTimestamp()); // result2 has been updated EXPECT_EQ(result2->getTimestamp(), stamped_tree.getLastUpdateTime()); // ------------------------------------------------------------ } else if (test_name == "OcTreeKey") { OcTree tree (0.05); point3d p(0.0,0.0,0.0); OcTreeKey key; tree.coordToKeyChecked(p, key); point3d p_inv = tree.keyToCoord(key); EXPECT_FLOAT_EQ (0.025, p_inv.x()); EXPECT_FLOAT_EQ (0.025, p_inv.y()); EXPECT_FLOAT_EQ (0.025, p_inv.z()); // ------------------------------------------------------------ } else { std::cerr << "Invalid test name specified: " << test_name << std::endl; return 1; } std::cerr << "Test successful.\n"; return 0; }