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);
}
