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