diff --git a/conf/airframes/tudelft/bebop_course2018_orangeavoid.xml b/conf/airframes/tudelft/bebop_course2018_orangeavoid.xml index cf8ce59d15..34c08bf6bb 100644 --- a/conf/airframes/tudelft/bebop_course2018_orangeavoid.xml +++ b/conf/airframes/tudelft/bebop_course2018_orangeavoid.xml @@ -37,7 +37,7 @@ - + @@ -235,8 +235,9 @@
- + +
diff --git a/conf/modules/bebop_cam.xml b/conf/modules/bebop_cam.xml index a68818ec4f..ffdec72349 100644 --- a/conf/modules/bebop_cam.xml +++ b/conf/modules/bebop_cam.xml @@ -67,6 +67,14 @@ + + + + + + + + diff --git a/conf/simulator/gazebo/airframes/bebop.xml b/conf/simulator/gazebo/airframes/bebop.xml index 9ec25a7765..5746880783 100644 --- a/conf/simulator/gazebo/airframes/bebop.xml +++ b/conf/simulator/gazebo/airframes/bebop.xml @@ -9,6 +9,5 @@ - diff --git a/conf/simulator/gazebo/models/bebop/bebop.sdf b/conf/simulator/gazebo/models/bebop/bebop.sdf index a58d495ba6..20a92a9fc5 100644 --- a/conf/simulator/gazebo/models/bebop/bebop.sdf +++ b/conf/simulator/gazebo/models/bebop/bebop.sdf @@ -192,19 +192,20 @@ 0 - + 15.0 - + - 3746 - 3288 + 4608 + 3288 R8G8B8 - 3.00 + 3.7 equisolid_angle true - 2048 + 1.8 + 2048 0.01 @@ -240,9 +241,9 @@ 0 - - 30.0 - + + 90.0 + 0.7175 240 diff --git a/conf/simulator/gazebo/models/bebop2/bebop2.sdf b/conf/simulator/gazebo/models/bebop2/bebop2.sdf index 0ea0ceb927..87648e4309 100644 --- a/conf/simulator/gazebo/models/bebop2/bebop2.sdf +++ b/conf/simulator/gazebo/models/bebop2/bebop2.sdf @@ -218,19 +218,20 @@ 0 - + 15.0 - + - 3746 - 3288 + 4608 + 3288 R8G8B8 - 3.00 + 3.7 equisolid_angle true - 2048 + 1.8 + 2048 0.01 @@ -266,9 +267,9 @@ 0 - - 30.0 - + + 90.0 + 0.7175 240 diff --git a/conf/simulator/gazebo/models/bebop2_with_slamdunk/bebop2_with_slamdunk.sdf b/conf/simulator/gazebo/models/bebop2_with_slamdunk/bebop2_with_slamdunk.sdf index 576e8457d8..ee0e9642b7 100644 --- a/conf/simulator/gazebo/models/bebop2_with_slamdunk/bebop2_with_slamdunk.sdf +++ b/conf/simulator/gazebo/models/bebop2_with_slamdunk/bebop2_with_slamdunk.sdf @@ -229,19 +229,20 @@ 0 - + 15.0 - + - 3746 - 3288 + 4608 + 3288 R8G8B8 - 3.00 + 3.7 equisolid_angle true - 2048 + 1.8 + 2048 0.01 @@ -277,9 +278,9 @@ 0 - - 30.0 - + + 90.0 + 0.7175 240 diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index 936e0c6be1..5376b7b16c 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -460,7 +460,7 @@ telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_optitrack.xml" settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml" - settings_modules="modules/optical_flow_hover.xml modules/cv_opticflow.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" + settings_modules="modules/optical_flow_hover.xml modules/cv_opticflow.xml modules/bebop_cam.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml" gui_color="red" /> diff --git a/sw/airborne/boards/bebop.h b/sw/airborne/boards/bebop.h index bc41d2f507..cfe115646e 100644 --- a/sw/airborne/boards/bebop.h +++ b/sw/airborne/boards/bebop.h @@ -27,7 +27,6 @@ #include "std.h" #include "peripherals/video_device.h" -#include "boards/bebop/mt9f002.h" /** uart connected to GPS internally */ #define UART1_DEV /dev/ttyPA1 diff --git a/sw/airborne/boards/bebop/isp/libisp_config.h b/sw/airborne/boards/bebop/isp/libisp_config.h index eefdb9d4f7..dcaa0a2591 100644 --- a/sw/airborne/boards/bebop/isp/libisp_config.h +++ b/sw/airborne/boards/bebop/isp/libisp_config.h @@ -3,7 +3,7 @@ #define ISP_CFA 2 -#include "boards/bebop.h" +#include "boards/bebop/mt9f002.h" #define MT9F002_BAYERSTATS_STATX 64 #define MT9F002_BAYERSTATS_STATY 48 diff --git a/sw/airborne/boards/bebop/mt9f002.c b/sw/airborne/boards/bebop/mt9f002.c index c4580c2eb1..1e36d06ea5 100644 --- a/sw/airborne/boards/bebop/mt9f002.c +++ b/sw/airborne/boards/bebop/mt9f002.c @@ -29,8 +29,7 @@ #include "mt9f002_regs.h" #include "isp/libisp.h" #include "math/pprz_algebra_int.h" -#include "boards/bebop.h" -#include "generated/airframe.h" +#include "peripherals/video_device.h" #include #include @@ -47,50 +46,6 @@ #define VERBOSE_PRINT(...) #endif -#define RES_VGA 0 -#define RES_720p 1 -#define RES_720p_4_3 2 -#define RES_1080p 3 -#define RES_1080p_4_3 4 -#define RES_FULL 5 - -#ifdef MT9F002_RESOLUTION -#if MT9F002_RESOLUTION == RES_VGA -#define MT9F002_OUTPUT_WIDTH 640 -#define MT9F002_OUTPUT_HEIGHT 480 -#elif MT9F002_RESOLUTION == RES_720p -#define MT9F002_OUTPUT_WIDTH 1280 -#define MT9F002_OUTPUT_HEIGHT 720 -#elif MT9F002_RESOLUTION == RES_720p_4_3 -#define MT9F002_OUTPUT_WIDTH 960 -#define MT9F002_OUTPUT_HEIGHT 720 -#elif MT9F002_RESOLUTION == RES_1080p -#define MT9F002_OUTPUT_WIDTH 1920 -#define MT9F002_OUTPUT_HEIGHT 1080 -#elif MT9F002_RESOLUTION == RES_1080p_4_3 -#define MT9F002_OUTPUT_WIDTH 1440 -#define MT9F002_OUTPUT_HEIGHT 1080 -#elif MT9F002_RESOLUTION == RES_FULL -// Doesn't work with isp -//#define MT9F002_OUTPUT_WIDTH 4384 -//#define MT9F002_OUTPUT_HEIGHT 3288 -#define MT9F002_OUTPUT_WIDTH 2048 -#define MT9F002_OUTPUT_HEIGHT 2048 -#else // default MT9F002_RESOLUTION -#define MT9F002_OUTPUT_WIDTH 640 -#define MT9F002_OUTPUT_HEIGHT 640 -#endif - -#else // MT9F002_RESOLUTION - -#ifndef MT9F002_OUTPUT_WIDTH -#define MT9F002_OUTPUT_WIDTH 640 -#endif -#ifndef MT9F002_OUTPUT_HEIGHT -#define MT9F002_OUTPUT_HEIGHT 640 -#endif -#endif - // The sequencing of the pixel array is controlled by the x_addr_start, y_addr_start, // x_addr_end, and y_addr_end registers. For both parallel and serial HiSPi interfaces, the // output image size is controlled by the x_output_size and y_output_size registers. @@ -98,40 +53,6 @@ // Horizontal Mirror // Vertical Flip -// Signed fractional offset from centre of image of original sensor [-0.5,0.5] -#ifndef MT9F002_OFFSET_X -#define MT9F002_OFFSET_X 0. -#endif - -// Signed fractional offset from centre of image of original sensor [-0.5,0.5] -#ifndef MT9F002_OFFSET_Y -#define MT9F002_OFFSET_Y 0. -#endif - -// Zoom factor of image -#ifndef MT9F002_ZOOM -#define MT9F002_ZOOM 1. -#endif - -/** Our output is only OUTPUT_SCALER of the pixels we take of the sensor - * It is programmable in 1/16 steps determined by ScaleFactor = 16/scale_m. - * Legal values for scale_m are 16 through 128, giving you the ability to scale from - * 1:1 to 1:8 (with m=128). - * Example: - * output_width = 512 - * output_height = 830 - * output_scaler = 0.25 - * We now get an image of 512 by 830 which contains a "compressed version" - * of what would normally be an image of 2048 by 3320. - * Be warned: set your offset x appropriately. - * Example of what could go wrong: - * output_width = 512 - * output_height = 830 - * output_scaler = 0.25 - * offset_x = 1500 - * We now ask for pixels outside the 4608H x 2592V sensor or the 3320H x 2048W of the ISP. - */ - /** Exposure of the front camera of the bebop. Experimental values: * Outside: 15 * Inside well lit: 30 @@ -141,10 +62,6 @@ #define MT9F002_TARGET_EXPOSURE 30 #endif -#ifndef MT9F002_TARGET_FPS -#define MT9F002_TARGET_FPS 30 -#endif - /* Set the colour balance gains */ #ifndef MT9F002_GAIN_GREEN1 #define MT9F002_GAIN_GREEN1 2.0 @@ -162,23 +79,6 @@ #define MT9F002_GAIN_BLUE 2.7 #endif -// parameters for undistortion, defaults are rough estimates -#ifndef MT9F002_FOCAL_X -#define MT9F002_FOCAL_X (MT9F002_ZOOM * MT9F002_OUTPUT_WIDTH / 2.f) -#endif -#ifndef MT9F002_FOCAL_Y -#define MT9F002_FOCAL_Y (MT9F002_ZOOM * MT9F002_OUTPUT_HEIGHT / 2.f) -#endif -#ifndef MT9F002_CENTER_X -#define MT9F002_CENTER_X (MT9F002_OUTPUT_WIDTH * (.5f - MT9F002_ZOOM * MT9F002_OFFSET_X)) -#endif -#ifndef MT9F002_CENTER_Y -#define MT9F002_CENTER_Y (MT9F002_OUTPUT_HEIGHT * (.5f - MT9F002_ZOOM * MT9F002_OFFSET_Y)) -#endif -#ifndef MT9F002_DHANE_K -#define MT9F002_DHANE_K 1.25f -#endif - /* Camera structure */ struct video_config_t front_camera = { .output_size = { diff --git a/sw/airborne/boards/bebop/mt9f002.h b/sw/airborne/boards/bebop/mt9f002.h index 002a2229f0..dedce1b696 100644 --- a/sw/airborne/boards/bebop/mt9f002.h +++ b/sw/airborne/boards/bebop/mt9f002.h @@ -29,6 +29,7 @@ #include "std.h" #include "mcu_periph/i2c.h" +#include "generated/airframe.h" #define CFG_SCALER_M_MIN 16 #define CFG_SCALER_M_MAX 128 @@ -41,6 +42,86 @@ #define CFG_MT9F002_Y_ADDR_MIN 0 #define CFG_MT9F002_Y_ADDR_MAX CFG_MT9F002_PIXEL_ARRAY_HEIGHT +#define RES_VGA 0 +#define RES_720p 1 +#define RES_720p_4_3 2 +#define RES_1080p 3 +#define RES_1080p_4_3 4 +#define RES_FULL 5 + +#ifdef MT9F002_RESOLUTION +#if MT9F002_RESOLUTION == RES_VGA +#define MT9F002_OUTPUT_WIDTH 640 +#define MT9F002_OUTPUT_HEIGHT 480 +#elif MT9F002_RESOLUTION == RES_720p +#define MT9F002_OUTPUT_WIDTH 1280 +#define MT9F002_OUTPUT_HEIGHT 720 +#elif MT9F002_RESOLUTION == RES_720p_4_3 +#define MT9F002_OUTPUT_WIDTH 960 +#define MT9F002_OUTPUT_HEIGHT 720 +#elif MT9F002_RESOLUTION == RES_1080p +#define MT9F002_OUTPUT_WIDTH 1920 +#define MT9F002_OUTPUT_HEIGHT 1080 +#elif MT9F002_RESOLUTION == RES_1080p_4_3 +#define MT9F002_OUTPUT_WIDTH 1440 +#define MT9F002_OUTPUT_HEIGHT 1080 +#elif MT9F002_RESOLUTION == RES_FULL +// Doesn't work with isp +//#define MT9F002_OUTPUT_WIDTH 4384 +//#define MT9F002_OUTPUT_HEIGHT 3288 +#define MT9F002_OUTPUT_WIDTH 2048 +#define MT9F002_OUTPUT_HEIGHT 2048 +#else // default MT9F002_RESOLUTION +#define MT9F002_OUTPUT_WIDTH 640 +#define MT9F002_OUTPUT_HEIGHT 640 +#endif + +#else // MT9F002_RESOLUTION + +#ifndef MT9F002_OUTPUT_WIDTH +#define MT9F002_OUTPUT_WIDTH 640 +#endif +#ifndef MT9F002_OUTPUT_HEIGHT +#define MT9F002_OUTPUT_HEIGHT 640 +#endif +#endif + +// Signed fractional offset from centre of image of original sensor [-0.5,0.5] +#ifndef MT9F002_OFFSET_X +#define MT9F002_OFFSET_X 0. +#endif + +// Signed fractional offset from centre of image of original sensor [-0.5,0.5] +#ifndef MT9F002_OFFSET_Y +#define MT9F002_OFFSET_Y 0. +#endif + +// Zoom factor of image +#ifndef MT9F002_ZOOM +#define MT9F002_ZOOM 1. +#endif + +#ifndef MT9F002_TARGET_FPS +#define MT9F002_TARGET_FPS 30 +#endif + +// parameters for undistortion, defaults are rough estimates +#ifndef MT9F002_FOCAL_X +#define MT9F002_FOCAL_X (MT9F002_ZOOM * MT9F002_OUTPUT_WIDTH / 2.f) +#endif +#ifndef MT9F002_FOCAL_Y +#define MT9F002_FOCAL_Y (MT9F002_ZOOM * MT9F002_OUTPUT_HEIGHT / 2.f) +#endif +#ifndef MT9F002_CENTER_X +#define MT9F002_CENTER_X (MT9F002_OUTPUT_WIDTH * (.5f - MT9F002_ZOOM * MT9F002_OFFSET_X)) +#endif +#ifndef MT9F002_CENTER_Y +#define MT9F002_CENTER_Y (MT9F002_OUTPUT_HEIGHT * (.5f - MT9F002_ZOOM * MT9F002_OFFSET_Y)) +#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 diff --git a/sw/airborne/boards/bebop/mt9f002_nps.c b/sw/airborne/boards/bebop/mt9f002_nps.c new file mode 100644 index 0000000000..1c1dc5d02a --- /dev/null +++ b/sw/airborne/boards/bebop/mt9f002_nps.c @@ -0,0 +1,8 @@ +// NPS wrapper around mt9f002.c + +#define front_camera mt9f002_front_camera // Do not overwrite NPS camera struct + +#pragma GCC diagnostic ignored "-Wsign-compare" +#pragma GCC diagnostic ignored "-Wunused-parameter" +#include "mt9f002.c" +#pragma GCC diagnostic pop diff --git a/sw/airborne/boards/bebop/mt9v117.c b/sw/airborne/boards/bebop/mt9v117.c index 21d3eab8fd..f77c912024 100644 --- a/sw/airborne/boards/bebop/mt9v117.c +++ b/sw/airborne/boards/bebop/mt9v117.c @@ -27,8 +27,7 @@ #include "std.h" #include "mt9v117.h" #include "mt9v117_regs.h" -#include "boards/bebop.h" -#include "generated/airframe.h" +#include "peripherals/video_device.h" #include #include @@ -38,27 +37,6 @@ #include #include -#ifndef MT9V117_TARGET_FPS -#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 - /* Camera structure */ struct video_config_t bottom_camera = { .output_size = { @@ -459,6 +437,9 @@ void mt9v117_init(struct mt9v117_t *mt) read_var(mt, MT9V117_CAM_CTRL_VAR, MT9V117_CAM_OUTPUT_FORMAT_OFFSET, 2) | MT9V117_CAM_OUTPUT_FORMAT_BT656_ENABLE, 2); + /* Set autoexposure luma */ + write_var(mt, MT9V117_CAM_CTRL_VAR, MT9V117_AE_LUMA, MT9V117_TARGET_LUMA, 2); + /* Apply the configuration */ write_var(mt, MT9V117_SYSMGR_VAR, MT9V117_SYSMGR_NEXT_STATE_OFFSET, MT9V117_SYS_STATE_ENTER_CONFIG_CHANGE, 1); write_reg(mt, MT9V117_COMMAND, MT9V117_COMMAND_OK | MT9V117_COMMAND_SET_STATE, 2); diff --git a/sw/airborne/boards/bebop/mt9v117.h b/sw/airborne/boards/bebop/mt9v117.h index ba2062fea7..96718f84d0 100644 --- a/sw/airborne/boards/bebop/mt9v117.h +++ b/sw/airborne/boards/bebop/mt9v117.h @@ -29,6 +29,31 @@ #include "std.h" #include "mcu_periph/i2c.h" +//#include "generated/airframe.h" + +#ifndef MT9V117_TARGET_FPS +#define MT9V117_TARGET_FPS 0 +#endif + +// parameters for undistortion, defaults are rough estimates +#ifndef MT9V117_FOCAL_X +#define MT9V117_FOCAL_X 349.f +#endif +#ifndef MT9V117_FOCAL_Y +#define MT9V117_FOCAL_Y 349.f +#endif +#ifndef MT9V117_CENTER_X +#define MT9V117_CENTER_X 120.f +#endif +#ifndef MT9V117_CENTER_Y +#define MT9V117_CENTER_Y 120.f +#endif +#ifndef MT9V117_DHANE_K +#define MT9V117_DHANE_K 1.f +#endif +#ifndef MT9V117_TARGET_LUMA +#define MT9V117_TARGET_LUMA 18000 +#endif struct mt9v117_t { struct i2c_periph *i2c_periph; ///< I2C peripheral used to communicate over diff --git a/sw/airborne/boards/bebop/mt9v117_nps.c b/sw/airborne/boards/bebop/mt9v117_nps.c new file mode 100644 index 0000000000..ee009b0e3f --- /dev/null +++ b/sw/airborne/boards/bebop/mt9v117_nps.c @@ -0,0 +1,8 @@ +// NPS wrapper for mt9v117.c + +#define bottom_camera mt9v117_bottom_camera // Do not overwrite NPS camera struct + +#pragma GCC diagnostic ignored "-Wsign-compare" +#pragma GCC diagnostic ignored "-Wunused-parameter" +#include "mt9v117.c" +#pragma GCC diagnostic pop diff --git a/sw/airborne/boards/bebop/mt9v117_regs.h b/sw/airborne/boards/bebop/mt9v117_regs.h index 42920c7cc9..eb29ba8e54 100644 --- a/sw/airborne/boards/bebop/mt9v117_regs.h +++ b/sw/airborne/boards/bebop/mt9v117_regs.h @@ -20,6 +20,7 @@ #define MT9V117_LOGICAL_ADDRESS_ACCESS 0x098E #define MT9V117_AE_TRACK_JUMP_DIVISOR 0xA812 #define MT9V117_CAM_AET_SKIP_FRAMES 0xC868 +#define MT9V117_AE_LUMA 0x0069 /* Variables */ #define MT9V117_AE_RULE_VAR 9 diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.c b/sw/airborne/modules/computer_vision/lib/vision/image.c index 30144d089f..d649609483 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/image.c +++ b/sw/airborne/modules/computer_vision/lib/vision/image.c @@ -736,8 +736,8 @@ void image_show_flow_color(struct image_t *img, struct flow_t *vectors, uint16_t .y = vectors[i].pos.y / subpixel_factor }; struct point_t to = { - .x = (vectors[i].pos.x + vectors[i].flow_x) / subpixel_factor, - .y = (vectors[i].pos.y + vectors[i].flow_y) / subpixel_factor + .x = (uint32_t)roundf(((float)vectors[i].pos.x + vectors[i].flow_x) / subpixel_factor), + .y = (uint32_t)roundf(((float)vectors[i].pos.y + vectors[i].flow_y) / subpixel_factor) }; if (vectors[i].error >= LARGE_FLOW_ERROR) { diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.h b/sw/airborne/modules/computer_vision/lib/vision/image.h index 397ef9169a..b7ce2fc141 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/image.h +++ b/sw/airborne/modules/computer_vision/lib/vision/image.h @@ -64,10 +64,10 @@ struct point_t { /* Vector structure for point differences */ struct flow_t { - struct point_t pos; ///< The original position the flow comes from - int16_t flow_x; ///< The x direction flow in subpixels - int16_t flow_y; ///< The y direction flow in subpixels - uint32_t error; ///< The matching error in the tracking process + struct point_t pos; ///< The original position the flow comes from in subpixels + int32_t flow_x; ///< The x direction flow in subpixels + int32_t flow_y; ///< The y direction flow in subpixels + uint32_t error; ///< The matching error in the tracking process in subpixels }; /* Image size structure */ diff --git a/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c index 4587e6ae20..1d1ec496f9 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c +++ b/sw/airborne/modules/computer_vision/lib/vision/lucas_kanade.c @@ -53,6 +53,7 @@ * @param[in] step_threshold The threshold of additional subpixel flow at which the iterations should stop * @param[in] max_points The maximum amount of points to track, we skip x points and then take a point. * @param[in] pyramid_level Level of pyramid used in computation (0 == no pyramids used) + * @param[in] keep_bad_points Do not filter out bad points. The error field will be set accordingly. * @return The vectors from the original *points in subpixels * * Pyramidal implementation of Lucas-Kanade feature tracker. @@ -84,7 +85,7 @@ struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, str } // Allocate some memory for returning the vectors - struct flow_t *vectors = malloc(sizeof(struct flow_t) * max_points); + struct flow_t *vectors = calloc(max_points, sizeof(struct flow_t)); // Determine patch sizes and initialize neighborhoods uint16_t patch_size = 2 * half_window_size + 1; @@ -144,6 +145,11 @@ struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, str || (((int32_t) vectors[new_p].pos.y + vectors[new_p].flow_y) < 0) || ((vectors[new_p].pos.y + vectors[new_p].flow_y) > (uint32_t)((pyramid_new[LVL].h - 1 - 2 * border_size)* subpixel_factor))) { + if (keep_bad_points) { + vectors[new_p].error = LARGE_FLOW_ERROR; + new_p++; + (*points_cnt)++; + } continue; } @@ -163,6 +169,11 @@ struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, str // Check if the determinant is bigger than 1 if (Det < 1) { + if (keep_bad_points) { + vectors[new_p].error = LARGE_FLOW_ERROR; + new_p++; + (*points_cnt)++; + } continue; } @@ -218,8 +229,6 @@ struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, str new_p++; (*points_cnt)++; } else if (keep_bad_points) { - vectors[new_p].pos.x = 0; - vectors[new_p].pos.y = 0; vectors[new_p].flow_x = 0; vectors[new_p].flow_y = 0; vectors[new_p].error = LARGE_FLOW_ERROR; @@ -263,8 +272,7 @@ struct flow_t *opticFlowLK(struct image_t *new_img, struct image_t *old_img, str * @param[in] max_point The maximum amount of points to track, we skip x points and then take a point. * @return The vectors from the original *points in subpixels */ -struct flow_t *opticFlowLK_flat(struct image_t *new_img, struct image_t *old_img, struct point_t *points, - uint16_t *points_cnt, +struct flow_t *opticFlowLK_flat(struct image_t *new_img, struct image_t *old_img, struct point_t *points, uint16_t *points_cnt, uint16_t half_window_size, uint16_t subpixel_factor, uint8_t max_iterations, uint8_t step_threshold, uint16_t max_points, uint8_t keep_bad_points) { @@ -280,7 +288,7 @@ struct flow_t *opticFlowLK_flat(struct image_t *new_img, struct image_t *old_img // [d] calculate the additional flow step and possibly terminate the iteration // Allocate some memory for returning the vectors - struct flow_t *vectors = malloc(sizeof(struct flow_t) * max_points); + struct flow_t *vectors = calloc(max_points, sizeof(struct flow_t)); uint16_t new_p = 0; uint16_t points_orig = *points_cnt; *points_cnt = 0; @@ -299,23 +307,26 @@ struct flow_t *opticFlowLK_flat(struct image_t *new_img, struct image_t *old_img image_create(&window_diff, patch_size, patch_size, IMAGE_GRADIENT); // Calculate the amount of points to skip - float skip_points = (points_orig > max_points) ? points_orig / max_points : 1; + float skip_points = (points_orig > max_points) ? (float)points_orig / max_points : 1; // Go through all points for (uint16_t i = 0; i < max_points && i < points_orig; i++) { uint16_t p = i * skip_points; - // If the pixel is outside ROI, do not track it - if (points[p].x < half_window_size || (old_img->w - points[p].x) < half_window_size - || points[p].y < half_window_size || (old_img->h - points[p].y) < half_window_size) { - continue; - } - // Convert the point to a subpixel coordinate vectors[new_p].pos.x = points[p].x * subpixel_factor; vectors[new_p].pos.y = points[p].y * subpixel_factor; - vectors[new_p].flow_x = 0; - vectors[new_p].flow_y = 0; + + // If the pixel is outside ROI, do not track it + if (points[p].x < half_window_size || (old_img->w - points[p].x) < half_window_size + || points[p].y < half_window_size || (old_img->h - points[p].y) < half_window_size) { + if (keep_bad_points) { + vectors[new_p].error = LARGE_FLOW_ERROR; + new_p++; + (*points_cnt)++; + } + continue; + } // (1) determine the subpixel neighborhood in the old image image_subpixel_window(old_img, &window_I, &vectors[new_p].pos, subpixel_factor, 0); @@ -332,6 +343,11 @@ struct flow_t *opticFlowLK_flat(struct image_t *new_img, struct image_t *old_img // Check if the determinant is bigger than 1 if (Det < 1) { + if (keep_bad_points) { + vectors[new_p].error = LARGE_FLOW_ERROR; + new_p++; + (*points_cnt)++; + } continue; } @@ -376,7 +392,7 @@ struct flow_t *opticFlowLK_flat(struct image_t *new_img, struct image_t *old_img vectors[new_p].flow_y += step_y; vectors[new_p].error = error; - // Check if we exceeded the treshold + // Check if we exceeded the threshold if ((abs(step_x) + abs(step_y)) < step_threshold) { break; } @@ -387,8 +403,6 @@ struct flow_t *opticFlowLK_flat(struct image_t *new_img, struct image_t *old_img new_p++; (*points_cnt)++; } else if (keep_bad_points) { - vectors[new_p].pos.x = 0; - vectors[new_p].pos.y = 0; vectors[new_p].flow_x = 0; vectors[new_p].flow_y = 0; vectors[new_p].error = LARGE_FLOW_ERROR; diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c index e5742fc438..9638b7d488 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c @@ -542,8 +542,7 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, 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) { - + if (strcmp(OPTICFLOW_CAMERA.dev_name, bottom_camera.dev_name) == 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; @@ -552,7 +551,6 @@ bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, 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); @@ -645,7 +643,7 @@ static struct flow_t *predict_flow_vectors(struct flow_t *flow_vectors, uint16_t float A, B, C; // as in Longuet-Higgins - if (strcmp(OPTICFLOW_CAMERA.dev_name, "/dev/video1") == 0) { + if (strcmp(OPTICFLOW_CAMERA.dev_name, front_camera.dev_name) == 0) { // specific for the x,y swapped Bebop 2 images: A = -psi_diff; B = theta_diff; diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h index 38276f060a..38f9363076 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h @@ -62,13 +62,13 @@ struct opticflow_t { uint8_t threshold_vec; ///< The threshold in x, y subpixels which the algorithm should stop uint8_t pyramid_level; ///< Number of pyramid levels used in Lucas Kanade algorithm (0 == no pyramids used) - uint8_t max_track_corners; ///< Maximum amount of corners Lucas Kanade should track + uint16_t max_track_corners; ///< Maximum amount of corners Lucas Kanade should track bool fast9_adaptive; ///< Whether the FAST9 threshold should be adaptive uint8_t fast9_threshold; ///< FAST9 corner detection threshold uint16_t fast9_min_distance; ///< Minimum distance in pixels between corners uint16_t fast9_padding; ///< Padding used in FAST9 detector - uint16_t fast9_rsize; ///< Amount of corners allocated + uint16_t fast9_rsize; ///< Amount of corners allocated struct point_t *fast9_ret_corners; ///< Corners bool feature_management; ///< Decides whether to keep track corners in memory for the next frame instead of re-detecting every time bool fast9_region_detect; ///< Decides whether to detect fast9 corners in specific regions of interest or the whole image (only for feature management) @@ -84,16 +84,16 @@ struct opticflow_t { #define FAST9_MAX_CORNERS 512 -void opticflow_calc_init(struct opticflow_t *opticflow); -bool opticflow_calc_frame(struct opticflow_t *opticflow, struct image_t *img, +extern void opticflow_calc_init(struct opticflow_t *opticflow); +extern bool opticflow_calc_frame(struct opticflow_t *opticflow, struct image_t *img, struct opticflow_result_t *result); -bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, +extern bool calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct image_t *img, struct opticflow_result_t *result); -bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img, +extern bool calc_edgeflow_tot(struct opticflow_t *opticflow, struct image_t *img, struct opticflow_result_t *result); -void kalman_filter_opticflow_velocity(float *velocity_x, float *velocity_y, float *acceleration_measurement, float fps, +extern void kalman_filter_opticflow_velocity(float *velocity_x, float *velocity_y, float *acceleration_measurement, float fps, float *measurement_noise, float process_noise, bool reinitialize_kalman); #endif /* OPTICFLOW_CALCULATOR_H */ diff --git a/sw/airborne/modules/computer_vision/opticflow/size_divergence.c b/sw/airborne/modules/computer_vision/opticflow/size_divergence.c index e089bec901..a8945512cf 100644 --- a/sw/airborne/modules/computer_vision/opticflow/size_divergence.c +++ b/sw/airborne/modules/computer_vision/opticflow/size_divergence.c @@ -32,10 +32,6 @@ #include "size_divergence.h" #include -#include "math/pprz_stat.h" - -#define NO_DIV 0.0 - /** * Get divergence from optical flow vectors based on line sizes between corners * @param[in] vectors The optical flow vectors @@ -46,7 +42,7 @@ float get_size_divergence(struct flow_t *vectors, int count, int n_samples) { float distance_1, distance_2; - float *divs; // divs will contain the individual divergence estimates: + float divs_sum = 0.f; uint32_t used_samples = 0; float dx, dy; int32_t i, j; @@ -54,14 +50,12 @@ float get_size_divergence(struct flow_t *vectors, int count, int n_samples) int32_t max_samples = (count * count - count) / 2; if (count < 2) { - return NO_DIV; + return 0.f; } else if (count >= max_samples) { n_samples = 0; } if (n_samples == 0) { - divs = (float *) malloc(sizeof(float) * max_samples); - // go through all possible lines: for (i = 0; i < count; i++) { for (j = i + 1; j < count; j++) { @@ -70,21 +64,20 @@ float get_size_divergence(struct flow_t *vectors, int count, int n_samples) dy = (float)vectors[i].pos.y - (float)vectors[j].pos.y; distance_1 = sqrtf(dx * dx + dy * dy); + if (distance_1 < 1E-5) { + continue; + } + // distance in current image: dx = (float)vectors[i].pos.x + (float)vectors[i].flow_x - (float)vectors[j].pos.x - (float)vectors[j].flow_x; dy = (float)vectors[i].pos.y + (float)vectors[i].flow_y - (float)vectors[j].pos.y - (float)vectors[j].flow_y; distance_2 = sqrtf(dx * dx + dy * dy); - // calculate divergence for this sample: - if (distance_1 > 1E-5) { - divs[used_samples] = (distance_2 - distance_1) / distance_1; - used_samples++; - } + divs_sum += (distance_2 - distance_1) / distance_1; + used_samples++; } } } else { - divs = (float *) malloc(sizeof(float) * n_samples); - // take random samples: for (uint16_t sample = 0; sample < n_samples; sample++) { // take two random indices: @@ -98,28 +91,26 @@ float get_size_divergence(struct flow_t *vectors, int count, int n_samples) // distance in previous image: dx = (float)vectors[i].pos.x - (float)vectors[j].pos.x; dy = (float)vectors[i].pos.y - (float)vectors[j].pos.y; - distance_1 = sqrt(dx * dx + dy * dy); + distance_1 = sqrtf(dx * dx + dy * dy); + + if (distance_1 < 1E-5) { + continue; + } // distance in current image: dx = (float)vectors[i].pos.x + (float)vectors[i].flow_x - (float)vectors[j].pos.x - (float)vectors[j].flow_x; dy = (float)vectors[i].pos.y + (float)vectors[i].flow_y - (float)vectors[j].pos.y - (float)vectors[j].flow_y; - distance_2 = sqrt(dx * dx + dy * dy); + distance_2 = sqrtf(dx * dx + dy * dy); - - // calculate divergence for this sample: - if (distance_1 > 1E-5) { - divs[used_samples] = (distance_2 - distance_1) / distance_1; - used_samples++; - } + divs_sum += (distance_2 - distance_1) / distance_1; + used_samples++; } } - // calculate the mean divergence: - float mean_divergence = mean_f(divs, used_samples); + if (used_samples < 1){ + return 0.f; + } - // free the memory of divs: - free(divs); - - // return the calculated divergence: - return mean_divergence; + // return the calculated mean divergence: + return divs_sum / used_samples; } diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 7d677e29c7..a6cce7e1bf 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -151,14 +151,12 @@ struct image_t *opticflow_module_calc(struct image_t *img) // Do the optical flow calculation static struct opticflow_result_t temp_result; // static so that the number of corners is kept between frames - bool flow_successful = opticflow_calc_frame(&opticflow, img, &temp_result); - - // Copy the result if finished - pthread_mutex_lock(&opticflow_mutex); - opticflow_result = temp_result; - opticflow_got_result = flow_successful; - - // release the mutex as we are done with editing the opticflow result - pthread_mutex_unlock(&opticflow_mutex); + if(opticflow_calc_frame(&opticflow, img, &temp_result)){ + // Copy the result if finished + pthread_mutex_lock(&opticflow_mutex); + opticflow_result = temp_result; + opticflow_got_result = true; + pthread_mutex_unlock(&opticflow_mutex); + } return img; } diff --git a/sw/airborne/modules/computer_vision/video_thread_nps.c b/sw/airborne/modules/computer_vision/video_thread_nps.c index 438dee5e1b..78fda914a4 100644 --- a/sw/airborne/modules/computer_vision/video_thread_nps.c +++ b/sw/airborne/modules/computer_vision/video_thread_nps.c @@ -50,20 +50,34 @@ struct video_config_t front_camera = { .buf_cnt = 10, .filters = 0, .cv_listener = NULL, - .fps = 0 + .fps = 0, + .camera_intrinsics = { + .focal_x = 300, + .focal_y = 300, + .center_x = 1280 / 2, + .center_y = 720 / 2, + .Dhane_k = 1 + } }; struct video_config_t bottom_camera = { - .output_size = { .w = 320, .h = 240 }, + .output_size = { .w = 240, .h = 240 }, .sensor_size = { .w = 320, .h = 240 }, - .crop = { .x = 0, .y = 0, .w = 320, .h = 240 }, + .crop = { .x = 40, .y = 0, .w = 240, .h = 240 }, .dev_name = "bottom_camera", .subdev_name = NULL, .format = V4L2_PIX_FMT_UYVY, .buf_cnt = 10, .filters = 0, .cv_listener = NULL, - .fps = 0 + .fps = 0, + .camera_intrinsics = { + .focal_x = 350, + .focal_y = 350, + .center_x = 240 / 2, + .center_y = 240 / 2, + .Dhane_k = 1 + } }; // Keep track of added devices. diff --git a/sw/simulator/nps/nps_fdm_gazebo.cpp b/sw/simulator/nps/nps_fdm_gazebo.cpp index 4a0b259c3e..5afe420ae9 100644 --- a/sw/simulator/nps/nps_fdm_gazebo.cpp +++ b/sw/simulator/nps/nps_fdm_gazebo.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Tom van Dijk, Kirk Scheeper + * Copyright (C) 2017 Tom van Dijk, Kirk Scheper * * This file is part of paparazzi. * @@ -82,6 +82,9 @@ extern "C" { #include "modules/computer_vision/video_thread_nps.h" #include "modules/computer_vision/lib/vision/image.h" #include "mcu_periph/sys_time.h" +#include "boards/bebop/mt9f002.h" +#include "boards/bebop/mt9v117.h" +struct mt9f002_t mt9f002 __attribute__((weak)); // Prevent undefined reference errors when Bebop code is not linked. } static void init_gazebo_video(void); @@ -95,8 +98,12 @@ struct gazebocam_t { }; static struct gazebocam_t gazebo_cams[VIDEO_THREAD_MAX_CAMERAS] = { { NULL, 0 } }; -#if NPS_SIMULATE_MT9F002 -#include "boards/bebop/mt9f002.h" + +// Reduce resolution of the simulated MT9F002 sensor (Bebop) to improve runtime +// performance at the cost of image resolution. +// Recommended values: 1 (realistic), 2, 4 (fast but slightly blurry) +#ifndef NPS_MT9F002_SENSOR_RES_DIVIDER +#define NPS_MT9F002_SENSOR_RES_DIVIDER 1 #endif #endif // NPS_SIMULATE_VIDEO @@ -369,6 +376,33 @@ static void init_gazebo(void) } // add or set up sensors before the vehicle gets loaded +#if NPS_SIMULATE_VIDEO + // Cameras + sdf::ElementPtr link = vehicle_sdf->Root()->GetFirstElement()->GetElement("link"); + while (link) { + if (link->Get("name") == "front_camera" && link->GetElement("sensor")->Get("name") == "mt9f002") { + if (NPS_MT9F002_SENSOR_RES_DIVIDER != 1) { + int w = link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("width")->Get(); + int h = link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("height")->Get(); + int env = link->GetElement("sensor")->GetElement("camera")->GetElement("lens")->GetElement("env_texture_size")->Get(); + link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("width")->Set(w / NPS_MT9F002_SENSOR_RES_DIVIDER); + link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("height")->Set(h / NPS_MT9F002_SENSOR_RES_DIVIDER); + link->GetElement("sensor")->GetElement("camera")->GetElement("lens")->GetElement("env_texture_size")->Set(env / NPS_MT9F002_SENSOR_RES_DIVIDER); + } + if (MT9F002_TARGET_FPS){ + int fps = Min(MT9F002_TARGET_FPS, link->GetElement("sensor")->GetElement("update_rate")->Get()); + link->GetElement("sensor")->GetElement("update_rate")->Set(fps); + } + } else if (link->Get("name") == "bottom_camera" && link->GetElement("sensor")->Get("name") == "mt9v117") { + if (MT9V117_TARGET_FPS){ + int fps = Min(MT9V117_TARGET_FPS, link->GetElement("sensor")->GetElement("update_rate")->Get()); + link->GetElement("sensor")->GetElement("update_rate")->Set(fps); + } + } + link = link->GetNextElement("link"); + } +#endif + // laser range array #if NPS_SIMULATE_LASER_RANGE_ARRAY vehicle_sdf->Root()->GetFirstElement()->AddElement("include")->GetElement("uri")->Set("model://range_sensors"); @@ -378,28 +412,6 @@ static void init_gazebo(void) range_joint->GetElement("parent")->Set("chassis"); range_joint->GetElement("child")->Set("range_sensors::base"); #endif - // bebop front camera -#ifdef NPS_SIMULATE_MT9F002 - sdf::ElementPtr link = vehicle_sdf->Root()->GetFirstElement()->GetElement("link"); - while (link) { - if (link->Get("name") == "front_camera") { - int w = link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("width")->Get(); - link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("width")->Set( - w * MT9F002_OUTPUT_SCALER); - int h = link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("height")->Get(); - link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("height")->Set( - h * MT9F002_OUTPUT_SCALER); - int env = link->GetElement("sensor")->GetElement("camera")->GetElement("lens")->GetElement("env_texture_size")->Get(); - link->GetElement("sensor")->GetElement("camera")->GetElement("lens")->GetElement("env_texture_size")->Set( - env * MT9F002_OUTPUT_SCALER); - cout << "Applied MT9F002_OUTPUT_SCALER (=" << MT9F002_OUTPUT_SCALER << ") to " << link->Get("name") << endl; - link->GetElement("sensor")->GetElement("update_rate")->Set(MT9F002_TARGET_FPS); - cout << "Applied MT9F002_TARGET_FPS (=" << MT9F002_TARGET_FPS << ") to " << link->Get("name") << endl; - } - link = link->GetNextElement("link"); - } -#endif // NPS_SIMULATE_MT9F002 - // get world string world_uri = "world://" + string(NPS_GAZEBO_WORLD); @@ -452,14 +464,14 @@ static void init_gazebo(void) cout << "Found sonar" << endl; } - gazebo::physics::LinkPtr link = model->GetLink("sonar"); - if (link) { + gazebo::physics::LinkPtr sonar_link = model->GetLink("sonar"); + if (sonar_link) { // Get a pointer to the sensor using its full name - if (link->GetSensorCount() != 1) { - cout << "ERROR: Link '" << link->GetName() + if (sonar_link->GetSensorCount() != 1) { + cout << "ERROR: Link '" << sonar_link->GetName() << "' should only contain 1 sensor!" << endl; } else { - string name = link->GetSensorName(0); + string name = sonar_link->GetSensorName(0); sonar = static_pointer_cast< gazebo::sensors::SonarSensor > (mgr->GetSensor(name)); if (!sonar) { cout << "ERROR: Could not get pointer to '" << name << "'!" << endl; @@ -669,8 +681,7 @@ static void gazebo_write(double act_commands[], int commands_nb) */ static void init_gazebo_video(void) { - gazebo::sensors::SensorManager *mgr = - gazebo::sensors::SensorManager::Instance(); + gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance(); cout << "Initializing cameras..." << endl; // Loop over cameras registered in video_thread_nps @@ -699,9 +710,12 @@ static void init_gazebo_video(void) } // Activate sensor cam->SetActive(true); + // Add to list of cameras gazebo_cams[i].cam = cam; gazebo_cams[i].last_measurement_time = cam->LastMeasurementTime(); + + // set default camera settings // Copy video_config settings from Gazebo's camera cameras[i]->output_size.w = cam->ImageWidth(); cameras[i]->output_size.h = cam->ImageHeight(); @@ -709,19 +723,21 @@ static void init_gazebo_video(void) cameras[i]->sensor_size.h = cam->ImageHeight(); cameras[i]->crop.w = cam->ImageWidth(); cameras[i]->crop.h = cam->ImageHeight(); + cameras[i]->fps = 0; cameras[i]->camera_intrinsics.focal_x = cameras[i]->output_size.w / 2.0f; cameras[i]->camera_intrinsics.center_x = cameras[i]->output_size.w / 2.0f; cameras[i]->camera_intrinsics.focal_y = cameras[i]->output_size.h / 2.0f; cameras[i]->camera_intrinsics.center_y = cameras[i]->output_size.h / 2.0f; -#if NPS_SIMULATE_MT9F002 - // See boards/bebop/mt9f002.c - if (cam->Name() == "front_camera") { + + if (cam->Name() == "mt9f002") { + // See boards/bebop/mt9f002.c cameras[i]->output_size.w = MT9F002_OUTPUT_WIDTH; cameras[i]->output_size.h = MT9F002_OUTPUT_HEIGHT; cameras[i]->sensor_size.w = MT9F002_OUTPUT_WIDTH; cameras[i]->sensor_size.h = MT9F002_OUTPUT_HEIGHT; cameras[i]->crop.w = MT9F002_OUTPUT_WIDTH; cameras[i]->crop.h = MT9F002_OUTPUT_HEIGHT; + cameras[i]->fps = MT9F002_TARGET_FPS; cameras[i]->camera_intrinsics = { .focal_x = MT9F002_FOCAL_X, .focal_y = MT9F002_FOCAL_Y, @@ -729,9 +745,17 @@ static void init_gazebo_video(void) .center_y = MT9F002_CENTER_Y, .Dhane_k = MT9F002_DHANE_K }; + } else if (cam->Name() == "mt9v117") { + // See boards/bebop/mt9v117.h + cameras[i]->fps = MT9V117_TARGET_FPS; + cameras[i]->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 + }; } -#endif - cameras[i]->fps = cam->UpdateRate(); cout << "ok" << endl; } } @@ -784,31 +808,34 @@ static void gazebo_read_video(void) * @param img * @param cam */ -static void read_image( - struct image_t *img, - gazebo::sensors::CameraSensorPtr cam) +static void read_image(struct image_t *img, gazebo::sensors::CameraSensorPtr cam) { - int xstart = 0; - int ystart = 0; -#if NPS_SIMULATE_MT9F002 - if (cam->Name() == "front_camera") { + bool is_mt9f002 = false; + if (cam->Name() == "mt9f002") { image_create(img, MT9F002_OUTPUT_WIDTH, MT9F002_OUTPUT_HEIGHT, IMAGE_YUV422); - xstart = cam->ImageWidth() * (0.5 + MT9F002_INITIAL_OFFSET_X) - MT9F002_OUTPUT_WIDTH / 2; - ystart = cam->ImageHeight() * (0.5 + MT9F002_INITIAL_OFFSET_Y) - MT9F002_OUTPUT_HEIGHT / 2; + is_mt9f002 = true; } else { image_create(img, cam->ImageWidth(), cam->ImageHeight(), IMAGE_YUV422); } -#else - image_create(img, cam->ImageWidth(), cam->ImageHeight(), IMAGE_YUV422); -#endif + // Convert Gazebo's *RGB888* image to Paparazzi's YUV422 const uint8_t *data_rgb = cam->ImageData(); uint8_t *data_yuv = (uint8_t *)(img->buf); - for (int x = 0; x < img->w; ++x) { - for (int y = 0; y < img->h; ++y) { - int idx_rgb = 3 * (cam->ImageWidth() * (y + ystart) + (x + xstart)); - int idx_yuv = 2 * (img->w * y + x); - int idx_px = img->w * y + x; + for (int x_yuv = 0; x_yuv < img->w; ++x_yuv) { + for (int y_yuv = 0; y_yuv < img->h; ++y_yuv) { + int x_rgb = x_yuv; + int y_rgb = y_yuv; + if (is_mt9f002) { + // Change sampling points for zoomed and/or cropped image. + // Use nearest-neighbour sampling for now. + x_rgb = (mt9f002.offset_x + ((float)x_yuv / img->w) * mt9f002.sensor_width) + / CFG_MT9F002_PIXEL_ARRAY_WIDTH * cam->ImageWidth(); + y_rgb = (mt9f002.offset_y + ((float)y_yuv / img->h) * mt9f002.sensor_height) + / CFG_MT9F002_PIXEL_ARRAY_HEIGHT * cam->ImageHeight(); + } + int idx_rgb = 3 * (cam->ImageWidth() * y_rgb + x_rgb); + int idx_yuv = 2 * (img->w * y_yuv + x_yuv); + int idx_px = img->w * y_yuv + x_yuv; if (idx_px % 2 == 0) { // Pick U or V data_yuv[idx_yuv] = - 0.148 * data_rgb[idx_rgb] - 0.291 * data_rgb[idx_rgb + 1] @@ -830,7 +857,7 @@ static void read_image( img->pprz_ts = ts.Double() * 1e6; img->buf_idx = 0; // unused } -#endif // NPS_SIMULATE_VIDEO +#endif #if NPS_SIMULATE_LASER_RANGE_ARRAY /*