Permalink
Browse files

O(n log n) fiducial finder

  • Loading branch information...
1 parent 56f0f6d commit 649ba7683b28585fa5aa159a137c84a6bd82c8c3 Richard Vaughan committed Jun 15, 2010
@@ -23,10 +23,10 @@ foreach( PLUGIN ${PLUGINS} )
endforeach( PLUGIN )
ADD_LIBRARY( fasr2 MODULE fasr2.cc astar/findpath.cpp )
-ADD_LIBRARY( fasr3 MODULE fasr3.cc astar/findpath.cpp )
+ADD_LIBRARY( econ MODULE econ.cc astar/findpath.cpp )
# add fasr2 to the list of plugins
-SET( PLUGINS ${PLUGINS} fasr2 fasr3)
+SET( PLUGINS ${PLUGINS} fasr2 econ )
set_source_files_properties( ${PLUGINS} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" )
@@ -48,5 +48,5 @@ SET_TARGET_PROPERTIES( ${PLUGINS} PROPERTIES PREFIX "" )
# install in <prefix>/lib
-install( TARGETS ${PLUGINS} fasr2 fasr3 DESTINATION lib)
+install( TARGETS ${PLUGINS} fasr2 econ DESTINATION lib)
View
@@ -342,18 +342,20 @@ Model::Model( World* world,
Model::~Model( void )
{
- UnMap(); // remove from the raytrace bitmap
-
// children are removed in ancestor class
// remove myself from my parent's child list, or the world's child
// list if I have no parent
- ModelPtrVec& vec = parent ? parent->children : world->children;
- EraseAll( this, vec );
- modelsbyid.erase(id);
+ if( world ) // if I'm not a worldless dummy model
+ {
+ UnMap(); // remove from the raytrace bitmap
- world->RemoveModel( this );
+ ModelPtrVec& vec = parent ? parent->children : world->children;
+ EraseAll( this, vec );
+ modelsbyid.erase(id);
+ world->RemoveModel( this );
+ }
}
@@ -453,7 +455,7 @@ Block* Model::AddBlockRect( stg_meters_t x,
}
-stg_raytrace_result_t Model::Raytrace( const Pose &pose,
+RaytraceResult Model::Raytrace( const Pose &pose,
const stg_meters_t range,
const stg_ray_test_func_t func,
const void* arg,
@@ -467,7 +469,7 @@ stg_raytrace_result_t Model::Raytrace( const Pose &pose,
ztest );
}
-stg_raytrace_result_t Model::Raytrace( const stg_radians_t bearing,
+RaytraceResult Model::Raytrace( const stg_radians_t bearing,
const stg_meters_t range,
const stg_ray_test_func_t func,
const void* arg,
@@ -491,7 +493,7 @@ void Model::Raytrace( const stg_radians_t bearing,
const stg_radians_t fov,
const stg_ray_test_func_t func,
const void* arg,
- stg_raytrace_result_t* samples,
+ RaytraceResult* samples,
const uint32_t sample_count,
const bool ztest )
{
@@ -180,7 +180,7 @@ void ModelBlobfinder::Update( void )
{
// generate a scan for post-processing into a blob image
- stg_raytrace_result_t* samples = new stg_raytrace_result_t[scan_width];
+ RaytraceResult* samples = new RaytraceResult[scan_width];
Raytrace( pan, range, fov, blob_match, NULL, samples, scan_width, false );
View
@@ -15,11 +15,11 @@ void Model::DrawSelected()
glTranslatef( pose.x, pose.y, pose.z+0.01 ); // tiny Z offset raises rect above grid
- Pose gpose = GetGlobalPose();
+ Pose gp = GetGlobalPose();
char buf[64];
snprintf( buf, 63, "%s [%.2f %.2f %.2f %.2f]",
- token.c_str(), gpose.x, gpose.y, gpose.z, rtod(gpose.a) );
+ token.c_str(), gp.x, gp.y, gp.z, rtod(gp.a) );
PushColor( 0,0,0,1 ); // text color black
Gl::draw_string( 0.5,0.5,0.5, buf );
@@ -44,7 +44,82 @@ void Model::DrawSelected()
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
PopColor();
+
+ // testing
+
+ // highlight all fiducial robots within a certain range
+
+// Gl::pose_inverse_shift( gp );
+
+// double rng = 10.0;
+
+// Model left( gp.x - rng, gp.y, world );
+// Model right( gp.x + rng, gp.y, world );
+// Model down( gp.x, gp.y - rng, world );
+// Model up( gp.x, gp.y + rng, world );
+
+// std::set<Model*>::iterator xmin = world->models_with_fiducials_byx.lower_bound( &left );
+// std::set<Model*>::iterator xmax = world->models_with_fiducials_byx.upper_bound( &right );
+
+// std::set<Model*>::iterator ymin = world->models_with_fiducials_byy.lower_bound( &down );
+// std::set<Model*>::iterator ymax = world->models_with_fiducials_byy.upper_bound( &up );
+
+
+// PushColor( Color(1,0,0,0.5) );
+
+
+// std::vector<Model*> candidates;
+// std::set<Model*> horiz, vert;
+
+// while( xmin != xmax )
+// {
+// //candidates.insert( *xmin );
+// horiz.insert( *xmin);
+
+// Pose op = (*xmin)->GetGlobalPose();
+// glRectf( op.x - 5, op.y - 5, op.x + 5, op.y + 5 );
+
+// xmin++;
+// }
+
+// PopColor();
+
+// PushColor( Color(0,0,1,0.5) );
+
+// while( ymin != ymax )
+// {
+// vert.insert( *ymin );
+
+// // candidates.insert( *ymin );
+
+// Pose op = (*ymin)->GetGlobalPose();
+// glRectf( op.x - 5, op.y - 5, op.x + 5, op.y + 5 );
+// ymin++;
+// }
+
+// PopColor();
+
+// PushColor( Color(0,1,0,0.5) );
+
+// std::set_intersection( horiz.begin(), horiz.end(),
+// vert.begin(), vert.end(),
+// std::inserter( candidates, candidates.end() ) );
+
+// //printf( "cand sz %lu\n", candidates.size() );
+
+// glTranslatef( 0,0,1.0 );
+
+// FOR_EACH( it, candidates )
+// {
+// Pose op = (*it)->GetGlobalPose();
+// glRectf( op.x - 5, op.y - 5, op.x + 5, op.y + 5 );
+// }
+
+// PopColor();
+
+
glPopMatrix();
+
}
View
@@ -176,7 +176,7 @@ void ModelFiducial::AddModelIfVisible( Model* him )
//printf( "range %.2f\n", range );
- stg_raytrace_result_t ray( Raytrace( dtheta,
+ RaytraceResult ray( Raytrace( dtheta,
max_range_anon, // TODOscan only as far as the object
fiducial_raytrace_match,
NULL,
@@ -209,6 +209,8 @@ void ModelFiducial::AddModelIfVisible( Model* him )
fid.geom.y = hisgeom.size.y;
fid.geom.a = normalize( hispose.a - mypose.a);
+ //fid.pose_rel = hispose - this->GetGlobalPose();
+
// store the global pose of the fiducial (mainly for the GUI)
fid.pose = hispose;
@@ -222,6 +224,7 @@ void ModelFiducial::AddModelIfVisible( Model* him )
fiducials.push_back( fid );
}
+
///////////////////////////////////////////////////////////////////////////
// Update the beacon data
//
@@ -231,12 +234,64 @@ void ModelFiducial::Update( void )
if( subs < 1 )
return;
-
+
// reset the array of detected fiducials
fiducials.clear();
+
+#if( 1 )
+ // BEGIN EXPERIMENT
+
+ // find two sets of fiducial-bearing models, within sensor range on
+ // the two different axes
+
+ double rng = max_range_anon;
+ Pose gp = GetGlobalPose();
+ Model edge; // dummy model used to find bounds in the sets
+
+ edge.pose = Pose( gp.x-rng, gp.y, 0, 0 ); // LEFT
+ std::set<Model*,World::ltx>::iterator xmin =
+ world->models_with_fiducials_byx.lower_bound( &edge );
+
+ edge.pose = Pose( gp.x+rng, gp.y, 0, 0 ); // RIGHT
+ const std::set<Model*,World::ltx>::iterator xmax =
+ world->models_with_fiducials_byx.upper_bound( &edge );
+
+ edge.pose = Pose( gp.x, gp.y-rng, 0, 0 ); // BOTTOM
+ std::set<Model*,World::lty>::iterator ymin =
+ world->models_with_fiducials_byy.lower_bound( &edge );
+
+ edge.pose = Pose( gp.x, gp.y+rng, 0, 0 ); // TOP
+ const std::set<Model*,World::lty>::iterator ymax =
+ world->models_with_fiducials_byy.upper_bound( &edge );
+
+ // put these models into sets keyed on model pointer, rather than position
+ std::set<Model*> horiz, vert;
+
+ for( ; xmin != xmax; xmin++)
+ horiz.insert( *xmin);
+
+ for( ; ymin != ymax; ymin++ )
+ vert.insert( *ymin );
+
+ // the intersection of the sets is all the fiducials closeby
+ std::vector<Model*> candidates;
+ std::set_intersection( horiz.begin(), horiz.end(),
+ vert.begin(), vert.end(),
+ std::inserter( candidates, candidates.end() ) );
- FOR_EACH( it, world->models_with_fiducials )
- AddModelIfVisible( *it );
+ // printf( "cand sz %lu\n", candidates.size() );
+
+ // create sets sorted by x and y position
+ FOR_EACH( it, candidates ) //world->models_with_fiducials )
+ AddModelIfVisible( *it );
+#else
+ // create sets sorted by x and y position
+ FOR_EACH( it, world->models_with_fiducials )
+ AddModelIfVisible( *it );
+
+#endif
+
+ // find the range of fiducials within range in X
Model::Update();
}
View
@@ -211,11 +211,11 @@ void Model::SetPose( const Pose& newpose )
// if( isnan( pose.a ) )
// printf( "SetPose bad angle %s [%.2f %.2f %.2f %.2f]\n",
// token, pose.x, pose.y, pose.z, pose.a );
-
+
NeedRedraw();
MapWithChildren();
world->dirty = true;
}
-
- CallCallbacks( CB_POSE );
+
+ CallCallbacks( CB_POSE );
}
@@ -359,7 +359,7 @@ void ModelGripper::UpdateBreakBeams()
double bbr =
(1.0 - cfg.paddle_position) * (geom.size.y - (geom.size.y * cfg.paddle_size.y * 2.0 ));
- stg_raytrace_result_t sample =
+ RaytraceResult sample =
Raytrace( pz, // ray origin
bbr, // range
gripper_raytrace_match, // match func
@@ -410,7 +410,7 @@ void ModelGripper::UpdateContacts()
// paddle beam max range
double bbr = cfg.paddle_size.x * geom.size.x;
- stg_raytrace_result_t leftsample =
+ RaytraceResult leftsample =
Raytrace( lpz, // ray origin
bbr, // range
gripper_raytrace_match, // match func
@@ -419,7 +419,7 @@ void ModelGripper::UpdateContacts()
cfg.contact[0] = leftsample.mod;
- stg_raytrace_result_t rightsample =
+ RaytraceResult rightsample =
Raytrace( rpz, // ray origin
bbr, // range
gripper_raytrace_match, // match func
View
@@ -186,18 +186,20 @@ void ModelLaser::Update( void )
// trace the ray, incrementing its heading for each sample
for( unsigned int t(0); t<sample_count; t += resolution )
{
- stg_raytrace_result_t r( world->Raytrace( ray ) );
+ RaytraceResult r( world->Raytrace( ray ) );
samples[t].range = r.range;
// if we hit a model and it reflects brightly, we set
// reflectance high, else low
if( r.mod && ( r.mod->vis.laser_return >= LaserBright ) )
- samples[t].reflectance = 1;
+ samples[t].reflectance = 1.0;
else
- samples[t].reflectance = 0;
+ samples[t].reflectance = 0.0;
// point the ray to the next angle
ray.origin.a += sample_incr;
+
+
}
// we may need to interpolate the samples we skipped
@@ -259,6 +261,11 @@ const std::vector<ModelLaser::Sample>& ModelLaser::GetSamples() const
return samples;
}
+std::vector<ModelLaser::Sample>& ModelLaser::GetSamples()
+{
+ return samples;
+}
+
// VIS -------------------------------------------------------------------
View
@@ -265,7 +265,7 @@ void ModelRanger::Update( void )
//for( int r=0; r<sensors[t].ray_count; r++ )
//{
- stg_raytrace_result_t ray = Raytrace( s.pose,
+ RaytraceResult ray = Raytrace( s.pose,
s.bounds_range.max,
ranger_match,
NULL );
View
@@ -49,6 +49,14 @@ bool Stg::InitDone()
return init_called;
}
+const Color Color::blue( 0,0,1 );
+const Color Color::red( 1,0,0 );
+const Color Color::green( 0,1,0 );
+const Color Color::yellow( 1,1,0 );
+const Color Color::magenta( 1,0,1 );
+const Color Color::cyan( 0,1,1 );
+
+
static inline uint8_t* pb_get_pixel( Fl_Shared_Image* img, int x, int y )
{
uint8_t* pixels = (uint8_t*)(img->data()[0]);
Oops, something went wrong.

0 comments on commit 649ba76

Please sign in to comment.