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