/*! @brief Loads the way points from WayPoints.cfg into m_way_points */ void WalkOptimisationProvider::loadWayPoints() { ifstream points_file((CONFIG_DIR + string("Motion/Optimisation/WayPoints.cfg")).c_str()); if (points_file.is_open()) { points_file >> m_way_points; #if DEBUG_BEHAVIOUR_VERBOSITY > 0 debug << "WalkOptimisationProvider::m_way_points " << m_way_points << endl; #endif points_file.close(); }
void OnlineMapsPlanner::initialize(std::string points_file_name) { std::string line; std::ifstream points_file(points_file_name.c_str()); if (points_file.is_open()) { getline(points_file, line); waypoints.poses.resize(extractLineCount(line)); for (unsigned int i = 0; i < waypoints.poses.size(); i++) { double lat, lng; getline(points_file, line); extractLatLng(line, lat, lng); waypoints.poses.at(i).pose.position.x = lat; // Meters waypoints.poses.at(i).pose.position.y = lng; } } waypoints.header.seq = 0; }
int main(int argc, char* argv[]) { std::cerr.precision(17); std::ifstream points_file(argv[2]); std::vector<Point> points; std::copy(std::istream_iterator<Point>(points_file), std::istream_iterator<Point>(), std::back_inserter(points)); int gridsize = 10; if(argc>3){ gridsize = boost::lexical_cast<int>(argv[3]); } std::cerr << "gridsize = " << gridsize << std::endl; int nb_points=points.size(); std::vector<bool> ray_res(nb_points); std::vector<bool> grid_res(nb_points); //using ray { Polyhedron polyhedron; std::ifstream polyhedron_file(argv[1]); polyhedron_file >> polyhedron; std::cerr << "|V| = " << polyhedron.size_of_vertices() << std::endl; CGAL::Timer timer; timer.start(); CGAL::Point_inside_polyhedron_3<Polyhedron,K> inside_with_ray(polyhedron,0); timer.stop(); std::cerr <<"Using ray"<< std::endl; std::cerr << " Preprocessing took " << timer.time() << " sec." << std::endl; timer.reset(); int n_inside = 0; timer.start(); for(int k=0;k<nb_points;++k){ ray_res[k]=inside_with_ray(points[k]); if(ray_res[k]){ ++n_inside; } } timer.stop(); std::cerr << " " << n_inside << " points inside " << std::endl; std::cerr << " " << points.size() - n_inside << " points outside " << std::endl; std::cerr << " Queries took " << timer.time() << " sec." << std::endl; } //using grid { Polyhedron polyhedron; std::ifstream polyhedron_file(argv[1]); polyhedron_file >> polyhedron; std::cerr << "|V| = " << polyhedron.size_of_vertices() << std::endl; CGAL::Timer timer; timer.start(); CGAL::Point_inside_polyhedron_3<Polyhedron,K> inside_with_grid(polyhedron, gridsize); timer.stop(); std::cerr <<"Using grid"<< std::endl; std::cerr << " Preprocessing took " << timer.time() << " sec." << std::endl; timer.reset(); if(argc>5){ random_points(argv[4],inside_with_grid.bbox() ,boost::lexical_cast<int>(argv[5]) ); } int n_inside = 0; timer.start(); for(int k=0;k<nb_points;++k){ grid_res[k]=inside_with_grid(points[k]); if(grid_res[k]){ ++n_inside; } } timer.stop(); std::cerr << " " << n_inside << " points inside " << std::endl; std::cerr << " " << points.size() - n_inside << " points outside " << std::endl; std::cerr << " Queries took " << timer.time() << " sec." << std::endl; } for(int k=0;k<nb_points;++k){ if(ray_res[k]!=grid_res[k]){ std::cerr << "WARNING: Result is different for point " << k << std::endl; } } //using original code { Polyhedron polyhedron; std::ifstream polyhedron_file(argv[1]); polyhedron_file >> polyhedron; std::cerr << "|V| = " << polyhedron.size_of_vertices() << std::endl; std::cerr <<"Using ray (original code)"<< std::endl; CGAL::Point_inside_polyhedron_3<Polyhedron,K> inside_with_ray(polyhedron,0); CGAL::Timer timer; int n_inside = 0; timer.start(); for(int k=0;k<nb_points;++k) if(inside_with_ray(points[k],true)) ++n_inside; timer.stop(); std::cerr << " " << n_inside << " points inside " << std::endl; std::cerr << " " << points.size() - n_inside << " points outside " << std::endl; std::cerr << " Queries took " << timer.time() << " sec." << std::endl; } return 0; }