/home/fwph/code/wurde/modules/visionModule/Operators/face/FaceOperator.cpp

Go to the documentation of this file.

#include "FaceOperator.H"
#include <Logger.H>
#include <string>

#include "cv.h"
#include "highgui.h"
#include "cvaux.h"

#include "cvtypes.h"

#include "MyHaar.H"


using namespace std;
using namespace RobotObjects;
using namespace RobotVision;

FaceOperator::FaceOperator(std::string pName) : VisionOperator(pName), mRangeFinder(STRAT_ASSIGNED, "LaserClient"), mPtu(STRAT_ASSIGNED, "PTUClient"), mFaceDetector(pName) {
        m_state = STATE_RUN;
        g_debug("Registering a SickLaser consumer");
        registerConsumer(&mRangeFinder);
        g_debug("Registering a PTU consumer");
        registerConsumer(&mPtu);
        g_debug("Registering a FaceDetector supplier");
        registerSupplier(&mFaceDetector);
        count = 0;
}

void FaceOperator::initOperator() {
        g_debug("Initializing Face Detector");
                
        g_debug("Creating HAAR Cascade");

  //  Hard-coded at the moment until I can get the configuration working
        //mCascade = (CvHaarClassifierCascade*)cvLoad( "haarcascade_frontalface_default.xml" ,0, 0, 0 );
        
        /*char msg[1024]; 
        sprintf(msg, "%'s Cascade name from config: %s", m_name.c_str(), g_globalConfiguration.getModuleOption(m_name,"HAARCascadeFile").c_str());      
  if(g_globalConfiguration.haveModuleOption(m_name,"HAARCascadeFile")) {        g_debug(msg); } else { g_debug("Don't have HAARCascadeFile"); }
        sprintf(msg, "%'s 'asdf' from config: %s", m_name.c_str(), g_globalConfiguration.getModuleOption(m_name,"asdf").c_str());       
  if(g_globalConfiguration.haveModuleOption(m_name,"asdf")) {   g_debug(msg); } else { g_debug("Don't have asdf"); }
  g_debug(msg);*/
        std::string configfile;

        if(g_globalConfiguration.haveModuleOption(m_name,"HAARCascadeFile")){
               configfile=g_globalConfiguration.getModuleOption(m_name,"HAARCascadeFile");
               if(configfile[0]!='/'){
                      configfile=g_globalConfiguration.getConfigDirectory()+"/"+configfile;
               }
        }else if(g_globalConfiguration.haveOption("HAARCascadeFile")){
               configfile=g_globalConfiguration.getOption("HAARCascadeFile");
               if(configfile[0]!='/'){
                      configfile=g_globalConfiguration.getConfigDirectory()+"/"+configfile;
               }
        }else{
               configfile=g_globalConfiguration.getConfigDirectory()+"/"+"haarcascade_frontalface_default.xml";
        }

        //      mCascade = (CvHaarClassifierCascade*)cvLoad( g_globalConfiguration.getOption("HAARCascadeFile").c_str(), 0, 0, 0 );
        mCascade = (CvHaarClassifierCascade*)cvLoad( configfile.c_str(), 0, 0, 0 );

        if(!g_globalConfiguration.haveModuleOption(m_name,"MinHeight")||
           !g_globalConfiguration.haveModuleOption(m_name,"MaxHeight")){
               mMinHeight=1;
               mMaxHeight=2;
        }else{
               mMinHeight = atof(g_globalConfiguration.getModuleOption(m_name,"MinHeight").c_str());
               mMaxHeight = atof(g_globalConfiguration.getModuleOption(m_name,"MaxHeight").c_str());
        }
        

        if(!g_globalConfiguration.haveModuleOption(m_name,"MinSize")||
           !g_globalConfiguration.haveModuleOption(m_name,"MaxSize")){
               mMinSize=0.08;
               mMaxSize=0.4;
        }else{
               mMinSize = atof(g_globalConfiguration.getModuleOption(m_name,"MinSize").c_str());
               mMaxSize = atof(g_globalConfiguration.getModuleOption(m_name,"MaxSize").c_str());
        }

        
        mStorage = cvCreateMemStorage(0);
        
        m_state = STATE_RUN;
        
        //This really should be FORMAT_CHAR_1_GRAY, but it's not implemented yet
        m_format=FORMAT_CHAR_1_GRAY;
        m_stereo=false;
        
        // Uncomment only if you are going to be debugging, as it creates an X window that displays the stream
        // cvNamedWindow("Name of Window", CV_WINDOW_AUTOSIZE);
        mFrames = 0;
        g_debug("Done initializing FD");
}

struct pixel {
  char b,g,r;
};

void ColorPixel(void * pImg, int pX, int pY, char pR, char pG, char pB) {
  pixel *img = (pixel *)pImg;
  if(pX >= 0 && pX < 320 && pY >= 0 && pY < 240) {
                img[pY * 320 + pX].r = pR;
                img[pY * 320 + pX].g = pG;
                img[pY * 320 + pX].b = pB;              
        }
}
void FaceOperator::DrawLaserDataDots(IplImage *pImg, double *pData, unsigned char r, unsigned char g, unsigned char b) {

        double pan, tilt;
                
        GetPTUData(&pan, &tilt);
  //Put some nice dots on the screen where the laser hit
  for(int i = 0; i < 181; i++) {
                double radians = i / 180.0 * 3.141592 - (3.141592 / 2.0);
                
                double cp = cos(pan);  //Might need to change these to positive
                double sp = sin(pan);
                double ct = cos(tilt);
                double st = sin(tilt);
                
                double wx = cos(radians) * pData[i];
                double wy = sin(radians) * pData[i];
                double wz = 0.38;
                
          double camx =  ct * cp * wx + ct * sp  * wy + st * wz - .13 * cp * ct ;
    double camy = -sp * wx + cp * wy + sp * 0.13 - 0.09;
    double camz = -st * cp * wx + -st * sp * wy + ct * wz + 0.13 * cp * st - 1.48 * ct;
                
                int xpix = 320 - int(387.5 * tan(atan2(camy, camx)) + 160);
                int ypix = 240 - int(387.5 * tan(atan2(camz, camx)) + 120);
                
                ColorPixel(pImg->imageData, xpix, ypix, r, g, b);
                ColorPixel(pImg->imageData, xpix+1, ypix, r, g, b);
                ColorPixel(pImg->imageData, xpix-1, ypix, r, g, b);
                ColorPixel(pImg->imageData, xpix, ypix+1, r, g, b);
                ColorPixel(pImg->imageData, xpix, ypix-1, r, g, b);
        }
        //printf("Pan: %f - Tilt: %f\n", pan, tilt);
}


bool done = false;

void FaceOperator::runOperator(RoleImage mImage) {
       if(mFrames == 0) {
              mBegin.now();
       }
       mFrames++;
        
       CvPoint pt1, pt2;
       char buff[256];

       cvClearMemStorage( mStorage );

       CvSize imgSize;
       imgSize.width = 320;
       imgSize.height = 240;

       /*       IplImage* temp= cvCreateImage(imgSize,IPL_DEPTH_8U,3);
       for(int i = 0;i < 76800;++i){
              temp[i*3]=mImage.second[i];
              temp[i*3+1]=mImage.second[i];
              temp[i*3+2]=mImage.second[i];
              }*/
       
        double ranges[181];
        for(int i =0; i < 181; i++) { ranges[i] = 99999999; }
        GetLaserData(ranges);
        //DrawLaserDataDots(mImage.second, ranges, 255,0,0);
        GetClosedLaserData(ranges);
        //DrawLaserDataDots(mImage.second, ranges, 0,255,0);
        double pan, tilt;
        GetPTUData(&pan, &tilt);

        CvSeq* faces = myHaarDetectObjects( mImage.second, mCascade, mStorage,1.1, 2, CV_HAAR_DO_CANNY_PRUNING, cvSize(20, 20), ranges, pan, tilt, mMinHeight, mMaxHeight, mImage.second,mMinSize,mMaxSize);
        //CvSeq* faces = cvHaarDetectObjects( mImage.second, mCascade, mStorage,1.1, 2, CV_HAAR_DO_CANNY_PRUNING, cvSize(40, 40));
        
        mFaceDetector.data.foundFaces.clear();
        
        //if(faces->total) {
         //std::cout << "FOUND A FACE!" << flush;
        //}
        
        mFaceDetector.data.foundFaces.clear();

        //if(faces->total > 0){
        g_logdebug << "Found "<<faces->total<< " faces."<<endl;
        //xs  }
        for( int i = 0; i < (faces ? faces->total : 0); i++ ){
               CvRect* r = (CvRect*)cvGetSeqElem( faces, i );
               FaceLocation f;
               f.width = r->width;
               f.height = r->height;
               f.origin.x(r->x);
               f.origin.y(r->y);
               f.centroid.x(r->x + r->width / 2.0);
               f.centroid.y(r->y + r->height / 2.0);
               mFaceDetector.data.foundFaces.push_back(f);
               g_logdebug << "Just added a face." << endl;
               
               pt1.x = r->x;
               pt2.x = (r->x+r->width);
               pt1.y = r->y;
               pt2.y = (r->y+r->height);
               // cvRectangle( temp, pt1, pt2, CV_RGB(255,0,0), 3, 8, 0 );
        }
        mFaceDetector.publishData();
        g_logdebug << "Just published some faces" << endl;
        
        //      sprintf(buff,"/usr/local/share/role/data/pict%05d.jpg",count);
        //cvSaveImage(buff, temp);
        ++count;

        //printf("(%i,%i)\n", mImage.second->width, mImage.second->height);
        
        //cvShowImage("Name of Window", mImage.second);

}

void FaceOperator::quitOperator() {
        
       //       mPtu.disconnect();
       //mRangeFinder.disconnect();
        
        mEnd.now();
        double seconds = mEnd.getSeconds() - mBegin.getSeconds();

  g_loginfo << "Frames per second: " << (mFrames / seconds) << endl;
}

void FaceOperator::GetLaserData(double *data) {
  bool gotdata = false;
                //printf("LOOP\n");
                while(mRangeFinder.newInfo()) {
                        mRangeFinder.getNextInfo();
                }
                while(mRangeFinder.newData()) {
                        mRangeFinder.getNextData();
                }
                if(mRangeFinder.data.ranges.size()) {
                for(int i = 0; i < 181; i++) {
                                data[i] = mRangeFinder.data.ranges[i];
          }
        }
}

void FaceOperator::GetPTUData(double *pPan, double *pTilt) {
  bool gotdata = false;
                while(mPtu.newInfo()) {
                mPtu.getNextInfo();  
                gotdata = true;
                }
        (*pPan) = mPtu.info.pan.getValue();
  (*pTilt) = mPtu.info.tilt.getValue();
}
void FaceOperator::GetClosedLaserData(double *pOut) {
  //We're assuming always 181 lasers here (!)
  
  double r = 0.3;
  PTUnit ptu("safeTomPTUserver");
  double data[181];
  GetLaserData(data);
  
  float angleMin = mRangeFinder.info.angle_min.getValue();
  float angleSep = mRangeFinder.info.theta_separation.getValue();
  
  

  for (int i = 0; i < 180; i++){
    double x = 9999999;
    int w = int(asin(r / data[i]) * 180 / 3.14159265358);     //FIXME: 90 is the biggest w will ever $                        float x = MAX_FLO
    //int w = 90;     //FIXME: 90 is the biggest w will ever $                        float x = MAX_FLOAT
    if(w == 0) {
      pOut[i] = data[i];
    }
    for (int j = 0; j < w; j++){
    
      float theta = j * 3.1415927 / 180.0;
      float xtemp;
      if (i+j >= 0 && i+j < 180){
        float l = data[i+j];
        xtemp = l * cos(theta) - sqrt( r*r - pow(l*sin(theta),2));
        if (xtemp < x) {
          x = xtemp;
        }
      }
      if (i-j >= 0 && i-j < 180){
        double l = data[i-j];
        xtemp = l * cos(-theta) - sqrt( r*r - pow(l*sin(-theta),2));
        if (xtemp < x){
          x = xtemp;
        }
      }
      pOut[i] = x;
    }
  } 
}

extern "C" {
       VisionOperator *FaceOperator_factory(std::string name) {
              return new FaceOperator(name);
       }
       
       class FaceOperatorProxy { 
       public:
              FaceOperatorProxy(){
                     operatorFactory["FaceOperator"] = FaceOperator_factory;
              }
       };
       
       FaceOperatorProxy p_Faceoperator;
}

---