act_fast.c

Go to the documentation of this file.

/*
Copyright (c) 2017, Guido de Croon, TU Delft
All rights reserved.
*/
 
 
#include "fast_rosten.h"
#include "act_fast.h"
#include "math.h"
#include "image.h"
#include "../../opticflow/opticflow_calculator.h"
#include "generated/airframe.h"
 
// ACT-FAST agents arrays
// equal to the maximal number of corners defined by fast9_rsize in opticflow_calculator.c
// TODO Currently hardcoded to two cameras
#define MAX_AGENTS FAST9_MAX_CORNERS
#ifdef OPTICFLOW_CAMERA2
#define FAST9_MAX_CAMERAS 2
#else
#define FAST9_MAX_CAMERAS 1
#endif
 
struct agent_t agents[FAST9_MAX_CAMERAS][MAX_AGENTS];
 
 
void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners, struct point_t **ret_corners, 
              uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient,
              int gradient_method, int camera_id)
{
  // Input protection:
  if (camera_id >= FAST9_MAX_CAMERAS) {
    *num_corners = 0;
    return;
  }
 
  /*
   * Procedure:
   * 1) initialize agent positions
   * 2) loop over the agents, moving and checking for corners
   */
 
  // ensure that n_agents is never bigger than max_agents
  n_agents = (n_agents < MAX_AGENTS) ? n_agents : MAX_AGENTS;
  // min_gradient should be bigger than 0:
  min_gradient = (min_gradient == 0) ? 1 : min_gradient;
 
  int border = 4;
 
  // ***********************************
  // 1) initialize the agents' positions
  // ***********************************
 
  // grid sampling with a border:
  int init_border = 10;
  float GRID_ROWS = (int) ceil(sqrtf((float) n_agents));
  float step_size_x = (img->w - 2 * init_border) / (GRID_ROWS - 1);
  float step_size_y = (img->h - 2 * init_border) / (GRID_ROWS - 1);
 
  int a = 0;
  float px, py, pnorm;
  for (int c = 0; c < GRID_ROWS; c++) {
    for (int r = 0; r < GRID_ROWS; r++) {
      // px, py represent the preferred direction of the agent when there is no texture
      // here we initialize it differently for each agent:
      // TODO: don't we have a randf function in Paparazzi?
      px = ((float)(rand() % 10000) + 1) / 10000.0f;
      py = ((float)(rand() % 10000) + 1) / 10000.0f;
      pnorm = sqrtf(px * px + py * py);
      struct agent_t ag = { (border + c * step_size_x), (border + r * step_size_y), 1, px / pnorm, py / pnorm};
      agents[camera_id][a] = ag;
      a++;
      if (a == n_agents) { break; }
    }
 
    // only initialize a maximum of n_agents agents.
    if (a == n_agents) { break; }
  }
 
  /* ********************************************************
   * 2) loop over the agents, moving and checking for corners
   * ********************************************************/
 
  // gradient
  int dx, dy;
 
  // loop over all time steps:
  for (int t = 0; t < n_time_steps; t++) {
    // loop over the agents
    for (a = 0; a < n_agents; a++) {
      // only do something if the agent is active:
      if (agents[camera_id][a].active) {
        // check if this position is a corner:
        uint16_t x = (uint16_t) agents[camera_id][a].x;
        uint16_t y = (uint16_t) agents[camera_id][a].y;
        if (fast9_detect_pixel(img, fast_threshold, x, y)) {
          // we arrived at a corner, yeah!!!
          agents[camera_id][a].active = 0;
          continue;
        } else {
          // make a step:
          struct point_t loc = { .x = agents[camera_id][a].x, .y = agents[camera_id][a].y};
          image_gradient_pixel(img, &loc, gradient_method, &dx, &dy);
          int gradient = (abs(dx) + abs(dy)) / 2;
          if (abs(gradient) >= min_gradient) {
            // determine the angle and make a step in that direction:
            float norm_factor = sqrtf((float)(dx * dx + dy * dy));
            agents[camera_id][a].x += (dy / norm_factor) * short_step;
            agents[camera_id][a].y += (dx / norm_factor) * short_step;
          } else {
            // make a step in the preferred direction:
            agents[camera_id][a].x += agents[camera_id][a].preferred_dir_x * long_step;
            agents[camera_id][a].y += agents[camera_id][a].preferred_dir_y * long_step;
          }
        }
 
        // let the agent move over the image in a toroid world:
        if (agents[camera_id][a].x > img->w - border) {
          agents[camera_id][a].x = border;
        } else if (agents[camera_id][a].x < border) {
          agents[camera_id][a].x = img->w - border;
        }
        if (agents[camera_id][a].y > img->h - border) {
          agents[camera_id][a].y = border;
        } else if (agents[camera_id][a].y < border) {
          agents[camera_id][a].y = img->h - border;
        }
      }
    }
  }
 
  // Transform agents to corners:
  (*num_corners) = 0;
  for (a = 0; a < n_agents; a++) {
 
    // for active agents do a last check on the new position:
    if (agents[camera_id][a].active) {
      // check if the last step brought the agent to a corner:
      uint16_t x = (uint16_t) agents[camera_id][a].x;
      uint16_t y = (uint16_t) agents[camera_id][a].y;
      if (fast9_detect_pixel(img, fast_threshold, x, y)) {
        // we arrived at a corner, yeah!!!
        agents[camera_id][a].active = 0;
      }
    }
 
    // if inactive, the agent is a corner:
    if (!agents[camera_id][a].active) {
      (*ret_corners)[(*num_corners)].x = (uint32_t) agents[camera_id][a].x;
      (*ret_corners)[(*num_corners)].y = (uint32_t) agents[camera_id][a].y;
      (*num_corners)++;
    }
  }
}