BlobDetectorDOM.cpp

/*
 * BlobDetectorDOM.cpp
 *
 *  Created on: Nov 10, 2009
 *      Author: jordt
 */


#include "BlobDetectorDOM.hh"
#include <Base/ImageUtils/ImageDraw.hh>

using namespace std;
using namespace BIAS;

template <class StorageType>
BlobDetectorDOM<StorageType>::BlobDetectorDOM(){
  // Just some default values
  diffThreshold_ = 20.0;
  minSeedSize_ = 5;
  maxSeedSize_ = 55;
  seedSizeStep_ = 10;
  minBlobSize_ = 2;
  maxBlobSize_ = 500;
  numberOfBins_ = 10;
  maximumBinPopulation_ = 7;
  similarityThreshold_ = 3;
  intImg_ = (double*)NULL;
}

template <class StorageType>
BlobDetectorDOM<StorageType>::~BlobDetectorDOM(){
  if (intImg_ != NULL) delete[] intImg_;
}

template <class StorageType>
int BlobDetectorDOM<StorageType>::Detect(BIAS::Image<StorageType> &image, vector<BIASBlob> &blobs){

  // Build integral image
   CreateIntegralImage(image);

   // Erase old data
   generatedBlobs_.resize(0);
   blobQualityVec_.resize(0);

   // If no blobs are provided, generate seeds and optimize them:
   if (blobs.size() == 0){

     GenerateAndOptimize(generatedBlobs_,blobQualityVec_);

     refineBlobPosAndSize(image);
     // Convert the HomgPoint2D data to the BIASBlob data
     for (unsigned int i = 0; i < generatedBlobs_.size(); i++){
       if (blobQualityVec_[i] >= diffThreshold_ && minBlobSize_ <=  generatedBlobs_[i][2] && maxBlobSize_ >=  generatedBlobs_[i][2]){
         BIASBlob blob;
         blob.UL[0] = generatedBlobs_[i][0] - generatedBlobs_[i][2];
         blob.UL[1] = generatedBlobs_[i][1] - generatedBlobs_[i][2];
         blob.LR[0] = generatedBlobs_[i][0] + generatedBlobs_[i][2];
         blob.LR[1] = generatedBlobs_[i][1] + generatedBlobs_[i][2];
         blob.centerofmass[0]  = generatedBlobs_[i][0];
         blob.centerofmass[1]  = generatedBlobs_[i][1];
         blob.weight = (unsigned int) blobQualityVec_[i];
         blobs.push_back(blob);

       }
     }
   }
   return 0;
}

template <class StorageType>
int BlobDetectorDOM<StorageType>::DrawInImage(Image<StorageType> &image, StorageType* color){

  for (unsigned int i = 0; i < generatedBlobs_.size(); i++)

    ImageDraw<StorageType>::InterpolatedCircleCenter(image, generatedBlobs_[i], (float)generatedBlobs_[i][2],color,1.5f,0.6f);
  

  return 0;
}

template <class StorageType>
void BlobDetectorDOM<StorageType>::SetThreshold(float threshold){
  diffThreshold_ = threshold;
}

template <class StorageType>
void BlobDetectorDOM<StorageType>:: SetCabin(int cabinsPerAxis, int maxPopulation){
  numberOfBins_ = cabinsPerAxis;
  maximumBinPopulation_ = maxPopulation;
}

template <class StorageType>
void BlobDetectorDOM<StorageType>::SetMinSize(unsigned int minSize){
  // Divided by 2 because we use half window size parameters
  minBlobSize_ = minSize / 2;
}

template <class StorageType>
void BlobDetectorDOM<StorageType>::SetMaxSize(unsigned int maxSize){
  // Divided by 2 because we use half window size parameters
  maxBlobSize_ = maxSize / 2;
}

template <class StorageType>
void BlobDetectorDOM<StorageType>::SetSeedGenerationParams(unsigned int minSeedSize,
                                 unsigned int maxSeedSize,
                                 unsigned int seedSizeStep){
  // Divided by 2 because we use half window size parameters
  minSeedSize_ = minSeedSize / 2;
  maxSeedSize_ = maxSeedSize / 2;
  seedSizeStep_ = seedSizeStep / 2;
}


template <class StorageType>
void BlobDetectorDOM<StorageType>::GetBlobsAsHomgPoint2D(vector<HomgPoint2D> &blobs,vector<float> &qual_vec){
  
  blobs = generatedBlobs_;
  qual_vec = blobQualityVec_;
}


template <class StorageType>
void BlobDetectorDOM<StorageType>::GenerateAndOptimize(vector<HomgPoint2D> &pos_vec, vector<float> &qual_vec){

 // Cycle through all blob sizes
  for (int blobSeedSize = minSeedSize_; (unsigned int)blobSeedSize <= maxSeedSize_; blobSeedSize += (int)seedSizeStep_){

    // Cycle through x and y coordinates to plant seeds
    for (int x = blobSeedSize; x < (int)width_ - (int)blobSeedSize; x += (int)blobSeedSize)
      for (int y = blobSeedSize; y < (int)width_ -(int)blobSeedSize; y += (int)blobSeedSize){

      // Get the nearest DOM maximum from seed position
      HomgPoint2D blob = GetLocalMaximum(x,y, blobSeedSize);
      float qual = GetDOM((int)blob[0], (int)blob[1], (int)blob[2]);

      // If the blob matches the criteria, compare it to blobs in the vicinity:
      if (qual > diffThreshold_ && blob[2] < maxBlobSize_){
        int size = pos_vec.size();
        bool found = false;

        unsigned int regionPopulation = 0;
        unsigned int worstInRegion = 0;
        float worstValueInRegion = FLT_MAX;

        for (int s = 0; s < size; s++){
          if (fabs(pos_vec[s][0] - blob[0]) < ((float)width_/(float)numberOfBins_) &&
              fabs(pos_vec[s][1] - blob[1]) < ((float)height_/(float)numberOfBins_)){
             if (fabs(pos_vec[s][0] - blob[0]) < similarityThreshold_ &&
                 fabs(pos_vec[s][1] - blob[1]) < similarityThreshold_ &&
                 fabs(pos_vec[s][2] - blob[2]) < similarityThreshold_){
               found = true;
               // if a similar blob was allready generated by an other seed, replace it
               // if the current dom value is better, otherwise drop it
               if ((float)qual > qual_vec[s]){
                 pos_vec[s] = blob;
                 qual_vec[s] = (float)qual;
               }
            } else {
              regionPopulation ++;
              if (qual_vec[s] <= worstValueInRegion){
                worstValueInRegion = (float)qual_vec[s];
                worstInRegion = s;
              }
            }
          }
        }
        if (!found){

          if (regionPopulation < maximumBinPopulation_){
            // If the maximum population is not reached, add the current blob
            pos_vec.push_back(blob);
            qual_vec.push_back(qual);
          } else {
            if (worstValueInRegion <= qual){
              // If the blob is better than local one and the maximum population is reached,
              // replace the worst one with the current one
              qual_vec[worstInRegion] = qual;
              pos_vec[worstInRegion] = blob;
            }
          }
        }
      }
    }
  }


}

template <class StorageType>
void BlobDetectorDOM<StorageType>::refineBlobPosAndSize(Image<StorageType> img){
   
  
  //Cycle through all blobs:
  for (int i = 0; i < (int)generatedBlobs_.size(); i++){
    // Get the inner mean value:
    
    int x = (int)generatedBlobs_[i][0];
    int y = (int)generatedBlobs_[i][1];
    int halfLevelSize = (int)generatedBlobs_[i][2];
    
    // For Documentation of this part see GetDOM()
    double innerValueSum = EvalIntegralImage(x-halfLevelSize,y-halfLevelSize,x+halfLevelSize+1,y+halfLevelSize+1);
    double completeValueSum = EvalIntegralImage(x-(halfLevelSize*2),y-(halfLevelSize*2),x+(halfLevelSize*2)+1,y+(halfLevelSize*2)+1);\
    double outerValueSum = completeValueSum - innerValueSum;
    double numOfInnerElements = (halfLevelSize*2+1)*(halfLevelSize*2+1);
    double numOfOuterElements = (halfLevelSize*4+1)*(halfLevelSize*4+1) - numOfInnerElements;
    //double threshold = (outerValueSum/numOfOuterElements) + 
    //                    ((innerValueSum/numOfInnerElements) - (outerValueSum/numOfOuterElements))/2.0;
    double DOMValue = ((innerValueSum/numOfInnerElements) - (outerValueSum/numOfOuterElements));
    bool positiveBlob = ((innerValueSum/numOfInnerElements) > (outerValueSum/numOfOuterElements));
    //int ElementsAboveThreshold = 0;
    float weightSum = 0.0;
    generatedBlobs_[i][0] = 0.0;
    generatedBlobs_[i][1] = 0.0;
    generatedBlobs_[i][2] = 0.0;
    StorageType** ida= img.GetImageDataArray();
    int numChannels = img.GetChannelCount();
    for (int xSearch = x-halfLevelSize; xSearch < x+halfLevelSize+1; xSearch++)
      for (int ySearch = y-halfLevelSize; ySearch < y+halfLevelSize+1; ySearch++){
        if ((positiveBlob && (float)ida[ySearch][numChannels*xSearch] >= (outerValueSum/numOfOuterElements))||
        (!positiveBlob && (float)ida[ySearch][numChannels*xSearch] <= (outerValueSum/numOfOuterElements))) {
          
          if ((float)ida[ySearch][numChannels*xSearch] >= (innerValueSum/numOfInnerElements) ||
          (!positiveBlob && (float)ida[ySearch][numChannels*xSearch] <= (innerValueSum/numOfInnerElements))){
            generatedBlobs_[i][0] += (float)xSearch;
            generatedBlobs_[i][1] += (float)ySearch;
            weightSum += 1.0;  
          } else {
            float weight = fabs(((float)ida[ySearch][numChannels*xSearch] - float(outerValueSum/numOfOuterElements))
                            / float(DOMValue));
            generatedBlobs_[i][0] += (float)xSearch * weight;
            generatedBlobs_[i][1] += (float)ySearch * weight;
            weightSum += weight;  
          }
          
        }
      }
  
    generatedBlobs_[i][0] /= (float)weightSum;
    generatedBlobs_[i][1] /= (float)weightSum;
    
    for (int xSearch = x-halfLevelSize; xSearch < x+halfLevelSize+1; xSearch++)
      for (int ySearch = y-halfLevelSize; ySearch < y+halfLevelSize+1; ySearch++){
        if ((positiveBlob && (float)ida[ySearch][numChannels*xSearch] >= (outerValueSum/numOfOuterElements))||
            (!positiveBlob && (float)ida[ySearch][numChannels*xSearch] <= (outerValueSum/numOfOuterElements))) {
          float xDiff = (float)xSearch -  (float)generatedBlobs_[i][0];
          float yDiff = (float)ySearch -  (float)generatedBlobs_[i][1];
          float dist = sqrt(xDiff*xDiff+yDiff*yDiff);
          float weight = fabs(((float)ida[ySearch][numChannels*xSearch] - (float)(outerValueSum/numOfOuterElements))
                                      / (float)DOMValue);
          generatedBlobs_[i][2] += dist*weight;
          
        }
      }
    generatedBlobs_[i][2] /=weightSum;
  }
  
}


template <class StorageType>
HomgPoint2D BlobDetectorDOM<StorageType>::GetLocalMaximum(int x, int y, int halfLevelSize){
  HomgPoint2D pos;
  pos[0] = x;
  pos[1] = y;
  pos[2] = halfLevelSize;
  float currentDOMVal = 0.0;
  float newDOMVal = GetDOM((int)pos[0], (int)pos[1], (int)pos[2]);
  int bestx_step = 0;
  int besty_step = 0;
  int bestl_step = 0;
  while(currentDOMVal < newDOMVal){
    currentDOMVal = newDOMVal;
    float bestDOMVal = newDOMVal;
    // Look in the direct vicinity of the current dom location and search for a direction
    // with a higher dom value
    for (int x_step = -1; x_step <= 1; x_step++){
      for (int y_step = -1; y_step <= 1; y_step++){
        for (int l_step = -1; l_step <= 1; l_step++){

          float DOMVal = GetDOM((int)pos[0]+x_step, (int)pos[1]+y_step, (int)pos[2]+l_step);
          if (DOMVal > bestDOMVal){
            bestDOMVal = DOMVal;
            bestx_step = x_step;
            besty_step = y_step;
            bestl_step = l_step;
          }

        }
      }
    }
    newDOMVal = bestDOMVal;
    pos[0] = pos[0]+bestx_step;
    pos[1] = pos[1]+besty_step;
    pos[2] = pos[2]+bestl_step;
    if (pos[2] < 3 || pos[2] > 30) return pos;
  }
  return pos;
}


template <class StorageType>
float BlobDetectorDOM<StorageType>::GetDOM(int x, int y, int halfLevelSize){

  // If the blob is out of bounds, return 0
  if (x - halfLevelSize*2 < 0 || y - halfLevelSize*2 < 0 || x + halfLevelSize*2 +1 >= (int)width_ || y + halfLevelSize*2 +1>= (int)height_)
    return 0.0;

  // Calculate the sum of the inner pixels via integral image:
  double innerValueSum = EvalIntegralImage(x-halfLevelSize,y-halfLevelSize,x+halfLevelSize+1,y+halfLevelSize+1);
  // Calculate the sum of pixel values of the doubled square via integral image:
  double completeValueSum = EvalIntegralImage(x-(halfLevelSize*2),y-(halfLevelSize*2),x+(halfLevelSize*2)+1,y+(halfLevelSize*2)+1);\
  // outer sum is the big sum minus the  inner sum
  double outerValueSum = completeValueSum - innerValueSum;
  // Calculate number of pixels:
  double numOfInnerElements = (halfLevelSize*2+1)*(halfLevelSize*2+1);
  double numOfOuterElements = (halfLevelSize*4+1)*(halfLevelSize*4+1) - numOfInnerElements;
  // DOM is average inner minus average outer pixel value
  double DOMValue = fabs((innerValueSum/numOfInnerElements) - (outerValueSum/numOfOuterElements));

  return (float)DOMValue;
}

template <class StorageType>
float BlobDetectorDOM<StorageType>::GetAbsoluteDOM(int x, int y, int halfLevelSize){

  // If the blob is out of bounds, return 0
  if (x - halfLevelSize*2 < 0 || y - halfLevelSize*2 < 0 || x + halfLevelSize*2 +1 >= (int)width_ || y + halfLevelSize*2 +1>= (int)height_)
    return 0.0;

  // Calculate the sum of the inner pixels via integral image:
  double innerValueSum = EvalIntegralImage(x-halfLevelSize,y-halfLevelSize,x+halfLevelSize+1,y+halfLevelSize+1);
  // Calculate the sum of pixel values of the doubled square via integral image:
  double completeValueSum = EvalIntegralImage(x-(halfLevelSize*2),y-(halfLevelSize*2),x+(halfLevelSize*2)+1,y+(halfLevelSize*2)+1);\
  // outer sum is the big sum minus the  inner sum
  double outerValueSum = completeValueSum - innerValueSum;
  // Calculate number of pixels:
  double numOfInnerElements = (halfLevelSize*2+1)*(halfLevelSize*2+1);
  double numOfOuterElements = (halfLevelSize*4+1)*(halfLevelSize*4+1) - numOfInnerElements;
  // DOM is average inner minus average outer pixel value
  return float((innerValueSum/numOfInnerElements) - (outerValueSum/numOfOuterElements));
}





// ***********************************************************
//                   Integral Image Routines
template <class StorageType>
void BlobDetectorDOM<StorageType>::CreateIntegralImage(Image<StorageType> &in){

  if (intImg_ != NULL) delete[]intImg_;
  width_ = in.GetWidth();
  height_ = in.GetHeight();

  unsigned int channels = in.GetChannelCount();

  intImg_ = new double[width_*height_];
  StorageType** idaIn = in.GetImageDataArray();

  intImg_[0] = (double)idaIn[0][0];

   for (unsigned int x=1; x < width_; x++)
     intImg_[x] = (double)idaIn[0][x]+intImg_[x-1];

   for (unsigned int y=1; y < height_; y++)
     intImg_[y*width_] = (double)idaIn[y][0]+intImg_[(y-1)*width_];

   for (unsigned int y=1; y < height_; y++)
     for (unsigned int x=1; x < width_; x++)
       intImg_[y*width_+x] = (double)idaIn[y][x*channels]+intImg_[(y-1)*width_+x]
                                      +intImg_[y*width_+(x-1)] - intImg_[(y-1)*width_+(x-1)];

}

template <class StorageType>
double BlobDetectorDOM<StorageType>::EvalIntegralImage(int xA,int yA,int xB,int yB){
  return (intImg_[yB*width_+xB] - intImg_[yB*width_+xA] - intImg_[yA*width_+xB] + intImg_[yA*width_+xA]);
}


namespace BIAS{
template class BlobDetectorDOM<unsigned char>;
template class BlobDetectorDOM<float>;

// fill in instances as required
#ifdef BUILD_IMAGE_INT
template class BlobDetectorDOM<int>;
#endif
#ifdef BUILD_IMAGE_CHAR
//template class BlobDetectorBFS<char>;
#endif
#ifdef BUILD_IMAGE_SHORT
#endif
#ifdef BUILD_IMAGE_USHORT
//template class BlobDetectorBFS<unsigned short>;
#endif
#ifdef BUILD_IMAGE_UINT
#endif
#ifdef BUILD_IMAGE_DOUBLE
#endif
}