C++ OcTree::coordToKeyChecked方法代码示例

//.........这里部分代码省略.........


    // 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();

//.........这里部分代码省略.........
      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;
}