Plugins/Nao/fvnao_loc

Vision plugin description: fvnao_loc

Requires

Interfaces

  • ObjectPositionInterface("Nao Camera Position") REQUIRED Either from the NaoQi fawkes integrator (FawkesIntegrator.xml -> provide_cam_pos) or from the naocampos plugin
  • ObjectPositionInterface("Nao Camera Position GPS") (required if use_gps is true) From the naosim plugin
  • ObjectPositionInterface("Sky-Blue Goal") From the nao_goals plugin
  • ObjectPositionInterface("Yellow Goal") From the nao_goals plugin

Config values

  • /firevision/nao/camera/
    • active_camera string Name of the sub-directory of the camera to use
  • /firevision/nao/camera/SUBDIR/
    • device string The CameraArgumentParser string to open the camera
    • horizontal_viewing_angle float [deg] The horizontal viewing angle of the camera is required for the intrinsic camera parameters that are used for the back projection of the line points
    • use_intrinsic bool If true the parameters of the intrinsic sub directory are used, else the intrinsic parameters gets calculated from the horizontal FOV and the image size
  • /firevision/nao/camera/SUBDIR/intrinsic/
    • focal_length_x float [px] The focal length in x direction
    • focal_length_y float [px] The focal length in y direction
    • principal_point_x float [sub px] The principal point's x-coordinate (center of projection plane)
    • principal_point_y float [sub px] The principal point's y-coordinate (center of projection plane)
  • /firevision/nao_loc/
    • enable_logging bool true to enable debug logging messages
    • draw_field bool If true a SharedMemoryImageBuffer named "FieldImage" is opened and the field, the detected line points and the own position (estimate) gets drawn
    • img_width uint The width of the field image
    • img_height uint The height of the field image
    • draw_line_points bool If true a SharedMemoryImageBuffer named "LinePointsImage" is opened and the currents image y-channel plus the detected line points gets drawn
    • field_name string The name of the current field description. Currently two fields ("Field6x4", "FieldCityTower" and "FieldLinesCityTowerSeminar") are hardcoded in fvutils/draw/field_lines.cpp
    • field_length float [m] The length of the area that should be displayed in the "FieldImage"
    • field_width float [m] The width of the area that should be displayed in the "FieldImage"
    • initial_x float [m] The initial x position used by the Cox algorithm
    • initial_y float [m] The initial y position used by the Cox algorithm
    • initial_ori float [rad] The initial orientation used by the Cox algorithm
    • update_interval uint To save computation power the camera position gets only calculated every update_interval loop
    • use_always_gps bool If true the reading of the GPS node is used as initial position for the Cox (Only on the simulator, used as ground truth)
    • use_always_initial bool If true the initial position gets used at every loop (kind of ground truth on the real Nao as long as we do not move)
    • calc_goals bool FUTURE USE: The idea is to use the goal for triangulation as initial position for the Cox
    • calc_cox bool If false the Cox algorithm is not used (only for debugging)
    • gld_threshold uint The minimum ramp height for a line to be detected
    • gld_max_line_width uint [px] The maximum width of a line (broader white areas are expected to be a part of the Nao)
    • gld_lines_are_bright bool If true the lines are brighter than the field (as usual).
    • gld_grid_size_x uint [px] scanline grid size
    • gld_grid_size_y uint [px] scanline grid size

Provides

  • ObjectPositionInterface("Nao Position")
    • world_x absolute x position on the field
    • world_y absolute y position on the field
    • yaw absolute orientation on the field
    • is_visible true if calc_cox is true and more than five line points were found.
    • is_valid true if is_visible and for n-cycles since is_visible became false

Description

  • The plugin uses a gradient edge detection to find line points.
  • The line points get projected to the ground plane to retrieve relative distances to the line points.
  • Together with the current initial position and the predefined field lines the Cox algorithm calculates the new absolute position.

Cox localization algorithm

The Cox algorithm is a point to feature localization algorithm. It tires to minimize the error function between a list of points and a set of features. The algorithm gets initialized with a set of features (the field lines in our case). For the actual execution a list of points (relative to the object of interest aka the Nao) and an estimated position. In a first step the points get transformed to the estimated position. Then each point gets the closest feature assigned and the distance to this feature is used to calculate the quadratic error function. The derrived error function is used to change the estimated position and the algorithm starts again until a local minima of the error function is found.

Appendix

Run plugin naocampos, fvbase, fvfountain, nao_loc and see the result either via loc_viewer or fvfuseviewer. I don't know nothing about webview so I can't tell.

After "build up a soccer field with your favorite green carpet and white duck tape" use either the field dimensions of the official 2008 field (and set the field to Field6x4) or the dimensions of the field of the City Tower in Graz (FieldCityTower) or define you own field in fvutils/draw/field_lines.* and extend the Field::field_for_name() function in fvutils/draw/field.cpp

The robot has to be placed on a known initial position (like the config suggests)

Depending on the light situation/camera settings the gld_threshold has to be adapted