示例#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
OcTree* extract_supporting_planes(OcTree* tree)
{
  OcTree* sp_tree = new OcTree(tree->getResolution());  

  int free = 0;
  int occupied = 0;
  int supported = 0;
  
  ROS_INFO("Extracting supporting planes from octomap");
  
  for(OcTree::leaf_iterator it = tree->begin_leafs(),
        end=tree->end_leafs(); it!= end; ++it)
    {
      if (tree->isNodeOccupied(*it))
        {
          occupied++;
          std::vector<point3d> normals;
          
          point3d p3d = it.getCoordinate();

          bool got_normals = tree->getNormals(p3d ,normals, true); 
          std::vector<point3d>::iterator normal_iter;
          
          point3d avg_normal (0.0, 0.0, 0.0);
          for(std::vector<point3d>::iterator normal_iter = normals.begin(), 
                end = normals.end(); normal_iter!= end; ++normal_iter)
            {
              avg_normal+= (*normal_iter);
            }
          if (normals.size() > 0) 
            {
              supported++;
              // cout << "#Normals: " << normals.size() << endl;

              avg_normal/= normals.size();       
              
              point3d z_axis ( 0.0, 0.0, 1.0);
              double angle = avg_normal.angleTo(z_axis);

              point3d coord = it.getCoordinate();

              if ( angle < ANGLE_MAX_DIFF)
                {
                  sp_tree->updateNode(coord,true);
                } 
            }  
        } 
      else 
        {
          free++;
        }
    }
  ROS_INFO("Extracted map size: %i (%i free, and %i occupied leaf nodes were discarded)", supported, free, occupied - supported);
  return sp_tree;
}
//float xx[70000], yy[70000], zz[70000];
void findProbabilityOfCones3D(Frustum frustum[], int num_poses3D)
{

  OcTree* input_tree = retrieve_octree(); //new OcTree(map)
  int free = 0;
  int occupied = 0;

  std::cerr<<"Inside findProbabilityOfCones3D"<< std::endl;
    for(OcTree::leaf_iterator it = input_tree->begin_leafs(),
        end=input_tree->end_leafs(); it!= end; ++it)
    {
      if (input_tree->isNodeOccupied(*it))
        {
		double size = it.getSize();
		double x = it.getX();
		double y = it.getY();
		double z = it.getZ();
		//point3d p = it.getCoordinate();
		//xx[occupied] = p.x;
		//yy[occupied] = p.y;
		//zz[occupied] = p.z;
		occupied++;
		//std::cerr<<"X: "<< x<< std::endl;
		//std::cerr<<"Y: "<< y<< std::endl;
		//std::cerr<<"Z: "<< z<< std::endl;
		for (int i = 0; i < num_poses3D; i++)
		{
			if (checkInsideCone3D(x, y, z, frustum[i]))
			{
        
				frustum[i].probability += 1 + (QSR_WEIGHT * (normal_dist_2d(x, y , QSR_MEAN_1 , QSR_VAR_1 , QSR_MEAN_2 , QSR_VAR_2)));
			}
			
		} 

        } 
	else 
        {
          free++;
        }
    }
    std::cerr<<"occupied "<< occupied<< std::endl;
    std::cerr<<"free "<< free<< std::endl;

}
示例#4
0
int compute_value(Frustum frustum, std::vector<unsigned short int> &keys, std::vector<int> &node_values)
{  
  int value = 0;
  int free = 0;
  int occupied = 0;
  int WEIGHT = 1; // compute weight from QSR model

  if (input_tree == NULL)
    return 0;

  for(OcTree::leaf_iterator it = input_tree->begin_leafs(),
        end=input_tree->end_leafs(); it!= end; ++it)
    {
      if (input_tree->isNodeOccupied(*it))
        {
          int size = (int) (it.getSize() / input_tree->getResolution());
          //ROS_INFO("Size %f", size); 
          double x = it.getX();
          double y = it.getY();
          double z = it.getZ();
          //std::cerr<< "xyz:" << x << "," << y << "," << z << " hash:" << hash << std::endl;
          occupied++;
          Vec3 point(x, y, z);
          if (frustum.is_inside(point))
            {
              //ROS_INFO("Node inside frustum");
              int node_value = WEIGHT * size;
              const OcTreeKey key = it.getKey();
              OcTreeKey::KeyHash computeHash;
              unsigned short int hash = computeHash(key);
              keys.push_back(hash);
              node_values.push_back(node_value);
              value += node_value;
            }
        }
      else 
        {
          free++;
        }
    }
  //std::cerr<<"occupied "<< occupied<< std::endl;
  //std::cerr<<"free "<< free<< std::endl;
  return value;
}
示例#5
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;
}
示例#6
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");
  }
}
示例#7
0
int main(int argc, char** argv)
{
  ros::init(argc, argv, "pose_3d");
  
  ros::NodeHandle node;
  
  ros::Rate rate(10.0);
  
  tf::TransformListener listener;
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  tf::StampedTransform stransform;
  tf::Quaternion q;

  ros::Publisher path_pub = node.advertise<nav_msgs::Path>("/youbotPath", 15);
  ros::Publisher grid_pub = node.advertise<nav_msgs::OccupancyGrid>("/nav_map", 15);
  ros::Publisher mark_pub = node.advertise<visualization_msgs::MarkerArray>("/NBVs", 15);
  
  //sleep(5);
  
  ros::Subscriber sub = node.subscribe("/octomap_binary", 15, map_cb);
  ros::Subscriber flag_sub = node.subscribe("/nbv_iter", 15, flag_cb);
  ros::Subscriber grid_sub = node.subscribe("/projected_map", 15, grid_cb);
  
  point3d sensorOffset(0.1, 0.3, 0);
  point3d firstPos;
  
  /*//Create transform from sensor to robot base
  transform.setOrigin( tf::Vector3(sensorOffset.x(), sensorOffset.y(), 0.0) );
  q.setRPY(0, 0, 0);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base", "xtion"));
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "baseGoal", "sensorGoal"));
  //-----------------------*/
  
  const clock_t start = std::clock();
  
  point3d curPose(-1.1, -0.05, 0.0);
  vector<point3d> NBVList;
  vector<point3d> PoseList;
  visualization_msgs::MarkerArray views;
  
  transform.setOrigin( tf::Vector3(curPose.x(), curPose.y(), 0) );
  q.setRPY(0, 0, DEG2RAD(-90));
  transform.setRotation(q);
  ros::Time gTime = ros::Time::now();
  br.sendTransform(tf::StampedTransform(transform, gTime, "vicon", "sensorGoal"));
  
  //Create transform from sensor to robot base
  //transform.setOrigin( tf::Vector3(sensorOffset.x(), sensorOffset.y(), 0.0) );
  //q.setRPY(0, 0, 0);
  //transform.setRotation(q);
  //br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "xtion", "base"));
  //br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "sensorGoal", "baseGoal"));
  //-----------------------
  
  sleep(1);
  
  //int x;
  //cin >> x;
  
  //moveBase(origin, yaw, &listener);
  
  cout << range << endl;
  
  cout << sRes << endl;
  
  //Setup Stuff
  //OcTree tree(res);
  
  cout << "Loading Sensor Model from File" << endl;
    
  ifstream readEm("SensorModel.bin", ios::in | ios::binary);
  
  while (!readEm.eof())
  {
    int buffer;
    readEm.read((char*)&buffer, sizeof(int));
    if (buffer < 0) break;
    optiMap.push_back(buffer);
    vector<int> tempRay;
    while (!readEm.eof())
    {
      readEm.read((char*)&buffer, sizeof(int));
      if (buffer < 0) break;
      tempRay.push_back(buffer);
    }
    optiRays.push_back(tempRay);
  }
  
  readEm.close();
  
  cout << "Finished Loading Sensor Model" << endl;
  
  double tempRoll, tempPitch, tempYaw;
  try{
  gTime = ros::Time::now();
  listener.waitForTransform("vicon", "xtion", gTime, ros::Duration(15.0));
  listener.lookupTransform("vicon", "xtion", gTime, stransform);
  }
  catch (tf::TransformException &ex) {
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
  }
  point3d origin(stransform.getOrigin().x(), stransform.getOrigin().y(), stransform.getOrigin().z());
  tf::Matrix3x3 rot(stransform.getRotation());
  rot.getRPY(tempRoll, tempPitch, tempYaw);
  NBVList.push_back(origin);
  
  cout << "Starting Pose: " << tempYaw << " : " << tempPitch << endl;
  
  visualization_msgs::Marker mark;
  mark.header.frame_id = "vicon";
  mark.header.stamp = ros::Time::now();
  mark.ns = "basic_shapes";
  mark.id = 0;
  mark.type = visualization_msgs::Marker::ARROW;
  mark.action = visualization_msgs::Marker::ADD;
  
  mark.pose.position.x = origin.x();
  mark.pose.position.y = origin.y();
  mark.pose.position.z = origin.z();
  
  q.setRPY(0, tempPitch, tempYaw);

  mark.pose.orientation.x = q.getX();
  mark.pose.orientation.y = q.getY();
  mark.pose.orientation.z = q.getZ();
  mark.pose.orientation.w = q.getW();
  
  mark.scale.x = 0.2;
  mark.scale.y = 0.02;
  mark.scale.z = 0.02;
  
  mark.color.r = 1.0f;
  mark.color.g = 0.0f;
  mark.color.a = 1.0;
  mark.color.b = 0.0f;
  
  views.markers.push_back(mark);
  
  mark_pub.publish(views);
  
  int NBVcount = 0;
  while (ros::ok())
  {
    /*cout << "NBV " << NBVcount++ << endl; 
    mapReceived = false;
    startAlg = false;
    while ((!mapReceived || !startAlg) && ros::ok())
    {
      mark_pub.publish(views);
      ros::spinOnce();
      rate.sleep();
    }
    
    //AbstractOcTree
    
    OcTree* treeTemp = binaryMsgToMap(mapMsg);
    
    //OcTree treeTemp(0.01);
    
    treeTemp->writeBinary("firstMap.bt");
    
    cout << "Loaded the Map" << endl;
    
    sleep(2);
    
    try{
    gTime = ros::Time::now();
    listener.waitForTransform("vicon", "xtion", gTime, ros::Duration(15.0));
    listener.lookupTransform("vicon", "xtion", gTime, stransform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }
    point3d camPosition (stransform.getOrigin().x(), stransform.getOrigin().y(), 0);
    origin = NBVList.back();
    
    cout << "Map Loaded " << treeTemp->getResolution() << endl;*/
    
    point3d egoPose(0, 0, 0);
    point3d endPoint(-1.4, -1.5, 0);
    
    //pObj Tuning Coefficients
    double alpha = 0.5;
    double beta = 2.0;
    
    size_t pointCount = 0;
    
    //point3d origin = curPose;
    point3d camPosition(0,0,0);
    
    OcTree tree(res);
    
    tree.setClampingThresMax(0.999);
    tree.setClampingThresMin(0.001);
    tree.setProbMiss(0.1);
    tree.setProbHit(0.995);
    
    tree.readBinary("firstMap.bt");
    
    tree.expand();
    
    for (OcTree::leaf_iterator iter = tree.begin_leafs(); iter != tree.end_leafs(); iter++)
    {
      bool flag = false;
      for (vector<point3d>::iterator nbvIt = NBVList.begin(); nbvIt != NBVList.end(); nbvIt++)
      {
	if (iter.getCoordinate().distanceXY(*nbvIt) < 0.6)
	{
	  flag = true;
	}
      }
      if (tree.isNodeOccupied(*iter)) // && (iter.getZ() < 0.03 || iter.getX() < -2 || iter.getX() > 1 || iter.getY() < -1 || iter.getY() > 1) || flag)
      {
	tree.setNodeValue(iter.getKey(), -lEmpty);
	tree.updateNode(iter.getKey(), false);
	
	if (tree.isNodeOccupied(*iter))
	{
	  cout << "Errrnk" << endl;
	}
      }
    }
    
    cout << origin.x() << ", " << origin.y() << endl;
    
    origin = tree.keyToCoord(tree.coordToKey(origin));
    
    cout << "Origin: " << origin.x() << " : " << origin.y() << " : " << origin.z() << endl;
    
    if (NBVcount == 1)
    {
      firstPos = origin;
    }
    
    //Use Second Tree to store pObj entities for NBV prediction    
    OcTree tree2(tree);
    
    string fileName = "algMap" + to_string(NBVcount) + ".bt";
    
    tree.writeBinary(fileName);
    
    //-------------------
    //Cost Function Work - 2D Map Creation, Dijkstras Path
    //-------------------
    double robotRad = 0.52;
    
    double minX, minY, minZ, maxX, maxY, maxZ;
    tree.getMetricMin(minX, minY, minZ);
    tree.getMetricMax(maxX, maxY, maxZ);
    point3d minPoint(minX, minY, minZ);
    point3d maxPoint(maxX, maxY, maxZ);
    
    cout << minX << ", " << maxX << ", " << minY << ", " << maxY << endl;
    minPoint = tree.keyToCoord(tree.coordToKey(minPoint));
    minX = minPoint.x();
    minY = minPoint.y();
    minZ = minPoint.z();
    maxPoint = tree.keyToCoord(tree.coordToKey(maxPoint));
    maxX = maxPoint.x();
    maxY = maxPoint.y();
    maxZ = maxPoint.z();
    
    cout << minX << ", " << maxX << ", " << minY << ", " << maxY << endl;
    
    tree.expand();
    
    int rangeX = round((maxX - minX) * rFactor);
    int rangeY = round((maxY - minY) * rFactor);
    
    vector<GridNode> gridMap(rangeX * rangeY, GridNode());
    vector<int> freeMap(rangeX * rangeY, 0);
    
    for (int x = 0; x < rangeX; x++)
    {
      for (int y = 0; y < rangeY; y++)
      {
	//cout << "XY: " << x << ", " << y << endl;
	gridMap[rangeX * y + x].coords = point3d(double(x) * res + minX, double(y) * res + minY, 0);
	
	for (int i = -1; i <= 1; i += 1)
	{
	  for (int j = -1; j <= 1; j += 1)
	  {
	    if ((i != 0 || j != 0) && x + i >= 0 && x + i < rangeX && y + j >= 0 && y + j < rangeY)
	    {
	      gridMap[rangeX * y + x].neighbors.push_back(&gridMap[rangeX * (y + j) + x + i]);
	      gridMap[rangeX * y + x].edges.push_back(sqrt(pow(i, 2) + pow(j, 2)) * res);
	      
	      //cout << gridMap[rangeX*y + x].neighbors.size() << ", " << gridMap[rangeX*y + x].edges.size() << endl;
	    }
	  }
	}
      }
    }
    
    cout << "First" << endl;
    
    tree.expand();
    for (OcTree::leaf_iterator iter = tree.begin_leafs(); iter != tree.end_leafs(); iter++)
    {
      /*if (!tree.isNodeOccupied(*iter) && iter.getZ() > 0)
      {
	int index = int((iter.getCoordinate().y() - minY) / res) * rangeX + (iter.getCoordinate().x() - minX) / res;
	
	gridMap[index].object = true;
	gridMap[index].occupied = true;
	
	for (double offX = -robotRad; offX <= robotRad; offX += res)
	{
	  for (double offY = -robotRad; offY <= robotRad; offY += res)
	  {
	    if (sqrt(pow(offX, 2) + pow(offY, 2)) <= robotRad)
	    {
	      int xInd = (gridMap[index].coords.x() + offX - minX) * rFactor;
	      int yInd = (gridMap[index].coords.y() + offY - minY) * rFactor;
	      
	      if (xInd >= 0 && xInd < rangeX && yInd >= 0 && yInd < rangeY)
	      {
		int offIdx = yInd * rangeX + xInd;
		gridMap[offIdx].occupied = true;
	      }
	    }
	  }
	}
      }*/
      
      if (!tree.isNodeOccupied(*iter) && iter.getZ() > 0 && iter.getZ() <= 0.5)
      {
	int index = round((iter.getCoordinate().y() - minY) / res) * rangeX + round((iter.getCoordinate().x() - minX) / res);
	freeMap[index]++;
      }
    }
    
    for (int index = 0; index < rangeX * rangeY; index++)
    {
      if (freeMap[index] < 20 && gridMap[index].coords.distanceXY(firstPos) > 0.8 && gridMap[index].coords.distanceXY(camPosition) > 0.2)
      {
	gridMap[index].object = true;
	gridMap[index].occupied = true;
	
	for (double offX = -robotRad; offX <= robotRad; offX += res)
	{
	  for (double offY = -robotRad; offY <= robotRad; offY += res)
	  {
	    if (sqrt(pow(offX, 2) + pow(offY, 2)) <= robotRad)
	    {
	      int xInd = round((gridMap[index].coords.x() + offX - minX) * rFactor);
	      int yInd = round((gridMap[index].coords.y() + offY - minY) * rFactor);
	      
	      if (xInd >= 0 && xInd < rangeX && yInd >= 0 && yInd < rangeY)
	      {
		int offIdx = yInd * rangeX + xInd;
		if (gridMap[offIdx].coords.distanceXY(firstPos) > 0.4 && gridMap[offIdx].coords.distanceXY(camPosition) > 0.3)
		{
		  gridMap[offIdx].occupied = true;
		}
	      }
	    }
	  }
	}
      }
    }
    
    cout << "Second" << endl;
    
    cout << rangeX << ", " << rangeY << " : " << int((origin.x() - minX) * rFactor) << ", " << int((origin.y() - minY) * rFactor) << endl;
    
    int origIdx = rangeX * int((origin.y() - minY) / res) + int((origin.x() - minX) / res);
    
    //round(((origin.y() - minY) * rFactor) * rangeX + (origin.x() - minX) * rFactor);
    
    cout << origIdx << "     " << gridMap.size() << endl;
    
    GridNode* originNode = &gridMap[origIdx];
    originNode->cost = 0;
    
    cout << "Got Here" << endl;
    
    MinHeap heapy;
    
    heapy.Push(originNode);
    
    while (heapy.GetLength() > 0)
    {
      GridNode* minNode = heapy.Pop();
      
      vector<float>::iterator edgeIt = minNode->edges.begin();
      for (vector<GridNode*>::iterator iter = minNode->neighbors.begin(); iter != minNode->neighbors.end(); iter++, edgeIt++)
      {
	GridNode* nodePt = *iter;
	if ((*iter)->occupied == 0 && (*iter)->cost > (minNode->cost + *edgeIt))
	{
	  (*iter)->parent = minNode;
	  
	  if ((*iter)->state == 0)
	  {
	    (*iter)->cost = minNode->cost + *edgeIt;
	    heapy.Push((*iter));
	    (*iter)->state = 1;
	  }
	  else if ((*iter)->state == 1)
	  {
	    heapy.Update((*iter)->heapIdx, minNode->cost + *edgeIt);
	  }
	}
      }
    }
    
    cout << "Dijkstra Path Complete" << endl;
    
    nav_msgs::OccupancyGrid grid;
    grid.header.stamp = ros::Time::now();
    grid.header.frame_id = "vicon";
    
    nav_msgs::MapMetaData metas;
    metas.resolution = res;
    metas.origin.position.x = minX;
    metas.origin.position.y = minY;
    metas.origin.position.z = 0.07;
    metas.height = rangeY;
    metas.width = rangeX;
    
    grid.info = metas;
    
    cout << gridMap.size() << endl;
    
    for (vector<GridNode>::iterator iter = gridMap.begin(); iter != gridMap.end(); iter++)
    {
      if (iter->occupied)
      {
	grid.data.push_back(100);
      }
      else
      {
	grid.data.push_back(0);
      }
    }
    
    grid_pub.publish(grid);
    
    cout << "Map Published" << endl;
    
    pointCount = 0;
    
    int cubeCount = 0;
    int emptyCount = 0;
    /*tree.expand();
    for (OcTree::leaf_iterator iter = tree.begin_leafs(); iter != tree.end_leafs(); iter++)
    {
      if (tree.isNodeOccupied(*iter))
      {
	pointCount++;
      }
      else
      {
	emptyCount++;
      }
    }*/
    
    cout << cubeCount << " cubes" << endl;
    
    cout << "Empties: " << emptyCount << endl;
    
    //OcTreeCustom infoTree(res);
    
    //infoTree.readBinary("kinect.bt");
    
    //Copy all occupancy values from known nodes into pObj variables for those nodes
    
    //infoTree.expand();
    
    /*for (OcTreeCustom::iterator iter = infoTree.begin(); iter != infoTree.end(); iter++)
    *   { *
    *   iter->setpObj(float(iter->getOccupancy()));
  }*/
    
    int cellCount = 0;
    int unkCount = 0;
    
    //pointCount = 0;
    
    cout << "Beginning pObj Processing" << endl;
    
    for (OcTree::leaf_iterator iter = tree.begin_leafs(); iter != tree.end_leafs(); iter++)
    {
      OcTreeKey occKey = iter.getKey();
      OcTreeKey unKey;
      OcTreeKey freeKey;
      OcTreeNode *cellNode;
      OcTreeNode *unNode;
      
      if (tree.isNodeOccupied(*iter))
      {
	cellCount++;
	
	for (int i = -1; i <= 1; i++)
	{
	  for (int j = -1; j <= 1; j++)
	  {
	    for (int k = -1; k <= 1; k++)
	    {
	      if (i != 0 || j != 0 || k != 0)
	      {
		unKey = occKey;
		unKey[0] += i;
		unKey[1] += j;
		unKey[2] += k;
		
		unNode = tree.search(unKey);
		
		if (unNode == NULL)
		{
		  bool critical = false;
		  for (int u = -1; u <= 1; u++)
		  {
		    for (int v = -1; v <= 1; v++)
		    {
		      for (int w = -1; w <= 1; w++)
		      {
			if (abs(u) + abs(v) + abs(w) == 1)
			{
			  freeKey = unKey;
			  freeKey[0] += u;
			  freeKey[1] += v;
			  freeKey[2] += w;
			  
			  cellNode = tree.search(freeKey);
			  
			  if (cellNode != NULL && !tree.isNodeOccupied(cellNode) && tree.keyToCoord(freeKey).z() > 0.15)
			  {//unKey is a critical unknown cell, proceed to update pObj of unknown cells in vicinity
			    critical = true;
			  }
			}
		      }
		    }
		  }
		  
		  if (critical)
		  {
		    //cout << "Occ: " << tree.keyToCoord(occKey).x() << ", " << tree.keyToCoord(occKey).y() << ", " << tree.keyToCoord(occKey).z() << endl;
		    //cout << "Unk: " << tree.keyToCoord(unKey).x() << ", " << tree.keyToCoord(unKey).y() << ", " << tree.keyToCoord(unKey).z() << endl;
		    tree2.updateNode(tree2.coordToKey(tree.keyToCoord(unKey)), (float)log(alpha / (1 - alpha)));
		    
		    //New method using custom tree
		    //infoTree.updateNode(tree.keyToCoord(unKey), float(0));
		    //infoTree.search(tree.keyToCoord(unKey))->setpObj((float)log(alpha / (1 - alpha)));
		    
		    unkCount++;
		    
		    for (double boxX = -5 * res; boxX <= 5 * res; boxX += res)
		    {
		      for (double boxY = -5 * res; boxY <= 5 * res; boxY += res)
		      {
			for (double boxZ = -5 * res; boxZ <= 5 * res; boxZ += res)
			{
			  point3d objPoint = tree.keyToCoord(unKey) + point3d(boxX, boxY, boxZ);
			  if (objPoint.z() > 0.05)
			  {
			    double pObj = alpha*exp(-sqrt(pow(boxX, 2) + pow(boxY, 2) + pow(boxZ, 2)) * beta);
			    cellNode = tree.search(objPoint);
			    if (cellNode == NULL)
			    {
			      if (tree2.search(objPoint) == NULL)
			      {
				tree2.setNodeValue(objPoint, log(pObj / (1 - pObj)));
				
				pointCount++;
			      }
			      else
			      {
				if ((tree2.search(objPoint))->getLogOdds() < (float)log(pObj / (1 - pObj)))
				{
				  tree2.setNodeValue(objPoint, log(pObj / (1 - pObj)));
				}
				//tree2.updateNode(objPoint, max((tree2.search(objPoint))->getLogOdds(), (float)log2(pObj / (1 - pObj))));
			      }
			    }
			    /*else
			    *											{
			    *												tree2.setNodeValue(objPoint, cellNode->getLogOdds());
			    * 
			    *												infoTree.search(objPoint)->setpObj(cellNode->getOccupancy());
			  }*/
			  }
			}
		      }
		    }
		  }
		}
	      }
	    }
	  }
	}
      }
    }
    
    /*infoTree.expand();
    * 
    *   f *or (OcTreeCustom::leaf_iterator iter = infoTree.begin_leafs(searchDepth); iter != infoTree.end_leafs(); iter++)
    *   {
    *   iter->updateOccupancyChildren();
    *   if (iter->getLogOdds() != 0 && abs(iter->getOccupancy() - iter->getpObj()) > 0.01)
    *   {
    *   cout << iter->getLogOdds() << ", " << iter->getOccupancy() << ", " << iter->getpObj() << endl;
    *   addCube(iter.getCoordinate(), res, 0, 0, 1, 0.5);
  }
  if (iter->getLogOdds() == 0 && iter->getpObj() > 0.001)
  {
  addCube(iter.getCoordinate(), sRes, 0, 1, 0, 0.5);
  }
  }*/
    
    /*tree.expand();
    tree2.expand();
    for (OcTree::leaf_iterator iter = tree.begin_leafs(); iter != tree.end_leafs(); iter++)
    {
      if (tree2.search(iter.getCoordinate())->getOccupancy() != iter->getOccupancy())
      {
	cout << tree2.search(iter.getCoordinate())->getOccupancy() << ", " << iter->getOccupancy() << endl;
      }
    }*/
    
    cout << "Nothing Different" << endl;
    
    tree2.writeBinary("objMap.bt");
    
    pointCount = 0;
    
    tree2.expand();
    
    KeyRay cellList;
    point3d egoCOG(2.95, 1.95, 0);
    Quaternion egoTheta(point3d(0, 0, -M_PI / 2));
    endPoint = point3d(3.0, 0, 0);
    OcTreeKey myCell;
    OcTreeNode *cellNode;
    double cellLike;
    
    Pose6D trialPose(egoCOG, egoTheta);
    endPoint = trialPose.transform(endPoint);
    
    //Pre-Computing Table Stuff
    point3d rayOrigin(0.005, 0.005, 0.005);
    point3d rayInit(nRayLength * res, 0, 0);
    point3d rayEnd;
    
    /*vector<int> castMap;
    *   v *ector<int> logAdd;
    *   
    *   cellCount = 0;
    *   int traversed = 0;
    *   
    *   Pointcloud castCloud;
    *   
    *   for (double yaw = 0; yaw < 360; yaw++)
    *   {
    *   cout << yaw << endl;
    *   for (double pitch = -90; pitch <= 90; pitch++)
    *   {
    *   Quaternion rayTheta(point3d(0, DEG2RAD(pitch), DEG2RAD(yaw)));
    *   Pose6D rayPose(rayOrigin, rayTheta);
    *   rayEnd = rayPose.transform(rayInit);
    *   
    *   cellList.reset();
    *   tree.computeRayKeys(rayOrigin, rayEnd, cellList);
    *   //cout << "cellList Size: " << cellList.size() << endl;
    *   for (KeyRay::iterator iter = ++cellList.begin(); iter != cellList.end(); iter++)
    *   {
    *   traversed++;
    *   point3d rayCoord = tree.keyToCoord(*iter) - rayOrigin;
    *   
    *   if (rayCoord.norm() * sRes / res > 0.6)
    *   {
    *   int index = round(rayCoord.x() / res) + nRayLength + (2 * nRayLength + 1) * round(rayCoord.y() / res + nRayLength) + pow(2 * nRayLength + 1, 2) * round(rayCoord.z() / res + nRayLength);
    *   vector<int>::iterator findIt = find(castMap.begin(), castMap.end(), index);
    *   if (findIt == castMap.end())
    *   {
    *   castMap.push_back(index);
    *   castCloud.push_back(rayCoord + rayOrigin);
    *   logAdd.push_back(lFilled);
    *   cellCount++;
  }
  else
  {
  int foundIndex = distance(castMap.begin(), findIt);
  logAdd[foundIndex] += lFilled;
  }
  }
  }
  }
  }
  cout << "Cells in CastMap: " << cellCount << "out of " << traversed << "cells traversed" << endl;
  cout << "Min Index: " << *min_element(castMap.begin(), castMap.end()) << endl;
  cout << "Max Index: " << *max_element(castMap.begin(), castMap.end()) << endl;
  cout << "Max logAdd: " << *max_element(logAdd.begin(), logAdd.end()) << endl;
  cout << nRayLength << endl;
  cout << "Cloud Size: " << castCloud.size() << endl;
  cout << "Inserting Cloud..." << endl;

  tree.insertPointCloud(castCloud, rayOrigin);
  tree.writeBinary("castMap.bt");

  //Write pre-computed tables to files for quick retrieval
  ofstream outFile("mapCast.bin", ios::out | ios::binary);
  for (vector<int>::iterator iter = castMap.begin(); iter != castMap.end(); iter++)
  {
  outFile.write((char*)&(*iter), sizeof(int));
  }
  outFile.close();

  outFile = ofstream("logAdd.bin", ios::out | ios::binary);
  for (vector<int>::iterator iter = logAdd.begin(); iter != logAdd.end(); iter++)
  {
  outFile.write((char*)&(*iter), sizeof(int));
  }
  outFile.close();

  cout << "Done Writing" << endl;

  while (true)
  {

  }*/
    
    //--------------------
    //Create 3D Arrays of object information for quick retrieval in infoGain calcs
    //--------------------
    
    //First pass to find limits of info bounding box	
    double temp_x, temp_y, temp_z;
    tree2.getMetricMax(temp_x, temp_y, temp_z);
    //infoTree.getMetricMin(temp_x, temp_y, temp_z);
    ibxMin = point3d(temp_x, temp_y, temp_z);
    tree2.getMetricMin(temp_x, temp_y, temp_z);
    //infoTree.getMetricMax(temp_x, temp_y, temp_z);
    ibxMax = point3d(temp_x, temp_y, temp_z);
    
    for (OcTree::iterator iter = tree2.begin_leafs(searchDepth); iter != tree2.end_leafs(); iter++)
    {
      if (iter->getOccupancy() > 0.001)
      {
	//addCube(iter.getCoordinate(), sRes, 0, 0, 1, 0.5);
	ibxMin.x() = min(ibxMin.x(), iter.getCoordinate().x());
	ibxMin.y() = min(ibxMin.y(), iter.getCoordinate().y());
	ibxMin.z() = min(ibxMin.z(), iter.getCoordinate().z());
	
	ibxMax.x() = max(ibxMax.x(), iter.getCoordinate().x());
	ibxMax.y() = max(ibxMax.y(), iter.getCoordinate().y());
	ibxMax.z() = max(ibxMax.z(), iter.getCoordinate().z());
      }
    }
    
    /*for (OcTreeCustom::leaf_iterator iter = infoTree.begin_leafs(searchDepth); iter != infoTree.end_leafs(); iter++)
    *   { *
    *   iter->updateOccupancyChildren();
    *   if (iter->getpObj() > 0.001)
    *   {
    *   if (iter.getCoordinate().x() < ibxMin.x()) ibxMin.x() = iter.getCoordinate().x();
    *   if (iter.getCoordinate().y() < ibxMin.y()) ibxMin.y() = iter.getCoordinate().y();
    *   if (iter.getCoordinate().z() < ibxMin.z()) ibxMin.z() = iter.getCoordinate().z();
    *   
    *   if (iter.getCoordinate().x() > ibxMax.x()) ibxMax.x() = iter.getCoordinate().x();
    *   if (iter.getCoordinate().y() > ibxMax.y()) ibxMax.y() = iter.getCoordinate().y();
    *   if (iter.getCoordinate().z() > ibxMax.z()) ibxMax.z() = iter.getCoordinate().z();
  }
  }*/
    
    cout << "Min Corner: " << ibxMin.x() << ", " << ibxMin.y() << ", " << ibxMin.z() << endl;
    cout << "Max Corner: " << ibxMax.x() << ", " << ibxMax.y() << ", " << ibxMax.z() << endl;
    
    cout << "S: " << sFactor << endl;
    
    //Determine required size of infoChunk array and initialize accordingly
    boxX = round((ibxMax.x() - ibxMin.x()) / sRes + 1);
    boxY = round((ibxMax.y() - ibxMin.y()) / sRes + 1);
    boxZ = round((ibxMax.z() - ibxMin.z()) / sRes + 1);
    
    cout << boxX << ", " << boxY << ", " << boxZ << endl;
    
    infoChunk = vector<vector<float> >(boxX * boxY * boxZ, { 0 , 0 });
    
    //Second pass to populate 3D array with info values
    tree2.expand();
    for (OcTree::leaf_bbx_iterator iter = tree2.begin_leafs_bbx(ibxMin, ibxMax, searchDepth); iter != tree2.end_leafs_bbx(); iter++)
    {
      if (iter.getCoordinate().z() > 0.05 && iter->getOccupancy() > 0.001)
      {
	int xInd = round((iter.getCoordinate().x() - ibxMin.x()) * sFactor);
	int yInd = round((iter.getCoordinate().y() - ibxMin.y()) * sFactor);
	int zInd = round((iter.getCoordinate().z() - ibxMin.z()) * sFactor);
	
	if (xInd < 0 || xInd >= boxX || yInd < 0 || yInd >= boxY || zInd < 0 || zInd >= boxZ) continue;
	
	int index = xInd + yInd * boxX + zInd * boxX * boxY;
	
	//addCube(iter.getCoordinate(), sRes, 0, 0, 1, 0.5);
	infoChunk[index][1] = iter->getOccupancy();
	infoChunk[index][0] = iter->getOccupancy();
      }
    }
    
    tree.expand();
    for (OcTree::leaf_bbx_iterator iter = tree.begin_leafs_bbx(ibxMin, ibxMax, searchDepth); iter != tree.end_leafs_bbx(); iter++)
    {
      int xInd = round((iter.getCoordinate().x() - ibxMin.x()) * sFactor);
      int yInd = round((iter.getCoordinate().y() - ibxMin.y()) * sFactor);
      int zInd = round((iter.getCoordinate().z() - ibxMin.z()) * sFactor);
      
      if (xInd < 0 || xInd >= boxX || yInd < 0 || yInd >= boxY || zInd < 0 || zInd >= boxZ) continue;
      
      int index = xInd + yInd * boxX + zInd * boxX * boxY;
      
      if (iter.getCoordinate().z() > 0.05 && tree.isNodeOccupied(*iter))
      {
	double p = exp(iter->getLogOdds()) / (1 + exp(iter->getLogOdds()));
	infoChunk[index][0] = (-p * log2(p) - (1 - p) * log2(1 - p)) * infoChunk[index][1];
	//infoChunk[index][0] = iter->getLogOdds();
      }
      else
      {
	infoChunk[index][0] = 0;
      }
    }
    
    /*for (OcTreeCustom::leaf_bbx_iterator iter = infoTree.begin_leafs_bbx(ibxMin, ibxMax, searchDepth); iter != infoTree.end_leafs_bbx(); iter++)
    *   { *
    *   iter->updateOccupancyChildren();
    *   if (iter->getpObj() > 0.001)
    *   {
    *   int xInd = round((iter.getCoordinate().x() - ibxMin.x()) / sRes);
    *   int yInd = round((iter.getCoordinate().y() - ibxMin.y()) / sRes);
    *   int zInd = round((iter.getCoordinate().z() - ibxMin.z()) / sRes);
    *   
    *   cout << iter->getLogOdds() << endl;
    *   infoChunk[xInd + yInd * boxX + zInd * boxX * boxY][0] = iter->getLogOdds();
    *   infoChunk[xInd + yInd * boxX + zInd * boxX * boxY][1] = iter->getpObj();
  }
  }*/
    
    cout << "infoChunk populated, uploading sensor model" << endl;
    
    cout << "Begin Test" << endl;
    
    //Search Algorithm
    vector<InfoNode> gains;
    float maxHeight = 0.62;
    float minHeight = 0.28;
    
    pointCount = 0;
    
    double maxIG = 0;
    double maxCost = 0;
    double maxPitch = 0;
    double maxYaw = 0;
    point3d bestPos(0, 0, 0);
    int viewCount = 0;
    
    int searchCount = 0;
    
    point3d searchBoxMin = ibxMin - point3d(1.5, 1.5, 1.5);
    point3d searchBoxMax = ibxMax + point3d(1.5, 1.5, 1.5);
    
    searchBoxMin.z() = max(searchBoxMin.z(), minHeight);
    searchBoxMax.z() = min(searchBoxMax.z(), maxHeight);
    
    cout << "Search Space: " << searchBoxMin.x() << ", " << searchBoxMin.y() << ", " << searchBoxMin.z() << endl;
    cout << "To: " << searchBoxMax.x() << ", " << searchBoxMax.y() << ", " << searchBoxMax.z() << endl;
    
    //-------------
    //Heuristic Search Method
    //-------------
    /*const clock_t searchStart = clock();
    * 
    *   i *nt neighbDist = 2;
    *   double neighborStepSize = 0.02;
    *   vector<InfoNode> neighbors(6 * neighbDist, InfoNode());
    *   
    *   tree.expand();
    *   double xMax, yMax, zMax, xMin, yMin, zMin;
    *   tree.getMetricMax(xMax, yMax, zMax);
    *   tree.getMetricMin(xMin, yMin, zMin);
    *   for (OcTree::leaf_bbx_iterator iter = tree.begin_leafs_bbx(searchBoxMin, searchBoxMax, 10); iter != tree.end_leafs_bbx(); iter++)
    *   {
    *   if (iter.getCoordinate().z() < 0 || iter.getCoordinate().x() < xMin || iter.getCoordinate().x() > xMax || iter.getCoordinate().y() < yMin || iter.getCoordinate().y() > yMax || iter.getCoordinate().z() < zMin || iter.getCoordinate().z() > zMax || tree.search(iter.getCoordinate()) == NULL || tree.isNodeOccupied(tree.search(iter.getCoordinate()))) continue;
    *   
    *   searchCount++;
  }

  cout << searchCount << " positions to calculate" << endl;
  for (OcTree::leaf_iterator iter = tree.begin_leafs(11); iter != tree.end_leafs(); iter++)
  {
  if (iter.getCoordinate().z() < 0 || iter.getCoordinate().x() < xMin || iter.getCoordinate().x() > xMax || iter.getCoordinate().y() < yMin || iter.getCoordinate().y() > yMax || iter.getCoordinate().z() < zMin || iter.getCoordinate().z() > zMax || tree.search(iter.getCoordinate()) == NULL || tree.isNodeOccupied(tree.search(iter.getCoordinate()))) continue;

  if (viewCount % 100 == 0)
  {
  cout << viewCount << endl;
  }
  point3d nextPos = iter.getCoordinate();
  float pitch, yaw;
  double infoGain = getInfoGain(&tree, &tree2, nextPos, &yaw, &pitch);

  if (infoGain > 0)
  {
  gains.push_back(InfoNode(infoGain, 0, nextPos, yaw, pitch));

  if (infoGain > maxIG)
  {
  maxIG = infoGain;
  bestPos = nextPos;
  maxYaw = yaw;
  maxPitch = pitch;
  }

  //addCube(nextPos, 0.05, 1 - (infoGain / 40), infoGain / 40, 0, 0.9);
  }

  viewCount++;
  }

  cout << "First Pass: "******" views" << endl;

  sort(gains.begin(), gains.end());

  vector<InfoNode>::iterator testIt = gains.end()--;

  vector<InfoNode> searchTails;

  vector<InfoNode>::iterator iter = gains.end()--;
  for (int i = 0; i < 5; i++)
  {
  cout << i << endl;
  InfoNode curNode = *iter;

  while (true)
  {
  cout << ".";
  point3d curPoint = curNode.coords;
  double curGain = curNode.infoGain;
  vector<InfoNode>::iterator neighbIt = neighbors.begin();

  for (int offX = -neighbDist; offX <= neighbDist; offX++)
  {
  for (int offY = -neighbDist; offY <= neighbDist; offY++)
  {
  for (int offZ = -neighbDist; offZ <= neighbDist; offZ++)
  {
  if ((offX != 0) + (offY != 0) + (offZ != 0) != 1) continue;

  point3d neighbor = curPoint + point3d(offX, offY, offZ) * neighborStepSize;
  neighbIt->coords = neighbor;

  if (neighbor.z() < 0 || neighbor.x() < xMin || neighbor.x() > xMax || neighbor.y() < yMin || neighbor.y() > yMax || neighbor.z() < zMin || neighbor.z() > zMax || tree.search(neighbor) == NULL || tree.isNodeOccupied(tree.search(neighbor)))
  {
  neighbIt->infoGain = 0;
  }
  else
  {
  float pitch, yaw;
  neighbIt->infoGain = getInfoGain(&tree, &tree2, neighbor, &yaw, &pitch);
  viewCount++;
  neighbIt->pitch = pitch;
  neighbIt->yaw = yaw;
  }

  neighbIt++;
  }
  }
  }

  InfoNode maxNeighb = *max_element(neighbors.begin(), neighbors.end());

  if (maxNeighb.infoGain <= curGain)
  {
  break;
  }
  else
  {
  curNode = maxNeighb;
  }
  }

  searchTails.push_back(curNode);

  iter--;

  cout << endl;
  }

  InfoNode hNBV = *max_element(searchTails.begin(), searchTails.end());

  const clock_t searchEnd = clock();

  cout << "Heuristic NBV: " << hNBV.coords.x() << ", " << hNBV.coords.y() << ", " << hNBV.coords.z() << endl;
  cout << "With Info Gain = " << hNBV.infoGain << endl;
  cout << "In " << double(searchEnd - searchStart) / CLOCKS_PER_SEC << " seconds, for " << viewCount << "candidate viewpoints" << endl;*/
    
    //Original Grid Search
    time_t time1 = time(0);
    viewCount = 0;
    
    tree.expand();
    for (OcTree::leaf_bbx_iterator iter = tree.begin_leafs_bbx(searchBoxMin, searchBoxMax, 14); iter != tree.end_leafs_bbx(); iter++)
    {
      if (!tree.isNodeOccupied(*iter))
      {
	searchCount++;
      }
    }
    
    vector<double> infoMap(rangeX * rangeY, 0);
    
    cout << searchCount << " positions to calculate" << endl;
    for (OcTree::leaf_iterator iter = tree.begin_leafs(14); iter != tree.end_leafs(); iter++)
    {
      if (tree.search(iter.getCoordinate()) == NULL || tree.isNodeOccupied(tree.search(iter.getCoordinate())) || iter.getCoordinate().z() < minHeight || iter.getCoordinate().z() > maxHeight) continue;
      
      if (viewCount % 100 == 0)
      {
	cout << viewCount << endl;
      }
      point3d nextPos = iter.getCoordinate();
      float pitch, yaw;
      double infoGain = getInfoGain(&tree, &tree2, nextPos, &yaw, &pitch);
      
      if (infoGain > 0)
      {
	double moveCost = gridMap[rangeX * int((nextPos.y() - minY) / res) + int((nextPos.x() - minX) / res)].cost;
	
	if (infoGain > infoMap[rangeX * int((nextPos.y() - minY) / res) + int((nextPos.x() - minX) / res)])
	{
	  infoMap[rangeX * int((nextPos.y() - minY) / res) + int((nextPos.x() - minX) / res)] = infoGain;
	}
	
	gains.push_back(InfoNode(infoGain, moveCost, nextPos, yaw, pitch));
	
	if (infoGain > maxIG)
	{
	  maxIG = infoGain;
	  bestPos = nextPos;
	  maxYaw = yaw;
	  maxPitch = pitch;
	}
	
	if (moveCost > maxCost && moveCost < 600)
	{
	  maxCost = moveCost;
	}
	
	/*infoCloud->points[pointCount].x = nextPos.x();
	*	infoCloud->points[pointCount].y = nextPos.y();
	*	infoCloud->points[pointCount].z = nextPos.z();
	*	infoCloud->points[pointCount].intensity = infoGain;
	*	pointCount++;*/
	
	//addCube(nextPos, 0.05, 1 - (infoGain / 40), infoGain / 40, 0, 0.9);
      }
      
      viewCount++;
    }
    
    cout << "Time for " << viewCount << " views: " << (int)time(0) - time1 << endl;
    cout << "The NBV is at (" << bestPos.x() << ", " << bestPos.y() << ", " << bestPos.z() << ") with an expected info gain of " << maxIG << endl;
    
    cout << maxYaw << ", " << maxPitch << endl;
    
    cout << "Max Cost: " << maxCost << endl;
    
    double maxQual = 0;
    double correctIG = 0;
    point3d NBV = origin;
    
    for (vector<InfoNode>::iterator iter = gains.begin(); iter != gains.end(); iter++)
    {
      //iter->infoGain /= maxIG;
      //iter->cost /= maxCost;
      
      if (iter->cost < 600) iter->quality = iter->infoGain - 0 * iter->cost;
      else iter->quality = 0;
      
      if (iter->quality > maxQual)
      {
	maxQual = iter->quality;
	correctIG = iter->infoGain;
	NBV = iter->coords;
	maxYaw = iter->yaw;
	maxPitch = iter->pitch;
      }
    }
    
    cout << "Time for " << viewCount << " views: " << (int)time(0) - time1 << endl;
    cout << "The corrected NBV is at (" << NBV.x() << ", " << NBV.y() << ", " << NBV.z() << ") with a quality of " << maxQual << "and IG of " << correctIG << endl;
    cout << "Yaw: " << maxYaw << ", " << "Pitch: " << maxPitch << endl;
    
    point3d oldBest = bestPos;
    
    if (maxIG < 10)
    {
      cout << "Algorithm Complete" << endl;
      break;
    }
    
    NBVList.push_back(NBV);
    visualization_msgs::Marker mark;
    mark.header.frame_id = "vicon";
    mark.header.stamp = ros::Time::now();
    mark.ns = "basic_shapes";
    mark.id = NBVcount;
    mark.type = visualization_msgs::Marker::ARROW;
    mark.action = visualization_msgs::Marker::ADD;
    
    mark.pose.position.x = NBV.x();
    mark.pose.position.y = NBV.y();
    mark.pose.position.z = NBV.z();
    
    q.setRPY(0, DEG2RAD(-maxPitch), DEG2RAD(maxYaw));

    mark.pose.orientation.x = q.getX();
    mark.pose.orientation.y = q.getY();
    mark.pose.orientation.z = q.getZ();
    mark.pose.orientation.w = q.getW();
    
    mark.scale.x = 0.2;
    mark.scale.y = 0.02;
    mark.scale.z = 0.02;
    
    mark.color.r = 0.0f;
    mark.color.g = 1.0f;
    mark.color.a = 1.0;
    mark.color.b = 0.0f;
    
    views.markers.push_back(mark);
    
    mark_pub.publish(views);
    
    /*transform.setOrigin( tf::Vector3(NBV.x(), NBV.y(), 0) );
    q.setRPY(0, 0, DEG2RAD(maxYaw));
    transform.setRotation(q);
    gTime = ros::Time::now();
    br.sendTransform(tf::StampedTransform(transform, gTime, "vicon", "sensorGoal"));
    
    sleep(1);
    
    try{
      listener.lookupTransform("vicon", "baseGoal",
				gTime, stransform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }
    point3d baseGoal(stransform.getOrigin().x(), stransform.getOrigin().y(), 0);
    m = tf::Matrix3x3(stransform.getRotation());
    double bRoll, bPitch, bYaw;
    m.getRPY(bRoll, bPitch, bYaw);*/ 
    
    point3d baseGoal = NBV;
    
    GridNode* endNode = &gridMap[rangeX * int((baseGoal.y() - minY) / res) + int((baseGoal.x() - minX) / res)];
    
    vector<point3d> waypoints;
    
    while (endNode != NULL)
    {
      //cout << endNode->coords.x() << ", " << endNode->coords.y() << endl;
      //cout << "Cost: " << endNode->cost << endl;
      endNode->cost = 0;
      waypoints.push_back(endNode->coords);
      //cout << endNode->coords.x() << ", " << endNode->coords.y() << endl;
      endNode = endNode->parent;
    }
    
    reverse(waypoints.begin(), waypoints.end());
    
    nav_msgs::Path path;
    path.header.stamp = ros::Time::now();
    path.header.frame_id = "vicon";
    
     int state = 0;
    int oldState = -1;
    for (vector<point3d>::iterator iter = waypoints.begin(); iter != waypoints.end(); iter++)
    {
      geometry_msgs::PoseStamped pose;
      pose.header.stamp = path.header.stamp;
      pose.header.frame_id = path.header.frame_id;
      pose.pose.position.x = iter->x();
      pose.pose.position.y = iter->y();
      pose.pose.position.z = NBV.z();
      pose.pose.orientation.y = maxPitch;
      pose.pose.orientation.z = maxYaw;
      
      if (path.poses.size() > 0)
      {
	if (iter->x() == path.poses.back().pose.position.x)
	{
	  state = 0;
	}
	else if (iter->y() == path.poses.back().pose.position.y)
	{
	  state = 1;
	}
	else
	{
	  state = 2;
	}
      }
      
      //cout << "State, Old State, Size: " << state << " : " << oldState << " : " << path.poses.size() << endl;
      
      if (path.poses.size() > 1 && state == oldState)
      {
	path.poses.pop_back();
	//cout << pose.pose.position.x << ", " << pose.pose.position.y << endl;
      }
      
      path.poses.push_back(pose);
      
      oldState = state;
    }
    
    /*for (vector<point3d>::iterator iter = waypoints.begin(); iter != waypoints.end(); iter++)
    {
      geometry_msgs::PoseStamped pose;
      pose.header.stamp = path.header.stamp;
      pose.header.frame_id = path.header.frame_id;
      pose.pose.position.x = iter->x();
      pose.pose.position.y = iter->y();
      pose.pose.position.z = NBV.z();
      pose.pose.orientation.y = maxPitch;
      pose.pose.orientation.z = maxYaw;
      
      
      path.poses.push_back(pose);
      
      //cout << path.poses.back().position.x << ", " << path.poses.back().position.y << endl;
    }*/
    
    cout << "Path Message Generated" << endl;
    
    path_pub.publish<nav_msgs::Path>(path);
    ros::spinOnce();
    
    cout << "Path Message Published" << endl;
  }
  
  while (ros::ok())
  {
    mark_pub.publish(views);
    rate.sleep();
    ros::spinOnce();
  }
  
  return 0;
}
int main(int argc, char** argv) {

  cout << endl;
  cout << "generating example map" << endl;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr static_cld(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr dynamic_cld(new pcl::PointCloud<pcl::PointXYZRGB>);
 
  if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("static.pcd",*static_cld)==-1){
     std::cout<<"ERror"<<std::endl;
}
  if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("chair.pcd",*dynamic_cld)==-1){
         std::cout<<"ERror"<<std::endl;
  }

	
  OcTree st_tree (0.01);  // create empty tree with resolution 0.1
  OcTree dy_tree (0.01);
  octomap::Pointcloud st_cld,dy_cld;
  //OccupancyOcTreeBase<OcTreeDataNode<float> >  st_occ(0.01);

  // insert some measurements of occupied cells
/*
  for (int x=-40; x<80; x++) {
    for (int y=-10; y<20; y++) {
      for (int z=-30; 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
      }
    }
  }
*/
  for(int i = 0;i<static_cld->size();i++){
//	cout<<static_cld->points[i]<<endl;
	point3d endpoint((float) static_cld->points[i].x,(float) static_cld->points[i].y,(float) static_cld->points[i].z);
	st_cld.push_back(endpoint);
	//st_tree.updateNode(endpoint,true);
  }
for(int i = 0;i<dynamic_cld->size();i++){
//	cout<<static_cld->points[i]<<endl;
	point3d endpoint((float) dynamic_cld->points[i].x,(float) dynamic_cld->points[i].y,(float) dynamic_cld->points[i].z);
	dy_cld.push_back(endpoint);
	//dy_tree.updateNode(endpoint,true);
  }

point3d origin(0.0,0.0,0.0);
st_tree.insertPointCloud(st_cld,origin);
st_tree.updateInnerOccupancy();

//st_occ.insertPointCloud(st_cld,origin);


for(OcTree::leaf_iterator it = st_tree.begin_leafs(),
       end=st_tree.end_leafs(); it!= end; ++it)
{
  //manipulate node, e.g.:
  std::cout << "Node center: " << it.getCoordinate() << std::endl;
  std::cout << "Node size: " << it.getSize() << std::endl;
  std::cout << "Node value: " << it->getValue() << std::endl;
  //v=v+(pow(it.getSize(),3));
}
//st_tree.computeUpdate(dy_cld,origin);

//dy_tree.insertPointCloud(dy_cld,origin);

/*
for(leaf_iterator it = st_tree->begin_leafs(),end = st_tree->end_leafs();it!=end;++it ){
	 std::cout << "Node center: " << it.getCoordinate() << std::endl;
	std::cout << "Node size: " << it.getSize() << std::endl;
	std::cout << "Node value: " << it->getValue() << std::endl;

}
*/

/*    
  point3d origin(0.0,0.0,0.0);	
  //tree.insertPointCloud(static_cld,origin); 
  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;
*/
  st_tree.writeBinary("static_occ.bt");
  
//  dy_tree.writeBinary("dynamic_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;  

}
示例#9
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();

}
示例#10
0
void findProbabilityOfCones2D(Cone cones2D[], int num_poses2D)
{

  std::string map = "octomap.bt";
  OcTree* input_tree = retrieve_octree(); //new OcTree(map);

  std::cerr<<"Inside findProbabilityOfCones2D"<< std::endl;
  
  for(OcTree::leaf_iterator it = input_tree->begin_leafs(),
        end=input_tree->end_leafs(); it!= end; ++it)
    {
      if (input_tree->isNodeOccupied(*it))
        {
          point3d p3d = it.getCoordinate();

          double x = p3d.x();
          double y = p3d.y();
               
          for (int k = 0; k < num_poses2D; k++)
            {
              if (checkInsideCone2D(y, x, cones2D[k]))
                {
                  //cones2D[k].probability += 1; 
                  cones2D[k].probability += 1 + (QSR_WEIGHT * (normal_dist_2d(x, y , QSR_MEAN_1 , QSR_VAR_1 , QSR_MEAN_2 , QSR_VAR_2))); 
                }
            } 
          
        }
    }

  for (int k = 0; k < num_poses2D; k++)
    {
      std::cerr<< std::endl<< "cone-"<< k <<" = "<<cones2D[k].probability<< std::endl;
    }
  
  // int count = 0;
  // int occupied_count = 0;
  // int occupied_count_check = 0;
  // int free_count = 0;
  // int unknown_count = 0;

  // std::cerr<< "inside findProbabilityOfCones2D:"<<  std::endl;
  
  // for (int i = 0; i < map_height; i++)
  //   {
  //     for (int j = 0; j < map_width; j++)
  //       {
  //         if (mapData.data[i*map_width+j] > 0)//(mapArray[i][j] > 0)
  //           {
  //             occupied_count_check++;
  //             for (int k = 0; k < num_poses2D; k++)
  //               {
  //                 if (checkInsideCone2D(j, i, cones2D[k]))
  //                   {
  //                     cones2D[k].probability += 1; 

  //                     double x_pos = j * map_resolution + map_origin_x;
  //                     double y_pos = i * map_resolution + map_origin_y;
            
  //                     cones2D[k].probability += 1 + (QSR_WEIGHT * (normal_dist_2d(x_pos, y_pos , QSR_MEAN_1 , QSR_VAR_1 , QSR_MEAN_2 , QSR_VAR_2))); 


  //                   }
  //               }
  //           }
  //       }
  //   }
  // for (int k = 0; k < num_poses2D; k++)
  //   {
  //     std::cerr<< std::endl<< "cone-"<< k <<" = "<<cones2D[k].probability<< std::endl;
  //   }
}