inline void dag_sp_dispatch1 (const VertexListGraph& g, typename graph_traits<VertexListGraph>::vertex_descriptor s, DistanceMap distance, WeightMap weight, ColorMap color, IndexMap id, DijkstraVisitor vis, const Params& params) { typedef typename property_traits<WeightMap>::value_type T; typename std::vector<T>::size_type n; n = is_default_param(distance) ? num_vertices(g) : 1; std::vector<T> distance_map(n); n = is_default_param(color) ? num_vertices(g) : 1; std::vector<default_color_type> color_map(n); dag_sp_dispatch2 (g, s, choose_param(distance, make_iterator_property_map(distance_map.begin(), id, distance_map[0])), weight, choose_param(color, make_iterator_property_map(color_map.begin(), id, color_map[0])), id, vis, params); }
void PolyhedronShortestPath(Polyhedron& P, vertex_descriptor &a, vertex_descriptor &b, HalfedgeVector &hv) { // create indices, predecessor and distance map VertexIdPMap vertex_index_pmap = get(boost::vertex_index, P); std::vector<vertex_descriptor> predecessor(boost::num_vertices(P)); PredecessorPMap predecessor_pmap = PredecessorPMap(predecessor.begin(), vertex_index_pmap); std::vector<double> distance(boost::num_vertices(P)); DistancePMap distance_pmap = DistancePMap(distance.begin(), vertex_index_pmap); // boost: set weights as edge length (default weight is squared length between vertices) std::map<edge_descriptor, double> edge2weight; WeightAPMap weight_apmap = WeightAPMap(edge2weight); edge_iterator ei, ei_end; for(boost::tie(ei,ei_end)=boost::edges(P); ei!=ei_end; ++ei) edge2weight.insert(make_pair(*ei,(*ei).opposite().halfedge()->weight())); // opposite: we compute from b to a // run dijkstra boost::detail::dijkstra_dispatch2(P, b, distance_pmap, weight_apmap, vertex_index_pmap, distance_map(distance_pmap).predecessor_map(predecessor_pmap)); // extract edges vertex_descriptor v = a; for(vertex_descriptor u = predecessor_pmap[v]; u!=v; v=u, u=predecessor_pmap[v]) hv.push_back(GetHalfedge(v,u)); }
int main(int argc,char* argv[]) { const char* filename = (argc > 1) ? argv[1] : "data/points.xy"; std::ifstream input(filename); Triangulation t; Filter is_finite(t); Finite_triangulation ft(t, is_finite, is_finite); Point p ; while(input >> p){ t.insert(p); } vertex_iterator vit, ve; // Associate indices to the vertices int index = 0; // boost::tie assigns the first and second element of the std::pair // returned by boost::vertices to the variables vit and ve for(boost::tie(vit,ve)=boost::vertices(ft); vit!=ve; ++vit ){ vertex_descriptor vd = *vit; vertex_id_map[vd]= index++; } // Dijkstra's shortest path needs property maps for the predecessor and distance // We first declare a vector std::vector<vertex_descriptor> predecessor(boost::num_vertices(ft)); // and then turn it into a property map boost::iterator_property_map<std::vector<vertex_descriptor>::iterator, VertexIdPropertyMap> predecessor_pmap(predecessor.begin(), vertex_index_pmap); std::vector<double> distance(boost::num_vertices(ft)); boost::iterator_property_map<std::vector<double>::iterator, VertexIdPropertyMap> distance_pmap(distance.begin(), vertex_index_pmap); // start at an arbitrary vertex vertex_descriptor source = *boost::vertices(ft).first; std::cout << "\nStart dijkstra_shortest_paths at " << source->point() <<"\n"; boost::dijkstra_shortest_paths(ft, source, distance_map(distance_pmap) .predecessor_map(predecessor_pmap) .vertex_index_map(vertex_index_pmap)); for(boost::tie(vit,ve)=boost::vertices(ft); vit!=ve; ++vit ){ vertex_descriptor vd = *vit; std::cout << vd->point() << " [" << vertex_id_map[vd] << "] "; std::cout << " has distance = " << boost::get(distance_pmap,vd) << " and predecessor "; vd = boost::get(predecessor_pmap,vd); std::cout << vd->point() << " [" << vertex_id_map[vd] << "]\n "; } return 0; }
inline void dijkstra_dispatch1 (const VertexListGraph& g, typename graph_traits<VertexListGraph>::vertex_descriptor s, DistanceMap distance, WeightMap weight, IndexMap index_map, const Params& params) { // Default for distance map typedef typename property_traits<WeightMap>::value_type D; typename std::vector<D>::size_type n = is_default_param(distance) ? num_vertices(g) : 1; std::vector<D> distance_map(n); detail::dijkstra_dispatch2 (g, s, choose_param(distance, make_iterator_property_map (distance_map.begin(), index_map, distance_map[0])), weight, index_map, params); }
result_distances dijkstra_shortest_paths(v_index s) { v_index N = num_verts(); result_distances to_return; std::vector<double> distances(N, (std::numeric_limits<double>::max)()); std::vector<vertex_descriptor> predecessors(N); try { boost::dijkstra_shortest_paths(*graph, (*vertices)[s], distance_map(boost::make_iterator_property_map(distances.begin(), index)) .predecessor_map(boost::make_iterator_property_map(predecessors.begin(), index))); } catch (boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::negative_edge> > e) { return to_return; } to_return.distances = distances; for (int i = 0; i < N; i++) { to_return.predecessors.push_back(index[predecessors[i]]); } return to_return; }
// ------------------------------------------------------------- load_glyph --- texture_glyph_t * load_glyph( const char * filename, const wchar_t charcode, const float highres_size, const float lowres_size, const float padding ) { size_t i, j; FT_Library library; FT_Face face; FT_Init_FreeType( &library ); FT_New_Face( library, filename, 0, &face ); FT_Select_Charmap( face, FT_ENCODING_UNICODE ); FT_UInt glyph_index = FT_Get_Char_Index( face, charcode ); // Render glyph at high resolution (highres_size points) FT_Set_Char_Size( face, highres_size*64, 0, 72, 72 ); FT_Load_Glyph( face, glyph_index, FT_LOAD_RENDER | FT_LOAD_NO_HINTING | FT_LOAD_NO_AUTOHINT); FT_GlyphSlot slot = face->glyph; FT_Bitmap bitmap = slot->bitmap; // Allocate high resolution buffer size_t highres_width = bitmap.width + 2*padding*highres_size; size_t highres_height = bitmap.rows + 2*padding*highres_size; double * highres_data = (double *) malloc( highres_width*highres_height*sizeof(double) ); memset( highres_data, 0, highres_width*highres_height*sizeof(double) ); // Copy high resolution bitmap with padding and normalize values for( j=0; j < bitmap.rows; ++j ) { for( i=0; i < bitmap.width; ++i ) { int x = i + padding; int y = j + padding; highres_data[y*highres_width+x] = bitmap.buffer[j*bitmap.width+i]/255.0; } } // Compute distance map distance_map( highres_data, highres_width, highres_height ); // Allocate low resolution buffer size_t lowres_width = round(highres_width * lowres_size/highres_size); size_t lowres_height = round(highres_height * lowres_width/(float) highres_width); double * lowres_data = (double *) malloc( lowres_width*lowres_height*sizeof(double) ); memset( lowres_data, 0, lowres_width*lowres_height*sizeof(double) ); // Scale down highres buffer into lowres buffer resize( highres_data, highres_width, highres_height, lowres_data, lowres_width, lowres_height ); // Convert the (double *) lowres buffer into a (unsigned char *) buffer and // rescale values between 0 and 255. unsigned char * data = (unsigned char *) malloc( lowres_width*lowres_height*sizeof(unsigned char) ); for( j=0; j < lowres_height; ++j ) { for( i=0; i < lowres_width; ++i ) { double v = lowres_data[j*lowres_width+i]; data[j*lowres_width+i] = (int) (255*(1-v)); } } // Compute new glyph information from highres value float ratio = lowres_size / highres_size; size_t pitch = lowres_width * sizeof( unsigned char ); // Create glyph texture_glyph_t * glyph = texture_glyph_new( ); glyph->offset_x = (slot->bitmap_left + padding*highres_width) * ratio; glyph->offset_y = (slot->bitmap_top + padding*highres_height) * ratio; glyph->width = lowres_width; glyph->height = lowres_height; glyph->charcode = charcode; /* printf( "Glyph width: %ld\n", glyph->width ); printf( "Glyph height: %ld\n", glyph->height ); printf( "Glyph offset x: %d\n", glyph->offset_x ); printf( "Glyph offset y: %d\n", glyph->offset_y ); */ ivec4 region = texture_atlas_get_region( atlas, glyph->width, glyph->height ); /* printf( "Region x : %d\n", region.x ); printf( "Region y : %d\n", region.y ); printf( "Region width : %d\n", region.width ); printf( "Region height : %d\n", region.height ); */ texture_atlas_set_region( atlas, region.x, region.y, glyph->width, glyph->height, data, pitch ); glyph->s0 = region.x/(float)atlas->width; glyph->t0 = region.y/(float)atlas->height; glyph->s1 = (region.x + glyph->width)/(float)atlas->width; glyph->t1 = (region.y + glyph->height)/(float)atlas->height; FT_Load_Glyph( face, glyph_index, FT_LOAD_RENDER | FT_LOAD_NO_HINTING | FT_LOAD_NO_AUTOHINT); glyph->advance_x = ratio * face->glyph->advance.x/64.0; glyph->advance_y = ratio * face->glyph->advance.y/64.0; /* printf( "Advance x : %f\n", glyph->advance_x ); printf( "Advance y : %f\n", glyph->advance_y ); */ free( highres_data ); free( lowres_data ); free( data ); return glyph; }
void LocalizationPlugin::process(const ed::WorldModel& world, ed::UpdateRequest& req) { last_laser_msg_.reset(); cb_queue_.callAvailable(); if (!last_laser_msg_) return; tue::Timer timer; timer.start(); // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - // - Update sensor model // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - if (lrf_.getNumBeams() != last_laser_msg_->ranges.size()) { lrf_.setNumBeams(last_laser_msg_->ranges.size()); lrf_.setRangeLimits(last_laser_msg_->range_min, last_laser_msg_->range_max); lrf_.setAngleLimits(last_laser_msg_->angle_min, last_laser_msg_->angle_max); } std::vector<double> sensor_ranges(last_laser_msg_->ranges.size()); for (unsigned int i = 0; i < last_laser_msg_->ranges.size(); ++i) sensor_ranges[i] = last_laser_msg_->ranges[i]; // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - // - Create world model cross section // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - std::vector<ed::EntityConstPtr> entities; for(ed::WorldModel::const_iterator it = world.begin(); it != world.end(); ++it) { // if (it->second->id() == "pico_case") if (it->second->shape()) entities.push_back(it->second); } geo::Pose3D laser_pose; laser_pose.t = laser_pose_.t + geo::Vector3(0, 0, 0); laser_pose.R.setRPY(0, 0, 0); double grid_resolution = 0.025; int grid_size = 800; std::queue<CellData> Q; LineRenderResult render_result(Q, grid_size, grid_resolution); for(std::vector<ed::EntityConstPtr>::const_iterator it = entities.begin(); it != entities.end(); ++it) { const ed::EntityConstPtr& e = *it; geo::LaserRangeFinder::RenderOptions options; geo::Transform t_inv = laser_pose.inverse() * e->pose(); options.setMesh(e->shape()->getMesh(), t_inv); lrf_.render(options, render_result); } // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - // - Create distance map // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - double max_dist = 0.3; cv::Mat distance_map(grid_size, grid_size, CV_32FC1, (max_dist / grid_resolution) * (max_dist / grid_resolution)); for(int i = 0; i < grid_size; ++i) { distance_map.at<float>(i, 0) = 0; distance_map.at<float>(i, grid_size - 1) = 0; distance_map.at<float>(0, i) = 0; distance_map.at<float>(grid_size - 1, i) = 0; } std::vector<KernelCell> kernel; kernel.push_back(KernelCell(-distance_map.cols, 0, -1)); kernel.push_back(KernelCell(distance_map.cols, 0, 1)); kernel.push_back(KernelCell(-1, -1, 0)); kernel.push_back(KernelCell( 1, 1, 0)); while(!Q.empty()) { const CellData& c = Q.front(); double current_distance = distance_map.at<float>(c.index); if (c.distance < current_distance) { distance_map.at<float>(c.index) = c.distance; for(unsigned int i = 0; i < kernel.size(); ++i) { const KernelCell& kc = kernel[i]; int new_index = c.index + kc.d_index; int dx_new = c.dx + kc.dx; int dy_new = c.dy + kc.dy; int new_distance = (dx_new * dx_new) + (dy_new * dy_new); Q.push(CellData(new_index, dx_new, dy_new, new_distance)); } } Q.pop(); } // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - // - Create samples // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - std::vector<geo::Pose3D> poses; if (!pose_initialized_) { // Uniform sampling over world for(double x = render_result.p_min.x; x < render_result.p_max.x; x += 0.2) { for(double y = render_result.p_min.y; y < render_result.p_max.y; y += 0.2) { for(double a = 0; a < 6.28; a += 0.1) { geo::Pose3D laser_pose; laser_pose.t = geo::Vector3(x, y, 0); laser_pose.R.setRPY(0, 0, a); poses.push_back(laser_pose); } } } } else { poses.push_back(best_laser_pose_); for(double dx = -0.3; dx < 0.3; dx += 0.1) { for(double dy = -0.3; dy < 0.3; dy += 0.1) { for(double da = -1; da < 1; da += 0.1) { geo::Pose3D dT; dT.t = geo::Vector3(dx, dy, 0); dT.R.setRPY(0, 0, da); poses.push_back(best_laser_pose_ * dT); } } } } // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - // - Test samples and find sample with lowest error // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - std::vector<geo::Vector3> sensor_points; lrf_.rangesToPoints(sensor_ranges, sensor_points); int i_step = 1; if (poses.size() > 10000) i_step = sensor_points.size() / 100; // limit to 100 beams std::cout << "i_step = " << i_step << std::endl; double min_sum_sq_error = 1e10; for(std::vector<geo::Pose3D>::const_iterator it = poses.begin(); it != poses.end(); ++it) { const geo::Pose3D& laser_pose = *it; double z1 = laser_pose.R.xx / grid_resolution; double z2 = laser_pose.R.xy / grid_resolution; double z3 = laser_pose.R.yx / grid_resolution; double z4 = laser_pose.R.yy / grid_resolution; double t1 = laser_pose.t.x / grid_resolution - distance_map.cols / 2; double t2 = laser_pose.t.y / grid_resolution - distance_map.cols / 2; double sum_sq_error = 0; for(unsigned int i = 0; i < sensor_points.size(); i += i_step) { const geo::Vector3& v = sensor_points[i]; double py = z3 * v.x + z4 * v.y + t2; int mx = -py; if (mx > 0 && mx < grid_size) { double px = z1 * v.x + z2 * v.y + t1; int my = -px; if (my > 0 && my < grid_size) { sum_sq_error += distance_map.at<float>(my, mx); } } } if (sum_sq_error < min_sum_sq_error) { min_sum_sq_error = sum_sq_error; best_laser_pose_ = laser_pose; pose_initialized_ = true; } } std::cout << min_sum_sq_error << std::endl; std::cout << best_laser_pose_ << std::endl; std::cout << timer.getElapsedTimeInMilliSec() << " ms" << std::endl; // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - // - Visualization // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - bool visualize = true; if (visualize) { cv::Mat rgb_image(distance_map.rows, distance_map.cols, CV_8UC3, cv::Scalar(0, 0, 0)); for(int y = 0; y < rgb_image.rows; ++y) { for(int x = 0; x < rgb_image.cols; ++x) { int c = distance_map.at<float>(y, x) / ((max_dist / grid_resolution) * (max_dist / grid_resolution)) * 255; rgb_image.at<cv::Vec3b>(y, x) = cv::Vec3b(c, c, c); } } for(unsigned int i = 0; i < sensor_points.size(); ++i) { const geo::Vector3& p = best_laser_pose_ * sensor_points[i]; int mx = -p.y / grid_resolution + grid_size / 2; int my = -p.x / grid_resolution + grid_size / 2; if (mx >= 0 && my >= 0 && mx < grid_size && my <grid_size) { rgb_image.at<cv::Vec3b>(my, mx) = cv::Vec3b(0, 255, 0); } } // Visualize sensor int lmx = -best_laser_pose_.t.y / grid_resolution + grid_size / 2; int lmy = -best_laser_pose_.t.x / grid_resolution + grid_size / 2; cv::circle(rgb_image, cv::Point(lmx,lmy), 0.3 / grid_resolution, cv::Scalar(0, 0, 255), 1); geo::Vector3 d = best_laser_pose_.R * geo::Vector3(0.3, 0, 0); int dmx = -d.y / grid_resolution; int dmy = -d.x / grid_resolution; cv::line(rgb_image, cv::Point(lmx, lmy), cv::Point(lmx + dmx, lmy + dmy), cv::Scalar(0, 0, 255), 1); cv::imshow("distance_map", rgb_image); cv::waitKey(1); } }
static depth findMaxWidth(const NGHolder &h, const SpecialEdgeFilter &filter, NFAVertex src) { if (isLeafNode(src, h.g)) { return depth::unreachable(); } if (hasReachableCycle(h, src)) { // There's a cycle reachable from this src, so we have inf width. return depth::infinity(); } boost::filtered_graph<NFAGraph, SpecialEdgeFilter> g(h.g, filter); assert(hasCorrectlyNumberedVertices(h)); const size_t num = num_vertices(h); vector<int> distance(num); vector<boost::default_color_type> colors(num); auto index_map = get(&NFAGraphVertexProps::index, g); // DAG shortest paths with negative edge weights. dag_shortest_paths( g, src, distance_map(make_iterator_property_map(distance.begin(), index_map)) .weight_map(boost::make_constant_property<NFAEdge>(-1)) .vertex_index_map(index_map) .color_map(make_iterator_property_map(colors.begin(), index_map))); depth acceptDepth, acceptEodDepth; if (colors.at(NODE_ACCEPT) == boost::white_color) { acceptDepth = depth::unreachable(); } else { acceptDepth = -1 * distance.at(NODE_ACCEPT); } if (colors.at(NODE_ACCEPT_EOD) == boost::white_color) { acceptEodDepth = depth::unreachable(); } else { acceptEodDepth = -1 * distance.at(NODE_ACCEPT_EOD); } depth d; if (acceptDepth.is_unreachable()) { d = acceptEodDepth; } else if (acceptEodDepth.is_unreachable()) { d = acceptDepth; } else { d = max(acceptDepth, acceptEodDepth); } if (d.is_unreachable()) { // If we're actually reachable, we'll have a min width, so we can // return infinity in this case. if (findMinWidth(h, filter, src).is_reachable()) { return depth::infinity(); } return d; } // Invert sign and subtract one for start transition. assert(d.is_finite() && d > depth(0)); return d - depth(1); }