[Robotflow-cvs] RobotFlow/Vision/src CvBiModalTest.cc,NONE,1.1

SourceForge Headquarters 225 Broadway Suite 1600 San Diego, CA 92101 +1 (858) 422-6466

Update of /cvsroot/robotflow/RobotFlow/Vision/src
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv23572

Added Files:
	CvBiModalTest.cc 
Log Message:
working version of the bi modal color model


--- NEW FILE: CvBiModalTest.cc ---
#ifndef _CVFINDCONTOURS_CC_
#define _CVFINDCONTOURS_CC_

#include "BufferedNode.h"
#include "cv.h"
#include "Image.h"
#include "highgui.h"

#include <cstdio>
#include <cstdlib>
#include <image.h>
#include <misc.h>
#include <pnmfile.h>
#include "segment-image.h"
#include <math.h>
#include <sys/timeb.h>

using namespace std;
using namespace FD;

namespace RobotFlow {

class CvBiModalTest;


DECLARE_NODE(CvBiModalTest)

/*Node
 *
 * @name CvBiModalTest
 * @category RobotFlow:Vision:OpenCV
 * @description Determine the contours in the input image.
 *
 * @input_name IN_IMAGE
 * @input_type Image
 * @input_description Current color frame to process.
 *
 * @parameter_name MAX_STD
 * @parameter_type float
 * @parameter_value 0
 * @parameter_description Low threshold used for edge searching.
 *
 * @parameter_name MIN_DIST
 * @parameter_type float
 * @parameter_value 255
 * @parameter_description High threshold used for edge searching.
 *
 * @output_name CONTOUR_IMG
 * @output_type Image
 * @output_description Image with contours drawed.
 *
 END*/
  
  class CvBiModalTest : public BufferedNode {
    
  private:
    IplImage *m_src;
    IplImage *m_dest;
    IplImage *m_mask;
    IplImage *m_edges;

  public:
    
    CvBiModalTest(string nodeName, ParameterSet params)
      : BufferedNode(nodeName, params)
    {
      m_imageInID = addInput("IN_IMAGE");
      m_imageOutID = addOutput("CONTOUR_IMG");
      
      m_minDist = dereference_cast<float>(parameters.get("MIN_DIST"));
      m_maxStd =  dereference_cast<float>(parameters.get("MAX_STD"));


      //Allocate memory
      m_src = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 3 );
      m_dest = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 3 );
      m_mask = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 1 );
      m_edges = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 1 );

    }

    ~CvBiModalTest() {
      cvReleaseImage(&m_src);
      cvReleaseImage(&m_dest);
      cvReleaseImage(&m_mask);
      cvReleaseImage(&m_edges);
     
    }

    bool processRect(CvRect rect, IplImage *src, IplImage *dest, double std_max, double min_dist) {

      //Set Region of interest
      cvSetImageROI(src,rect);
      cvSetImageROI(m_mask,rect);
      bool bimodal = false;
      int x1,y1,x2,y2;
      
      x1 = rect.x;
      y1 = rect.y;
      x2 = rect.x + rect.width;
      y2 = rect.y + rect.height;


      CvScalar mean0,mean1,mean2;
      CvScalar stddev0,stddev1,stddev2;

      //Get Mean, std dev
      cvAvgSdv(src,&mean0,&stddev0);

      
      //cerr<<"CV made (mean0): "<<mean0.val[0]<<" "<<mean0.val[1]<<" "<<mean0.val[2]<<endl;
      //cerr<<"CV made (stddev0): "<<stddev0.val[0]<<" "<<stddev0.val[1]<<" "<<stddev0.val[2]<<endl;
      //cerr<<"JM TEST "<<sqrt(stddev0.val[0] * stddev0.val[0] + 
      //			     stddev0.val[1] * stddev0.val[1] + 
      //		     stddev0.val[2] * stddev0.val[2])<<endl;

      //LOOK FOR STDDEV1 IF WE SHOULD CONTINUE...
      
      //BRIGHTER
      mean1.val[0] = min(mean0.val[0] + 1,255.0);
      mean1.val[1] = min(mean0.val[1] + 1,255.0);
      mean1.val[2] = min(mean0.val[2] + 1,255.0);
      
      //DARKER
      mean2.val[0] = max(mean0.val[0] - 1,0.0);
      mean2.val[1] = max(mean0.val[1] - 1,0.0);
      mean2.val[2] = max(mean0.val[2] - 1,0.0);

      int nb[2] = {0 ,0 };
      
      //calculate euclidian distance
      for (int row = y1; row < y2; row++) {

	char *basePtr = &m_src->imageData[(row * m_src->widthStep) + (rect.x * m_src->nChannels)];
	char *imgPtr = basePtr;
	char *maskPtr = &m_mask->imageData[(row * m_mask->widthStep) + (rect.x * m_mask->nChannels)];

	for (; imgPtr < basePtr + (m_src->nChannels *  rect.width); imgPtr += m_src->nChannels, maskPtr += m_mask->nChannels) {
	  
	  //get image data
	  double Red = (unsigned char) imgPtr[0];
	  double Green = (unsigned char) imgPtr[1];
	  double Blue = (unsigned char) imgPtr[2];
  	  	       
	  double dist1 = (Red - mean1.val[0]) * (Red - mean1.val[0]) +
	    (Green - mean1.val[1]) * (Green - mean1.val[1]) +
	    (Blue - mean1.val[2]) * (Blue - mean1.val[2]);

	  double dist2 = (Red - mean2.val[0]) * (Red - mean2.val[0]) +
	    (Green - mean2.val[1]) * (Green - mean2.val[1]) +
	    (Blue - mean2.val[2]) * (Blue - mean2.val[2]);

	  if (dist1 <= dist2) {
	    //DIST1 SMALLER, SET MASK PROPERLY FOR MEAN FUNCTION TO BE CALCULATED PROPERLY
	    *maskPtr = 1;	    nb[0]++;
	  }
	  else {
	    //DIST2 SMALLER, SET MASK PROPERLY FOR MEAN FUNCTION TO BE CALCULATED PROPERLY
	    *maskPtr = 0;
	    nb[1]++;
	  }
	}
      }
      
      //CALCULATE NEW AVG AND STD FOR NEWLY FOUND PIXELS
      cvAvgSdv(src,&mean1,&stddev1,m_mask);

      
      //INVERT MASK
      for (int row = y1; row < y2; row++) {	
	char *basePtr = &m_mask->imageData[(row * m_mask->widthStep) + (rect.x * m_mask->nChannels)];
	char *maskPtr = basePtr;
	
	for (; maskPtr < basePtr + (m_mask->nChannels *  rect.width); maskPtr += m_mask->nChannels) {
	  if (*maskPtr == 1) {
	    *maskPtr = 0;
	  }
	  else {
	    *maskPtr = 1;
	  }
	}
      }
      
      //TODO STDDEV ON NO ELEMENT ?

      //CALCULATE NEW AVG AND STD FOR NEWLY FOUND PIXELS
      cvAvgSdv(src,&mean2,&stddev2,m_mask);

      //cerr<<"CV made (mean1): "<<mean1.val[0]<<" "<<mean1.val[1]<<" "<<mean1.val[2]<<endl;
      //cerr<<"CV made (stddev1): "<<stddev1.val[0]<<" "<<stddev1.val[1]<<" "<<stddev1.val[2]<<endl;
      //cerr<<"JM TEST "<<sqrt(stddev1.val[0] * stddev1.val[0] + 
      //			     stddev1.val[1] * stddev1.val[1] + 
      //			     stddev1.val[2] * stddev1.val[2])<<endl;

      //cerr<<"CV made (mean2): "<<mean2.val[0]<<" "<<mean2.val[1]<<" "<<mean2.val[2]<<endl;
      //cerr<<"CV made (stddev2): "<<stddev2.val[0]<<" "<<stddev2.val[1]<<" "<<stddev2.val[2]<<endl;
      //cerr<<"JM TEST "<<sqrt(stddev2.val[0] * stddev2.val[0] + 
      //		     stddev2.val[1] * stddev2.val[1] + 
      //		     stddev2.val[2] * stddev2.val[2])<<endl;

      //cerr<<"nbsample " <<nb[0]<<" "<<nb[1]<<endl;

      //DISTANCE ENTRE LES 2 MOYENNES SOIT SUFFISANTE
      //STD SOIT PETIT PAR RAPPORT A LA MOYENNE
      double mean_dist = sqrt((mean1.val[0] - mean2.val[0]) * (mean1.val[0] - mean2.val[0]) +
			      (mean1.val[1] - mean2.val[1]) * (mean1.val[1] - mean2.val[1]) +
			      (mean1.val[2] - mean2.val[2]) * (mean1.val[2] - mean2.val[2]));

      double std1sum = sqrt(stddev1.val[0] * stddev1.val[0]+ 
			    stddev1.val[1] * stddev1.val[1] + 
			    stddev1.val[2] * stddev1.val[2]);

      double std2sum = sqrt(stddev2.val[0] * stddev2.val[0]+ 
			    stddev2.val[1] * stddev2.val[1] + 
			    stddev2.val[2] * stddev2.val[2]);


      if (mean_dist > min_dist /*&&
	  std1sum < std_max &&
	  std2sum < std_max */ ) {
	bimodal = true;
      }

      return bimodal;

    }

    
    void calculate(int output_id, int count, Buffer &out)
    {
      try {
	RCPtr<Image> imageRef = getInput(m_imageInID, count);

	//copy source image
	memcpy(m_src->imageData, imageRef->get_data(), imageRef->get_size());
	
	
	//reset ROI
	cvResetImageROI(m_src);
	cvResetImageROI(m_dest);
	cvResetImageROI(m_mask);
	cvResetImageROI(m_edges);

	cvCopy(m_src,m_dest,NULL);

	struct timeb t1, t2;
	ftime(&t1);

	//TILE MODE
	/*

	for (int y = 0; y < 240; y+= 16) {
	  for (int x = 0; x < 320; x+= 16) {
	    CvRect rect;
	    rect.x = x;
	    rect.y = y;
	    rect.width = 16;
	    rect.height = 16;
	    if (processRect(rect,m_src,m_dest,m_maxStd,m_minDist)) {
	       CvScalar color = CV_RGB(255,0,0);
	        CvPoint p1,p2;
		p1.x = rect.x;
		p1.y = rect.y;		
		p2.x = rect.x  + rect.width;
		p2.y = rect.y  + rect.height;
		cvRectangle(m_dest,p1,p2,color);
	    }
	  }
	}
	*/
	//EDGE MODE
	cvCvtColor(m_src, m_edges, CV_BGR2GRAY);
	cvSmooth(m_edges,m_edges,CV_GAUSSIAN,3);
	cvCanny(m_edges, m_edges, 128, 255, 3);

	CvMemStorage* storage = cvCreateMemStorage(0);
	CvSeq* contour = 0;
	

	cvFindContours(m_edges, storage, &contour, sizeof(CvContour), CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
	
	for( ; contour != 0; contour = contour->h_next ) {
	    
	  CvRect rect = cvBoundingRect(contour,0);
	
	  if (processRect(rect,m_src,m_dest,m_maxStd,m_minDist)) {
	    CvScalar color = CV_RGB(255,0,0);
	    CvPoint p1,p2;
	    p1.x = rect.x;
	    p1.y = rect.y;		
	    p2.x = rect.x  + rect.width;
	    p2.y = rect.y  + rect.height;
	    cvRectangle(m_dest,p1,p2,color);
	  }
	}

	cvReleaseMemStorage(&storage);

	ftime(&t2);
	double timeDiff=(t2.time-t1.time)+((t2.millitm-t1.millitm)/1000.0);
	cout << "Total run time (sec): " << timeDiff << endl;
	
	Image *m_outImg = Image::alloc( imageRef->get_width(), imageRef->get_height(), 3);
	
	//Copy destination image (convert from CV format)
	memcpy(m_outImg->get_data(), m_dest->imageData, m_outImg->get_size());
	
	if( output_id == m_imageOutID ) {			
	  out[count] = ObjectRef(m_outImg);
	}		
      }
      catch (BaseException *e) {
	throw e->add(new GeneralException("Exception in CvBiModalTest::calculate:",__FILE__,__LINE__));
      }
    }
    
  private:
    int m_imageInID;
    int m_imageOutID;
    
    double m_minDist;
    double m_maxStd;
   
  };
}//namespace RobotFlow
#endif