示例#1
1
文件: bt2vrml.cpp 项目: 2maz/octomap
int main(int argc, char** argv) {
  // default values:
  string vrmlFilename = "";
  string btFilename = "";

  if (argc != 2 || (argc > 1 && strcmp(argv[1], "-h") == 0)){
    printUsage(argv[0]);
  }

  btFilename = std::string(argv[1]);
  vrmlFilename = btFilename + ".wrl";


  cout << "\nReading OcTree file\n===========================\n";
  // TODO: check if file exists and if OcTree read correctly?
  OcTree* tree = new OcTree(btFilename);


  cout << "\nWriting occupied volumes to VRML\n===========================\n";

  std::ofstream outfile (vrmlFilename.c_str());

  outfile << "#VRML V2.0 utf8\n#\n";
  outfile << "# created from OctoMap file "<<btFilename<< " with bt2vrml\n";


  size_t count(0);
  for(OcTree::leaf_iterator it = tree->begin(), end=tree->end(); it!= end; ++it) {
    if(tree->isNodeOccupied(*it)){
      count++;
      double size = it.getSize();
      outfile << "Transform { translation "
          << it.getX() << " " << it.getY() << " " << it.getZ()
          << " \n  children ["
          << " Shape { geometry Box { size "
          << size << " " << size << " " << size << "} } ]\n"
          << "}\n";
    }
  }

  delete tree;

  outfile.close();

  std::cout << "Finished writing "<< count << " voxels to " << vrmlFilename << std::endl;

  return 0;
}
示例#2
0
void printChanges(OcTree& tree){
  unsigned int changedOccupied = 0;
  unsigned int changedFree = 0;
  unsigned int actualOccupied = 0;
  unsigned int actualFree = 0;
  unsigned int missingChanged = 0;

  tree.expand();

  // iterate through the changed nodes
  KeyBoolMap::const_iterator it;
  for (it=tree.changedKeysBegin(); it!=tree.changedKeysEnd(); it++) {
    OcTreeNode* node = tree.search(it->first);
    if (node != NULL) {
      if (tree.isNodeOccupied(node)) {
        changedOccupied += 1;
      }
      else {
        changedFree += 1;
      }
    } else {
      missingChanged +=1;
    }
  }


  // iterate through the entire tree
  for(OcTree::tree_iterator it=tree.begin_tree(),
      end=tree.end_tree(); it!= end; ++it) {
    if (it.isLeaf()) {
      if (tree.isNodeOccupied(*it)) {
        actualOccupied += 1;
      }
      else {
        actualFree += 1;
      }
    }
  }
  
  cout << "change detection: " << changedOccupied << " occ; " << changedFree << " free; "<< missingChanged << " missing" << endl;
  cout << "actual: " << actualOccupied << " occ; " << actualFree << " free; " << endl;

  tree.prune();
}
示例#3
0
文件: test_io.cpp 项目: 2maz/octomap
int main(int argc, char** argv) {

  if (argc != 2){
    std::cerr << "Error: you need to specify a testfile (.bt) as argument to read" << std::endl;
    return 1; // exit 1 means failure
  }

  std::cout << "Testing empty OcTree...\n";
  //empty tree
  {
    OcTree emptyTree(0.999);
    EXPECT_EQ(emptyTree.size(), 0);
    EXPECT_TRUE(emptyTree.writeBinary("empty.bt"));
    EXPECT_TRUE(emptyTree.write("empty.ot"));

    OcTree emptyReadTree(0.2);
    EXPECT_TRUE(emptyReadTree.readBinary("empty.bt"));
    EXPECT_EQ(emptyReadTree.size(), 0);
    EXPECT_TRUE(emptyTree == emptyReadTree);

    
    AbstractOcTree* readTreeAbstract = AbstractOcTree::read("empty.ot");
    EXPECT_TRUE(readTreeAbstract);
    OcTree* readTreeOt = dynamic_cast<OcTree*>(readTreeAbstract);
    EXPECT_TRUE(readTreeOt);  
    EXPECT_EQ(readTreeOt->size(), 0);
    EXPECT_TRUE(emptyTree == *readTreeOt);
    delete readTreeOt;
  }

  std::cout << "Testing reference OcTree from file ...\n";
  string filename = string(argv[1]);

  {
    string filenameOt = "test_io_file.ot";
    string filenameBtOut = "test_io_file.bt";
    string filenameBtCopyOut = "test_io_file_copy.bt";

    // read reference tree from input file
    OcTree tree (0.1);
    EXPECT_TRUE (tree.readBinary(filename));

    std::cout << "    Copy Constructor / assignment / ==\n";
    // test copy constructor / assignment:
    OcTree* treeCopy = new OcTree(tree);
    EXPECT_TRUE(tree == *treeCopy);
    EXPECT_TRUE(treeCopy->writeBinary(filenameBtCopyOut));

    // change a tree property, trees must be different afterwards
    treeCopy->setResolution(tree.getResolution()*2.0);
    EXPECT_FALSE(tree == *treeCopy);
    treeCopy->setResolution(tree.getResolution());
    EXPECT_TRUE(tree == *treeCopy);

    // flip one value, trees must be different afterwards:
    point3d pt(0.5, 0.5, 0.5);
    OcTreeNode* node = treeCopy->search(pt);
    if (node && treeCopy->isNodeOccupied(node))
      treeCopy->updateNode(pt, false);
    else
      treeCopy->updateNode(pt, true);

    EXPECT_FALSE(tree == *treeCopy);


    delete treeCopy;

    std::cout << "    Swap\n";
    // test swap:
    OcTree emptyT(tree.getResolution());
    OcTree emptySw(emptyT);
    OcTree otherSw(tree);
    emptySw.swapContent(otherSw);
    EXPECT_FALSE(emptyT == emptySw);
    EXPECT_TRUE(emptySw == tree);
    EXPECT_TRUE(otherSw == emptyT);


    // write again to bt, read & compare
    EXPECT_TRUE(tree.writeBinary(filenameBtOut));
    OcTree readTreeBt(0.1);
    EXPECT_TRUE(readTreeBt.readBinary(filenameBtOut));
    EXPECT_TRUE(tree == readTreeBt);

    std::cout <<"    Write to .ot / read through AbstractOcTree\n";
    // now write to .ot, read & compare
    EXPECT_TRUE(tree.write(filenameOt));
    
    AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameOt);
    EXPECT_TRUE(readTreeAbstract);

    OcTree* readTreeOt = dynamic_cast<OcTree*>(readTreeAbstract);
    EXPECT_TRUE(readTreeOt);
    EXPECT_TRUE(tree == *readTreeOt);

    // sanity test for "==": flip one node, compare again
    point3d coord(0.1f, 0.1f, 0.1f);
    node = readTreeOt->search(coord);
    if (node && readTreeOt->isNodeOccupied(node))
      readTreeOt->updateNode(coord, false);
    else
      readTreeOt->updateNode(coord, true);

    EXPECT_FALSE(tree == *readTreeOt);
    
    delete readTreeOt;
  }

  // Test for tree headers and IO factory registry (color)
  {
    std::cout << "Testing ColorOcTree...\n";

    double res = 0.02;
    std::string filenameColor = "test_io_color_file.ot";
    ColorOcTree colorTree(res);
    EXPECT_EQ(colorTree.getTreeType(), "ColorOcTree");
    ColorOcTreeNode* colorNode = colorTree.updateNode(point3d(0.0, 0.0, 0.0), true);
    ColorOcTreeNode::Color color_red(255, 0, 0);
    colorNode->setColor(color_red);
    colorTree.setNodeColor(0.0, 0.0, 0.0, 255, 0, 0);
    colorTree.updateNode(point3d(0.1f, 0.1f, 0.1f), true);
    colorTree.setNodeColor(0.1f, 0.1f, 0.1f, 0, 0, 255);

    EXPECT_TRUE(colorTree.write(filenameColor));
    AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameColor);
    EXPECT_TRUE(readTreeAbstract);
    EXPECT_EQ(colorTree.getTreeType(),  readTreeAbstract->getTreeType());
    ColorOcTree* readColorTree = dynamic_cast<ColorOcTree*>(readTreeAbstract);
    EXPECT_TRUE(readColorTree);
    EXPECT_TRUE(colorTree == *readColorTree);
    colorNode = colorTree.search(0.0, 0.0, 0.0);
    EXPECT_TRUE(colorNode);
    EXPECT_EQ(colorNode->getColor(), color_red);
    delete readColorTree;
  }

  // Test for tree headers and IO factory registry (stamped)
  {
    std::cout << "Testing OcTreeStamped...\n";
    double res = 0.05;
    std::string filenameStamped = "test_io_stamped_file.ot";
    OcTreeStamped stampedTree(res);
    EXPECT_EQ(stampedTree.getTreeType(), "OcTreeStamped");
    // TODO: add / modify some stamped nodes
    //ColorOcTreeNode* colorNode = colorTree.updateNode(point3d(0.0, 0.0, 0.0), true);
    //ColorOcTreeNode::Color color_red(255, 0, 0);
    //colorNode->setColor(color_red);
    //colorTree.setNodeColor(0.0, 0.0, 0.0, 255, 0, 0);
    //colorTree.updateNode(point3d(0.1f, 0.1f, 0.1f), true);
    //colorTree.setNodeColor(0.1f, 0.1f, 0.1f, 0, 0, 255);

    EXPECT_TRUE(stampedTree.write(filenameStamped));
    AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameStamped);
    EXPECT_TRUE(readTreeAbstract);
    EXPECT_EQ(stampedTree.getTreeType(), readTreeAbstract->getTreeType());
    OcTreeStamped* readStampedTree = dynamic_cast<OcTreeStamped*>(readTreeAbstract);
    EXPECT_TRUE(readStampedTree);
    EXPECT_TRUE(stampedTree == *readStampedTree);
    //colorNode = colorTree.search(0.0, 0.0, 0.0);
    //EXPECT_TRUE(colorNode);
    //EXPECT_EQ(colorNode->getColor(), color_red);    
    
    delete readStampedTree;    
  }


  std::cerr << "Test successful.\n";
  return 0;
}
示例#4
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;
}
示例#5
0
int main(int argc, char** argv) {
  // default values:
  double res = 0.1;

  if (argc < 2)
    printUsage(argv[0]);

  string graphFilename = std::string(argv[1]);

  double maxrange = -1;
  int max_scan_no = -1;
  int skip_scan_eval = 5;

  int arg = 1;
  while (++arg < argc) {
    if (! strcmp(argv[arg], "-i"))
      graphFilename = std::string(argv[++arg]);
    else if (! strcmp(argv[arg], "-res"))
      res = atof(argv[++arg]);
    else if (! strcmp(argv[arg], "-m"))
      maxrange = atof(argv[++arg]);
    else if (! strcmp(argv[arg], "-n"))
      max_scan_no = atoi(argv[++arg]);
    else {
      printUsage(argv[0]);
    }
  }

  cout << "\nReading Graph file\n===========================\n";
  ScanGraph* graph = new ScanGraph();
  if (!graph->readBinary(graphFilename))
    exit(2);
  
  size_t num_points_in_graph = 0;
  if (max_scan_no > 0) {
    num_points_in_graph = graph->getNumPoints(max_scan_no-1);
    cout << "\n Data points in graph up to scan " << max_scan_no << ": " << num_points_in_graph << endl;
  }
  else {
    num_points_in_graph = graph->getNumPoints();
    cout << "\n Data points in graph: " << num_points_in_graph << endl;
  }

  cout << "\nCreating tree\n===========================\n";
  OcTree* tree = new OcTree(res);

  size_t numScans = graph->size();
  unsigned int currentScan = 1;
  for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {

    if (currentScan % skip_scan_eval != 0){
      if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
      else cout << "("<<currentScan << "/" << numScans << ") " << flush;
      tree->insertPointCloud(**scan_it, maxrange);
    } else
      cout << "(SKIP) " << flush;

    if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
      break;

    currentScan++;
  }

  tree->expand();

  
  cout << "\nEvaluating scans\n===========================\n";
  currentScan = 1;
  size_t num_points = 0;
  size_t num_voxels_correct = 0;
  size_t num_voxels_wrong = 0;
  size_t num_voxels_unknown = 0;


  for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {

    if (currentScan % skip_scan_eval == 0){
      if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
      else cout << "("<<currentScan << "/" << numScans << ") " << flush;


      pose6d frame_origin = (*scan_it)->pose;
      point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans());

      // transform pointcloud:
      Pointcloud scan (*(*scan_it)->scan);
      scan.transform(frame_origin);
      point3d origin = frame_origin.transform(sensor_origin);

      KeySet free_cells, occupied_cells;
      tree->computeUpdate(scan, origin, free_cells, occupied_cells, maxrange);

      num_points += scan.size();

      // count free cells
      for (KeySet::iterator it = free_cells.begin(); it != free_cells.end(); ++it) {
        OcTreeNode* n = tree->search(*it);
        if (n){
          if (tree->isNodeOccupied(n))
            num_voxels_wrong++;
          else
            num_voxels_correct++;
        } else
          num_voxels_unknown++;
      } // count occupied cells
      for (KeySet::iterator it = occupied_cells.begin(); it != occupied_cells.end(); ++it) {
        OcTreeNode* n = tree->search(*it);
        if (n){
          if (tree->isNodeOccupied(n))
            num_voxels_correct++;
          else
            num_voxels_wrong++;
        } else
          num_voxels_unknown++;
      }


    }

    if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
      break;

    currentScan++;


  }

  cout << "\nFinished evaluating " << num_points <<"/"<< num_points_in_graph << " points.\n"
      <<"Voxels correct: "<<num_voxels_correct<<" #wrong: " <<num_voxels_wrong << " #unknown: " <<num_voxels_unknown
      <<". % correct: "<< num_voxels_correct/double(num_voxels_correct+num_voxels_wrong)<<"\n\n";


  delete graph;
  delete tree;
  
  return 0;
}
示例#6
0
void execute(const fremen::informationGoalConstPtr& goal, Server* as)
{

    /*              Octmap Estimation and Visualization             */

    octomap_msgs::Octomap bmap_msg;
    OcTree octree (resolution);
    geometry_msgs::Point initialPt, finalPt;

    //Create pointcloud:
    octomap::Pointcloud octoCloud;
    sensor_msgs::PointCloud fremenCloud;
    float x = 0*gridPtr->positionX;
    float y = 0*gridPtr->positionY;
    geometry_msgs::Point32 test_point;
    int cnt = 0;

    int cell_x, cell_y, cell_z;
    cell_x = (int)(goal->x/resolution);
    cell_y = (int)(goal->y/resolution);
    cell_z = (int)(head_height/resolution);

    for(double i = LIM_MIN_X; i < LIM_MAX_X; i+=resolution){
        for(double j = LIM_MIN_Y; j < LIM_MAX_Y; j+=resolution){
            for(double w = LIM_MIN_Z; w < LIM_MAX_Z; w+=resolution){
                point3d ptt(x+i+resolution/2,y+j+resolution/2,w+resolution/2);
                int s = goal->stamp;
                if(gridPtr->retrieve(cnt, goal->stamp)>0)
                {
                    //finalPt.z = (int)((w+resolution/2)/resolution)-cell_z;
                    //finalPt.y = (int)((j+resolution/2)/resolution)-cell_y;
                    //finalPt.x = (int)((i+resolution/2)/resolution)-cell_x;

                    //int cnta = ((cell_x+finalPt.x-LIM_MIN_X/resolution)*dim_y + (finalPt.y + cell_y-LIM_MIN_Y/resolution))*dim_z + (finalPt.z + cell_z-LIM_MIN_Z/resolution);
                    //ROS_INFO("something %d %d",cnt,cnta);
                    octoCloud.push_back(x+i+resolution/2,y+j+resolution/2,w+resolution/2);
                    octree.updateNode(ptt,true,true);
                }
                cnt++;
            }
        }
    }
    //Update grid
    octree.updateInnerOccupancy();

    //init visualization markers:
    visualization_msgs::MarkerArray occupiedNodesVis;
    unsigned int m_treeDepth = octree.getTreeDepth();

    //each array stores all cubes of a different size, one for each depth level:
    occupiedNodesVis.markers.resize(m_treeDepth + 1);
    geometry_msgs::Point cubeCenter;

    std_msgs::ColorRGBA m_color;
    m_color.r = 0.0;
    m_color.g = 0.0;
    m_color.b = 1.0;
    m_color.a = 0.5;

    for (unsigned i = 0; i < occupiedNodesVis.markers.size(); ++i)
    {
        double size = octree.getNodeSize(i);
        occupiedNodesVis.markers[i].header.frame_id = "/map";
        occupiedNodesVis.markers[i].header.stamp = ros::Time::now();
        occupiedNodesVis.markers[i].ns = "map";
        occupiedNodesVis.markers[i].id = i;
        occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
        occupiedNodesVis.markers[i].scale.x = size;
        occupiedNodesVis.markers[i].scale.y = size;
        occupiedNodesVis.markers[i].scale.z = size;
        occupiedNodesVis.markers[i].color = m_color;
    }

    ROS_INFO("s %i",cnt++);
    x = gridPtr->positionX;
    y = gridPtr->positionY;


    for(OcTree::leaf_iterator it = octree.begin_leafs(), end = octree.end_leafs(); it != end; ++it)
    {
        if(it != NULL && octree.isNodeOccupied(*it))
        {
            unsigned idx = it.getDepth();
            cubeCenter.x = x+it.getX();
            cubeCenter.y = y+it.getY();
            cubeCenter.z = it.getZ();
            occupiedNodesVis.markers[idx].points.push_back(cubeCenter);
            double minX, minY, minZ, maxX, maxY, maxZ;
            octree.getMetricMin(minX, minY, minZ);
            octree.getMetricMax(maxX, maxY, maxZ);
            double h = (1.0 - fmin(fmax((cubeCenter.z - minZ) / (maxZ - minZ), 0.0), 1.0)) * m_colorFactor;
            occupiedNodesVis.markers[idx].colors.push_back(heightMapColorA(h));
        }
    }

    /**** Robot Head Marker ****/

    //Robot Position


    visualization_msgs::Marker marker_head;
    marker_head.header.frame_id = "/map";
    marker_head.header.stamp = ros::Time();
    marker_head.ns = "my_namespace";
    marker_head.id = 1;
    marker_head.type = visualization_msgs::Marker::SPHERE;
    marker_head.action = visualization_msgs::Marker::ADD;
    marker_head.pose.position.x = goal->x;
    marker_head.pose.position.y = goal->y;
    marker_head.pose.position.z = head_height;
    marker_head.pose.orientation.x = 0.0;
    marker_head.pose.orientation.y = 0.0;
    marker_head.pose.orientation.z = 0.0;
    marker_head.pose.orientation.w = 1.0;
    marker_head.scale.x = 0.2;
    marker_head.scale.y = 0.2;
    marker_head.scale.z = 0.2;
    marker_head.color.a = 1.0;
    marker_head.color.r = 0.0;
    marker_head.color.g = 1.0;
    marker_head.color.b = 0.0;


    /****** Ray Traversal ******/

    //ROS_INFO("Robot Position (%f,%f,%f) = (%d,%d,%d)", goal->x, goal->y, head_height, cell_x, cell_y, cell_z);

    //Ray Casting (Grid Traversal - Digital Differential Analyzer)

    visualization_msgs::Marker marker_rays;
    marker_rays.header.frame_id = "/map";
    marker_rays.header.stamp = ros::Time();
    marker_rays.ns = "my_namespace";
    marker_rays.id = 2;
    marker_rays.type = visualization_msgs::Marker::LINE_LIST;
    marker_rays.action = visualization_msgs::Marker::ADD;
    marker_rays.scale.x = 0.01;


    geometry_msgs::Vector3 rayDirection, deltaT, cellIndex;

    std_msgs::ColorRGBA line_color;

    initialPt.x = goal->x;
    initialPt.y = goal->y;
    initialPt.z = head_height;

    line_color.a = 0.2;
    line_color.r = 0.0;
    line_color.g = 0.0;
    line_color.b = 1.0;

    float delta, H;
    bool free_cell;

    ROS_INFO("Performing Ray Casting...");
            H = 0;
    int gridsize = dim_x*dim_y*dim_z;
    for(float ang_v = -0.174; ang_v < 0.174; ang_v+=angular_step){
        for(float ang_h = 0; ang_h < 2*M_PI; ang_h+= angular_step){ //0 - 360 degrees
            //Initial conditions:
            rayDirection.z = sin(ang_v);//z
            rayDirection.x = cos(ang_v) * cos(ang_h);//x
            rayDirection.y = cos(ang_v) * sin(ang_h);//y

            delta = fmax(fmax(fabs(rayDirection.x), fabs(rayDirection.y)), fabs(rayDirection.z));

            deltaT.x = rayDirection.x/delta;
            deltaT.y = rayDirection.y/delta;
            deltaT.z = rayDirection.z/delta;

            free_cell = true;


            int max_it = RANGE_MAX/resolution * 2/sqrt(pow(deltaT.x,2) + pow(deltaT.y,2) + pow(deltaT.z,2));

            //    ROS_INFO("Max_it: %d %d %d", cell_x,cell_y,cell_z);

            finalPt.x = 0;
            finalPt.y = 0;
            finalPt.z = 0;

            for(int i = 0; i < max_it && free_cell; i++){
                finalPt.x += deltaT.x/2;
                finalPt.y += deltaT.y/2;
                finalPt.z += deltaT.z/2;

                //cnt?
                int cnt = ((int)((cell_x+finalPt.x-LIM_MIN_X/resolution))*dim_y + (int)(finalPt.y + cell_y-LIM_MIN_Y/resolution))*dim_z + (int)(finalPt.z + cell_z-LIM_MIN_Z/resolution);//get fremen grid index!

                //TODO
                if (cnt < 0){
                    cnt = 0;
                    free_cell = false;
                }
                if (cnt > gridsize-1){
                    cnt = gridsize-1;
                    free_cell = false;
                }
                //ROS_INFO("CNT: %d %d", cnt,gridsize);
                //ROS_INFO("DIM %d %d", dim_x, dim_z);
                if(gridPtr->retrieve(cnt, goal->stamp) > 0) free_cell = false;
                if(aux_entropy[cnt] ==0){
                    aux_entropy[cnt] = 1;
                    //float p = gridPtr->estimate(cnt,goal->stamp);
                    //h+=-p*ln(p);
                    H++;
                }
            }


            marker_rays.points.push_back(initialPt);
            marker_rays.colors.push_back(line_color);
            finalPt.x = finalPt.x*resolution+initialPt.x;
            finalPt.y = finalPt.y*resolution+initialPt.y;
            finalPt.z = finalPt.z*resolution+initialPt.z;
            marker_rays.points.push_back(finalPt);
            marker_rays.colors.push_back(line_color);

        }
    }

    //Entropy (text marker):

    visualization_msgs::Marker marker_text;
    marker_text.header.frame_id = "/map";
    marker_text.header.stamp = ros::Time();
    marker_text.ns = "my_namespace";
    marker_text.id = 1;
    marker_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    marker_text.action = visualization_msgs::Marker::ADD;
    marker_text.pose.position.x = goal->x;
    marker_text.pose.position.y = goal->y;
    marker_text.pose.position.z = head_height + 2;
    marker_text.pose.orientation.x = 0.0;
    marker_text.pose.orientation.y = 0.0;
    marker_text.pose.orientation.z = 0.0;
    marker_text.pose.orientation.w = 1.0;
    marker_text.scale.z = 0.5;
    marker_text.color.a = 1.0;
    marker_text.color.r = 0.0;
    marker_text.color.g = 1.0;
    marker_text.color.b = 0.0;
    char output[1000];
    sprintf(output,"Gain: %f",H);
    marker_text.text = output;


    //Publish Results:
    ROS_INFO("Data published!");
    head_pub_ptr->publish(marker_head);
    estimate_pub_ptr->publish(occupiedNodesVis);
    rays_pub_ptr->publish(marker_rays);
    text_pub_ptr->publish(marker_text);


    as->setSucceeded();

}