CondensHisto.cpp

/*
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


#include "CondensHisto.hh"
#include "Base/ImageUtils/ImageDraw.hh"

#include <Base/Common/BIASpragma.hh>

using namespace BIAS;
using namespace std;

//#define DEBUG true


CondensHisto::CondensHisto(){
  stateDim_=3;
  img_ = NULL;
  diffScale_=0.05;
  diffScaleSize_=0.05;
  minWeight_=0.0;
  useMultiHistogram_=false;
  secondOrderScale_ = 0.01;
  firstOrderScale_ = 0.99;
  bIntegralHisto_=false;
  m_dMaxRefHistoSize=DBL_MAX;
  secondHistPos_[0]=0;  secondHistPos_[1]=1;
}

void CondensHisto::UseIntegralHistogram(bool bIntHisto)
{
  bIntegralHisto_ = bIntHisto;
}

void CondensHisto::InitModelDefaults()
{

  // the process mean is the image center
  processMean_=areaMax_ * 0.5;

  // this draws the sample to the image center a little
  // its 99% the old position, 1% the mean
  processFirstOrderScale_.Set(firstOrderScale_);

  // a second Order ARP, set scale to zero to have just a first order
  // or set to 0.1 to have 10% second order and 90% first e.g.
  processSecondOrderScale_.Set(secondOrderScale_);

  // and a diffusion
  diffusionSigma_[0]= diffScale_ * areaMax_[0];
  diffusionSigma_[1]= diffScale_ * areaMax_[1];

  diffusionSigma_[stateDim_-1] = diffScaleSize_ * areaMax_[stateDim_-1];

  //here set the percentage of pixels that are sampled
  //with importanceFraction()
  importanceFraction_ = 0.0;
}




//Here create uniform distribution of the samples
void CondensHisto::InitPriorPositions(unsigned int nrOfInitSamples)
{
  // uniform distribution
  for (unsigned int n=0; n<nrOfInitSamples; n++) {
    for (unsigned int d=0;d<stateDim_;d++) {
      samplePosNew_[n][d]=
        random_.GetUniformDistributed(areaMin_[d],areaMax_[d]);
    }
  }
}

void CondensHisto::EvaluateFromIntegralHistoImage_()
{
  double dist=0.0;
  //here generate the Integral Histo Image
  intHisto_.GenerateIntegralHistDiffBin(*img_,refHisto_.GetBinSize(),
                                        refHisto_.GetBinSizeSat());

  BCDOUT(D_COND_INT,"in Integral EOD\n");
  int width = (int)img_->GetWidth();
  int height = (int)img_->GetHeight();

  int heightHalf = refHisto_.GetHeightROI()/2;
  int widthHalf = refHisto_.GetWidthROI()/2;
  //for each sample in samplePosNew_ compute the coords and
  //CalcSimilarity between refHisto_ and IntegralHistoImage
  int upperleftx,upperlefty,lowerrightx, lowerrighty;
  int upperleftx2,upperlefty2,lowerrightx2, lowerrighty2;

  for(unsigned int x = 0; x<Nsamples_; x++){
    BCDOUT(D_COND_PROCESS,"Sample Pos: "<<(int)samplePosNew_[x][0]<<
           ","<<(int)samplePosNew_[x][1]<<std::endl);
    //calculate coords for the comparison of the hists
    //coord = pos - width-/heighthalf*sizefactor
    upperleftx=
      (int)rint(samplePosNew_[x][0]-widthHalf * samplePosNew_[x][2]);
    upperlefty=
      (int)rint(samplePosNew_[x][1]-heightHalf* samplePosNew_[x][2]);
    lowerrightx=
      (int)rint(samplePosNew_[x][0]+widthHalf* samplePosNew_[x][2]);
    lowerrighty=
      (int)rint(samplePosNew_[x][1]+heightHalf* samplePosNew_[x][2]);

    BCDOUT(D_COND_INT,"ROI: "<<upperleftx<<","<<upperlefty<<
             ","<<lowerrightx<<","<<lowerrighty<<std::endl);

    // if coords out of image sample weight is 0
    if((upperleftx>=width)||(upperleftx<0)||(upperlefty<0)
       ||(upperlefty>=height)||(lowerrightx<0)
       ||(lowerrightx>=width)||(lowerrighty<0)
       ||(lowerrighty>=height) ||(samplePosNew_[x][2]<=0)
       ||(samplePosNew_[x][2]> m_dMaxRefHistoSize)
       ||(lowerrighty <= upperlefty)||(lowerrightx <= upperleftx)) {
      sampleWeights_[x] = 0.0;
    } else {
      if (useMultiHistogram_) { // compute corners of second histogram
        upperleftx2= upperleftx + int(secondHistPos_[0]*2)*widthHalf;
        lowerrightx2=upperleftx2+ 2*widthHalf;
        upperlefty2= upperlefty+ int(secondHistPos_[1]*2)*heightHalf;
        lowerrighty2=upperlefty2+2*heightHalf;
      }
      if (useMultiHistogram_ &&
          !((upperleftx2>=width)||(upperleftx2<0)||(upperlefty2<0)
           ||(upperlefty2>=height)||(lowerrightx2<0)
           ||(lowerrightx2>=width)||(lowerrighty2<0)
           ||(lowerrighty2>=height)) )
        dist=intHisto_.CalcSimilarity(refHisto_,upperleftx,upperlefty,
                                      lowerrightx,lowerrighty,
                                      refHisto2_,upperleftx2,upperlefty2,
                                      lowerrightx2, lowerrighty2);
      else
        //compute similarity only from top histogram
        dist=intHisto_.CalcSimilarity(refHisto_,upperleftx,upperlefty,
                                      lowerrightx,lowerrighty);
    }
    BCDOUT(D_COND_PROCESS,"Distance of Histograms: "<<dist<<std::endl);
    if(dist<minWeight_)
      sampleWeights_[x] = minWeight_;
    else
      sampleWeights_[x] = dist;
  }
}


/**
   Here assign a weight to each sample. The weight must correspond
   to the similarity of the histograms of the regions.
*/
void CondensHisto::EvaluateObservationDensities()
{
  double dist = 0.0;

  std::vector<int> point;
  point.assign(2,0);

  if(bIntegralHisto_){
    EvaluateFromIntegralHistoImage_();
  } else {
    int heightHalf = refHisto_.GetHeightROI()/2;
    int widthHalf = refHisto_.GetWidthROI()/2;

    //for each sample in samplePosNew_ generate a Histogram in the present
    //picture and CalcSimilarity to the refHisto_
    int upperleftx,upperlefty,lowerrightx, lowerrighty;
    int upperleftx2,upperlefty2,lowerrightx2, lowerrighty2;
    int width=img_->GetWidth();
    int height=img_->GetHeight();
    for(unsigned int x = 0; x<Nsamples_; x++){
      BCDOUT(D_COND_PROCESS,"Sample Pos: "<<(int)samplePosNew_[x][0]<<
               ","<<(int)samplePosNew_[x][1]<<std::endl);
      //calculate coords for the comparison of the hists
      //coord = pos - width-/heighthalf*sizefactor

      upperleftx=(int)(samplePosNew_[x][0]-widthHalf * samplePosNew_[x][2]);
      upperlefty=(int)(samplePosNew_[x][1]-heightHalf* samplePosNew_[x][2]);
      lowerrightx=(int)(samplePosNew_[x][0]+widthHalf* samplePosNew_[x][2]);
      lowerrighty=(int)(samplePosNew_[x][1]+heightHalf* samplePosNew_[x][2]);

      BCDOUT(D_COND_DEBUG,"RIU: "<<upperleftx<<","<<upperlefty<<
               ","<<lowerrightx<<","<<lowerrighty<<std::endl);
      //if borders out of bounds, weight is zero
      if ( upperleftx>=width || upperleftx<0 || upperlefty<0 ||
           upperlefty>=height || lowerrightx<0 ||
           lowerrightx>=width || lowerrighty<0 ||
           lowerrighty>=height ) {
        sampleWeights_[x] = 0;
      }  else {
        if (useMultiHistogram_) { // compute corners of second histogram
          upperleftx2= upperleftx + int(secondHistPos_[0]*2)*widthHalf;
          lowerrightx2=upperleftx2+ 2*widthHalf;
          upperlefty2= upperlefty+ int(secondHistPos_[1]*2)*heightHalf;
          lowerrighty2=upperlefty2+2*heightHalf;
        }
        if (useMultiHistogram_ &&
            !((upperleftx2>=width)||(upperleftx2<0)||(upperlefty2<0)
              ||(upperlefty2>=height)||(lowerrightx2<0)
              ||(lowerrightx2>=width)||(lowerrighty2<0)
              ||(lowerrighty2>=height))
            ) {
          chist_.GenerateHist(*img_,upperleftx2, upperlefty2,
                              lowerrightx2, lowerrighty2);
          dist = refHisto_.CalcSimilarity(chist_);
        } else
          dist=-1;
        chist_.GenerateHist(*img_,upperleftx, upperlefty,
                            lowerrightx, lowerrighty);
        if (dist<0) // only top histo
          dist = refHisto_.CalcSimilarity(chist_);
        else // compute for both histograms
          dist = 0.5*dist+ 0.5*refHisto_.CalcSimilarity(chist_);
        BCDOUT(D_COND_PROCESS,"Distance of Histograms: "<<dist<<std::endl);
      }
      if(dist<minWeight_)
        sampleWeights_[x] = minWeight_;
      else
        sampleWeights_[x] = dist;
    }//end for
  }

}//end function


//3-dim case
void CondensHisto::SetArea(Vector3<double> min, Vector3<double> max)
{
  areaMin_=min;
  areaMax_=max;
}

void CondensHisto::CheckImageQuality(double percent, int &shutter, int &gain)
{

  int heightHalf = refHisto_.GetHeightROI()/2;
  int widthHalf = refHisto_.GetWidthROI()/2;
  unsigned int upperleftx,upperlefty,lowerrightx, lowerrighty;

  // histogram around mean, with mean size
  upperleftx=(int)(mean_[0]-widthHalf * mean_[2]);
  upperlefty=(int)(mean_[1]-heightHalf* mean_[2]);
  lowerrightx=(int)(mean_[0]+widthHalf* mean_[2]);
  lowerrighty=(int)(mean_[1]+heightHalf* mean_[2]);


  if(!useMultiHistogram_)
  {
    BIASERR("Only implemented for Integral Histogram");
  }
  else if(bIntegralHisto_)
   {
     std::vector<int> roi;
     roi.push_back(upperleftx);
     roi.push_back(upperlefty);
     roi.push_back(lowerrightx);
     roi.push_back(lowerrighty);
     Vector<int> hist = intHisto_.GetHistogram(upperleftx,upperlefty,
                                               lowerrightx,lowerrighty);

     int nBinSat = intHisto_.GetBinSizeSat();
     int nBinHue = intHisto_.GetBinSize();
     int nLumStart =  nBinSat * nBinHue;

     if(nBinHue == 8)
       {

         double lowerLum =hist[nLumStart]+hist[nLumStart+1]+hist[nLumStart+2]+
           hist[nLumStart+3];
         double higherLum = hist[nLumStart+5]+hist[nLumStart+6]+
           hist[nLumStart+7];
         int numPixel = hist[nLumStart+8];
         if(lowerLum / numPixel > percent)
           {
             cout<<"Luminance is low: increase!"<<endl;
             shutter = 1;
           }
         else if(higherLum / numPixel > percent)
          {
             cout<<"Luminance is high: decrease!"<<endl;
             shutter = -1;
           }
       }
     else
       {
         BIASERR("Only implemented for binSizeHue==8");
       }
   }
  else
    {
      BIASERR("Only implemented for Integral Histogram");
    }
}


void CondensHisto::GetMaxValue(Vector<double> &max)
{
  max.newsize(3);
  int pos=0;
  GetMaxWeight(pos);
  max[0] = samplePosOld_[pos][0];
  max[1] = samplePosOld_[pos][1];
  max[2] = samplePosOld_[pos][2];
}


void CondensHisto::GetMeanValue(Vector<double> &mean)
{
  mean = mean_;
}

/**
  @param meanVar= MeanVariance is a vector of struct:
  [varPosx, varPosy, varSize, varWeight]
*/
void CondensHisto::GetMeanVariance(Vector<double>  &meanVar)
{
  meanVar.newsize(4);
  double meanWeight = GetMeanWeight();
  double varWeight=0.0, ddiff;

  Vector<double> diff, mVar;
  mVar.newsize(3);
  mVar.SetZero();
  for(unsigned int i=0;i<Nsamples_;i++){
    diff = samplePosOld_[i] - mean_;
    mVar += diff.ElementwiseProduct(diff);
    ddiff = (sampleWeights_[i]-meanWeight);
    varWeight += (ddiff*ddiff);
  }

  mVar/=double(Nsamples_-1);

  meanVar[0]=mVar[0];
  meanVar[1]=mVar[1];
  meanVar[2]=mVar[2];
  meanVar[3]=varWeight/double(Nsamples_-1);
}

/**
   @return = the maximum weight of all samples
   @param index = the index in samplePosNew_ where maxWeight is found
*/
double CondensHisto::GetMaxWeight(int &index)
{
  double maxWeight=0.0;
  index = -1;
  for(unsigned int i=0;i<Nsamples_;i++)
    {
      if(sampleWeights_[i] > maxWeight)
        {
          index = i;
          maxWeight = sampleWeights_[i];
        }
    }
  return maxWeight;
}


/**
  @param maxVar= MaxVariance is a vector of struct:
  [varPosx, varPosy, varSize, varWeight]
*/
void CondensHisto::GetMaxVariance(Vector<double> &maxVar)
{
  maxVar.newsize(4);
  int index = 0;
  double maxWeight = GetMaxWeight(index);
  double varWeight=0.0, ddiff;

  Vector<double> max=samplePosOld_[index], var, diff;
  var.newsize(3);
  var.SetZero();
  for(unsigned int i=0;i<Nsamples_;i++){
    diff = samplePosOld_[i] - max;
    var += diff.ElementwiseProduct(diff);
    ddiff = (sampleWeights_[i]-maxWeight);
    varWeight += (ddiff*ddiff);
  }

  var/=double(Nsamples_-1);

  maxVar[0]=var[0];
  maxVar[1]=var[1];
  maxVar[2]=var[2];
  maxVar[3]=varWeight/double(Nsamples_-1);
}





//this returns the SamplePositions in an image!
//Does not overload GetSamplePositions() in Condensation.hh,
void CondensHisto::GetSamplePositions(Image<float> &sampleImg){

  float **imgData = sampleImg.GetImageDataArray();
  unsigned int y,x;
  for (unsigned int i =0;i<Nsamples_;i++){
    y = (unsigned int)fabs(samplePosNew_[i][1]);
    x = (unsigned int)fabs(samplePosNew_[i][0]);
    imgData[y][x] = float(sampleWeights_[i]);
  }
}

/**
   @return the active image in condensation algo
*/
Image<unsigned char> CondensHisto::GetAktImage(){

  return *img_;
}


void CondensHisto::DrawHistoSizes(Image<unsigned char> &img)
{

  if (!img.IsEmpty()) img.Release();
  img.Init(img_->GetWidth(),img_->GetHeight(),3);
  //  img.SetColorModel(ImageBase::CM_Grey);
  img.FillImageWithConstValue((unsigned char)0);
  unsigned int radius;
  unsigned int histosize;
  unsigned char values[1];//=new unsigned char[Nsamples_];
  values[0]=255;
  for (unsigned int i=0;i<Nsamples_;i++)
    {

    histosize = 3*
      (unsigned int)(samplePosNew_[i][2]*
                     sqrt(double(refHisto_.GetHeightROI()*
                                 refHisto_.GetWidthROI())));

    radius = (unsigned int)(sampleWeights_[i]* Nsamples_ /
                           sumOfWeights_ + 0.5);

    if ( (samplePosNew_[i][0]-radius>0) && (samplePosNew_[i][1]-radius>0) &&
         (samplePosNew_[i][0]+radius<img.GetWidth()) &&
         (samplePosNew_[i][1]+radius<img.GetHeight()) )
      ImageDraw<unsigned char>::CircleCenter(img,
                                     (unsigned int)samplePosNew_[i][0],
                                     img.GetHeight()-histosize,
                                     radius, &values[0]);
  }

}


/**
   draws the sample positions in the given image with the given value
*/
void CondensHisto::DrawSamples(Image<unsigned char> &img)
{
  if (img.IsEmpty()) {
    img.Init(img_->GetWidth(),img_->GetHeight(),1);
    img.SetColorModel(ImageBase::CM_Grey);
  }
  unsigned char *values = new unsigned char[img.GetChannelCount()];
  unsigned int size;
  double maxWeight=0;

  for (unsigned int n=0;n<Nsamples_;n++)
    if (sampleWeights_[n]>maxWeight) maxWeight=sampleWeights_[n];
  for (unsigned int n=0;n<Nsamples_;n++) {
    for (unsigned int c=0;c<img.GetChannelCount();c++)
      values[c]=(unsigned char)(sampleWeights_[n]/maxWeight*255.0+0.5);

   size=(unsigned int)(samplePosNew_[n][2]*double(refHisto_.GetHeightROI()));
   if (size<=0) size=5;
   if ( (samplePosNew_[n][0]-size/2>0) &&
         (samplePosNew_[n][1]-size/2>0) &&
         (samplePosNew_[n][0]+size/2<img.GetWidth()) &&
         (samplePosNew_[n][1]+size/2<img.GetHeight()) )
      ImageDraw<unsigned char>::RectangleCenter(img,int(samplePosNew_[n][0]),
                                                int(samplePosNew_[n][1]),
                                                size, values);

  }
  delete values;
}

/**
   draws the sample positions in the given image with the given value
*/
void CondensHisto::DrawSamplesWhite(Image<unsigned char> &img)
{
  if (img.IsEmpty()) {
    img.Init(img_->GetWidth(),img_->GetHeight(),1);
    img.SetColorModel(ImageBase::CM_Grey);
  }
  img.FillImageWithConstValue(255);

  unsigned char *values = new unsigned char[img.GetChannelCount()];
  unsigned int size;
  double maxWeight=0;
  unsigned int maxSampleIndex = 0;
  for (unsigned int n=0;n<Nsamples_;n++)
  {
    if (sampleWeights_[n]>maxWeight){
      maxSampleIndex = n;//save max sample index
      maxWeight=sampleWeights_[n];
    }
  }
  for (unsigned int n=0;n<Nsamples_;n++)
  {
    size= (unsigned int)(samplePosNew_[n][2]*double(refHisto_.GetHeightROI()));
    if (size<=0) size=5;
    if ( (samplePosNew_[n][0]-size/2>0) &&
         (samplePosNew_[n][1]-size/2>0) &&
         (samplePosNew_[n][0]+size/2<img.GetWidth()) &&
         (samplePosNew_[n][1]+size/2<img.GetHeight()) )
      {
        if(n==maxSampleIndex)
          {
            values[0] = (unsigned char)0;
            values[1] = (unsigned char)0;
            values[2] = (unsigned char)0;
          }
        else{
          values[0] = (unsigned char)100;
          values[1] = (unsigned char)100;
          values[2] = (unsigned char)100;
        }
        ImageDraw<unsigned char>::RectangleCenter(img,int(samplePosNew_[n][0]),
                                                  int(samplePosNew_[n][1]),
                                                  size, values);
      }
  }
  //draw the last with max value again on top
  values[0] = (unsigned char)0;
  values[1] = (unsigned char)0;
  values[2] = (unsigned char)0;
  size= (unsigned int)(samplePosNew_[maxSampleIndex][2]*
                       double(refHisto_.GetHeightROI()));
  ImageDraw<unsigned char>::RectangleCenter(img,
                        int(samplePosNew_[maxSampleIndex][0]),
                        int(samplePosNew_[maxSampleIndex][1]),
                                                  size, values);
  delete values;
}

void CondensHisto::DrawSamplesExt(BIAS::Image<unsigned char> &img)
{
  unsigned char *values = new unsigned char[img.GetChannelCount()];
  unsigned int size;
  double maxWeight=0;
  for (unsigned int n=0;n<Nsamples_;n++)
    if (sampleWeights_[n]>maxWeight) maxWeight=sampleWeights_[n];
  for (unsigned int n=0;n<Nsamples_;n++) {
    for (unsigned int c=0;c<img.GetChannelCount();c++)
      values[c]=(unsigned char)(sampleWeights_[n]/maxWeight*255.0);
    size=  (unsigned int)(samplePosNew_[n][2]*refHisto_.GetHeightROI());
    if (size<=0) size=5;
    if ( (samplePosNew_[n][0]-size/2>0) &&
         (samplePosNew_[n][1]-size/2>0) &&
         (samplePosNew_[n][0]+size/2<img.GetWidth()) &&
         (samplePosNew_[n][1]+size/2<img.GetHeight()) )
      ImageDraw<unsigned char>::RectangleCenter(img,
                                (unsigned int)samplePosNew_[n][0],
                                (unsigned int)samplePosNew_[n][1],
                                                size, values);
  }

  delete values;
}



/**
   draws the samples in the positions
   and draws the weights of the samples as circles,
   the bigger the circle the higher the weight
   and the grey value indicates the size of the ROI of histogram
*/
void CondensHisto::DrawWeightedSamples(Image<float> &img)
{
  if (!img.IsEmpty()) img.Release();
  img.Init(img_->GetWidth(),img_->GetHeight(),1);
  img.SetColorModel(ImageBase::CM_Grey);
  img.FillImageWithConstValue((float)0);
  unsigned int radius;
  float *values=new float[Nsamples_];

  for (unsigned int i=0;i<Nsamples_;i++) {
    //samplePosOld_[stateDim_-1] hold distance to object
    values[i]=float(samplePosOld_[i][stateDim_-1]);
    radius = (unsigned int)(sampleWeights_[i]* Nsamples_ /
                           sumOfWeights_ + 0.5);

    if ( (samplePosOld_[i][0]-radius>0) && (samplePosOld_[i][1]-radius>0) &&
         (samplePosOld_[i][0]+radius<img.GetWidth()) &&
         (samplePosOld_[i][1]+radius<img.GetHeight()) )
      ImageDraw<float>::CircleCenter(img,
                                     (unsigned int)samplePosOld_[i][0],
                                     (unsigned int)samplePosOld_[i][1],
                                     radius, &values[i]);
  }
  delete[] values;
}

/**
   draws the poserior distribution of the samples in the image
*/
void CondensHisto::DrawPosteriorDistribution(BIAS::Image<unsigned char> &img)
{
  if (fImg_.IsEmpty())
    fImg_.Init(img.GetWidth(), img.GetHeight(),1);

  double max = DrawPosteriorDistribution(fImg_);

  float *pixel;
  // now copy values into result img.
  pixel = fImg_.GetImageData();
  unsigned char *data= img.GetImageData();
  unsigned int size=img.GetPixelCount();
  for (unsigned int i=0;i<size;i++) {
    *data = (unsigned char)(*pixel / max  * 255.0);
    data++;
    pixel++;
  }
}


/**
   same as above but in grey value picture
*/
double CondensHisto::DrawPosteriorDistribution(BIAS::Image<float> &img)
{
  float max=0;
  float *pixel;
  double x,y,dx,dy;
  int width=img.GetWidth() ,height=img.GetHeight();
  img.FillImageWithConstValue((float)0);
  for (unsigned int i=0;i<Nsamples_;i++) {
    x = samplePosOld_[i][0];
    y = samplePosOld_[i][1];

    if ( (x>0) && (y>0) &&
         (x<width-1) && (y<height-1) ) {
      pixel= (img.GetImageDataArray()[(int)y])+ (int)x;
      // inverse bilinear interpolation
      dx = x - (double)floor(x);  dy = y - (double)floor(y);
      // top left
      *pixel+= float((1-dx) * (1-dy) * sampleWeights_[i]);
      if (*pixel>max) max=*pixel;
      // top right
      pixel++;
      *pixel+= float((1-dy) * dx * sampleWeights_[i]);
      if (*pixel>max) max=*pixel;
      // bottom right
      pixel+=width;
      *pixel+= float(dx * dy * sampleWeights_[i]);
      if (*pixel>max) max=*pixel;
      // bottom left
      pixel--;
      *pixel+= float((1-dx) * dy * sampleWeights_[i]);
      if (*pixel>max) max=*pixel;
    }
  }
  return max;
}

/**
   draws the mean value(mean position of all samples) in
   the image, with the size of the rectangle indicating
   the size of the histogram
*/
void CondensHisto::DrawMean(BIAS::Image<unsigned char> &img)
{
#ifdef DEBUG
  cout<<"Mean: "<<mean_[0]<<","<<mean_[1]<<","<<mean_[2]<<endl;

  std::vector<double> point;
  refMultiHisto_.GetDistROIs(point);//now dist in point
  point[0] = mean_[0]+point[0]*mean_[2];
  point[1] = mean_[1]+point[1]*mean_[2];
  cout<<"In Draw, Point: "<<point[0]<<","<<point[1]<<endl;
#endif
  //draw first rectangle
  int upperleftx,upperlefty,lowerrightx, lowerrighty;
  int upperleftx2,upperlefty2,lowerrightx2, lowerrighty2;
  float i=float(mean_[2]*((refHisto_.GetHeightROI()+
                           refHisto_.GetWidthROI())/2));
  if (i<=0) return;

  int heightHalf = int(double(refHisto_.GetHeightROI())/2.0 * mean_[2] + 0.5);
  int widthHalf = int(double(refHisto_.GetWidthROI())/2.0 * mean_[2] + 0.5);
  upperleftx=
    (int)rint(mean_[0]-widthHalf);
  upperlefty=
    (int)rint(mean_[1]-heightHalf);
  lowerrightx=
    (int)rint(mean_[0]+widthHalf);
  lowerrighty=
    (int)rint(mean_[1]+heightHalf);

  //  cout<<"Image Dimensions in Draw: "<<img.GetWidth()<<","<<img.GetHeight()
  //    <<endl;

  if ((upperleftx>0) && (upperlefty>0) && (lowerrightx<(int)img.GetWidth()) &&
      lowerrighty<(int)img.GetHeight())
    {
      ImageDraw<unsigned char>::RectangleCorners(img,upperleftx,upperlefty,
                                                 lowerrightx,lowerrighty);
      ImageDraw<unsigned char>::RectangleCorners(img,upperleftx-1,upperlefty-1,
                                                 lowerrightx-1,lowerrighty-1);
      ImageDraw<unsigned char>::RectangleCorners(img,upperleftx+1,upperlefty+1,
                                                 lowerrightx+1,lowerrighty+1);
    }

  if(useMultiHistogram_){
    upperleftx2= upperleftx + (int)(secondHistPos_[0]*2*widthHalf);
    lowerrightx2=upperleftx2+ 2*widthHalf;
    upperlefty2= upperlefty+ (int)(secondHistPos_[1]*2*heightHalf);
    lowerrighty2=upperlefty2+2*heightHalf;

    //draw second rectangle
    if ((upperleftx2>0) && (upperlefty2>0) &&
        (lowerrightx2<(int)img.GetWidth()) &&
        lowerrighty2<(int)img.GetHeight())
      ImageDraw<unsigned char>::RectangleCorners(img,upperleftx2,upperlefty2,
                                                 lowerrightx2,lowerrighty2);
  }
}