/*! @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;
}
