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; }
int main(int argc, char** argv) { if (argc != 3) printUsage(argv[0]); string inputFilename = argv[1]; string outputFilename = argv[2]; // pcl point cloud pcl::PointCloud<pcl::PointXYZ>::Ptr pclcloud( new pcl::PointCloud<pcl::PointXYZ>() ); pcl::io::loadPCDFile( inputFilename, *pclcloud ); // data conversion Pointcloud * cloud = new Pointcloud; for ( size_t i = 0; i < pclcloud->size(); ++ i ) { point3d pt(pclcloud->points[i].x, pclcloud->points[i].y, pclcloud->points[i].z); cloud->push_back( pt ); } point3d sensor_origin(0,0,0); OcTree* tree = new OcTree(0.1); tree->insertPointCloud( cloud, sensor_origin ); tree->writeBinary(outputFilename); }
OcTree* generateSphereTree(point3d origin, float radius){ OcTree* tree = new OcTree(0.05); point3d point_on_surface = origin; point_on_surface.x() += radius; for (int i=0; i<360; i++) { for (int j=0; j<360; j++) { if (!tree->insertRay(origin, origin+point_on_surface)) { cout << "ERROR while inserting ray from " << origin << " to " << point_on_surface << endl; } point_on_surface.rotate_IP (0,0,DEG2RAD(1.)); } point_on_surface.rotate_IP (0,DEG2RAD(1.),0); } return tree; }
void App::octreeHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const drc::map_octree_t* msg){ std::cout << "MAP_OCTREE received\n"; // TODO: Currently not handling transform, assuming identity transform std::stringstream datastream; datastream.write((const char*) msg->data.data(), msg->num_bytes); tree_ = new octomap::OcTree(1); //resolution will be set by data from message tree_->readBinary(datastream); std::stringstream s; s << "/tmp/map_octomap.bt" ; printf("Saving MAP_OCTREE to: %s\n", s.str().c_str()); tree_->writeBinary(s.str().c_str()); exit(-1); }
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); std::list<OcTreeVolume> occupiedVoxels; tree->getOccupied(occupiedVoxels); delete tree; cout << "\nWriting " << occupiedVoxels.size()<<" 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"; std::list<OcTreeVolume>::iterator it; for (it = occupiedVoxels.begin(); it != occupiedVoxels.end(); ++it){ outfile << "Transform { translation " << it->first.x() << " " << it->first.y() << " " << it->first.z() << " \n children [" << " Shape { geometry Box { size " << it->second << " " << it->second << " " << it->second << "} } ]\n" << "}\n"; } outfile.close(); std::cout << "Finished writing to " << vrmlFilename << std::endl; }
OcTree* retrieve_octree() { ros::NodeHandle n; std::string servname = "octomap_binary"; ROS_INFO("Requesting the map from %s...", n.resolveName(servname).c_str()); GetOctomap::Request req; GetOctomap::Response resp; while(n.ok() && !ros::service::call(servname, req, resp)) { ROS_WARN("Request to %s failed; trying again...", n.resolveName(servname).c_str()); usleep(1000000); } OcTree* octree = octomap_msgs::binaryMsgToMap(resp.map); if (octree){ ROS_INFO("Map received (%zu nodes, %f m res)", octree->size(), octree->getResolution()); return extract_supporting_planes(octree); } return NULL; }
void printChanges(OcTree& tree){ unsigned int changedOccupied = 0; unsigned int changedFree = 0; unsigned int actualOccupied = 0; unsigned int actualFree = 0; unsigned int missingChanged = 0; tree.expand(); // iterate through the changed nodes KeyBoolMap::const_iterator it; for (it=tree.changedKeysBegin(); it!=tree.changedKeysEnd(); it++) { OcTreeNode* node = tree.search(it->first); if (node != NULL) { if (tree.isNodeOccupied(node)) { changedOccupied += 1; } else { changedFree += 1; } } else { missingChanged +=1; } } // iterate through the entire tree for(OcTree::tree_iterator it=tree.begin_tree(), end=tree.end_tree(); it!= end; ++it) { if (it.isLeaf()) { if (tree.isNodeOccupied(*it)) { actualOccupied += 1; } else { actualFree += 1; } } } cout << "change detection: " << changedOccupied << " occ; " << changedFree << " free; "<< missingChanged << " missing" << endl; cout << "actual: " << actualOccupied << " occ; " << actualFree << " free; " << endl; tree.prune(); }
int main(int argc, char** argv) { cout << endl; cout << "generating example map" << endl; OcTree tree (0.1); // create empty tree with resolution 0.1 // insert some measurements of occupied cells for (int x=-20; x<20; x++) { for (int y=-20; y<20; y++) { for (int z=-20; 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 } } } 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; tree.writeBinary("simple_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; }
// Callback function for the service 'view_eval' bool view_eval(viper::ViewValue::Request &req, viper::ViewValue::Response &res) { ROS_INFO("Received service request: view_eval (%f %f %f)", req.pose.position.x, req.pose.position.y, req.pose.position.z); geometry_msgs::Pose camera_pose = req.pose; //OcTree* octree = octomap_msgs::binaryMsgToMap(req.octomap); AbstractOcTree* tree = octomap_msgs::fullMsgToMap(req.octomap); if (!tree){ ROS_ERROR("Failed to recreate octomap"); return false; } OcTree* octree = dynamic_cast<OcTree*>(tree); if (octree){ ROS_INFO("Map received (%zu nodes, %f m res)", octree->size(), octree->getResolution()); input_tree = extract_supporting_planes(octree); } else{ ROS_ERROR("No map received!"); input_tree = NULL; } // Generate frustum. Frustum f = generate_frustum(camera_pose); int value = compute_value(f, res.keys, res.values); ROS_INFO("VALUE: %i", value); ROS_INFO("KEYS SIZE: %i", res.keys.size()); ROS_INFO("VALUES SIZE: %i", res.values.size()); res.value = value; res.frustum = get_points(generate_local_frustum()); ROS_INFO("Finished service request."); return true; }
void generateBoxesFromOctomap(std::vector<CollisionObject*>& boxes, OcTree& tree) { std::vector<boost::array<FCL_REAL, 6> > boxes_ = tree.toBoxes(); for(std::size_t i = 0; i < boxes_.size(); ++i) { FCL_REAL x = boxes_[i][0]; FCL_REAL y = boxes_[i][1]; FCL_REAL z = boxes_[i][2]; FCL_REAL size = boxes_[i][3]; FCL_REAL cost = boxes_[i][4]; FCL_REAL threshold = boxes_[i][5]; Box* box = new Box(size, size, size); box->cost_density = cost; box->threshold_occupied = threshold; CollisionObject* obj = new CollisionObject(boost::shared_ptr<CollisionGeometry>(box), Transform3f(Vec3f(x, y, z))); boxes.push_back(obj); } std::cout << "boxes size: " << boxes.size() << std::endl; }
int main(int argc, char** argv) { //############################################################## OcTree tree (0.05); tree.enableChangeDetection(true); point3d origin (0.01f, 0.01f, 0.02f); point3d point_on_surface (4.01f,0.01f,0.01f); tree.insertRay(origin, point_on_surface); printChanges(tree); tree.updateNode(point3d(2.01f, 0.01f, 0.01f), 2.0f); printChanges(tree); tree.updateNode(point3d(2.01f, 0.01f, 0.01f), -2.0f); printChanges(tree); cout << "generating spherical scan at " << origin << " ..." << endl; for (int i=-100; i<101; i++) { Pointcloud cloud; for (int j=-100; j<101; j++) { point3d rotated = point_on_surface; rotated.rotate_IP(0, DEG2RAD(i*0.5), DEG2RAD(j*0.5)); cloud.push_back(rotated); } // insert in global coordinates: tree.insertPointCloud(cloud, origin, -1); } printChanges(tree); cout << "done." << endl; return 0; }
int main(int argc, char** argv) { cout << endl; cout << "generating example map" << endl; OcTree tree (0.1); // create empty tree with resolution 0.1 // insert some measurements of occupied cells for (int x=-20; x<20; x++) { for (int y=-20; y<20; y++) { for (int z=-20; 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 } } } cout << endl; cout << "performing some queries around the desired voxel:" << endl; point3d query; OcTreeNode* result = NULL; for(float z = -0.6; z < -0.21; z += 0.1){ for(float y = -0.6; y < -0.21; y += 0.1){ for(float x = -0.6; x < -0.21; x += 0.1){ query = point3d(x, y, z); result = tree.search(query); print_query_info(query, result); } } } query = point3d(-0.5, -0.4, -0.4); result = tree.search(query); vector<point3d> normals; if (tree.getNormals(query, normals)){ cout << endl; string s_norm = (normals.size() > 1) ? " normals " : " normal "; cout << "MC algorithm gives " << normals.size() << s_norm << "in voxel at " << query << endl; for(unsigned i = 0; i < normals.size(); ++i) cout << "\t" << normals[i].x() << "; " << normals[i].y() << "; " << normals[i].z() << endl; } else{ cout << "query point unknown (no normals)\n"; } }
int main(int argc, char** argv) { if (argc != 2){ std::cerr << "Error: you need to specify a testfile (.bt) as argument to read" << std::endl; return 1; // exit 1 means failure } std::cout << "Testing empty OcTree...\n"; //empty tree { OcTree emptyTree(0.999); EXPECT_EQ(emptyTree.size(), 0); EXPECT_TRUE(emptyTree.writeBinary("empty.bt")); EXPECT_TRUE(emptyTree.write("empty.ot")); OcTree emptyReadTree(0.2); EXPECT_TRUE(emptyReadTree.readBinary("empty.bt")); EXPECT_EQ(emptyReadTree.size(), 0); EXPECT_TRUE(emptyTree == emptyReadTree); AbstractOcTree* readTreeAbstract = AbstractOcTree::read("empty.ot"); EXPECT_TRUE(readTreeAbstract); OcTree* readTreeOt = dynamic_cast<OcTree*>(readTreeAbstract); EXPECT_TRUE(readTreeOt); EXPECT_EQ(readTreeOt->size(), 0); EXPECT_TRUE(emptyTree == *readTreeOt); delete readTreeOt; } std::cout << "Testing reference OcTree from file ...\n"; string filename = string(argv[1]); { string filenameOt = "test_io_file.ot"; string filenameBtOut = "test_io_file.bt"; string filenameBtCopyOut = "test_io_file_copy.bt"; // read reference tree from input file OcTree tree (0.1); EXPECT_TRUE (tree.readBinary(filename)); std::cout << " Copy Constructor / assignment / ==\n"; // test copy constructor / assignment: OcTree* treeCopy = new OcTree(tree); EXPECT_TRUE(tree == *treeCopy); EXPECT_TRUE(treeCopy->writeBinary(filenameBtCopyOut)); // change a tree property, trees must be different afterwards treeCopy->setResolution(tree.getResolution()*2.0); EXPECT_FALSE(tree == *treeCopy); treeCopy->setResolution(tree.getResolution()); EXPECT_TRUE(tree == *treeCopy); // flip one value, trees must be different afterwards: point3d pt(0.5, 0.5, 0.5); OcTreeNode* node = treeCopy->search(pt); if (node && treeCopy->isNodeOccupied(node)) treeCopy->updateNode(pt, false); else treeCopy->updateNode(pt, true); EXPECT_FALSE(tree == *treeCopy); delete treeCopy; std::cout << " Swap\n"; // test swap: OcTree emptyT(tree.getResolution()); OcTree emptySw(emptyT); OcTree otherSw(tree); emptySw.swapContent(otherSw); EXPECT_FALSE(emptyT == emptySw); EXPECT_TRUE(emptySw == tree); EXPECT_TRUE(otherSw == emptyT); // write again to bt, read & compare EXPECT_TRUE(tree.writeBinary(filenameBtOut)); OcTree readTreeBt(0.1); EXPECT_TRUE(readTreeBt.readBinary(filenameBtOut)); EXPECT_TRUE(tree == readTreeBt); std::cout <<" Write to .ot / read through AbstractOcTree\n"; // now write to .ot, read & compare EXPECT_TRUE(tree.write(filenameOt)); AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameOt); EXPECT_TRUE(readTreeAbstract); OcTree* readTreeOt = dynamic_cast<OcTree*>(readTreeAbstract); EXPECT_TRUE(readTreeOt); EXPECT_TRUE(tree == *readTreeOt); // sanity test for "==": flip one node, compare again point3d coord(0.1f, 0.1f, 0.1f); node = readTreeOt->search(coord); if (node && readTreeOt->isNodeOccupied(node)) readTreeOt->updateNode(coord, false); else readTreeOt->updateNode(coord, true); EXPECT_FALSE(tree == *readTreeOt); delete readTreeOt; } // Test for tree headers and IO factory registry (color) { std::cout << "Testing ColorOcTree...\n"; double res = 0.02; std::string filenameColor = "test_io_color_file.ot"; ColorOcTree colorTree(res); EXPECT_EQ(colorTree.getTreeType(), "ColorOcTree"); ColorOcTreeNode* colorNode = colorTree.updateNode(point3d(0.0, 0.0, 0.0), true); ColorOcTreeNode::Color color_red(255, 0, 0); colorNode->setColor(color_red); colorTree.setNodeColor(0.0, 0.0, 0.0, 255, 0, 0); colorTree.updateNode(point3d(0.1f, 0.1f, 0.1f), true); colorTree.setNodeColor(0.1f, 0.1f, 0.1f, 0, 0, 255); EXPECT_TRUE(colorTree.write(filenameColor)); AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameColor); EXPECT_TRUE(readTreeAbstract); EXPECT_EQ(colorTree.getTreeType(), readTreeAbstract->getTreeType()); ColorOcTree* readColorTree = dynamic_cast<ColorOcTree*>(readTreeAbstract); EXPECT_TRUE(readColorTree); EXPECT_TRUE(colorTree == *readColorTree); colorNode = colorTree.search(0.0, 0.0, 0.0); EXPECT_TRUE(colorNode); EXPECT_EQ(colorNode->getColor(), color_red); delete readColorTree; } // Test for tree headers and IO factory registry (stamped) { std::cout << "Testing OcTreeStamped...\n"; double res = 0.05; std::string filenameStamped = "test_io_stamped_file.ot"; OcTreeStamped stampedTree(res); EXPECT_EQ(stampedTree.getTreeType(), "OcTreeStamped"); // TODO: add / modify some stamped nodes //ColorOcTreeNode* colorNode = colorTree.updateNode(point3d(0.0, 0.0, 0.0), true); //ColorOcTreeNode::Color color_red(255, 0, 0); //colorNode->setColor(color_red); //colorTree.setNodeColor(0.0, 0.0, 0.0, 255, 0, 0); //colorTree.updateNode(point3d(0.1f, 0.1f, 0.1f), true); //colorTree.setNodeColor(0.1f, 0.1f, 0.1f, 0, 0, 255); EXPECT_TRUE(stampedTree.write(filenameStamped)); AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameStamped); EXPECT_TRUE(readTreeAbstract); EXPECT_EQ(stampedTree.getTreeType(), readTreeAbstract->getTreeType()); OcTreeStamped* readStampedTree = dynamic_cast<OcTreeStamped*>(readTreeAbstract); EXPECT_TRUE(readStampedTree); EXPECT_TRUE(stampedTree == *readStampedTree); //colorNode = colorTree.search(0.0, 0.0, 0.0); //EXPECT_TRUE(colorNode); //EXPECT_EQ(colorNode->getColor(), color_red); delete readStampedTree; } std::cerr << "Test successful.\n"; return 0; }
int main(int argc, char** argv) { string inputFilename = ""; string outputFilename = ""; if (argc < 2 || argc > 3 || (argc > 1 && strcmp(argv[1], "-h") == 0)){ printUsage(argv[0]); } inputFilename = std::string(argv[1]); if (argc == 3) outputFilename = std::string(argv[2]); else{ outputFilename = inputFilename + ".ot"; } cout << "\nReading OcTree file\n===========================\n"; std::ifstream file(inputFilename.c_str(), std::ios_base::in |std::ios_base::binary); if (!file.is_open()){ OCTOMAP_ERROR_STR("Filestream to "<< inputFilename << " not open, nothing read."); exit(-1); } std::istream::pos_type streampos = file.tellg(); AbstractOcTree* tree; // reading binary: if (inputFilename.length() > 3 && (inputFilename.compare(inputFilename.length()-3, 3, ".bt") == 0)){ OcTree* binaryTree = new OcTree(0.1); if (binaryTree->readBinary(file) && binaryTree->size() > 1) tree = binaryTree; else { OCTOMAP_ERROR_STR("Could not detect binary OcTree format in file."); exit(-1); } } else { tree = AbstractOcTree::read(file); if (!tree){ OCTOMAP_WARNING_STR("Could not detect OcTree in file, trying legacy formats."); // TODO: check if .cot extension, try old format only then // reset and try old ColorOcTree format: file.clear(); // clear eofbit of istream file.seekg(streampos); ColorOcTree* colorTree = new ColorOcTree(0.1); colorTree->readData(file); if (colorTree->size() > 1 && file.good()){ OCTOMAP_WARNING_STR("Detected Binary ColorOcTree to convert. \nPlease check and update the new file header (resolution will likely be wrong)."); tree = colorTree; } else{ delete colorTree; std::cerr << "Error reading from file " << inputFilename << std::endl; exit(-1); } } } // close filestream file.close(); if (outputFilename.length() > 3 && (outputFilename.compare(outputFilename.length()-3, 3, ".bt") == 0)){ std::cerr << "Writing binary (BonsaiTree) file" << std::endl; AbstractOccupancyOcTree* octree = dynamic_cast<AbstractOccupancyOcTree*>(tree); if (octree){ if (!octree->writeBinary(outputFilename)){ std::cerr << "Error writing to " << outputFilename << std::endl; exit(-2); } } else { std::cerr << "Error: Writing to .bt is not supported for this tree type: " << tree->getTreeType() << std::endl; exit(-2); } } else{ std::cerr << "Writing general OcTree file" << std::endl; if (!tree->write(outputFilename)){ std::cerr << "Error writing to " << outputFilename << std::endl; exit(-2); } } std::cout << "Finished writing to " << outputFilename << std::endl; exit(0); }
int main(int argc, char** argv) { if (argc != 2){ std::cerr << "Error: you need to specify a test as argument" << std::endl; return 1; // exit 1 means failure } std::string test_name (argv[1]); // ------------------------------------------------------------ if (test_name == "MathVector") { // test constructors Vector3* twos = new Vector3(); Vector3* ones = new Vector3(1,1,1); for (int i=0;i<3;i++) { (*twos)(i) = 2; } // test basic operations Vector3 subtraction = *twos - *ones; Vector3 addition = *twos + *ones; Vector3 multiplication = *twos * 2.; for (int i=0;i<3;i++) { EXPECT_FLOAT_EQ (subtraction(i), 1.); EXPECT_FLOAT_EQ (addition(i), 3.); EXPECT_FLOAT_EQ (multiplication(i), 4.); } // copy constructor Vector3 rotation = *ones; // rotation rotation.rotate_IP (M_PI, 1., 0.1); EXPECT_FLOAT_EQ (rotation.x(), 1.2750367); EXPECT_FLOAT_EQ (rotation.y(), (-1.1329513)); EXPECT_FLOAT_EQ (rotation.z(), 0.30116868); // ------------------------------------------------------------ } else if (test_name == "MathPose") { // constructors Pose6D a (1.0f, 0.1f, 0.1f, 0.0f, 0.1f, (float) M_PI/4. ); Pose6D b; Vector3 trans(1.0f, 0.1f, 0.1f); Quaternion rot(0.0f, 0.1f, (float) M_PI/4.); Pose6D c(trans, rot); // comparator EXPECT_TRUE ( a == c); // toEuler EXPECT_FLOAT_EQ (c.yaw() , M_PI/4.); // transform Vector3 t = c.transform (trans); EXPECT_FLOAT_EQ (t.x() , 1.6399229); EXPECT_FLOAT_EQ (t.y() , 0.8813442); EXPECT_FLOAT_EQ (t.z() , 0.099667005); // inverse transform Pose6D c_inv = c.inv(); Vector3 t2 = c_inv.transform (t); EXPECT_FLOAT_EQ (t2.x() , trans.x()); EXPECT_FLOAT_EQ (t2.y() , trans.y()); EXPECT_FLOAT_EQ (t2.z() , trans.z()); // ------------------------------------------------------------ } else if (test_name == "InsertRay") { double p = 0.5; EXPECT_FLOAT_EQ(p, probability(logodds(p))); p = 0.1; EXPECT_FLOAT_EQ(p, probability(logodds(p))); p = 0.99; EXPECT_FLOAT_EQ(p, probability(logodds(p))); float l = 0; EXPECT_FLOAT_EQ(l, logodds(probability(l))); l = -4; EXPECT_FLOAT_EQ(l, logodds(probability(l))); l = 2; EXPECT_FLOAT_EQ(l, logodds(probability(l))); OcTree tree (0.05); tree.setProbHit(0.7); tree.setProbMiss(0.4); point3d origin (0.01f, 0.01f, 0.02f); point3d point_on_surface (2.01f,0.01f,0.01f); for (int i=0; i<360; i++) { for (int j=0; j<360; j++) { EXPECT_TRUE (tree.insertRay(origin, origin+point_on_surface)); point_on_surface.rotate_IP (0,0,DEG2RAD(1.)); } point_on_surface.rotate_IP (0,DEG2RAD(1.),0); } EXPECT_TRUE (tree.writeBinary("sphere_rays.bt")); EXPECT_EQ ((int) tree.size(), 50615); // ------------------------------------------------------------ // ray casting is now in "test_raycasting.cpp" // ------------------------------------------------------------ // insert scan test // insert graph node test // write graph test } else if (test_name == "InsertScan") { Pointcloud* measurement = new Pointcloud(); point3d origin (0.01f, 0.01f, 0.02f); point3d point_on_surface (2.01f, 0.01f, 0.01f); for (int i=0; i<360; i++) { for (int j=0; j<360; j++) { point3d p = origin+point_on_surface; measurement->push_back(p); point_on_surface.rotate_IP (0,0,DEG2RAD(1.)); } point_on_surface.rotate_IP (0,DEG2RAD(1.),0); } OcTree tree (0.05); tree.insertPointCloud(*measurement, origin); EXPECT_EQ (tree.size(), 53959); ScanGraph* graph = new ScanGraph(); Pose6D node_pose (origin.x(), origin.y(), origin.z(),0.0f,0.0f,0.0f); graph->addNode(measurement, node_pose); EXPECT_TRUE (graph->writeBinary("test.graph")); delete graph; // ------------------------------------------------------------ // graph read file test } else if (test_name == "ReadGraph") { // not really meaningful, see better test in "test_scans.cpp" ScanGraph graph; EXPECT_TRUE (graph.readBinary("test.graph")); // ------------------------------------------------------------ } else if (test_name == "StampedTree") { OcTreeStamped stamped_tree (0.05); // fill tree for (int x=-20; x<20; x++) for (int y=-20; y<20; y++) for (int z=-20; z<20; z++) { point3d p ((float) x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f); stamped_tree.updateNode(p, true); // integrate 'occupied' measurement } // test if update times set point3d query (0.1f, 0.1f, 0.1f); OcTreeNodeStamped* result = stamped_tree.search (query); EXPECT_TRUE (result); unsigned int tree_time = stamped_tree.getLastUpdateTime(); unsigned int node_time = result->getTimestamp(); std::cout << "After 1st update (cube): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp() << std::endl; EXPECT_TRUE (tree_time > 0); #ifdef _WIN32 Sleep(1000); #else sleep(1); #endif stamped_tree.integrateMissNoTime(result); // reduce occupancy, no time update std::cout << "After 2nd update (single miss): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << node_time << std::endl; EXPECT_EQ (node_time, result->getTimestamp()); // node time updated? point3d query2 = point3d (0.1f, 0.1f, 0.3f); stamped_tree.updateNode(query2, true); // integrate 'occupied' measurement OcTreeNodeStamped* result2 = stamped_tree.search (query2); EXPECT_TRUE (result2); result = stamped_tree.search (query); EXPECT_TRUE (result); std::cout << "After 3rd update (single hit at (0.1, 0.1, 0.3): Tree time " << stamped_tree.getLastUpdateTime() << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp() << "; node(0.1, 0.1, 0.3) time " << result2->getTimestamp() << std::endl; EXPECT_TRUE (result->getTimestamp() < result2->getTimestamp()); // result2 has been updated EXPECT_EQ(result2->getTimestamp(), stamped_tree.getLastUpdateTime()); // ------------------------------------------------------------ } else if (test_name == "OcTreeKey") { OcTree tree (0.05); point3d p(0.0,0.0,0.0); OcTreeKey key; tree.coordToKeyChecked(p, key); point3d p_inv = tree.keyToCoord(key); EXPECT_FLOAT_EQ (0.025, p_inv.x()); EXPECT_FLOAT_EQ (0.025, p_inv.y()); EXPECT_FLOAT_EQ (0.025, p_inv.z()); // ------------------------------------------------------------ } else { std::cerr << "Invalid test name specified: " << test_name << std::endl; return 1; } std::cerr << "Test successful.\n"; return 0; }
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) { // default values: double res = 0.1; if (argc < 2) printUsage(argv[0]); string graphFilename = std::string(argv[1]); double maxrange = -1; int max_scan_no = -1; int skip_scan_eval = 5; int arg = 1; while (++arg < argc) { if (! strcmp(argv[arg], "-i")) graphFilename = std::string(argv[++arg]); else if (! strcmp(argv[arg], "-res")) res = atof(argv[++arg]); else if (! strcmp(argv[arg], "-m")) maxrange = atof(argv[++arg]); else if (! strcmp(argv[arg], "-n")) max_scan_no = atoi(argv[++arg]); else { printUsage(argv[0]); } } cout << "\nReading Graph file\n===========================\n"; ScanGraph* graph = new ScanGraph(); if (!graph->readBinary(graphFilename)) exit(2); size_t num_points_in_graph = 0; if (max_scan_no > 0) { num_points_in_graph = graph->getNumPoints(max_scan_no-1); cout << "\n Data points in graph up to scan " << max_scan_no << ": " << num_points_in_graph << endl; } else { num_points_in_graph = graph->getNumPoints(); cout << "\n Data points in graph: " << num_points_in_graph << endl; } cout << "\nCreating tree\n===========================\n"; OcTree* tree = new OcTree(res); size_t numScans = graph->size(); unsigned int currentScan = 1; for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) { if (currentScan % skip_scan_eval != 0){ if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush; else cout << "("<<currentScan << "/" << numScans << ") " << flush; tree->insertPointCloud(**scan_it, maxrange); } else cout << "(SKIP) " << flush; if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no)) break; currentScan++; } tree->expand(); cout << "\nEvaluating scans\n===========================\n"; currentScan = 1; size_t num_points = 0; size_t num_voxels_correct = 0; size_t num_voxels_wrong = 0; size_t num_voxels_unknown = 0; for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) { if (currentScan % skip_scan_eval == 0){ if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush; else cout << "("<<currentScan << "/" << numScans << ") " << flush; pose6d frame_origin = (*scan_it)->pose; point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans()); // transform pointcloud: Pointcloud scan (*(*scan_it)->scan); scan.transform(frame_origin); point3d origin = frame_origin.transform(sensor_origin); KeySet free_cells, occupied_cells; tree->computeUpdate(scan, origin, free_cells, occupied_cells, maxrange); num_points += scan.size(); // count free cells for (KeySet::iterator it = free_cells.begin(); it != free_cells.end(); ++it) { OcTreeNode* n = tree->search(*it); if (n){ if (tree->isNodeOccupied(n)) num_voxels_wrong++; else num_voxels_correct++; } else num_voxels_unknown++; } // count occupied cells for (KeySet::iterator it = occupied_cells.begin(); it != occupied_cells.end(); ++it) { OcTreeNode* n = tree->search(*it); if (n){ if (tree->isNodeOccupied(n)) num_voxels_correct++; else num_voxels_wrong++; } else num_voxels_unknown++; } } if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no)) break; currentScan++; } cout << "\nFinished evaluating " << num_points <<"/"<< num_points_in_graph << " points.\n" <<"Voxels correct: "<<num_voxels_correct<<" #wrong: " <<num_voxels_wrong << " #unknown: " <<num_voxels_unknown <<". % correct: "<< num_voxels_correct/double(num_voxels_correct+num_voxels_wrong)<<"\n\n"; delete graph; delete tree; return 0; }
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(); }
int main(int argc, char** argv) { // default values: double res = 0.1; string graphFilename = ""; string treeFilename = ""; double maxrange = -1; int max_scan_no = -1; bool detailedLog = false; bool simpleUpdate = false; bool discretize = false; unsigned char compression = 1; // get default sensor model values: OcTree emptyTree(0.1); double clampingMin = emptyTree.getClampingThresMin(); double clampingMax = emptyTree.getClampingThresMax(); double probMiss = emptyTree.getProbMiss(); double probHit = emptyTree.getProbHit(); timeval start; timeval stop; int arg = 0; while (++arg < argc) { if (! strcmp(argv[arg], "-i")) graphFilename = std::string(argv[++arg]); else if (!strcmp(argv[arg], "-o")) treeFilename = std::string(argv[++arg]); else if (! strcmp(argv[arg], "-res") && argc-arg < 2) printUsage(argv[0]); else if (! strcmp(argv[arg], "-res")) res = atof(argv[++arg]); else if (! strcmp(argv[arg], "-log")) detailedLog = true; else if (! strcmp(argv[arg], "-simple")) simpleUpdate = true; else if (! strcmp(argv[arg], "-discretize")) discretize = true; else if (! strcmp(argv[arg], "-compress")) OCTOMAP_WARNING("Argument -compress no longer has an effect, incremental pruning is done during each insertion.\n"); else if (! strcmp(argv[arg], "-compressML")) compression = 2; else if (! strcmp(argv[arg], "-m")) maxrange = atof(argv[++arg]); else if (! strcmp(argv[arg], "-n")) max_scan_no = atoi(argv[++arg]); else if (! strcmp(argv[arg], "-clamping") && (argc-arg < 3)) printUsage(argv[0]); else if (! strcmp(argv[arg], "-clamping")){ clampingMin = atof(argv[++arg]); clampingMax = atof(argv[++arg]); } else if (! strcmp(argv[arg], "-sensor") && (argc-arg < 3)) printUsage(argv[0]); else if (! strcmp(argv[arg], "-sensor")){ probMiss = atof(argv[++arg]); probHit = atof(argv[++arg]); } else { printUsage(argv[0]); } } if (graphFilename == "" || treeFilename == "") printUsage(argv[0]); // verify input: if (res <= 0.0){ OCTOMAP_ERROR("Resolution must be positive"); exit(1); } if (clampingMin >= clampingMax || clampingMin < 0.0 || clampingMax > 1.0){ OCTOMAP_ERROR("Error in clamping values: 0.0 <= [%f] < [%f] <= 1.0\n", clampingMin, clampingMax); exit(1); } if (probMiss >= probHit || probMiss < 0.0 || probHit > 1.0){ OCTOMAP_ERROR("Error in sensor model (hit/miss prob.): 0.0 <= [%f] < [%f] <= 1.0\n", probMiss, probHit); exit(1); } std::string treeFilenameOT = treeFilename + ".ot"; std::string treeFilenameMLOT = treeFilename + "_ml.ot"; cout << "\nReading Graph file\n===========================\n"; ScanGraph* graph = new ScanGraph(); if (!graph->readBinary(graphFilename)) exit(2); unsigned int num_points_in_graph = 0; if (max_scan_no > 0) { num_points_in_graph = graph->getNumPoints(max_scan_no-1); cout << "\n Data points in graph up to scan " << max_scan_no << ": " << num_points_in_graph << endl; } else { num_points_in_graph = graph->getNumPoints(); cout << "\n Data points in graph: " << num_points_in_graph << endl; } // transform pointclouds first, so we can directly operate on them later for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) { pose6d frame_origin = (*scan_it)->pose; point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans()); (*scan_it)->scan->transform(frame_origin); point3d transformed_sensor_origin = frame_origin.transform(sensor_origin); (*scan_it)->pose = pose6d(transformed_sensor_origin, octomath::Quaternion()); } std::ofstream logfile; if (detailedLog){ logfile.open((treeFilename+".log").c_str()); logfile << "# Memory of processing " << graphFilename << " over time\n"; logfile << "# Resolution: "<< res <<"; compression: " << int(compression) << "; scan endpoints: "<< num_points_in_graph << std::endl; logfile << "# [scan number] [bytes octree] [bytes full 3D grid]\n"; } cout << "\nCreating tree\n===========================\n"; OcTree* tree = new OcTree(res); tree->setClampingThresMin(clampingMin); tree->setClampingThresMax(clampingMax); tree->setProbHit(probHit); tree->setProbMiss(probMiss); gettimeofday(&start, NULL); // start timer unsigned int numScans = graph->size(); unsigned int currentScan = 1; for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) { if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush; else cout << "("<<currentScan << "/" << numScans << ") " << flush; if (simpleUpdate) tree->insertPointCloudRays((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange); else tree->insertPointCloud((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange, false, discretize); if (compression == 2){ tree->toMaxLikelihood(); tree->prune(); } if (detailedLog) logfile << currentScan << " " << tree->memoryUsage() << " " << tree->memoryFullGrid() << "\n"; if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no)) break; currentScan++; } gettimeofday(&stop, NULL); // stop timer double time_to_insert = (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec); // get rid of graph in mem before doing anything fancy with tree (=> memory) delete graph; if (logfile.is_open()) logfile.close(); cout << "\nDone building tree.\n\n"; cout << "time to insert scans: " << time_to_insert << " sec" << endl; cout << "time to insert 100.000 points took: " << time_to_insert/ ((double) num_points_in_graph / 100000) << " sec (avg)" << endl << endl; std::cout << "Pruned tree (lossless compression)\n" << "===========================\n"; outputStatistics(tree); tree->write(treeFilenameOT); std::cout << "Pruned max-likelihood tree (lossy compression)\n" << "===========================\n"; tree->toMaxLikelihood(); tree->prune(); outputStatistics(tree); cout << "\nWriting tree files\n===========================\n"; tree->write(treeFilenameMLOT); std::cout << "Full Octree (pruned) written to "<< treeFilenameOT << std::endl; std::cout << "Full Octree (max.likelihood, pruned) written to "<< treeFilenameMLOT << std::endl; tree->writeBinary(treeFilename); std::cout << "Bonsai tree written to "<< treeFilename << std::endl; cout << endl; delete tree; exit(0); }
int main(int argc, char** argv) { //############################################################## OcTree tree (0.05); // point3d origin (10.01, 10.01, 10.02); point3d origin (0.01f, 0.01f, 0.02f); point3d point_on_surface (2.01f, 0.01f, 0.01f); cout << "generating sphere at " << origin << " ..." << endl; for (int i=0; i<360; i++) { for (int j=0; j<360; j++) { if (!tree.insertRay(origin, origin+point_on_surface)) { cout << "ERROR while inserting ray from " << origin << " to " << point_on_surface << endl; } point_on_surface.rotate_IP (0,0,DEG2RAD(1.)); } point_on_surface.rotate_IP (0,DEG2RAD(1.),0); } cout << "done." << endl; cout << "writing to sphere.bt..." << endl; tree.writeBinary("sphere.bt"); // ----------------------------------------------- cout << "casting rays ..." << endl; OcTree sampled_surface (0.05); point3d direction = point3d (1.0,0.0,0.0); point3d obstacle(0,0,0); unsigned int hit (0); unsigned int miss (0); double mean_dist(0); for (int i=0; i<360; i++) { for (int j=0; j<360; j++) { if (!tree.castRay(origin, direction, obstacle, true, 3.)) { miss++; } else { hit++; mean_dist += (obstacle - origin).norm(); sampled_surface.updateNode(obstacle, true); } direction.rotate_IP (0,0,DEG2RAD(1.)); } direction.rotate_IP (0,DEG2RAD(1.),0); } cout << "done." << endl; mean_dist /= (double) hit; std::cout << " hits / misses: " << hit << " / " << miss << std::endl; std::cout << " mean obstacle dist: " << mean_dist << std::endl; cout << "writing sampled_surface.bt" << endl; sampled_surface.writeBinary("sampled_surface.bt"); // ----------------------------------------------- cout << "generating single rays..." << endl; OcTree single_beams(0.03333); int num_beams = 17; float beamLength = 10.0f; point3d single_origin (1.0f, 0.45f, 0.45f); point3d single_endpoint(beamLength, 0.0f, 0.0f); for (int i=0; i<num_beams; i++) { if (!single_beams.insertRay(single_origin, single_origin+single_endpoint)) { cout << "ERROR while inserting ray from " << single_origin << " to " << single_endpoint << endl; } single_endpoint.rotate_IP (0,0,DEG2RAD(360.0/num_beams)); } cout << "done." << endl; cout << "writing to beams.bt..." << endl; single_beams.writeBinary("beams.bt"); return 0; }
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(5.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);*/ point3d origin(0, 0, 0); NBVList.push_back(origin); cout << "Starting Pose: " << tempYaw << " : " << tempPitch << endl; visualization_msgs::Marker mark; mark.header.frame_id = "map"; 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, 0, 0); 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; while (map2d.data.size() == 0 || !mapReceived) { ros::spinOnce(); } //AbstractOcTree OcTree* treeTemp = binaryMsgToMap(mapMsg); //OcTree treeTemp(0.01); treeTemp->writeBinary("firstMap.bt"); cout << "Loaded the Map" << endl; /*try{ gTime = ros::Time::now(); listener.waitForTransform("vicon", "xtion", gTime, ros::Duration(5.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; 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++) { if (tree.isNodeOccupied(*iter)) && (iter.getZ() < 0.03 || iter.getX() < -2 || iter.getX() > 1 || iter.getY() < -1 || iter.getY() > 1)) { 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.3; nav_msgs::MapMetaData mapMeta = map2d.info; double minX = mapMeta.origin.position.x; double minY = mapMeta.origin.position.y; cout << "2D Res: " << mapMeta.resolution; double rangeX = mapMeta.width; double rangeY = mapMeta.height; /*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()); 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; cout << "Relative Sizes: " << map2d.data.size() << " : " << gridMap.size() << endl; cout << "Dimensions: " << mapMeta.width << " by " << mapMeta.height << endl; for (int x = 0; x < rangeX; x++) { for (int y = 0; y < rangeY; y++) { int index = rangeX * y + x; if (map2d.data[index] > 50) { 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; 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 = "map"; 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 = "map"; 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 = "map"; 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; }