Skip to content

Image whitening #96

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
jlecoeur wants to merge 20 commits into from
Closed

Image whitening #96

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
20 commits
Select commit Hold shift + click to select a range
0076278
Improve flow quality
lericson Jun 15, 2017
cfc4d6e
Use floats for whitening
lericson Jun 15, 2017
e9d9e1f
Add 4.8.2 as allowed compiler
lericson Jun 15, 2017
cc7fbe1
Add 7.1.0 as allowed compiler
Aug 31, 2017
326edfd
Add whitening parameter
lericson Jun 19, 2017
580844e
Optimize types of whitening variables
lericson Jun 19, 2017
41c153d
Misleading indentation
Aug 31, 2017
dd2425b
Fix compile errors on hosts with python3 as default
Sep 18, 2015
e230142
Revert SONAR_TIMER_COUNT
Jun 19, 2017
7ab4534
Fall back to non-whitened images if flow quality is low
Jun 23, 2017
0edbe5e
Send average DT
Aug 31, 2017
f189d77
Allow GCC 7.2.0
jlecoeur Sep 15, 2017
06526c3
Use NUM_BLOCKS for size of arrays
jlecoeur Sep 16, 2017
692061a
Avoid 2nd computation of compute_diff for optic flow drawing
jlecoeur Sep 16, 2017
79d1dd5
Display flow points at center of blocks
jlecoeur Sep 16, 2017
ef15b31
Add param USB_DRAW_FLOW to display optic flow vectors on image
jlecoeur Sep 16, 2017
9a136a6
Compute OF std dev to scale quality
jlecoeur Sep 16, 2017
cebbc4b
USB: Send image with highest flow quality
jlecoeur Sep 17, 2017
398e9c3
scale flow quality more restrictive
ChristophTobler Sep 18, 2017
b4ddfb8
Lower OF recomputation min quality threshold to adapt to stricter qua…
jlecoeur Sep 18, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion Makefile
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -254,7 +254,7 @@ endif
.PHONY: upload-usb
upload-usb: archives all
@echo Attempting to flash $(notdir $(LEGACY_PX4)) PX4FLOW board via USB
@python -u Tools/px_uploader.py $(LEGACY_PX4) --baud 921600 --port $(SERIAL_PORTS)
@python2 -u Tools/px_uploader.py $(LEGACY_PX4) --baud 921600 --port $(SERIAL_PORTS)


#
Expand Down
2 changes: 1 addition & 1 deletion Tools/make_can_boot_descriptor.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -258,7 +258,7 @@ def app_descriptor(self, value):
try:
options.vcs_commit = int(GitWrapper.command("rev-list HEAD --max-count=1 --abbrev=8 --abbrev-commit"),16)
except Exception as e:
print "Git Command failed "+ str(e) +"- Exiting!"
print("Git Command failed "+ str(e) +"- Exiting!")
quit()

if args:
Expand Down
2 changes: 1 addition & 1 deletion makefiles/baremetal/baremetal_px4.mk
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ $(PRODUCT_BIN): $(PRODUCT_ELF)
ifdef MKUAVCANBL
ifdef MAKE_UAVCAN_BOOT_LOADABLE_ID
$(info %% Generating UAVCAN Bootable $(MAKE_UAVCAN_BOOT_LOADABLE_ID) as $(IMAGE_DIR)$(MAKE_UAVCAN_BOOT_LOADABLE_ID).$(UAVCAN_BL_EXT))
$(Q) python $(MKUAVCANBL) -v --use-git-hash $@ $(IMAGE_DIR)$(MAKE_UAVCAN_BOOT_LOADABLE_ID).$(UAVCAN_BL_EXT)
$(Q) python2 $(MKUAVCANBL) -v --use-git-hash $@ $(IMAGE_DIR)$(MAKE_UAVCAN_BOOT_LOADABLE_ID).$(UAVCAN_BL_EXT)
endif
endif

Expand Down
3 changes: 1 addition & 2 deletions makefiles/baremetal/toolchain_gnu-arm-eabi.mk
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ OBJDUMP = $(CROSSDEV)objdump

# Check if the right version of the toolchain is available
#
CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 4.9.3 5.4.1
CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.2 4.8.4 4.9.3 5.4.1 7.1.0 7.2.0
CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion)

ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED)))
Expand Down Expand Up @@ -277,4 +277,3 @@ define SYM_TO_BIN
@$(MKDIR) -p $(dir $2)
$(Q) $(OBJCOPY) -O binary $1 $2
endef

5 changes: 5 additions & 0 deletions src/drivers/boards/px4flow-v1/px4flow_init.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,11 @@
/****************************************************************************
* Public Functions
****************************************************************************/
static int errn;
int *__errno _PARAMS ((void))
{
return &errn;
}

/****************************************************************************
* Name: board_initialize
Expand Down
5 changes: 5 additions & 0 deletions src/include/dcmi.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,11 @@ void dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image,
*/
void send_calibration_image(uint8_t ** image_buffer_fast_1, uint8_t ** image_buffer_fast_2);

/**
* @brief Whiten image assuming pixels are iid Gaussian
*/
void whitened_image(uint8_t *source, uint8_t *dest, uint16_t image_size);

/**
* @brief Initialize DCMI DMA and enable image capturing
*/
Expand Down
2 changes: 2 additions & 0 deletions src/include/mt9v034.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,8 @@

#define MTV_HDR_ENABLE_REG 0x0F
#define MTV_ADC_RES_CTRL_REG 0x1C
#define MTV_BLACK_LEVEL_CTRL_REG 0x47
#define MTV_BLACK_LEVEL_CALIBRATION_REG 0x48
#define MTV_ROW_NOISE_CORR_CTRL_REG 0x70
#define MTV_TEST_PATTERN_REG 0x7F
#define MTV_TILED_DIGITAL_GAIN_REG 0x80
Expand Down
23 changes: 23 additions & 0 deletions src/include/settings.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,14 @@
#define BOTTOM_FLOW_IMAGE_WIDTH 64
#define BOTTOM_FLOW_SEARCH_WINDOW_SIZE 8

/* Translate normalized pixels to have their mean around 127. */
#define WHITENING_MEAN 127.0f

/* Rescale normalized pixels so 32 is one standard deviation,
* giving a range of four sigmas.
*/
#define WHITENING_STDDEV 32.0f

/******************************************************************
* ALL TYPE DEFINITIONS
*/
Expand Down Expand Up @@ -88,6 +96,18 @@ typedef enum
DPS2000 = 2000 /*!< 2000 dps */
} GyroSensitivity_TypeDef;

/**
* @brief gyro sensitivity enumeration
*/
typedef enum
{
IMAGE_WHITENING_DISABLED = 0, /*!< do not perform image whitening */
IMAGE_WHITENING_ALWAYS = 1, /*!< always perform image whitening */
IMAGE_WHITENING_AUTO = 2, /*!< perform image whitening, fall back to non-whitened images
if flow quality is lower than PARAM_IMAGE_WHITEN_QUALITY_THRESHOLD */
} ImageWhitening_TypeDef;


/******************************************************************
* ALL SETTINGS VARIABLES
*/
Expand Down Expand Up @@ -117,13 +137,16 @@ enum global_param_id_t
PARAM_MAX_FLOW_PIXEL,
PARAM_IMAGE_LOW_LIGHT,
PARAM_IMAGE_ROW_NOISE_CORR,
PARAM_IMAGE_WHITENING,
PARAM_IMAGE_WHITENING_QUALITY_THRESHOLD,
PARAM_IMAGE_TEST_PATTERN,
PARAM_GYRO_SENSITIVITY_DPS,
PARAM_GYRO_COMPENSATION_THRESHOLD,
PARAM_SONAR_FILTERED,
PARAM_SONAR_KALMAN_L1,
PARAM_SONAR_KALMAN_L2,

PARAM_USB_DRAW_FLOW,
PARAM_USB_SEND_VIDEO,
PARAM_USB_SEND_FLOW,
PARAM_USB_SEND_GYRO,
Expand Down
114 changes: 62 additions & 52 deletions src/modules/flow/dcmi.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -251,35 +251,55 @@ void dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image,
*current_image = *previous_image;
*previous_image = tmp_image;

TODO(NB dma_copy_image_buffers is calling uavcan_run());

/* wait for new image if needed */
while(image_counter < image_step) {
PROBE_1(false);
uavcan_run();
PROBE_1(true);
}
while(image_counter < image_step);

image_counter = 0;

/* time between images */
time_between_images = time_between_next_images;

uint8_t *source = NULL;

/* copy image */
if (dcmi_image_buffer_unused == 1)
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
(*current_image)[pixel] = (uint8_t)(dcmi_image_buffer_8bit_1[pixel]);
if (dcmi_image_buffer_unused == 1) {
source = dcmi_image_buffer_8bit_1;
} else if (dcmi_image_buffer_unused == 2) {
source = dcmi_image_buffer_8bit_2;
} else {
source = dcmi_image_buffer_8bit_3;
}
else if (dcmi_image_buffer_unused == 2)
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
(*current_image)[pixel] = (uint8_t)(dcmi_image_buffer_8bit_2[pixel]);

for (uint16_t pixel = 0; pixel < image_size; pixel++)
(*current_image)[pixel] = source[pixel];
}

/**
* @brief Whiten image by mean shift and scaling by standard deviation
*
* @param source Source image
* @param dest Destination image (may be the same as source)
* @param image_size Size of source and dest images. Must be <= 256.
*/
void whitened_image(uint8_t *source, uint8_t *dest, uint16_t image_size) {
uint32_t sum = 0;
for (uint16_t pixel = 0; pixel < image_size; pixel++)
sum += source[pixel];
uint8_t mean = sum / image_size;
uint32_t rss = 0;
for (uint16_t pixel = 0; pixel < image_size; pixel++) {
int16_t v = source[pixel] - mean;
rss += v*v;
}
else
{
for (uint16_t pixel = 0; pixel < image_size; pixel++)
(*current_image)[pixel] = (uint8_t)(dcmi_image_buffer_8bit_3[pixel]);
float stddev = sqrtf(rss/(image_size - 1)) / WHITENING_STDDEV;

for (uint16_t pixel = 0; pixel < image_size; pixel++) {
float v = WHITENING_MEAN + (source[pixel] - mean)/stddev;
if (v < 0.0f)
v = 0;
if (v > 255.0f)
v = 255;
dest[pixel] = (uint8_t)v;
}
}

Expand All @@ -304,16 +324,11 @@ void send_calibration_image(uint8_t ** image_buffer_fast_1, uint8_t ** image_buf
100);

uint16_t frame = 0;
uint8_t image = 0;
uint8_t frame_buffer[MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN];

for (int i = 0; i < FULL_IMAGE_SIZE * 4; i++)
{

if (i % FULL_IMAGE_SIZE == 0 && i != 0)
{
image++;
}
uint8_t image = i / FULL_IMAGE_SIZE;

if (i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN == 0 && i != 0)
{
Expand All @@ -322,44 +337,39 @@ void send_calibration_image(uint8_t ** image_buffer_fast_1, uint8_t ** image_buf
delay(2);
}

if (image == 0 )
{
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = (uint8_t)(*image_buffer_fast_1)[i % FULL_IMAGE_SIZE];
}
else if (image == 1 )
{
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = (uint8_t)(*image_buffer_fast_2)[i % FULL_IMAGE_SIZE];
}
else if (image == 2)
{
// The whole camera capture is stored in five parts: two buffers given
// as arguments, and three internal DMA buffers.
uint8_t *source;

if (image == 0)
source = *image_buffer_fast_1;
else if (image == 1)
source = *image_buffer_fast_2;
else if (image == 2) {
if (calibration_unused == 1)
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_1[i % FULL_IMAGE_SIZE];
source = dcmi_image_buffer_8bit_1;
else if (calibration_unused == 2)
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_2[i % FULL_IMAGE_SIZE];
source = dcmi_image_buffer_8bit_2;
else
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_3[i % FULL_IMAGE_SIZE];
}
else
{
if (calibration_used)
{
source = dcmi_image_buffer_8bit_3;
} else {
if (calibration_used) {
if (calibration_mem0 == 1)
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_1[i % FULL_IMAGE_SIZE];
source = dcmi_image_buffer_8bit_1;
else if (calibration_mem0 == 2)
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_2[i % FULL_IMAGE_SIZE];
source = dcmi_image_buffer_8bit_2;
else
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_3[i % FULL_IMAGE_SIZE];
}
else
{
source = dcmi_image_buffer_8bit_3;
} else {
if (calibration_mem1 == 1)
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_1[i % FULL_IMAGE_SIZE];
source = dcmi_image_buffer_8bit_1;
else if (calibration_mem1 == 2)
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_2[i % FULL_IMAGE_SIZE];
source = dcmi_image_buffer_8bit_2;
else
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_3[i % FULL_IMAGE_SIZE];
source = dcmi_image_buffer_8bit_3;
}
}
frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = source[i % FULL_IMAGE_SIZE];
}

mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, frame_buffer);
Expand Down
Loading