Beispiel #1
0
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());
}