mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-21 20:04:09 +08:00
Derotate flow front camera (#2315)
* Predicting flow * setting the camera intrinsic paramters in the videoconfig struct (and potentially in the airframe file) * Code style fixed * implemented all suggested changes * Removed remaining error - checked both front and bottom cam * Last changes for pull reques, plus removed warning local shadowing success variable
This commit is contained in:
guidoAI
Kirk Scheper
@@ -29,10 +29,6 @@
|
||||
<module name="cv_opticflow">
|
||||
<define name="OPTICFLOW_CAMERA" value="bottom_camera"/>
|
||||
<define name="MAX_HORIZON" value="10"/>
|
||||
<define name="OPTICFLOW_FX" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
|
||||
<define name="OPTICFLOW_FY" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
|
||||
<define name="OPTICFLOW_FOV_W" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
|
||||
<define name="OPTICFLOW_FOV_H" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
|
||||
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X" value="0.8"/> <!--Obtained from a linefit-->
|
||||
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y" value="0.85"/> <!--Obtained from a linefit-->
|
||||
</module>
|
||||
|
||||
@@ -39,10 +39,6 @@
|
||||
<module name="cv_opticflow">
|
||||
<define name="OPTICFLOW_CAMERA" value="bottom_camera"/>
|
||||
<define name="MAX_HORIZON" value="10"/>
|
||||
<define name="OPTICFLOW_FX" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
|
||||
<define name="OPTICFLOW_FY" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
|
||||
<define name="OPTICFLOW_FOV_W" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
|
||||
<define name="OPTICFLOW_FOV_H" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
|
||||
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X" value="0.8"/> <!--Obtained from a linefit-->
|
||||
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y" value="0.85"/> <!--Obtained from a linefit-->
|
||||
</module>
|
||||
|
||||
@@ -56,11 +56,7 @@
|
||||
<module name="cv_opticflow">
|
||||
<define name="OPTICFLOW_CAMERA" value="front_camera"/>
|
||||
<define name="MAX_HORIZON" value="10"/>
|
||||
<define name="OPTICFLOW_FX" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
|
||||
<define name="OPTICFLOW_FY" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
|
||||
<define name="OPTICFLOW_FOV_W" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
|
||||
<define name="OPTICFLOW_FOV_H" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
|
||||
<define name="OPTICFLOW_DEROTATION" value="0"/> <!-- formulas are not correct for the front cam -->
|
||||
<define name="OPTICFLOW_DEROTATION" value="1"/>
|
||||
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X" value="0.8"/> <!--Obtained from a linefit-->
|
||||
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y" value="0.85"/> <!--Obtained from a linefit-->
|
||||
<define name="OPTICFLOW_FEATURE_MANAGEMENT" value="0"/> <!-- feature management still sucks -->
|
||||
|
||||
@@ -37,10 +37,6 @@
|
||||
<module name="cv_opticflow">
|
||||
<define name="OPTICFLOW_CAMERA" value="bottom_camera"/>
|
||||
<define name="MAX_HORIZON" value="10"/>
|
||||
<define name="OPTICFLOW_FX" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
|
||||
<define name="OPTICFLOW_FY" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
|
||||
<define name="OPTICFLOW_FOV_W" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
|
||||
<define name="OPTICFLOW_FOV_H" value="0.665499265"/> <!-- 2 * arctan(240 / (2*347.22222222)) -->
|
||||
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X" value="0.8"/> <!--Obtained from a linefit-->
|
||||
<define name="OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y" value="0.85"/> <!--Obtained from a linefit-->
|
||||
</module>
|
||||
|
||||
@@ -17,10 +17,6 @@
|
||||
<define name="CAMERA" value="bottom_camera|front_camera" description="The V4L2 camera device that is used for the calculations"/>
|
||||
|
||||
<!-- Camera parameters -->
|
||||
<define name="FOV_W" value="0.89360857702" description="The field of view width of the bottom camera (Defaults are from an ARDrone 2)"/>
|
||||
<define name="FOV_H" value="0.67020643276" description="The field of view height of the bottom camera (Defaults are from an ARDrone 2)"/>
|
||||
<define name="FX" value="343.1211" description="Field in the x direction of the camera (Defaults are from an ARDrone 2)"/>
|
||||
<define name="FY" value="348.5053" description="Field in the y direction of the camera (Defaults are from an ARDrone 2)"/>
|
||||
<define name="DEROTATION_CORRECTION_FACTOR_X" value="1.0" description="Correction factor for derotation (in x direction), estimated from a fit between the gyro's rates and the resulting flow (caused by the camera not being exactly in the middle (Defaults are from an ARDrone 2)"/>
|
||||
<define name="DEROTATION_CORRECTION_FACTOR_Y" value="1.0" description="Correction factor for derotation (in y direction), estimated from a fit between the gyro's rates and the resulting flow (caused by the camera not being exactly in the middle (Defaults are from an ARDrone 2)"/>
|
||||
<configure name="BODY_TO_CAM_PHI" value="0" description="Rotation from body frame to camera frame around x axis"/>
|
||||
@@ -136,5 +132,7 @@
|
||||
<file name="fast_rosten.c" dir="modules/computer_vision/lib/vision"/>
|
||||
<file name="lucas_kanade.c" dir="modules/computer_vision/lib/vision"/>
|
||||
<file name="edge_flow.c" dir="modules/computer_vision/lib/vision"/>
|
||||
<file name="undistortion.c" dir="modules/computer_vision/lib/vision"/>
|
||||
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -65,7 +65,14 @@ struct video_config_t front_camera = {
|
||||
.buf_cnt = 5,
|
||||
.filters = VIDEO_FILTER_ISP,
|
||||
.cv_listener = NULL,
|
||||
.fps = MT9F002_TARGET_FPS
|
||||
.fps = MT9F002_TARGET_FPS,
|
||||
.camera_intrinsics = {
|
||||
.focal_x = MT9F002_FOCAL_X,
|
||||
.focal_y = MT9F002_FOCAL_Y,
|
||||
.center_x = MT9F002_CENTER_X,
|
||||
.center_y = MT9F002_CENTER_Y,
|
||||
.Dhane_k = MT9F002_DHANE_K
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
@@ -103,6 +103,23 @@
|
||||
#define MT9F002_Y_ODD_INC_VAL 1
|
||||
#endif
|
||||
|
||||
// parameters for undistortion
|
||||
#ifndef MT9F002_FOCAL_X
|
||||
#define MT9F002_FOCAL_X 311.59304538f
|
||||
#endif
|
||||
#ifndef MT9F002_FOCAL_Y
|
||||
#define MT9F002_FOCAL_Y 313.01338397f
|
||||
#endif
|
||||
#ifndef MT9F002_CENTER_X
|
||||
#define MT9F002_CENTER_X 158.37457814f
|
||||
#endif
|
||||
#ifndef MT9F002_CENTER_Y
|
||||
#define MT9F002_CENTER_Y 326.49375925f
|
||||
#endif
|
||||
#ifndef MT9F002_DHANE_K
|
||||
#define MT9F002_DHANE_K 1.25f
|
||||
#endif
|
||||
|
||||
/* Interface types for the MT9F002 connection */
|
||||
enum mt9f002_interface {
|
||||
MT9F002_MIPI, ///< MIPI type connection
|
||||
|
||||
@@ -60,7 +60,14 @@ struct video_config_t bottom_camera = {
|
||||
.buf_cnt = 5,
|
||||
.filters = 0,
|
||||
.cv_listener = NULL,
|
||||
.fps = MT9V117_TARGET_FPS
|
||||
.fps = MT9V117_TARGET_FPS,
|
||||
.camera_intrinsics = {
|
||||
.focal_x = MT9V117_FOCAL_X,
|
||||
.focal_y = MT9V117_FOCAL_Y,
|
||||
.center_x = MT9V117_CENTER_X,
|
||||
.center_y = MT9V117_CENTER_Y,
|
||||
.Dhane_k = MT9V117_DHANE_K
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -34,6 +34,24 @@
|
||||
#define MT9V117_TARGET_FPS 0
|
||||
#endif
|
||||
|
||||
// parameters for undistortion
|
||||
#ifndef MT9V117_FOCAL_X
|
||||
#define MT9V117_FOCAL_X 347.22f
|
||||
#endif
|
||||
#ifndef MT9V117_FOCAL_Y
|
||||
#define MT9V117_FOCAL_Y 347.22f
|
||||
#endif
|
||||
#ifndef MT9V117_CENTER_X
|
||||
#define MT9V117_CENTER_X 120.0f
|
||||
#endif
|
||||
#ifndef MT9V117_CENTER_Y
|
||||
#define MT9V117_CENTER_Y 120.0f
|
||||
#endif
|
||||
#ifndef MT9V117_DHANE_K
|
||||
#define MT9V117_DHANE_K 1.0f
|
||||
#endif
|
||||
|
||||
|
||||
struct mt9v117_t {
|
||||
struct i2c_periph *i2c_periph; ///< I2C peripheral used to communicate over
|
||||
struct i2c_transaction i2c_trans; ///< I2C transaction for comminication with CMOS chip
|
||||
|
||||
@@ -601,17 +601,26 @@ void image_show_points_color(struct image_t *img, struct point_t *points, uint16
|
||||
}
|
||||
}
|
||||
|
||||
void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor)
|
||||
{
|
||||
static uint8_t color[4] = {255, 255, 255, 255};
|
||||
static uint8_t bad_color[4] = {0, 0, 0, 0};
|
||||
image_show_flow_color(img, vectors, points_cnt, subpixel_factor, color, bad_color);
|
||||
}
|
||||
|
||||
/**
|
||||
* Shows the flow from a specific point to a new point
|
||||
* This works on YUV422 and Grayscale images
|
||||
* @param[in,out] *img The image to show the flow on
|
||||
* @param[in] *vectors The flow vectors to show
|
||||
* @param[in] *points_cnt The amount of points and vectors to show
|
||||
* @param[in] subpixel_factor
|
||||
* @param[in] color: color for good vectors
|
||||
* @param[in] bad_color: color for bad vectors
|
||||
*/
|
||||
void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor)
|
||||
void image_show_flow_color(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor,
|
||||
const uint8_t *color, const uint8_t *bad_color)
|
||||
{
|
||||
static uint8_t color[4] = {255, 255, 255, 255};
|
||||
static uint8_t bad_color[4] = {0, 0, 0, 0};
|
||||
static int size_crosshair = 5;
|
||||
|
||||
// Go through all the points
|
||||
|
||||
@@ -102,6 +102,8 @@ uint32_t image_difference(struct image_t *img_a, struct image_t *img_b, struct i
|
||||
int32_t image_multiply(struct image_t *img_a, struct image_t *img_b, struct image_t *mult);
|
||||
void image_show_points(struct image_t *img, struct point_t *points, uint16_t points_cnt);
|
||||
void image_show_points_color(struct image_t *img, struct point_t *points, uint16_t points_cnt, uint8_t *color);
|
||||
void image_show_flow_color(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor,
|
||||
const uint8_t *color, const uint8_t *bad_color);
|
||||
void image_show_flow(struct image_t *img, struct flow_t *vectors, uint16_t points_cnt, uint8_t subpixel_factor);
|
||||
void image_draw_crosshair(struct image_t *img, struct point_t *loc, uint8_t *color, int size_crosshair);
|
||||
void image_draw_rectangle(struct image_t *img, int x_min, int x_max, int y_min, int y_max, uint8_t *color);
|
||||
|
||||
@@ -41,10 +41,15 @@
|
||||
#include "lib/vision/fast_rosten.h"
|
||||
#include "lib/vision/act_fast.h"
|
||||
#include "lib/vision/edge_flow.h"
|
||||
#include "lib/vision/undistortion.h"
|
||||
#include "size_divergence.h"
|
||||
#include "linear_flow_fit.h"
|
||||
#include "modules/sonar/agl_dist.h"
|
||||
|
||||
|
||||
// to get the definition of front_camera / bottom_camera
|
||||
#include BOARD_CONFIG
|
||||
|
||||
// whether to show the flow and corners:
|
||||
#define OPTICFLOW_SHOW_CORNERS 0
|
||||
|
||||
@@ -66,29 +71,6 @@ uint16_t n_agents = 25;
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_CORNER_METHOD)
|
||||
|
||||
// Camera parameters (defaults are from an ARDrone 2)
|
||||
#ifndef OPTICFLOW_FOV_W
|
||||
#define OPTICFLOW_FOV_W 0.89360857702
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_FOV_W)
|
||||
|
||||
#ifndef OPTICFLOW_FOV_H
|
||||
#define OPTICFLOW_FOV_H 0.67020643276
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_FOV_H)
|
||||
|
||||
#ifndef OPTICFLOW_FX
|
||||
// This can be estimated by total possible image width / total Field of view
|
||||
#define OPTICFLOW_FX 343.1211
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_FX)
|
||||
|
||||
#ifndef OPTICFLOW_FY
|
||||
// This can be estimated by total possible image height / total Field of view
|
||||
#define OPTICFLOW_FY 348.5053
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(OPTICFLOW_FY)
|
||||
|
||||
/* Set the default values */
|
||||
#ifndef OPTICFLOW_MAX_TRACK_CORNERS
|
||||
#define OPTICFLOW_MAX_TRACK_CORNERS 25
|
||||
@@ -257,6 +239,8 @@ static int cmp_array(const void *a, const void *b);
|
||||
static void manage_flow_features(struct image_t *img, struct opticflow_t *opticflow,
|
||||
struct opticflow_result_t *result);
|
||||
|
||||
static struct flow_t *predict_flow_vectors(struct flow_t *flow_vectors, uint16_t n_points, float phi_diff,
|
||||
float theta_diff, float psi_diff, struct opticflow_t *opticflow);
|
||||
/**
|
||||
* Initialize the opticflow calculator
|
||||
* @param[out] *opticflow The new optical flow calculator
|
||||
@@ -475,7 +459,9 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img,
|
||||
}
|
||||
|
||||
if (opticflow->show_flow) {
|
||||
image_show_flow(img, vectors, result->tracked_cnt, opticflow->subpixel_factor);
|
||||
uint8_t color[4] = {0, 0, 0, 0};
|
||||
uint8_t bad_color[4] = {0, 0, 0, 0};
|
||||
image_show_flow_color(img, vectors, result->tracked_cnt, opticflow->subpixel_factor, color, bad_color);
|
||||
}
|
||||
|
||||
static int n_samples = 100;
|
||||
@@ -528,38 +514,80 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img,
|
||||
|
||||
// TODO scale flow to rad/s here
|
||||
|
||||
// ***************
|
||||
// Flow Derotation
|
||||
// ***************
|
||||
|
||||
float diff_flow_x = 0.f;
|
||||
float diff_flow_y = 0.f;
|
||||
|
||||
if (opticflow->derotation && result->tracked_cnt > 5) {
|
||||
diff_flow_x = (opticflow->img_gray.eulers.phi - opticflow->prev_img_gray.eulers.phi) * OPTICFLOW_FX;
|
||||
diff_flow_y = (opticflow->img_gray.eulers.theta - opticflow->prev_img_gray.eulers.theta) * OPTICFLOW_FY;
|
||||
/*diff_flow_x = (cam_state->rates.p) / result->fps * img->w /
|
||||
OPTICFLOW_FOV_W;// * img->w / OPTICFLOW_FOV_W;
|
||||
diff_flow_y = (cam_state->rates.q) / result->fps * img->h /
|
||||
OPTICFLOW_FOV_H;// * img->h / OPTICFLOW_FOV_H;*/
|
||||
}
|
||||
|
||||
float rotation_threshold = M_PI / 180.0f;
|
||||
if (fabs(opticflow->img_gray.eulers.phi - opticflow->prev_img_gray.eulers.phi) > rotation_threshold
|
||||
|| fabs(opticflow->img_gray.eulers.theta - opticflow->prev_img_gray.eulers.theta) > rotation_threshold) {
|
||||
result->flow_der_x = 0.0f;
|
||||
result->flow_der_y = 0.0f;
|
||||
} else {
|
||||
result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor *
|
||||
opticflow->derotation_correction_factor_x;
|
||||
result->flow_der_y = result->flow_y - diff_flow_y * opticflow->subpixel_factor *
|
||||
opticflow->derotation_correction_factor_y;
|
||||
float rotation_threshold = M_PI / 180.0f;
|
||||
if (fabs(opticflow->img_gray.eulers.phi - opticflow->prev_img_gray.eulers.phi) > rotation_threshold
|
||||
|| fabs(opticflow->img_gray.eulers.theta - opticflow->prev_img_gray.eulers.theta) > rotation_threshold) {
|
||||
|
||||
// do not apply the derotation if the rotation rates are too high:
|
||||
result->flow_der_x = 0.0f;
|
||||
result->flow_der_y = 0.0f;
|
||||
|
||||
} else {
|
||||
|
||||
// determine the roll, pitch, yaw differencces between the images.
|
||||
float phi_diff = opticflow->img_gray.eulers.phi - opticflow->prev_img_gray.eulers.phi;
|
||||
float theta_diff = opticflow->img_gray.eulers.theta - opticflow->prev_img_gray.eulers.theta;
|
||||
float psi_diff = opticflow->img_gray.eulers.psi - opticflow->prev_img_gray.eulers.psi;
|
||||
|
||||
if (strcmp(OPTICFLOW_CAMERA.dev_name, "/dev/video0") == 0) {
|
||||
|
||||
// bottom cam: just subtract a scaled version of the roll and pitch difference from the global flow vector:
|
||||
diff_flow_x = phi_diff * OPTICFLOW_CAMERA.camera_intrinsics.focal_x; // phi_diff works better than (cam_state->rates.p)
|
||||
diff_flow_y = theta_diff * OPTICFLOW_CAMERA.camera_intrinsics.focal_y;
|
||||
result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor *
|
||||
opticflow->derotation_correction_factor_x;
|
||||
result->flow_der_y = result->flow_y - diff_flow_y * opticflow->subpixel_factor *
|
||||
opticflow->derotation_correction_factor_y;
|
||||
} else {
|
||||
|
||||
// frontal cam, predict individual flow vectors:
|
||||
struct flow_t *predicted_flow_vectors = predict_flow_vectors(vectors, result->tracked_cnt, phi_diff, theta_diff,
|
||||
psi_diff, opticflow);
|
||||
if (opticflow->show_flow) {
|
||||
uint8_t color[4] = {255, 255, 255, 255};
|
||||
uint8_t bad_color[4] = {255, 255, 255, 255};
|
||||
image_show_flow_color(img, predicted_flow_vectors, result->tracked_cnt, opticflow->subpixel_factor, color, bad_color);
|
||||
}
|
||||
|
||||
for (int i = 0; i < result->tracked_cnt; i++) {
|
||||
// subtract the flow:
|
||||
vectors[i].flow_x -= predicted_flow_vectors[i].flow_x;
|
||||
vectors[i].flow_y -= predicted_flow_vectors[i].flow_y;
|
||||
}
|
||||
|
||||
// vectors have to be re-sorted after derotation:
|
||||
qsort(vectors, result->tracked_cnt, sizeof(struct flow_t), cmp_flow);
|
||||
|
||||
if (result->tracked_cnt % 2) {
|
||||
// Take the median point
|
||||
result->flow_der_x = vectors[result->tracked_cnt / 2].flow_x;
|
||||
result->flow_der_y = vectors[result->tracked_cnt / 2].flow_y;
|
||||
} else {
|
||||
// Take the average of the 2 median points
|
||||
result->flow_der_x = (vectors[result->tracked_cnt / 2 - 1].flow_x + vectors[result->tracked_cnt / 2].flow_x) / 2.f;
|
||||
result->flow_der_y = (vectors[result->tracked_cnt / 2 - 1].flow_y + vectors[result->tracked_cnt / 2].flow_y) / 2.f;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Velocity calculation
|
||||
// Right now this formula is under assumption that the flow only exist in the center axis of the camera.
|
||||
// TODO Calculate the velocity more sophisticated, taking into account the drone's angle and the slope of the ground plane.
|
||||
// TODO: Calculate the velocity more sophisticated, taking into account the drone's angle and the slope of the ground plane.
|
||||
// TODO: This is actually only correct for the bottom camera:
|
||||
result->vel_cam.x = (float)result->flow_der_x * result->fps * agl_dist_value_filtered /
|
||||
(opticflow->subpixel_factor * OPTICFLOW_FX);
|
||||
(opticflow->subpixel_factor * OPTICFLOW_CAMERA.camera_intrinsics.focal_x);
|
||||
result->vel_cam.y = (float)result->flow_der_y * result->fps * agl_dist_value_filtered /
|
||||
(opticflow->subpixel_factor * OPTICFLOW_FY);
|
||||
(opticflow->subpixel_factor * OPTICFLOW_CAMERA.camera_intrinsics.focal_y);
|
||||
result->vel_cam.z = result->divergence * result->fps * agl_dist_value_filtered;
|
||||
|
||||
//Apply a median filter to the velocity if wanted
|
||||
@@ -594,6 +622,75 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img,
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
* Predict flow vectors by means of the rotation rates:
|
||||
*/
|
||||
static struct flow_t *predict_flow_vectors(struct flow_t *flow_vectors, uint16_t n_points, float phi_diff,
|
||||
float theta_diff, float psi_diff, struct opticflow_t *opticflow)
|
||||
{
|
||||
|
||||
// reserve memory for the predicted flow vectors:
|
||||
struct flow_t *predicted_flow_vectors = malloc(sizeof(struct flow_t) * n_points);
|
||||
|
||||
float K[9] = {OPTICFLOW_CAMERA.camera_intrinsics.focal_x, 0.0f, OPTICFLOW_CAMERA.camera_intrinsics.center_x,
|
||||
0.0f, OPTICFLOW_CAMERA.camera_intrinsics.focal_y, OPTICFLOW_CAMERA.camera_intrinsics.center_y,
|
||||
0.0f, 0.0f, 1.0f
|
||||
};
|
||||
// TODO: make an option to not do distortion / undistortion (Dhane_k = 1)
|
||||
float k = OPTICFLOW_CAMERA.camera_intrinsics.Dhane_k;
|
||||
|
||||
float A, B, C; // as in Longuet-Higgins
|
||||
|
||||
if (strcmp(OPTICFLOW_CAMERA.dev_name, "/dev/video1") == 0) {
|
||||
// specific for the x,y swapped Bebop 2 images:
|
||||
A = -psi_diff;
|
||||
B = theta_diff;
|
||||
C = phi_diff;
|
||||
} else {
|
||||
A = theta_diff;
|
||||
B = phi_diff;
|
||||
C = psi_diff;
|
||||
}
|
||||
|
||||
float x_n, y_n;
|
||||
float x_n_new, y_n_new, x_pix_new, y_pix_new;
|
||||
float predicted_flow_x, predicted_flow_y;
|
||||
for (uint16_t i = 0; i < n_points; i++) {
|
||||
// the from-coordinate is always the same:
|
||||
predicted_flow_vectors[i].pos.x = flow_vectors[i].pos.x;
|
||||
predicted_flow_vectors[i].pos.y = flow_vectors[i].pos.y;
|
||||
|
||||
bool success = distorted_pixels_to_normalized_coords((float)flow_vectors[i].pos.x / opticflow->subpixel_factor,
|
||||
(float)flow_vectors[i].pos.y / opticflow->subpixel_factor, &x_n, &y_n, k, K);
|
||||
if (success) {
|
||||
// predict flow as in a linear pinhole camera model:
|
||||
predicted_flow_x = A * x_n * y_n - B * x_n * x_n - B + C * y_n;
|
||||
predicted_flow_y = -C * x_n + A + A * y_n * y_n - B * x_n * y_n;
|
||||
|
||||
x_n_new = x_n + predicted_flow_x;
|
||||
y_n_new = y_n + predicted_flow_y;
|
||||
|
||||
success = normalized_coords_to_distorted_pixels(x_n_new, y_n_new, &x_pix_new, &y_pix_new, k, K);
|
||||
|
||||
if (success) {
|
||||
predicted_flow_vectors[i].flow_x = (int16_t)(x_pix_new * opticflow->subpixel_factor - (float)flow_vectors[i].pos.x);
|
||||
predicted_flow_vectors[i].flow_y = (int16_t)(y_pix_new * opticflow->subpixel_factor - (float)flow_vectors[i].pos.y);
|
||||
predicted_flow_vectors[i].error = 0;
|
||||
} else {
|
||||
predicted_flow_vectors[i].flow_x = 0;
|
||||
predicted_flow_vectors[i].flow_y = 0;
|
||||
predicted_flow_vectors[i].error = LARGE_FLOW_ERROR;
|
||||
}
|
||||
} else {
|
||||
predicted_flow_vectors[i].flow_x = 0;
|
||||
predicted_flow_vectors[i].flow_y = 0;
|
||||
predicted_flow_vectors[i].error = LARGE_FLOW_ERROR;
|
||||
}
|
||||
}
|
||||
return predicted_flow_vectors;
|
||||
}
|
||||
|
||||
|
||||
/* manage_flow_features - Update list of corners to be tracked by LK
|
||||
* Remembers previous points and tries to find new points in less dense
|
||||
* areas of the image first.
|
||||
@@ -782,9 +879,9 @@ bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img,
|
||||
|
||||
if (opticflow->derotation) {
|
||||
der_shift_x = (int16_t)((edge_hist[current_frame_nr].eulers.phi - edge_hist[previous_frame_nr[0]].eulers.phi) *
|
||||
OPTICFLOW_FX * opticflow->derotation_correction_factor_x);
|
||||
OPTICFLOW_CAMERA.camera_intrinsics.focal_x * opticflow->derotation_correction_factor_x);
|
||||
der_shift_y = (int16_t)((edge_hist[current_frame_nr].eulers.theta - edge_hist[previous_frame_nr[1]].eulers.theta) *
|
||||
OPTICFLOW_FY * opticflow->derotation_correction_factor_y);
|
||||
OPTICFLOW_CAMERA.camera_intrinsics.focal_y * opticflow->derotation_correction_factor_y);
|
||||
}
|
||||
|
||||
// Estimate pixel wise displacement of the edge histograms for x and y direction
|
||||
@@ -849,8 +946,10 @@ bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img,
|
||||
// TODO scale flow to rad/s here
|
||||
|
||||
// Calculate velocity
|
||||
result->vel_cam.x = edgeflow.flow_x * fps_x * agl_dist_value_filtered * OPTICFLOW_FX / RES;
|
||||
result->vel_cam.y = edgeflow.flow_y * fps_y * agl_dist_value_filtered * OPTICFLOW_FY / RES;
|
||||
result->vel_cam.x = edgeflow.flow_x * fps_x * agl_dist_value_filtered * OPTICFLOW_CAMERA.camera_intrinsics.focal_x /
|
||||
RES;
|
||||
result->vel_cam.y = edgeflow.flow_y * fps_y * agl_dist_value_filtered * OPTICFLOW_CAMERA.camera_intrinsics.focal_y /
|
||||
RES;
|
||||
result->vel_cam.z = result->divergence * fps_x * agl_dist_value_filtered;
|
||||
|
||||
//Apply a median filter to the velocity if wanted
|
||||
|
||||
@@ -40,6 +40,17 @@ struct video_thread_t {
|
||||
struct v4l2_device *dev; ///< The V4L2 device that is used for the video stream
|
||||
};
|
||||
|
||||
// camera intrinsics: capture lens properties that determine how world points are projected to image points
|
||||
// These properties can be obtained with a camera calibration (as done in openCV)
|
||||
// The Dhane (un)distortion parameter is normally tuned by hand, for example with the undistortion module in Paparazzi
|
||||
struct camera_intrinsics_t {
|
||||
float focal_x; ///< focal length in the x-direction in pixels
|
||||
float focal_y; ///< focal length in the y-direction in pixels
|
||||
float center_x; ///< center image coordinate in the x-direction
|
||||
float center_y; ///< center image coordinate in the y-direction
|
||||
float Dhane_k; //< (un)distortion parameter for a fish-eye lens
|
||||
};
|
||||
|
||||
/** V4L2 device settings */
|
||||
struct video_config_t {
|
||||
struct img_size_t output_size; ///< Output image size
|
||||
@@ -54,8 +65,9 @@ struct video_config_t {
|
||||
struct video_thread_t thread; ///< Information about the thread this camera is running on
|
||||
struct video_listener *cv_listener; ///< The first computer vision listener in the linked list for this video device
|
||||
int fps; ///< Target FPS
|
||||
struct camera_intrinsics_t
|
||||
camera_intrinsics; ///< Intrinsics of the camera; camera calibration parameters and distortion parameter(s)
|
||||
};
|
||||
extern struct video_config_t dummy_camera;
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
Reference in New Issue
Block a user