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;
}
Beispiel #2
0
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
  // Usage:
  //   Constructors/Destructor:
  //    octree = octomapWrapper(resolution);  // constructor: new tree with
  //    specified resolution
  //    octree = octomapWrapper(filename);    // constructor: load from file
  //    octomapWrapper(octree);     // destructor
  //
  //   Queries:
  //    results = octomapWrapper(octree, 1, pts) // search
  //    leaf_nodes = octomapWrapper(octree, 2)  // getLeafNodes
  //
  //   Update tree:
  //    octomapWrapper(octree, 11, pts, occupied)   // updateNote(pts, occupied).
  //    pts is 3-by-n, occupied is 1-by-n logical
  //
  //   General operations:
  //    octomapWrapper(octree, 21, filename)    // save to file

  OcTree* tree = NULL;

  if (nrhs == 1) {
    if (mxIsNumeric(prhs[0])) {  // constructor w/ resolution
      if (nlhs > 0) {
        double resolution = mxGetScalar(prhs[0]);
        //        mexPrintf("Creating octree w/ resolution %f\n", resolution);
        tree = new OcTree(resolution);
        plhs[0] = createDrakeMexPointer((void*)tree, "OcTree");
      }
    } else if (mxIsChar(prhs[0])) {
      if (nlhs > 0) {
        char* filename = mxArrayToString(prhs[0]);
        //        mexPrintf("Loading octree from %s\n", filename);
        tree = new OcTree(filename);
        plhs[0] = createDrakeMexPointer((void*)tree, "OcTree");
        mxFree(filename);
      }
    } else {  // destructor.  note: assumes prhs[0] is a DrakeMexPointer (todo:
              // could check)
              //      mexPrintf("Deleting octree\n");
      destroyDrakeMexPointer<OcTree*>(prhs[0]);
    }
    return;
  }

  tree = (OcTree*)getDrakeMexPointer(prhs[0]);
  int COMMAND = (int)mxGetScalar(prhs[1]);

  switch (COMMAND) {
    case 1:  // search
    {
      mexPrintf("octree search\n");
      if (mxGetM(prhs[2]) != 3)
        mexErrMsgTxt("octomapWrapper: pts must be 3-by-n");
      int n = mxGetN(prhs[2]);
      double* pts = mxGetPrSafe(prhs[2]);
      if (nlhs > 0) {
        plhs[0] = mxCreateDoubleMatrix(1, n, mxREAL);
        double* presults = mxGetPrSafe(plhs[0]);
        for (int i = 0; i < n; i++) {
          OcTreeNode* result =
              tree->search(pts[3 * i], pts[3 * i + 1], pts[3 * i + 2]);
          if (result == NULL)
            presults[i] = -1.0;
          else
            presults[i] = result->getOccupancy();
        }
      }
    } break;
    case 2:  // get leaf nodes
    {
      //      mexPrintf("octree get leaf nodes\n");
      int N = tree->getNumLeafNodes();
      plhs[0] = mxCreateDoubleMatrix(3, N, mxREAL);
      double* leaf_xyz = mxGetPrSafe(plhs[0]);

      double* leaf_value = NULL, * leaf_size = NULL;
      if (nlhs > 1) {  // return value
        plhs[1] = mxCreateDoubleMatrix(1, N, mxREAL);
        leaf_value = mxGetPrSafe(plhs[1]);
      }
      if (nlhs > 2) {  // return size
        plhs[2] = mxCreateDoubleMatrix(1, N, mxREAL);
        leaf_size = mxGetPrSafe(plhs[2]);
      }

      for (OcTree::leaf_iterator leaf = tree->begin_leafs(),
                                 end = tree->end_leafs();
           leaf != end; ++leaf) {
        leaf_xyz[0] = leaf.getX();
        leaf_xyz[1] = leaf.getY();
        leaf_xyz[2] = leaf.getZ();
        leaf_xyz += 3;
        if (leaf_value) *leaf_value++ = leaf->getValue();
        if (leaf_size) *leaf_size++ = leaf.getSize();
      }
    } break;
    case 11:  // add occupied pts
    {
      //        mexPrintf("octree updateNode\n");
      if (mxGetM(prhs[2]) != 3)
        mexErrMsgTxt("octomapWrapper: pts must be 3-by-n");
      int n = mxGetN(prhs[2]);
      double* pts = mxGetPrSafe(prhs[2]);
      mxLogical* occupied = mxGetLogicals(prhs[3]);
      for (int i = 0; i < n; i++) {
        tree->updateNode(pts[3 * i], pts[3 * i + 1], pts[3 * i + 2],
                         occupied[i]);
      }
    } break;
    case 12:  // insert a scan of endpoints and sensor origin
    {
      // pointsA should be 3xN, originA is 3x1
      double* points = mxGetPrSafe(prhs[2]);
      double* originA = mxGetPrSafe(prhs[3]);
      int n = mxGetN(prhs[2]);
      point3d origin((float)originA[0], (float)originA[1], (float)originA[2]);
      Pointcloud pointCloud;
      for (int i = 0; i < n; i++) {
        point3d point((float)points[3 * i], (float)points[3 * i + 1],
                      (float)points[3 * i + 2]);
        pointCloud.push_back(point);
      }
      tree->insertPointCloud(pointCloud, origin);
    } break;
    case 21:  // save to file
    {
      char* filename = mxArrayToString(prhs[2]);
      //        mexPrintf("writing octree to %s\n", filename);
      tree->writeBinary(filename);
      mxFree(filename);
    } break;
    default:
      mexErrMsgTxt("octomapWrapper: Unknown command");
  }
}