cv_georeference.c

Go to the documentation of this file.

/*
 * Copyright (C) C. De Wagter
 *
 * This file is part of paparazzi
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, see
 * <http://www.gnu.org/licenses/>.
 */
#include "modules/computer_vision/cv_georeference.h"

#include "math/pprz_trig_int.h"
#include "math/pprz_algebra.h"
#include "math/pprz_algebra_int.h"

#include "state.h"
#include "generated/flight_plan.h"
#include "subsystems/datalink/downlink.h"

struct georeference_filter_t {
  struct Int32Vect3 x;          
  struct Int32Vect3 v;          
  int32_t P;                    
};

struct georeference_t {
  struct Int32Vect3 target_i;   
  struct Int32Vect3 target_l;   

  struct Int32Vect3 x_t;        

  struct georeference_filter_t filter;  
};

struct georeference_t geo;

void georeference_project(struct camera_frame_t *tar, int wp)
{
  // Target direction in camera frame: Zero is looking down in body frames
  // Pixel with x (width) value 0 projects to the left (body-Y-axis)
  // and y = 0 (height) to the top (body-X-axis)
  VECT3_ASSIGN(geo.target_i,
               ((tar->h / 2) - tar->py),
               (tar->px - (tar->w / 2)),
               (tar->f)
              );
  INT32_VECT3_LSHIFT(geo.target_i, geo.target_i, 4)

  // Camera <-> Body
  // Looking down in body frame
  // Bebop has 180deg Z rotation in camera (butt up yields normal webcam)
  struct Int32RMat body_to_cam_rmat;
  INT32_MAT33_ZERO(body_to_cam_rmat);
  MAT33_ELMT(body_to_cam_rmat, 0, 0) = -1 << INT32_TRIG_FRAC;
  MAT33_ELMT(body_to_cam_rmat, 1, 1) = -1 << INT32_TRIG_FRAC;
  MAT33_ELMT(body_to_cam_rmat, 2, 2) =  1 << INT32_TRIG_FRAC;

  struct Int32Vect3 target_b;
  int32_rmat_transp_vmult(&target_b, &body_to_cam_rmat, &geo.target_i);

  // Body <-> LTP
  struct Int32RMat *ltp_to_body_rmat = stateGetNedToBodyRMat_i();
  int32_rmat_transp_vmult(&geo.target_l, ltp_to_body_rmat, &target_b);

  // target_l is now a scale-less [pix<<POS_FRAC] vector in LTP from the drone to the target
  // Divide by z-component to normalize the projection vector
  int32_t zi = geo.target_l.z;
  if (zi <= 0)
  {
    // Pointing up or horizontal -> no ground projection
    return;
  }

  // Multiply with height above ground
  struct NedCoor_i *pos = stateGetPositionNed_i();
  int32_t zb = pos->z;
  geo.target_l.x *= zb;
  geo.target_l.y *= zb;

  // Divide by z-component
  geo.target_l.x /= zi;
  geo.target_l.y /= zi;
  geo.target_l.z = zb;

  // NED
  geo.x_t.x = pos->x - geo.target_l.x;
  geo.x_t.y = pos->y - geo.target_l.y;
  geo.x_t.z = 0;

  // ENU
  if (wp > 0) {
    waypoint_set_xy_i(wp, geo.x_t.y, geo.x_t.x);
    waypoint_set_alt_i(wp, geo.x_t.z);

    int32_t h = -geo.x_t.z;
    uint8_t wp_id = wp;
    DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(geo.x_t.y),
                                   &(geo.x_t.x), &(h));

  }
}

void georeference_filter(bool kalman, int wp, int length)
{
  struct Int32Vect3 err;

  if (kalman)
  {
    // Predict
    VECT3_ADD(geo.filter.x, geo.filter.v);

    // Error = prediction - observation
    VECT3_COPY(err,geo.filter.x);
    VECT3_SUB(err, geo.x_t);
  }
  else // Average
  {
    VECT3_SMUL(geo.filter.x,geo.filter.x,geo.filter.P);
    VECT3_ADD(geo.filter.x, geo.x_t);
    geo.filter.P++;
    VECT3_SDIV(geo.filter.x,geo.filter.x,geo.filter.P);
    if (geo.filter.P > length) {
      geo.filter.P = length;
    }
  }

  // ENU
  waypoint_set_xy_i(wp, geo.filter.x.y, geo.filter.x.x);
  //waypoint_set_alt_i(wp, geo.filter.x.z);

  int32_t h = 0;
  uint8_t wp_id = wp;
  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(geo.filter.x.y),
                                 &(geo.filter.x.x), &(h));
}

int32_t focus_length;

void georeference_run(void)
{
  struct camera_frame_t target;
  target.w = 320;
  target.h = 240;
  target.f = focus_length;
  target.px = 0;
  target.py = 0;
  georeference_project(&target,WP_p1);
  target.px = 320;
  target.py = 0;
  georeference_project(&target,WP_p2);
  target.px = 320;
  target.py = 240;
  georeference_project(&target,WP_p3);
  target.px = 0;
  target.py = 240;
  georeference_project(&target,WP_p4);

  target.px = 0;
  target.py = 120;
  georeference_project(&target,0);
  georeference_filter(FALSE, WP_CAM,50);
}

void georeference_init(void)
{
  INT32_VECT3_ZERO(geo.target_i);
  INT32_VECT3_ZERO(geo.target_l);

  VECT3_ASSIGN(geo.x_t, 0, 0, 0);

  INT32_VECT3_ZERO(geo.filter.v);
  INT32_VECT3_ZERO(geo.filter.x);
  geo.filter.P = 0;
  focus_length = 400;
}