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