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; }
void compareCallbackUsingRegions(const ros::TimerEvent&) { // MODE 2: COMPARE OCTOMAP WITH REGIONS // Compare a Octomap retrieved by a topic with Regions previously defined and check for inconsistencies. ros::Time t= ros::Time::now(); ROS_INFO("Compare callback using regions triggered"); // Checks if the OcTree Target was already received. if (octree_target == NULL) { ROS_INFO("OcTree Target Not Found"); return; } // Checks if the received target has changed, if not, exits the callback. if (flg_received_new_target==true) { flg_received_new_target = false; } else { return; } // Visualization Message Marker Array Initialization visualization_msgs::MarkerArray ma; visualization_msgs::MarkerArray ma_inconsistencies; visualization_msgs::MarkerArray ma_clusters; visualization_msgs::Marker marker_deleteall; marker_deleteall.header.stamp = ros::Time(); marker_deleteall.header.frame_id = octree_frame_id ; marker_deleteall.ns = "target_inconsistent"; marker_deleteall.action = 3; size_t id=0; size_t id_inconsistencies=0; size_t id_noneighbors=0; // Color initialization std_msgs::ColorRGBA color_occupied; color_occupied.r = 0; color_occupied.g = 0; color_occupied.b = 0.5; color_occupied.a = .8; std_msgs::ColorRGBA color_inconsistent; color_inconsistent.r = .5; color_inconsistent.g = 0; color_inconsistent.b = 0; color_inconsistent.a = .4; std_msgs::ColorRGBA color_inconsistent_missing; color_inconsistent_missing.r = .0; color_inconsistent_missing.g = 0.5; color_inconsistent_missing.b = 0; color_inconsistent_missing.a = .4; std_msgs::ColorRGBA color_target_volume; color_target_volume.r = .5; color_target_volume.g = 0.5; color_target_volume.b = 0; color_target_volume.a = 1; std_msgs::ColorRGBA color_noneighbors; color_noneighbors.r = .5; color_noneighbors.g = 0; color_noneighbors.b = 1; color_noneighbors.a = .8; // Vector of Inconsistencies Initialization std::vector<ClassBoundingBox> vi; std::vector<ClassBoundingBox> vi_missing; // Creates the target volume message array ma.markers.push_back(target_volume.getMarkerWithEdges("target_volume", octree_frame_id , color_target_volume, ++id)); ROS_INFO_STREAM("Starting Iteration"); // Iterates over each region for (size_t i=0; i < boxes.size(); ++i) { size_t num_occupied = 0; size_t num_neighbors = 0; // Iterates over target Octree for(OcTree::leaf_bbx_iterator it = octree_target->begin_leafs_bbx(boxes[i].getMinimumPoint(), boxes[i].getMaximumPoint(), depth), end=octree_target->end_leafs_bbx(); it!= end; ++it) { // Verifies if the node exists if (octree_target->search(it.getKey())) { num_neighbors++; // Verifies if the node is free if (!octree_target->isNodeOccupied(*it)) { // Do nothing } else { num_occupied++; } } } double occupation_ratio=0; // Occupation Ratio computation if (num_neighbors !=0) { occupation_ratio = (double)num_occupied/(double)num_neighbors; } // Checks for Inconsistencies of type EXCEEDING if (boxes[i].occupied == false && occupation_ratio >= exceeding_threshold_with_regions && num_neighbors !=0) { // Add the inconsistency cell into a vector vi.push_back(boxes[i]); } // Checks for Inconsistencies of type MISSING if (boxes[i].occupied == true && occupation_ratio <= missing_threshold_with_regions && num_neighbors !=0) //If no occupied cell was found out of all iterated in the model's bbox, then an inconsistency is detected { // Add the inconsistency cell into a vector vi_missing.push_back(boxes[i]); } } //Cluster the EXCEEDING cells vector< vector<size_t> > cluster; clusterBoundingBoxes(vi, cluster); ROS_INFO("There are %ld clusters", cluster.size()); class_colormap cluster_colors("autumn", cluster.size(), 0.8); // Cluster the MISSING cells vector< vector<size_t> > cluster_missing; clusterBoundingBoxes(vi_missing, cluster_missing); ROS_INFO("There are %ld clusters_missing", cluster_missing.size()); class_colormap cluster_missing_colors("summer", cluster_missing.size(), 0.8); //Select only EXCEEDING clusters above a given volume threshold vector< vector<size_t> > selected_cluster; filterClustersByVolume(vi, cluster, selected_cluster, volume_threshold); ROS_INFO("Selected %ld clusters using volume threshold", selected_cluster.size()); //Select only MISSING clusters above a given volume threshold vector< vector<size_t> > selected_cluster_missing; filterClustersByVolume(vi_missing, cluster_missing, selected_cluster_missing, volume_threshold); ROS_INFO("Selected %ld clusters_missing using volume threshold", selected_cluster_missing.size()); //Draw filtered inconsistencies clusters in RVIZ class_colormap inconsistencies_colors(0.5,0,0,0.4); clustersToMarkerArray(vi, selected_cluster, ma_inconsistencies, id_inconsistencies, octree_frame_id, "inconsistencies", inconsistencies_colors); class_colormap inconsistencies_missing_colors(0,0.5,0,0.4); clustersToMarkerArray(vi_missing, selected_cluster_missing, ma_inconsistencies, id_inconsistencies, octree_frame_id, "inconsistencies", inconsistencies_missing_colors); // Draw all the clusters in RVIZ size_t id_clusters=0; clustersToMarkerArray(vi, selected_cluster, ma_clusters, id_clusters, octree_frame_id, "clusters", cluster_colors); clustersToMarkerArray(vi_missing, selected_cluster_missing, ma_clusters, id_clusters, octree_frame_id, "clusters", cluster_missing_colors); // Draw the Center of Mass Sphere and Volume Information visualization_msgs::MarkerArray ma_centerofmass; visualization_msgs::MarkerArray ma_volumeText; size_t id_ma_centerofmass = 0; centerOfMass(vi, selected_cluster, ma_centerofmass, ma_volumeText, id_ma_centerofmass, octree_frame_id); centerOfMass(vi_missing, selected_cluster_missing, ma_centerofmass, ma_volumeText, id_ma_centerofmass, octree_frame_id); // Publish the Marker Arrays marker_pub_inconsistencies->publish(ma_inconsistencies); marker_pub_clusters->publish(ma_clusters); marker_pub->publish(ma); marker_pub_center_of_mass->publish(ma_centerofmass); marker_pub_volume->publish(ma_volumeText); // Paint the Point Cloud (if available) with the Inconsistencies information. if (!flg_received_point_cloud) { ROS_ERROR_STREAM("No point_cloud2 has been received yet"); } else { ROS_INFO("Processing point cloud ..."); *pc = *pcin; pcl_ros::transformPointCloud(octree_frame_id, *pc, *pc, *listener); *pc2 = *pc; pc2->points.erase(pc2->points.begin(), pc2->points.end()); for (size_t k = 0; k < selected_cluster.size(); ++k) { std::vector<size_t> lpoints; for (size_t l = 0; l < selected_cluster[k].size(); ++l) { size_t idx = selected_cluster[k][l]; std::vector<size_t> ltmp; ltmp = vi[idx].pointsInPointCloud(pc); lpoints.insert(lpoints.end(), ltmp.begin(), ltmp.end()); } // Change color of points to cluster color for (size_t i=0; i< lpoints.size(); ++i) { cv::Scalar c = cluster_colors.cv_color(k); int8_t r = c[2], g = c[1], b = c[0]; uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b); pc->points[lpoints[i]].rgb = *reinterpret_cast<float*>(&rgb);; pc2->points.push_back(pc->points[lpoints[i]]); } } pc2->is_dense = false; pc2->width = pc2->points.size(); pc2->height = 1; sensor_msgs::PointCloud2 pcmsgout; pcl::toROSMsg(*pc2, pcmsgout); pub_pointcloud->publish(pcmsgout); } ros::Duration d = (ros::Time::now() - t); ROS_INFO("Comparisson took %f secs", d.toSec()); }