/scratchProc.cpp

  1# include <iostream>
  2# include <opencv/cv.h>
  3# include <opencv/highgui.h>
  4# include "utils.h"
  5
  6using namespace cv;
  7int main(int argc, char** argv) {
  8    int cwa = CV_WINDOW_AUTOSIZE;
  9    
 10    Mat img, mask;
 11    std::string inpath = argv[1];
 12    mask = imread(inpath.c_str(), GRAYSCALE_IMAGE);
 13    if (mask.data == NULL) {
 14        std::cerr << "ERROR reading image 2" << std::endl;
 15        exit(-1);
 16    }
 17    namedWindow("mask", cwa);
 18    imshow("mask", mask);
 19    
 20    vector<vector<Point> > contours;
 21    findContours(mask, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
 22    std::cout << "contours size" << contours.size() << std::endl;
 23    std::cout << "1st contour size" << contours[0].size() << std::endl;
 24    
 25    Mat disp;
 26    cvtColor(mask, disp, CV_GRAY2RGB);
 27    drawContours(disp, contours, -1, Scalar(0,255,0),2,4);
 28    namedWindow("contours", cwa);
 29    imshow("contours", disp);
 30    waitKey(0);
 31
 32    float x = atof(argv[2]);
 33    float y = atof(argv[3]);
 34    Point midpt(x,y);
 35    float dx = atof(argv[4]);
 36    float dy = atof(argv[5]);
 37    float z = sqrt(dx*dx + dy*dy); 
 38    dx = dx/z;
 39    dy = dy/z;
 40    Vec2f norm(dx, dy);
 41    std::cout << "in params: " << x << " " << y << " " << dx << " " << dy << std::endl;
 42    
 43    float radius = 100;
 44    float ex_armw = 23;
 45    Point point1(norm[0]*ex_armw + x, norm[1]*ex_armw + y);
 46    Point point2(-norm[0]*ex_armw + x, -norm[1]*ex_armw + y);
 47    
 48    vector<Point> chosenPts1; 
 49    vector<Point> chosenPts2; 
 50    
 51    for (int i=0; (unsigned)i<contours.size(); i++) {
 52        for (int j=0; (unsigned)j<contours[i].size(); j++) {
 53            Point diff1 = point1 - contours[i][j];
 54            Point diff2 = point2 - contours[i][j];
 55            float dist1sq = diff1.x*diff1.x + diff1.y*diff1.y;
 56            float dist2sq = diff2.x*diff2.x + diff2.y*diff2.y;
 57            
 58            if (dist1sq <= radius*radius) {
 59                Point newpoint(contours[i][j]);
 60                chosenPts1.push_back(newpoint);
 61            }
 62            
 63            if (dist2sq <= radius*radius) {
 64                Point newpoint(contours[i][j]);
 65                chosenPts2.push_back(newpoint);
 66            }
 67            
 68        }
 69    }
 70    
 71    Mat Dx, Dy;
 72    inpath = argv[6];
 73    Dx = imread(inpath.c_str(), GRAYSCALE_IMAGE);
 74    if (Dx.data == NULL) {
 75        std::cerr << "ERROR reading image 3" << std::endl;
 76        exit(-1);
 77    }
 78
 79    inpath = argv[7];
 80    Dy = imread(inpath.c_str(), GRAYSCALE_IMAGE);
 81    if (Dy.data == NULL) {
 82        std::cerr << "ERROR reading image 4" << std::endl;
 83        exit(-1);
 84    }
 85    namedWindow("Dx", cwa);
 86    imshow("Dx", Dx);
 87    namedWindow("Dy", cwa);
 88    imshow("Dy", Dy);
 89    
 90    std::cout << "Dx, Dy type: " << Dx.type() << std::endl;
 91    float dirSimThresh = .9;
 92    Point bestPt1(0,0);
 93    float bestscore = -10000000;
 94    
 95    for (int i=0; (unsigned)i<chosenPts1.size(); i++) {
 96        int r = (int)chosenPts1[i].y;
 97        int c = (int)chosenPts1[i].x;
 98        
 99        float gradx = Dx.at<float>(r,c);
100        float grady = Dy.at<float>(r,c);
101        
102        float dirSim = (gradx*dx + grady*dy)/sqrt(gradx*gradx + grady*grady);
103        float gradStrength = sqrt(gradx*gradx + grady*grady);
104        
105        if (dirSim > dirSimThresh) {
106            std::cout << chosenPts1[i].x << " " << chosenPts1[i].y << std::endl;
107            if (gradStrength > bestscore) {
108                bestPt1.x = chosenPts1[i].x;   
109                bestPt1.y = chosenPts1[i].y;
110                bestscore = gradStrength;
111            }
112        }
113    }
114    
115    std::cout << "bestPt1=" << bestPt1.x << " " << bestPt1.y << std::endl;
116
117}