lots of vision stuff from Ben
diff --git a/vision/CameraProcessor.cpp b/vision/CameraProcessor.cpp
new file mode 100644
index 0000000..41e6a2d
--- /dev/null
+++ b/vision/CameraProcessor.cpp
@@ -0,0 +1,451 @@
+
+#include "CameraProcessor.h"
+
+// create a new target
+Target::Target(std::vector<cv::Point> new_contour,
+ std::vector<cv::Point> new_raw_contour,
+ FullRect new_rect, int new_weight, bool _is_90) {
+ this_contour = new_contour;
+ raw_contour = new_raw_contour;
+ rect = new_rect;
+ weight = new_weight;
+ height = getHeight(_is_90);
+}
+
+// calculate distance to target
+double Target::getHeight(bool is_90) {
+ // The 780.3296 is at full resolution 640x480, and we need
+ // to scale back to 320x240
+ //static const double cam_l = 780.3296 / 2.0;
+ ////static const double cam_d = 20.78096;
+ double height;
+ if (is_90) {
+ height = ((rect.ul.x - rect.ur.x) +
+ (rect.bl.x - rect.br.x)) / 2.0;
+ } else {
+ height = ((rect.ur.y + rect.ul.y) -
+ (rect.br.y + rect.bl.y)) / 2.0;
+ }
+ //return cam_l * 12.0 / height;
+ return height;
+}
+
+void Target::refineTarget() {
+ printf("okay refined\n");
+}
+
+FullRect::FullRect() {
+ ur.x = -1;
+ ur.y = -1;
+ ul.x = -1;
+ ul.y = -1;
+ br.x = -1;
+ br.y = -1;
+ bl.x = -1;
+ bl.y = -1;
+}
+
+// turns a contour into easier to access structure
+FullRect calcFullRect(std::vector<cv::Point> *contour){
+ FullRect rect;
+ for(int i=0; i<4; i++){
+ cv::Point2f pt = (*contour)[i];
+ rect.centroid.x += pt.x;
+ rect.centroid.y += pt.y;
+ }
+ rect.centroid.x /= 4;
+ rect.centroid.y /= 4;
+ for(int i=0; i<4; i++){
+ cv::Point2f pt = (*contour)[i];
+ if(pt.y > rect.centroid.y ){
+ if(pt.x > rect.centroid.x){
+ if (rect.ul.x < 0) {
+ rect.ul = pt;
+ } else {
+ rect.ur = pt;
+ }
+ }else{
+ if (rect.ur.x < 0) {
+ rect.ur = pt;
+ } else {
+ rect.ul = pt;
+ }
+ }
+ if (rect.ul.x > 0 && rect.ur.x > 0) {
+ // both are set, so if we got it wrong correct it here
+ if (rect.ul.x > rect.ur.x) {
+ pt = rect.ur;
+ rect.ur = rect.ul;
+ rect.ul = pt;
+ }
+ }
+ }else{
+ if(pt.x > rect.centroid.x){
+ if (rect.bl.x < 0) {
+ rect.bl = pt;
+ } else {
+ rect.br = pt;
+ }
+ }else{
+ if (rect.br.x < 0) {
+ rect.br = pt;
+ } else {
+ rect.bl = pt;
+ }
+ }
+ if (rect.bl.x > 0 && rect.br.x > 0) {
+ // both are set, so if we got it wrong correct it here
+ if (rect.bl.x > rect.br.x) {
+ pt = rect.br;
+ rect.br = rect.bl;
+ rect.bl = pt;
+ }
+ }
+ }
+ }
+ return rect;
+}
+
+// quickly remove targets that do not fit a very broad set of constraints
+bool cullObvious(FullRect rect, double outside_size){
+ // first check that could see a target this size
+ // Calculated from dave's simulation, shloud be 850 and 72000 if filled
+ if((outside_size < 500) || (outside_size > 90000)){
+ return false;
+ }
+ // Targets on the edge are at best inaccurate.
+ // In this case, we just want to point the right way,
+ // so this is no longer a valid assumption.
+ /*if( rect.ur.x < 2 || rect.ur.y < 2 || rect.ur.x > 637 || rect.ur.y > 477 ||
+ rect.ul.x < 2 || rect.ul.y < 2 || rect.ul.x > 637 || rect.ul.y > 477 ||
+ rect.br.x < 2 || rect.br.y < 2 || rect.br.x > 637 || rect.br.y > 477 ||
+ rect.bl.x < 2 || rect.bl.y < 2 || rect.bl.x > 637 || rect.bl.y > 477){
+ return false;
+ }*/
+ // make sure the sides are close to the right ratio of a rect
+ // some really odd shapes get confusing
+ double ratio = norm(rect.ur-rect.ul)/norm(rect.br-rect.bl);
+ if( ratio < .7 || ratio > 1.4 ) {
+ return false;
+ }
+ ratio = norm(rect.ur-rect.br)/norm(rect.ul-rect.bl);
+ if( ratio < .7 || ratio > 1.4 ) {
+ return false;
+ }
+
+ return true;
+}
+
+// sum over values between these two points and normalize
+// see Bresenham's Line Algorithm for the logic of moving
+// over all the pixels between these two points.
+double ProcessorData::calcHistComponent(
+ cv::Point2i start,
+ cv::Point2i end,
+ cv::Mat thresh_img){
+ int dx = abs(end.x - start.x);
+ int dy = abs(end.y - start.y);
+ int sx = (start.x < end.x) ? 1 : -1;
+ int sy = (start.y < end.y) ? 1 : -1;
+ int error = dx-dy;
+
+ int total = 0;
+ int value = 0;
+ int total_error;
+#if LOCAL_DEBUG
+ IplImage gd = *global_display;
+#endif
+ IplImage ti = thresh_img;
+ while(1){
+ total++;
+
+ uchar* ptr = (uchar*) (ti.imageData + start.y * ti.widthStep + start.x);
+ if((int) *ptr) value++;
+ // draw line checked
+#if LOCAL_DEBUG
+ uchar* ptr2 = (uchar*) (gd.imageData + start.y * gd.widthStep + start.x*3);
+ *ptr2++ = 0;
+ *ptr2++ = 255;
+ *ptr2 = 0;
+#endif
+ if(start.x == end.x && start.y == end.y) break;
+ total_error = 2 * error;
+ if(total_error > -dy){
+ error -= dy;
+ start.x += sx;
+ }
+ if(total_error < dx){
+ error += dx;
+ start.y += sy;
+ }
+ }
+ return (double)value/(double)total;
+}
+
+// just a distance function
+double chiSquared(int length, double* histA, double* histB){
+ double sum = 0;
+ for(int i=0; i<length;i++){
+ double diff = *(histB+i) - *(histA+i);
+ sum += (diff * diff) / *(histA+i);
+ }
+ return sum;
+}
+// euclidiean dist function
+double L2_dist(int length, double* histA, double* histB){
+ double sum = 0;
+ for(int i=0; i<length;i++){
+ double diff = *(histB+i) - *(histA+i);
+ sum += (diff * diff);
+ }
+ return sqrt(sum);
+}
+
+// calc and compare the histograms to the desired
+double ProcessorData::checkHistogram(FullRect rect, cv::Mat thresh_img){
+ // found horiz histogram
+ double hist_lr[HIST_SIZE];
+ // step size along left edge
+ cv::Point2f delta_left = (rect.ul - rect.bl)*HIST_SIZE_F;
+ // step size along right edge
+ cv::Point2f delta_right = (rect.ur - rect.br)*HIST_SIZE_F;
+ // sum each left to right line for the histogram
+ for(int i=0; i<HIST_SIZE; i++){
+ hist_lr[i] = calcHistComponent(rect.bl+i*delta_left,
+ rect.br+i*delta_right,thresh_img);
+ }
+ double check_vert = L2_dist(HIST_SIZE, vert_hist, hist_lr);
+ // found vert histogram
+ double hist_ub[HIST_SIZE];
+ // step size along bottom edge
+ cv::Point2f delta_bottom = (rect.bl - rect.br)*HIST_SIZE_F;
+ // step size along top edge
+ cv::Point2f delta_top = (rect.ul - rect.ur)*HIST_SIZE_F;
+ // sum each top to bottom line for the histogram
+ for(int i=0; i<HIST_SIZE; i++){
+ hist_ub[i] = calcHistComponent(rect.ur+i*delta_top,
+ rect.br+i*delta_bottom,thresh_img);
+ }
+ double check_horz = L2_dist(HIST_SIZE, horz_hist, hist_ub);
+
+ // average the two distances
+ double check = (check_vert + check_horz)/2.0;
+ return check;
+}
+
+// return smallest
+template<class T> inline T Min3(T x, T y, T z) {
+ return y <= z ? (x <= y ? x : y)
+ : (x <= z ? x : z);
+}
+
+// return largest
+template<class T> inline T Max3(T x, T y, T z) {
+ return y >= z ? (x >= y ? x : y)
+ : (x >= z ? x : z);
+}
+
+// transforms the contour
+void makeConvex(std::vector<cv::Point> *contour){
+ std::vector<cv::Point2i> hull;
+ convexHull(*contour, hull, false);
+ *contour = hull;
+}
+
+// basic init
+ProcessorData::ProcessorData(int width, int height, bool is_90_) {
+ is_90 = is_90_;
+ // series b images from nasa
+ h1=79; s1=53; v1=82;
+ h2=200; s2=255; v2=255;
+ // For images from Jerry
+ //h1=79; s1=0; v1=11;
+ //h2=160; s2=255; v2=255;
+ img_width = width;
+ img_height = height;
+ buffer_size = img_height * img_width * 3;
+#if LOCAL_DEBUG
+ global_display = cvCreateImage(cvSize(width, height),
+ IPL_DEPTH_8U, 3);
+#endif
+ grey_image = cvCreateImage(cvSize(width, height),
+ IPL_DEPTH_8U, 1);
+ grey_mat = new cv::Mat(grey_image);
+
+ // calculate a desired histogram before we start
+ int j = 0;
+ for(double i=0; j<HIST_SIZE; i+=HIST_SIZE_F){
+ if (is_90) {
+ if(i < 2.0/12.0 || i > (1.0-2.0/12.0) ) horz_hist[j] = 1;
+ else horz_hist[j] = 0.10;
+ if(i < 2.0/24.0 || i > (1.0-2.0/24.0) ) vert_hist[j] = 1;
+ else vert_hist[j] = 4.0/24.0;
+ } else {
+ if(i < 2.0/12.0 || i > (1.0-2.0/12.0) ) vert_hist[j] = 1;
+ else vert_hist[j] = 0.10;
+ if(i < 2.0/24.0 || i > (1.0-2.0/24.0) ) horz_hist[j] = 1;
+ else horz_hist[j] = 4.0/24.0;
+ }
+ j++;
+ }
+}
+
+// throw stuff away
+ProcessorData::~ProcessorData() {
+ cvReleaseImage(&grey_image);
+ cvReleaseImage(&src_header_image);
+ delete(grey_mat);
+}
+
+// reset processor data freeing as little as possible.
+void ProcessorData::clear() {
+ target_list.clear();
+ contour_pairs.clear();
+ hierarchy.clear();
+}
+
+
+// r,g,b values are from 0 to 255
+// h = [0,255], s = [0,255], v = [0,255]
+// if s == 0, then h = 0 (undefined)
+void ProcessorData::RGBtoHSV(uchar r, uchar g, uchar b,
+ uchar *h, uchar *s, uchar *v ) {
+ uchar min, max, delta;
+ min = Min3( r, g, b );
+ max = Max3( r, g, b );
+ *v = max;
+ delta = max - min;
+ if (max == 0 || delta == 0) {
+ *s = 0;
+ *h = 0;
+ return;
+ }
+ *s = (255 * long(delta))/max;
+ if (max == r) {
+ *h = 0 + 43*(g - b)/(delta);
+ } else if (max == g) {
+ *h = 85 + 43*(b - r)/(delta);
+ } else {
+ *h = 171 + 43*(r - g)/(delta);
+ }
+}
+
+// keep anything that is in our HVS wedge
+// by far the longest running function in this code
+void ProcessorData::threshold(uchar* buffer) {
+#if LOCAL_DEBUG
+ uchar * draw_buffer = (uchar *) global_display->imageData;
+#endif
+ for (int i = 0,j = 0;
+ i < (buffer_size);
+ i+= 3,j++) {
+ uchar r = buffer[i + 2];
+ uchar g = buffer[i + 1];
+ uchar b = buffer[i + 0];
+ uchar h, s, v;
+
+ RGBtoHSV(r, g, b, &h, &s, &v);
+
+ /* if (h == 0 && s == 0 && v >= v1 && v <= v2) {
+#if LOCAL_DEBUG
+ draw_buffer[i + 0] = 200;
+ draw_buffer[i + 1] = 50;
+ draw_buffer[i + 2] = 100;
+#endif
+ grey_image->imageData[j] = 255;
+ } else */if(h >= h1 && h <= h2 && v >= v1 && v <= v2 && s >= s1 && s <= s2){
+#if LOCAL_DEBUG
+/* draw_buffer[i + 0] = 255;
+ draw_buffer[i + 1] = 0;
+ draw_buffer[i + 2] = 0;*/
+#endif
+ grey_image->imageData[j] = 255;
+ }else{
+#if LOCAL_DEBUG
+/* draw_buffer[i + 0] = buffer[i + 0];
+ draw_buffer[i + 1] = buffer[i + 1];
+ draw_buffer[i + 2] = buffer[i + 2];*/
+#endif
+ grey_image->imageData[j] = 0;
+ }
+ }
+}
+
+// run find contours and try to make them squares
+void ProcessorData::getContours() {
+ std::vector<std::vector<cv::Point> > raw_contours;
+ //cv::findContours(*grey_mat, raw_contours, hierarchy, CV_RETR_LIST,
+ // CV_CHAIN_APPROX_SIMPLE);
+ cv::findContours(*grey_mat, raw_contours, hierarchy, CV_RETR_EXTERNAL,
+ CV_CHAIN_APPROX_SIMPLE);
+#if LOCAL_DEBUG
+ cv::Mat global_mat(global_display);
+ cv::Scalar color(255,0,0);
+ drawContours(global_mat,raw_contours,-1,color,1);
+
+ std::vector<std::vector<cv::Point> > p_contours;
+#endif
+ if (!raw_contours.empty()) {
+ std::vector<std::vector<cv::Point2i> >::iterator contour_it;
+ for (contour_it=raw_contours.begin();
+ contour_it != raw_contours.end();
+ contour_it++) {
+ // make the contours convex
+ makeConvex(&(*contour_it));
+ std::vector<cv::Point> contour;
+ //then make them rectangle
+ approxPolyDP(*contour_it, contour, 9, true);
+ // stick the raw and processed contours together
+ std::pair<std::vector<cv::Point>,
+ std::vector<cv::Point> > a_pair(
+ *contour_it, contour);
+#if LOCAL_DEBUG
+ p_contours.push_back(contour);
+#endif
+ // and put them in a list
+ contour_pairs.push_back(a_pair);
+ }
+ }
+#if LOCAL_DEBUG
+ cv::Scalar color2(0,0,255);
+ drawContours(global_mat,p_contours,-1,color2,CV_FILLED);
+#endif
+}
+
+// filter the contours down to a list of targets
+void ProcessorData::filterToTargets() {
+ std::vector<std::pair<
+ std::vector<cv::Point>,
+ std::vector<cv::Point> > >::iterator contour_it;
+ for(contour_it=contour_pairs.begin();
+ contour_it != contour_pairs.end();
+ contour_it++){
+ double check = 0;
+ std::vector<cv::Point> raw_contour = std::get<0>(*contour_it);
+ std::vector<cv::Point> contour = std::get<1>(*contour_it);
+ FullRect rect = calcFullRect(&contour);
+ if(contour.size() == 4 &&
+ cullObvious(rect, contourArea(contour)) &&
+ (check = checkHistogram(rect, *grey_mat)) <= HIST_MATCH){
+ // now we have a target, try to improve the square
+#if LOCAL_DEBUG
+ /* printf("________\n");
+ printf("\tcont= %d raw= %d\n",
+ (int)contour.size(), (int)raw_contour.size());
+ std::vector<cv::Point2i>::iterator point_it;
+ for(point_it=raw_contour.begin();
+ point_it != raw_contour.end(); point_it++){
+ printf("(%d,%d)", point_it->x, point_it->y);
+ }
+ printf("\n");*/
+#endif
+ target_list.push_back(Target(contour,
+ raw_contour, rect, check, is_90));
+ }
+ //if(contour.size() == 4 &&
+ // cullObvious(rect, contourArea(contour)) ) {
+ // printf("check= %.2f\n", check);
+ //}
+ }
+}
+