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