mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
Add linear regression
For easy line following mainly. In non-robust mode the line is computed using least squares. In robust mode the line is computed using the Theil-Sen median of slopes method. We do not use the Siegel Median of Medians operation because it costs more CPU time... but, more importantly there's no way to improve the centroid estimate so even if the slope is more robust the line will be drawn in the wrong place.
This commit is contained in:
parent
7d29104ed6
commit
cd4ad0dff3
@ -1134,6 +1134,8 @@ void imlib_lens_corr(image_t *img, float strength, float zoom);
|
||||
void imlib_get_histogram(histogram_t *out, image_t *ptr, rectangle_t *roi);
|
||||
void imlib_get_percentile(percentile_t *out, image_bpp_t bpp, histogram_t *ptr, float percentile);
|
||||
void imlib_get_statistics(statistics_t *out, image_bpp_t bpp, histogram_t *ptr);
|
||||
bool imlib_get_regression(find_lines_list_lnk_data_t *out, image_t *ptr, rectangle_t *roi, unsigned int x_stride, unsigned int y_stride,
|
||||
list_t *thresholds, bool invert, bool robust);
|
||||
// Color Tracking
|
||||
void imlib_find_blobs(list_t *out, image_t *ptr, rectangle_t *roi, unsigned int x_stride, unsigned int y_stride,
|
||||
list_t *thresholds, bool invert, unsigned int area_threshold, unsigned int pixels_threshold,
|
||||
|
||||
@ -425,3 +425,307 @@ void imlib_get_statistics(statistics_t *out, image_bpp_t bpp, histogram_t *ptr)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static int get_median(int *array, int array_sum, int array_len)
|
||||
{
|
||||
const int median_threshold = (array_sum + 1) / 2;
|
||||
int median_count = 0;
|
||||
|
||||
for (int i = 0; i < array_len; i++) {
|
||||
if ((median_count < median_threshold) && (median_threshold <= (median_count + array[i]))) return i;
|
||||
median_count += array[i];
|
||||
}
|
||||
|
||||
return array_len - 1;
|
||||
}
|
||||
|
||||
static int get_median_l(long long *array, long long array_sum, int array_len)
|
||||
{
|
||||
const long long median_threshold = (array_sum + 1) / 2;
|
||||
long long median_count = 0;
|
||||
|
||||
for (int i = 0; i < array_len; i++) {
|
||||
if ((median_count < median_threshold) && (median_threshold <= (median_count + array[i]))) return i;
|
||||
median_count += array[i];
|
||||
}
|
||||
|
||||
return array_len - 1;
|
||||
}
|
||||
|
||||
bool imlib_get_regression(find_lines_list_lnk_data_t *out, image_t *ptr, rectangle_t *roi, unsigned int x_stride, unsigned int y_stride,
|
||||
list_t *thresholds, bool invert, bool robust)
|
||||
{
|
||||
bool result = false;
|
||||
memset(out, 0, sizeof(find_lines_list_lnk_data_t));
|
||||
|
||||
if (!robust) { // Least Squares
|
||||
int blob_pixels = 0;
|
||||
int blob_cx = 0;
|
||||
int blob_cy = 0;
|
||||
long long blob_a = 0;
|
||||
long long blob_b = 0;
|
||||
long long blob_c = 0;
|
||||
|
||||
for (list_lnk_t *it = iterator_start_from_head(thresholds); it; it = iterator_next(it)) {
|
||||
color_thresholds_list_lnk_data_t lnk_data;
|
||||
iterator_get(thresholds, it, &lnk_data);
|
||||
|
||||
switch (ptr->bpp) {
|
||||
case IMAGE_BPP_BINARY: {
|
||||
for (int y = roi->y, yy = roi->y + roi->h; y < yy; y += y_stride) {
|
||||
uint32_t *row_ptr = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(ptr, y);
|
||||
for (int x = roi->x + (y % x_stride), xx = roi->x + roi->w; x < xx; x += x_stride) {
|
||||
if (COLOR_THRESHOLD_BINARY(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, x), &lnk_data, invert)) {
|
||||
blob_pixels += 1;
|
||||
blob_cx += x;
|
||||
blob_cy += y;
|
||||
blob_a += x*x;
|
||||
blob_b += x*y;
|
||||
blob_c += y*y;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case IMAGE_BPP_GRAYSCALE: {
|
||||
for (int y = roi->y, yy = roi->y + roi->h; y < yy; y += y_stride) {
|
||||
uint8_t *row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(ptr, y);
|
||||
for (int x = roi->x + (y % x_stride), xx = roi->x + roi->w; x < xx; x += x_stride) {
|
||||
if (COLOR_THRESHOLD_GRAYSCALE(IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, x), &lnk_data, invert)) {
|
||||
blob_pixels += 1;
|
||||
blob_cx += x;
|
||||
blob_cy += y;
|
||||
blob_a += x*x;
|
||||
blob_b += x*y;
|
||||
blob_c += y*y;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case IMAGE_BPP_RGB565: {
|
||||
for (int y = roi->y, yy = roi->y + roi->h; y < yy; y += y_stride) {
|
||||
uint16_t *row_ptr = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(ptr, y);
|
||||
for (int x = roi->x + (y % x_stride), xx = roi->x + roi->w; x < xx; x += x_stride) {
|
||||
if (COLOR_THRESHOLD_RGB565(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, x), &lnk_data, invert)) {
|
||||
blob_pixels += 1;
|
||||
blob_cx += x;
|
||||
blob_cy += y;
|
||||
blob_a += x*x;
|
||||
blob_b += x*y;
|
||||
blob_c += y*y;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (blob_pixels) {
|
||||
// http://www.cse.usf.edu/~r1k/MachineVisionBook/MachineVision.files/MachineVision_Chapter2.pdf
|
||||
// https://www.strchr.com/standard_deviation_in_one_pass
|
||||
//
|
||||
// a = sigma(x*x) + (mx*sigma(x)) + (mx*sigma(x)) + (sigma()*mx*mx)
|
||||
// b = sigma(x*y) + (mx*sigma(y)) + (my*sigma(x)) + (sigma()*mx*my)
|
||||
// c = sigma(y*y) + (my*sigma(y)) + (my*sigma(y)) + (sigma()*my*my)
|
||||
//
|
||||
// blob_a = sigma(x*x)
|
||||
// blob_b = sigma(x*y)
|
||||
// blob_c = sigma(y*y)
|
||||
// blob_cx = sigma(x)
|
||||
// blob_cy = sigma(y)
|
||||
// blob_pixels = sigma()
|
||||
|
||||
int mx = blob_cx / blob_pixels; // x centroid
|
||||
int my = blob_cy / blob_pixels; // y centroid
|
||||
int small_blob_a = blob_a - ((mx * blob_cx) + (mx * blob_cx)) + (blob_pixels * mx * mx);
|
||||
int small_blob_b = blob_b - ((mx * blob_cy) + (my * blob_cx)) + (blob_pixels * mx * my);
|
||||
int small_blob_c = blob_c - ((my * blob_cy) + (my * blob_cy)) + (blob_pixels * my * my);
|
||||
|
||||
float rotation = ((small_blob_a != small_blob_c) ? (fast_atan2f(2 * small_blob_b, small_blob_a - small_blob_c) / 2.0f) : 1.570796f) + 1.570796f; // PI/2
|
||||
|
||||
out->theta = fast_roundf(rotation * 57.295780) % 180; // * (180 / PI)
|
||||
if (out->theta < 0) out->theta += 180;
|
||||
out->rho = fast_roundf(((mx - roi->x) * cos_table[out->theta]) + ((my - roi->y) * sin_table[out->theta]));
|
||||
|
||||
float part0 = (small_blob_a + small_blob_c) / 2.0f;
|
||||
float f_b = (float) small_blob_b;
|
||||
float f_a_c = (float) (small_blob_a - small_blob_c);
|
||||
float part1 = fast_sqrtf((4 * f_b * f_b) + (f_a_c * f_a_c)) / 2.0f;
|
||||
float p_add = fast_sqrtf(part0 + part1);
|
||||
float p_sub = fast_sqrtf(part0 - part1);
|
||||
float e_min = IM_MIN(p_add, p_sub);
|
||||
float e_max = IM_MAX(p_add, p_sub);
|
||||
out->magnitude = fast_roundf(e_max / e_min) - 1; // Circle -> [0, INF) -> Line
|
||||
|
||||
if ((45 <= out->theta) && (out->theta < 135)) {
|
||||
// y = (r - x cos(t)) / sin(t)
|
||||
out->line.x1 = 0;
|
||||
out->line.y1 = fast_roundf((out->rho - (out->line.x1 * cos_table[out->theta])) / sin_table[out->theta]);
|
||||
out->line.x2 = roi->w - 1;
|
||||
out->line.y2 = fast_roundf((out->rho - (out->line.x2 * cos_table[out->theta])) / sin_table[out->theta]);
|
||||
} else {
|
||||
// x = (r - y sin(t)) / cos(t);
|
||||
out->line.y1 = 0;
|
||||
out->line.x1 = fast_roundf((out->rho - (out->line.y1 * sin_table[out->theta])) / cos_table[out->theta]);
|
||||
out->line.y2 = roi->h - 1;
|
||||
out->line.x2 = fast_roundf((out->rho - (out->line.y2 * sin_table[out->theta])) / cos_table[out->theta]);
|
||||
}
|
||||
|
||||
if(lb_clip_line(&out->line, 0, 0, roi->w, roi->h)) {
|
||||
out->line.x1 += roi->x;
|
||||
out->line.y1 += roi->y;
|
||||
out->line.x2 += roi->x;
|
||||
out->line.y2 += roi->y;
|
||||
result = true;
|
||||
} else {
|
||||
memset(out, 0, sizeof(find_lines_list_lnk_data_t));
|
||||
}
|
||||
}
|
||||
} else { // Theil-Sen Estimator
|
||||
int blob_pixels = 0;
|
||||
|
||||
fifo_t fifo;
|
||||
fifo_alloc(&fifo, roi->w * roi->h, sizeof(point_t));
|
||||
int *x_histogram = fb_alloc0(ptr->w * sizeof(int)); // Not roi so we don't have to adjust, we can burn the RAM.
|
||||
int *y_histogram = fb_alloc0(ptr->h * sizeof(int)); // Not roi so we don't have to adjust, we can burn the RAM.
|
||||
|
||||
for (list_lnk_t *it = iterator_start_from_head(thresholds); it; it = iterator_next(it)) {
|
||||
color_thresholds_list_lnk_data_t lnk_data;
|
||||
iterator_get(thresholds, it, &lnk_data);
|
||||
|
||||
switch (ptr->bpp) {
|
||||
case IMAGE_BPP_BINARY: {
|
||||
for (int y = roi->y, yy = roi->y + roi->h; y < yy; y += y_stride) {
|
||||
uint32_t *row_ptr = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(ptr, y);
|
||||
for (int x = roi->x + (y % x_stride), xx = roi->x + roi->w; x < xx; x += x_stride) {
|
||||
if (COLOR_THRESHOLD_BINARY(IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, x), &lnk_data, invert)) {
|
||||
blob_pixels += 1;
|
||||
x_histogram[x]++;
|
||||
y_histogram[y]++;
|
||||
|
||||
point_t p;
|
||||
point_init(&p, x, y);
|
||||
fifo_enqueue(&fifo, &p);
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case IMAGE_BPP_GRAYSCALE: {
|
||||
for (int y = roi->y, yy = roi->y + roi->h; y < yy; y += y_stride) {
|
||||
uint8_t *row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(ptr, y);
|
||||
for (int x = roi->x + (y % x_stride), xx = roi->x + roi->w; x < xx; x += x_stride) {
|
||||
if (COLOR_THRESHOLD_GRAYSCALE(IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, x), &lnk_data, invert)) {
|
||||
blob_pixels += 1;
|
||||
x_histogram[x]++;
|
||||
y_histogram[y]++;
|
||||
|
||||
point_t p;
|
||||
point_init(&p, x, y);
|
||||
fifo_enqueue(&fifo, &p);
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case IMAGE_BPP_RGB565: {
|
||||
for (int y = roi->y, yy = roi->y + roi->h; y < yy; y += y_stride) {
|
||||
uint16_t *row_ptr = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(ptr, y);
|
||||
for (int x = roi->x + (y % x_stride), xx = roi->x + roi->w; x < xx; x += x_stride) {
|
||||
if (COLOR_THRESHOLD_RGB565(IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, x), &lnk_data, invert)) {
|
||||
blob_pixels += 1;
|
||||
x_histogram[x]++;
|
||||
y_histogram[y]++;
|
||||
|
||||
point_t p;
|
||||
point_init(&p, x, y);
|
||||
fifo_enqueue(&fifo, &p);
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (blob_pixels) {
|
||||
long long delta_sum = (fifo_size(&fifo) * (fifo_size(&fifo) - 1)) / 2;
|
||||
|
||||
if (delta_sum) {
|
||||
// The code below computes the average slope between all pairs of points.
|
||||
// This is a N^2 operation that can easily blow up if the image is not threshold carefully...
|
||||
long long *x_delta_histogram = fb_alloc0((2 * ptr->w) * sizeof(long long)); // Not roi so we don't have to adjust, we can burn the RAM.
|
||||
long long *y_delta_histogram = fb_alloc0((2 * ptr->h) * sizeof(long long)); // Not roi so we don't have to adjust, we can burn the RAM.
|
||||
|
||||
while (fifo_is_not_empty(&fifo)) {
|
||||
point_t p0;
|
||||
fifo_dequeue(&fifo, &p0);
|
||||
|
||||
for (size_t i = 0, j = fifo_size(&fifo); i < j; i++) {
|
||||
point_t p1;
|
||||
fifo_dequeue(&fifo, &p1);
|
||||
|
||||
x_delta_histogram[p0.x - p1.x + ptr->w]++; // Note we allocated 1 extra above so we can do ptr->w instead of (ptr->w-1).
|
||||
y_delta_histogram[p0.y - p1.y + ptr->h]++; // Note we allocated 1 extra above so we can do ptr->h instead of (ptr->h-1).
|
||||
|
||||
fifo_enqueue(&fifo, &p1);
|
||||
}
|
||||
}
|
||||
|
||||
int mx = get_median(x_histogram, blob_pixels, ptr->w); // Output doesn't need adjustment.
|
||||
int my = get_median(y_histogram, blob_pixels, ptr->h); // Output doesn't need adjustment.
|
||||
int mdx = get_median_l(x_delta_histogram, delta_sum, 2 * ptr->w) - ptr->w; // Fix offset.
|
||||
int mdy = get_median_l(y_delta_histogram, delta_sum, 2 * ptr->h) - ptr->h; // Fix offset.
|
||||
|
||||
float rotation = (mdx ? fast_atan2f(mdy, mdx) : 1.570796f) + 1.570796f; // PI/2
|
||||
|
||||
out->theta = fast_roundf(rotation * 57.295780) % 180; // * (180 / PI)
|
||||
if (out->theta < 0) out->theta += 180;
|
||||
out->rho = fast_roundf(((mx - roi->x) * cos_table[out->theta]) + ((my - roi->y) * sin_table[out->theta]));
|
||||
|
||||
out->magnitude = fast_roundf(fast_sqrtf((mdx * mdx) + (mdy * mdy)));
|
||||
|
||||
if ((45 <= out->theta) && (out->theta < 135)) {
|
||||
// y = (r - x cos(t)) / sin(t)
|
||||
out->line.x1 = 0;
|
||||
out->line.y1 = fast_roundf((out->rho - (out->line.x1 * cos_table[out->theta])) / sin_table[out->theta]);
|
||||
out->line.x2 = roi->w - 1;
|
||||
out->line.y2 = fast_roundf((out->rho - (out->line.x2 * cos_table[out->theta])) / sin_table[out->theta]);
|
||||
} else {
|
||||
// x = (r - y sin(t)) / cos(t);
|
||||
out->line.y1 = 0;
|
||||
out->line.x1 = fast_roundf((out->rho - (out->line.y1 * sin_table[out->theta])) / cos_table[out->theta]);
|
||||
out->line.y2 = roi->h - 1;
|
||||
out->line.x2 = fast_roundf((out->rho - (out->line.y2 * sin_table[out->theta])) / cos_table[out->theta]);
|
||||
}
|
||||
|
||||
if(lb_clip_line(&out->line, 0, 0, roi->w, roi->h)) {
|
||||
out->line.x1 += roi->x;
|
||||
out->line.y1 += roi->y;
|
||||
out->line.x2 += roi->x;
|
||||
out->line.y2 += roi->y;
|
||||
result = true;
|
||||
} else {
|
||||
memset(out, 0, sizeof(find_lines_list_lnk_data_t));
|
||||
}
|
||||
|
||||
fb_free(); // y_delta_histogram
|
||||
fb_free(); // x_delta_histogram
|
||||
}
|
||||
}
|
||||
|
||||
fb_free(); // y_histogram
|
||||
fb_free(); // x_histogram
|
||||
fifo_free(&fifo);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
@ -1964,6 +1964,175 @@ static mp_obj_t py_image_get_statistics(uint n_args, const mp_obj_t *args, mp_ma
|
||||
return o;
|
||||
}
|
||||
|
||||
// Line Object //
|
||||
#define py_line_obj_size 8
|
||||
typedef struct py_line_obj {
|
||||
mp_obj_base_t base;
|
||||
mp_obj_t x1, y1, x2, y2, length, magnitude, theta, rho;
|
||||
} py_line_obj_t;
|
||||
|
||||
static void py_line_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind)
|
||||
{
|
||||
py_line_obj_t *self = self_in;
|
||||
mp_printf(print,
|
||||
"{x1:%d, y1:%d, x2:%d, y2:%d, length:%d, magnitude:%d, theta:%d, rho:%d}",
|
||||
mp_obj_get_int(self->x1),
|
||||
mp_obj_get_int(self->y1),
|
||||
mp_obj_get_int(self->x2),
|
||||
mp_obj_get_int(self->y2),
|
||||
mp_obj_get_int(self->length),
|
||||
mp_obj_get_int(self->magnitude),
|
||||
mp_obj_get_int(self->theta),
|
||||
mp_obj_get_int(self->rho));
|
||||
}
|
||||
|
||||
static mp_obj_t py_line_subscr(mp_obj_t self_in, mp_obj_t index, mp_obj_t value)
|
||||
{
|
||||
if (value == MP_OBJ_SENTINEL) { // load
|
||||
py_line_obj_t *self = self_in;
|
||||
if (MP_OBJ_IS_TYPE(index, &mp_type_slice)) {
|
||||
mp_bound_slice_t slice;
|
||||
if (!mp_seq_get_fast_slice_indexes(py_line_obj_size, index, &slice)) {
|
||||
mp_not_implemented("only slices with step=1 (aka None) are supported");
|
||||
}
|
||||
mp_obj_tuple_t *result = mp_obj_new_tuple(slice.stop - slice.start, NULL);
|
||||
mp_seq_copy(result->items, &(self->x1) + slice.start, result->len, mp_obj_t);
|
||||
return result;
|
||||
}
|
||||
switch (mp_get_index(self->base.type, py_line_obj_size, index, false)) {
|
||||
case 0: return self->x1;
|
||||
case 1: return self->y1;
|
||||
case 2: return self->x2;
|
||||
case 3: return self->y2;
|
||||
case 4: return self->length;
|
||||
case 5: return self->magnitude;
|
||||
case 6: return self->theta;
|
||||
case 7: return self->rho;
|
||||
}
|
||||
}
|
||||
return MP_OBJ_NULL; // op not supported
|
||||
}
|
||||
|
||||
mp_obj_t py_line_line(mp_obj_t self_in)
|
||||
{
|
||||
return mp_obj_new_tuple(4, (mp_obj_t []) {((py_line_obj_t *) self_in)->x1,
|
||||
((py_line_obj_t *) self_in)->y1,
|
||||
((py_line_obj_t *) self_in)->x2,
|
||||
((py_line_obj_t *) self_in)->y2});
|
||||
}
|
||||
|
||||
mp_obj_t py_line_x1(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->x1; }
|
||||
mp_obj_t py_line_y1(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->y1; }
|
||||
mp_obj_t py_line_x2(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->x2; }
|
||||
mp_obj_t py_line_y2(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->y2; }
|
||||
mp_obj_t py_line_length(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->length; }
|
||||
mp_obj_t py_line_magnitude(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->magnitude; }
|
||||
mp_obj_t py_line_theta(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->theta; }
|
||||
mp_obj_t py_line_rho(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->rho; }
|
||||
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_line_obj, py_line_line);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_x1_obj, py_line_x1);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_y1_obj, py_line_y1);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_x2_obj, py_line_x2);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_y2_obj, py_line_y2);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_length_obj, py_line_length);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_magnitude_obj, py_line_magnitude);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_theta_obj, py_line_theta);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_rho_obj, py_line_rho);
|
||||
|
||||
STATIC const mp_rom_map_elem_t py_line_locals_dict_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR_line), MP_ROM_PTR(&py_line_line_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_x1), MP_ROM_PTR(&py_line_x1_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_y1), MP_ROM_PTR(&py_line_y1_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_x2), MP_ROM_PTR(&py_line_x2_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_y2), MP_ROM_PTR(&py_line_y2_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_length), MP_ROM_PTR(&py_line_length_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_magnitude), MP_ROM_PTR(&py_line_magnitude_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_theta), MP_ROM_PTR(&py_line_theta_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_rho), MP_ROM_PTR(&py_line_rho_obj) },
|
||||
};
|
||||
|
||||
STATIC MP_DEFINE_CONST_DICT(py_line_locals_dict, py_line_locals_dict_table);
|
||||
|
||||
static const mp_obj_type_t py_line_type = {
|
||||
{ &mp_type_type },
|
||||
.name = MP_QSTR_line,
|
||||
.print = py_line_print,
|
||||
.subscr = py_line_subscr,
|
||||
.locals_dict = (mp_obj_t) &py_line_locals_dict,
|
||||
};
|
||||
|
||||
static mp_obj_t py_image_get_regression(uint n_args, const mp_obj_t *args, mp_map_t *kw_args)
|
||||
{
|
||||
image_t *arg_img = py_image_cobj(args[0]);
|
||||
PY_ASSERT_FALSE_MSG(IM_IS_JPEG(arg_img), "Operation not supported on JPEG or RAW frames.");
|
||||
|
||||
rectangle_t roi;
|
||||
py_helper_lookup_rectangle(kw_args, arg_img, &roi);
|
||||
|
||||
mp_uint_t arg_thresholds_len;
|
||||
mp_obj_t *arg_thresholds;
|
||||
mp_obj_get_array(args[1], &arg_thresholds_len, &arg_thresholds);
|
||||
if (!arg_thresholds_len) return mp_const_none;
|
||||
|
||||
list_t thresholds;
|
||||
list_init(&thresholds, sizeof(color_thresholds_list_lnk_data_t));
|
||||
|
||||
for(mp_uint_t i = 0; i < arg_thresholds_len; i++) {
|
||||
mp_uint_t arg_threshold_len;
|
||||
mp_obj_t *arg_threshold;
|
||||
mp_obj_get_array(arg_thresholds[i], &arg_threshold_len, &arg_threshold);
|
||||
if (arg_threshold_len) {
|
||||
color_thresholds_list_lnk_data_t lnk_data;
|
||||
lnk_data.LMin = (arg_threshold_len > 0) ? IM_MAX(IM_MIN(mp_obj_get_int(arg_threshold[0]),
|
||||
IM_MAX(COLOR_L_MAX, COLOR_GRAYSCALE_MAX)), IM_MIN(COLOR_L_MIN, COLOR_GRAYSCALE_MIN)) : IM_MIN(COLOR_L_MIN, COLOR_GRAYSCALE_MIN);
|
||||
lnk_data.LMax = (arg_threshold_len > 1) ? IM_MAX(IM_MIN(mp_obj_get_int(arg_threshold[1]),
|
||||
IM_MAX(COLOR_L_MAX, COLOR_GRAYSCALE_MAX)), IM_MIN(COLOR_L_MIN, COLOR_GRAYSCALE_MIN)) : IM_MAX(COLOR_L_MAX, COLOR_GRAYSCALE_MAX);
|
||||
lnk_data.AMin = (arg_threshold_len > 2) ? IM_MAX(IM_MIN(mp_obj_get_int(arg_threshold[2]), COLOR_A_MAX), COLOR_A_MIN) : COLOR_A_MIN;
|
||||
lnk_data.AMax = (arg_threshold_len > 3) ? IM_MAX(IM_MIN(mp_obj_get_int(arg_threshold[3]), COLOR_A_MAX), COLOR_A_MIN) : COLOR_A_MAX;
|
||||
lnk_data.BMin = (arg_threshold_len > 4) ? IM_MAX(IM_MIN(mp_obj_get_int(arg_threshold[4]), COLOR_B_MAX), COLOR_B_MIN) : COLOR_B_MIN;
|
||||
lnk_data.BMax = (arg_threshold_len > 5) ? IM_MAX(IM_MIN(mp_obj_get_int(arg_threshold[5]), COLOR_B_MAX), COLOR_B_MIN) : COLOR_B_MAX;
|
||||
color_thresholds_list_lnk_data_t lnk_data_tmp;
|
||||
memcpy(&lnk_data_tmp, &lnk_data, sizeof(color_thresholds_list_lnk_data_t));
|
||||
lnk_data.LMin = IM_MIN(lnk_data_tmp.LMin, lnk_data_tmp.LMax);
|
||||
lnk_data.LMax = IM_MAX(lnk_data_tmp.LMin, lnk_data_tmp.LMax);
|
||||
lnk_data.AMin = IM_MIN(lnk_data_tmp.AMin, lnk_data_tmp.AMax);
|
||||
lnk_data.AMax = IM_MAX(lnk_data_tmp.AMin, lnk_data_tmp.AMax);
|
||||
lnk_data.BMin = IM_MIN(lnk_data_tmp.BMin, lnk_data_tmp.BMax);
|
||||
lnk_data.BMax = IM_MAX(lnk_data_tmp.BMin, lnk_data_tmp.BMax);
|
||||
list_push_back(&thresholds, &lnk_data);
|
||||
}
|
||||
}
|
||||
|
||||
unsigned int x_stride = py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_x_stride), 2);
|
||||
PY_ASSERT_TRUE_MSG(x_stride > 0, "x_stride must not be zero.");
|
||||
unsigned int y_stride = py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_y_stride), 1);
|
||||
PY_ASSERT_TRUE_MSG(y_stride > 0, "y_stride must not be zero.");
|
||||
bool invert = py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_invert), false);
|
||||
bool robust = py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_robust), false);
|
||||
|
||||
find_lines_list_lnk_data_t out;
|
||||
fb_alloc_mark();
|
||||
if (!imlib_get_regression(&out, arg_img, &roi, x_stride, y_stride, &thresholds, invert, robust)) return mp_const_none;
|
||||
fb_alloc_free_till_mark();
|
||||
list_free(&thresholds);
|
||||
|
||||
py_line_obj_t *o = m_new_obj(py_line_obj_t);
|
||||
o->base.type = &py_line_type;
|
||||
o->x1 = mp_obj_new_int(out.line.x1);
|
||||
o->y1 = mp_obj_new_int(out.line.y1);
|
||||
o->x2 = mp_obj_new_int(out.line.x2);
|
||||
o->y2 = mp_obj_new_int(out.line.y2);
|
||||
int x_diff = out.line.x2 - out.line.x1;
|
||||
int y_diff = out.line.y2 - out.line.y1;
|
||||
o->length = mp_obj_new_int(fast_roundf(fast_sqrtf((x_diff * x_diff) + (y_diff * y_diff))));
|
||||
o->magnitude = mp_obj_new_int(out.magnitude);
|
||||
o->theta = mp_obj_new_int(out.theta);
|
||||
o->rho = mp_obj_new_int(out.rho);
|
||||
|
||||
return o;
|
||||
}
|
||||
|
||||
// Blob Object //
|
||||
#define py_blob_obj_size 10
|
||||
typedef struct py_blob_obj {
|
||||
@ -2223,104 +2392,6 @@ static mp_obj_t py_image_find_blobs(uint n_args, const mp_obj_t *args, mp_map_t
|
||||
return objects_list;
|
||||
}
|
||||
|
||||
// Line Object //
|
||||
#define py_line_obj_size 8
|
||||
typedef struct py_line_obj {
|
||||
mp_obj_base_t base;
|
||||
mp_obj_t x1, y1, x2, y2, length, magnitude, theta, rho;
|
||||
} py_line_obj_t;
|
||||
|
||||
static void py_line_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind)
|
||||
{
|
||||
py_line_obj_t *self = self_in;
|
||||
mp_printf(print,
|
||||
"{x1:%d, y1:%d, x2:%d, y2:%d, length:%d, magnitude:%d, theta:%d, rho:%d}",
|
||||
mp_obj_get_int(self->x1),
|
||||
mp_obj_get_int(self->y1),
|
||||
mp_obj_get_int(self->x2),
|
||||
mp_obj_get_int(self->y2),
|
||||
mp_obj_get_int(self->length),
|
||||
mp_obj_get_int(self->magnitude),
|
||||
mp_obj_get_int(self->theta),
|
||||
mp_obj_get_int(self->rho));
|
||||
}
|
||||
|
||||
static mp_obj_t py_line_subscr(mp_obj_t self_in, mp_obj_t index, mp_obj_t value)
|
||||
{
|
||||
if (value == MP_OBJ_SENTINEL) { // load
|
||||
py_line_obj_t *self = self_in;
|
||||
if (MP_OBJ_IS_TYPE(index, &mp_type_slice)) {
|
||||
mp_bound_slice_t slice;
|
||||
if (!mp_seq_get_fast_slice_indexes(py_line_obj_size, index, &slice)) {
|
||||
mp_not_implemented("only slices with step=1 (aka None) are supported");
|
||||
}
|
||||
mp_obj_tuple_t *result = mp_obj_new_tuple(slice.stop - slice.start, NULL);
|
||||
mp_seq_copy(result->items, &(self->x1) + slice.start, result->len, mp_obj_t);
|
||||
return result;
|
||||
}
|
||||
switch (mp_get_index(self->base.type, py_line_obj_size, index, false)) {
|
||||
case 0: return self->x1;
|
||||
case 1: return self->y1;
|
||||
case 2: return self->x2;
|
||||
case 3: return self->y2;
|
||||
case 4: return self->length;
|
||||
case 5: return self->magnitude;
|
||||
case 6: return self->theta;
|
||||
case 7: return self->rho;
|
||||
}
|
||||
}
|
||||
return MP_OBJ_NULL; // op not supported
|
||||
}
|
||||
|
||||
mp_obj_t py_line_line(mp_obj_t self_in)
|
||||
{
|
||||
return mp_obj_new_tuple(4, (mp_obj_t []) {((py_line_obj_t *) self_in)->x1,
|
||||
((py_line_obj_t *) self_in)->y1,
|
||||
((py_line_obj_t *) self_in)->x2,
|
||||
((py_line_obj_t *) self_in)->y2});
|
||||
}
|
||||
|
||||
mp_obj_t py_line_x1(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->x1; }
|
||||
mp_obj_t py_line_y1(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->y1; }
|
||||
mp_obj_t py_line_x2(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->x2; }
|
||||
mp_obj_t py_line_y2(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->y2; }
|
||||
mp_obj_t py_line_length(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->length; }
|
||||
mp_obj_t py_line_magnitude(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->magnitude; }
|
||||
mp_obj_t py_line_theta(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->theta; }
|
||||
mp_obj_t py_line_rho(mp_obj_t self_in) { return ((py_line_obj_t *) self_in)->rho; }
|
||||
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_line_obj, py_line_line);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_x1_obj, py_line_x1);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_y1_obj, py_line_y1);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_x2_obj, py_line_x2);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_y2_obj, py_line_y2);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_length_obj, py_line_length);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_magnitude_obj, py_line_magnitude);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_theta_obj, py_line_theta);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_line_rho_obj, py_line_rho);
|
||||
|
||||
STATIC const mp_rom_map_elem_t py_line_locals_dict_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR_line), MP_ROM_PTR(&py_line_line_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_x1), MP_ROM_PTR(&py_line_x1_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_y1), MP_ROM_PTR(&py_line_y1_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_x2), MP_ROM_PTR(&py_line_x2_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_y2), MP_ROM_PTR(&py_line_y2_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_length), MP_ROM_PTR(&py_line_length_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_magnitude), MP_ROM_PTR(&py_line_magnitude_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_theta), MP_ROM_PTR(&py_line_theta_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_rho), MP_ROM_PTR(&py_line_rho_obj) },
|
||||
};
|
||||
|
||||
STATIC MP_DEFINE_CONST_DICT(py_line_locals_dict, py_line_locals_dict_table);
|
||||
|
||||
static const mp_obj_type_t py_line_type = {
|
||||
{ &mp_type_type },
|
||||
.name = MP_QSTR_line,
|
||||
.print = py_line_print,
|
||||
.subscr = py_line_subscr,
|
||||
.locals_dict = (mp_obj_t) &py_line_locals_dict,
|
||||
};
|
||||
|
||||
static mp_obj_t py_image_find_lines(uint n_args, const mp_obj_t *args, mp_map_t *kw_args)
|
||||
{
|
||||
image_t *arg_img = py_image_cobj(args[0]);
|
||||
@ -3485,6 +3556,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_image_mask_ellipse_obj, py_image_mask_ellips
|
||||
/* Image Statistics */
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_get_histogram_obj, 1, py_image_get_histogram);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_get_statistics_obj, 1, py_image_get_statistics);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_get_regression_obj, 2, py_image_get_regression);
|
||||
/* Color Tracking */
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_find_blobs_obj, 2, py_image_find_blobs);
|
||||
/* Shape Detection */
|
||||
@ -3579,6 +3651,7 @@ static const mp_map_elem_t locals_dict_table[] = {
|
||||
{MP_OBJ_NEW_QSTR(MP_QSTR_get_stats), (mp_obj_t)&py_image_get_statistics_obj},
|
||||
{MP_OBJ_NEW_QSTR(MP_QSTR_get_statistics), (mp_obj_t)&py_image_get_statistics_obj},
|
||||
{MP_OBJ_NEW_QSTR(MP_QSTR_statistics), (mp_obj_t)&py_image_get_statistics_obj},
|
||||
{MP_OBJ_NEW_QSTR(MP_QSTR_get_regression), (mp_obj_t)&py_image_get_regression_obj},
|
||||
/* Color Tracking */
|
||||
{MP_OBJ_NEW_QSTR(MP_QSTR_find_blobs), (mp_obj_t)&py_image_find_blobs_obj},
|
||||
/* Shape Detection */
|
||||
|
||||
@ -407,10 +407,29 @@ Q(b_max)
|
||||
Q(b_lq)
|
||||
Q(b_uq)
|
||||
|
||||
// Find Blobs
|
||||
Q(find_blobs)
|
||||
// Get Regression
|
||||
Q(get_regression)
|
||||
// duplicate Q(roi)
|
||||
Q(x_stride)
|
||||
Q(y_stride)
|
||||
// duplicate Q(invert)
|
||||
Q(robust)
|
||||
// Line Object
|
||||
Q(line)
|
||||
// duplicate Q(line)
|
||||
Q(x1)
|
||||
Q(y1)
|
||||
Q(x2)
|
||||
Q(y2)
|
||||
Q(length)
|
||||
Q(magnitude)
|
||||
Q(theta)
|
||||
Q(rho)
|
||||
|
||||
// Find Blobs
|
||||
Q(find_blobs)
|
||||
// duplicate Q(x_stride)
|
||||
// duplicate Q(y_stride)
|
||||
Q(area_threshold)
|
||||
Q(pixels_threshold)
|
||||
Q(merge)
|
||||
@ -442,17 +461,6 @@ Q(find_lines)
|
||||
// duplicate Q(threshold)
|
||||
Q(theta_margin)
|
||||
Q(rho_margin)
|
||||
// Line Object
|
||||
Q(line)
|
||||
// duplicate Q(line)
|
||||
Q(x1)
|
||||
Q(y1)
|
||||
Q(x2)
|
||||
Q(y2)
|
||||
Q(length)
|
||||
Q(magnitude)
|
||||
Q(theta)
|
||||
Q(rho)
|
||||
|
||||
// Find Line Segments
|
||||
Q(find_line_segments)
|
||||
|
||||
43
usr/examples/09-Feature-Detection/linear_regression_fast.py
Normal file
43
usr/examples/09-Feature-Detection/linear_regression_fast.py
Normal file
@ -0,0 +1,43 @@
|
||||
# Fast Linear Regression Example
|
||||
#
|
||||
# This example shows off how to use the get_regression() method on your OpenMV Cam
|
||||
# to get the linear regression of a ROI. Using this method you can easily build
|
||||
# a robot which can track lines which all point in the same general direction
|
||||
# but are not actually connected. Use find_blobs() on lines that are nicely
|
||||
# connected for better filtering options and control.
|
||||
#
|
||||
# This is called the fast linear regression because we use the least-squares
|
||||
# method to fit the line. However, this method is NOT GOOD FOR ANY images that
|
||||
# have a lot (or really any) outlier points which corrupt the line fit...
|
||||
|
||||
THRESHOLD = (0, 100) # Grayscale threshold for dark things...
|
||||
BINARY_VISIBLE = True # Does binary first so you can see what the linear regression
|
||||
# is being run on... might lower FPS though.
|
||||
|
||||
import sensor, image, time
|
||||
|
||||
sensor.reset()
|
||||
sensor.set_pixformat(sensor.GRAYSCALE)
|
||||
sensor.set_framesize(sensor.QQVGA)
|
||||
sensor.skip_frames(time = 2000)
|
||||
clock = time.clock()
|
||||
|
||||
while(True):
|
||||
clock.tick()
|
||||
img = sensor.snapshot().binary([THRESHOLD]) if BINARY_VISIBLE else sensor.snapshot()
|
||||
|
||||
# Returns a line object similar to line objects returned by find_lines() and
|
||||
# find_line_segments(). You have x1(), y1(), x2(), y2(), length(),
|
||||
# theta() (rotation in degrees), rho(), and magnitude().
|
||||
#
|
||||
# magnitude() represents how well the linear regression worked. It goes from
|
||||
# (0, INF] where 0 is returned for a circle. The more linear the
|
||||
# scene is the higher the magnitude.
|
||||
line = img.get_regression([(255,255) if BINARY_VISIBLE else THRESHOLD])
|
||||
|
||||
if (line): img.draw_line(line.line(), color = 127)
|
||||
print("FPS %f, mag = %s" % (clock.fps(), str(line.magnitude()) if (line) else "N/A"))
|
||||
|
||||
# About negative rho values:
|
||||
#
|
||||
# A [theta+0:-rho] tuple is the same as [theta+180:+rho].
|
||||
@ -0,0 +1,45 @@
|
||||
# Robust Linear Regression Example
|
||||
#
|
||||
# This example shows off how to use the get_regression() method on your OpenMV Cam
|
||||
# to get the linear regression of a ROI. Using this method you can easily build
|
||||
# a robot which can track lines which all point in the same general direction
|
||||
# but are not actually connected. Use find_blobs() on lines that are nicely
|
||||
# connected for better filtering options and control.
|
||||
#
|
||||
# We're using the robust=True argument for get_regression() in this script which
|
||||
# computes the linear regression using a much more robust algorithm... but potentially
|
||||
# much slower. The robust algorithm runs in O(N^2) time on the image. So, YOU NEED
|
||||
# TO LIMIT THE NUMBER OF PIXELS the robust algorithm works on or it can actually
|
||||
# take seconds for the algorithm to give you a result... THRESHOLD VERY CAREFULLY!
|
||||
|
||||
THRESHOLD = (0, 100) # Grayscale threshold for dark things...
|
||||
BINARY_VISIBLE = True # Does binary first so you can see what the linear regression
|
||||
# is being run on... might lower FPS though.
|
||||
|
||||
import sensor, image, time
|
||||
|
||||
sensor.reset()
|
||||
sensor.set_pixformat(sensor.GRAYSCALE)
|
||||
sensor.set_framesize(sensor.QQQVGA) # 80x60 (4,800 pixels) - O(N^2) max = 2,3040,000.
|
||||
sensor.skip_frames(time = 2000) # WARNING: If you use QQVGA it may take seconds
|
||||
clock = time.clock() # to process a frame sometimes.
|
||||
|
||||
while(True):
|
||||
clock.tick()
|
||||
img = sensor.snapshot().binary([THRESHOLD]) if BINARY_VISIBLE else sensor.snapshot()
|
||||
|
||||
# Returns a line object similar to line objects returned by find_lines() and
|
||||
# find_line_segments(). You have x1(), y1(), x2(), y2(), length(),
|
||||
# theta() (rotation in degrees), rho(), and magnitude().
|
||||
#
|
||||
# magnitude() represents how well the linear regression worked. It means something
|
||||
# different for the robust linear regression. In general, the larger the value the
|
||||
# better...
|
||||
line = img.get_regression([(255,255) if BINARY_VISIBLE else THRESHOLD], robust = True)
|
||||
|
||||
if (line): img.draw_line(line.line(), color = 127)
|
||||
print("FPS %f, mag = %s" % (clock.fps(), str(line.magnitude()) if (line) else "N/A"))
|
||||
|
||||
# About negative rho values:
|
||||
#
|
||||
# A [theta+0:-rho] tuple is the same as [theta+180:+rho].
|
||||
Loading…
Reference in New Issue
Block a user