C++ OcTree类代码示例

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;
}

int main(int argc, char** argv) {
  if (argc != 3)
    printUsage(argv[0]);

  string inputFilename = argv[1];
  string outputFilename = argv[2];

  // pcl point cloud
  pcl::PointCloud<pcl::PointXYZ>::Ptr pclcloud( new pcl::PointCloud<pcl::PointXYZ>() );
  pcl::io::loadPCDFile( inputFilename, *pclcloud );

  // data conversion
  Pointcloud * cloud = new Pointcloud;
  for ( size_t i = 0; i < pclcloud->size(); ++ i ) {
    point3d pt(pclcloud->points[i].x, pclcloud->points[i].y, pclcloud->points[i].z);
    cloud->push_back( pt );
  }
  point3d sensor_origin(0,0,0);
  OcTree* tree = new OcTree(0.1);
  tree->insertPointCloud( cloud, sensor_origin );
  tree->writeBinary(outputFilename);







}

OcTree* generateSphereTree(point3d origin, float radius){
	OcTree* tree = new OcTree(0.05);

	point3d point_on_surface = origin;
	point_on_surface.x() += radius;
	for (int i=0; i<360; i++) {
		for (int j=0; j<360; j++) {
			if (!tree->insertRay(origin, origin+point_on_surface)) {
				cout << "ERROR while inserting ray from " << origin << " to " << point_on_surface << endl;
			}
			point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
		}
		point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
	}
	return tree;
}

void App::octreeHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::map_octree_t* msg){
  std::cout << "MAP_OCTREE received\n";

  // TODO: Currently not handling transform, assuming identity transform

  std::stringstream datastream;
  datastream.write((const char*) msg->data.data(), msg->num_bytes);
  tree_ = new octomap::OcTree(1); //resolution will be set by data from message
  tree_->readBinary(datastream);

  std::stringstream s;
  s <<  "/tmp/map_octomap.bt" ;
  printf("Saving MAP_OCTREE to: %s\n", s.str().c_str());
  tree_->writeBinary(s.str().c_str());

  exit(-1);
}

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

  std::list<OcTreeVolume> occupiedVoxels;
  tree->getOccupied(occupiedVoxels);
  delete tree;

  cout << "\nWriting " << occupiedVoxels.size()<<" 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";

  std::list<OcTreeVolume>::iterator it;

  for (it = occupiedVoxels.begin(); it != occupiedVoxels.end(); ++it){
    outfile << "Transform { translation "
          << it->first.x() << " " << it->first.y() << " " << it->first.z()
          << " \n  children ["
          << " Shape { geometry Box { size "
          << it->second << " " << it->second << " " << it->second << "} } ]\n"
          << "}\n";
  }

  outfile.close();

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

}

OcTree* retrieve_octree()
{
  ros::NodeHandle n;
  std::string servname = "octomap_binary";
  ROS_INFO("Requesting the map from %s...", n.resolveName(servname).c_str());
  GetOctomap::Request req;
  GetOctomap::Response resp;
  while(n.ok() && !ros::service::call(servname, req, resp))
    {
      ROS_WARN("Request to %s failed; trying again...", n.resolveName(servname).c_str());
      usleep(1000000);
    }
 
  OcTree* octree = octomap_msgs::binaryMsgToMap(resp.map);

  if (octree){
    ROS_INFO("Map received (%zu nodes, %f m res)", octree->size(), octree->getResolution());
    return extract_supporting_planes(octree);
  }
  return NULL;
}

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

int main(int argc, char** argv) {

  cout << endl;
  cout << "generating example map" << endl;

  OcTree tree (0.1);  // create empty tree with resolution 0.1


  // insert some measurements of occupied cells

  for (int x=-20; x<20; x++) {
    for (int y=-20; y<20; y++) {
      for (int z=-20; z<20; z++) {
        point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
        tree.updateNode(endpoint, true); // integrate 'occupied' measurement
      }
    }
  }

  // insert some measurements of free cells

  for (int x=-30; x<30; x++) {
    for (int y=-30; y<30; y++) {
      for (int z=-30; z<30; z++) {
        point3d endpoint ((float) x*0.02f-1.0f, (float) y*0.02f-1.0f, (float) z*0.02f-1.0f);
        tree.updateNode(endpoint, false);  // integrate 'free' measurement
      }
    }
  }

  cout << endl;
  cout << "performing some queries:" << endl;
  
  point3d query (0., 0., 0.);
  OcTreeNode* result = tree.search (query);
  print_query_info(query, result);

  query = point3d(-1.,-1.,-1.);
  result = tree.search (query);
  print_query_info(query, result);

  query = point3d(1.,1.,1.);
  result = tree.search (query);
  print_query_info(query, result);


  cout << endl;
  tree.writeBinary("simple_tree.bt");
  cout << "wrote example file simple_tree.bt" << endl << endl;
  cout << "now you can use octovis to visualize: octovis simple_tree.bt"  << endl;
  cout << "Hint: hit 'F'-key in viewer to see the freespace" << endl  << endl;  

}

// Callback function for the service 'view_eval'
bool view_eval(viper::ViewValue::Request  &req,
               viper::ViewValue::Response &res)
{
  
  ROS_INFO("Received service request: view_eval (%f %f %f)", req.pose.position.x, req.pose.position.y, req.pose.position.z);
  geometry_msgs::Pose camera_pose = req.pose; 

  //OcTree* octree = octomap_msgs::binaryMsgToMap(req.octomap);
  
  AbstractOcTree* tree = octomap_msgs::fullMsgToMap(req.octomap);
  if (!tree){
    ROS_ERROR("Failed to recreate octomap");
    return false;
  }
  OcTree* octree = dynamic_cast<OcTree*>(tree);
  
  if (octree){
    ROS_INFO("Map received (%zu nodes, %f m res)", octree->size(), octree->getResolution());
    input_tree = extract_supporting_planes(octree);
  } else{
    ROS_ERROR("No map received!");
    input_tree = NULL;
  }
  
  // Generate frustum.
  Frustum f = generate_frustum(camera_pose);
	
  int value = compute_value(f, res.keys, res.values);
  ROS_INFO("VALUE: %i", value);
  ROS_INFO("KEYS SIZE: %i", res.keys.size());
  ROS_INFO("VALUES SIZE: %i", res.values.size());
  res.value = value;
  res.frustum = get_points(generate_local_frustum());

  ROS_INFO("Finished service request.");
  return true;
}

void generateBoxesFromOctomap(std::vector<CollisionObject*>& boxes, OcTree& tree)
{
  std::vector<boost::array<FCL_REAL, 6> > boxes_ = tree.toBoxes();

  for(std::size_t i = 0; i < boxes_.size(); ++i)
  {
    FCL_REAL x = boxes_[i][0];
    FCL_REAL y = boxes_[i][1];
    FCL_REAL z = boxes_[i][2];
    FCL_REAL size = boxes_[i][3];
    FCL_REAL cost = boxes_[i][4];
    FCL_REAL threshold = boxes_[i][5];

    Box* box = new Box(size, size, size);
    box->cost_density = cost;
    box->threshold_occupied = threshold;
    CollisionObject* obj = new CollisionObject(boost::shared_ptr<CollisionGeometry>(box), Transform3f(Vec3f(x, y, z)));
    boxes.push_back(obj);
  }

  std::cout << "boxes size: " << boxes.size() << std::endl;

}

int main(int argc, char** argv) {


  //##############################################################

  OcTree tree (0.05);
  tree.enableChangeDetection(true);

  point3d origin (0.01f, 0.01f, 0.02f);
  point3d point_on_surface (4.01f,0.01f,0.01f);
  tree.insertRay(origin, point_on_surface);
  printChanges(tree);
  tree.updateNode(point3d(2.01f, 0.01f, 0.01f), 2.0f);
  printChanges(tree);
  tree.updateNode(point3d(2.01f, 0.01f, 0.01f), -2.0f);
  printChanges(tree);

  cout << "generating spherical scan at " << origin << " ..." << endl;

  for (int i=-100; i<101; i++) {
    Pointcloud cloud;
    for (int j=-100; j<101; j++) {
      point3d rotated = point_on_surface;
      rotated.rotate_IP(0, DEG2RAD(i*0.5), DEG2RAD(j*0.5));
      cloud.push_back(rotated);
    }

    // insert in global coordinates:
    tree.insertPointCloud(cloud, origin, -1);
  }

  printChanges(tree);


  cout << "done." << endl;

  return 0;
}

int main(int argc, char** argv) {

  cout << endl;
  cout << "generating example map" << endl;

  OcTree tree (0.1);  // create empty tree with resolution 0.1


  // insert some measurements of occupied cells

  for (int x=-20; x<20; x++) {
    for (int y=-20; y<20; y++) {
      for (int z=-20; z<20; z++) {
        point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
        tree.updateNode(endpoint, true); // integrate 'occupied' measurement
      }
    }
  }

  // insert some measurements of free cells

  for (int x=-30; x<30; x++) {
    for (int y=-30; y<30; y++) {
      for (int z=-30; z<30; z++) {
        point3d endpoint ((float) x*0.02f-1.0f, (float) y*0.02f-1.0f, (float) z*0.02f-1.0f);
        tree.updateNode(endpoint, false);  // integrate 'free' measurement
      }
    }
  }

  cout << endl;
  cout << "performing some queries around the desired voxel:" << endl;
  
  point3d query;
  OcTreeNode* result = NULL;

  for(float z = -0.6; z < -0.21; z += 0.1){
    for(float y = -0.6; y < -0.21; y += 0.1){
      for(float x = -0.6; x < -0.21; x += 0.1){
        query = point3d(x, y, z);
        result = tree.search(query);
        print_query_info(query, result);
      }
    }
  }
  
  query = point3d(-0.5, -0.4, -0.4);
  result = tree.search(query);
	
  vector<point3d> normals;
  if (tree.getNormals(query, normals)){
  
    cout << endl;
    string s_norm = (normals.size() > 1) ? " normals " : " normal ";
    cout << "MC algorithm gives " << normals.size() << s_norm << "in voxel at " << query << endl;
    for(unsigned i = 0; i < normals.size(); ++i)
      cout << "\t" << normals[i].x() << "; " << normals[i].y() << "; " << normals[i].z() << endl;
  } else{
    cout << "query point unknown (no normals)\n";
  }
}

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

int main(int argc, char** argv) {
  string inputFilename = "";
  string outputFilename = "";

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

  inputFilename = std::string(argv[1]);
  if (argc == 3)
    outputFilename = std::string(argv[2]);
  else{
    outputFilename = inputFilename + ".ot";
  }


  cout << "\nReading OcTree file\n===========================\n";
  std::ifstream file(inputFilename.c_str(), std::ios_base::in |std::ios_base::binary);

  if (!file.is_open()){
    OCTOMAP_ERROR_STR("Filestream to "<< inputFilename << " not open, nothing read.");
    exit(-1);
  }

  std::istream::pos_type streampos = file.tellg();
  AbstractOcTree* tree;

  // reading binary:
  if (inputFilename.length() > 3 && (inputFilename.compare(inputFilename.length()-3, 3, ".bt") == 0)){
    OcTree* binaryTree = new OcTree(0.1);

    if (binaryTree->readBinary(file) && binaryTree->size() > 1)
      tree = binaryTree;
    else {
      OCTOMAP_ERROR_STR("Could not detect binary OcTree format in file.");
      exit(-1);

    }
  } else {
    tree = AbstractOcTree::read(file);
    if (!tree){
      OCTOMAP_WARNING_STR("Could not detect OcTree in file, trying legacy formats.");
      // TODO: check if .cot extension, try old format only then
      // reset and try old ColorOcTree format:
      file.clear(); // clear eofbit of istream
      file.seekg(streampos);
      ColorOcTree* colorTree = new ColorOcTree(0.1);
      colorTree->readData(file);
      if (colorTree->size() > 1 && file.good()){
        OCTOMAP_WARNING_STR("Detected Binary ColorOcTree to convert. \nPlease check and update the new file header (resolution will likely be wrong).");
        tree = colorTree;
      } else{
        delete colorTree;
        std::cerr << "Error reading from file " << inputFilename << std::endl;
        exit(-1);
      }
    }


  }

  // close filestream
  file.close();


  if (outputFilename.length() > 3 && (outputFilename.compare(outputFilename.length()-3, 3, ".bt") == 0)){
    std::cerr << "Writing binary (BonsaiTree) file" << std::endl;
    AbstractOccupancyOcTree* octree = dynamic_cast<AbstractOccupancyOcTree*>(tree);
    if (octree){
      if (!octree->writeBinary(outputFilename)){
        std::cerr << "Error writing to " << outputFilename << std::endl;
        exit(-2);
      }
    } else {
      std::cerr << "Error: Writing to .bt is not supported for this tree type: " << tree->getTreeType() << std::endl;
      exit(-2);
    }
  } else{
    std::cerr << "Writing general OcTree file" << std::endl;
    if (!tree->write(outputFilename)){
      std::cerr << "Error writing to " << outputFilename << std::endl;
      exit(-2);
    }
  }





  std::cout << "Finished writing to " << outputFilename << std::endl;
  exit(0);

}

int main(int argc, char** argv) {

  if (argc != 2){
    std::cerr << "Error: you need to specify a test as argument" << std::endl;
    return 1; // exit 1 means failure
  }
  std::string test_name (argv[1]);


  // ------------------------------------------------------------
  if (test_name == "MathVector") {
    // test constructors
    Vector3* twos = new Vector3();        
    Vector3* ones = new Vector3(1,1,1);    
    for (int i=0;i<3;i++) {
      (*twos)(i) = 2;
    }  
    // test basic operations
    Vector3 subtraction = *twos - *ones;
    Vector3 addition = *twos + *ones;
    Vector3 multiplication = *twos * 2.;
  
    for (int i=0;i<3;i++) {
      EXPECT_FLOAT_EQ (subtraction(i), 1.);
      EXPECT_FLOAT_EQ (addition(i), 3.);
      EXPECT_FLOAT_EQ (multiplication(i), 4.);
    }

    // copy constructor
    Vector3 rotation =  *ones;

    // rotation
    rotation.rotate_IP (M_PI, 1., 0.1);
    EXPECT_FLOAT_EQ (rotation.x(), 1.2750367);
    EXPECT_FLOAT_EQ (rotation.y(), (-1.1329513));
    EXPECT_FLOAT_EQ (rotation.z(), 0.30116868);
  
  // ------------------------------------------------------------
  } else if (test_name == "MathPose") {
    // constructors  
    Pose6D a (1.0f, 0.1f, 0.1f, 0.0f, 0.1f, (float) M_PI/4. );
    Pose6D b;

    Vector3 trans(1.0f, 0.1f, 0.1f);
    Quaternion rot(0.0f, 0.1f, (float) M_PI/4.);
    Pose6D c(trans, rot);

    // comparator
    EXPECT_TRUE ( a == c);
    // toEuler
    EXPECT_FLOAT_EQ (c.yaw() , M_PI/4.);

    // transform
    Vector3 t = c.transform (trans);
    EXPECT_FLOAT_EQ (t.x() , 1.6399229);
    EXPECT_FLOAT_EQ (t.y() , 0.8813442);
    EXPECT_FLOAT_EQ (t.z() , 0.099667005);

    // inverse transform
    Pose6D c_inv = c.inv();
    Vector3 t2 = c_inv.transform (t);
    EXPECT_FLOAT_EQ (t2.x() , trans.x());
    EXPECT_FLOAT_EQ (t2.y() , trans.y());
    EXPECT_FLOAT_EQ (t2.z() , trans.z());

  // ------------------------------------------------------------
  } else if (test_name == "InsertRay") {
    double p = 0.5;
    EXPECT_FLOAT_EQ(p, probability(logodds(p)));
    p = 0.1;
    EXPECT_FLOAT_EQ(p, probability(logodds(p)));
    p = 0.99;
    EXPECT_FLOAT_EQ(p, probability(logodds(p)));

    float l = 0;
    EXPECT_FLOAT_EQ(l, logodds(probability(l)));
    l = -4;
    EXPECT_FLOAT_EQ(l, logodds(probability(l)));
    l = 2;
    EXPECT_FLOAT_EQ(l, logodds(probability(l)));


    OcTree tree (0.05);
    tree.setProbHit(0.7);
    tree.setProbMiss(0.4);

    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++) {
        EXPECT_TRUE (tree.insertRay(origin, origin+point_on_surface));
        point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
      }
      point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
    }
    EXPECT_TRUE (tree.writeBinary("sphere_rays.bt"));
    EXPECT_EQ ((int) tree.size(), 50615);
  
  // ------------------------------------------------------------
//.........这里部分代码省略.........