Ejemplo n.º 1
0
    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);
    }
Ejemplo n.º 2
0
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));
}
Ejemplo n.º 3
0
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;
}
Ejemplo n.º 4
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);
    }
Ejemplo n.º 5
0
    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;
     }
Ejemplo n.º 6
0
// ------------------------------------------------------------- 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);
    }
}
Ejemplo n.º 8
0
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);
}