// Merge a connected compontent of lines to a new line with width.
void merge(const forward_list<LineSeg*>& component, LineSegW& out) {
// TODO: in principle no loop/collection is necessary
// Note: profiling indicates this is not even close to a bottleneck, so ok for now
forward_list<Point*> component2i;
for (auto vec = component.begin(); vec != component.end(); ++vec) {
component2i.push_front(&(*vec)->s);
component2i.push_front(&(*vec)->e);
//component2i.push_front((Vec2i*)&(*vec)->val[0]);
//component2i.push_front((Vec2i*)&(*vec)->val[2]);
}
line_fit(component2i, out);
}
// Merge a connected compontent of lines to a new line with width.
void merge(const forward_list<LineSegW*>& component, LineSegW& out) {
// TODO: in principle no loop/collection is necessary
// Note: profiling indicates this is not even close to a bottleneck, so ok for now
float maxw = 0.0f;
forward_list<Point*> component2i;
for (auto vec = component.begin(); vec != component.end(); ++vec) {
component2i.push_front(&(*vec)->s);
component2i.push_front(&(*vec)->e);
if ((*vec)->width > maxw) {
maxw = (*vec)->width;
}
}
line_fit(component2i, out);
out.width = max(maxw, out.width);
}
/**
* Run the optical flow with EDGEFLOW on a new image frame
* @param[in] *opticflow The opticalflow structure that keeps track of previous images
* @param[in] *state The state of the drone
* @param[in] *img The image frame to calculate the optical flow from
* @param[out] *result The optical flow result
*/
void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result)
{
// Define Static Variables
static struct edge_hist_t edge_hist[MAX_HORIZON];
static uint8_t current_frame_nr = 0;
struct edge_flow_t edgeflow;
static uint8_t previous_frame_offset[2] = {1, 1};
// Define Normal variables
struct edgeflow_displacement_t displacement;
displacement.x = malloc(sizeof(int32_t) * img->w);
displacement.y = malloc(sizeof(int32_t) * img->h);
// If the methods just switched to this one, reintialize the
// array of edge_hist structure.
if (opticflow->just_switched_method == 1) {
int i;
for (i = 0; i < MAX_HORIZON; i++) {
edge_hist[i].x = malloc(sizeof(int32_t) * img->w);
edge_hist[i].y = malloc(sizeof(int32_t) * img->h);
FLOAT_RATES_ZERO(edge_hist[i].rates);
}
}
uint16_t disp_range;
if (opticflow->search_distance < DISP_RANGE_MAX) {
disp_range = opticflow->search_distance;
} else {
disp_range = DISP_RANGE_MAX;
}
uint16_t window_size;
if (opticflow->window_size < MAX_WINDOW_SIZE) {
window_size = opticflow->window_size;
} else {
window_size = MAX_WINDOW_SIZE;
}
uint16_t RES = opticflow->subpixel_factor;
//......................Calculating EdgeFlow..................... //
// Calculate current frame's edge histogram
int32_t *edge_hist_x = edge_hist[current_frame_nr].x;
int32_t *edge_hist_y = edge_hist[current_frame_nr].y;
calculate_edge_histogram(img, edge_hist_x, 'x', 0);
calculate_edge_histogram(img, edge_hist_y, 'y', 0);
// Copy frame time and angles of image to calculated edge histogram
edge_hist[current_frame_nr].frame_time = img->ts;
edge_hist[current_frame_nr].rates = state->rates;
// Calculate which previous edge_hist to compare with the current
uint8_t previous_frame_nr[2];
calc_previous_frame_nr(result, opticflow, current_frame_nr, previous_frame_offset, previous_frame_nr);
//Select edge histogram from the previous frame nr
int32_t *prev_edge_histogram_x = edge_hist[previous_frame_nr[0]].x;
int32_t *prev_edge_histogram_y = edge_hist[previous_frame_nr[1]].y;
//Calculate the corresponding derotation of the two frames
int16_t der_shift_x = 0;
int16_t der_shift_y = 0;
if (opticflow->derotation) {
der_shift_x = (int16_t)(edge_hist[current_frame_nr].rates.p /
result->fps *
(float)img->w / (OPTICFLOW_FOV_W) * opticflow->derotation_correction_factor_x);
der_shift_y = (int16_t)(edge_hist[current_frame_nr].rates.q /
result->fps *
(float)img->h / (OPTICFLOW_FOV_H) * opticflow->derotation_correction_factor_y);
}
// Estimate pixel wise displacement of the edge histograms for x and y direction
calculate_edge_displacement(edge_hist_x, prev_edge_histogram_x,
displacement.x, img->w,
window_size, disp_range, der_shift_x);
calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y,
displacement.y, img->h,
window_size, disp_range, der_shift_y);
// Fit a line on the pixel displacement to estimate
// the global pixel flow and divergence (RES is resolution)
line_fit(displacement.x, &edgeflow.div_x,
&edgeflow.flow_x, img->w,
window_size + disp_range, RES);
line_fit(displacement.y, &edgeflow.div_y,
&edgeflow.flow_y, img->h,
window_size + disp_range, RES);
/* Save Resulting flow in results
* Warning: The flow detected here is different in sign
* and size, therefore this will be multiplied with
* the same subpixel factor and -1 to make it on par with
* the LK algorithm of t opticalflow_calculator.c
* */
edgeflow.flow_x = -1 * edgeflow.flow_x;
edgeflow.flow_y = -1 * edgeflow.flow_y;
result->flow_x = (int16_t)edgeflow.flow_x / previous_frame_offset[0];
result->flow_y = (int16_t)edgeflow.flow_y / previous_frame_offset[1];
//Fill up the results optic flow to be on par with LK_fast9
result->flow_der_x = result->flow_x;
result->flow_der_y = result->flow_y;
result->corner_cnt = getAmountPeaks(edge_hist_x, 500 , img->w);
result->tracked_cnt = getAmountPeaks(edge_hist_x, 500 , img->w);
result->divergence = (float)edgeflow.flow_x / RES;
result->div_size = 0.0f;
result->noise_measurement = 0.0f;
result->surface_roughness = 0.0f;
//......................Calculating VELOCITY ..................... //
/*Estimate fps per direction
* This is the fps with adaptive horizon for subpixel flow, which is not similar
* to the loop speed of the algorithm. The faster the quadcopter flies
* the higher it becomes
*/
float fps_x = 0;
float fps_y = 0;
float time_diff_x = (float)(timeval_diff(&edge_hist[previous_frame_nr[0]].frame_time, &img->ts)) / 1000.;
float time_diff_y = (float)(timeval_diff(&edge_hist[previous_frame_nr[1]].frame_time, &img->ts)) / 1000.;
fps_x = 1 / (time_diff_x);
fps_y = 1 / (time_diff_y);
result->fps = fps_x;
// Calculate velocity
float vel_x = edgeflow.flow_x * fps_x * state->agl * OPTICFLOW_FOV_W / (img->w * RES);
float vel_y = edgeflow.flow_y * fps_y * state->agl * OPTICFLOW_FOV_H / (img->h * RES);
//Apply a median filter to the velocity if wanted
if (opticflow->median_filter == true) {
result->vel_x = (float)update_median_filter(&vel_x_filt, (int32_t)(vel_x * 1000)) / 1000;
result->vel_y = (float)update_median_filter(&vel_y_filt, (int32_t)(vel_y * 1000)) / 1000;
} else {
result->vel_x = vel_x;
result->vel_y = vel_y;
}
result->noise_measurement = 0.2;
#if OPTICFLOW_SHOW_FLOW
draw_edgeflow_img(img, edgeflow, prev_edge_histogram_x, edge_hist_x);
#endif
// Increment and wrap current time frame
current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON;
}
int geometric_descriptors (Protein * protein) {
int helix_ctr, strand_ctr, res_ctr, atom_ctr, ctr, no_of_points;
double **point;
double p[3]; /* the direction vector */
double cm[3]; /* the CM vector */
int sheet[100][100] = {{0}}; /* TODO I can have up to a 100 sheets, 100 starnds each */
char sheet_id [100][PDB_SHEET_SHEET_LEN+1] = {{'\0'}}; /* TODO id for each sheet */
int sheet_ctr, no_of_sheets = 0;
int n_params = 7;
double *params;
Residue * residue;
int lm_fit (double **points, int n_points, double *params, int n_params);
int line_fit (double **point, int no_of_points,
double p[], double center_of_mass[]);
int perp_line_fit (double **point, int no_of_points,
double p[], double cm[], double avg[]);
double normal_to_catenoid (double **point, int no_of_points,
double *params, double normal[3], double foot[3]);
if ( ! (params = emalloc (n_params*sizeof(double) ) ) ) return 1;
/* allocate 3 times the protein length (= numbr of residues),
bcs for the fiting purposes
we might extract C,N and perhaps O backbone atoms */
if ( ! (point = dmatrix (3*protein->length, 3) ) ) return 1;
/************************************************************************/
/************************************************************************/
/************************************************************************/
/***** HELIX PROCESSING ********************************/
/************************************************************************/
/************************************************************************/
/*for each helix, extact C-alpha in an array, and fit a line through it */
for (helix_ctr=0; helix_ctr<protein->no_helices; helix_ctr++) {
ctr = 0;
/* extract C-alphas */
for (res_ctr = protein->helix[helix_ctr].begin;
res_ctr <= protein->helix[helix_ctr].end; res_ctr++) {
residue = protein->sequence+res_ctr;
for (atom_ctr = 0; atom_ctr < residue->no_atoms; atom_ctr++) {
if ( ! strcmp (residue->atom[atom_ctr].type, "CA") ) {
point[ctr][0] = residue->atom[atom_ctr].x;
point[ctr][1] = residue->atom[atom_ctr].y;
point[ctr][2] = residue->atom[atom_ctr].z;
ctr++;
break;
}
}
}
no_of_points = ctr;
/* fit a line through them */
line_fit (point, no_of_points, p, cm);
if ( ! (protein->helix[helix_ctr].p = dmatrix (1,3)) ) return 1;
if ( ! (protein->helix[helix_ctr].cm = dmatrix (1,3)) ) return 1;
memcpy (protein->helix[helix_ctr].p[0], p, 3*sizeof(double) );
memcpy (protein->helix[helix_ctr].cm[0], cm, 3*sizeof(double) );
protein->helix[helix_ctr].no_of_vectors = 1;
}
/************************************************************************/
/************************************************************************/
/************************************************************************/
/***** SHEET PROCESSING ********************************/
/************************************************************************/
/************************************************************************/
/*for the moment, do the same for each strand */
/*for each strand, extact C-alpha in an array, and fit a line through it */
for (strand_ctr=0; strand_ctr<protein->no_strands; strand_ctr++) {
/* which sheet does this strand belong to? */
for ( sheet_ctr=0; sheet_ctr < no_of_sheets; sheet_ctr++ ) {
if ( ! strcmp (sheet_id[sheet_ctr],
protein->strand[strand_ctr].sheet_id) ) {
break;
}
}
/* some sheet we have not seen so far: */
if ( sheet_ctr == no_of_sheets ) {
memcpy (sheet_id[sheet_ctr], protein->strand[strand_ctr].sheet_id,
PDB_SHEET_SHEET_LEN);
no_of_sheets ++;
}
sheet[sheet_ctr][0] ++;
sheet[sheet_ctr][ sheet[sheet_ctr][0] ] = strand_ctr;
protein->strand[strand_ctr].sheet_number = sheet_ctr+1;
/**** associate a direction with each strand ****/
/* extract C's and N's */
ctr = 0;
for (res_ctr = protein->strand[strand_ctr].begin;
res_ctr <= protein->strand[strand_ctr].end; res_ctr++) {
residue = protein->sequence+res_ctr;
for (atom_ctr = 0; atom_ctr < residue->no_atoms; atom_ctr++) {
if ( ! strcmp (residue->atom[atom_ctr].type, "C")
|| ! strcmp (residue->atom[atom_ctr].type, "N") ) {
point[ctr][0] = residue->atom[atom_ctr].x;
point[ctr][1] = residue->atom[atom_ctr].y;
point[ctr][2] = residue->atom[atom_ctr].z;
ctr++;
}
}
}
no_of_points = ctr;
/* allocate */
if ( ! (protein->strand[strand_ctr].p = dmatrix (1,3)) ) return 1;
if ( ! (protein->strand[strand_ctr].cm = dmatrix (1,3)) ) return 1;
/***********/
/* fit a line through C's and N's ,--- cannot use CA trace*/
line_fit (point, no_of_points, p, cm);
memcpy (protein->strand[strand_ctr].p[0], p, 3*sizeof(double) );
memcpy (protein->strand[strand_ctr].cm[0], cm, 3*sizeof(double) );
protein->strand[strand_ctr].no_of_vectors = 1;
/***********/
}
protein->no_sheets = no_of_sheets;
if ( ! no_of_sheets) return 0;
free (params);
free_dmatrix (point);
return 0;
}
/**
* Run the optical flow with EDGEFLOW on a new image frame
* @param[in] *opticflow The opticalflow structure that keeps track of previous images
* @param[in] *state The state of the drone
* @param[in] *img The image frame to calculate the optical flow from
* @param[out] *result The optical flow result
*/
void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result)
{
// Define Static Variables
static struct edge_hist_t edge_hist[MAX_HORIZON];
static uint8_t current_frame_nr = 0;
static struct edge_flow_t edgeflow;
static uint8_t previous_frame_offset[2] = {1, 1};
// Define Normal variables
struct edgeflow_displacement_t displacement;
uint16_t disp_range;
if (opticflow->search_distance < DISP_RANGE_MAX) {
disp_range = opticflow->search_distance;
} else {
disp_range = DISP_RANGE_MAX;
}
uint16_t window_size;
if (opticflow->window_size < MAX_WINDOW_SIZE) {
window_size = opticflow->window_size;
} else {
window_size = MAX_WINDOW_SIZE;
}
uint16_t RES = opticflow->subpixel_factor;
//......................Calculating EdgeFlow..................... //
// Calculate current frame's edge histogram
int32_t *edge_hist_x = edge_hist[current_frame_nr].x;
int32_t *edge_hist_y = edge_hist[current_frame_nr].y;
calculate_edge_histogram(img, edge_hist_x, 'x', 0);
calculate_edge_histogram(img, edge_hist_y, 'y', 0);
// Copy frame time and angles of image to calculated edge histogram
memcpy(&edge_hist[current_frame_nr].frame_time, &img->ts, sizeof(struct timeval));
edge_hist[current_frame_nr].pitch = state->theta;
edge_hist[current_frame_nr].roll = state->phi;
// Calculate which previous edge_hist to compare with the current
uint8_t previous_frame_nr[2];
calc_previous_frame_nr(result, opticflow, current_frame_nr, previous_frame_offset, previous_frame_nr);
//Select edge histogram from the previous frame nr
int32_t *prev_edge_histogram_x = edge_hist[previous_frame_nr[0]].x;
int32_t *prev_edge_histogram_y = edge_hist[previous_frame_nr[1]].y;
//Calculate the corresponding derotation of the two frames
int16_t der_shift_x = 0;
int16_t der_shift_y = 0;
if (opticflow->derotation) {
der_shift_x = -(int16_t)((edge_hist[previous_frame_nr[0]].roll - edge_hist[current_frame_nr].roll) *
(float)img->w / (OPTICFLOW_FOV_W));
der_shift_y = -(int16_t)((edge_hist[previous_frame_nr[1]].pitch - edge_hist[current_frame_nr].pitch) *
(float)img->h / (OPTICFLOW_FOV_H));
}
// Estimate pixel wise displacement of the edge histograms for x and y direction
calculate_edge_displacement(edge_hist_x, prev_edge_histogram_x,
displacement.x, img->w,
window_size, disp_range, der_shift_x);
calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y,
displacement.y, img->h,
window_size, disp_range, der_shift_y);
// Fit a line on the pixel displacement to estimate
// the global pixel flow and divergence (RES is resolution)
line_fit(displacement.x, &edgeflow.div_x,
&edgeflow.flow_x, img->w,
window_size + disp_range, RES);
line_fit(displacement.y, &edgeflow.div_y,
&edgeflow.flow_y, img->h,
window_size + disp_range, RES);
/* Save Resulting flow in results
* Warning: The flow detected here is different in sign
* and size, therefore this will be multiplied with
* the same subpixel factor and -1 to make it on par with
* the LK algorithm of t opticalflow_calculator.c
* */
edgeflow.flow_x = -1 * edgeflow.flow_x;
edgeflow.flow_y = -1 * edgeflow.flow_y;
result->flow_x = (int16_t)edgeflow.flow_x / previous_frame_offset[0];
result->flow_y = (int16_t)edgeflow.flow_y / previous_frame_offset[1];
//Fill up the results optic flow to be on par with LK_fast9
result->flow_der_x = result->flow_x;
result->flow_der_y = result->flow_y;
result->corner_cnt = getAmountPeaks(edge_hist_x, 500 , img->w);
result->tracked_cnt = getAmountPeaks(edge_hist_x, 500 , img->w);
result->divergence = (float)edgeflow.flow_x / RES;
result->div_size = 0.0f;
result->noise_measurement = 0.0f;
result->surface_roughness = 0.0f;
//......................Calculating VELOCITY ..................... //
/*Estimate fps per direction
* This is the fps with adaptive horizon for subpixel flow, which is not similar
* to the loop speed of the algorithm. The faster the quadcopter flies
* the higher it becomes
*/
float fps_x = 0;
float fps_y = 0;
float time_diff_x = (float)(timeval_diff(&edge_hist[previous_frame_nr[0]].frame_time, &img->ts)) / 1000.;
float time_diff_y = (float)(timeval_diff(&edge_hist[previous_frame_nr[1]].frame_time, &img->ts)) / 1000.;
fps_x = 1 / (time_diff_x);
fps_y = 1 / (time_diff_y);
result->fps = fps_x;
// Calculate velocity
float vel_x = edgeflow.flow_x * fps_x * state->agl * OPTICFLOW_FOV_W / (img->w * RES);
float vel_y = edgeflow.flow_y * fps_y * state->agl * OPTICFLOW_FOV_H / (img->h * RES);
result->vel_x = vel_x;
result->vel_y = vel_y;
/* Rotate velocities from camera frame coordinates to body coordinates.
* IMPORTANT This frame to body orientation should be the case for the Parrot
* ARdrone and Bebop, however this can be different for other quadcopters
* ALWAYS double check!
*/
result->vel_body_x = - vel_y;
result->vel_body_y = vel_x;
#if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_FLOW
draw_edgeflow_img(img, edgeflow, displacement, *edge_hist_x)
#endif
// Increment and wrap current time frame
current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON;
}
