示例#1
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");
  }
}
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;  

}