00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00013
00014 #undef DEBUG
00015
00016 #include "stage.hh"
00017 #include "option.hh"
00018 #include "worldfile.hh"
00019 using namespace Stg;
00020
00021
00022 Option ModelFiducial::showData( "Fiducials", "show_fiducial", "", true, NULL );
00023 Option ModelFiducial::showFov( "Fiducial FOV", "show_fiducial_fov", "", false, NULL );
00024
00067 ModelFiducial::ModelFiducial( World* world,
00068 Model* parent,
00069 const std::string& type ) :
00070 Model( world, parent, type ),
00071 fiducials(),
00072 max_range_anon( 8.0 ),
00073 max_range_id( 5.0 ),
00074 min_range( 0.0 ),
00075 fov( M_PI ),
00076 heading( 0 ),
00077 key( 0 ),
00078 ignore_zloc(false)
00079 {
00080
00081
00082
00083
00084 thread_safe = true;
00085
00086
00087
00088
00089 this->ClearBlocks();
00090
00091 Geom geom;
00092 geom.Zero();
00093 SetGeom( geom );
00094
00095 RegisterOption( &showData );
00096 RegisterOption( &showFov );
00097 }
00098
00099 ModelFiducial::~ModelFiducial( void )
00100 {
00101 }
00102
00103 static bool fiducial_raytrace_match( Model* candidate,
00104 Model* finder,
00105 const void* dummy )
00106 {
00107 (void)dummy;
00108 return( ! finder->IsRelated( candidate ) );
00109 }
00110
00111
00112 void ModelFiducial::AddModelIfVisible( Model* him )
00113 {
00114
00115
00116
00117 assert( him->vis.fiducial_return != 0 );
00118
00119
00120
00121 if( vis.fiducial_key != him->vis.fiducial_key )
00122 {
00123
00124 return;
00125 }
00126
00127 Pose mypose = this->GetGlobalPose();
00128
00129
00130 Pose hispose = him->GetGlobalPose();
00131 double dx = hispose.x - mypose.x;
00132 double dy = hispose.y - mypose.y;
00133 double range = hypot( dy, dx );
00134
00135
00136
00137 if( range >= max_range_anon )
00138 {
00139
00140
00141
00142
00143 return;
00144 }
00145
00146
00147 double bearing = atan2( dy, dx );
00148 double dtheta = normalize(bearing - mypose.a);
00149
00150 if( fabs(dtheta) > fov/2.0 )
00151 {
00152
00153 return;
00154 }
00155
00156 if( IsRelated( him ) )
00157 return;
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179 RaytraceResult ray( Raytrace( dtheta,
00180 max_range_anon,
00181 fiducial_raytrace_match,
00182 NULL,
00183 true ) );
00184
00185
00186 if( ignore_zloc && ray.mod == NULL )
00187 ray.mod = him;
00188
00189
00190
00191
00192
00193
00194 if( ray.mod != him )
00195 return;
00196
00197 assert( range >= 0 );
00198
00199
00200
00201 Geom hisgeom( him->GetGeom() );
00202
00203
00204 Fiducial fid;
00205 fid.mod = him;
00206 fid.range = range;
00207 fid.bearing = dtheta;
00208 fid.geom.x = hisgeom.size.x;
00209 fid.geom.y = hisgeom.size.y;
00210 fid.geom.z = hisgeom.size.z;
00211 fid.geom.a = normalize( hispose.a - mypose.a);
00212
00213
00214
00215
00216 fid.pose = hispose;
00217
00218
00219
00220 fid.id = range < max_range_id ? him->vis.fiducial_return : 0;
00221
00222
00223
00224
00225 fiducials.push_back( fid );
00226 }
00227
00228
00230
00231
00232 void ModelFiducial::Update( void )
00233 {
00234
00235
00236 if( subs < 1 )
00237 return;
00238
00239
00240 fiducials.clear();
00241
00242 #if( 1 )
00243
00244
00245
00246
00247
00248 double rng = max_range_anon;
00249 Pose gp = GetGlobalPose();
00250 Model edge;
00251
00252 edge.pose = Pose( gp.x-rng, gp.y, 0, 0 );
00253 std::set<Model*,World::ltx>::iterator xmin =
00254 world->models_with_fiducials_byx.lower_bound( &edge );
00255
00256 edge.pose = Pose( gp.x+rng, gp.y, 0, 0 );
00257 const std::set<Model*,World::ltx>::iterator xmax =
00258 world->models_with_fiducials_byx.upper_bound( &edge );
00259
00260 edge.pose = Pose( gp.x, gp.y-rng, 0, 0 );
00261 std::set<Model*,World::lty>::iterator ymin =
00262 world->models_with_fiducials_byy.lower_bound( &edge );
00263
00264 edge.pose = Pose( gp.x, gp.y+rng, 0, 0 );
00265 const std::set<Model*,World::lty>::iterator ymax =
00266 world->models_with_fiducials_byy.upper_bound( &edge );
00267
00268
00269 std::set<Model*> horiz, vert;
00270
00271 for( ; xmin != xmax; ++xmin)
00272 horiz.insert( *xmin);
00273
00274 for( ; ymin != ymax; ++ymin )
00275 vert.insert( *ymin );
00276
00277
00278 std::vector<Model*> nearby;
00279 std::set_intersection( horiz.begin(), horiz.end(),
00280 vert.begin(), vert.end(),
00281 std::inserter( nearby, nearby.end() ) );
00282
00283
00284
00285
00286 FOR_EACH( it, nearby )
00287 AddModelIfVisible( *it );
00288 #else
00289 FOR_EACH( it, world->models_with_fiducials )
00290 AddModelIfVisible( *it );
00291
00292 #endif
00293
00294
00295
00296 Model::Update();
00297 }
00298
00299 void ModelFiducial::Load( void )
00300 {
00301 PRINT_DEBUG( "fiducial load" );
00302
00303 Model::Load();
00304
00305
00306 min_range = wf->ReadLength( wf_entity, "range_min", min_range );
00307 max_range_anon = wf->ReadLength( wf_entity, "range_max", max_range_anon );
00308 max_range_id = wf->ReadLength( wf_entity, "range_max_id", max_range_id );
00309 fov = wf->ReadAngle ( wf_entity, "fov", fov );
00310 ignore_zloc = wf->ReadInt ( wf_entity, "ignore_zloc", ignore_zloc);
00311 }
00312
00313
00314 void ModelFiducial::DataVisualize( Camera* cam )
00315 {
00316 (void)cam;
00317
00318 if( showFov )
00319 {
00320 PushColor( 1,0,1,0.2 );
00321
00322 GLUquadric* quadric = gluNewQuadric();
00323
00324 gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );
00325
00326 gluPartialDisk( quadric,
00327 0,
00328 max_range_anon,
00329 20,
00330 1,
00331 rtod( M_PI/2.0 + fov/2.0),
00332 rtod(-fov) );
00333
00334 gluDeleteQuadric( quadric );
00335
00336 PopColor();
00337 }
00338
00339 if( showData )
00340 {
00341 PushColor( 1,0,1,0.4 );
00342
00343
00344 glLineWidth( 2.0 );
00345 glLineStipple( 1, 0x00FF );
00346
00347
00348 FOR_EACH( it, fiducials )
00349 {
00350 Fiducial& fid = *it;
00351
00352 double dx = fid.range * cos( fid.bearing);
00353 double dy = fid.range * sin( fid.bearing);
00354
00355 glEnable(GL_LINE_STIPPLE);
00356 glBegin( GL_LINES );
00357 glVertex2f( 0,0 );
00358 glVertex2f( dx, dy );
00359 glEnd();
00360 glDisable(GL_LINE_STIPPLE);
00361
00362 glPushMatrix();
00363 Gl::coord_shift( dx,dy,0,fid.geom.a );
00364
00365 glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
00366 glRectf( -fid.geom.x/2.0, -fid.geom.y/2.0,
00367 fid.geom.x/2.0, fid.geom.y/2.0 );
00368
00369
00370 char idstr[32];
00371 snprintf(idstr, 31, "%d", fid.id );
00372 Gl::draw_string( 0,0,0, idstr );
00373
00374 glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
00375 glPopMatrix();
00376 }
00377
00378 PopColor();
00379 glLineWidth( 1.0 );
00380 }
00381 }
00382
00383 void ModelFiducial::Shutdown( void )
00384 {
00385
00386 fiducials.clear();
00387 Model::Shutdown();
00388 }