stereo.h

Go to the documentation of this file.

#ifndef STEREO_H
#define STEREO_H

#include <cmath>
#include <float.h>
#include <fstream>
#include <iostream> 
#include "image.h"
#include "image_fileoutput.h"
#include "multi_array_op.h"

namespace mimas {
template<typename T>
class stereo : public image <T>
{
//  protected:
//    image<T> leftImage, rightImage;
    
    protected:
  image<double> depthMap;
  
    public:
    bool fillgaps;
      int xsearch, ysearch;
  int xwinsize, ywinsize;
        void stereoMap(image<T> &leftImage, image<T> &rightImage);
  void writeNormalisedDepthMapToFilePGM(char *fn);
        stereo()
        {
      xsearch=21;
      ysearch=5;
      xwinsize=5;
      ywinsize=2;
     fillgaps=false;
  }
  ~stereo()
  {
  }
};

template<typename T>
void stereo<T>::stereoMap(image<T> &leftImage, image<T> &rightImage)
{
  // put in some error checking here like 
  // 1. whether image pairs have been loaded
  // 2. size of pairs etc
  
  int x,y,i,j;
  int offx, offy;


  if (!fillgaps)
  {
    depthMap.init(leftImage.getWidth(),leftImage.getHeight());
  }
  
  for (y=ysearch+ywinsize/2; y<leftImage.getHeight()-ysearch-ywinsize/2; y++)
  {
#ifndef NDEBUG
    std::cerr << "y = " << y << std::endl;
#endif
    for (x=xsearch+xwinsize/2; x<leftImage.getWidth()-xsearch-xwinsize/2; x++)
    {
      int xpoint, ypoint;
      double minEnergy, energy;
      int energy1, energy2, energy3;
      double depth;
      
      minEnergy=DBL_MAX;
      
      for (i=x-xsearch/2; i<=x+xsearch/2; i++)
        for (j=y-ysearch/2; j<=y+ysearch/2; j++)
        {
          //energy=0;
          
          energy1=0;
          energy2=0;
          energy3=0;
          for (offx=-xwinsize/2; offx<=xwinsize/2; offx++)
            for (offy=-ywinsize/2; offy<=ywinsize/2; offy++)
            {
              //energy+=(int)sqrt( (leftImage.getPixel(x+offx,y+offy)-rightImage.getPixel(i+offx,j+offy)) *
              //      (leftImage.getPixel(x+offx,y+offy)-rightImage.getPixel(i+offx,j+offy)) );
                    
              energy1+=( (leftImage.pixel(x+offx,y+offy)-rightImage.pixel(i+offx,j+offy)) *
                    (leftImage.pixel(x+offx,y+offy)-rightImage.pixel(i+offx,j+offy)) );
              energy2+=leftImage.pixel(x+offx,y+offy)*leftImage.pixel(x+offx,y+offy);
              energy3+=rightImage.pixel(i+offx,j+offy)*rightImage.pixel(i+offx,j+offy);
              

            }

          energy=(double)energy1/sqrt((double)energy2*(double)energy3);
          
          if (energy<minEnergy)
          {
            minEnergy=energy;
            xpoint=i;
            ypoint=j;
          }

        }
        
      depth=(double)( ((x-xpoint)*(x-xpoint)) + ( (y-ypoint)*(y-ypoint) ));
      depth=sqrt(depth);
      //if ((int)minEnergy==0) depth=0.0;
      if (minEnergy<1e-16) depth=0.0;
          
//      int depthint=(int)depth;

      if (fillgaps)
      {
        if (depthMap.pixel(x,y)<1e-1)
              depthMap.pixel(x,y) = (double)depth;
      }
      else
           depthMap.pixel(x,y) = (double)depth;

//      depthMap.setPixel(x,y,(double)depthint);
//      depthMap.setPixel(x,y,(double)depth);
//      cout << (int)minEnergy << " ";
    }
  }
}


template<typename T>
void stereo<T>::writeNormalisedDepthMapToFilePGM(char *fn)
{
  int i,j;
  double maxIntensity;

  maxIntensity=DBL_MIN;
  for (i=0; i<depthMap.getWidth(); i++)
    for (j=0; j<depthMap.getHeight(); j++)
        if (maxIntensity<depthMap.pixel(i,j)) maxIntensity=depthMap.pixel(i,j);

#ifndef NDEBUG
  std::cerr << "maxIntensity = " << maxIntensity << endl;
#endif

  image< double > tempImage;
  tempImage.init( depthMap.getWidth(), depthMap.getHeight() );
  tempImage.rawData() = depthMap.rawData() * 255.0 / maxIntensity;

  std::ofstream outFile( fn, std::ios::binary );
  outFile << tempImage;
}
}

#endif

---