Skip to content

Commit

Permalink
[opticflow] commenting, renames, dox
Browse files Browse the repository at this point in the history
  • Loading branch information
@dewagter
dewagter committed Jan 15, 2015
1 parent c3c7676 commit bd9d549
Show file tree
Hide file tree
Showing 7 changed files with 19 additions and 115 deletions.
4 changes: 2 additions & 2 deletions conf/modules/cv_opticflow.xml
View file
Expand Up @@ -46,9 +46,9 @@
<define name="USE_ARDRONE_VIDEO" />

<file name="opticflow_module.c"/>
<file name="opticflow_code.c" dir="modules/computer_vision/opticflow"/>
<file name="visual_estimator.c" dir="modules/computer_vision/opticflow"/>
<file name="hover_stabilization.c" dir="modules/computer_vision/opticflow"/>
<file name="optic_flow_ardrone.c" dir="modules/computer_vision/cv/opticflow"/>
<file name="optic_flow_int.c" dir="modules/computer_vision/cv/opticflow"/>
<file name="fastRosten.c" dir="modules/computer_vision/cv/opticflow/fast9"/>
<file name="trig.c" dir="modules/computer_vision/cv"/>
<file name="jpeg.c" dir="modules/computer_vision/cv/encoding"/>
Expand Down
111 changes: 8 additions & 103 deletions ..._vision/cv/opticflow/optic_flow_ardrone.c → ...uter_vision/cv/opticflow/optic_flow_int.c
View file
@@ -1,9 +1,5 @@
/*
* Copyright (C) 2014 Hann Woei Ho
*
* - Initial fixed-point C implementation by G. de Croon
* - Algorithm: Lucas-Kanade by Yves Bouguet
* - Publication: http://robots.stanford.edu/cs223b04/algo_tracking.pdf
* Copyright (C) 2014
*
* This file is part of Paparazzi.
*
Expand All @@ -23,18 +19,20 @@
*/

/**
* @file modules/computer_vision/cv/opticflow/optic_flow_ardrone.c
* @brief optical-flow based hovering for Parrot AR.Drone 2.0
* @file modules/computer_vision/cv/opticflow/optic_flow_int.c
* @brief efficient fixed-point optical-flow
*
* Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
* - Initial fixed-point C implementation by G. de Croon
* - Algorithm: Lucas-Kanade by Yves Bouguet
* - Publication: http://robots.stanford.edu/cs223b04/algo_tracking.pdf
*/

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include "optic_flow_ardrone.h"
#include "../opticflow_module.h"
#include "optic_flow_int.h"
#include "modules/computer_vision/opticflow_module.h"

#define int_index(x,y) (y * IMG_WIDTH + x)
#define uint_index(xx, yy) (((yy * IMG_WIDTH + xx) * 2) & 0xFFFFFFFC)
Expand Down Expand Up @@ -83,12 +81,10 @@ void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int ce
int subpixel_factor)
{
int x, y, x_0, y_0, x_0_or, y_0_or, i, j, window_size, alpha_x, alpha_y, max_x, max_y;
//int printed, limit;
unsigned int ix1, ix2, Y;
window_size = half_window_size * 2 + 1;
max_x = (IMG_WIDTH - 1) * subpixel_factor;
max_y = (IMG_HEIGHT - 1) * subpixel_factor;
//printed = 0; limit = 4;

for (i = 0; i < window_size; i++) {
for (j = 0; j < window_size; j++) {
Expand All @@ -107,18 +103,9 @@ void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int ce
x_0 = x_0_or * subpixel_factor;
y_0_or = (y / subpixel_factor);
y_0 = y_0_or * subpixel_factor;
/*if(printed < limit)
{
printf("x_0_or = %d, y_0_or = %d;\n\r", x_0_or, y_0_or);
printf("x_0 = %d, y_0 = %d\n\r");
printed++;
}*/


if (x == x_0 && y == y_0) {
// simply copy the pixel:
// ix2 = uint_index(x_0_or, y_0_or);
// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1;
ix2 = y_0_or * IMG_WIDTH + x_0_or;
Y = (unsigned int)frame_buf[ix2 + 1];
Patch[ix1] = (int) Y;
Expand All @@ -130,42 +117,22 @@ void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int ce
// the patch pixel is a blend from the four surrounding pixels:
ix2 = y_0_or * IMG_WIDTH + x_0_or;
Y = (unsigned int)frame_buf[ix2 + 1];
// ix2 = uint_index(x_0_or, y_0_or);
// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1;
Patch[ix1] = (subpixel_factor - alpha_x) * (subpixel_factor - alpha_y) * ((int) Y);

ix2 = y_0_or * IMG_WIDTH + (x_0_or + 1);
Y = (unsigned int)frame_buf[ix2 + 1];
// ix2 = uint_index((x_0_or+1), y_0_or);
// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1;
//if(printed < limit) printf("subpixel: TR = %d\n\r", Y);
Patch[ix1] += alpha_x * (subpixel_factor - alpha_y) * ((int) Y);

ix2 = (y_0_or + 1) * IMG_WIDTH + x_0_or;
Y = (unsigned int)frame_buf[ix2 + 1];
// ix2 = uint_index(x_0_or, (y_0_or+1));
// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1;
//if(printed < limit) printf("subpixel: BL = %d\n\r", Y);
Patch[ix1] += (subpixel_factor - alpha_x) * alpha_y * ((int) Y);

ix2 = (y_0_or + 1) * IMG_WIDTH + (x_0_or + 1);
Y = (unsigned int)frame_buf[ix2 + 1];
// ix2 = uint_index((x_0_or+1), (y_0_or+1));
// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1;
//if(printed < limit) printf("subpixel: BR = %d\n\r", Y);
Patch[ix1] += alpha_x * alpha_y * ((int) Y);

// normalize patch value
Patch[ix1] /= (subpixel_factor * subpixel_factor);

/*if(printed < limit)
{
printf("alpha_x = %d, alpha_y = %d, x_0 = %d, y_0 = %d, x = %d, y = %d, Patch[ix1] = %d\n\r", alpha_x, alpha_y, x_0, y_0, x, y, Patch[ix1]);
// printed++;
}
*/

}
}
}
Expand All @@ -191,14 +158,12 @@ void getGradientPatch(int *Patch, int *DX, int *DY, int half_window_size)
Y1 = Patch[ix1];
ix1 = (unsigned int)(y * padded_patch_size + x + 1);
Y2 = Patch[ix1];
// DX[ix2] = Y2 - Y1;
DX[ix2] = (Y2 - Y1) / 2;

ix1 = (unsigned int)((y - 1) * padded_patch_size + x);
Y1 = Patch[ix1];
ix1 = (unsigned int)((y + 1) * padded_patch_size + x);
Y2 = Patch[ix1];
// DY[ix2] = Y2 - Y1;
DY[ix2] = (Y2 - Y1) / 2;


Expand All @@ -215,26 +180,11 @@ int getSumPatch(int *Patch, int size)

// in order to keep the sum within range:
//threshold = 50000; // typical values are far below this threshold

sum = 0;
for (x = 0; x < size; x++) {
for (y = 0; y < size; y++) {
ix = (y * size) + x;
//if(sum < threshold && sum > -threshold)
//{
sum += Patch[ix]; // do not check thresholds
//}
/*else
{
if(sum > threshold)
{
sum = threshold;
}
else
{
sum = -threshold;
}
}*/
}
}

Expand Down Expand Up @@ -342,20 +292,16 @@ int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int
}

for (p = 0; p < n_found_points; p++) {
//printf("*** NEW POINT ***\n\r");
// status: point is not yet lost:
status[p] = 1;

//printf("Normal coordinate: (%d,%d)\n\r", p_x[p], p_y[p]);
// We want to be able to take steps in the image of 1 / subpixel_factor:
p_x[p] *= subpixel_factor;
p_y[p] *= subpixel_factor;
//printf("Subpixel coordinate: (%d,%d)\n\r", p_x[p], p_y[p]);

// if the pixel is outside the ROI in the image, do not track it:
if (!(p_x[p] > ((half_window_size + 1) * subpixel_factor) && p_x[p] < (IMG_WIDTH - half_window_size) * subpixel_factor
&& p_y[p] > ((half_window_size + 1) * subpixel_factor) && p_y[p] < (IMG_HEIGHT - half_window_size)*subpixel_factor)) {
// printf("Outside of ROI, P1[%d,%d]\n\r",p_x[p],p_y[p]);
status[p] = 0;
}

Expand All @@ -380,18 +326,13 @@ int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int
if (error == NO_MEMORY) { return NO_MEMORY; }

for (it = 0; it < 4; it++) {
// printf("G[%d] = %d\n\r", it, G[it]);
G[it] /= 255; // to keep values in range
// printf("G[%d] = %d\n\r", it, G[it]);
}
// calculate G's determinant:
Det = G[0] * G[3] - G[1] * G[2];
//printf("Det = %d\n\r", Det);
Det = Det / subpixel_factor; // so that the steps will be expressed in subpixel units
//printf("Det = %d\n\r", Det);
if (Det < 1) {
status[p] = 0;
// printf("irrevertible G\n");
}

// (4) iterate over taking steps in the image to minimize the error:
Expand All @@ -403,14 +344,11 @@ int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int
step_y = step_threshold + 1;

while (status[p] == 1 && it < max_iterations && (abs(step_x) >= step_threshold || abs(step_y) >= step_threshold)) {
//printf("it = %d, (p_x+v_x,p_y+v_y) = (%d,%d)\n\r", it, p_x[p]+v_x, p_y[p]+v_y);
//printf("it = %d;", it);
// if the pixel goes outside the ROI in the image, stop tracking:
if (!(p_x[p] + v_x > ((half_window_size + 1) * subpixel_factor)
&& p_x[p] + v_x < ((int)IMG_WIDTH - half_window_size) * subpixel_factor
&& p_y[p] + v_y > ((half_window_size + 1) * subpixel_factor)
&& p_y[p] + v_y < ((int)IMG_HEIGHT - half_window_size)*subpixel_factor)) {
// printf("Outside of ROI, P1[%d,%d]\n\r",p_x[p],p_y[p]);
status[p] = 0;
break;
}
Expand All @@ -427,40 +365,14 @@ int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int

getSubPixel_gray(J_neighborhood, new_image_buf, p_x[p] + v_x, p_y[p] + v_y, half_window_size, subpixel_factor);
// [b] determine the image difference between the two neighborhoods
//printf("I = ");
//printIntMatrix(I_neighborhood, patch_size, patch_size);
//printf("J = ");
//printIntMatrix(J_neighborhood, patch_size, patch_size);
//getSubPixel(J_neighborhood, new_image_buf, subpixel_factor * ((p_x[p]+v_x)/subpixel_factor), subpixel_factor * ((p_y[p]+v_y) / subpixel_factor), half_window_size, subpixel_factor);
//printf("J2 = ");
//printIntMatrix(J_neighborhood, patch_size, patch_size);
//printf("figure(); subplot(1,2,1); imshow(I/255); subplot(1,2,2); imshow(J/255);\n\r");
getImageDifference(I_neighborhood, J_neighborhood, ImDiff, patch_size, patch_size);
//printf("ImDiff = ");
//printIntMatrix(ImDiff, patch_size, patch_size);
error = calculateError(ImDiff, patch_size, patch_size) / 255;

// if(error > error_threshold) printf("error threshold\n");
if (error > error_threshold && it > max_iterations / 2) {
status[p] = 0;
// printf("occlusion\n");
break;
}
//printf("error(%d) = %d;\n\r", it+1, error);
// [c] calculate the 'b'-vector
//printf("DX = ");
//printIntMatrix(DX, patch_size, patch_size);
multiplyImages(ImDiff, DX, IDDX, patch_size, patch_size);
//printf("IDDX = ");
//printIntMatrix(IDDX, patch_size, patch_size);
multiplyImages(ImDiff, DY, IDDY, patch_size, patch_size);
//printf("DY = ");
//printIntMatrix(DY, patch_size, patch_size);
//printf("IDDY = ");
//printIntMatrix(IDDY, patch_size, patch_size);
//printf("figure(); subplot(2,3,1); imagesc(ImDiff); subplot(2,3,2); imagesc(DX); subplot(2,3,3); imagesc(DY);");
//printf("subplot(2,3,4); imagesc(IDDY); subplot(2,3,5); imagesc(IDDX);\n\r");
// division by 255 to keep values in range:
b_x = getSumPatch(IDDX, patch_size) / 255;
b_y = getSumPatch(IDDY, patch_size) / 255;
//printf("b_x = %d; b_y = %d;\n\r", b_x, b_y);
Expand All @@ -469,17 +381,10 @@ int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int
step_y = (G[0] * b_y - G[2] * b_x) / Det;
v_x += step_x;
v_y += step_y; // - (?) since the origin in the image is in the top left of the image, with y positive pointing down
//printf("step = [%d,%d]; v = [%d,%d];\n\r", step_x, step_y, v_x, v_y);
//printf("pause(0.5);\n\r");
// next iteration
it++;
// step_size = abs(step_x);
// step_size += abs(step_y);
//printf("status = %d, it = %d, step_size = %d\n\r", status[p], it, step_size);
} // iteration to find the right window in the new image

//printf("figure(); plot(error(1:(it+1)));\n\r");
// printf("it = %d\n",it);
new_x[p] = (p_x[p] + v_x) / subpixel_factor;
new_y[p] = (p_y[p] + v_y) / subpixel_factor;
p_x[p] /= subpixel_factor;
Expand Down
7 changes: 3 additions & 4 deletions ..._vision/cv/opticflow/optic_flow_ardrone.h → ...uter_vision/cv/opticflow/optic_flow_int.h
View file
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2014 Hann Woei Ho
* Copyright (C) 2014
*
* This file is part of Paparazzi.
*
Expand All @@ -19,10 +19,9 @@
*/

/**
* @file modules/computer_vision/cv/opticflow/optic_flow_ardrone.h
* @brief optical-flow based hovering for Parrot AR.Drone 2.0
* @file modules/computer_vision/cv/opticflow/optic_flow_int.h
* @brief efficient fixed-point optical-flow
*
* Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
*/

#ifndef OPTIC
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c
View file
Expand Up @@ -29,7 +29,7 @@
#include "hover_stabilization.h"

// Vision Data
#include "opticflow_code.h"
#include "visual_estimator.h"

// Stabilization
//#include "stabilization.h"
Expand Down
6 changes: 3 additions & 3 deletions ...omputer_vision/opticflow/opticflow_code.c → ...puter_vision/opticflow/visual_estimator.c
View file
Expand Up @@ -19,7 +19,7 @@
*/

/**
* @file modules/computer_vision/optic_flow/opticflow_code.c
* @file modules/computer_vision/optic_flow/visual_estimator.c
* @brief optical-flow based hovering for Parrot AR.Drone 2.0
*
* Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
Expand All @@ -29,10 +29,10 @@
#include <stdlib.h>

// Own Header
#include "opticflow_code.h"
#include "visual_estimator.h"

// Computer Vision
#include "opticflow/optic_flow_ardrone.h"
#include "opticflow/optic_flow_int.h"
#include "opticflow/fast9/fastRosten.h"
#include "modules/computer_vision/opticflow_module.h"

Expand Down
2 changes: 1 addition & 1 deletion ...omputer_vision/opticflow/opticflow_code.h → ...puter_vision/opticflow/visual_estimator.h
View file
Expand Up @@ -19,7 +19,7 @@
*/

/**
* @file modules/computer_vision/optic_flow/opticflow_code.h
* @file modules/computer_vision/optic_flow/visual_estimator.h
* @brief optical-flow based hovering for Parrot AR.Drone 2.0
*
* Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/modules/computer_vision/opticflow_module.c
View file
Expand Up @@ -101,7 +101,7 @@ void opticflow_module_run(void)
#include "resize.h"

// Payload Code
#include "opticflow/opticflow_code.h"
#include "opticflow/visual_estimator.h"

// Downlink Video
//#define DOWNLINK_VIDEO 1
Expand Down

0 comments on commit bd9d549

Please sign in to comment.