// returns approximate angle of yellow road line // or UNKNOWN if no pixel of yellow line visible double process_camera_image(const unsigned char *image) { int num_pixels = camera_height * camera_width; // number of pixels in the image const unsigned char REF[3] = { 132, 119, 48 }; // road yellow int sumx = 0; // summed x position oupf pixels int pixel_count = 0; // yellow pixels count const unsigned char *pixel = image; int x; for (x = 0; x < num_pixels; x++, pixel += 3) { if (color_diff(pixel, REF) < 30) { sumx += x % camera_width; pixel_count++; // count yellow pixels } } // if no pixels was detected... if (pixel_count == 0) return UNKNOWN; return ((double)sumx / pixel_count / camera_width - 0.5) * camera_fov; }
void DespeckleOperation::executePixel(float output[4], int x, int y, void * /*data*/) { float w = 0.0f; float color_org[4]; float color_mid[4]; float color_mid_ok[4]; float in1[4]; int x1 = x - 1; int x2 = x; int x3 = x + 1; int y1 = y - 1; int y2 = y; int y3 = y + 1; CLAMP(x1, 0, getWidth() - 1); CLAMP(x2, 0, getWidth() - 1); CLAMP(x3, 0, getWidth() - 1); CLAMP(y1, 0, getHeight() - 1); CLAMP(y2, 0, getHeight() - 1); CLAMP(y3, 0, getHeight() - 1); float value[4]; this->m_inputValueOperation->read(value, x2, y2, NULL); // const float mval = 1.0f - value[0]; this->m_inputOperation->read(color_org, x2, y2, NULL); #define TOT_DIV_ONE 1.0f #define TOT_DIV_CNR (float)M_SQRT1_2 #define WTOT (TOT_DIV_ONE * 4 + TOT_DIV_CNR * 4) #define COLOR_ADD(fac) \ { \ madd_v4_v4fl(color_mid, in1, fac); \ if (color_diff(in1, color_org, this->m_threshold)) { \ w += fac; \ madd_v4_v4fl(color_mid_ok, in1, fac); \ } \ } zero_v4(color_mid); zero_v4(color_mid_ok); this->m_inputOperation->read(in1, x1, y1, NULL); COLOR_ADD(TOT_DIV_CNR) this->m_inputOperation->read(in1, x2, y1, NULL); COLOR_ADD(TOT_DIV_ONE) this->m_inputOperation->read(in1, x3, y1, NULL); COLOR_ADD(TOT_DIV_CNR) this->m_inputOperation->read(in1, x1, y2, NULL); COLOR_ADD(TOT_DIV_ONE) #if 0 this->m_inputOperation->read(in2, x2, y2, NULL); madd_v4_v4fl(color_mid, in2, this->m_filter[4]); #endif this->m_inputOperation->read(in1, x3, y2, NULL); COLOR_ADD(TOT_DIV_ONE) this->m_inputOperation->read(in1, x1, y3, NULL); COLOR_ADD(TOT_DIV_CNR) this->m_inputOperation->read(in1, x2, y3, NULL); COLOR_ADD(TOT_DIV_ONE) this->m_inputOperation->read(in1, x3, y3, NULL); COLOR_ADD(TOT_DIV_CNR) mul_v4_fl(color_mid, 1.0f / (4.0f + (4.0f * (float)M_SQRT1_2))); // mul_v4_fl(color_mid, 1.0f / w); if ((w != 0.0f) && ((w / WTOT) > (this->m_threshold_neighbor)) && color_diff(color_mid, color_org, this->m_threshold)) { mul_v4_fl(color_mid_ok, 1.0f / w); interp_v4_v4v4(output, color_org, color_mid_ok, value[0]); } else { copy_v4_v4(output, color_org); } }